[发明专利]一种水下无人船航行控制方法及系统

权利要求书

1.一种水下无人船航行控制系统，其特征在于，包括，控制器、驱动模块和姿态获取模块，所述控制器与驱动模块和姿态获取模块相连，所述控制器根据接收到的航行命令和/或姿态获取模块获取的姿态数据，利用驱动模块调整水下无人船的航行姿态；

所述姿态获取模块设置在电路板上，包括，陀螺仪、加速度计和磁强计，所述姿态数据包括，所述陀螺仪检测的平衡数据、所述加速度计检测的水下无人船的加速度数据和磁强计检测的方位数据；

所述电路板上设有至少两个磁强计，所述至少两个磁强计重合堆叠放置或以电路板为对称平面对称设置在电路板两侧；

所述姿态数据包括，所述陀螺仪检测的平衡数据、所述加速度计检测的水下无人船的加速度数据和磁强计检测的方位数据；

通过两个磁强计对检测的方位信息进行校准，然后将校准结果作为磁强计检测的最终方位信息结果，控制器在进行校准计算时只针对垂直方向进行相应计算校准，以便于减少控制器的计算量，加快计算速率，提高水下无人船方位检测的准确性；或者，将两个磁强计整合成一体，进而使两个磁强计检测的偏差进一步减小；

对磁强计进行零偏校正和椭圆校正，然后再利用校正后的磁强计获取水下无人船在东、南、西、北四个方向中所处的方位，并将该方位数据与利用陀螺仪和加速度计获得的俯仰速度及滚转角速度进行结合，确定出水下无人船的当前航行方向；

所述加速度计进行姿态补偿，之后将获取的加速度数据去除重力项得到去重加速度数据，对获取的三轴的去重加速度数据进行积分，确定出水下无人船的当前航行速度；

对磁强计进行标定，对数据进行校准，之后刨除俯仰角和滚转角对测量值的影响，最后计算得到无人船偏航角的估计值；

通过陀螺仪得到旋转角速度数据，通过加速度计得到加速度数据，测量的角速度数据和加速度数据均在无人船本体坐标系下，根据重力加速度在本体坐标系的分量估算无人船相对于参考坐标系的俯仰角和滚转角；

陀螺仪测量无人船俯仰角和滚转角的变化，俯仰角和滚转角由角度的估计值和角度在一个周期内的变化值，通过卡尔曼滤波，得到角度的最小方差估计；

对所述陀螺仪进行零偏校正，利用零偏校正后的陀螺仪获取平衡数据，利用所述加速度计检测的加速度数据计算俯仰速度和滚转角速度，并将平衡数据和俯仰速度及滚转角速度进行结合确定水下无人船的俯仰姿态和滚转姿态；

水下无人船航行控制系统还包括智能跟随模块，所述智能跟随模块与所述控制器相连，所述控制器根据对移动目标的跟随命令获取移动目标的位置信息，并利用智能跟随模块控制驱动模块对移动目标进行跟随；

所述水下无人船航行控制系统还包括与控制器相连的全球定位模块，能够实时获取水下无人船的位置信息；

水下无人船航行控制系统还包括与控制器相连的通信模块，所述控制器将接收的姿态数据通过通信模块发送至水上控制端，并利用水上控制端上的显示屏将姿态数据进行显示，在所述显示屏上显示水下无人船的模型，并将姿态数据通过所述水下无人船的模型进行直观展示。

2.根据权利要求1所述的水下无人船航行控制系统，其特征在于，所述驱动模块包括，设置在水下无人船重心前方的垂直推进器、分别设置在水下无人船尾部两侧的水平推进器，其中，垂直推进器调整水下无人船的垂直方向的运动，两个水平推进器控制水下无人船的前进、后退和转弯。

3.一种包括如权利要求1-2任一所述水下无人船航行控制系统的无人船控制方法，其特征在于，步骤包括，

S1，实时获取水下无人船的姿态数据；

S2，根据姿态数据确定当前航行方向并与目标航行方向进行比较，确定偏移量；

S3，根据偏移量计算驱动模块的调整数据，并根据调整数据调整水下无人船的航行姿态；

其中，所述姿态数据包括陀螺仪检测的平衡数据、加速度计检测的加速度数据、磁强计检测的方位数据；根据所述平衡数据和加速度数据确定水下无人船的俯仰数据和滚转数据。