[发明专利]一种水下无人机的定俯仰角运动控制方法及其装置

权利要求书

1.一种水下无人机的定俯仰角运动控制方法，其特征在于，包括如下步骤：

步骤S1、开启定俯仰角运动模式；

步骤S2、根据水下无人机当前飞行姿态，判定水下无人机的俯仰角；

步骤S3、根据水下无人机的俯仰角，将水下无人机当前的各推进器的推进力进行调整分配；

在步骤S3中，如果该俯仰角为0°，则采用普通推力分配矩阵将水下无人机当前的各推进器的推进力进行转换调整分配；普通推力分配矩阵为：

在步骤S3中，如果该俯仰角不为0°，则采用实时推力分配矩阵将水下无人机当前的各推进器的推进力进行转换调整分配；实时推力分配矩阵为：

在步骤S3中，如果该俯仰角为0°，水下无人机处于平衡状态，则其坐标定义为O-XYZ坐标系；如果俯仰角不为0°，水下无人机处于定俯仰角状态，则其坐标定义为O-xyz坐标系；O-XYZ坐标系与O-xyz坐标系的夹角的大小等于俯仰角的大小；

在步骤S3中，在水下无人机处于平衡状态下，在O-XYZ坐标系内，各方向的推进器的推进力分别为Fx-in、Fz-in和Nz-in；

在步骤S3中，在水下无人机处于定俯仰角状态下，在O-xyz坐标系内，各方向的推进器的推进力分别为Fx-in’、Fz-in’和Nz-in’；

在O-XYZ坐标系中的推进器的推进力的合力即可通过CP矩阵转换为O-xyz坐标系的推进器的推进力的合力，即为：

2.根据权利要求1所述的一种水下无人机的定俯仰角运动控制方法，其特征在于，在步骤S2中，水下无人机反馈其俯仰状态而确定其的纵倾角，该纵倾角为俯仰角。

3.一种水下无人机的定俯仰角运动控制装置，其特征在于，采用如权利要求1-2任意一项所述的控制方法，该控制装置包括用于提供动力的推进器、用于控制该推进器运行的定倾PID控制器和用于实时反馈水下无人机的俯仰角的定俯仰角运动控制器；所述定倾PID控制器分别与所述推进器、定俯仰角运动控制器电性连接。

4.根据权利要求3所述的一种水下无人机的定俯仰角运动控制装置，其特征在于，所述推进器的数量为6个，其中在水下无人机的左右两侧分别设置有2个推进器，在水下无人机的尾侧设置有2个推进器。

5.根据权利要求4所述的一种水下无人机的定俯仰角运动控制装置，其特征在于，所述定俯仰角运动控制器内设置有用于感应水下无人机的俯仰的感应模块和用于计算水下无人机的俯仰角度的检测模块。