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

权利要求书

1.一种带有机械臂的水下四旋翼无人机控制方法，其特征在于，包括以下步骤：

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

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

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

	r]]>	d	a	r]]>
A1	90°	0	0	0
A2	180°	0	2L	90°

其中，A₁，A₂表示机械臂的两个关节，θ_r表示机械臂关节旋转变量，d表示关节滑动的变量，a表示关节偏移，α_r表示关节扭转，L为无人机质心到机械臂质心的距离；

由DH参数表变换顺序θ_r，d，a，α_r可得出无人机坐标系和机械臂坐标系间的齐次变换矩阵，继而得到机械臂的末端位姿方程；

通过设置无人机位置向量和机械臂位置向量之间的转换关系，从而得到机械臂的位置；根据所述的位姿方程以及机械臂的位置，以实现对机械臂的控制。