一种考虑通信情况的多USV群体协同避碰规划方法与流程

本发明属于usv控制技术领域，具体涉及一种考虑通信情况的多usv群体协同避碰规划方法。

背景技术：

随着无人水面艇usv技术的发展，单一usv的执行任务的能力、航行的鲁棒性、安全性等性能都有很大提升。为了有效弥补单一usv难以完成的任务领域，各国科研人员设计多usv群体协同的作业模式，最大限度地发挥整体优势而改善任务的完成质量。多usv的航行环境包含海岛、礁石等静态障碍物与运动船舶等动态障碍物，并且还受海浪、海流、海风等海况的影响。在这样复杂多变的海洋环境中，多usv能安全航行、顺利完成各项任务的一个重要的前提就是实现避碰功能。在多usv避碰规划的研究与实际避碰过程中，usv之间普遍会进行通信，然而同样会有通信模块失效的情况出现，此时则完全依靠雷达感知周围环境。目前，国内外对多usv避碰规划技术的研究较少，大多都不考虑通信情况或者默认通过通信模块才能获取的信息为已知/未知，其中考虑有/无通信模块的具有代表性如文献“王兢.水下机器人编队系统研究[d].哈尔滨：哈尔滨工程大学硕士学位论文,2003.”，设计基于环境信息的分层记忆学习法来实现auv的编队，采取黑板结构的通信模块,各机器人实时将位置信息写入供所有机器人获取。文献“王爱兵等.不同优先级的多水下机器人避碰技术研究[j].微计算机信息,2007,24(5-2):232-234.”提出基于kqml协议的信息交互显式通讯模块，采用动态优先级策略解决水下机器人编队航行过程中的避碰避障问题。文献“李东正等.基于主从结构的多水下机器人协同路径规划[j].计算机仿真,2015,32(1):382-387.”提出了一种主从结构的并行多水下机器人auv协同避碰规划方法，在无通信条件下利用声呐传感器感知周围环境信息并采用工作空间环境法建立环境模型，算法子层结构和主层结构分别应用粒子群并行算法和微分进化算法实时考虑到障碍物和其他水下机器人的距离来生成当前的最优路径。

现有技术的缺陷：现有多usv群体协同避碰规划方法通信模块作用模糊，经常发生一定范围内通信模块与传感器模块作用混淆的问题，通信频率设置不合理，没有充分利用通信模块互通位置、速度、航向信息的作用来辅助避碰。而考虑无通信时没有充分利用传感器模块探测的环境信息来辅助避碰，传感器探测范围和探测信息种类理想化，且有的文献也会在无通信情况下利用通信模块才能得到的信息进行避碰。文献“王兢.水下机器人编队系统研究[d].哈尔滨：哈尔滨工程大学硕士学位论文,2003.”各auv写入与读取的实时进行容易导致信息混乱、信息错误的情况，通信模块只互通位置信息而忽略航速、航向信息，固定的通信频率导致远距离资源占用或者近距离通信缓慢的问题，文献“王爱兵等.不同优先级的多水下机器人避碰技术研究[j].微计算机信息,2007,24(5-2):232-234.”的通信只是尝试构建了一种通用的通信框架来实时传递优先级，显式通讯的带宽和稳定性还无法完全保证，并且只传递优先级并没有充分利用通信模块的作用。文献“李东正等.基于主从结构的多水下机器人协同路径规划[j].计算机仿真,2015,32(1):382-387.”主从结构auv算法子层与算法主层约束条件复杂且存在耦合，存在求解困难、迭代时间长、陷入局部最优的为题，算法中默认探测到空间所有障碍物和其他auv，未考虑声呐的探测范围和角度，不符合实际且没有进行仿真验证。

技术实现要素：

本发明的目的在于提供一种考虑通信情况的多usv群体协同避碰规划方法。

一种考虑通信情况的多usv群体协同避碰规划方法，该方法包括以下步骤：

步骤1：选取参数构建通信模块和雷达探测模型；

步骤1.1：有通信时利用通信模块互通的位置、速度、艏向信息和雷达探测的静态障碍物位置和动态usv位置、动态usv速度、动态usv艏向信息，计算多usv运动参数及碰撞危险度；

步骤1.2：无通信时利用雷达模块探测的静态障碍物位置和动态usv位置、动态usv速度、动态usv艏向信息，计算多usv运动参数及碰撞危险度；

步骤2：判断是否需要采取避碰措施及采取的时间并根据相对距离设置通信频率；

步骤3：构建有、无通信情况下ga评价函数。

步骤1.1所述的有通信时利用通信模块互通的位置、速度、艏向信息和雷达探测的静态障碍物位置和动态usv位置、动态usv速度、动态usv艏向信息包括：在两个usv的情况下，本usv的大地惯性坐标为m(xm,ym)，本usv运动速度为vm(vxm,vym)，另一usv的大地惯性坐标为n(xn,yn)，另一usv的运动速度为vn(vxn,vyn)，在有通信情况下，usv之间通过通信模块得知对方的实时位置，那么usv之间的相对距离：

另一usv相对于本usv的方位：

γ＝α-β

usv之间的航向交叉角：

η＝βn-βm

usv之间的相对速度表示为：

usv之间的相对速度大小：

usv之间相对速度的方向：

步骤1.2所述的无通信时利用雷达模块探测的静态障碍物位置和动态usv位置、动态usv速度、动态usv艏向信息计算多usv运动参数及碰撞危险度的计算过程包括：这里取雷达探测到障碍物后的2个连续时刻检测到的障碍物信息，其中第i时刻雷达探测范围内探测出有障碍物的扫描点集合为：

{(xi1，yi1)，(xi2，yi2)，，(xin，yin)}

其中，探测出的扫描点集合的坐标为大地环境坐标系下的坐标值；

由障碍物的扫描点集合估算出障碍物的中心位置：

其中，为第i时刻障碍物中心估计位置横坐标，为第i时刻障碍物中心估计位置纵坐标，xij为第i时刻雷达扫描出的第j障碍物扫描点的横坐标，yij为第i时刻雷达扫描出的第j障碍物扫描点的纵坐标；

相邻时刻障碍物估算位置之间的距离：

若则判定障碍物为静态障碍物；若则判定障碍物为动态障碍物；当障碍物为动态usv时，根据两次障碍物中心估算位置的变化和2个时刻的时间间隔估算出动态目标的运动速度和航向，动态目标的运动速度用下式表示：

动态目标的航向由下式表示：

其中，δt为两个连续时刻的时间间隔；

另一usv速度在横纵方向上的分量为：

同理已知本usv的速度和艏向β计算出横坐标方向上的分量和纵坐标方向上的分量那么两usv的相对速度为：

usv之间的相对速度大小：

由估算的中心位置和此时本usv位置估算出usv之间的相对距离和相对方位，usv之间的相对距离由下式表示：

usv之间的相对方位由下式表示：

其中，(x0,y0)为本usv的位置；

usv之间的最近会遇距离dcpa：

其中，dcpa的符号由usv之间的相对位置决定，当另一usv位于本usv的船艏方向时，dcpa为正；当另一usv位于本usv的船尾方向时，dcpa为负；

到达最近会遇地点的时间tcpa：

其中，tcpa的符号由usv之间的相对位置决定，当usv未到达最近相遇地点时，tcpa为正；当usv超过最近相遇地点时，tcpa为负；

空间碰撞危险度是衡量usv之间发生碰撞可能性大小的度量之一，影响空间碰撞危险度大小的因素主要包括usv之间的最近会遇距离dcpa、最近会遇地点相对本usv的距离和方向、另一usv所在方向和距离以及无人航行器之间的最低安全会遇距离；

其中，dcpa为usv之间的最近会遇距离，u为dcpa的隶属函数值，d1为本usv最晚进行避碰时到另一usv的距离，d2为恰好构成碰撞紧迫局面时本usv与另一usv的临界距离；当d2＜|dcpa|时，本usv航行安全；当|dcpa|＜d1时，本usv与另一usv存在碰撞危险，需要采取避碰措施来消除；当d1＜|dcpa|＜d2时，不确定是否存在碰撞危险，需要计算碰撞危险度来判定；

时间碰撞危险度是衡量usv之间发生碰撞可能性大小的度量之一，影响时间碰撞危险度大小的因素主要包括两个usv的航行速度、本usv与另一usv的距离、两个usv的长度；

其中，

综合空间碰撞危险度和时间碰撞危险度得出usv系统的碰撞危险度：

其中，的含义为：若ud＝0或ut＝0，则u＝0；若ud≠0且ut≠0，则u＝max(ud,ut)。

步骤3所述的ga评价函数表示为下式：

其中，d0为雷达坐标系的原点到障碍物的最短距离，d1表示本usv到另一usv的最短距离，d表示usv当前位置到对应终点的距离；μ为障碍物边界下限角，即雷达坐标系原点与雷达检测到的障碍物下边界点的夹角，ν为障碍物边界上限角，即雷达坐标系原点与雷达检测到的障碍物上边界点的夹角，δ为另一usv边界下限角，即雷达坐标系原点与雷达检测到的另一usv下边界点的夹角，γ为另一usv边界上限角，即雷达坐标系原点与雷达检测到的另一usv上边界点的夹角，f为评价函数值，其值衡为正，f值越大被选择成为子代的可能性越大；ωi+1为usv下一时刻选取第i个体的艏向角，评价函数采取奖惩机制增大区分度。

本发明的有益效果在于：

本发明在有/无通信的情况下利用通信模块的互通位置、速度、艏向等信息和雷达探测一定范围内的静态障碍物位置和动态usv位置、速度、艏向等信息辅助避碰规划，使多个usv从起点向终点航行过程中避开所有静态障碍物、usv之间不发生碰撞且不出现大角度转向、大范围加减速，且航行路径符合经济性要求。本发明致力提出一种适应于有/无通信情况下充分利用通信模块和雷达探测模块来辅助避碰的方法，并且制定合理的通信频率和通信内容来减轻系统的压力。本发明的关键点：设置合适的通信模块和雷达模块参数；设计有/无通信情况下的运动参数及避碰危险度计算方法；构建充分利用通信模块和雷达模块作用的有/无通信情况下的遗传算法ga评价函数。

附图说明

图1是有通信情况下两个usv相对运动的参数图。

图2是雷达搜索图。

图3是多usv任务有通信式避碰规划仿真效果图。

图4是多usv任务无通信式避碰规划仿真效果图。

图5是多usv任务有通信式避碰规划速度调节量变化趋势图。

图6是多usv任务有通信式避碰规划艏向调节量变化趋势图。

图7是多usv任务有通信式避碰规划三艇相对距离变化趋势图。

图8是多usv任务无通信式避碰规划速度调节量变化趋势图。

图9是多usv任务无通信式避碰规划艏向调节量变化趋势图。

图10是多usv任务有通信式避碰规划遗传算法迭代优化图。

图11是多usv任务无通信式避碰规划三艇相对距离变化趋势图。

图12是多usv任务无通信式避碰规划遗传算法迭代优化图。

具体实施方式

下面结合附图对本发明做进一步描述。

本发明的目的是提供有/无通信情况下的多usv群体协同避碰规划方法，有通信时充分利用通信模块互通的位置、速度、艏向等信息辅助避碰规划，无通信时则充分利用雷达模块在一定范围内探测到的障碍物位置与其他usv的位置、速度、艏向等信息辅助避碰规划，以使usv从起点出发驶向终点的过程中避开环境中静态障碍物、usv之间不发生碰撞、不出现大角度转向、大范围加减速，且航行路径符合经济性要求。

本发明为解决上述技术问题采取的技术方案是：

多usv群体协同避碰规划方法中采用几何环境模型建模来描述usv航行环境、usv实时位置等，环境地图赋值法表示存在障碍物和其他usv。

所述usv群体协同避碰规划方法的实现过程为：

步骤一、选取适当参数构建通信模块和雷达探测模型，有通信时利用通信模块互通的位置、速度、艏向等信息和雷达探测一定范围内的静态障碍物位置和动态usv位置、速度、艏向等信息计算多usv运动参数及碰撞危险度，无通信时利用雷达模块探测一定范围内的静态障碍物位置和动态usv位置、速度、艏向等信息，从而判断是否需要采取避碰措施及采取的时间并根据相对距离设置通信频率。

步骤二、利用步骤一中有/无通信时通信或者探测到的静态障碍物和动态usv的信息构建有/无通信情况下遗传算法评价函数。

步骤三、选择qt软件构建usv避碰规划仿真软件平台，添加通信模块、雷达探测模块调试usv避碰规划软件，设计典型的仿真案例验证算法的有效性。

雷达模块参数设置如表1:

表1雷达参数表

步骤一中多usv运动参数及碰撞危险度的计算过程如下：

如图1，有通信情况下两个usv相对运动的参数图，设本usv的大地惯性坐标为m(xm,ym)，其运动速度为vm(vxm,vym)，另一usv的大地惯性坐标为n(xn,yn)，其运动速度为vn(vxn,vyn)，在有通信情况下，usv之间可通过通信模块得知对方的实时位置，那么usv之间的相对距离：

另一usv相对于本usv的方位：

γ＝α-β

usv之间的航向交叉角：

η＝βn-βm

usv之间的相对速度表示为：

usv之间的相对速度大小：

usv之间相对速度的方向：

如图1，在无通信情况下，usv之间只能通过导航雷达测量静态障碍物位置和其他usv位置、速度、艏向参数。这里取雷达探测到障碍物后的2个连续时刻检测到的障碍物信息，其中第i时刻雷达探测范围内探测出有障碍物的扫描点集合为：

{(xi1，yi1)，(xi2，yi2)，，(xin，yin)}

其中，探测出的扫描点集合的坐标为大地环境坐标系下的坐标值，且每一时刻探测出的扫描点集合点数不一定相等。

由障碍物的扫描点集合可估算出障碍物的中心位置：

其中，为第i时刻障碍物中心估计位置横坐标，为第i时刻障碍物中心估计位置纵坐标，xij为第i时刻雷达扫描出的第j障碍物扫描点的横坐标，yij为第i时刻雷达扫描出的第j障碍物扫描点的横坐标。

相邻时刻障碍物估算位置之间的距离：

这里设置一个阈值，如果则判定障碍物为静态障碍物；如果则判定障碍物为动态障碍物。综合考虑usv航行情况，这里设置阈值为0.2海里。当障碍物为动态usv时，可以根据两次障碍物中心估算位置的变化和2个时刻的时间间隔估算出动态目标的运动速度和航向：

其中，t为两个连续时刻的时间间隔。

另一usv速度在横纵方向上的分量为：

同理已知本usv的速度和艏向β可计算出横纵方向上的分量那么两usv的相对速度为：

usv之间的相对速度大小：

由估算的中心位置和此时本usv位置可估算出usv之间的相对距离和相对方位：

其中，(x0,y0)为本usv的位置。

usv之间的最近会遇距离dcpa：

其中，dcpa的符号由usv之间的相对位置决定，当另一usv位于本usv的船艏方向时，dcpa为正；当另一usv位于本usv的船尾方向时，dcpa为负。

到达最近会遇地点的时间tcpa：

其中，tcpa的符号由usv之间的相对位置决定，当usv未到达最近相遇地点时，tcpa为正；当usv超过最近相遇地点时，tcpa为负。

空间碰撞危险度是衡量usv之间发生碰撞可能性大小的度量之一，影响空间碰撞危险度大小的因素主要包括usv之间的最近会遇距离dcpa、最近会遇地点相对本usv的距离和方向、另一usv所在方向和距离以及无人航行器之间的最低安全会遇距离。

其中，dcpa为usv之间的最近会遇距离，u为dcpa的隶属函数值，d1为本usv最晚进行避碰时到另一usv的距离，d2为恰好构成碰撞紧迫局面时本usv与另一usv的临界距离。当d2＜|dcpa|时，本usv航行安全；当|dcpa|＜d1时，本usv与另一usv存在碰撞危险，需要采取避碰措施来消除；当d1＜|dcpa|＜d2时，不确定是否存在碰撞危险，需要计算碰撞危险度来判定。

则d1＝1.5ρ(θt)、d2＝2d1。

时间碰撞危险度是衡量usv之间发生碰撞可能性大小的度量之一，影响时间碰撞危险度大小的因素主要包括两个usv的航行速度、本usv与另一usv的距离、两个usv的长度。

其中，

综合空间碰撞危险度和时间碰撞危险度得出usv系统的碰撞危险度：

其中，的含义为：

若ud＝0或ut＝0，则u＝0；

若ud≠0且ut≠0，则u＝max(ud,ut)。

步骤二中构建遗传算法评价函数的过程如下：

如图2的usv雷达搜索图，其模拟同时搜索到障碍物与其他usv的状态，图中坐标系为本usv雷达随动坐标系，圆表示雷达检测的范围。其中a为障碍物，o1为另一usv的雷达坐标系原点，d0为雷达坐标系的原点到障碍物的最短距离，d1表示本usv到另一usv的最短距离，d表示usv当前位置到对应终点的距离。μ为障碍物边界下限角(雷达坐标系原点与雷达检测到的障碍物下边界点的夹角)，ν为障碍物边界上限角(雷达坐标系原点与雷达检测到的障碍物上边界点的夹角)。δ为另一usv边界下限角(雷达坐标系原点与雷达检测到的另一usv下边界点的夹角)，γ为另一usv边界上限角(雷达坐标系原点与雷达检测到的另一usv上边界点的夹角)。那么，本文中usv避碰规划的评价函数取为：

式中，f为评价函数值，其值衡为正，f值越大被选择成为子代的可能性越大。ωi+1为usv下一时刻选取第i个体的艏向角，d为当前位置到终点的距离。评价函数采取奖惩机制增大区分度，当下一时刻的艏向角在障碍物边界上下限角范围内或者另一usv边界上下限角范围内，继续航行会接近障碍物或另一usv，此时对这样个体的评价函数值缩小100倍使其被选择的概率减小；当下一时刻的艏向角不在障碍物边界上下限角范围内和另一usv边界上下限角范围内，继续航行能避开障碍物，此时对这样个体的评价函数值扩大100倍使其被选择的概率增大。

步骤三多usv群体协同避碰规划方法仿真验证过程如下：

进行多usv任务有/无通信式避碰规划实验时，首先设置多usv对应的起点、终点位置，这里可以设置任意位置，为了更好的实验效果这里选择3-5个usv；再设置航行环境中障碍物的形状、大小、位置，任意选择障碍物的数量形状大小位置参数；设置usv启动避碰的距离为2海里，最小安全距离为0.2海里；最后选择多usv实验的任务有/无通信的控制模式开始实验。

如图3的多usv任务有通信式避碰规划实验仿真效果图，实验中usv从起点向终点出发，当usv离障碍物较远时沿直线航行，当usv航行到障碍物附近时通过雷达探测到了障碍物信息，从两侧绕开障碍物后继续向终点航行，且航行过程中usv之间没有发生碰撞，避碰效果良好。相比于多usv任务无通信式避碰规划运动仿真图，这里usv之间一直保持有安全距离，证明了通信模块的作用。

如图4的多usv任务无通信式避碰规划实验仿真效果图，实验中usv从起点向终点出发，当usv离障碍物较远时沿直线航行，当usv航行到障碍物附近时通过雷达探测到了障碍物信息，从两侧绕开障碍物后继续向终点航行，并且usv之间未发生碰撞，避碰效果良好。

如图5的多usv任务有通信式避碰规划速度调节量变化趋势图和图6多usv任务有通信式避碰规划艏向调节量变化趋势图，速度和艏向变化趋势与实验效果图相对应，效果图中轨迹曲线和速度艏向具有较好的平滑性。如图7多usv任务有通信式避碰规划三艇相对距离变化趋势图显示的航行过程中三个usv的相对距离变化趋势，usv之间的距离始终大于一定的阈值，所以这里很少存在紧急的会遇局势。相比于多usv任务无通信式避碰规划，usv之间的距离始终大于50海里，这是因为usv之间通过通信模块互相得知对方的位置、航速、艏向等信息，控制usv提前改变航速、航向进行躲避，避免出现紧急相遇情况。当其他条件不变时，改变通信频率，发现通信频率越高usv之间保持一定距离的效果越好，考虑到通信模块远距离通信的自身的限制，这里设置距离大于50海里时为十分钟通信一次，距离小于50海里时一分钟通信一次。

如图8的多usv任务式避碰规划速度调节量变化趋势图和图9多usv任务无通信式避碰规划艏向调节量变化趋势图，速度艏向变化趋势与实验效果图相对应，效果图中轨迹曲线和速度艏向具有较好的平滑性。

如图10的多usv任务有通信式避碰规划遗传算法迭代优化图，图中显示的优化过程中种群个体评价函数值的变化趋势随着算法的迭代次数增多，种群个体的评价函数值增大，属于极大值迭代优化，且每个usv的优化过程都大致相同。相比于多usv任务无通信式避碰规划优化趋势图，这里收敛更加快速。如果保持其他参数不变，改变迭代次数多次试验，比较不同迭代次数的运行效果，测试出迭代次数500最合适。经程序测试，程序完成一次500代的寻优大概需要1.1s。

如图11多usv任务无通信式避碰规划三艇相对距离变化趋势图显示的航行过程中三个usv的相对距离变化趋势，usv之间并未发生相撞。如图12多usv任务无通信式避碰规划遗传算法迭代优化图显示的优化过程中种群个体评价函数值的变化趋势，每个usv评价函数值的变化趋势都大致相似，即算法的迭代次数越多，种群个体的评价函数值越大，为最大值优化。保持其他参数不变，改变迭代次数多次试验，测试出迭代次数500最合适。