多无人船编队控制半实物仿真系统开发

严卫生，王卫国，高 剑，李勇强

（西北工业大学 航海学院，陕西 西安 710072）

多无人船编队控制半实物仿真系统开发

严卫生，王卫国，高 剑，李勇强

（西北工业大学 航海学院，陕西 西安 710072）

设计并实现了用于模拟多无人船编队的实时半实物仿真系统。该系统通过对电压信号的数模模数转换模拟电机控制信号的输出和采集，通过串行通讯端口模拟测量设备的数据输出和数据采集。利用PC104平台搭建无人船控制器，使用基于VxWorks的实时控制软件；利用工控机和VC++6.0环境搭建无人船仿真中心，实现无人船仿真过程数据的图形化显示。最后仿真实验表明该系统可以良好的模拟多无人船的编队行为，在验证编队算法和测试无人船嵌入式控制软件方面有很强的实用性。

无人船；实时操作系统；编队控制；半实物仿真

无人船（Unmanned Surface Vehicle，USV）作为开发和利用海洋的重要工具之一，凭借其模块化、无人化、小型化、智能化等优点，正受到越来越多国家的重视并投入研究。对于单个无人船难以完成复杂的任务，无人船的协同编队显得尤为重要。在一些具体任务中，维持适当的队形能够提高工作效率，加快对目标任务的完成，降低了系统消耗，同时又增强了无人船队系统的鲁棒性。目前，无人船编队系统开发主要阶段有：系统设计，控制器设计，控制算法实现及实验验证；当实验遇到问题时需要重新回到控制器设计环节重新设计。然而无人船编队实验代价大，过程受多方面因素影响，在开发初期，不宜通过实际编队实验去分析检验编队系统设计。因此为了在开发初期更好的对系统进行验证和校准，需要设计在实验室可以更容易实现的半实物仿真系统，以半实物仿真代替实物实验。同时，在Matlab仿真编队算法时忽略了模块间数据通信的延迟及精度丢失，这一问题可能导致在实际系统中编队算法的发散，而半实物仿真可以弥补这一问题更有效的完成算法的验证。因此，无人船编队半实物仿真对系统设计的验证和编队算法的验证具有重要意义[1-3]。

1 系统工作原理

双推进器无人船系统主要由后面的两个推进器控制船的运动，左右推进器合力推动船前进，而两个推进器推力不同时，控制船的转向。无人船上控制器负责采集无人船运动状态参数，根据控制目标解算出无人船运动所需的实时推力和转矩，转化为相对应的左右推进器控制电压并输出出去驱动电机转动从而控制无人船运动[4]。无人船运动状态可由一组状态参数表示，具体为：前向速度u，侧向速度v，航向角角速度w，北向位置坐标x，东向位置坐标y，航向角Ψ。

编队仿真系统中有一个运动仿真中心，一个遥控中心和多个无人船控制节点。其中，仿真中心完成多条无人船航行过程的动力学仿真，根据无人船运动的差分模型，以控制节点的推进器控制电压为输入，解算无人船下一时刻状态为输出。在这个过程中，仿真中心接收各控制节点输出的推进器控制电压，然后，将新的船体运动状态参数以标准传感器格式通过串口发送给各船的控制节点。控制节点完成对船上控制器的仿真，即包括接收传感器数据，航行控制解算和控制输出。多个控制节点通过串口以标准传感器格式分别接收对应船的状态参数，参与到航行解算中去，得到各自船的推进器控制电压，并以模拟电压形式输出到仿真中心。这样，仿真中心与多个控制节点构成了一个完整的控制回路，在仿真中心将可以看到多个船只按照设定的运动轨迹航行，这样的工作模式也称为自主运动模式，既仅需要控制人员输入控制目标即可。

图1 系统工作原理结构图Fig.1 Structure diagram of simulation system

遥控中心完成对各船运动的实时监控和紧急控制的功能，在岸上可由控制人员直接参与控制船只的运动，这种情况下船工作在遥控运动模式。在这种模式中，遥控中心接收各控制节点发送过来的船运动状态参数，然后将人工控制指令，转化为对推进器电压的直接控制，发送到各控制节点。遥控中心与控制节点的通信需要构建无线通信网络来实现。系统工作原理结构图，如图 1所示。

2 硬件设计

2.1 仿真中心

仿真中心需要完成显示船体状态，串口信息输出和读取模拟电压值这3个功能。因此设计仿真中心由一台工控机实现，为这台工控机配置3个标准RS-232串口和一个PCI扩展的A/D，D/A扩展板，这里选用9112型扩展板，包含16路A/D通道，可以满足需求[1]。

2.2 控制节点

控制节点需要完成传感器数据接收，无线通信数据接收发送和电机电压输出等功能，其中，数据接收发送由串口实现，电压以模拟电压信号形式输出。因此，设计控制节点由嵌入式PC104控制平台实现，配置两个RS-232串口，及两路D/A输出通道。控制节点结构图，如图 2所示。

船上控制器PC104工控机挂载的设备有一个MTI-G传感器，一个无线模块和两个电机调速模块。MTI-G传感器周期性的提供姿态，3个角速度信息和GPS位置信息，与控制器的接口为RS232；无线模块周期性的将无人船的航行信息发送给遥控中心，并且实时接收遥控中心的控制指令，与控制器的接口为RS232；电机调速模块根据控制器发出的模拟信号，控制电机转动，与控制器的接口为D/A通道。

图2 控制节点结构图Fig.2 Structure diagram of node system

2.3 遥控中心

遥控中心需要完成无线通信数据接收发送的功能，其中，数据接收发送由串口实现。因此，设计遥控中心由普通笔记本电脑实现，配置一个USB转RS-232串口即可。

2.4 接口描述

仿真中心与控制节点有两个接口，分别是标准串口和两路D/A，A/D通道。其中，串口实现传感器数据的模拟，两路D/A，A/D通道实现两路电机控制电压传输的模拟。控制节点与遥控中心有一个由无线模块构建的无线通信网络实现数据交流，而这个通信接口在逻辑上也可简化理解为串口通信。

3 软件设计

3.1 仿真中心软件

仿真中心软件为VC++6.0下编写的MFC程序，包含串口驱动，PCI9112扩展卡驱动和曲线绘图组件等。半实物仿真系统要求仿真系统实时接收控制节点的输入，并产生实时动态输出，较高的实时性才可以保证半实物仿真系统的稳定性和精度。本软件采用了多媒体定时器（multimedia timer），保证模型迭代的实时性，定时精度为1 ms，无人船模型的仿真步长根据经验选择为10 ms。对于传感器的定时输出，采用Visual C++的WM_TIMER消息映射实现。

对于无人船的仿真采用面向对象的软件设计方法[5]，设计类CAutoUSV完成对无人船的运动学仿真，将无人船的所有运动状态参数抽象出来封装进这个类中作为私有产量。这样关于无人船的运动将完全由该类的内部函数运算实现，分析无人船的特性可知，需要一个函数实现无人船在时间上的状态递推，解算下一时刻船的状态，设计函数CAutoUSV：：onestep（）实现船的状态递推，既模拟船在时间上的运动。还需要设计函数CAutoUSV：：getStates（）实现无人船状态的输出，相当于使用传感器测量船的状态。还需要设计函数CAutoUSV：：control（）函数实现外部控制的输入，相当于推进器作用于船的运动。由于模型解算对于时间的要求很高，所以函数control（）和onestep（）需要在多媒体时钟的响应函数中执行，而getStates（）需要在时钟消息的响应函数中执行，获得船的实时状态，并以传感器格式发送出去。

仿真中心软件提供了良好的图形界面，实时显示各个无人船的位置曲线，可以直观的看到仿真的效果。仿真软件模拟的传感器输出格式如表 1所示。

3.2 控制节点软件

控制节点软件运行于船上控制器，其实时性要求很高，因此控制中心软件采用VxWorks嵌入式实时多任务操作系统，进行任务调度，以保证系统的实时性。船上软件中的任务主要有主任务，控制指令接收任务，MTI-G数据采集任务，无线通信驱动模块，电机控制任务。针对大量串口数据的收发，控制节点软件设计了高速有效串口数据读写缓冲区，保证了串口数据的实时更新。同时把串口的I/O操作放在任务中去，与主控制任务分开，保证了软件的稳定性和实时性。

表1 MTI-G传感器数据格式Tab.1 MTI-G sensor data format

对于电机控制，不能有剧烈的提速。因此，控制机设计了提速保障机制，即在单位时间内设置提速上限，将过大的提速，逐渐提高上去，确保电机稳定工作。

图3 控制节点软件流程图Fig.3 Flow chart the software of node system

3.3 遥控中心软件

遥控中心软件工作在岸上的遥控计算机上，配备1个标准串口接无线模块，与各船上的无线模块构建无线通信网络，实时监控两条船的航行情况。并且在必要情况下通过无线模块发出控制信息，直接控制船的航行。

遥控软件接收控制节点发送过来的无人船航行状态数据，其具体格式如表 1所示。同时遥控软件发送到控制节点的数据格式如表 2所示。

表2 遥控软件发送至控制节点的数据格Tab.2 Communication data format

4 实验验证

为了验证本系统的实用性，进行3条无人船的编队运动控制实验。本次实验采用主从跟随的编队算法，设定一条船为主船，引领船队的航行，跟踪一条既定轨迹[6]，其余两条为跟随船，以三角性姿态跟随主船运动。主船跟踪的轨迹曲线为：y=5sin（0.02πx），三条船的初始坐标为：主船（0，0），从船分别为（-50，-50），（0，50）。跟随船以三角性编队跟随主船[7-8]，队形保持为边长为10的等边三角形，以主船为顶点的中垂线时刻指向船的航向。实验结果如图 4所示。

图4 三船编队实验结果图Fig.4 Experimental Results Figure

实验结果表明，本系统有足够的稳定性和实时性，可以满足多无人船编队仿真的要求。

5 结 论

针对无人船编队系统开发的需要，本文设计并实现了一种用于模拟多无人船编队的实时半实物仿真系统，该系统通过对电压信号的数模模数转换模拟电机控制信号的输出和采集，通过串行通讯端口模拟测量设备的数据输出和数据采集。利用PC104平台搭建无人船控制器，使用基于VxWorks的实时控制软件；利用工控机和VC++6.0环境搭建无人船仿真中心，实现无人船仿真过程数据的图形化显示。分析仿真实验结果，可以良好的模拟多无人船的编队行为。在验证编队算法和测试无人船嵌入式控制软件方面有很强的使用价值。

[1] 高剑，严卫生.一种用于AUV导航控制软件开发与系统测试的半实物仿真系统[J].西北工业大学学报，2007，25（1）：87-91.

GAO Jian，YAN Wei-sheng.Improving hardware-in-the-Loop simulation system for AUV navigation and control software development and testing[J].Journal of Northwestern Polytechnical University，2007，25（1）：87-91.

[2] 段翀，谢寿生.某型涡扇发动机半实物仿真控制系统[J].推进技术，2005，26（5）：434-438。

DUAN Chong，XIE Shou-sheng.Hardware-in-the-loop simulation of a turbofan aeroengine control system[J].Journal of Propulsion Technology，2005，26（5）：434：438

[3] 严卫生，高剑.水下航行器导航与控制实时仿真系统[J].西北工业大学学报，2003，21（4）：423-437

YAN Wei-sheng，GAO Jian.Real-time navigation and control simulation system for a long-distance autonomous underwater vehicle[J].Journal of Northwestern Polytechnical University，2003，21（4）：423-437.

[4] Fossen Thor I.Marine control systems：Guidance，nacigation and control of ships，rigs and underwater vehicles[M].Trondheim，Norway：Marine Cybernetics，2002.

[5] Stephen Prata.C++ Primer Plus中文版[M].5版.北京：人民邮电出版社，2005.

[6] 高剑，刘富樯.欠驱动自主水面船的非线性路径跟踪控制[J].机器人，2012.34（3）：329-336.

GAO Jian，LIU Fu-qiang.Non-linear path following control of underactuated autonomous surface vehicles[J].Robot，2012，34（3）：329-336.

[7] 赵宁宁，徐德民.多自主水下航行器编队控制系统设计[J].火力与指挥控制，2011，36（1）：154-159.

ZHAO Ning-ning，XU De-min.Coordinated control system design for multiple autonomous underwater vehicle[J].Fire Control and Command Control，2011，36（1）：154-159.

[8] 严卫生，徐德民.自主水下航行器导航技术[J].火力与指挥控制，2004，29（6）：11-19.

YAN Wei-sheng，XU De-min.Surver of navigation technology for autonmous underwater vehicle[J].Fire Control ＆ Command Control，2004，29（6）：11-19.

Design and development of a simulation system for multi-USV formation control

YAN Wei-sheng，WANG Wei-guo，GAO Jian，LI Yong-qiang
（College of Marine Engineering，Northwestern Polytechnical University，Xi'an 710072，China）

A simulation system for multi-USV formation control is designed and accomplished in this paper.The system simulates the input and output of voltage signal which is used to control the motor by way of A/D and D/A conversion，and simulate the communication of sensor data via RS-232 port.PC104 platform and VxWorks real-time control software is used to build the USV's controller.The IPC and VC++6.0 software environment is utilized to build multi-USV simulation center，which will achieve the graphical display of USVs' states parameter.Finally Simulation results show that the system can properly and effectively simulate multi-USV formation.The system has a strong practicality in aspect of the validating of multi-USV formation algorithms and the testing of embedded control software.

USV；PC104；VxWorks；Formation control

TP302

A

1674-6236（2014）11-0043-04

2013-09-21 稿件编号：201309151

严卫生（1968—），男，江苏南通人，博士，教授。研究方向：水下航行器的制导、控制与仿真、水下目标跟踪、变结构控制与智能控制。