AUV水下机器人运动控制系统设计(李思乐)

中国海洋大学工程学院

机械电子工程研究生课程考核论文

题目：AUV水下机器人运动控制系统研究报告

课程名称：运动控制技术

姓名：李思乐

学号：21100933077

院系：工程学院机电工程系

专业：机械电子工程

时间：2010-12-26

课程成绩：

任课老师：谭俊哲

AUV水下机器人运动控制系统设计

摘要：以主推加舵控制的小型自治水下机器人为研究对象，建立了水下机器人的数学模型并进行了分析。根据机器人结构的特点，对模型进行了必要的简化。设计了机器人的运动控制系统。以成功研制的无缆自治水下机器人(AUV) 为基础，对其航行控制和定位控制方法进行了较详细的分析. 同时介绍了它的推进器布置、控制系统结构、推力分配等方法。最后展示了它的运行实验结果。

关键词：水下机器人；总体设计方案；运动控制系统；电机仿真

1 引言

近年来国外水下机器人技术发展迅速，技术水平较高。其中，具有代表性的产品有：美国Video Ray 公司开发出的Scout、Explorer、Pro 等系列遥控式水下机器人，美国Seabotix公司研发的LBV-ROV 系列，英国AC-CESS 公司的AC-ROV系列。

随着海洋开发、探测的需求越来越强，水下机器人成为全世界研究的热门课题。小型自治水下机器人具有低成本、小型化、操作灵活等特点成为近年来国内外研究的热点。自治水下机器人（Autonomous Underwater Vehicles, AUV）,载体采用模块化设计思想, 可根据需要适当增减作业或传感器模块, 载体采用鱼雷状流线外形, 总长约2 m, 外径25 cm, 基本模块包括推进器模块、能源模块、电子舱模块、传感器模块以及GPS、无线电通讯模块, 基本传感器有姿态传感器、高度计、深度计和视觉传感器, 支持光纤通讯, 载体可外挂声学设备, 通过光纤系统进行遥控操作可实现其半自主作业, 也可在预编程指令下实现自主作业。系统基本模块组成设计如图1-1 所示[1]。它具有开放式、模块化的体系结构和多种控制方式（自主/半自主/遥控），自带能源。这种小型水下机器人可在大范围、大深度和复杂海洋环境下进行海洋科学研究和深海资源调查，具有更广泛的应用前景。在控制系统的设计过程中充分考虑了系统的稳定性和操纵性。控制器具有足够的鲁棒性来克服建模误差，以及水动力参数变化。

图1-1 系统基本模块组成设计

2 机器人物理模型

2.1 AUV 物理模型

为了研究AUV 的运动规律，确定运行过程中AUV 的位置和姿态，需要建立AUV

的动力学模型。为了便于分析，建立适合于描述AUV 运动的两种参考坐标系，即固定坐标系Eξηζ 和运动坐标系Oxyz，如图2-1 所示：包含5 个推进器，分别是艉部的2 个主推进器、艉部的1 个垂向推进器和艏部的2 个垂向推进器。左右对称于纵中剖面，上和下、前和后都不对称[2]。

图2-1 AUV水下机器人物理模型

1.2 微小型水下机器人动力学分析

微小型水下机器人总长 1.5m，采用锂电池作为能源，尾部为一对水平舵和一对垂直舵，单桨推进，可携带惯导设备、探测声纳、水下摄像机、深度计等设备，设计巡航速度约2 节。首先建立适合描述水下机器人空间运动的坐标系，其定义如图2-2 所示，惯性坐标系为E ?ξη? ，运动坐标系为o ? xyz 。

建立的坐标系，如图1 所示。图中：E-ξηζ—惯性坐标系；Oxyz—载体坐标系。因为机器人在航行时速度不高（<4 节），可以对机器人模型进行线性化及一些简化。载体坐标系原点取于载体浮心处，在此坐标系下，载体在三个方向上的受力及运动量表达为：力：F=[X,Y,Z]T力矩：M=[K,M,N]T速度：V=[u,v,w]T角速率：ω=[p,q,r]T。

图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—其在载体坐标系中的转动角速度（ω）[3]。

2 总体方案设计

2.1 系统组成及工作原理

小型水下观测机器人主要由人机交互平台、上位系统、下位系统、摄像机四部分组成，操作人员通过有线遥控，结合人机交互界面上的水下视频图像，只需扳动上位系统控制面板上相应的运动控制按钮即可实现对水下机器人的运动控制，操作简单、实用。图2-3为水下机器人控制系统框图：

图2-3 水下机器人控制系统框图

2.2导航系统设计

捷联惯性导航是最常见和应用最广泛的导航系统，捷联式惯性导航系统在工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰破坏，是一种自主式导航系统。但单独使用很难满足水下航行所需的导航精度与定位要求，仅靠提高惯性传感器的性能来提高的导航、定位精度是非常有限的。组合导航系统融合不同类型的导航传感器的信息，使它们优势互补，经过卡尔曼滤波，得出系统导航参数的最优估计，以获得比使用单一导航系统更高的性能和导航精度。采用磁罗盘和深度计分别与捷联惯导系统构成的组合量测值作为卡尔曼滤波的量测值，既可以用精度高的子系统的信息修正惯导误差，又可以用惯导对动态响应慢的子系统作补偿和校正，从而综合发挥各自优点。导航计算机在保证导航运算速度和精度的同时，还要具有丰富的外设接口，方便与外部多传感器进行数据通信[4]。

导航系统的传感器包括惯性测量器件IMU （陀螺仪和加速度计）、磁罗盘、深度计。其中IMU 通过三陀螺仪、三加速度计捷联解算后获得位置、速度、姿态共9 维信息，通过RS232 串口与导航计算机相连。以NEMA0183 格式传输信息到导航计算机。磁罗盘可以获取当前载体三维姿态信息，通过RS232 串口与导航计算机相连，以NEMA0183 格式传输信息到导航计算机。深度计为液压变送器，通过膜片感应内外侧水压差来确定水深，其输出为4～20mA 模拟电流信号，转换为0～5V 的电压信号后经过16 位ADC 转换模块，串行传送到导航计算机。如图2-4所示。

图2-4组合导航系统总体设计框图

2.3 驱动方式的选用

几乎所有的水下机器人都采用螺旋桨式推进器。80% 以上采用电机推进器，其余采用油压电机推进器。水下机器人要实现水下空间的六维（六自由度）运动，即三个平移运动：推进（Surge，沿x 轴）、升沉（Heave，沿z 轴）、横移（Sway，沿y 轴）和三个回转运动：转首（Yaw，绕z 轴）、纵倾（Pitch，绕y 轴）、横摇（Roll，绕x 轴）。为使水下机器人在所有六维上的运动都是可控的，须适当选用推进器的数量和给予不同的布置。根据本水下机器人的使用目的，不需要使用六维运动，只要三个自由度即可，即推进、升沉和转首。我们选用了五个直流电机推进器，分别布置在机器人本体的水平左右两侧和后部部垂直处，左右推进器完成推进和转首两个动作，垂推进器完成升沉动作。

2.4 推力器的组成

推力器是由电机和螺旋桨组成的，水下机器人用的电机需要密封。密封主要有两种方式，一种是机械密封，另一种采用磁耦合器。机械密封相对而言比较简单，但因密封处要承受海水的压力，其特性因摩擦力的增加而变坏。对电机来说，则表现为电机的空载电流增大（有时会增大1-3倍），这样的电机用于推力器，会使启动电压升高，从而加重推力器非线性。为了改善这种情况可以采用充油电机，由于电机内部充油，因而耐水压的性能得到极大的改善，而且电机因密封而引起的摩擦力要小得多，其空载电流的增加也很小，故可以忽略不计。

采用磁耦合器就是利用电磁力传递扭矩，这样减速器和螺旋桨之间没有直接的机械联系。依据磁场传递扭矩，密封问题很容易解决，只要用非导磁材料将电机、减速器包围起来就解决了动密封难题。采用磁耦合器，推力器的效率略有下降，但性能基本上不受影响。电机的转速与螺旋桨的转速不一定完全匹配，为了得到较高的效率，需要采用减速器，有时为了减小尺寸，采用高速电机（例如采用10000转/分以上的高速电机），这时也需要减速器。这样组成的推力器如图2-5所示。

图2-5 推力器组成

图2-6 螺旋桨与推进器示意图

考虑带定子的导管桨在无限宽广的静止流体域中工作的情况。设流体为理想且不可压缩。如图2-6所示，建立固定于导管上的直角坐标系O-xyz，以螺旋桨桨叶参考线与桨轴交点为原点，x 轴与桨轴中心线重合，指向桨的下游，y 轴垂直向上，z 轴方向由右手法则确定。推进器工作时，导管、定子与桨共同沿x 轴负方向以匀速V0 前进，同时桨叶绕x 轴以等角速度Ω旋转[5]。

2.5 能源供给方式的选用

其能源供给方式有两种选择：有缆方式或无缆方式，对于无缆水下机器人能源供给一般在机器人舱体安装蓄电池或是带燃油发电机组，这就造成水下机器人本体体积庞大、超重，此外蓄电池所储存的能力有限，且受电池质量、充电工艺等因素的影响。根据实际应用环境，此机器人工作所要求的行走距离不是很大，故设计时采用了有缆远程遥控方式，这样既可减小本体尺寸、重量，又保证了控制操作的有效性和可靠性，当设备出现不可预料的故障时可通过缆线撤回安全区域，不至于丢失。

3 控制系统设计

3.1 水下机器人控制系统设计

主要包含主处理器核心模块、电机驱动模块、传感器模块和视频切换模块等，实现对机器人推进器的动力驱动、上下位机的通讯以及视频图像的切换等。

水下机器人本体的左右两边各安装2个主推进器，分别由2 个直流电机通过联动轴与螺旋桨相连，实现水下机器人前进、后退、左转、右转运动；垂直方向安装有3个垂推进器，实现机器人上升、下沉运动。前变焦摄像机安装有垂直方向一维云台，避免摄像死区。在机器人电子舱内安装有深度计、温度计和数字式电子罗盘传感器，满足实际作业环境需要，为检修人员提供了丰富的作业环境信息。

根据功能需要，我们选择了TI 公司推出的MSP430 系列的MSP430F149 作为主

处理器，这是一类具有16 位总线的带FLASH 的单片机，由于其性价比和集成度高，受到了广大技术开发人员的青睐。该控制器可以在超低功耗模式下工作，对环境和人体的辐射小，可靠性能好，加强电干扰运行不受影响，适应工业级的运行环境[6]。利用MSP430F149 定时器B 比较单元产生的6 路PWM 信号和5 路方向信号，分别控制主推进器、垂推进器、机械手、摄像机云台电机速度和照明灯亮度；两路串口实现了罗盘数据的采集和上位系统的通讯；外部传感器反馈的模拟信息通过ADC 模块实现转化，使芯片丰富的外设资源得以充分利用。

总线型结构的所有节点都共享一个公共的物理通道(即总线)。具有延迟小、速度快、易扩展、单个节点故障影响小的优点。本系统即采用总线型拓扑结构, 系统采用单片机作为控制单元完成机器人控制系统中的各种控制任务(如传感器控制、电机驱动器控制和通信模块控制等)。各功能单元直接挂接在CAN总线上,成为控制网络中的一个节点，从而构成多主机结构,即每个节点均为一个主机，通过CAN通信协议协同完成控制任务。将CAN总线的分布式控制网络应用于水下机器人中。总线上各节点完成不同的任务和功能，它们大部分时间并行工作，必要时通过总线与其他节点进行通信。如图3-1所示。CAN总线是一种多主总线,理论上任何一个节点都可以作为主节点。在控制系统中设置与上位PC机相连的节点1为上位节点,其它节点为底层节点。PC机通过串口与节点1上的CPU通信, CPU再与CAN收发器通信,实现信息在CAN总线上的发送与接收。节点1与12个底层节点根据应用的不同具有不同的功能,但它们都具有与CAN总线通信的能力,上传数据和接收数据。

图3-1 CAN 总线通信节点的硬件结构及接口设计框图

CAN总线上各节点可采用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)当接收一个信息时，CAN内核模块将串行数据流转换成并行数据传输到验收滤波器,通过该可编程滤波器，P87C591确认接收需要的信息。图3-2为以P87C591为核心的CAN总线接口电路。P1. 0、P1. 1分别对应CAN的收(RxDC)和发(TxDC)线。

图3-2 CAN总线接口电路

CAN总线收发器选用Philips公司的PCA82C250，以差分方式发送。引脚8与地之间的电阻为斜率电阻，其取值决定了系统处于高速方式、斜率控制方式或待机方式。本系统中PCA82C250工作于斜率方式，通过Rs对地连接的电阻对总线进行斜率控制，以控制射频干扰。使用双绞线作为传输介质。通过高速光耦6N137将P87C591与CAN 总线收发器进行光电隔离，以增强CAN总线节点的抗干扰能力。总线末端加124Ω的终端匹配电阻，以减少信号反射干扰[6]。

3.2 水上控制箱控制系统设计

设计中，我们采用了常见的小型控制箱作为岸上控制平台，其外形尺寸为372mm 3266mm3134mm，具有体积小，携带方便的特点。其内部硬件模块主要包含主处理器核心模块和液晶显示模块，完成上下位机间的通讯，视频图像、传感器信息的显示，实现人机交互。其结构框图如图3-3所示。

图3-3 水上控制箱控制系统

在该控制系统中采用MSP430F135 作为主处理器，充分应用了其内部集成的14 路12 位模数转换器和串口通讯模块，操作人员只需通过设定控制面板上各按键、摇杆，经AD 转换，将各模拟量转换为数字量，经串口通讯模块来实现控制箱对下位控制系统各推进器速度、方向的控制、照明灯的亮度调节以及摄像机云台的运动设定。

3.3 控制系统软件设计

上位系统程序主要完成系统上电初始化，控制面板各按键、摇杆信息的采集，液晶模块显示，与下位系统的通讯等工作。它采用顺序式结构往复运行。运行过程中可以被中断子程序中断，执行完中断子程序之后返回断点处继续执行主程序。下位系统程序主要完成电机速度、方向设定，各传感器信息的采集，与上位系统的通讯等工作。同上位系统一样，也采用了顺序结构往复运行。系统上电后，MSP430F149 微处理器首先进入系统初始化程序，包括时钟配置、I/O 口的初始化、外设模块的初始化等。初始化结束后，开中断，处理器进入串口接收数据的判断循环当中[7]。程序流程图如图3-5 所示。

图3-5 下位系统程序流程图

4 控制算法

4. 1 转艏控制

选用正确的控制方法是保证控制系统稳定控制的重要环节。采用神经网络控制、自适应控制、PID 控制算法等都可以达到潜器稳定运行的目的。具体的方法需根据不同的载体结构、不同的控制对象、不同的任务要求而确定。

图4-1 速度控制

经用多普勒声学测试原理可以获得水下机器人相对于海底或流层的速度，对速度积分后就可以得到行程，这就是多普勒计程仪的基本原理。速度闭环控制回路的结构如图4-1所示。构成航行速度闭环后可以较为精确地控制水下机器人在海底的航行速度。

我们采用分段线性PID 控制算法，即根据潜器运行的速度不同采用不同的PID 参数. 离散后的PID控制算法为：

(1)

潜器在不同的速度下可采用不同的控制参数值，经过试验即可确定Kp、Ki、Kd 的数值.在上式中，如果微分项采用( ek- ek- 1) / T 直接计算，由于采样周期较短, 因此会产生较大的噪声信号，进而影响控制效果。为了避免这种情况的发生,可采用最小二乘法对角速度进行滤波平滑，然后再求出角速度测量值，这样可获得较好的控制效果。

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 控制算法等均能达

到相应的控制要求，定深控制方法与定高控制方法相似,垂直控制量是一个力矩，它被用来改变AUV 的纵倾角。为了节省能源, 垂直控制通常是在AUV 的前进中实现的，即由纵倾角的改变而引起上升和下潜。

5电机系统仿真

5.1 海洋水下机器人电动机的特点

海洋水下机器人电动机除具有不同的电气性能和结构参数外, 还必须具备耐海水腐蚀性、耐水压机械结构以及可靠的密封结构。海洋水下机器人的驱动方式分为电力传动和液力传动, 供电方式分为电缆供电和蓄电池( 镍氢电池、银锌电池和燃料电池等) , 供电电压为几十伏到几千伏, 功率为几百瓦到一百多千瓦, 交流异步电动机用于大功率液力传动, 永磁钕铁硼直流电动机则用于小功率多台电机电力传动, 近年来又在研制开发无刷直流电动机, 使之更适于充油式平衡压力结构。

中小型水下机器人大多用电动机直接连接螺旋桨。可以用直流电机、交流电机。直流电机成本低，调速、控制系统简单，而交流电机需要逆变器把直流变成交流，成本高，系统复杂。尤其以电池组做动力源的水下机器人，都采用直流电机。无刷直流电机是近几年来随着电子技术的迅速发展而发展起来的一种新型直流电机，其最大特点是没有换向器（整流子）和电刷组成的机械接触机构。通常采用永磁体为转子，没有激磁损耗，没有换向火花，没有无线电干扰，运行可靠，维护简便。

无刷直流电机(Brushless DC Motor，以下简称BLDC)是随着电力电子技术及新型永磁材料的发展而迅速成熟起来的一种新型电机。以其体积小、重量轻、效率高、惯量小和控制精度高等优点,同时还保留了普通直流电动机优良的机械特性,广泛应用于伺服控制、数控机床、机器人等领域，随着无刷直流电机应用领域的不断扩大，要求控制系统设计简易、成本低廉、控制算法合理、开发周期短。建立无刷直流电机控制系统的仿真模型，可以有效的节省控制系统设计时间，及时验证施加于系统的控制算法，观察系统的控制输出；同时可以充分利用计算机仿真的优越性，人为地改变系统的结构、加入不同的扰动和参数变化，以便考察系统在不同结构和不同工况下的动、静态特性[8]。

5.2永磁无刷直流电机(BLDC)的数学模型

无刷直流电机由定子三相绕组、永磁转子、逆变器、转子磁极位置检测器等组成，其转子采用瓦形磁钢，定子采用整距集中绕组，由逆变器供给方波电流。BLDC气隙磁场感应的反电动势和相电流之间的关系，如图5-1所示。由于BLDC的感应电动势为梯形波，包含有较多的高次谐波，并且BLDC的电感为非线性，在分析和仿真BLDC控制系统时，直接采用相变量法，根据转子位置，采用分段线性表示感应电动势。

图5-1 A相反电动势和电流波形

5.2.1电压方程

BLDC三相定子电压的平衡方程可用以下的状态方程表示:

(3)

式中，ua,ub,uc为三相定子电压(V)；ea,eb,ec为三相定子的反电动势(V)；ia,ib,ic为三相定子相电流(A)；La,Lb,Lc为三相定子自感(H)；Lab,Lac,Lba,Lbc,Lca,Lcb为三相定子绕组之间的互感(H)；Ra,Rb,Rc为三相定子绕组的相电阻；p为微分算子（d/dt）。由电机的结构决定，在360°电角度内，转子的磁阻不随转子位置的变化而变化，并假定三相绕组对称，则有：La=Lb=Lc=L,Lab=La=Lba=Lbc=Lca=Lcb=M,Ra=Rb=Rc=R。由于三相对称的电机中,ia+ib+ic=0,以及Mib+Mic=-Mia，则式(3)可改写为：

(4)

5.2.2转矩方程

BLDC的电磁转矩方程可表示

(5)

其中，ω为BLDC的角速度(rad/s)。BLDC的运动方程可表示为：

(6)

5.2.3等效电路

由BLDC的电压方程，可以将其等效地表示为图5-2所示的等效电路，BLDC的每相由定子绕组电阻R、电感(L-M)及一个反电动势e串联构成。

图5-2永磁无刷直流电机等效电路图

5.3 基于Matlab的BLDC系统模型的建立

在Matlab7.0的Simulink环境下，利用SimPowerSystemToolbox提供的丰富模块库，在分析BLDC数学模型的基础上，提出了建立BLDC控制系统仿真模型的方法，系统设计框图如图5-3所示。BLDC建模仿真系统采用双闭环控制方案：转速环由PID调节器构成，电流环由电流滞环调节器构成。根据模块化建模的思想，将图3所示的控制系统分割为各个功能独立的子模块。图5-4即为BLDC建模的整体控制框图，其中主要包括：BLDCM本体模块、速度控制模块、参考电流模块、电流滞环控制模块、转矩计算模块和电压逆变器模块。把这些功能模块和S函数相结合，在Matlab/Simulink中搭建出BLDC控制系统的仿真模型，并实现双闭环的控制算法，图中各功能模块的作用与结构简述如下。

图5-3 BLDC控制系统设计框图

图5-4 Matlab/Simulink中BLDC仿真建模整体控制框图

5. 3.1 BLDCM本体模块

在整个控制系统的仿真模型中，BLDCM本体模块是最重要的部分，该模块根据BLDC电压方程式求取BLDC三相相电流，结构框图如图5-5所示。

由电压方程式可得，要获得三相电流信号ia、ib、ic，必需首先求得三相反电动势信号ea、eb、ec。而BLDC建模过程中，梯形波反电动势的求取方法一直是较难解决

的问题，反电动势波形不理想会造成转矩脉动增大、相电流波形

不理想等问题，严重时会导致换向失败，电机失控。

图5-5 BLDCM本体模块结构框图及其封装

因此，获得理想的反电动势波形是BLDC仿真建模的关键问题之一。目前求取反电动势较常用的三种方法为：(1)有限元法，应用有限元法求得的反电动势脉动小，精度高，但方法复杂、专业性强、不易推广。(2)傅立叶变换(FFT)法，FFT法应用简单，但需要进行大量三角函数值的计算，对仿真速度影响较大。(3)分段线性法，如图5-6所示，将一个运行周期0-360°分为6个阶段，每60°为一个换向阶段，每一相的每一个运行阶段都可用一段直线进行表示，根据某一时刻的转子位置和转速信号，确定该时刻各相所处的运行状态，通过直线方程即可求得反电动势波形。分段线性法简单易行，且精度较高，能够较好的满足建模仿真的设计要求。因而，本文采用分段线性法建立梯形波反电动势波形。理想情况下，二相导通星形三相六状态的BLDC定子三相反电动势的波形如图6所示。根据转子位置将运行周期分为6个阶段：0～π/3，π/3～2π/3，2π/3～π，π～4π/3，4π/3～5π/3，5π/3～2π。

以第一阶段0～π/3为例，A相反电动势处于正向最大值Em，B相反电动势处负向最大值-Em，C相反电动势处于换向阶段，由正的最大值Em沿斜线规律变化到负的最大值-Em。根据转子位置和转速信号就可以求出各相反电动势变化轨迹的直线方程；其它5个阶段，也是如此。据此规律，可以推得转子位置和反电动势之间的线性关系，如表1所示，从而采用分段线性法，解决了在BLDC本体模块中梯形波反电动势的求取问题。

表1转子位置和反电动势之间的线性关系表

表1中：k为反电动势系数(V/(r/min))，Pos为电角度信号(rad)，w为转速信号(rad/s)。

根据电机转过的电角度来求反电动势，用S函数编写。

图5-6 三相反电动势波形

5.3.2电流滞环控制模块

在这个仿真模块中采用滞环控制原理来实现电流的调节，使得实际电流随给定电流的变化。图5-7表示的是滞环型PWM逆变器的工作原理。其工作原理是：当给定电流值与反馈电流值的瞬时值之差达到滞环宽度正边缘时，逆变器的开关管VT1导通，开关管VT4关断，电动机接通直流母线的正端，电流开始上升反之，当给定电流值与反馈电流值的瞬时值之差达到滞环宽度负边缘时，逆变器的开关管VT1关断，开关管VT4导通，电动机接通直流母线的负端，电流开始下降。选择适当的滞环环宽，即可使实际电流不断跟踪参考电流的波形，实现电流闭环控制。模块结构框图如图5-8所示，输入为三相参考电流和三相实际电流，输出为PWM逆变器控制信号。

(a)滞环电流跟踪型PWM逆变器单项结构示意图

(b)滞环电流跟踪型PWM逆变器输出电流电压波形

图5-7 滞环电流跟踪型PWM逆变器的工作原理

图5-8电流滞环控制模块结构框图及其封装

5.3.3速度控制模块

速度调节采用离散PID算法，以获得最佳的动态效果。速度为积分的参数Kd为微分的参数。控制模块的结构较为简单，如图5-9所示，单输入：参考转速(n_ref)和实际转速(n)的差值，单输出：三相参考相电流的幅值Is。其中，Kp为PID控制器中比例的参数，Ki为积分的参数，Kd为微分的参数。Saturation饱和限幅模块将输出的三相参考相电流的幅值限定在要求范围内。

（a）离散PID控制器

(b)速度控制模块

图5-9速度控制模块结构框图及其封装

5.3.4参考电流模块

参考电流模块的作用是根据电流幅值信号Is和位置信号给出三相参考电流，输出的三相参考电流直接输入电流滞环控制模块，用于与实际电流比较进行电流滞环控制。转子位置和三相参考电流之间的对应关系如表2所示，参考电流模块的这一功能可通过S函数编程实现。

5.3.5转矩计算模块

根据BLDC数学模型中的电磁转矩方程式，可以建立图5-10所示的转矩计算模块，模块输入为三相相电流与三相反电动势，通过加、乘模块即可求得电磁转矩信号Te。

图5-10转矩计算模块结构框图及其封装

5.3.6转速计算模块

根据运动方程式(4)，由电磁转矩、负载转矩以及摩擦转矩，通过加乘、积分环节，即可得到转速信号，求得的转速信号经过积分就可得到电机位置信号，如图5-11所示。

图5-11转速计算模块结构框图及其封装

5.3.7电压逆变器模块

逆变器对BLDC来说，首先是功率变换装置，也就是电子换向器，每一个桥臂上的一个功率器件相当于直流电动机的一个机械换向器，还同时兼有PWM电流调节器功能。对逆变器的建模，本文采用Simulink的SimPowerSystem工具箱提供的三相全桥IGBT模块。给IGBT的A、B、C三相加三个电压表，输出的Simulink信号可以与BLDC 直接连接，如图5-12所示。逆变器根据电流控制模块所控制PWM信号，顺序导通和关断，产生方波电流输出。

图5-12电压逆变器模块结构框图及其封装

5.4仿真结果

基于Matlab/Simulink建立了BLDC控制系统的仿真模型，并对该模型进行了BLDC 双闭环控制系统的仿真。仿真中，BLDC电机参数设置为：定子相绕组电阻R＝1Ω，定子相绕组自感L＝0.02L，互感M＝-0.061H，转动惯量J＝0.005kg.m2，阻尼系数B=0.0002N2m2s/rad，额定转速n＝1000r/min，极对数p＝1，220V直流电源供电。离散PID控制器三个参数Kp=5，Ki=0.01，Kd=0.001，饱和限幅模块幅值限定在±35内，采样周期T=0.001s。为了验证所设计的BLDC控制系统仿真模型的静、动态性能，系统空载起动，待进入稳态后，在t＝0.3s时突然加负载TL＝5N2m，在t＝0.65s时突然撤去负载。可得到系统转速、转矩、三相电流和三相反电动势仿真曲线如图5-13～5-16所示。

由仿真波形可以看出，在n＝1000r/min的参考转速下，系统响应快速且平稳，相电流和反电动势波形较为理想。仿真波形图5-14、5-15表明：起动阶段系统保持转矩恒定，因而没有造成较大的转矩和相电流冲击，参考电流的限幅作用十分有效；空载稳速运行时，忽略系统的摩擦转矩，因而此时的电磁转矩均值为零；在t＝0.5s时突加负载，转速发生突降，但又能迅速恢复到平衡状态，稳态运行时无静差。仿真波形图5-14中，突加负载后，负载转矩有较大的脉动，这主要是由电流换向和电流滞环控制器的频繁切换造成的。本系统经过硬件试验，其结果与仿真波形一致，证明了本文所提出的这种新型BLDC仿真建模方法的有效性及控制系统的合理性。

图5-13 转速响应曲线图5-14 转矩响应曲线

图5-15 电流Ia，Ib，Ic波形图5-16 反电动势Ea, Ea, Ea波形

6 结论

本文在超小型水下机器人的设计、算法及实验等方面进行了相关论述，初步建立了超小型水下机器人基础智能控制实验平台。利用自适应PID控制算法在超小型水下机器人航向角锁定方面进行了理论及实验分析。本研究也为自行设计开发新型超小型水下机器人提供了实验平台且积累了相关经验。介绍的小型自治水下机器人以单主推和舵组合的方式操纵航行，可实现自主定深定向航行。基于简化模型设计了航行控制系统，立了开架式水下机器人物理模型及数学模型，并对数学模型简化。对推进器电机进行了仿真计算分析。

参考文献

[1]．吴宝举，李硕，李一平，王晓辉。小型自治水下机器人运动控制系统研究[J]机

械设计与制造。2010，6：158-160

[2]．杨晓华，侯巍，王树新，梁捷，刘颉。基于CAN 总线的水下机器人执行节点设

计与实现[J]海洋技术。2005，24（4）：15-16

[3]．冯常，窦普，陈树才。小型水下观测机器人设计与控制的研究[J]，计算机测量与

控制。2009，17（4）：672-678

[4]．王波，苏玉民，秦再白.微小型水下机器人操纵性能与运动仿真研究[J]，系统仿真

学报。2009，21（13）：4149-4158.

[5]．袁伟杰，刘贵杰, 朱绍锋。基于遗传算法的自治水下机器人水动力参数辨识方法

[J]，机械工程学报。2010，46（11）：97-100.

[6]．严龙，苏永清。基于DSP 的水下机器人组合导航系统设计[J]，工业控制计算机。

2010，23( 4 )：11-13

[7]．蔡昊鹏，苏玉民。水下航行体推进器设计方法研究[J]，哈尔滨工程大学学报。2010，

31（4）：429-469.

[8]．邢逾，李伟力，程鹏。永磁无刷直流电动机时步有限元与Matlab协同仿真计算

[J]，微电机。2007，40（1）：40-44