[发明专利]一种运用无人机进行海上态势感知监测船舶路径的方法

权利要求书

1.一种运用无人机进行海上态势感知监测船舶路径的方法，其特征在于包括以下步骤：

步骤一、构建MATLAB核心单元

船舶计划路径的数据文件由MATLAB核心代码单元加载，船舶的起始位置B点作为NED坐标系的原点，根据坐标原点定义相应的纬度和经度；按顺序，定义航点的更新速率，以及船舶应从无人机接收有关其航路的信息的时间；

无人机的位置，速度和空中姿态信息用作解决二维优化问题的输入，假设无人机将飞行在恒定的高度；有关无人机的以下信息来自DUNE单元：

●机体在地球坐标系中的起始位置A点，包括经度lat，纬度lon和高度alt；

●机体运动中自身位置C点相对于NED坐标系原点B点的位置偏差(x,y,z)；

●机体坐标系下二维线速度(u,v)；

●机体横滚角φ，纵摇角θ，偏航角ψ；

●机体坐标系下角速度(p,q,r)；

船舶的起始位置B点用作NED坐标系的坐标原点，将无人机在大地坐标系下的位置C点转换为NED坐标系下的位置坐标D点；首先使用无人机所在的大地坐标系将无人机的纬度和经度坐标变换，然后将无人机所在的大地坐标的原点A点变换到船舶NED坐标系下船舶所在位置B点；无人机的大地坐标系的原点A点一次变换移动一公里距离，直至大地坐标系的原点A点移动到船舶所在的NED坐标系原点B点；所有坐标变换以1984年的世界大地测量系统WGS 84作为参考椭球；

在无人机控制优化问题中使用二维模型，利用(1)中欧拉角速率矢量与机体固定角速度矢量[p,q,r]^T之间的关系计算偏航角速度；

使用二维模型，从(1)获得的为实际偏航角速度r，将新r记作r_new

无人机的位置和航向在NED坐标系中表示为

p_UAV＝[x_UAV,y_UAV,ψ]^T (2)

其中x_UAV和y_UAV水平位置，ψ是偏航角；机体坐标系下的速度和偏航角的变化率组成以下矢量

v_UAV＝[u,v,r]^T (3)

(2)与(3)式的关系为

其中R_z(ψ)为机体坐标系与NED坐标系之间的旋转矩阵，如(5)所示

系统的约束是无人机的最大转弯率(r)，ψ是无人机的偏航角；

在无人机飞越航迹点上某位置的瞬间，基于船舶在其预定路径中的位置和速度构建线性运动模型

其中是t时刻船舶在路径上的位置，是t+τ时刻船舶在路径上的位置，τ是两次测量之间的时间间隔，船舶t时刻的速度v_x和v_y分别是船舶在纵荡和横荡方向上的分速度；

用最小二乘函数(7)用作MPC算法的目标函数，求解最优化问题

使用模型预测控制解决优化问题后，使用“Desired Path()”函数将目标路径点发送到DUNE；使用该函数发送第二个路径点作为备份，以便在出现临时通信问题时使用，提高系统的冗余；程序重复每个选定的更新时间步骤，并一直运行，直到无人机到达船舶的最后一个计划位置；模块间通讯协议Java库向MATLAB提供必要的函数，核心代码作为模块间通讯协议节点启动，连接到所需的飞行器节点；优化问题使用ACADO for MATLAB实现，用C++实现；

步骤二、构建DUNE和Ardupilot单元

构建DUNE模块与不同的自动驾驶仪进行兼容连接；使用自动驾驶仪ArduPilot，DUNE接收“DesiredPath()”命令，调用负责与ArduPilot通信的任务；DUNE的ArduPilot任务发生变化，无人机在任务模式下运行，在新的任务中，一个包含路径点的任务发送到自动驾驶仪；从自动驾驶仪确认后，命令开始任务然后发送；在无监测任务的情况下，DUNE存储库中可用的标准任务在导航模式下操作UAV进行待机操作；

步骤三、构建状态估计器单元和地面基站单元

状态估计器与自动驾驶仪ArduPilot协同工作；运用基于局部线性化的广义卡尔曼滤波器、贝叶斯或极大后验估模拟无人机在真实飞行中的行为；

地面基站发出导航模式的控制命令，调用DUNE存储库中可用的标准任务操作UAV，基站保证各节点之间通讯要求。