水下采捕装置运动姿态控制方法研究

蔡卫国，武立波，李明智，魏鸿磊

（1.大连海洋大学 机械与动力工程学院，大连 116023；2.大连海洋大学 航海与船舶工程学院，大连 116023）

0 引言

随着近海养殖业的蓬勃发展，近海捕捞所面临的问题日益突出。近海养殖的海珍品主要有扇贝、海参、鲍鱼等，传统的近海捕捞方式以人工下潜作业为主，辅以拖网和定置网，不但劳动强度大、效率低、成本高，而且对海底的生态环境造成了严重破坏。近几年，我国的水下机器人研究发展迅猛，如我国自行研制的“蛟龙号”载人潜水器，可以实施深海勘探、海底作业等任务[1]。水下机器人技术的迅速发展为海珍品的生态采捕提供了技术支持和可能，复杂的水下环境对机器人性能提出了更高的要求，防倾覆控制是其完成特殊任务的前提和保障，是水下机器人关键技术之一[2～4]。本文首先确定水下采捕装置的运动姿态控制系统方案，通过单片机对采捕装置上三轴陀螺仪和三轴加速度计传感器的姿态数据进行采样，经过卡尔曼滤波处理、PID调节控制电机驱动器实现对螺旋桨电机的速度控制，从而实现了采捕装置的平衡控制[5～7]。

1 总体方案设计

水下采捕装置的试验模型如图1所示，其运动由四个螺旋桨控制，可以实现装置的上浮和下潜。 三轴加速度计和三轴陀螺仪与水下采捕装置的三轴重合并固定在水下采捕装置内部。 当水下采捕装置的姿态发生变化时，加速度计能够读出重力加速度在X,Y,Z三轴的分量，通过计算可得到每一轴与原始坐标系的夹角，同时可以通过陀螺仪读出各轴的角加速度，经过卡尔曼滤波融合就可以得到精确的水下采捕装置的姿态。再将姿态角度作为变量通过PID编程控制，将角度的变化量转化为控制水下采捕装置平衡电机的转速，从而纠正它在水中的姿态。通过加速度陀螺仪实时采集数据不断改变电机转速，这样就实现了水下采捕装置的平衡控制。

图1 试验模型

2 控制系统方案

控制系统总体方案如图2所示。

图2 控制系统方案

2.1 硬件结构

水下采捕装置运动姿态控制主要以单片机ATmega328P为控制核心，主要采用了Arduino Pro Nimi微处理模块、三轴加速度和三轴陀螺仪MPU6050模块、L298电机驱动器模块和Arduino IDE编程软件。主要辅助功能包括自检、校准、低电压报警等，硬件电路接线如图3所示。

图3 硬件电路接线图

2.2 控制电路设计

水下采捕装置运动姿态控制电路采用模块化设计，主要包括Arduino Pro Nimi微处理模块、三轴加速度+三轴陀螺仪MPU6050模块、L298电机驱动器模块和Arduino IDE编程软件。

2.2.1 Arduino Pro Mini微处理器模块

水下采捕装置通过以单片机ATmega328P为控制核心的Arduino Pro Nimi模块来实现平衡控制功能。因为它的尺寸小、重量轻、价格便宜，适合安装在空间较小的环境，唯一的缺点是它缺少用于编程的USB接口，需要一块TTL/USB适配器模块来进行编程。ATmega328单片机可以提供32k的储存空间、2k RAM和1k的EEPROM。

2.2.2 三轴加速度+三轴陀螺仪MPU6050模块

MPU-6050模块作为整合性六轴运动处理组件，与多组件方案相比消除了组合陀螺仪与加速度计的轴间差问题，以单一数据流的形式向应用端输出完整的9轴融合的运动处理资料库，可对运动感测的复杂数据进行处理，从而降低了运动处理运算对操作系统的负荷。连接电路如图4所示。

2.2.3 L298N直流电机/步进电机驱动模块

L298N是ST公司生产的一种高电压、大电流电机驱动芯片。该芯片采用了15脚封装，它的主要特点是：工作电压高、输出电流大，内部含有两个H桥的高电压大电流全桥式驱动器，用来驱动直流电动机或步进电动机。L298N芯片驱动电机可以驱动一台两相步进电机或四相步进电机或两台直流电机。连接电路如图5所示。

3 实验结果

本次实验主要测试卡尔曼滤波效果，测试对象为加速度计、陀螺仪采集角度曲线，实验过程以水上测试为主，通过串行口监视窗口数据，对数据稍作调试即可进行水下调试。测试指标为：

1）通过串行口输出的角度无漂移且变化平滑；

2）通过串行口输出的电机转速控制量与角度的关系满足预期PID控制输出量与角度关系，不断调整Kp、Kd、Ki参数是电机转速与角度的变化与所处的环境相适应。

图4 MPU6050模块连接电路

图5 L298N驱动模块连接电路

加速度传感器要矫正偏移，先读取若干数值，然后将数值取平均值，该平均值就是传感器当前状态下的偏移，在后面的角度积分中要先用原始加速度值减掉对应的偏移。加速度计经过卡尔曼滤波后采集的角度曲线变得平滑，如图6所示。

图6 加速度计滤除偏移量后的角度曲线

如图7所示，经过卡尔曼滤波后的陀螺仪三轴角度变化曲线，漂移消除且曲线变化比较平滑。

图7 陀螺仪+卡尔曼滤波后采集角度曲线

4 结论

针对水下采捕装置的运动稳定性问题，通过单片机对采捕装置上陀螺仪和加速度计传感器的姿态数据进行采样，经过卡尔曼滤波处理、PID调节控制电机驱动器，从而对螺旋桨电机的速度控制，实现了采捕装置的平衡控制。实验结果初步实现了倾斜超过设定的平衡角度范围时，四个螺旋桨转动产生扶正力矩，而且随着倾斜角度的增加转速也会增加，从而增大扶正力矩。尚需解决的问题是：所选电机启动转矩小，导致在倾斜时电机不能同一时间启动，且它们的转速也与程序控制量有偏差，电机转速的代数和不为零会使其产生旋转力矩。

[1] 徐玉如,李彭超,周俊.水下机器人发展趋势[J].自然杂志,2011,33(3):125-131.

[2] 蒋新松,封锡盛,王棣棠.水下机器人[M].沈阳:辽宁科学技术出版社,2002.

[3] 徐玉如,庞永杰,甘永,孙玉山.智能水下机器人技术展望[J].智能系统学报,2006,1(1):9-16.

[4] 朱玉华,袁瑞琴.基于嵌入式系统的水下机器人航向控制[J].自动化应用,2013,(9):8-10,39.

[5] 魏延辉,彭富国,盛超,周卫祥.自主式水下机器人运动稳定性控制方法[J].华中科技大学学报（自然科学版）,2014,42(2):127-132.

[6] Weiguo Cai,Zhenguo Shang,Libo Wu. Optimal Design of a Four-legged Walking Mechanism Based on Bionics[A].Applied Mechanics and Materials Vols[C].496-500(2014) pp719-723.

[7] 程晗,王内,寿晓波.基于姿态角递增算法的6轴机器人通用运动规划方法[J].制造业自动化,2015,37(5):72-74.