CN107479558A - 基于车辆运动模型的无人车辆野外路径规划方法 - Google Patents

Abstract

本发明涉及无人驾驶技术领域，尤其是一种基于车辆运动模型的无人车辆野外路径规划方法。包括以下步骤：一、建立环境三维地图；二、定义车辆行驶模型；三、车辆按控制周期行驶；四、输出规划方案。按照本发明进行路径规划后，能够顺利躲避障碍，满足车轮转向角速度限制，使用尽量少的路径完成了规划要求，而且每段路径都给出了详细的转弯角度、行驶方向，为路径跟踪提供了指导，融合了优化算法、车辆运动学多方面的信息，为解决现有的路径规划存在的问题做了一个新的探索。

Description

基于车辆运动模型的无人车辆野外路径规划方法

技术领域

本发明涉及无人驾驶技术领域，尤其是一种基于车辆运动模型的无人车辆野外路径规划方法。

背景技术

路径规划是无人驾驶的关键技术之一，在野外场地行驶时，往往没有既定的道路，车辆按照预定的航向进行行驶。行驶过程中不可避免会遇到各种各样无法通过的障碍，这时车辆需要根据外界环境信息自动规划一条可行驶路径，该路径的终点要落在既定的航线上，同时车辆的航向也要与既定航向一致。这本质上是一个优化问题，限制条件包括车辆不能碰触障碍，车辆行驶要满足车辆最小转弯半径、转弯速度等车辆运动学要求，车辆最终既要落在既定航线上，也要满足航向要求，优化条件要求车辆行驶最短的路程，尽快完成路径规划任务。

车辆完成路径规划之后，车辆实际行驶过程中将进入路径跟踪阶段，会将规划路径划分为不同的线段，不断调整车辆航向角，准确驶向规划路径中的目标点，最终达到规划的终点与航向。路径规划就是要在满足可行驶的条件下，使用最小的行驶路程，同时也要考虑到路径跟踪问题，考虑车辆运动学和自身转向角、转向调节速度限制，能够采取可行、简洁的控制行驶方案。

现有的路径规划方案比如A*算法采用栅格地图，但栅格划分稀疏会降低精确度，过密又会导致计算量太大。有些路径规划方法没有考虑车辆的最小转弯半径，或者是考虑了转弯半径但没有考虑车辆的转向角变化速度。另外一些方案的路径规划和路径跟踪是完全分开考虑的，仅考虑了路径规划的路线，这又会增加路径跟踪的计算量和难度。

发明内容

本发明要解决的技术问题是提供一个野外环境下无人车辆的路径规划方案，该方案能够保证车辆不碰触障碍，满足车辆的最小转弯半径和转向角变化速度要求，用尽量小的行驶路程到达行驶地点并满足航向要求，同时为路径跟踪提供指导。

为解决上述技术问题，本发明通过以下技术方案实现，包括以下步骤：

步骤一，建立环境三维地图

根据传感器获取的信息进行初步处理，删除噪点，对临近障碍进行合并、膨胀，最终确定障碍Obs_i(i＝1,2,3…)，每个Obs_i包含了该区域中的离散点坐标，确定起点S_pos，在目标航线上，按照目标航线的方向以地图最远处为起点向地图内延伸，直到碰触到障碍物为止，该线段定义为目标航线段；

步骤二，定义车辆行驶模型

车辆在行驶中有以下假设：每个控制周期与行驶速度为固定，因此在该控制周期车辆行驶距离为C，车辆行驶时，在每个控制周期内，车辆转向角可以变化的最大值为S_angle，为了简化计算，将车轮转向角变化值离散为N个角度Δ_angle_i(i＝1,2,3…N)，在最小转弯半径下，车辆的转弯角度最大，为Angle_range，在一个控制周期内，车轮的转动变化值Δ_angle_i要小于S_angle，车辆当前转向角为Angle，那么车辆的实际转向角-Angle_range≤Angle+Δ_angle_i≤Angle_range，定义开集Open_set，每一个位置对应一个向量，该向量包括车辆中心位置，航向向量，上一个控制周期的车辆中心位置，车辆当前状态下的车轮转角，从起点到当前位置所行驶的路程Cost，启发性值H，总代价Value＝Cost+H，启发性信息一般为车辆当前位置与目标点的距离，为了保证航线一致，这里可以选用车头与目标航线段的距离以及车尾与目标航线段距离之和，对各个参数进行初始化，其中将起点及其对应参数加入开集；

车辆是否可以通行，不仅要考虑转向角的问题，还要考虑是否触碰障碍，是否触碰障碍包括两种计算：(1)如果在当前控制周期内碰触障碍，该路线肯定不可通行；(2)如果行驶到新的位置后，即使在所有可以变化的转向角范围内，车辆仍然会碰触到障碍，那么该点也属于不可通行，此外，还要判断新的位置是否有可能到达目标航线上，如果在新的位置，即使在最大的转向角时，车辆仍然无法到达目标航线段，则也判断为不可通行；

步骤三，车辆按控制周期行驶

按下面内容进行迭代循环：

(1)寻找Open_set中总代价Value最小的向量，所对应的车辆中心位置为车辆当前位置，第一次迭代时，当前位置为起点位置。闭集Closed_set为空；

(2)遍历i＝1,2,3…N：

①如果车辆的转向角变化量满足-S_angle≤Δ_angle_i≤S_angle，并且车辆的实际转向角满足-Angle_range≤Angle+Δ_angle_i≤Angle_range，根据当前位置以及车辆方向向量，按照一个控制周期车辆行驶的距离，计算车辆可以行驶到的新位置、新的方向向量；

②判断是否碰触障碍，是否能够到达目标航线段，如果不碰触障碍且有可能到达目标航线段，则计算新位置的车辆坐标、航线、转向角、Cost、H、Value等值，并加入到Open_set中；

③i＝i+1，进入下一循环，当计算完所有的转向角对应的参数后，将当前位置所对应的开集向量加入到闭集Closed_set中；

(3)停止条件：

①检验车辆是否已经到达目标航线段，并且航线偏差在规定误差内，如果满足，则停止循环；

②判断开集Open_set是否为空，如果为空，则说明找不到可以行驶的路线，程序结束，显示无可行驶路线；

如果不满足停止条件，则继续迭代循环；

步骤四，输出规划方案

如果找到了规划方案，将开集和闭集合并，首先找到终点位于的向量，保存该向量中的车辆当前位置、转向、车辆方向向量到路径矩阵中，并根据上一周期所对应的坐标寻找该坐标所对应的向量，保存相关信息到路径矩阵中，按照如下进行迭代：

按照当前坐标与上一周期坐标依次循环直到起点，所对应的路线即为搜索到的路线。

按照本发明进行路径规划后，能够顺利躲避障碍，满足车辆最小转弯半径要求，满足车轮转向角速度限制，使用尽量少的路径完成了规划要求，而且每段路径都给出了详细的转弯角度、行驶方向，为路径跟踪提供了指导，融合了优化算法、车辆运动学多方面的信息，为解决现有的路径规划存在的问题做了一个新的探索。

附图说明

图1为本发明单控制周期内车辆转向图；

图2为本发明路径规划过程图；

图3为本发明路径规划结果图。

具体实施方式

下面结合附图对本发明进行详细的说明，所列举的实施例，只是用于帮助理解本发明，不应理解为对本发明保护范围的限定，对于本技术领域的普通技术人员来说，在不脱离本发明思想的前提下，还可以对本发明进行改进和修饰，这些改进和修饰也落入本发明权利要求保护的范围内。

本实施例在以某车辆单障碍避障为具体应用背景。实例中地图大小为120×300，车辆中心位于起点(1,120)，起始向量为(0,1)，转向角为0，终点位于(300,120)，终点航线向量为(0,1)，障碍位于(55:65,100:120)区域，每个栅格大小为0.1米。根据车速与控制周期的时间可知每个控制周期车辆行走1.25米，车长与车宽分别为6米、4.1米，车辆最大转向角最大为Angle_range＝0.427弧度，将车辆的转向角离散为Angle_range/4，一个周期内能够转向的最大角度为Angle_range/2。

本实施例包括以下步骤：

步骤一，建立环境三维地图

根据传感器获取的信息进行初步处理，删除噪点，对临近障碍进行合并、膨胀，最终确定一个障碍区域Obs₁。确定起点S_pos(1,120)，在目标航线上，以最远处(300,120)为起点按照目标航线的方向向地图内延伸，直到碰触到障碍物为止，该线段定义为目标航线段。

步骤二，定义车辆行驶模型

每个控制周期车辆行驶距离为1.25米。车辆行驶时，车轮转向角离散为集合 在一个控制周期内，车轮的转动变化值初始化开集Open_set，将当前位置信息添加到空集中，该向量的元素依次为当前位置的横纵坐标、航向向量的横纵值，上一个控制周期的车辆中心位置(由于缺少该信息，设置为(0,0))，车辆当前状态下的车轮转角，从起点到当前位置所行驶的路程Cost，启发性值H，总代价Value＝Cost+H。这里的启发性信息为车头与目标航线段的距离以及车尾与目标航线段距离之和。

步骤三，车辆按控制周期行驶

按下面内容进行迭代循环：

(1)寻找Open_set中总代价Value最小的向量，所对应的车辆中心位置为车辆当前位置，第一次迭代时，当前位置为起点位置。闭集Closed_set为空。

(2)遍历i＝1,2,3…N：

①如果车辆的转向角变化量满足-S_angle≤Δ_angle_i≤S_angle，并且车辆的实际转向角满足-Angle_range≤Angle+Δ_angle_i≤Angle_range，根据当前位置以及车辆方向向量，按照一个控制周期车辆行驶的距离，计算车辆可以行驶到的新位置、新的方向向量，参见附图1。

②判断是否碰触障碍，是否能够到达目标航线段，如果不碰触障碍且能够到达目标航线段，则计算新位置的车辆坐标、航线、转向角、Cost、H、Value等值，并加入到Open_set中，否则不执行任何操作，进入下一步。

③i＝i+1，进入下一循环。当计算完所有的转向角对应的参数后，将当前位置所对应的开集向量加入到闭集Closed_set中。

(3)停止条件：

①检验车辆是否已经到达目标航线段，并且航线偏差在规定误差内，如果满足，则停止循环。

②判断开集是否为空，如果为空，则说明找不到可以行驶的路线，程序结束，显示无可行驶路线。

如果不满足停止条件，则继续迭代循环，参见附图2。

步骤四，输出规划方案

如果找到了规划方案，将开集和闭集合并，首先找到终点位于的向量，保存该向量中的车辆当前位置、转向、车辆方向向量到路径矩阵中，并根据上次位置所对应的坐标寻找该坐标所对应的向量，保存相关信息到路径矩阵中，按照如下进行迭代：

按照当前位置与上一点位置依次循环直到起点，所对应的路线即为搜索到的路线，参见附图3。

本实施例在单障碍的路径规划模型，实际运行中对障碍的个数并没有限制，实例中假定车速不变，当车速变化时，仅仅调整单控制周期内行驶距离即可适应新的算法。在后续的实验中，将综合融入车辆安全距离、倒车模型、改进启发性信息公式，实现在复杂野外场景下的路径规划，提高车辆在野外环境下自主行走的能力和效率。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本发明的实质内容。

Claims (1)

1.基于车辆运动模型的无人车辆野外路径规划方法，其特征在于，包括以下步骤：

步骤一，建立环境三维地图

根据传感器获取的信息进行初步处理，删除噪点，对临近障碍进行合并、膨胀，最终确定障碍Obs_i(i＝1,2,3…)，每个Obs_i包含了该区域中的离散点坐标，确定起点S_pos，在目标航线上，按照目标航线的方向以地图最远处为起点向地图内延伸，直到碰触到障碍物为止，该线段定义为目标航线段；

步骤二，定义车辆行驶模型

车辆在行驶中有以下假设：每个控制周期与行驶速度为固定，因此在该控制周期车辆行驶距离为C，车辆行驶时，在每个控制周期内，车辆转向角可以变化的最大值为S_angle，为了简化计算，将车轮转向角变化值离散为N个角度Δ_angle_i(i＝1,2,3…N)，在最小转弯半径下，车辆的转弯角度最大，为Angle_range，在一个控制周期内，车轮的转动变化值Δ_angle_i要小于S_angle，车辆当前转向角为Angle，那么车辆的实际转向角-Angle_range≤Angle+Δ_angle_i≤Angle_range，定义开集Open_set，每一个位置对应一个向量，该向量包括车辆中心位置，航向向量，上一个控制周期的车辆中心位置，车辆当前状态下的车轮转角，从起点到当前位置所行驶的路程Cost，启发性值H，总代价Value＝Cost+H，启发性信息一般为车辆当前位置与目标点的距离，为了保证航线一致，这里可以选用车头与目标航线段的距离以及车尾与目标航线段距离之和，对各个参数进行初始化，其中将起点及其对应参数加入开集；

步骤三，车辆按控制周期行驶

按下面内容进行迭代循环：

(1)寻找Open_set中总代价Value最小的向量，所对应的车辆中心位置为车辆当前位置，第一次迭代时，当前位置为起点位置，闭集Closed_set为空；

(2)遍历i＝1,2,3…N：

①如果车辆的转向角变化量满足-S_angle≤Δ_angle_i≤S_angle，并且车辆的实际转向角满足-Angle_range≤Angle+Δ_angle_i≤Angle_range，根据当前位置以及车辆方向向量，按照一个控制周期车辆行驶的距离，计算车辆可以行驶到的新位置、新的方向向量；

②判断是否碰触障碍，是否能够到达目标航线段，如果不碰触障碍且有可能到达目标航线段，则计算新位置的车辆坐标、航线、转向角、Cost、H、Value等值，并加入到Open_set中；

③i＝i+1，进入下一循环，当计算完所有的转向角对应的参数后，将当前位置所对应的开集向量加入到闭集Closed_set中；

(3)停止条件：

①检验车辆是否已经到达目标航线段，并且航线偏差在规定误差内，如果满足，则停止循环；

②判断Open_set是否为空，如果为空，则说明找不到可以行驶的路线，程序结束，显示无可行驶路线；

如果不满足停止条件，则继续迭代循环；

步骤四，输出规划方案

如果找到了规划方案，将开集和闭集合并，首先找到终点位于的向量，保存该向量中的车辆当前位置、转向、车辆方向向量到路径矩阵中，并根据上一周期所对应的坐标寻找该坐标所对应的向量，保存相关信息到路径矩阵中，按照如下进行迭代：

按照当前坐标与上一周期坐标依次循环直到起点，所对应的路线即为搜索到的路线。