3 水下机器人控制系统设计
方案设计
水下机器人总体控制方案，如图3.1。水下机器人总体控制方案是设计的核心所在，控制系统主要包括水上控制器、姿态控制系统、图像控制系统三部分。姿态控制是对水下机器人进行姿态调整，包括水下机器人悬停控制、水下机器人前进、后退以及下潜等控制。图像控制主要是对于水下图像的采集，包括拍照、录像等操作。
图3.1 总体控制方案
Fig.3.1 General control plan
3.1.1姿态控制
姿态控制系统水下部分主要包括主控制器STM32、传感器的读取、包括姿态传感器、深度传感器、温度传感器，控制电机转速以及电灯亮灭。水上部分包括，STM32单片机、电子遥感、上位机控制软件组成。
机器人的运动控制系统需要完成的任务，包括机器人所有自由度的协调控制、机器
人与上位机软件的实时通讯、以及在自动模式下控制机器人进行自平衡调整。所示图3.2为姿态控制系统总体框图。
在这里插入图片描述
图3.2 姿态控制系统
Fig.3.2 Attitude control system
3.1.2电机伺服控制方式
水下机器人的控制具体实现方案，如图3.3，采用开环控制控制和闭环控制相结合的方式。
开环控制上位机可以直接控制水下机器人的运动，也可以利用控制箱内的摇杆，通过模-数转换器，把摇杆的模拟量转化成数字量传入STM32F407,再通过CAN总线传入水下的控制器直接控制PWM的占空比来控制电机的转速和转向。
闭环控制则是利用姿态传感器的反馈水下机器人姿态，然后对水下机器人进行位置控制，但为了获得更多和更高质量获得角度数据通常使用6轴姿态传感器（陀螺仪）即MPU6500，通过该传感器可以通过姿态解算算法的获得相对准确欧拉角，把欧拉角再传入姿态控制器，通过姿态解算出控制每个电机转动PWM调整水下机器人到合适的姿态。
图3.3 控制原理图
Fig.3.3 Control specific implementation
3.1.3 CAN总线通信
控制器局域网（CAN）是一种串行通信总线，最初由博世公司发明，后来编入ISO11898-1标准，可以定义开放系统互连（OSI）模型的数据链路和物理层，为高速车内通信提供一种低层的网络解决方案。特别是，CAN被开发用来减少电缆布线，因此车辆内的独立电子控制单元（ecu）只能与一对电线通信。
图3.4 CAN通信电路
Fig.3.4 CAN communication
CAN总线通信电路,如图3.4。水下机器人中的控制器获取机器人传感器数据通过CAN总线上传到上位机的控制。在开环控制时，上位机也把摇杆数据通过CAN传如水下控制器，从而控制水下机器人执行相应的运动。
3.2图像采集系统
3.2.1图像采集系统原理分析
图像采集系统原理，如图3.5，图像采集的主控板选择树莓派，树莓派通过控制舵机转动从而控制小型云台的位置，改变摄像头位置。摄像头则通过SCI接口直接连接到树莓派，通过远程连接，将电脑和树莓派相连，控制摄像头完成响应的动作。
图3.5 图像采集系统原理
Fig.3.5 Principle of image acquisition system
3.2.2图像采集系统电路图
图像采集系统电路图，如图3.6。主要由树莓派、摄像头以及舵机驱动的控制云台组成。
图3.6 图像采集系统电路接线图
Fig.3.6 Circuit diagram of image acquisition system
3.2.3 图像采集系统云台设计
图像采集云台，如图3.7，该云台由两个舵机组成，上边的舵机控制摄像头俯仰角度，下边舵机控制摄像头的方位角。使用30到150度的云台机制， 这个范围足够用于相机。
图3.7 图像采集云台
Fig.3.7 Image acquisition gimbal
3.3算法方案设计
3.3.1姿态解算方案
水下机器人姿态解算算法选用mahony互补滤波算法，算法的基本原理为，根据加速度计和地磁计的数据，转换到地理坐标系后，与对应参考的重力向量和地磁向量进行求误差，这个误差用来校正陀螺仪的输出，然后用陀螺仪数据进行四元数更新，再转换到欧拉角。
用陀螺仪解算出四元数，后用四元数推导出理论重力加速度，则实际重力加速的是由加速度计直接测得，由于陀螺仪是有偏差的所以推导出的理论重力加速度机与实际加速度之间有误差，要想用代数的方法量化误差的大小，则这里利用向量叉乘的思想量化得到误差的大小，再把误差补偿到陀螺仪上，这样反复的迭代最终得到一个准确的四元数，用一个比较准确的四元数推出一个比较准确的欧拉角，从而完成姿态的解算。
3.3.2姿态算法设计
姿态解算图，如图3.8，水下机器人接收遥控器发送来的姿态角，作为期望角度输入到控制系统中与姿态解算算法解算出的实际姿态角度进行求取偏差，分别经过各自的PID控制器进行PID运算，运算结果转化成电机调速的PWM方波来调节四个电机的转速，从而调节水下机器人的姿态和运动。调节后的姿态角又进过姿态反馈环反馈给了输入为下一次控制做准备。经过反复试验调试发现采用单级PID控制飞行姿态。水下机器人悬停则是姿态算法控制的一种特殊的情况，即期待的横滚角设定在零度。
图3.8 姿态控制原理图
Fig.3.8 Attitude algorithm control
PID是比例积分微分控制器（Proportional Plus Integral Plus Derivative Controller）的缩写，它是一种在工业控制系统中广泛应用的控制回路反馈机制（Controller），如图3.9。
比例积分微分（PID）控制是工业控制中最常用的控制算法，在工业控制中得到了广泛的应用。这是因为系统的所有设计规范都可以通过优化常数Kp、Ki和Kd来达到最大性能在自动过程。控制的早期历史中，PID控制器被用作船舶操纵系统中的机械装置 PID温度控制器广泛应用于工业烘箱、注塑机、烫金机、包装等行业。电子模拟PID控制回路通常出现在更复杂的电子系统中，例如磁盘驱动器的磁头定位、电源调节，甚至现代地震仪的运动检测电路。工业上的大多数现代PID控制器都是用可编程逻辑控制器（plc）或面板式数字控制器来实现的。但是PID控制器也有局限性：非线性系统（如暖通空调系统）中PID控制器的性能是可变的，因为PID控制器是线性的 导数项Kd易受噪声干扰。少量的测量或过程噪声会导致大量的输出变化。通常用低通滤波器对测量值进行滤波以去除高频噪声成分是有帮助的需要调整PID参数以获得所需的最佳响应不能很好地处理非对称系统（例如，加热速度比冷却速度快得多的系统）比例控制可快速、及时、按比例调节偏差，提高控制灵敏度，但有静差，控制精度低。
图3.9 PID控制框图
Fig.3.9 PID
PID控制器在水下机器人深度控制中的作用，如图3.10，以水深控制为例，用户设置水下机器人下潜的深度，通过观察传感器这里为深度传感器反馈过来的数据，经过PID运算，运算结果转化成电机调速的PWM方波来调节上边两个电机的转速，从而调节水下机器人的深度，调节后的深度又进行深度传感器的反馈给输入下一次控制做准备。使水下机器人深度比较精确的保持到设定值。
图3.10 PID在闭环系统的作用
Fig.3.8 The function of PID in closed loop system