[发明专利]基于几何姿态转换算法的水空两栖四旋翼无人机控制方法

权利要求书

1.一种基于几何姿态转换算法的水空两栖四旋翼无人机控制方法，其特征在于：采用的控制结构包括外位置环、几何姿态转换算法和内姿态环，所述外位置环实现位置跟踪，根据期望轨迹的位置点信息、机体当前位置ξ₁和当前速度v_I，计算出机体跟踪期望轨迹的期望推力值T_d和期望推力方向n_d；

所述几何姿态转换算法能够克服机体在空中、水中受力状态和运动形式差异影响，根据机体当前推力方向n、当前姿态q、期望推力方向n_d处理得到期望姿态q_d，不需要切换控制结构就能实现水空两栖四旋翼无人机空中水中的运动控制。

2.根据权利要求1所述的基于几何姿态转换算法的水空两栖四旋翼无人机控制方法，其特征在于：所述的内姿态环用于控制机体姿态，内姿态环根据机体当前角速度v₂、当前姿态q、期望姿态q_d解算得到期望力矩τ；期望推力值T_d和期望力矩τ再输出给水空两栖四旋翼无人机的电机，实现其在空中和水中运动的轨迹跟踪。

3.根据权利要求1或2所述的基于几何姿态转换算法的水空两栖四旋翼无人机控制方法，其特征在于：包括如下步骤：

步骤1、设置期望轨迹的参数；

步骤2、根据动力学模型计算期望推力大小T_d和推力方向n_d；

步骤3、几何姿态转换算法推导期望姿态q_d；

步骤4、姿态控制器计算期望力矩τ₂(τ_x，τ_y，τ_z)；

步骤5、调整机体的位置ξ₁。

4.根据权利要求3所述的基于几何姿态转换算法的水空两栖四旋翼无人机控制方法，其特征在于：步骤3所述的几何姿态转换算法推导期望姿态q_d，具体是：几何姿态转换算法根据期望推力方向n_d、机体当前推力方向n和当前姿态q，通过两次轴角旋转计算得到期望姿态q_d。

5.根据权利要求4所述的基于几何姿态转换算法的水空两栖四旋翼无人机控制方法，其特征在于：所述的几何姿态转换算法是基于四元数坐标转换和旋转轴角坐标转换方法提出的，具体步骤如下：

步骤3.1：计算第一次旋转q₁

由n到n_d的旋转轴角转换方法可知，旋转轴角可以定义为矢量轴u和旋转角度β，分别由如下公式计算得到：

β＝atan2(u·(n×n_d),n·n_d)

由旋转轴角定义单位四元数q₁

q₁表示当前推力方向n到期望推力方向n_d的旋转，有

式中，表示四元数乘法；

步骤3.2：计算过渡姿态q′_d

根据单位四元数乘法的定义，得到基于单位四元数的过渡姿态q′_d为

经过一次旋转，当前推力方向n沿路径l₁由到达期望推力方向n_d，这条路径比任何其它连接两个矢量终点的路径都要短，沿路径l₁也就是当前姿态q到过渡姿态q′_d的最短路径；过渡姿态q′_d能够保证期望的推力方向n_d，但机体偏航角不一定为0，还需要绕期望推力方向再旋转一次，保证机体偏航角趋近于0；

步骤3.3：计算第二次旋转q₂

由q′_d可以获取相应的旋转矩阵R_d(q′_d)，令n_bx n_bz为q′_d对应的体坐标系的OX_B轴和OZ_B轴的单位向量，则有n_bx＝R_d(q′_d)e₁、n_bz＝n_d＝R_d(q′_d)e₃；若将q′_d对应的体坐标系的OX_B轴绕OZ_B轴旋转到惯性坐标系的X_EO_EZ_E平面内，则能够保证偏航角为0；令X_EO_EZ_E平面内OX′_B为OX_B轴旋转所得到的，则OX′_B垂直于OZ_B轴，继而可以获得OX′_B轴方向的单位向量n′_bx；因而，为保证偏航角趋近于0，需要绕n_d旋转β′＝atan2(n_bz·(n_bx×n′_bx),n_bx·n′_bx)，有

q₂表示n_bx到n′_bx的旋转，有

步骤3.4：计算期望姿态q_d

因而，同时保证期望推力方向n_d和期望偏航角为0的期望姿态q_d为