一种基于航迹重规划的水面无人艇动态避碰方法技术

本发明专利技术公开了一种基于航迹重规划的水面无人艇动态避碰方法，包含以下步骤：通过艇载传感器获取邻域船舶的航行信息和无人艇自身的位姿信息；构造无人艇和邻域船舶之间的碰撞锥；引入对邻域艇舶运动信息观测的不确定度，对碰撞锥施加一层软约束；施加无人艇的速度和艏向限制范围；获得最终的候选速度集合；引入代价函数选择最优避碰速度；以最优避碰速度进行航行模拟的内循环，得到无人艇动态避碰的航迹重规划点。本发明专利技术将水面无人艇的动态避碰决策以航迹重规划的形式输出，满足国际海上避碰规则的约束，能够很好的适应无人艇自身的操纵控制，满足无人艇的动态避碰需求。

A dynamic collision avoidance method for surface unmanned vehicle based on path re planning

【技术实现步骤摘要】
一种基于航迹重规划的水面无人艇动态避碰方法
本专利技术涉及无人艇自主智能控制的
，特别涉及一种基于航迹重规划的水面无人艇动态避碰方法。
技术介绍
水面无人艇在军用方面作为未来海洋战争的先驱是国家维护海洋权益的重要工具，在民用方面可以替代船员执行任务，从而有效减少人工作业量和降低作业危险性，在军事作战、海域巡逻、岛礁补给等领域具有广泛的应用需求。由于海洋环境复杂，存在较大的不确定性，因此水面无人艇具备高度智能化的避碰水平是其在复杂海域安全航行的关键。目前我国虽然在水下无人潜航器上取得了一定的成果，但在水面无人艇自主智能化技术，尤其是水面无人艇的动态避碰技术上面仍未取得显著突破，因此，研究一种基于航迹重规划的水面无人艇动态避碰方法，对于提高水面无人艇自主智能化水平，实现水面无人艇军用、民用的普及具有重大意义。
技术实现思路
本专利技术的目的在于克服现有技术的缺点与不足，提供一种基于航迹重规划的水面无人艇动态避碰方法。本专利技术针对水面无人艇的动态避碰问题，引入基于方向包围盒的速度障碍区域几何模型描述和基于速度不确定度的速度障碍区域运动模型描述进行速度障碍区域的建模。结合水面无人艇本体动力学特性和国际海上避碰规则对水面无人艇可行速度集合进行了进一步约束，引入代价函数从中计算最优避障速度并进行航迹重规划。本专利技术的目的至少通过如下技术方案之一实现。本专利技术提供了一种基于航迹重规划的水面无人艇动态避碰方法，包括以下步骤：S1、采集艇载传感器的实时数据，获取邻域航行船舶的几何尺寸信息、运动信息和水面无人艇本体的位姿信息；S2、结合邻域航行船舶的几何尺寸信息、运动信息和水面无人艇本体的位姿信息，采用方向包围盒描述邻域航行船舶和水面无人艇的几何信息，建立水面无人艇和邻域航行船舶碰撞时的速度集合，即构造水面无人艇和邻域航行船舶之间的碰撞锥；S3、引入对邻域航行船舶速度信息观测的不确定度，将碰撞锥向外平移一个不确定度上界值的量，即对碰撞锥施加一层软约束，以约束外的速度集合为无碰速度集；S4、结合水面无人艇本身的动力学特性，求解水面无人艇的速度和艏向限制范围，与无碰速度集进行交集运算，获得水面无人艇的可行无碰速度集；S5、结合国际海上避碰规则的约束，根据不同的会遇局面对可行无碰速度集进行划分，获得最终的候选速度集合；S6、对候选速度集合进行离散化处理，引入代价函数对其中的每个速度进行评价，选择代价最小的速度为最优避碰速度；S7、以最优避碰速度进行航行模拟的内循环，得到水面无人艇动态避碰的航迹重规划点。优选地，所述步骤S1具体为：使用AIS系统获取邻域航行船舶的几何尺寸信息和运动信息，使用GPS获取水面无人艇的经纬度，使用电子罗经获取水面无人艇的三维航向角。优选地，所述步骤S2具体为：沿着水面无人艇和邻域航行船舶的主成分方向生成一个最小的矩形包围框来统一相应描述水面无人艇和邻域航行船舶的形状，将水面无人艇自身的几何尺寸沿着航行方向叠加到邻域航行船舶上，具体过程为以邻域航行船舶矩形包围框边线上的点为几何中心，不断叠加水面无人艇矩形包围框，将叠加后的最外围区域视为障碍物区域，从而将水面无人艇视为质点；从质点出发，作障碍物区域的两条切线，将所述两条切线包围的区域为速度障碍区域，所述切线为一条从水面无人艇几何中心Pu沿相对速度vr出发的射线：λ(Pu,vr)＝{Pu+vrt|t≥0}(1)，(1)式中t代表时间，vr为水面无人艇相对于邻域航行船舶的速度vr＝vu-vo，Pu和vu分别表示水面无人艇的位置矢量和速度矢量，vo表示邻域航行船舶的速度矢量；当射线λ(Pu,vr)与速度障碍区域相交时，也即射线落在速度障碍区域的两条切线之间时，可以认为水面无人艇与邻域航行船舶在某一时刻必然发生碰撞；定义导致水面无人艇和邻域航行船舶发生碰撞的相对速度vr集合为相对速度集下的碰撞锥RCCUO；定义导致水面无人艇和邻域航行船舶发生碰撞的所有水面无人艇绝对速度vu集合为绝对速度集下的碰撞锥VOUO：(2)式中，为闵可夫斯基矢量和运算，实际物理意义是将相对碰撞区域RCCUO沿着邻域航行船舶速度vo进行平移。优选地，所述步骤S3具体为：在构建碰撞锥后引入邻域航行船舶的速度不确定度δO，将邻域航行船舶速度参量中的所有不确定部分都归为集合{WO|δO∈WO}，WO集合是一个有界集合且其上界为常数，将碰撞锥向外平移一个不确定度上界值的量，故而可以构建不确定度最大时即最坏情况下的碰撞锥区域WVOUO：由于动态障碍物的速度不确定集合上界为一个常数，因此在原碰撞锥的基础上大了一圈，记为WVOUO，相当于为水面无人艇局部避障提供了具有安全缓冲作用的软约束，将WVOUO外的速度集合记为无碰速度集。优选地，所述步骤S4具体为：结合水面无人艇本身的动力学特性，引入动态窗口的概念对水面无人艇的可行速度空间做进一步的约束，给定一个时间窗口Δt，计算水面无人艇在Δt时间内可以达到的速度窗口vd和角速度窗口ωd，(4)式和(5)式中，ω、ν分别为水面无人艇的角速度、速度；vc和ωc分别表示水面无人艇当前的速度和角速度，和分别为水面无人艇加速情况下的最大加速度和角加速度，和分别为水面无人艇减速情况下的最大加速度和角加速度；其中，(6)式和(7)式中，RT为水面无人艇和邻域船舶之间的相对距离以使水面无人艇在碰到会遇船舶之前能够停下来；水面无人艇在Δt时间内可以达到的艏向窗口θd：(8)式中，θ表示水面无人艇的艏向角；θc表示水面无人艇当前的艏向角；通过速度窗口vd和艏向窗口θd即可构建水面无人艇的可行速度集，与步骤S3获得的无碰速度集进行交集运算，获得无人艇的可行无碰速度集RAV。优选地，所述步骤S5具体为：引入分割线将水面无人艇的绝对速度集合即绝对速度空间划分为4个区域，分别为WVOUO、V1、V2和V3，其中分割线与速度障碍区域两条切线的角平分线相垂直，并且与WVOUO的底端相切；V1区域的速度表示水面无人艇从左舷通过邻域航行船舶，V1区域的数学描述如下：(9)式中，[]z用于提取向量的z轴分量；在这里以水面无人艇几何中心为圆点，正前方为x轴正向，正右方为y轴正向，z轴正向指向纸面向内；V2区域的速度表示水面无人艇从右舷通过邻域航行船舶，V2区域的数学描述如下：V3区域的速度表示水面无人艇背离邻域航行船舶，V3区域的数学描述如下：结合国际海上避碰规则，可以得出满足国际海上避碰规则的最终候选速度集合CRAV，当水面无人艇与邻域航行船舶处于追越或者左交叉局面时，将水面无人艇的速度约束在V1和V3区域，CRAV＝{v|v∈RAV,v∈V1∪V3}(12)，当水面无人艇与邻域航行船舶处于对遇或者右交叉局面时，将水面无人艇的速度约束在V2和V3区域，CRAV＝{v|v∈RAV,v∈V2∪V3}(13)，(9)至(11)式中Po表示邻域航行船舶的位置矢量，(9)至(13)式中v为水面无人艇的绝对速度。优选地，所述步骤S6具体为：对候选速度集合中的速度v和艏向θ进行离散化网格处理，具体可将速度v按等间距网格离散为M个离散速度，将艏向θ按等间距网格离散为个N离散艏向，每一个离散速度和离散艏向组成一个完整的速度矢量，最终将整个集合离散为M×N个速度矢量；其中M和N是自然数，M＝INT((vmax-v本文档来自技高网...

【技术保护点】
1.一种基于航迹重规划的水面无人艇动态避碰方法，其特征在于，包括以下步骤：S1、采集艇载传感器的实时数据，获取邻域航行船舶的几何尺寸信息、运动信息和水面无人艇本体的位姿信息；S2、结合邻域航行船舶的几何尺寸信息、运动信息和水面无人艇本体的位姿信息，采用方向包围盒描述邻域航行船舶和水面无人艇的几何信息，建立水面无人艇和邻域航行船舶发生碰撞时的速度集合，即构造水面无人艇和邻域航行船舶之间的碰撞锥；S3、引入对邻域航行船舶速度信息观测的不确定度，将碰撞锥向外平移一个不确定度上界值的量，即对碰撞锥施加一层软约束，以约束外的速度集合为无碰速度集；S4、结合水面无人艇本身的动力学特性，求解水面无人艇的速度和艏向限制范围，与无碰速度集进行交集运算，获得水面无人艇的可行无碰速度集；S5、结合国际海上避碰规则的约束，根据不同的会遇局面对可行无碰速度集进行划分，获得最终的候选速度集合；S6、对候选速度集合进行离散化处理，引入代价函数对其中的每个速度进行评价，选择代价最小的速度为最优避碰速度；S7、以最优避碰速度进行航行模拟的内循环，得到水面无人艇动态避碰的航迹重规划点。

【技术特征摘要】
1.一种基于航迹重规划的水面无人艇动态避碰方法，其特征在于，包括以下步骤：S1、采集艇载传感器的实时数据，获取邻域航行船舶的几何尺寸信息、运动信息和水面无人艇本体的位姿信息；S2、结合邻域航行船舶的几何尺寸信息、运动信息和水面无人艇本体的位姿信息，采用方向包围盒描述邻域航行船舶和水面无人艇的几何信息，建立水面无人艇和邻域航行船舶发生碰撞时的速度集合，即构造水面无人艇和邻域航行船舶之间的碰撞锥；S3、引入对邻域航行船舶速度信息观测的不确定度，将碰撞锥向外平移一个不确定度上界值的量，即对碰撞锥施加一层软约束，以约束外的速度集合为无碰速度集；S4、结合水面无人艇本身的动力学特性，求解水面无人艇的速度和艏向限制范围，与无碰速度集进行交集运算，获得水面无人艇的可行无碰速度集；S5、结合国际海上避碰规则的约束，根据不同的会遇局面对可行无碰速度集进行划分，获得最终的候选速度集合；S6、对候选速度集合进行离散化处理，引入代价函数对其中的每个速度进行评价，选择代价最小的速度为最优避碰速度；S7、以最优避碰速度进行航行模拟的内循环，得到水面无人艇动态避碰的航迹重规划点。2.根据权利要求1所述的基于航迹重规划的水面无人艇动态避碰方法，其特征在于，所述步骤S1具体为：使用AIS系统获取邻域航行船舶的几何尺寸信息和运动信息，使用GPS获取水面无人艇的经纬度，使用电子罗经获取水面无人艇的三维航向角。3.根据权利要求1所述的基于航迹重规划的水面无人艇动态避碰方法，其特征在于，所述步骤S2具体为：沿着水面无人艇和邻域航行船舶的主成分方向生成最小的矩形包围框来分别统一相应描述水面无人艇和邻域航行船舶的形状，将水面无人艇自身的几何尺寸沿着航行方向叠加到邻域航行船舶上，具体过程为以邻域航行船舶的矩形包围框边线上的点为几何中心，叠加水面无人艇的矩形包围框，将叠加后的最外围区域视为障碍物区域，从而将水面无人艇视为质点；从质点出发，作障碍物区域的两条切线，将所述两条切线包围的区域为速度障碍区域，所述切线为一条从水面无人艇几何中心Pu沿相对速度vr出发的射线：λ(Pu,vr)＝{Pu+vrt|t≥0}(1)，(1)式中t代表时间，vr为水面无人艇相对于邻域航行船舶的速度vr＝vu-vo，Pu和vu分别表示水面无人艇的位置矢量和速度矢量，vo表示邻域航行船舶的速度矢量；当射线λ(Pu,vr)与速度障碍区域相交时，也即射线落在速度障碍区域的两条切线之间时，认为水面无人艇与邻域航行船舶在某一时刻必然发生碰撞；定义导致水面无人艇和邻域航行船舶发生碰撞的相对速度vr集合为相对速度集下的碰撞锥RCCUO；定义导致水面无人艇和邻域航行船舶发生碰撞的所有水面无人艇绝对速度vu集合为绝对速度集下的碰撞锥VOUO：VOUO＝RCCUO⊕vo(2)，(2)式中，⊕为闵可夫斯基矢量和运算，实际物理意义是将相对碰撞区域RCCUO沿着邻域航行船舶速度vo进行平移。4.根据权利要求3所述的基于航迹重规划的水面无人艇动态避碰方法，其特征在于，所述步骤S3具体为：在构建碰撞锥后引入邻域航行船舶的速度不确定度δO，将邻域航行船舶速度参量中的所有不确定部分都归为集合{WO|δO∈WO}，WO集合是一个有界集合且其上界为常数，将碰撞锥向外平移一个不确定度上界值的量，故而可以构建不确定度δO最大时的碰撞锥区域WVOUO：WVOUO＝VOUO⊕WO(3)，将WVOUO外的速度集合记为无碰速度集。5.根据权利要求4所述的一种基于航迹重规划的水面无人艇动态避碰方法，其特征在于，所述步骤S4具体为：结合水面无人艇本身的动力学特性，引入动态窗口的概念对水面无人艇的可行速度空间做进一步的约束，给定一个时间窗口Δt，计算水面无人艇在Δt时间内可以达到的速度窗口vd和角速度窗口ωd，(4)式和(5)式中，ν和ω分别为水面无人艇的速度和角速度；vc和ωc分别表示水面无人...

【专利技术属性】
技术研发人员：洪晓斌，朱坤财，魏新勇，
申请(专利权)人：华南理工大学，
类型：发明
国别省市：广东,44