水下机器人位姿控制方法
【专利摘要】本发明提供一种水下机器人位姿控制方法，采用平衡结构设计，对对称的框架结构进行建模，传感器测得机器人姿态的俯仰角、翻滚角、偏航角；与期望值做差值得到误差，通过PID算法对误差进行调节，对电机进行控制，使姿态保持平衡；该种水下机器人位姿控制方法，可以使机器人在水下具有能抵抗环境扰动的姿态稳定性，确保机器人顺利的完成细致，复杂的工作。
【专利说明】水下机器人位姿控制方法
【技术领域】
[0001] 本发明涉及一种水下机器人位姿控制方法。
【背景技术】
[0002] 近年来，水下机器人的研究逐渐成为航海研究中的新的前沿和热点的问题。水下 机器人在深海中的应用前景也随着海底探矿，海底电缆维护的发展而更为宽广，同时对水 下机器人机动性和操纵性的要求也越来越高，在水下机器人执行任务的过程中，不但要求 其在环境扰动下能够按照预定的轨迹运动，而且在许多情况下需要利用水下机器人对目标 物进行更细致的观察和操作，这就需要水下机器人相对于目标物的位置保持不变，即要求 水下机器人具有能够抵抗环境扰动的较高的姿态稳定性，这样才能顺利的完成细致、复杂 的工作。
[0003] 但是目前，许多水下机器人在水下姿态自动调整方面没有投入太大的精力，机器 人定点悬浮，抵抗扰流的能力较弱。机器人在水下完成的任务有限。
【发明内容】
[0004] 本发明的目的是提供一种水下机器人位姿控制方法，能够实现机器人在水中的姿 态平稳，解决现有技术中存在的机器人定点悬浮，抵抗扰流的能力较弱，机器人在水下完成 的任务有限的问题。
[0005] 水下机器人，又称无人潜水器，是一种作业于水下的极限作业机器人；
[0006] 位姿：位姿分为位置和姿态，是描述机器人在空间中的位置和其自身的姿态；
[0007] 欧拉角：确定定点转动刚体位置的3个一组独立角参量，分别有偏航角Ψ，俯仰角 Θ，翻滚角Φ。
[0008] 本发明的技术解决方案是：
[0009] 一种水下机器人位姿控制方法，
[0010] 采用平衡结构设计，对对称的框架结构进行建模，
[0011] 传感器测得机器人姿态的俯仰角、翻滚角、偏航角；
[0012] 与期望值做差值得到误差，通过PID算法对误差进行调节，对电机进行控制，使姿 态保持平衡。
[0013] 进一步地，平衡结构为重心向下、对称的自稳定框架结构。
[0014] 进一步地，平衡结构具体为：采用方形框架结构，八个电机对称的安装在框架上， 形成对称的结构，八个电机分成两个部分，
[0015] 其中，四个电机分别坚直的安装在框架的四个直立的杆子上，通过正反转控制机 器人的上升和下降；另外四个电机安装在框架的底部，控制机器人的前进后退。
[0016] 进一步地，传感器系统包括MPU6050三轴陀螺仪、加速度计、AK8975电子罗盘、 HSTL-18液位变送器；MPU6050三轴陀螺仪、加速度计测得机器人在X，y，z轴上旋转的角度 以及加速度，经过转换成四元数；AK8975电子罗盘测得机器人方向的磁偏角；HSTL-18液位 变送器测量出机器人所在水中的深度。
[0017] 进一步地，仿照四旋翼飞行器对水下机器人进行数学建模；传感器MPU6050三轴 陀螺仪与加速度计，AK8975电子罗盘，HSTL-18液位变送器分别测出机器人姿态的四元数， 偏航角和深度；利用PID算法对误差调节，输出量控制电机，使机器人的位姿保持平稳。
[0018] 进一步地，实现水下机器人姿态的控制：
[0019] 首先进行数学建模，采用欧拉角与四元数两种工具相结合的方法描述机器人在水 下的姿态，先用传感器测定机器人姿态的四元数（《，X，y，Z)，再将四元数转换成欧拉角得到 偏航角（Ψ)，俯仰角（Θ)，翻滚角（Φ);坚直放置的电机控制俯仰角、翻滚角，当机器人的 姿态不是水平时，俯仰角和翻滚角就不是零，输入的俯仰角和翻滚角就是误差；
[0020] 然后通过PID算法控制四个坚直方向的电机，将俯仰角和翻滚角调整为零。
[0021] 进一步地，利用如下公式（1)将四元数转换成欧拉角得到偏航角（Ψ)，俯仰角 (Θ)，翻滚角（Φ)，
【权利要求】
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)将四 元数转换成欧拉角得到偏航角（Ψ)，俯仰角（Θ)，翻滚角（Φ)，
(1)〇
8. 如权利要求7所述的水下机器人位姿控制方法，其特征在于，当机器人偏离任务水 深时，液位变送器所测的深度与原来的深度相比较得到误差，然后利用PID算法，使机器人 保持在任务水深。
9. 如权利要求8所述的水下机器人位姿控制方法，其特征在于，当机器人偏离理想直 线时，电子罗盘的数值发生变化，这就是方向的误差，利用PID算法，对误差进行纠正，确保 机器人运动保持理想直线状态。
【文档编号】G05D1/08GK104155991SQ201410422945
【公开日】2014年11月19日 申请日期:2014年8月25日 优先权日:2014年8月25日
【发明者】陈巍, 汤忠强, 罗浩珏, 陈丝雨 申请人:南京工程学院