[发明专利]水下机器人位姿控制方法

权利要求书

1.一种水下机器人位姿控制方法，其特征在于：

采用平衡结构设计，对对称的框架结构进行建模，

传感器测得机器人姿态的俯仰角、翻滚角、偏航角；

与期望值做差值得到误差，通过PID算法对误差进行调节，对电机进行控制，使姿态保持平衡。

2.如权利要求1所述的水下机器人位姿控制方法，其特征在于：平衡结构为重心向下、对称的自稳定框架结构。

3.如权利要求2所述的水下机器人位姿控制方法，其特征在于，平衡结构具体为：采用方形框架结构，八个电机对称的安装在框架上，形成对称的结构，八个电机分成两个部分，

其中，四个电机分别竖直的安装在框架的四个直立的杆子上，通过正反转控制机器人的上升和下降；另外四个电机安装在框架的底部，控制机器人的前进后退。

4.如权利要求3所述的水下机器人位姿控制方法，其特征在于：传感器系统包括MPU6050三轴陀螺仪、加速度计、AK8975电子罗盘、HSTL-18液位变送器；MPU6050三轴陀螺仪、加速度计测得机器人在x,y,z轴上旋转的角度以及加速度，经过转换成四元数；AK8975电子罗盘测得机器人方向的磁偏角；HSTL-18液位变送器测量出机器人所在水中的深度。

5.如权利要求4所述的水下机器人位姿控制方法，其特征在于：

仿照四旋翼飞行器对水下机器人进行数学建模；传感器MPU6050三轴陀螺仪与加速度计，AK8975电子罗盘，HSTL-18液位变送器分别测出机器人姿态的四元数，偏航角和深度；利用PID算法对误差调节，输出量控制电机，使机器人的位姿保持平稳。

6.如权利要求1-5任一项所述的水下机器人位姿控制方法，其特征在于，实现水下机器人姿态的控制：

首先进行数学建模，采用欧拉角与四元数两种工具相结合的方法描述机器人在水下的姿态，先用传感器测定机器人姿态的四元数(w,x,y,z)，再将四元数转换成欧拉角得到偏航角(ψ)，俯仰角(θ)，翻滚角(Φ)；竖直放置的电机控制俯仰角、翻滚角，当机器人的姿态不是水平时，俯仰角和翻滚角就不是零，输入的俯仰角和翻滚角就是误差；

然后通过PID算法控制四个竖直方向的电机，将俯仰角和翻滚角调整为零。

7.如权利要求6所述的水下机器人位姿控制方法，其特征在于，利用如下公式(1)将四元数转换成欧拉角得到偏航角(ψ)，俯仰角(θ)，翻滚角(Φ)，

8.如权利要求7所述的水下机器人位姿控制方法，其特征在于，当机器人偏离任务水深时，液位变送器所测的深度与原来的深度相比较得到误差，然后利用PID算法，使机器人保持在任务水深。

9.如权利要求8所述的水下机器人位姿控制方法，其特征在于，当机器人偏离理想直线时，电子罗盘的数值发生变化，这就是方向的误差，利用PID算法，对误差进行纠正，确保机器人运动保持理想直线状态。