一种无人机自动投饵装置的控制方法及系统

1.本发明涉及农用飞行器控制技术领域，特别是一种无人机自动投饵装置的 控制方法及系统。


背景技术：

2.近些年来，随着信息和制造业技术的快速发展，涌现出了种类繁多、场景 丰富的多旋翼无人机，在现实农业中，人们开始逐渐使用植保作业型的无人机， 这在很大程度上减轻了劳动力、降低成本，实现增产增收。
3.然而，在水产养殖这片区域中，尤其是在大面积的对虾养殖中，人们每天 需要对虾进行定时定量地多次均匀投饵，目前往往是以人工投饵为主，劳动强 度大，耗时费力，工作效率低，且人工投饵无法实现虾塘均匀投饵的问题，从 而导致虾塘饵料堆积严重，污染水体，使虾类感染病毒，进而造成经济受损。 为了解决该问题，目前市场上出现一些为虾塘投饵而专门研发的移动式投饵设 备，如无人船移动平台等，但是这些设备并没有很好地实现大面积虾塘高效率 投饵，主要存在一下缺陷：
4.(1)现有投饵设备不能有效做到一个设备对多塘进行投饵操作，不能远程 装填饲料、饲料搅拌均匀和定时定量投饵等。
5.(2)现有投饵设备结构单一、饲料量搅拌不合理、装填量少、投饵不均匀， 不能有效地满足对虾严格的养殖需求。
6.(3)现有的无人机投饵装置采用传统单一的惯性测量元件采集数据以完成 低性能的姿态估计进而实现对无人机飞行进行控制，这就使得其难以在对虾养 殖作业过程中进行高效率解算得到最优的姿态解，无法对无人机投饵装置实现 更精准快速的控制，无人机投饵装置飞行不够灵活，投饵操作精度低。


技术实现要素：

7.针对上述缺陷，本发明的目的在于提出无人机自动投饵装置，其在用户远 程控制下，可以实现泵液混料、搅拌和甩料自动化操作。
8.本发明的另一目的在于提出投饵系统及投饵控制方法，应用上述无人机自 动投饵装置后，可以实现使得对虾塘的甩料操作更加高效和精准。
9.为达此目的，本发明采用以下技术方案：
10.一种无人机自动投饵装置，其包括：无人机本体、填料箱、水箱、水泵和 漏斗搅拌器；所述无人机本体为四旋翼无人机，其四根旋翼支撑臂的驱动端竖 直设有固定轴，所述固定轴的上下两端分别设有一个旋翼驱动电机，一个旋翼 驱动电机与固定轴顶部水平设置的第一旋翼传动连接，另一个旋翼驱动电机与 固定轴底部水平设置的第二旋翼传动连接；所述填料箱的一侧设有进水口，底 部设有喷料口；所述漏斗搅拌器包括漏斗状搅拌腔和设置于漏斗状搅拌腔内的 搅拌组件；所述漏斗搅拌腔的顶面设有进料口，其底端设甩喷料口；所述填料 箱可拆卸的设置于所述无人机本体的顶部；所述水泵的进液端与所述水箱
连通， 所述水泵的出液端与所述进水口连通；所述漏斗搅拌器设置于所述喷料口的正 下方。
11.更优的，所述水箱内设有液位计，所述液位计用于检测所述水箱内液体存 储量；所述填料箱的底部应变式压力传感器，所述应变式压力传感器用于检测 所述填料箱内饲料的存储重量。
12.更优的，所述无人机自动投饵装置还设有显示模块，所述显示模块与所述 液位计和应变式压力传感器电联接，所述显示模块用于显示所述水箱内液体的 存储量和所述填料箱内饲料的存储重量。
13.一种无人机投饵系统，其包括：实时建图与定位系统、用户虾塘料仓系统 和飞行通讯控制系统。
14.所述实时建图与定位系统包括：摄像识别模块、定位模块和工控电脑、；所 述摄像识别模块，用于拍摄虾塘图片信息，并识别其地理信息；所述定位模块， 用于对所述无人机自动投饵装置和虾塘建立坐标信息并提取位置信息；所述工 控电脑，用于根据虾塘的地理信息、位置信息和坐标信息建立虾塘三维地图， 并将无人机自动投饵装置实时定位在所述虾塘三维地图内。
15.所述用户虾塘料仓系统包括：地面遥控模块和料仓；所述地面遥控模块， 用于与所述无人机自动投饵装置实现远程通讯；所述料仓，用于储存饲料，并 根据投饵需要对无人机自动投饵装置填装饲料。
16.飞行通讯控制系统包括：通讯模块、机载传感模块和无人机主控模块；所 述通讯模块，用于实现所述无人机自动投饵装置、所述工控电脑和地面遥控模 块之间的通讯；所述机载传感模块，其用于实时检测所述无人机自动投饵装置 的航向、姿态、速度和位置信息；所述无人机主控模块，用于接收实时建图与 定位系统、用户虾塘料仓系统和机载传感模块的信息，对所述无人机自动投饵 装置进行飞行控制和甩饵操作。
17.所述工控电脑与所述定位模块、地面遥控模块和摄像设别模块通讯联接； 所述无人机主控模块与地面遥控模块、机载传感模块、工控电脑和无人机自动 投饵装置通讯连接。
18.具体的，所述摄像识别模块为深度摄像头；所述通讯控制模块包括5g模块 和接收机，所述地面遥控模块与所述5g模块和接收机通讯连接。
19.具体的，所述机载传感模块捷联式惯性传感器，其包括陀螺仪、加速度计 和磁力计。
20.一种投饵控制方法，其包括如下内容：
21.a.划定要被投饵的虾塘，无人机自动投饵装置的拍摄模块拍摄该虾塘的图 片信息，所述工控电脑根据图片信息构建虾塘的三维图地图；
22.b.无人机自动投饵装置利用定位模块得到位置信息和坐标信息；
23.c.所述工控电脑根据三维图地图、定位信息和坐标信息及用户设定投饵要 求得到信息反馈，并传输给用户虾塘料仓设有的用户终端；用户终端向工控电 脑发送任务规划信息；
24.d.所述工控电脑向无人机主控模块发送指令，无人机飞行器装置起飞，在 所述无人机主控的控制下对虾塘进行投饵操作。
25.更优的，上述步骤d中还包括如下内容：
26.d1.所述无人机主控模块通过接收机接和/或5g模块接收用户终端发送的手 动控制指令，所述无人机主控模块根据该手动控制指令控制无人机飞行器装置 的飞行情况和甩饵操作；
27.d2.所述无人机主控模块实时接收记载传感发送的姿态控制指令进行矢量 观测姿态评估处理和矢量姿态估计处理得到飞行姿态控制指令，所述无人机主 控模块根据该飞行姿态控制指令调整无人机飞行器装置的飞行姿态，并在指定 配料程序下完成泵水、搅拌和甩料操作。
28.更优的，上述步骤d2中矢量观测姿态评估算法处理包括如下内容：
29.(1)建立坐标系，所述无人机自动投饵装置的坐标系b为前左上坐标系， 选取惯性参考坐标系n为东北天坐标系，定义分别如下：
30.b＝(x
b y
b z
b
)
τ
31.n＝(x
n y
n z
n
)
τ
32.采用z
‑
x
‑
y顺规次序，其中φ、θ、γ分别为对应的偏航角、俯仰角和横滚 角，算出坐标系n到坐标系b的旋转矩阵如下：
[0033][0034]
令r定义为的矩阵元素如下，可分别解得对应的姿态角表达式：
[0035][0036][0037]
(2)由坐标变换的四元数乘法表示式，计算坐标系b到坐标系n的四元数 旋转矩阵表示即：
[0038][0039]
其中r
b
、r
n
为零标量的四元数，m(q)、m
*
(q)为对应的四元数矩阵表示形式， 则得到坐标系b到坐标系n的四元数旋转矩阵如下：
[0040][0041]
根据旋转矩阵性质，得到从坐标系n到坐标系b的四元数旋转矩阵，即：
[0042][0043]
(3)加速度计和磁力计矢量分别观测得到
n
a＝(a
x a
y a
z
)
τ
和 n
m＝(m
x m
y m
z
)
τ
，将两者分别转化到机体坐标系得到矢量方程，如下：
[0044][0045]
(4)转化为wahba’s代价函数，即求得误差最小时旋转矩阵r
[0046]
j(r)＝arg min(λ
a
||
b
a
‑
r
·
n
a||2+λ
m
||
b
m
‑
r
·
n
m||2)
[0047]
其中，λ
a
，λ
m
分别为加速度计和磁力计的信任权重值，且两者之和为1；将 其转化为四元数q的形式，p1(q)，p2(q)，p3(q)为四元数运算过程时的矩阵因子， 则分别有：
[0048]
f(q,a)＝[a
x
·
p1(q)+a
y
·
p2(q)+a
z
·
p3(q)]q
‑
b
a
[0049]
f(q,m)＝[m
x
·
p1(q)+m
y
·
p2(q)+m
z
·
p3(q)]q
‑
b
m
[0050]
再将其转化为关于求解误差最小时四元数q的代价函数，即：
[0051][0052]
(5)梯度下降优化，由梯度下降公式
[0053][0054]
其中q
obs,k
为当前时刻观测得到四元数，q
obs,k
‑1为上一时刻观测得到的四元数， η
为梯度下降步长。优化得到如下的梯度更新表达式：
[0055]
q
obs,k
＝q
obs,k
‑1‑
2η
·
σ(q
obs,k
‑1)
·
q
obs,k
‑1[0056]
其中，σ(q
obs,k
‑1)为优化运算过程的权重因子项；
[0057]
(6)陀螺仪采样得到的机体系三轴原始角速度数据
b
w，即：
[0058]
b
w＝(w
x w
y w
z
)，由四元数微分更新方程：
[0059][0060]
其中，q
mes,k
为陀螺仪当前时刻直接测量求解得到的四元数；
[0061]
(7)根据线性观测器，构造互补滤波器，即可得到如下的姿态估计更新方 程：
[0062][0063]
并对其进行单位化处理，得到：
[0064][0065]
其中，l为观测器的反馈增益矩阵，其值为经验对角常数矩阵，
△
t为陀螺仪 采样三轴角速度时的采样周期，g为互补滤波器的增益，且g＝(l
‑1+i)
‑1,[
b
w]
×
为 陀螺仪采集的三轴数据构成的反对称矩阵，为当前时刻估计的四元数输出，为上一时刻估计的四元数输出；
[0066]
(8)最后根据四元数对应的旋转矩阵，求解对应的姿态角φ、θ、γ得到：
[0067][0068]
更优的，所述步骤d2中矢量姿态估计处理包括如下内容：
[0069]
(9)先对程序的参数完成初始化，初始操作包括互补增益的g的确定、初 始姿态q
init
的给定、梯度下降步长η的设置、陀螺仪的采样周期
△
t、加速度计和 磁力计的传感器的权重信任值λ
a
、λ
m
给定、加速度计和磁力计的标准模值snorm 以及阈值异常检验时的阈值μ
a
,μ
m
；其中标准模值snorm的值是先通过对加速度 计和磁力计在没有剧烈运动，且在平滑环境多次运行下离线处理计算得到的常 数值，阈值则根据实验多次设定的一个关于投饵无人机实际运行时的矢量观测 得到模值norm与标准模值差的绝对值的一个界限,即如下分段函数表达式：
[0070][0071]
(10)判断是否有捷联式观测矢量输入，即加速度计和磁力计在机体系采 集的传感数据输入，如果没有，则按初始姿态输出结束，反之则继续对给定的 参考矢量进行单位化，单位化完成后判断陀螺仪是否能得到有效响应校验，如 果没有，则令互补增益g＝i(i为单位矩阵)，如果有，则按数值正常计算互补增 益g；
[0072]
(11)实时计算捷联式观测矢量的模norm和它对应的实时单位化数值；计 算完成
后，进行阈值异常检测，也即根据步骤(1)里的分段函数进行判断，如 果异常，则设置对应的传感器权重为0，反之则保持正常权重值，之后计算关于 权重的因子项之和，最后根据互补滤波器的姿态更新方程计算对应的姿态四元 数，进而求得估计的当前机体的姿态角度；
[0073]
(12)程序判断是否继续下一时刻矢量观测，重复完成(1)至(11)步骤。
[0074]
本发明的实施例的有益效果：
[0075]
(1)所述无人机自动投饵装置在常规的四旋翼的结构基础上，考虑在虾塘 环境进行大面积的作业而需要承载一定重量的配料，设计了稳定可靠的共轴反 桨上下两层旋翼结构来综合提升无人机的升力和速度、同时在较大程度上改善 机体的平稳性来充分满足在虾塘进行精细投饵的作业需求，在大程度上提升了 虾塘投饵装置的整机性能，可以实现一机多塘高效率、预设路线规划和远距离 大面积对虾塘进行空中均匀投饵作业任务。
[0076]
(2)所述无人机自动投饵装置的配料、搅拌、甩料投饵这套装置流程能够 解决投饵常规虾塘投饵过程的配料繁琐、搅拌不均匀、甩料不精细等弊端，预 设好的投饵程序只需要用户选定好相应的投饵塘口数量及指定位置，虾塘投饵 无人机即可按照规划好的路线进行定时定量地、精准高效地投饵作业。
[0077]
(3)所述无人机自动投饵系统搭载的惯性测量元件等传感器，通过其分别 实时采集对应的传感数据信息，得到捷联式观测矢量，接着对其进行梯度下降 优化，进而完成传感器的性能互补，得到满足嵌入式硬件计算且算法时间消耗 度比较低的互补滤波算法，从而实时准确计算虾塘投饵无人机作业时的当前姿 态，即实现对投饵无人机机体比较准确的姿态估计算法，使得整个系统能够维 持在长时间、跨度面积大等环境下平稳精准高效地完成对虾均匀投饵所需的严 格条件的要求。
[0078]
(4)所述无人机自动投饵系统配合所述投饵控制方法，通过深度摄像头、 gps和rtk完成对虾塘环境的实时三维建图和定位，每次投饵作业时都依据上一 次作业时所建好的地图，且结合当前用户设定或者默认的虾塘路线的上一次地 图进行投饵作业，同时也记录和保存当前投饵作业时的路线地图，用于下一次 作业，这实现了相对灵活且可以定时定量定路线的投饵作业模式。
[0079]
(5)在现有技术的基础上，本技术技术首先着重解决普通的自动养殖投饵 无人机结构单一、饲料量搅拌不合理、装填量少、投饵不均匀的一些缺点，构 建一种新型的共轴反桨无人机自动投饵装置的外观结构。其次，就对对虾养殖 环境投饵的过程中，本技术技术设计的无人机系统能够自主搅拌饲料、定时定 量、高效均匀地完成大面积虾塘的投饵任务。最后，就共轴反桨虾塘无人机自 动投饵装置在虾塘环境中作业时承重量的改变等影响因素如何有效进行平稳控 制，本技术技术设计了通过捷联式惯性传感器得到观测矢量，进而通过梯度下 降方法优化后并进行互补滤波算法后得到最优的投饵无人机的当前状态的估 计，能够为无人机自动投饵系统自动投饵过程实现可靠性高地的姿态平稳控制。
附图说明
[0080]
图1是本发明的一个实施例中所述无人机自动投饵装置的结构示意图；
[0081]
图2是本发明的一个实施例中所述无人机投饵系统的示意框图；
[0082]
图3是本发明的一个实施例中所述矢量观测姿态评估算法处理步骤的原理 示意图；
[0083]
图4是本发明的一个实施例中所述矢量姿态估计处理步骤的程序流程图。
[0084]
其中：无人机本体110，第一旋翼111，第二旋翼112，填料箱120，水箱 130，加水口131，水泵140，摄像识别模块150，显示模块160，电池170。
具体实施方式
[0085]
下面结合附图并通过具体实施方式来进一步说明本发明的技术方案。
[0086]
实施例一
[0087]
如图1所示，一种无人机自动投饵装置，其包括：无人机本体110、填料箱 120、水箱130、水泵140和漏斗搅拌器；所述无人机本体为四旋翼无人机，其 四根旋翼支撑臂的驱动端竖直设有固定轴，所述固定轴的上下两端分别设有一 个旋翼驱动电机，一个旋翼驱动电机与固定轴顶部水平设置的第一旋翼111传 动连接，另一个旋翼驱动电机与固定轴底部水平设置的第二旋翼112传动连接； 所述第一旋翼和所示第二旋翼是共轴设置的，但是两个旋翼具体安装位置和驱 动方向是不一样的，位于所述固定轴顶部的所述第一旋翼是正向桨叶，下所述 第二旋翼则是反向桨叶，两个旋翼是分别由上下两个旋翼驱动电机进行控制的， 给所述无人机自动投饵装置的驱动力方向是一致向上的，因为在单侧旋翼转动 时会产生偏转力矩，两侧转动后可以有效抵消偏转力矩，并且综合提升力，改 善投饵过程受力不均的平稳性。所述填料箱120的一侧设有进水口，底部设有 喷料口；所述漏斗搅拌器包括漏斗状搅拌腔和设置于漏斗状搅拌腔内的搅拌组 件；所述漏斗搅拌腔的顶面设有进料口，其底端设甩喷料口；所述填料箱120 可拆卸的设置于所述无人机本体110的顶部；所述水泵140的进液端与所述水 箱130连通，所述水泵150的出液端与所述进水口连通；所述漏斗搅拌器设置 于所述喷料口的正下方；所述水箱设有加水口131；所述无人机本体110还设有 摄像识别模块150。
[0088]
更优的，所述水箱内设有液位计，所述液位计用于检测所述水箱130内液 体存储量；所述填料箱120的底部应变式压力传感器，所述应变式压力传感器 用于检测所述填料箱120内饲料的存储重量。
[0089]
更优的，所述无人机自动投饵装置还设有显示模块160，所述显示模块与所 述液位计和应变式压力传感器电联接，所述显示模块用于显示所述水箱120内 液体的存储量和所述填料箱120内饲料的存储重量；同时会根据用户计划投饵 的虾塘范围计算出需要填装多少饲料和液体，当用户填装不够，但是所述水箱 130和填料箱120又有空余空间时，所述显示模块会及时提醒用户可以继续填装 多少饲料和液体。
[0090]
实施例二
[0091]
如图2所示，一种可应用于实施例一中的无人机投饵系统，其包括：实时 建图与定位系统、用户虾塘料仓系统和飞行通讯控制系统。
[0092]
所述实时建图与定位系统包括：摄像识别模块、定位模块和工控电脑、；所 述摄像识别模块，用于拍摄虾塘图片信息，并识别其地理信息；所述定位模块， 用于对所述无人机自动投饵装置和虾塘建立坐标信息并提取位置信息；所述工 控电脑，用于根据虾塘的地理信息、位置信息和坐标信息建立虾塘三维地图， 并将无人机自动投饵装置实时定位在所述虾塘三维地图内。
[0093]
所述用户虾塘料仓系统包括：地面遥控模块和料仓；所述地面遥控模块， 用于与
所述无人机自动投饵装置实现远程通讯；所述料仓，用于储存饲料，并 根据投饵需要对无人机自动投饵装置填装饲料。
[0094]
飞行通讯控制系统包括：通讯模块、机载传感模块和无人机主控模块；所 述通讯模块，用于实现所述无人机自动投饵装置、所述工控电脑和地面遥控模 块之间的通讯；所述机载传感模块，其用于实时检测所述无人机自动投饵装置 的航向、姿态、速度和位置信息；所述无人机主控模块，用于接收实时建图与 定位系统、用户虾塘料仓系统和机载传感模块的信息，对所述无人机自动投饵 装置进行飞行控制和甩饵操作。
[0095]
所述工控电脑与所述定位模块、地面遥控模块和摄像设别模块通讯联接； 所述无人机主控模块与地面遥控模块、机载传感模块、工控电脑和无人机自动 投饵装置通讯连接。
[0096]
具体的，所述无人机自动投饵装置设有多块电池，能够提升其巡航能力， 也能为各个记载电器元件提供稳定电能保证投饵工作正常进行。所述无人机主 控模块可以为nvdia tx2微型工控电脑；所述摄像模块可以为深度摄像头模组； 所述定位给模块可以为gps模组、rtk高精度差分定位模块和磁力计等。
[0097]
具体的，所述摄像识别模块为深度摄像头；所述通讯控制模块包括5g模块 和接收机，所述地面遥控模块与所述5g模块和接收机通讯连接。
[0098]
具体的，所述机载传感模块捷联式惯性传感器，其包括陀螺仪、加速度计 和磁力计。捷联惯导系统是先有的一种飞平台式惯导系统，其由于省去了复杂 的机电平台，结构简单、体积小、重量轻、成本低、维护简单、可靠性高、还 可以通过冗余技术提高其容错能力，其可以用来测量无人机的角运动信息和线 运动信息，飞行控制模块根据这些测量信息可以解算出运载体的航向、姿态、 速度和位置等飞行信息。
[0099]
具体的，所述无人机投饵系统方案框图如图2所示，其中用户虾塘料仓以 地面站、遥控器作为用户的控制端、虾塘投饵任务规划端和信息反馈接收端， 且作为投饵无人机的填料点；无人机主控模块主要以高性能嵌入式stm32芯片 为虾塘投饵无人机的主控，以惯性测量元件、磁力计等外设传感器作为无人机 飞行的机载传感模块，并依靠5g模块作为系统的通信；实时三维建图与定位系 统包括了搭载了装有机器人操作系统的工控电脑，其可以完成以gps和rtk模 组的定位模块以及深度摄像头的虾塘实时三维建图与定位。
[0100]
同时，所述无人机主控模块可以获取工控电脑所依据建图与定位后的一些 状态信息以及对其发送相应通信指令。用户先对虾塘环境进行遥控控制进行三 维建图与定位，完成后可以对无人机进行虾塘进行任务规划，同时收集当前作 业时的实时三维图像等信息；进而可以使得所述无人机自动投饵装置可以实现 自动建立三维地图并定位、远程填料、搅拌、甩料等精准操作，使得其对虾塘 的投饵操作更加智能和精准。
[0101]
实施例三
[0102]
一种投饵控制方法，其包括如下内容：
[0103]
a.划定要被投饵的虾塘，无人机自动投饵装置的拍摄模块拍摄该虾塘的图 片信息，所述工控电脑根据图片信息构建虾塘的三维图地图；
[0104]
b.无人机自动投饵装置利用定位模块得到位置信息和坐标信息；
[0105]
c.所述工控电脑根据三维图地图、定位信息和坐标信息及用户设定投饵要 求得到信息反馈，并传输给用户虾塘料仓设有的用户终端；用户终端向工控电 脑发送任务规划
信息；
[0106]
d.所述工控电脑向无人机主控模块发送指令，无人机飞行器装置起飞，在 所述无人机主控的控制下对虾塘进行投饵操作。
[0107]
所述步骤d中还包括如下内容：
[0108]
d1.所述无人机主控模块通过接收机接和/或5g模块接收用户终端发送的手 动控制指令，所述无人机主控模块根据该手动控制指令控制无人机飞行器装置 的飞行情况和甩饵操作；
[0109]
d2.所述无人机主控模块实时接收记载传感发送的姿态控制指令，所述无人 机主控模块根据该姿态控制指令调整无人机飞行器装置的飞行姿态，并在指定 配料程序下完成泵水、搅拌和甩料操作。
[0110]
如图3所示，所述步骤d2中矢量观测姿态评估算法处理步骤中还包括如下 内容：
[0111]
(1)建立坐标系，所述无人机自动投饵装置的坐标系b为前左上坐标系， 选取惯性参考坐标系n为东北天坐标系，定义分别如下：
[0112]
b＝(x
b y
b z
b
)
τ
[0113]
n＝(x
n y
n z
n
)
τ
[0114]
采用z
‑
x
‑
y顺规次序，其中φ、θ、γ分别为对应的偏航角、俯仰角和横滚 角，算出坐标系n到坐标系b的旋转矩阵如下：
[0115][0116]
令r定义为的矩阵元素如下，可分别解得对应的姿态角表达式：
[0117][0118][0119]
(2)由坐标变换的四元数乘法表示式，计算坐标系b到坐标系n的四元数 旋转矩阵表示即：
[0120][0121]
其中r
b
、r
n
为零标量的四元数，m(q)、m
*
(q)为对应的四元数矩阵表示形式， 则得到坐标系b到坐标系n的四元数旋转矩阵如下：
[0122][0123]
根据旋转矩阵性质，得到从坐标系n到坐标系b的四元数旋转矩阵，即：
[0124][0125]
(3)加速度计和磁力计矢量分别观测得到
n
a＝(a
x a
y a
z
)
τ
和 n
m＝(m
x m
y m
z
)
τ
，将两者分别转化到机体坐标系得到矢量方程，如下：
[0126][0127]
(4)转化为wahba’s代价函数，即求得误差最小时旋转矩阵r
[0128]
j(r)＝arg min(λ
a
||
b
a
‑
r
·
n
a||2+λ
m
||
b
m
‑
r
·
n
m||2)
[0129]
其中，λ
a
，λ
m
分别为加速度计和磁力计的信任权重值，且两者之和为1；将 其转化为四元数q的形式，p1(q)，p2(q)，p3(q)为四元数运算过程时的矩阵因子， 则分别有：
[0130]
f(q,a)＝[a
x
·
p1(q)+a
y
·
p2(q)+a
z
·
p3(q)]q
‑
b
a
[0131]
f(q,m)＝[m
x
·
p1(q)+m
y
·
p2(q)+m
z
·
p3(q)]q
‑
b
m
[0132]
再将其转化为关于求解误差最小时四元数q的代价函数，即：
[0133][0134]
(5)梯度下降优化，由梯度下降公式
[0135][0136]
其中q
obs,k
为当前时刻观测得到四元数，q
obs,k
‑1为上一时刻观测得到的四元数， η
为梯度下降步长。优化得到如下的梯度更新表达式：
[0137]
q
obs,k
＝q
obs,k
‑1‑
2η
·
σ(q
obs,k
‑1)
·
q
obs,k
‑1[0138]
其中，σ(q
obs,k
‑1)为优化运算过程的权重因子项；
[0139]
(6)陀螺仪采样得到的机体系三轴原始角速度数据
b
w，即：
[0140]
b
w＝(w
x w
y w
z
)，由四元数微分更新方程：
[0141][0142]
其中，q
mes,k
为陀螺仪当前时刻直接测量求解得到的四元数；
[0143]
(7)根据线性观测器，构造互补滤波器，即可得到如下的姿态估计更新方 程：
[0144][0145]
并对其进行单位化处理，得到：
[0146][0147]
其中，l为观测器的反馈增益矩阵，其值为经验对角常数矩阵，
△
t为陀螺仪 采样三轴角速度时的采样周期，g为互补滤波器的增益，且g＝(l
‑1+i)
‑1,[
b
w]
×
为 陀螺仪采集的三轴数据构成的反对称矩阵，为当前时刻估计的四元数输出，为上一时刻估计的四元数输出；
[0148]
(8)最后根据四元数对应的旋转矩阵，求解对应的姿态角φ、θ、γ得到：
[0149][0150]
所述步骤d2中矢量姿态估计处理步骤中所述无人机自动投饵装置的矢量姿 态估计算法的流程如图4所示，具体包括如下内容：
[0151]
(9)程序一开始先完成一些参数的初始化，其中包括互补增益的g的确定、 初始姿态q
init
的给定、梯度下降步长η的设置、陀螺仪的采样周期
△
t、加速度计 和磁力计的传感器的权重信任值λ
a
、λ
m
给定、加速度计和磁力计的标准模值 snorm以及阈值异常检验时的阈值μ
a
,μ
m
；其中标准模值snorm的值是先通过对 加速度计和磁力计传感器在没有剧烈运动、在平滑环境多次运行下离线处理计 算得到的常数值，阈值则根据实验多次设定的一个关于投饵无人机实际运行时 的矢量观测得到模值norm与标准模值差的绝对值的一个界限,即如下分段函数 表达式：
[0152][0153]
(10)判断是否有捷联式观测矢量输入，即加速度计和磁力计在机体系采 集的传感数据输入，如果没有，则按初始姿态输出结束，反之则继续对给定的 参考矢量进行单位化，单位化完成后判断陀螺仪是否能得到有效响应校验，如 果没有，则令互补增益g＝i(i为单位矩阵)，如果有，则按数值正常计算互补增 益g；
[0154]
(11)实时计算捷联式观测矢量的模norm和它对应的实时单位化数值。计 算完成后，进行阈值异常检测，也即根据步骤(1)里的分段函数进行判断，如 果异常，则设置对应的传感器权重为0，反之则保持正常权重值，之后计算关于 权重的因子项之和，最后根据互补滤波器的姿态更新方程计算对应的姿态四元 数，进而求得估计的当前机体的姿态角度；
[0155]
最后，程序判断是否继续下一时刻矢量观测，重复完成上述(1)至(11) 步骤。
[0156]
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图 限制根据本技术的示例性实施方式。如在这里所使用的，除非上下文另外明确 指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说 明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器 件、组件和/或它们的组合。
[0157]
除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、 数字表达式和数值不限制本发明的范围。同时，应当明白，为了便于描述，附 图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领 域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下， 所述技术、方法和设备应当被视为授权说明书的一部分。在这里示出和讨论的 所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此， 示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在 下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后 的附图中不需要对其进行进一步讨论。
[0158]
在本发明的描述中，需要理解的是，方位词如“前、后、上、下、左、右”、
ꢀ“
横向、竖向、垂直、水平”和“顶、底”等所指示的方位或位置关系通常是 基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，在未 作相反说明的情况下，这些方位词并不指示和暗示所指的装置或元件必须具有 特定的方位或者以特定的方位构造和操作，因此不能理解为对本发明保护范围 的限制；方位词“内、外”是指相对于各部件本身的轮廓的内外。
[0159]
为了便于描述，在这里可以使用空间相对术语，如“在
……
之上”、“在
……ꢀ
上方”、“在
……
上表面”、“上面的”等，用来描述如在图中所示的一个器件或 特征与其他器件或特征的空间位置关系。应当理解的是，空间相对术语旨在包 含除了器件在图中所描述的方位之外的在使用或操作中的不同方位。例如，如 果附图中的器件被倒置，则描述为“在其他器件或构造上方”或“在其他器件 或构造之上”的器件之后将被定位为“在其他器件或构造下方”或“在其他器 件或构造之下”。因而，示例性术语“在
……
上方”可以包括“在
……
上方”和
ꢀ“
在
……
下方”两种方位。该器件也可以其他不同方式定位(旋转90度或处于 其他方位)，并且对这里所使用的空间相对描述作出相应解释。
[0160]
以上结合具体实施例描述了本发明的技术原理。这些描述只是为了解释本 发明的原理，而不能以任何方式解释为对本发明保护范围的限制。基于此处的 解释，本领域的技术人员不需要付出创造性的劳动即可联想到本发明的其它具 体实施方式，这些方式都将落入本发明的保护范围之内。