[发明专利]一种无人机自动投饵装置的控制方法及系统

权利要求书

1.一种投饵控制方法，其特征在于，应用于无人机投饵系统；

所述无人机投饵系统包括：实时建图与定位系统、用户虾塘料仓系统和飞行通讯控制系统；

所述实时建图与定位系统包括：摄像识别模块、定位模块和工控电脑；

所述摄像识别模块，用于拍摄虾塘图片信息，并识别其地理信息；

所述定位模块，用于对无人机自动投饵装置和虾塘建立坐标信息并提取位置信息；

所述工控电脑，用于根据虾塘的地理信息、位置信息和坐标信息建立虾塘三维地图，并将无人机自动投饵装置实时定位在所述虾塘三维地图内；

所述用户虾塘料仓系统包括：地面遥控模块和料仓；

所述地面遥控模块，用于与所述无人机自动投饵装置实现远程通讯；

所述料仓，用于储存饲料，并根据投饵需要对无人机自动投饵装置填装饲料；

飞行通讯控制系统包括：通讯模块、机载传感模块和无人机主控模块；

所述通讯模块，用于实现所述无人机自动投饵装置、所述工控电脑和地面遥控模块之间的通讯；

所述机载传感模块，其用于实时检测所述无人机自动投饵装置的航向、姿态、速度和位置信息；

所述无人机主控模块，用于接收实时建图与定位系统、用户虾塘料仓系统和机载传感模块的信息，对所述无人机自动投饵装置进行飞行控制和甩饵操作；

所述工控电脑与所述定位模块、地面遥控模块和摄像识别模块通讯连接；

所述无人机主控模块与地面遥控模块、机载传感模块、工控电脑和无人机自动投饵装置通讯连接；

所述无人机自动投饵装置包括：无人机本体、填料箱、水箱、水泵和漏斗搅拌器；

所述无人机本体为四旋翼无人机，其四根旋翼支撑臂的驱动端竖直设有固定轴，所述固定轴的上下两端分别设有一个旋翼驱动电机，一个旋翼驱动电机与固定轴顶部水平设置的第一旋翼传动连接，另一个旋翼驱动电机与固定轴底部水平设置的第二旋翼传动连接；

所述填料箱的一侧设有进水口，底部设有喷料口；

所述漏斗搅拌器包括漏斗状搅拌腔和设置于漏斗状搅拌腔内的搅拌组件；所述漏斗状搅拌腔的顶面设有进料口，其底端设甩料口；

所述填料箱可拆卸的设置于所述无人机本体的顶部；所述水泵的进液端与所述水箱连通，所述水泵的出液端与所述进水口连通；

所述漏斗搅拌器设置于所述喷料口的正下方；所述水箱内设有液位计，所述液位计用于检测所述水箱内液体存储量；所述填料箱的底部设有应变式压力传感器，所述应变式压力传感器用于检测所述填料箱内饲料的存储重量；

所述的无人机自动投饵装置还设有显示模块，所述显示模块与所述液位计和应变式压力传感器连接，所述显示模块用于显示所述水箱内液体的存储量和所述填料箱内饲料的存储重量；

投饵控制方法包括如下内容：

a.划定要被投饵的虾塘，无人机自动投饵装置的拍摄模块拍摄该虾塘的图片信息，工控电脑根据图片信息构建虾塘的三维图地图；

b.无人机自动投饵装置利用定位模块得到位置信息和坐标信息；

c.所述工控电脑根据三维图地图、定位信息和坐标信息及用户设定投饵要求得到信息反馈，并传输给用户虾塘料仓设有的用户终端；用户终端向工控电脑发送任务规划信息；

d.所述工控电脑向无人机主控模块发送指令，无人机飞行器装置起飞，在无人机主控的控制下对虾塘进行投饵操作；

所述步骤d中还包括如下内容：

d1.所述无人机主控模块通过接收机和/或5G模块接收用户终端发送的手动控制指令，所述无人机主控模块根据该手动控制指令控制无人机飞行器装置的飞行情况和甩饵操作；

d2.所述无人机主控模块实时接收记载传感发送的姿态控制指令进行矢量观测姿态评估算法处理和矢量姿态估计处理得到飞行姿态控制指令，所述无人机主控模块根据该飞行姿态控制指令调整无人机飞行器装置的飞行姿态，并在指定配料程序下完成泵水、搅拌和甩料操作；

所述步骤d2中矢量观测姿态评估算法处理包括如下内容：

(1)建立坐标系，所述无人机自动投饵装置的坐标系b为前左上坐标系，选取惯性参考坐标系n为东北天坐标系，定义分别如下：

b＝(x_b y_b z_b)^T

n＝(x_n y_n z_n)^T

采用Z-X-Y顺规次序，其中φ、θ、γ分别为对应的偏航角、俯仰角和横滚角，算出坐标系n到坐标系b的旋转矩阵如下：

令R定义为的矩阵元素如下，可分别解得对应的姿态角表达式：

(2)由坐标变换的四元数乘法表示式，计算坐标系b到坐标系n的四元数旋转矩阵表示即：

其中r^b、rⁿ为零标量的四元数，M(Q)、M^*(Q)为对应的四元数矩阵表示形式，则得到坐标系b到坐标系n的四元数旋转矩阵如下：

根据旋转矩阵性质，得到从坐标系n到坐标系b的四元数旋转矩阵，即：

(3)加速度计和磁力计矢量分别观测得到ⁿa＝(a_x a_y a_z)^Τ和ⁿm＝(m_x m_y m_z)^Τ，将两者分别转化到机体坐标系得到矢量方程，如下：

(4)转化为Wahba’s代价函数，即求得误差最小时旋转矩阵R

J(R)＝argmin(λ_a||^ba-R·ⁿa||²+λ_m||^bm-R·ⁿm||²)

其中，λ_a，λ_m分别为加速度计和磁力计的权重信任值，且两者之和为1；将其转化为四元数q的形式，p₁(q)，p₂(q)，p₃(q)为四元数运算过程时的矩阵因子，则分别有：

f(q,a)＝[a_x·p₁(q)+a_y·p₂(q)+a_z·p₃(q)]q-^ba

f(q,m)＝[m_x·p₁(q)+m_y·p₂(q)+m_z·p₃(q)]q-^bm

再将其转化为关于求解误差最小时四元数q的代价函数，即：

(5)梯度下降优化，由梯度下降公式

其中q_obs,k为当前时刻观测得到四元数，q_obs,k-1为上一时刻观测得到的四元数，η为梯度下降步长；优化得到如下的梯度更新表达式：

q_obs,k＝q_obs,k-1-2η·Σ(q_obs,k-1)·q_obs,k-1

其中，Σ(q_obs,k-1)为优化运算过程的权重因子项；

(6)陀螺仪采样得到的机体系三轴原始角速度数据^bw，即：

^bw＝(w_x w_y w_z)，由四元数微分更新方程：

其中，q_mes,k为陀螺仪当前时刻直接测量求解得到的四元数；

(7)根据线性观测器，构造互补滤波器，即可得到如下的姿态估计更新方程：

并对其进行单位化处理，得到：

其中，L为观测器的反馈增益矩阵，其值为经验对角常数矩阵，△T为陀螺仪采样三轴角速度时的采样周期，G为互补滤波器的增益，且G＝(L^-1+I)^-1,[^bw]_×为陀螺仪采集的三轴数据构成的反对称矩阵，为当前时刻估计的四元数输出，为上一时刻估计的四元数输出；

(8)最后根据四元数对应的旋转矩阵，求解对应的姿态角φ、θ、γ得到：

所述步骤d2中矢量姿态估计处理包括如下内容：

(9)先对程序的参数完成初始化，初始操作包括互补增益的G的确定、初始姿态q_init的给定、梯度下降步长η的设置、陀螺仪的采样周期△T、加速度计和磁力计的传感器的权重信任值λ_a、λ_m给定、加速度计和磁力计的标准模值SNorm以及阈值异常检验时的阈值μ_a,μ_m；其中标准模值SNorm的值是先通过对加速度计和磁力计在没有剧烈运动，且在平滑环境多次运行下离线处理计算得到的常数值，阈值则根据实验多次设定的一个关于投饵无人机实际运行时的矢量观测得到模值Norm与标准模值差的绝对值的一个界限,即如下分段函数表达式：

(10)判断是否有捷联式观测矢量输入，即加速度计和磁力计在机体系采集的传感数据输入，如果没有，则按初始姿态输出结束，反之则继续对给定的参考矢量进行单位化，单位化完成后判断陀螺仪是否能得到有效响应校验，如果没有，则令互补增益G＝I(I为单位矩阵)，如果有，则按数值正常计算互补增益G；

(11)实时计算捷联式观测矢量的模Norm和它对应的实时单位化数值；计算完成后，进行阈值异常检测，也即根据步骤(1)里的分段函数进行判断，如果异常，则设置对应的传感器权重为0，反之则保持正常权重值，之后计算关于权重的因子项之和，最后根据互补滤波器的姿态更新方程计算对应的姿态四元数，进而求得估计的当前机体的姿态角度；

(12)程序判断是否继续下一时刻矢量观测，重复完成(1)至(11)步骤。