[发明专利]一种体感控制方法

权利要求书

1.一种体感控制方法，其特征在于，步骤包括：

S1，遥控器接收到体感激活命令时，获得遥控器当前姿态的初始矩阵DCM_init；

S2，实时获取遥控器姿态变化的当前矩阵DCM_cur；

S3，根据初始矩阵DCM_init和当前矩阵DCM_cur计算得到中间矩阵DCM_relative；

S4，将中间矩阵DCM_relative转换为最终矩阵DCM_final；

S5，将最终矩阵DCM_final换算成能够控制无人机姿态的控制量；

S6，遥控器与无人机通信，无人机根据所述控制量实施姿态调整。

2.根据权利要求1所述的体感控制方法，其特征在于，所述步骤S1具体包括：

当接收到体感激活命令之后，记录初始四元数q_init，将初始四元数q_init转换成初始矩阵DCM_init。

3.根据权利要求2所述的体感控制方法，其特征在于，所述步骤S2具体包括：

每间隔预定时间就记录一个当前四元数q_cur，并将当前四元数q_cur转换成当前矩阵DCM_cur。

4.根据权利要求3所述的体感控制方法，其特征在于，所述预定时间为10ms至20ms。

5.根据权利要求1所述的体感控制方法，其特征在于，所述中间矩阵DCM_relative为：

DCM_relative＝DCM_init^T*DCM_cur。

6.根据权利要求1所述的体感控制方法，其特征在于，所述步骤S4具体包括：

将中间矩阵DCM_relative旋转90度得到最终矩阵DCM_final。

7.根据权利要求1所述的体感控制方法，其特征在于，所述步骤S5具体包括：

将所述最终矩阵DCM_final转换成欧拉角，并根据所述欧拉角换算得到能够控制无人机姿态的控制量。

8.根据权利要求7所述的体感控制方法，其特征在于，遥控器上设有姿态传感器以姿态传感器中心为坐标原点O建立三维坐标系，所述欧拉角包括：

遥控器围绕X轴旋转值roll，遥控器围绕Y轴旋转值pitch，遥控器围绕Z轴旋转值yaw。

9.根据权利要求8所述的体感控制方法，其特征在于，所述遥控器围绕Y轴旋转值pitch为：

pitch＝arcsin(-DCM_final[2][0])。

10.根据权利要求9所述的体感控制方法，其特征在于，根据pitch的大小确定roll和yaw的值：

当时，则roll＝0，

yaw=arctan(DCMfinal[1][2]-DCMfinal[0][1]DCMfinal[0][2]+DCMfinal[1][1]);]]>

当时，则

yaw=arctan(DCMfinal[1][0]DCMfinal[0][0]).]]> 1