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

摘要

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

主权项

1.一种带有机械臂的水下四旋翼无人机控制方法，其特征在于，包括以下步骤：通过无人机上的三轴陀螺仪得到无人机机体旋转的角速度，利用四元数求出无人机的旋转矩阵，根据旋转矩阵与无人机角速度的转换关系，不断以角速度实时更新四元数；利用四元数与欧拉角的转换公式，求出当前无人机的姿态角；将无人机上的三轴加速度计与三轴磁力计的测量向量反馈给旋转矩阵，从而得出姿态角的理想输出，以理想输出与实际输出的向量积作为误差，通过PI控制器将此误差加至利用角速度实时更新四元数的过程中，进行数据融合从而不断更新四元数，建立起无人机的位姿动力学模型；基于位姿动力学模型，继续进行四元数及姿态角的更新，更新过程中加入扩展卡尔曼滤波器，对四元数进行动态估计，以此补偿水下GPS信号不准确产生的误差；最后结合PID控制来实现对水下无人机位姿的控制；以一个二自由度爪型机械臂作为飞行载荷，机械臂具有两个关节，机械臂的手指部分用橡胶覆盖，其中2个机械手指是一体且静止的，另一个活动的机械手指在舵机的带动下运动，从而实现对物体的抓握与释放；将机械臂固定在无人机的质心上，使机械臂搭载稳定；用DH法设计该机械臂的DH参数表如下： θ_rdaα_rA190°000A2180°02L90°其中，A₁,A₂表示机械臂的两个关节，θ_r表示机械臂关节旋转变量，d表示关节滑动的变量，a表示关节偏移，α_r表示关节扭转，L为无人机质心到机械臂质心的距离；由DH参数表变换顺序θ_r,d,a,α_r可得出无人机坐标系和机械臂坐标系间的齐次变换矩阵，继而得到机械臂的末端位姿方程；通过设置无人机位置向量和机械臂位置向量之间的转换关系，从而得到机械臂的位置；根据所述的位姿方程以及机械臂的位置，以实现对机械臂的控制。