改进双向快速搜索随机树的无人艇路径规划方法技术

本发明专利技术涉及水面无人艇路径规划技术领域，具体为一种改进双向快速搜索随机树的无人艇路径规划方法，包括以下步骤：确定起始点Xstart和目标点Xgoal，并对地图二值化处理，将不可航行区域表示为黑色，可航行水域表示为白色，对障碍物进行膨胀处理；判断Xstart和目标点Xgoal之间的连线是否有障碍物，若无障碍物，则路径搜索成功，否则进行下一步。本发明专利技术结合极度贪心算法的思想，使得双向快速搜索随机树在障碍物较少的环境中能够快速的找到路径。在障碍物较少的环境中能够快速的找到路径。在障碍物较少的环境中能够快速的找到路径。

【技术实现步骤摘要】
改进双向快速搜索随机树的无人艇路径规划方法


[0001]本专利技术涉及水面无人艇路径规划
，具体为一种改进双向快速搜索随机树的无人艇路径规划方法。

技术介绍

[0002]水面无人艇是一种轻量化、智能化的水上交通工具，具有操纵性好、成本较低等优点，常被用于执行近海水域的海洋调查、环境监测等工作。由于水面无人艇适用于复杂的工况，因此需要具备更强的自主性，而路径规划是其自主系统的重要基础。目前，水面无人艇的算法可分为基于图搜索的算法、基于仿生智能算法和基于随机抽样的算法。与其他先进的算法相比，基于随机抽样的算法的主要优点是避免了障碍物配置空间的明确构建，以随机采样的方式在任务空间中采样，能够确保概率上的完整性，意味着当迭代次数增加到无穷大时，若路径存在就一定能找到。基于随机抽样的算法主要包括随机采样法和快速搜索随机树法。PRM将连续空间分割成若干个离散空间，然后再利用其他算法寻找路径，但由于分割离散间过程的随机性，使得随机采样法搜索路径的效率不高。相比之下，快速搜索随机树算法具有模型简单、搜索速度快等优点。因此，为了在实践中得出高效的运动规划解决方案，各种改进的快速搜索随机树算法应运而生。虽然现有的双向快速搜索随机树算法具有强大的搜索能力，但存在规划路径随机采样点较为盲目、时间长、转折点角度较大等缺点。

技术实现思路

[0003]针对现有技术的不足，本专利技术提供了一种改进双向快速搜索随机树的无人艇路径规划方法。
[0004]为实现以上目的，本专利技术通过以下技术方案予以实现：一种改进双向快速搜索随机树的无人艇路径规划方法，包括以下步骤：
[0005]步骤1：确定起始点Xstart和目标点Xgoal，并对地图二值化处理，将不可航行区域表示为黑色，可航行水域表示为白色，对障碍物进行膨胀处理；
[0006]步骤2：判断Xstart和目标点Xgoal之间的连线是否有障碍物，若无障碍物，则路径搜索成功，否则进行下一步；
[0007]步骤3：首先对第一棵搜索树节点扩展，通过基于高斯分布的目标偏置确定随机点X1
‑
rand；
[0008]步骤4：通过启发式代价确定X1near；
[0009]步骤5：判断X1near与X1rand的连线与上一扩展节点是否满足角度约束，若满足执行下一步，否则返回步骤3；
[0010]步骤6：从X1near向X1rand方向延长步长的距离，判断连线是否可视，若可视，得到确定X1new，否则返回步骤3；
[0011]步骤7：判断X1new得是否可以与另一棵树上最近的节点之间是否有障碍物，若无障碍物，路径搜索完成，否则执行下一步；
[0012]步骤8：同理对随机树T2进行扩展，记录随机树T2点和边的集合；
[0013]步骤9：依次交替循环迭代，直至两颗树成功连接，判断连接点是否满则角度约束，若不满足，则返回步骤3，否则进行下一步；
[0014]步骤10：对初始路径进行剪枝优化处理；
[0015]步骤11：对剪枝后的路径进行三次B样条优化处理。
[0016]优选地，步骤3中，随机点X1
‑
rand的计算公式如下：
[0017][0018]式中rand为地图中的随机点；p为(0,1)之间的随机数；η,λ为目标偏向阈值；f(x,μ,B)为二元高斯联合分布密度函数，其中x为目标点的坐标，μ为目标点的坐标值，B为协方差矩阵，高斯随机点取值如下：
[0019][0020]协方差矩阵会影响二维高斯随机点的分布，调整将二维高斯随机点分布的长轴方向与起始点和目标点方向垂直，θ1为起始点和目标点的连线与水平轴的夹角；θ2为二维协方差不为零的高斯点分布的长轴与起始点和目标点连线的夹角；θ3为随机点需要旋转的角度。
[0021][0022]θ2＝π/4
‑
θ1[0023]θ3＝π/2
‑
θ2ꢀꢀꢀꢀ
(15)
[0024][0025]式中x
rand'
,y
rand'
为旋转后得到的新的坐标点；x
rand
,y
rand
为高斯随机点；x,y为高斯随机点的均值。同理，可以得到以Xgoal为中心的随机点。
[0026]优选地，步骤4通过启发式代价确定X1near，根据A
‑
Star算法中的启发式路径代价的思想计算X1near，X1near的计算如下式：
[0027]x
near
＝min(Cost(x
tree
,x
start
)+dis(x
tree
,x
goal
))
ꢀꢀꢀꢀ
(17)
[0028]其中Cost(x
tree
,x
start
)为搜索树上的节点至起始点的路径之和；dis(x
tree
,x
goal
)为搜索树上的节点到目标点的欧式距离。
[0029]优选地，步骤5中，对随机树的扩展进行了角度的约束，若X1near与X1rand的连线与上一扩展节点夹角θ小于θ
max
，则连接生成的路径。θ的计算公式如下：
[0030][0031]式中，X1
‑
parent为X1
‑
near的父节点。
[0032]优选地，步骤9中对两棵树的连接处进行角度的约束，当下式中满足θ5≤θ
max
且θ6≤θ
max
时则说明两棵树满足角度约束。
[0033][0034][0035]优选地，步骤10中对初始路径进行剪枝优化处理，具体步骤如下：输入初始路径的节点P(i)，P(i)＝[P(0),P(2),P(3),P(4),
……
,P(s)]；将P(0)为优化起始点，Pnew＝[P(0)]，判断P(0)与P(s)是否可以直接连通，若可以连通则优化程序结束，否则执行下一步骤；依次判断P(0)与P(s
‑
1)是否可以连通，若不能连通，则s＝s
‑
1,继续循环此过程，若可以连通，将P(i)视为优化起始点，Pnew＝[P(0),P(i)],不断循环此过程，若P(i)与P(s)可以连通，则结束优化程序，Pnew＝[P(0),P(i)
……
P(s)]。
[0036]优选地，步骤11中对剪枝后的路径进行三次B样条优化处理，三次B样条的计算公式如下：
[0037][0038]其中基函数B
i,3
的求法如下：
[0039]B
0,3
(x)＝(
‑
x3+3x2‑
3x+1)/3！
[0040]B
1,3
(x)＝(3x2‑
6x+4)/3！
[0041]B
2,3
(x)＝(<本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种改进双向快速搜索随机树的无人艇路径规划方法，其特征在于：包括以下步骤：步骤1：确定起始点Xstart和目标点Xgoal，并对地图二值化处理，将不可航行区域表示为黑色，可航行水域表示为白色，对障碍物进行膨胀处理；步骤2：判断Xstart和目标点Xgoal之间的连线是否有障碍物，若无障碍物，则路径搜索成功，否则进行下一步；步骤3：首先对第一棵搜索树节点扩展，通过基于高斯分布的目标偏置确定随机点X1
‑
rand；步骤4：通过启发式代价确定X1near；步骤5：判断X1near与X1rand的连线与上一扩展节点是否满足角度约束，若满足执行下一步，否则返回步骤3；步骤6：从X1near向X1rand方向延长步长的距离，判断连线是否可视，若可视，得到确定X1new，否则返回步骤3；步骤7：判断X1new得是否可以与另一棵树上最近的节点之间是否有障碍物，若无障碍物，路径搜索完成，否则执行下一步；步骤8：同理对随机树T2进行扩展，记录随机树T2点和边的集合；步骤9：依次交替循环迭代，直至两颗树成功连接，判断连接点是否满则角度约束，若不满足，则返回步骤3，否则进行下一步；步骤10：对初始路径进行剪枝优化处理；步骤11：对剪枝后的路径进行三次B样条优化处理。2.根据权利要求1所述的改进双向快速搜索随机树的无人艇路径规划方法，其特征在于：步骤3中，随机点X1
‑
rand的计算公式如下：式中rand为地图中的随机点；p为(0,1)之间的随机数；η,λ为目标偏向阈值；f(x,μ,B)为二元高斯联合分布密度函数，其中x为目标点的坐标，μ为目标点的坐标值，B为协方差矩阵，高斯随机点取值如下：协方差矩阵会影响二维高斯随机点的分布，调整将二维高斯随机点分布的长轴方向与起始点和目标点方向垂直，θ1为起始点和目标点的连线与水平轴的夹角；θ2为二维协方差不为零的高斯点分布的长轴与起始点和目标点连线的夹角；θ3为随机点需要旋转的角度。θ2＝π/4
‑
θ1θ3＝π/2
‑
θ
2 (4)
式中x
rand'
,y
rand'
为旋转后得到的新的坐标点；x
rand
,y
rand
为高斯随机点；x,y为高斯随机点的均值。同理，可以得到以Xgoal为中心的随机点。3.根据权利要求1所述的改进双向快速搜索随机树的无人艇路径规划方法，其特征在于：步骤4通过启发式代价确定X1near，根据A
‑
Star算法中的启发式路径代价的思想计算X1near，X1near的计算如下式：x
near
＝min(Cost(x
tree
,x
start
)+dis(x

【专利技术属性】
技术研发人员：王晨旭，赵贵祥，李云淼，王贺平，
申请(专利权)人：天津大学，
类型：发明
国别省市：