[发明专利]一种带有机械臂的水下四旋翼无人机控制方法

说明书

本发明公开了一种带有机械臂的水下四旋翼无人机控制方法，在无人机水下飞行的基础上做出了水下应用的创新，提出了无人机机身与机械臂协同控制的方法，并提出了一种带有机械臂的水下无人机机械结构。相较于现有技术大多为单纯无人机水下飞行控制技术，本发明提供水下无人机的复合实时更新反馈动力学模型控制方法，以拓宽了水下无人机的应用前景，在水下无人机应用方面技术优势明显，具备更好的实用性。

技术领域

本发明涉及无人机控制技术领域，具体涉及一种带有机械臂的水下四旋翼无人机控制方法。

背景技术

带有机械臂的水下无人机是一种在某种环境下可以依靠水下摄影实现侦查及抓取的机器人：主要应用复杂工作环境中，在特殊环境勘探、取样和灾害探测等领域具有重要应用。

在目前的水下无人机运动控制中，由于水下无人机的姿态平衡控制难度较高，目前市面上大部分的水下无人机仅能实现无人机水下飞行，无法实现附带设备时稳定姿态平衡控制，这样会导致水下无人机上的附带设备无法正常工作，故目前水下无人机无法有效控制携带的抓取设备，只能粗略勘探不能取样。

因此，如何提供一种能够协同控制无人机机身及附带设备的水下无人机控制方法是本领域技术人员目前需要解决的技术问题。

发明内容

本发明的目的是提供一种带有机械臂的水下四旋翼无人机控制方法，保证水下无人机的稳定飞行要求，同时结合机械臂控制实现水下无人机勘探、取样一步化，解决目前存在的技术问题。

为了实现上述任务，本发明采用以下技术方案：

一种带有机械臂的水下四旋翼无人机控制方法，包括以下步骤：

通过无人机上的三轴陀螺仪得到无人机机体旋转的角速度，利用四元数求出无人机的旋转矩阵，根据旋转矩阵与无人机角速度的转换关系，不断以角速度实时更新四元数；利用四元数与欧拉角的转换公式，求出当前无人机的姿态角；将无人机上的三轴加速度计与三轴磁力计的测量向量反馈给旋转矩阵，从而得出姿态角的理想输出，以理想输出与实际输出的向量积作为误差，通过PI控制器将此误差加至利用角速度实时更新四元数的过程中，进行数据融合从而不断更新四元数，建立起无人机的位姿动力学模型；基于位姿动力学模型，继续进行四元数及姿态角的更新，更新过程中加入扩展卡尔曼滤波器，对四元数进行动态估计，以此补偿水下GPS信号不准确产生的误差，最后结合PID控制来实现对水下无人机位姿的控制；

以一个二自由度爪型机械臂作为飞行载荷，机械臂具有两个关节，机械臂的手指部分用橡胶覆盖，其中2个机械手指是一体且静止的，另一个活动的机械手指在舵机的带动下运动，从而实现对物体的抓握与释放；将机械臂固定在无人机的质心上，使机械臂搭载稳定；

用DH法设计该机械臂的DH参数表如下：