考虑转角约束的无碰撞检测快速随机树全局路径规划方法

1.本发明属于无人驾驶路径规划和导航技术领域，具体涉及一种考虑转角约束的无碰撞检测快速随机树全局路径规划方法。


背景技术：

2.无人车路径规划就是在综合考虑行驶时间、速度、油耗和安全等因素的前提下，为无人车规划出最优等行驶轨迹。无人车为了完成在复杂的道路上的行驶，其自主移动能力则是基础，其中路径规划和导航是保证无人车自主移动能力的核心算法，是体现车辆智慧水平的关键。
3.从路径规划的目标范围看，通常分为全局路径规划和局部路径规划；从规划环境是否随时间变化方面看，还可以分为静态路径规划和动态路径规划。全局路径规划通常假设环境信息完全已知，在无人车运动之前，离线计算出一条从起点到终点的路径；但是环境并不是一成不变的，当环境信息改变时全局路径规划方法不能快速规划出一条安全的路径。


技术实现要素：

4.本发明的目的在于针对复杂狭窄环境智能车路径规划问题，提供一种考虑转角约束的无碰撞检测快速随机树全局路径规划方法，该方法使得无人车可以在全局路径的引领下，安全平稳的避开障碍物。
5.为实现上述目的，本发明的技术方案是：一种考虑转角约束的无碰撞检测快速随机树全局路径规划方法，包括如下步骤：
6.s1、建立无人车运动模型，并获取地图的可行区域信息与障碍物区域信息；
7.s2、启发式可变范围随机点采样策略：利用步骤s1的障碍物区域信息生成启发概率，此时产生0到1之间的随机数，若随机数大于启发概率，则在障碍物区域预定范围内的可行区域生成随机采样点；反之，在障碍物区域预定范围之外生成随机采样点；
8.s3、利用步骤s1建立的无人车运动模型得到车辆转角约束关系，引入考虑车辆转角约束的临近点采样策略，使得随机采样点的角度选取范围符合车辆转角约束特性；
9.s4、引入自适应步长目标偏向引导策略：目标偏向有时会导致随机采样点的角度选取超出步骤s3得到的车辆转角约束范围，此时随机树扩展方向偏离目标点，故对目标偏向后的角度进行归一化处理，使得处理后的角度在车辆转角约束范围内，且步长的选取与目标偏向角度呈反比关系，使随机树扩展方向尽快偏向目标点；
10.s5、通过步骤s1获取的地图的可行区域信息与障碍物区域信息，使随机采样点完成无需碰撞检测的随机树扩展；
11.s6、随机树找到目标点后，生成一条可行路径，算法对路径进行冗余点删除，并完成无碰撞的路径点简化；
12.s7、使用杜宾斯平滑曲线拟合步骤s6得到的路径简化点，得到符合车辆行驶特性
的路径。
13.在本发明一实施例中，所述步骤s1具体实现方式为：建立无人车运动模型并通过无人车自身携带的包括摄像头、激光雷达、gps、惯导及里程计的传感器的数据融合进行建模得到地图信息，对地图进行灰度化与二值化处理，将地图区分出可行区域与障碍物区域，获取可行区域与障碍物区域的坐标信息。
14.在本发明一实施例中，所述步骤s2具体实现方式为：由步骤s1获取的可行区域信息与障碍物区域信息，统计各区域坐标点个数；即状态空间坐标个数m、可行区域坐标个数m
f
与障碍物区域坐标个数m
o
；通过状态空间的障碍域系数c
o
对采样范围进行调节，同时利用障碍域系数c
o
确定采样点的启发概率p
h
，即调整在障碍物周围采样的概率；产生区间为(0,1)的随机数p
rand
，若p
rand
＜p
h
，在障碍物区域内进行随机采样，反之，在可行域内进行随机采样；具体计算公式如下：
[0015][0016]
式中：可行域系数c
f
为可行域坐标点个数m
f
与状态空间坐标点个数m的比值，障碍域系数c
o
为障碍域坐标点个数m
o
与状态空间坐标点个数m的比值，c
r
为可变范围系数，p
h
为可变范围内的采样启发概率，r为可变范围的大小。
[0017]
在本发明一实施例中，所述步骤s3具体实现方式为：
[0018]
设当前点q
current
的车辆姿态与下一个路径点q
next
的车辆姿态的夹角为则车辆转弯半径r与车辆转向角的关系为：
[0019][0020]
式中，b为步长；
[0021]
在随机树t中找出与随机点q
rand
距离最近的点q
i
，计算随机点q
rand
和当前点q
i
的连线与x轴的夹角q
i
和其父节点q
i
‑1的连线与x轴的夹角再比较与的角度差值θ的绝对值是否小于车辆最大转向角若小于车辆最大转向角，计算q
rand
与q
i
的距离d(q
i
,q
rand
)；比较d(q
i
,q
rand
)是否小于步长b，若小于步长b，在q
rand
与q
i
的连线上以d(q
i
,q
rand
)作为q
i
与q
i+1
的距离；若大于步长b，在q
rand
与q
i
的连线上以b作为q
i
与q
i+1
的距离；具体计算公式如下：
[0022][0023][0024]
式中，q
rand
为可行区域中生成的随机点，q
i
为距离q
rand
最近的点，q
i
‑1为q
i
的父节
点，为车辆最大转向角；d(q
i
,q
rand
)为q
rand
与q
i
的距离,d(q
i
，q
i+1
)为q
i
与其子节点q
i+1
的距离。
[0025]
在本发明一实施例中，所述步骤s4具体实现方式为：利用步骤s3中计算得到的与相减得到角度差值θ与车辆最大转向角的大小关系对d(q
i
，q
i+1
)与进行自适应调整，即当角度差值θ大于车辆最大转向角时，令角度差值θ等于车辆最大转向角当角度差值θ小于车辆最大转向角时，角度差值θ取其本身数值；具体计算公式如下：
[0026][0027][0028][0029]
式中，为随机点q
rand
和当前点q
i
的连线与x轴的夹角，为随机树t当前点q
i
和其父节点q
i+1
的连线与x轴的夹角，为经过自适应步长目标偏向引导策略采样的采样点q
i+1
和其父节点q
i
的连线与x轴的夹角，θ为与的角度差值，θ
′
为与的角度差值，d(q
i
，q
i+1
)为随机树t当前点q
i
与其子节点q
i+1
的距离，为车辆最大转向角。
[0030]
在本发明一实施例中，所述步骤s5具体实现方式为：将步骤s1获取的障碍物区域作为当前障碍物区域，将当前障碍物尺寸加上两倍步长作为运行时的障碍物区域；将快速随机树t的随机采样点设定为随机落在可行区域内，由此随机采样点总是落在距离实际障碍物两倍步长的位置，不与障碍物发生碰撞，免去传统rrt的碰撞检测环节，加快算法运算速度。
[0031]
在本发明一实施例中，所述步骤s6具体实现方式为：利用随机树t可得到从起始状态到目标状态的可行路径点，将起点q
start
作为第一次连接的开始点q
k
，开始依次连接后续的路径点，在两路径点间的连线不与障碍物发生碰撞的情况下，对开始点q
k
与路径点q
i
‑1之间的路径进行剔除；依次类推，直到不满足条件的路径点q
i
的父节点q
i
‑1作为新的开始点q
k
，重复上述操作，直到连接到目标点q
goal
。
[0032]
相较于现有技术，本发明具有以下有益效果：本发明方法使得无人车可以在全局路径的引领下，安全平稳的避开障碍物。
附图说明
[0033]
图1为本发明方法流程图。
[0034]
图2为转角约束示意图。
[0035]
图3为自适应步长目标偏向引导示意图。
[0036]
图4为传统rrt算法。
[0037]
图5为msf
‑
算法。
[0038]
图6为路径简化示意图。
[0039]
图7为路径平滑示意图。
具体实施方式
[0040]
下面结合附图，对本发明的技术方案进行具体说明。
[0041]
本发明一种考虑转角约束的无碰撞检测快速随机树全局路径规划方法，包括如下步骤：
[0042]
s1、建立无人车运动模型，并获取地图的可行区域信息与障碍物区域信息；
[0043]
s2、启发式可变范围随机点采样策略：利用步骤s1地图信息中的障碍物区域信息并生成启发概率，此时产生0到1之间的随机数，若随机数大于启发概率，则在障碍物区域一定范围内的可行区域生成随机采样点；反之，在障碍物区域一定范围之外生成随机采样点。以此限定随机采样点的选取范围；
[0044]
s3、利用步骤s1建立的无人车运动模型得到车辆转角约束关系，引入考虑车辆转角约束的临近点采样策略，使得随机采样点的角度选取范围符合车辆转角约束特性；
[0045]
s4、引入自适应步长目标偏向引导策略使算法加快向终点收敛的速度，但目标偏向有时会导致随机采样点的角度选取超出步骤s3得到的车辆转角约束范围，此时随机树扩展方向偏离目标点。故对目标偏向后的角度进行归一化处理，使得处理后的角度在车辆转角约束范围内，且步长的选取与目标偏向角度呈反比关系，使随机树扩展方向尽快偏向目标点；
[0046]
s5、通过步骤s1获取地图信息，使随机采样点完成无需碰撞检测的随机树扩展，加快算法运行速度；
[0047]
s6、随机树找到目标点后，生成一条可行路径，算法对路径进行冗余点删除，并完成无碰撞的路径点简化；
[0048]
s7、使用杜宾斯平滑曲线拟合步骤s6得到的路径简化点，得到符合车辆行驶特性的路径。
[0049]
以下为本发明具体实现过程。
[0050]
图1为本发明一种考虑转角约束的无碰撞检测快速随机树全局路径规划方法流程图，具体实现步骤如下：
[0051]
步骤一：地图预处理以获取地图信息，建立无人车运动模型；
[0052]
建立无人车运动模型并通过无人车自身携带的摄像头、激光雷达、gps、惯导及里程计等传感器的数据融合进行建模得到地图信息。算法对地图进行灰度化与二值化处理，将地图区分出可行区域与障碍物区域，获取可行区域与障碍物区域的坐标信息。
[0053]
步骤二：启发式可变范围随机点采样；
[0054]
在步骤一获取的可行区域与障碍物区域的坐标信息的基础上，统计各区域坐标点个数。即状态空间坐标个数m、可行区域坐标个数m
f
与障碍物区域坐标个数m
o
。通过状态空间的障碍域系数c
o
对采样范围进行调节，同时利用障碍域系数c
o
确定采样点的启发概率p
h
，即调整算法在障碍物周围采样的概率。算法产生区间为(0,1)的随机数p
rand
，若p
rand
＜p
h
，算法在障碍物采样范围内进行随机采样，反之，算法在其余可行域内进行随机采样。这种方法使得无人车既能在狭窄复杂环境中快速穿过狭窄可行区域减少路径规划时间，也使得算法在
空旷环境中保持随机性，通过障碍物周围区域引导也加快了向目标状态的收敛速度。
[0055]
具体计算公式如下：
[0056][0057]
式中：可行域系数c
f
为可行域坐标点个数m
f
与状态空间坐标点个数m的比值，障碍域系数c
o
为障碍域坐标点个数m
o
与状态空间坐标点个数m的比值，c
r
为可变范围系数，p
h
为算法在可变范围内的采样启发概率,r为可变范围的大小。
[0058]
步骤三：考虑车辆转角约束的临近点采样策略(如图2所示)
[0059]
当前点q
current
的车辆姿态与下一个路径点q
next
的车辆姿态的夹角为则车辆转弯半径r与车辆转向角的关系为：
[0060][0061]
式中，b为步长。
[0062]
在随机树t中找出与随机点q
rand
距离最近的点q
i
，计算随机点q
rand
和当前点q
i
的连线与x轴的夹角q
i
和其父节点q
i
‑1的连线与x轴的夹角再比较与的角度差值θ的绝对值是否小于车辆最大转向角若小于车辆最大转向角，计算q
rand
与q
i
的距离d(q
i
,q
rand
)。比较d(q
i
,q
rand
)是否小于步长b，若小于步长b，在q
rand
与q
i
的连线上以d(q
i
,q
rand
)作为q
i
与q
i+1
的距离；反之，在q
rand
与q
i
的连线上以b作为q
i
与q
i+1
的距离。
[0063]
具体计算公式如下：
[0064][0065][0066]
式中，q
rand
为可行域中生成的随机点，q
i
为距离q
rand
最近的点，q
i
‑1为q
i
的父节点，为车辆最大转向角；d(q
i
,q
rand
)为q
rand
与q
i
的距离,d(q
i
，q
i+1
)为q
i
与其子节点q
i+1
的距离，b为步长。
[0067]
步骤四：自适应步长目标偏向引导策略(如图3所示)
[0068]
结合步骤三提出考虑角度约束的自适应步长目标偏向引导策略。该策略利用步骤三中计算得到的与相减得到角度差值θ与车辆最大转向角的大小关系对d(q
i
，q
i+1
)与进行自适应调整，即当角度差值θ大于车辆最大转向角时，角度差值θ等于车辆最大转向角当角度差值θ小于车辆最大转向角时，角度差值θ取其本身数值。使算法出现目标偏向引导情形时实现有效采样并快速朝目标点方向收敛。
[0069]
具体公式如下：
[0070][0071][0072][0073]
式中，为随机点q
rand
与当前点q
i
的连线与x轴的夹角，为随机树t当前点q
i
与其父节点q
i+1
的连线与x轴的夹角，为经过改进目标偏向引导策略采样的合理采样点q
i+1
与其父节点q
i
的连线与x轴的夹角。θ为与的角度差值，θ
′
为与的角度差值。d(q
i
，q
i+1
)为随机树t当前点q
i
与其子节点q
i+1
的距离，b为步长，为车辆最大转向角。
[0074]
步骤五：无需碰撞检测的随机树扩展(如图5所示)
[0075]
将步骤一获取障碍物区域作为当前障碍物区域，将当前障碍物尺寸加上两倍步长作为算法运行时的障碍物区域。将快速随机树t的随机采样点设定为随机落在可行区域内，由此随机采样点总是落在距离实际障碍物两倍步长的位置，不与障碍物发生碰撞，免去传统rrt(如图4所示)的碰撞检测环节，加快算法运算速度。
[0076]
步骤六：无碰撞的路径点简化(如图6所示)
[0077]
利用随机树t可以得到从起始状态到目标状态的可行路径点，将起点q
start
作为第一次连接的开始点q
k
，开始依次连接后续的路径点，在两路径点间的连线不与障碍物发生碰撞的情况下，对开始点q
k
与路径点q
i
‑1之间的路径进行剔除。依次类推，直到不满足条件的路径点q
i
的父节点q
i
‑1作为新的开始点q
k
，重复上述操作，直到连接到目标点q
goal
。
[0078]
步骤七：杜宾斯平滑曲线拟合(如图7所示)
[0079]
采用杜宾斯曲线对步骤六得到的路径简化点进行路径平滑拟合，得到符合车辆行驶特性的路径。
[0080]
以上是本发明的较佳实施例，凡依本发明技术方案所作的改变，所产生的功能作用未超出本发明技术方案的范围时，均属于本发明的保护范围。