1、中国海洋大学工程学院 机械电子工程研究生课程考核论文 题目 ： AUV 水下机器人 运动控制系统 研究 报告 课程名称： 运动控制 技术 姓 名： 李思乐 学 号： 21100933077 院 系： 工程学院机电工程系 专 业： 机械电子工程 时 间： 2010-12-26 课程成绩： 任课老师： 谭俊哲 AUV 水下机器人 运动控制系统 设计 摘要： 以主推加舵控制的小型自治水下机器人为研究对象，建立了水下机器人的数学模型并进行了分析。根据机器人结构的特点，对模型进行了必要的简化。设计了机器人的运动控制系统。 以成功研制的无缆自治水下机器人 (AUV) 为基础 ， 对其航行控制和定位控制方法
2、进行了较详细的分析 . 同时介绍了它的推进器布置、控制系统结构、推力分配等方法 。 最后展示了它的运行实验结果 。 关键词： 水下机器人； 总体设计方案；运动 控制系统 ；电机仿真 1 引言 近年来国外水下机器人技术发展迅速，技术水平较高。其中，具 有代表性的产品有：美国 Video Ray 公司开发出的 Scout、 Explorer、 Pro 等系列遥控式水下机器人，美国Seabotix 公司研发的 LBV-ROV 系列，英国 AC-CESS 公司的 AC-ROV 系列。 随着海洋开发、探测的需求越来越强，水下机器人成为全世界研究的热门课题。小型自治水下机器人具有低成本、小型化、操作灵活等
3、特点成为近年来国内外研究的热点。自治水下机器人 （ Autonomous Underwater Vehicles, AUV） ,载体采用模块化设计思想 , 可根据需要适当增减作业或传感器模 块 , 载体采用鱼雷状流线外形 , 总长约 2 m, 外径 25 cm, 基本模块包括推进器模块、能源模块、电子舱模块、传感器模块以及 GPS、无线电通讯模块 , 基本传感器有姿态传感器、高度计、深度计和视觉传感器 , 支持光纤通讯 , 载体可外挂声学设备 , 通过光纤系统进行遥控操作可实现其半自主作业 , 也可在预编程指令下实现自主作业。系统基本模块组成设计如图 1-1 所示 1。 它具有开放式、模块化的
4、体系结构和多种控制方式（自主 /半自主 /遥控），自带能源。这种小型水下机器人可在大范围、大深度和复杂海洋环境下进行海洋科学研究 和深海资源调查，具有更广泛的应用前景。在控制系统的设计过程中充分考虑了系统的稳定性和操纵性。控制器具有足够的鲁棒性来克服建模误差，以及水动力参数变化。 图 1-1 系统基本模块组成设计 2 机器人 物理 模型 2.1 AUV 物理模型 为了研究 AUV 的运动规律，确定运行过程中 AUV 的位置和姿态，需要建立 AUV 的动力学模型。为了便于分析，建立适合于描述 AUV 运动的两种参考坐标系，即固定坐标系 E 和运动坐标系 Oxyz，如图 2-1 所示 ： 包含 5
5、 个推进器，分别是艉部的 2 个主推进器、艉部的 1 个垂向 推进器和艏部的 2 个垂向推进器。左右对称于纵中剖面，上和下、前和后都不对称 2。 图 2-1 AUV 水下机器人 物理模型 1.2 微小型水下机器人动力学分析 微小型水下机器人总长 1.5m，采用锂电池作为能源，尾部为一对水平舵和一对垂直舵，单桨推进，可携带惯导设备、探测声纳、水下摄像机、深度计等设备，设计巡航速度约 2 节。首先建立适合描述水下机器人空间运动的坐标系，其定义如图 2-2 所示，惯性坐标系为 E ，运动坐标系为 o xyz 。 建立的坐标系， 如图 1 所示。图中： E- 惯性坐标系； Oxyz 载体坐标系。因为机
6、器人在航行时速度不高（ 4 节），可以对机器人模型进行线性化及一些简化。载体坐标系原点取于载体浮心处，在此坐标系下，载体在三个方向上的受力及运动量表达为：力： F=X,Y,ZT 力矩： M=K,M,NT 速度： V=u,v,wT 角速率： =p,q,rT。 图 2-2 惯性和载体坐标系 在图 2-2 定义的惯性坐标系和运动坐标系中，机器人的空间运动向量表达为： 1= x y z T； 2= T 1= u v w T； 2= p q r T 式中： 向量 1 机器人在惯性坐标系中的位置； 2 其在惯性坐标系中的姿态； 横滚角； 俯仰角； 航向角； 1 机器人在载体坐标系中的线速度（ V）；2 其
7、在载体坐标系中的转动角速度（） 3。 2 总体方案设计 2.1 系统组成及工作原理 小型水下观测机器人主要由人机交互平台、上位系统、下位系统、摄像机四部分组成，操作人员通过有线遥控，结合人机交互界面上的水下视频图像，只需扳动上位系统控制面板上相应的运动控制按钮即可实现对水下机器人的运动控制，操作简单、实用 。图 2-3 为 水下机器人控制系统框图 ： 图 2-3 水下机器人控制系统框图 2.2 导航系统设计 捷联惯性导航是最常见和应用最广泛的导航系统， 捷联式惯性导航系统在工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰破坏，是一种自主式导航系统。 但单独使用很难满足水下航行所需的导航精
8、度与定位要求，仅靠提高惯性传感器的性能来提高的导航、定位精度是非常有限的。组合导航系统融合不同类型的导航传感器的信息，使它们优势互补，经过卡尔曼滤波，得出系统导航参数的最优估计， 以获得比使用单一导航系统更高的性能和导航精度。采用磁罗盘和深度计分别与捷联惯导系统构成的组合量 测值作为卡尔曼滤波的量测值， 既可以用精度高的子系统的信息修正惯导误差， 又可以用惯导对动态响应慢的子系统作补偿和校正，从而综合发挥各自优点。导航计算机在保证导航运算速度和精度的同时， 还要具有丰富的外设接口，方便与外部多传感器进行数据通信 4。 导航系统的传感器包括惯性测量器件 IMU （陀螺仪和加速度计）、磁罗盘、深度
9、计。其中 IMU 通过三陀螺仪、三加速度计捷联解算后获得位置、速度、姿态共 9 维信息，通过 RS232 串口与导航计算机相连 。 以 NEMA0183 格式传输信息到导航计算机。磁罗盘可以获 取当前载体三维姿态信息，通过 RS232 串口与导航计算机相连，以NEMA0183 格式传输信息到导航计算机。深度计为液压变送器，通过膜片感应内外侧水压差来确定水深，其输出为 4 20mA 模拟电流信号， 转换为 0 5V 的电压信号后经过 16 位 ADC 转换模块，串行传送到导航计算机。 如 图 2-4 所示。 图 2-4 组合导航系统总体设计框图 2.3 驱动方式的选用 几乎所有的水下机器人都采用
10、螺旋桨式推进器。 80% 以上采用电机推进器，其余采用油压电机推进器。水下机器人要实现水下空间的六维（六自由度）运动，即三个 平移运动：推进（ Surge，沿 x 轴）、升沉（ Heave，沿 z 轴）、横移（ Sway，沿 y 轴）和三个回转运动：转首（ Yaw，绕 z 轴）、纵倾（ Pitch，绕 y 轴）、横摇（ Roll，绕 x 轴）。为使水下机器人在所有六维上的运动都是可控的，须适当选用推进器的数量和给予不同的布置。根据本水下机器人的使用目的，不需要使用六维运动，只要三个自由度即可，即推进、升沉和转首。我们选用了 五 个直流电机推进器，分别布置在机器人本体的水平左右两侧和 后部 部垂
11、直处，左右推进器完成推进和转首两个动作，垂推进器完成升沉动作。 2.4 推力器的组成 推力器是 由 电机和螺旋桨组成的，水下机器人用的电机需要密封。密封主要有两种方式，一种是机械密封，另一种采用磁耦合器。机械密封相对而言比较简单，但因密封处要承受海水的压力，其特性因摩擦力的增加而变坏。对电机来说，则表现为电机的空载电流增大（有时会增大 1-3 倍），这样的电机用于推力器，会使启动电压升高，从而加重推力器非线性。为了改善这种情况可以采用充油电机 ，由于电机内部充油，因而耐水压的性能得到极大的改善，而且电机因密封而引起的摩擦力要小得多，其空载电流的增加也很小，故可以忽略不计。 采用磁耦合器就是利用
12、电磁力传递扭矩，这样减速器和螺旋桨之间没有直接的机械联系。依据磁场传递扭矩，密封问题很容易解决，只要用非导磁材料将电机、减速器包围起来就解决了动密封难题。采用磁耦合器，推力器的效率略有下降，但性能基本上不受影响。电机的转速与螺旋桨的转速不一定完全匹配，为了得到较高的效率，需要采用减速器，有时为了减小尺寸，采用高速电机（例如采用 10000 转 /分以上的高速电机），这时也需要减速器。这样组成的推力器如图 2-5 所示。 图 2-5 推力器组成 图 2-6 螺旋桨与推进器示意图 考 虑带定子的导管桨在无限宽广的静止流体域中工作的情况 。 设流体为理想且不可压缩 。 如图 2-6 所示 ， 建立固
13、定于导管上的直角坐标系 O-xyz， 以螺旋桨桨叶参考线与桨轴交点为原点 ， x 轴与桨轴中心线重合 ， 指向桨的下游 ， y 轴垂直向上 ， z 轴方向由右手法则确定 。 推进器工作时 ， 导管、定子与桨共同沿 x 轴负方向以匀速 V0 前进 ，同时桨叶绕 x 轴以等角速度 旋转 5。 2.5 能源供给方式的选用 其能源供给方式有两种选择：有缆方式或无缆方式，对于无缆水下机器人能源供给一般在机器人舱体安装蓄电池或是带燃油发电机组，这就造成水 下机器人本体体积庞大、超重，此外蓄电池所储存的能力有限，且受电池质量、充电工艺等因素的影响。根据实际应用环境，此机器人工作所要求的行走距离不是很大，故设
14、计时采用了有缆远程遥控方式，这样既可减小本体尺寸、重量，又保证了控制操作的有效性和可靠性，当设备出现不可预料的故障时可通过缆线撤回安全区域，不至于丢失。 3 控制系统设计 3.1 水下机器人控制系统设计 主要包含主处理器核心模块、电机驱动模块、传感器模块和视频切换模块等，实现对机器人推进器的动力驱动、上下位机的通讯以及视频图像的切换等。 水下机器人本体的左 右两边各安装 2 个主推进器，分别由 2 个直流电机通过联动轴与螺旋桨相连，实现水下机器人前进、后退、左转、右转运动；垂直方向安装有 3个垂推进器，实现机器人上升、下沉运动。前变焦摄像机安装有垂直方向一维云台，避免摄像死区 。 在机器人电子
15、舱内安装有深度计、温度计和数字式电子罗盘传感器，满足实际作业环境需要，为检修人员提供了丰富的作业环境信息 。 根据功能需要，我们选择了 TI 公司推出的 MSP430 系列的 MSP430F149 作为主处理器，这是一类具有 16 位总线的带 FLASH 的单片机，由于其性价比和集成度高 ，受到了广大技术开发人员的青睐。该控制器可以在超低功耗模式下工作，对环境和人体的辐射小，可靠性能好，加强电干扰运行不受影响，适应工业级的运行环境 6。利用MSP430F149 定时器 B 比较单元产生的 6 路 PWM 信号和 5 路方向信号，分别控制主推进器、垂推进器、机械手、摄像机云台电机速度和照明灯亮度
16、；两路串口实现了罗盘数据的采集和上位系统的通讯；外部传感器反馈的模拟信息通过 ADC 模块实现转化，使芯片丰富的外设资源得以充分利用。 总 线型结构的所有节点都共享一个公共的物理通道 (即总线 )。具有延迟小、速度快 、易扩展、单个节点故障影响小的优点。本系统即采用总线型拓扑结构 , 系统采用单片机作为控制单元完成机器人控制系统中的各种控制任务 (如传感器控制、电机驱动器控制和通信模块控制等 )。各功能单元直接挂接在 CAN 总线上 ,成为控制网络中的一个节点，从而构成多主机结构 ,即每个节点均为一个主机，通过 CAN 通信协议协同完成控制任务。将 CAN 总线的分布式控制网络应用于水下机器人
17、中。总线上各节点完成不同的任务和功能，它们大部分时间并行工作，必要时通过总线与其他节点进行通信。如图 3-1所示。 CAN 总线是一种多主总线 ,理论上任何一个 节点都可以作为主节点。在控制系统中设置与上位 PC 机相连的节点 1 为上位节点 ,其它节点为底层节点。 PC 机通过串口与节点 1 上的 CPU 通信 , CPU 再与 CAN 收发器通信 ,实现信息在 CAN 总线上的发送与接收。节点 1 与 12 个底层节点根据应用的不同具有不同的功能 ,但它们都具有与 CAN 总线通信的能力 ,上传数据和接收数据。 图 3-1 CAN 总线通信节点的硬件结构及接口设计框图 CAN 总线上各节点
18、可采用 Philips 公司生产的单片机 P87C591，除完成节点自身的控制功能外，还实现了 CAN 通信接口。 P87C591 兼容 80C51 指令集，并成功地集成了Philips 公司的 SJAl000 CAN 控制器，该嵌入式 CAN 控制器具有以下特点： (1)完全符合 CAN 2. 0 规范，控制 CAN 帧的接收和发送； (2) CAN 接口包含 5 个实现 CPU 与 CAN 控制器连接的特殊功能寄存器； (3) CAN 控制器的发送缓冲区能够保存一个完整的 CAN 信息帧 (扩展或标准帧 )，通过 CPU 启动发送 ,信息就从发送缓冲区传输到 CAN 内核模块； (4)当接
19、收一个信息时， CAN 内核模块将串行数据流转换成并行数据传输到验收滤波器 ,通过该可编程滤波器 ， P87C591 确认接收需要的信息。图 3-2 为以 P87C591 为核心的 CAN 总线接口电路。 P1. 0、 P1. 1 分别对应 CAN 的收 (RxDC)和发 (TxDC)线。 图 3-2 CAN 总线接口电路 CAN 总线收发器选用 Philips 公司的 PCA82C250，以差分方式发送。引脚 8 与地之间的电阻为斜率电阻，其取值决定了系统处于高速方式、斜率控制方式或待机方式。本系统中 PCA82C250 工作于斜率方式，通过 Rs 对地连接的电阻对总线进行斜率控制，以控制射
20、频干扰。使 用双绞线作为传输介质。通过高速光耦 6N137 将 P87C591 与 CAN总线收发器进行光电隔离，以增强 CAN 总线节点的抗干扰能力。总线末端加 124的终端匹配电阻，以减少信号反射干扰 6。 3.2 水上控制箱控制系统设计 设计中，我们采用了常见的小型控制箱作为岸上控制平台，其外形尺寸为 372mm 266mm 134mm，具有体积小，携带方便的特点。其内部硬件模块主要包含主处理器核心模块和液晶显示模块，完成上下位机间的通讯，视频图像、传感器信息的显示，实现人机交互。其结构框图如图 3-3 所示。 图 3-3 水上控制箱控制系统 在该控制系统中采用 MSP430F135 作
21、为主处理器，充分应用了其内部集成的 14 路12 位模数转换器和串口通讯模块，操作人员只需通过设定控制面板上各按键、摇杆，经 AD 转换，将各模拟量转换为数字量，经串口通讯模块来实现控制箱对下位控制系统各推进器速度、方向的控制、照明灯的亮度调节以及摄像机云台的运动设定。 3.3 控制系统软件设计 上位系统程序主要完成系统上电初始化，控制面板各按键、摇杆信息的采集，液晶模块显示，与下位系统的通讯等工作。它采用顺序式结构往复运行。运行过程中 可以被中断子程序中断，执行完中断子程序之后返回断点处继续执行主程序。下位系统程序主要完成电机速度、方向设定 ，各传感器信息的采集，与上位系统的通讯等工作。同上
22、位系统一样， 也采用了顺序结构往复运行。系统上电后， MSP430F149 微处理器首先进入系统初始化程序，包括时钟配置、 I/O 口的初始化、外设模块的初始化等。初始化结束后，开中断，处理器进入串口接收数据的判断循环当中 7。程序流程图如图 3-5 所示。 图 3-5 下位系统程序流程图 4 控制算法 4. 1 转艏控制 选用正确的控制方法是保证控制系 统稳定控制的重要环节 。 采用神经网络控制、自适应控制、 PID 控制算法等都可以达到潜器稳定运行的目的 。 具体的方法需根据不同的载体结构、不同的控制对象、不同的任务要求而确定 。 图 4-1 速度控制 经 用多普勒声学测试原理可以获得水下
23、机器人相对于海底或流层的速度，对速度积分后就可以得到行程，这就是多普勒计程仪的基本原理。 速度闭环控制回路的结构如图4-1 所示。构成航行速度闭环后可以较为精确地控制水下机器人在海底的航行速度。 我们采用分段线性 PID 控制算法 ， 即根据潜器运行的速度不同采用不同的 PID 参数 . 离散后的 PID 控制算法为 ： (1) 潜器在不同的速度下可采用不同的控制参数值 ， 经过试验即可确定 Kp、 Ki、 Kd 的数值 .在上式中 ， 如果微分项采用 ( ek- ek- 1) / T 直接计算 ， 由于采样周期较短 , 因此会产生较大的噪声信号 ， 进而影响控制效果 。 为了避免这种情况的发
24、生 ,可采用最小二乘法对角速度进行滤波平滑 ， 然后再求出角速度测量值 ， 这样可获得较好的控制效果 。 4. 2 深度 ( 高度 ) 和纵倾控制 深度 ( 高度 ) 和纵倾控制又称为垂直面的控制 。 潜器在航行过程中 ， 其高度或深 度的变化通常是在运动中实现的 。 一但要求的高度或深度有变化 ， 则必然会引起纵倾角的变化 ; 而纵倾角的变化也常常引起高度或深度的变化 。 因此潜器在垂直面的控制实际上是一个多输入多输出系统 。 这里 A0 为给定矩阵 ， A 为反馈矩阵 ， E 为误差矩阵 ， H0、 P 0 分别为给定的高度和纵倾角 ， H、 P 分别为采集的高度和纵倾角 。 将控制算法的矩阵解耦后 ， 其输出 Pn 的计算方法如下 : A0 =H0 P0 A =H P E =e1 e2=H0 H P0 P (2) 其中 , f 1 和 f 2 分别为 e1 和 e2 的函数 , f ( x) 为其它有关变量的函数 。 根据我们以往的工作经验 ， 采用自适应控制算法、神经网络控制算法、 PID 控制算法等均能达
