无人艇运动控制仿真试验训练系统设计研究

刘 峰,许二旭

(1. 中国人民解放军92941部队43分队,辽宁 葫芦岛 125000;2. 中国人民解放军91976部队,广东 广州 510080)

无人装备模型构建和运动控制仿真技术是近年来国内外军工靶场的研究重点,也是无人装备数字化鉴定及模拟训练的核心关键,其鉴定考核具有标准依据少、方法设计难等特点,传统试训方法不能直接观察到控制系统各参数变化对运动稳定性的影响,试训人员对控制参数的调试理解不深,使得训练效果不佳,因此,通过模型构建及仿真技术来验证无人装备运动控制参数和性能,对其融入作战体系及能力生成具有重要意义[1]。本文基于CoppliaSim仿真软件设计无人艇运动控制仿真试验训练系统,基于构建的无人艇动力学模型和运动学模型,设计构建稳健比例积分(PID)控制、滑膜控制和模糊PID控制三种控制器,仿真系统与实装艇通过通信系统连接,试训人员能够通过仿真系统调节参数,结合实船演示和仿真可视化的方法验证不同控制器下的轨迹变化特性,为无人艇试验训练提供手段方法和基础支撑[2]。

设计过程包括CoppeliaSim中的仿真运动过程和软件界面设计整合过程。CoppeliaSim中的仿真过程包括路径规划、控制器设计、运动学仿真三大部分,仿真全过程的逻辑框图如图1所示。在实现运动仿真后,软件界面对系统功能进行整合,以运动模式、行程点和控制参数为输入量,经运动学仿真后,将运动状态输出显示并进行运动渲染,其具体逻辑框图如图2所示。

图1 仿真过程逻辑图Fig.1 Logic diagram of simulation process

图2 软件逻辑框架Fig.2 Software logic framework

1 无人艇实体模型及环境搭建

为仿真试验提供可视化效果,需要构建相应的无人艇实体模型和水面环境模型[3]。

1.1 无人艇实体模型

考虑仿真平台运行时的性能和无人艇的运动特性,本系统采用CoppliaSim仿真软件中的简单立方体代替无人艇实体模型进行仿真。螺旋桨也用两个简单的立方体代替,并用力传感器将替代螺旋桨的立方体与无人艇体进行连接,虚拟无人艇组成部分从属关系图如图3所示。

图3 虚拟无人艇组成部分从属关系图Fig.3 Dependency diagram of virtual unmanned surface vehicle components

为了画面效果,依据试验用实物艇建立无人艇的实体建模,将模型放置在显示的图层,而将仿真的实体放在隐藏的图层,如图4所示。

图4 导入完成的全部模型显示Fig.4 Shows of imported model

1.2 环境模型搭建

CoppliaSim软件提供的模型不具备如水浮力、水阻力等物理特征,而本系统关注的是无人艇的水面运动状态,即我们是基于三自由度无人艇模型进行后续设计工作,因此,对模型进行简化,将无人艇实体放在一个支撑大平面上进行仿真。水阻力以及风浪等干扰可以通过在调用软件中添加力和力矩的函数进行仿真。

2 动力学模型构建

2.1 无人艇坐标系

无人艇在水面上航行时,可将运动行为看成在六个自由度上的运动。运动行为分为线性运动和旋转运动。线性运动为沿轴x0、y0、z0方向的运动,旋转运动则是沿这三轴的往复运动。为了更加详细地描述无人艇的运动状态和位置,我们建立了描述无人艇位置的惯性坐标系和描述其运动姿态的机体坐标系,如图5所示。

图5 坐标系构建Fig.5 Construction of coordinate system

惯性坐标系也称为大地坐标系,其原点选为地球表面某个运动参考点,通常情况下为无人艇某个时刻的重心。o0x0轴的正方向指向正北方,o0y0轴的正方向指向正东方,o0z0轴的正方向垂直向下指向地心。机体坐标系也称为运动坐标系,用于描述无人艇的运动信息[4]。o为无人艇的重心,该坐标系会跟随艇一起进行移动。规定:ox轴正方向指向艇首,oy轴正方向指向艇体的右舷,oz轴正方向垂直向下。

无人艇状态变量定义为η1=[x,y,z],表示无人艇在惯性坐标系的位置,η2=[φ,θ,ψ]表示无人艇在惯性坐标系中的姿态;v1=[u,v,w]为无人艇在运动坐标系中的线速度,分别对应了纵荡、横荡和垂荡速度,v2=[p,q,r]则是代表在同一坐标系中的角速度,分别对应了横摇、纵摇和艏摇角速度;τ1=[X,Y,Z]代表了无人艇在运动坐标系下所受到的各方向外力,τ2=[K,M,N]则代表在同一坐标系下的各方向力矩。综上,定义η=[η1,η2]为无人艇的位置向量,v=[v1,v2]为速度向量,τ=[τ1,τ2]为其所受外力/力矩[5]。

2.2 无人艇动力学模型

无人艇线速度在两坐标系之间转换关系为

(1)

其中,J1(η2)为转换矩阵,表示为

(2)

无人艇角速度在两坐标系之间转换关系为

(3)

其中,J2(η2)为转换矩阵,表示为

(4)

结合式(1)和(3)可得无人艇水面运动模型:

(5)

式(5)可简化为

(6)

将无人艇视为刚体,由牛顿-欧拉方程推导出的刚体力和力矩平衡方程为

(7)

其中,rg=[xg,yg,zg]为重心坐标,I0为刚体转动惯量。

将式(7)改写为

(8)

式中,v=[u,v,w,p,q,r],τRB=[X,Y,Z,K,M,N]。

MRB为刚体惯性矩阵:

(9)

CRB(v)是刚体科里奥利向心力矩阵,令CRB(v)=[C1,C2],则:

(10)

向量τRB由流体力和力矩向量τH、外部环境干扰力和力矩向量τE、推进力和力矩向量τ组成。下面重点介绍τH的组成。

由流体力学可知,作用于刚体上的流体动力和力矩是线性叠加的[6]。流体动力和力矩由附加质量、水动力阻尼和重力与浮力引起的回复力三部分组成,因此,τH可以写成

(11)

MA是附加质量矩阵。

(12)

CA(v)是流体力学科里奥利向心力矩阵。

(13)

其中,

(14)

D(v)是阻尼矩阵,可写为线性和非线性阻尼矩阵的和。

D(v)=D+Dn(v)

(15)

其中,线性阻尼矩阵D为

(16)

g(η)是回复力和力矩矩阵向量。

g(η)=

(17)

其中,W为重力,B为浮力。

W=mg,B=ρgρ′

(18)

式中,m为无人艇质量,g为重力加速度,ρ为水密度,ρ′为排水密度。

2.3 三自由度无人艇运动模型

在无人艇的实际航行过程中,主要关注运动坐标系纵向运动、横向运动以及转艏运动(水平面上的运动),因此,可以将六自由度无人艇运动数学模型简化为三自由度的平面运动数学模型,使运动控制问题得到简化,水平坐标系如图6所示。

图6 水平坐标系Fig.6 Horizontal coordinate system

假设无人艇的几何中心与重心重合,同时忽略风、浪和流等外界干扰因素,则可以得到无人艇水面三自由度运动数学模型:

(19)

其中,位置矢量η=[x,y,ψ],速度矢量v=[u,v,r],而仿真对象是双桨无人艇,则其所受推力和力矩矢量τ=[T1+T2,0,(T1-T2)B/2]。T1和T2分别是无人艇左右桨提供的推力,B是桨间的水平距离。

对于螺旋桨提供的推力,用以下数学模型进行描述:

(20)

式中,ρ表示水密度;(JP)为桨推力系数,其中,JP为进速系数;tP为伴流系数;DP表示螺旋桨直径;n是螺旋桨转速。

转换矩阵J(η)可表示为

(21)

惯性矩阵M=MRB+MA,科里奥利力矩阵C(v)=CRB(v)+CA(v),阻尼矩阵D(v)=D+Dn(v)。其中:

(22)

(23)

(24)

CA(v)=

(25)

(26)

(27)

(28)

结合式(24)和(25)可得C(v)的具体表达形式:

(29)

阻尼矩阵包含线性阻尼矩阵D以及非线性阻尼矩阵Dn(v),忽略高于二阶的流体动力阻尼项,则非线性阻尼矩阵中各项系数均为零,因此:

(30)

综上,将式(19)展开为

(31)

无人艇基本参数及无人艇水动力模型参数如表1、表2所示。

表1 无人艇基本参数Tab.1 Basic parameters of unmanned surface vehicle

表2 无人艇水动力模型参数Tab.2 Hydrodynamic model parameters of unmanned surface vehicle

3 控制器构建

无人水面艇在海上航行,由于受到风、浪、流等环境的影响,给定的航向会不可避免地产生偏移。精准的航向控制是解决航迹跟踪、自主航行、自动避碰等问题的前提。相对民船而言,无人艇的重量小,速度快,动力有限,风、浪、流等海洋环境的干扰显得非常突出,其控制要求更高。因此,必须研究出更加先进的无人艇运动控制技术来代替传统的控制方法,以满足控制要求[7]。无人艇的运动控制器由航向控制器、航迹控制器和速度控制器三个控制器组成,为了让试训人员更直观地体验控制器的设计,我们分别采用了PID控制器、滑模控制器和模糊PID控制器来进行无人艇的仿真航向控制,并预留可供修改的控制器参数来体验控制器的特性,控制逻辑如图7所示。

图7 无人艇控制逻辑Fig.7 Control logic of unmanned boat

我们使用的无人艇是一种双推进器无人艇。对于该艇,航迹控制器的输入为无人艇与目标航迹的距离偏差(e),输出为预期航向角(ψd);航向控制器的输入为航向误差(Δψ),输出为推进器转速差值;速度控制器输入为速度误差(ΔV),输出为推进器转速均值。航向控制器起到了调整航向的作用,在目标航向已知的情况下向目标航向靠拢;航迹控制器在航向控制器表现较好时才能发挥更好的作用,其会根据航行实际位置与规划航迹综合分析,对无人艇航行航向进行直接决策,扮演指令下达者的角色;速度控制器用来控制无人水面艇航速,在不同水面情况下会有不同航速要求,速度控制器负责达到预期的航速目标。

设计三种不同的控制器,分别是PID控制器、滑模控制器和鲁棒控制器(模糊PID控制器)。三种控制器各有优劣。其中,PID控制器和模糊PID控制器在存在未知干扰的情况下会有更好的控制效果,具有更高的鲁棒性能。而滑模控制器对模型精准度要求更高,但在模型确定的情况下控制精度更好。下面是我们用三个控制器进行航向角控制,结果如图8～图10所示。

图8 PID航向角控制结果Fig.8 PID heading angle control results

图9 滑模航向角控制结果Fig.9 Sliding mode heading angle control results

图10 模糊PID航向角控制结果Fig.10 Fuzzy PID heading angle control results

由仿真结果可知,三个控制器都可以达到较好的结果,三个控制器对航向角阶跃信号的响应时间和控制精度分别为:

PID控制器响应时间:21.65 s,收敛后的波动范围是(179.041 3,180.424 5);

滑膜控制器响应时间:26.35 s,收敛后的波动范围是(169.521 2,170.752 5);

模糊PID控制器响应时间是21.55 s,收敛后的波动范围是(175.114 2,175.658 7)。

三个控制器中,PID控制器最为简单,具有一定的鲁棒性;模糊PID鲁棒性最高,有自适应能力;滑膜控制器对模型的精度要求最高,参数调整较为困难,若相关参数调整不合理,容易出现抖震现象。

4 Navmesh寻路算法

Navmesh (Navigation Mesh)寻路算法是目前比较主流的一种寻路算法,是一种基于凸多边形网格的寻路方式[8]。Navmesh是对基于导航网格寻路体系的统称,实现方式复杂多样。但无论具体使用怎样的方法,Navmesh寻路算法的全部流程至少包含两个部分:导航网格构建和寻路算法。本平台使用的Navmesh寻路算法原理如图11所示。

图11 Navmesh寻路算法实现原理Fig.11 Navmesh pathfinding algorithm implementation principle

4.1 导航网格构建

导航网格构建(Navigation Mesh Construction)是指根据得到的地图信息中障碍物和陆地的位置,在地图上静态地构建可供寻路算法使用的导航网格的过程。导航网格构建的基本流程如下:

体素化场景:将提供的源地图数据通过体素化方式建立实体Span,与此同时,对体素化模型进行简单过滤,以去除无人艇可能无法到达的位置;

创建Regions:将第一步生成的体素模型描述的可行走区域划分为重叠的2D区域,检测可跨越的Span,并为其生成简单的轮廓数据;

创建Polygon:根据轮廓数据对区域进行化简,获得导航多边形,再对得到的导航多边形进行处理,得到凸多边形。

4.2 寻路算法

寻路算法(Path Finding Algorithm)是指根据前面构建的导航网格体,迅速生成一条可供无人艇在自动驾驶模式下无碰撞行驶的合理路径的过程。寻路算法的实现主要分为以下三个阶段。

4.2.1 确定优化目标

为构建的凸多边形网格体添加一个“成本”值,这样在搜索路径时,寻路算法会尝试找到总成本最低的路径。

4.2.2 确定凸多边形集合

以经过导航网格的总“成本”值最小为目标,使用其他寻路算法可以获得无人艇达到终点所需要经过的导航网格集合。这里所使用的寻路算法通常为A*算法[9]。A*算法是一种应用十分广泛的寻路算法,不一定能找到最短的路径,但可以在较短的时间内搜索到一条相对较短的路径,在Dijkstra算法基础上引入启发因子,寻找那些“看起来离终点更近”的节点。A*算法的运行结果如图12所示。

图12 A*算法的运行结果*注:绿色网格为起点,红色网格为终点,灰色网格为障碍物,黄色折线为搜索到的最短路径。Fig.12 Operation results of the A* algorithm

4.3 算法仿真应用

通过使用Navmesh寻路算法,系统可以生成无人艇到达终点的合理路径。本文设计的路径规划方法在仿真平台中实现了以下目标:

根据较复杂的场景中的障碍物信息,构建导航网格体,并在短时间内迅速生成一条成本相对较低的路径;

将得到的路径转化为保障无人艇不接触障碍的路径点,同时有利于提高控制器的控制效果;

根据路径的整体情况以及船的转弯、制动、加速性能得到速度矩阵,并根据路径点的分布、位置、距离等参数进行速度矩阵的再次修正,最终传输给控制器进行每一段速度的控制,如图13所示。

图13 路径的初步规划Fig.13 Preliminary planning of the path

4.4 路径生成优化

4.4.1 不同速度下的寻路优化

在实际运行过程中,随着速度的增大,转弯半径也会增大,进而影响无人艇的路径规划[10]。为了能够在仿真平台中体现出这一特点,我们为障碍物的碰撞模型增添了一段安全距离,以确保有一定宽度的无人艇可以安全通过。设置安全距离后的效果如图14所示。图中,绿色方格是构建的导航网格体,而不规则的灰色环围成的范围则是设置的安全距离。由于转弯半径与速度有关,随着速度的增大,其安全距离也应随之增大,在寻路算法运行的时候,会把安全距离以内的区域认定为被阻挡的区域,进而保证船在高速状态下也可选择合适且安全的路径。

图14 障碍物实际碰撞范围影响导航网格体Fig.14 Actual collision range of obstacles affects the navigation grid body

图15、16是在搭建合适的场景后,无人艇行驶速度慢与快对路径生成带来的影响。在慢速情况下,无人艇的转弯半径较小,因此,寻路算法可以在障碍物比较密集的地方生成路径;而在快速情况下,无人艇的转弯半径增大,为了行驶的安全以及保持较快的行驶速度,此时路径轨迹的生成则会选择在宽阔的海面上。

图15 较低速度下的路径规划Fig.15 Path planning at lower speeds

4.4.2 减少船的转弯

在实际运行过程中,频繁的转弯会影响无人艇的运行速度,同时可能会影响控制器的效果。从图16可以看出,使用A*算法得到的路径效果并不是真正意义上的最短路径,这是由A*算法所使用的启发式函数决定的,而之后使用的路径平滑算法也无法减少转弯的次数,如图17所示。因此,需要对生成的路径点进行额外的处理。下面将对处理的方式进行介绍:

图16 较高速度下的路径规划Fig.16 Path planning at higher speeds

图17 仿真平台中减少转弯次数的算法示意图Fig.17 Schematic diagram of the algorithm for reducing the number of turns in the simulation platform

Step1:对于寻路算法得到的点集,如果点集中的元素个数不少于3个,则转入Step2处理;否则不需要处理;

Step2:取出点集中的第一个点与第三个点,记为C1和C3,由C1向C3发射一条射线,如果射线没有命中障碍物,说明无人艇可以通过直线直接到达C3,可以把二者间的点删除,再取出点集中的第三个点进行检测;如果命中障碍物,说明C1和C3间的点需要保留,此时取出C2和C4进行检测;

Step3:重复上述过程,直到取出点集中的倒数第二个点为止。

在减少无人艇转弯次数时,如果删除过多点,可能会导致无人艇在某次转弯过程中转弯幅度过大,会对行驶造成不好的影响。因此,我们基于一定规则,对本该删除的点进行必要的取舍,以保证在不降低无人艇转弯平稳性的同时,减少无人艇转弯次数。最后达到的路径规划效果如图18所示。相对图16,无人艇自动生成的路径中转弯次数出现了较明显减少。

图18 使用优化方法减少路径中的转弯次数Fig.18 Using optimization methods to reduce the number of turns in the path

5 结束语

本文设计的仿真试验训练系统,弥补了靶场无人艇仿真试验鉴定领域不足,基本解决了无人艇模拟训练手段缺乏的难题。基于该系统开展试验训练工作,可加深对无人装备运动控制理论知识的理解掌握,有利于试训人员对各种控制算法及数字化试验鉴定方法的理解。同时,基于该系统,能够对无人装备运动控制试验训练方法进行创新设计,有助于提升海军靶场试验训练综合保障能力。