一种用于车辆模型的路径规划系统及方法与流程

本发明涉及车辆安全行驶技术领域，具体地，涉及一种用于车辆模型的路径规划系统及方法。

背景技术：

随着人工智能的逐渐发展，无人驾驶智能设备例如无人驾驶汽车越来越受各行业和研究机构的关注，而国外已经有特定场景下的无人驾驶汽车投入试运营。而路径规划则是无人驾驶汽车的关键技术之一，是无人驾驶汽车安全行驶在道路上的前提，而行驶安全问题一直是无人驾驶最注重的问题。

现有的基于栅格地图的路径规划方法(全局搜索算法诸如a_star，d_star算法)，都是在给定起始点和目标点之间搜索一条全局最优路径，但是该路径不能满足车辆的非完整约束，导致规划出来的路径不适合车辆行驶，这会导致严重的安全问题。

技术实现要素：

针对现有技术中的缺陷，本发明的目的是提供一种用于车辆模型的路径规划系统及方法，其可以有效提高车辆模型的安全性和稳定性。

一种用于车辆模型的路径规划系统，包括：

地图生成单元，用于生成可供路径规划的栅格地图；

感知单元，用于感知并检测车辆模型周围障碍物信息；

融合定位单元，用于对车辆模型进行定位；

全局路径生成单元，用于生成初始点至目标点的全局路径；所述全局路径由一系列路径点组成，将路径点集成在一起就是一条路径；

局部路径跟随单元，用于跟踪上述全局路径并进行车辆模型的速度参数修正值输出；所述速度参数至少包括：车辆模型的前轮偏角、行驶线速度、加速度；

车辆模型运动控制单元，用于将上述输出的速度参数修正值作为输入对车辆模型进行运动控制；

其中，融合定位单元将定位信息输入给全局路径生成单元；感知单元将障碍物信息输入给全局路径生成单元；地图生成单元将地图信息输入给全局路径生成单元；全局路径单元接受信息后输出全局路径，并将该全局路径输入给局部路径跟随单元；局部路径跟随单元接受全局路径之后生成速度参数修正值；局部路径跟随单元输入所述速度参数修正值给车辆模型；运动控制单元以控制车辆模型的运动；

所述感知单元将所述障碍物信息输入给所述融合定位单元，以配合融合定位单元将车辆模型进行定位；

所述感知单元将所述障碍物信息输入给所述地图生成单元，辅助所述地图生成单元进行实时更新。

可选地，所述感知单元至少包括摄像头、激光雷达；所述摄像头检测的信息以及激光雷达扫描到的信息形成点云数据被上传到上位机；所述上位机将该述信息分别输入给所述融合定位单元、所述地图生成单元、全局路径生成单元。

可选地，所述地图生成单元可实时更新上述栅格地图；

所述地图生成单元设置有初始的静态地图，后通过输入map_server算法包生成的栅格地图，对地图进行二值化，通过点云数据投影到当前的栅格地图上生成新的占据栅格地图，然后与之前的静态地图叠加。

可选地，所述“对地图进行二值化”，即为：map_server算法包生成的栅格地图具有一系列灰度值，通过设置阈值来划分当前栅格是否为可行驶栅格；大于阈值设为1，表示被占据，小于阈值设为0，表示未占据。

可选地，所述融合定位单元依据全局卡尔曼滤波算法融合了全局定位信息，输出当前栅格地图的起始点到车辆模型里程计的坐标转换；所述融合定位单元依据局部卡尔曼滤波算法融合了局部定位信息，输出车辆模型里程计到车辆模型本身的坐标转换。

可选地，所述全局定位信息由所述点云数据加上gps数据产生；所述全局定位为车辆模型当前位置基于当前栅格地图起始点的一个位置；所述全局定位信息为离散的；

所述局部定位信息由里程计和惯性导航模块imu产生，所述局部定位为根据车辆模型已经走过的距离计算出当前位置基于起始点的哪个位置；所述局部定位信息为连续的。

可选地，所述融合定位单元融合全局定位信息和局部定位信息，该述“融合”，即为将全局定位信息和局部定位信息的每个数据设置协方差来设置权重，然后将各个数据进行滤波，消除跳变点从而得到稳定，连续的数据。

可选地，所述全局路径生成单元采用hybrid_astar算法生成初始点至目标点的全局路径；

首先初始化openlist列表，获取起始点和目标点的车辆模型运动模型(x，y，θ，k，δ)，(x，y)为车辆模型位置坐标，θ为车辆模型的朝向，k为转向曲率；其中，所述openlist列表用来存放车辆模型当前位置周围可以被考虑的全局路径的路径点的数据集合；

从openlist列表中找到代价值cost价值最小的节点作为父节点，并计算g和h的值；其中，g代表从初始结点到当前点的实际代价值；h代表从当前点到目标点的预期花费估计代价值；

然后判断是否到达目标点，如若到达则对路径进行平滑处理，然后输出路径点，没有到达则继续搜索。

可选地，h值的计算方法如下：

reeds-shepp曲线、dubins曲线、曼哈顿距离三种cost解算出来的最大值来作为上述hybrid_astar的预期花费估计代价值；

其中reeds-shepp曲线由几段半径固定的圆弧和一段直线段拼接组成，而且圆弧的半径就是车辆模型的最小转向半径，它是车辆模型行驶的最短路径；

dubins曲线和reeds-shepp曲线相比，多了一个约束条件：车辆模型只能朝前开，不能后退。

可选地，局部路径跟随单元基于pure_pursuit算法跟踪上述全局路径并进行车辆模型的速度参数修正值输出；

根据车辆模型当前路径和全局路径之间的位置关系，确定预瞄点的距离，从而确定预瞄点的位置，生成前轮偏角控制量表达式控制前轮偏角来追踪全局路径；控制前轮偏转角，再输出各个速度参数的修正值，通过追踪一个个预瞄点来追踪全局路径并控制车辆模型运动；

所述预瞄点即为pure_pursuit算法当前所要追踪的全局路径点。

可选地，所述前轮偏角控制量表达式为：

l为车辆模型轴距，l为预瞄距离，α为车辆模型与预瞄点的夹角。

可选地，预先设定所述各个速度参数的期望最大值和最小值；所述期望最大值为正数，所述期望最小值为零或负数；

按上述公式计算得到当前的车辆模型前轮偏角、使用车辆模型的里程计测量当前的车辆模型的行驶线速度、加速度；

上述参数的计算值和测量值分别通过pid算法与其对应的期望最大值和最小值进行比较；

若车辆模型要向前运动，则修正上述计算值或测量值使其以最快时间达到其对应的期望最大值；

若车辆模型要向后或停止运动，则修正上述计算值或测量值使其以最快时间达到其对应的期望最小值；

将上述各个参数的修正值输入给上位机；

所述上位机将上述各个修正值输入给所述车辆模型运动控制单元。

可选地，所述车辆模型运动控制单元至少包括车辆模型驱动器，所述上位机将所述各个速度参数的修正值输入给车辆模型驱动器以控制车辆模型的运动。

一种用于车辆模型的路径规划方法，应用于上述的系统，包括以下步骤：

s1：地图生成单元生成可供规划路径的栅格地图；

s2：感知单元感知并检测车辆模型周围的障碍物信息；

s3：感知单元将上述环境信息输送给融合定位单元，所述融合定位单元将车辆模型进行定位；

s4：依据s1步骤的地图信息、s2步骤得到的障碍物信息以及s3步骤得到的车辆模型定位信息，全局路径生成单元对起始点至目标点进行全局路径规划，得到起始点至目标点的全局路径；

s5：将上述全局路径输送给局部路径跟随单元进行路径跟随和速度参数修正值输出；所述速度参数至少包括：车辆模型的前轮偏角、行驶线速度、加速度；

s6：局部路径跟随单元将上述速度参数修正值输入给车辆模型运动控制单元，车辆模型运动控制单元控制车辆模型的运动。

可选地，感知单元至少包括摄像头、激光雷达；所述摄像头检测的信息以及激光雷达扫描到的信息形成点云数据被上传到上位机；所述上位机将该述信息分别输入给所述融合定位单元、所述地图生成单元、全局路径生成单元。

可选地，步骤s1中，所述地图生成单元可实时更新上述栅格地图；

所述地图生成单元设置有初始的静态地图，后通过输入map_server算法包生成的栅格地图，对地图进行二值化，通过点云数据投影到当前的栅格地图上生成新的占据栅格地图，然后与之前的静态地图叠加。

可选地，所述“对地图进行二值化”，即为：map_server算法包生成的栅格地图具有一系列灰度值，通过设置阈值来划分当前栅格是否为可行驶栅格；大于阈值设为1，表示被占据，小于阈值设为0，表示未占据。

可选地，步骤s3中，所述融合定位单元依据全局卡尔曼滤波算法融合了全局定位信息，输出当前栅格地图的起始点到车辆模型里程计的坐标转换；所述融合定位单元依据局部卡尔曼滤波算法融合了局部定位信息，输出车辆模型里程计到车辆模型本身的坐标转换。

可选地，所述全局定位信息由所述点云数据加上gps数据产生；所述全局定位为车辆模型当前位置基于当前栅格地图起始点的一个位置；所述全局定位信息为离散的；

所述局部定位信息由里程计和惯性导航模块imu产生，所述局部定位为根据车辆模型已经走过的距离计算出当前位置基于起始点的哪个位置；所述局部定位信息为连续的。

可选地，所述融合定位单元融合全局定位信息和局部定位信息，该述“融合”，即为将全局定位信息和局部定位信息的每个数据设置协方差来设置权重，然后将各个数据进行滤波，消除跳变点从而得到稳定，连续的数据。

可选地，步骤s4中，所述全局路径生成单元采用hybrid_astar算法生成初始点至目标点的全局路径，其具体包括以下步骤：

首先初始化openlist列表，获取起始点和目标点的车辆模型运动模型(x，y，θ，k，δ)，(x，y)为车辆模型位置坐标，θ为车辆模型的朝向，k为转向曲率；其中，所述openlist列表用来存放车辆模型当前位置周围可以被考虑的全局路径的路径点的数据集合；

从openlist列表中找到代价值cost价值最小的节点作为父节点，并计算g和h的值；其中，g代表从车辆模型初始点到车辆模型当前点的实际代价值；h代表从车辆模型当前点到车辆模型目标点的预期花费估计代价值；

然后判断是否到达目标点，如若到达则对路径进行平滑处理，然后输出路径点，没有到达则继续搜索。

可选地，h值的计算方法如下：

reeds-shepp曲线、dubins曲线、曼哈顿距离三种cost解算出来的最大值来作为上述hybrid_astar的预期花费估计代价值；

其中reeds-shepp曲线由几段半径固定的圆弧和一段直线段拼接组成，而且圆弧的半径就是车辆模型的最小转向半径，它是车辆模型行驶的最短路径；

dubins曲线和reeds-shepp曲线相比，多了一个约束条件：车辆模型只能朝前开，不能后退。

可选地，步骤s5中，局部路径跟随单元基于pure_pursuit算法跟踪上述全局路径并进行车辆模型的速度参数修正值输出，具体包括以下步骤：

根据车辆模型当前路径和全局路径之间的位置关系，确定预瞄点的距离，从而确定预瞄点的位置，生成前轮偏角控制量表达式控制前轮偏角来追踪全局路径；控制前轮偏转角，再输出各个速度参数的修正值，通过追踪一个个预瞄点来追踪全局路径；

所述预瞄点即为pure_pursuit算法当前所要追踪的全局路径点。

可选地，所述前轮偏角控制量表达式为：

l为车辆模型轴距，l为预瞄距离，α为车辆模型与预瞄点的夹角。

可选地，“输出各个速度参数的修正值”的方法如下：

预先设定所述各个速度参数的期望最大值和最小值；所述期望最大值为正数，所述期望最小值为零或负数；

按上述公式计算得到当前的车辆模型前轮偏角、使用车辆模型的里程计测量当前的车辆模型的行驶线速度、加速度；

上述参数的计算值和测量值分别通过pid算法与其对应的期望最大值和最小值进行比较；

若车辆模型要向前运动，则修正上述计算值或测量值使其以最快时间达到其对应的期望最大值；

若车辆模型要向后或停止运动，则修正上述计算值或测量值使其以最快时间达到其对应的期望最小值；

将上述各个参数的修正值输入给上位机；

所述上位机将上述各个修正值输入给所述车辆模型运动控制单元。

可选地，所述车辆模型运动控制单元至少包括车辆模型驱动器，所述上位机将所述各个速度参数的修正值输入给车辆模型驱动器以控制车辆模型的运动。

与现有技术相比，本发明具有如下的有益效果：

本发明通过输入map_server生成的栅格地图，对地图进行二值化，通过点云数据投影到当前的栅格地图上生成新的占据栅格地图，然后与之前的静态地图叠加，这里将躲避静态障碍物和动态障碍物放在一起，更加简洁有效，同时增加了路径预判提高车辆行驶的安全性。

定位方式采用全局定位和局部定位相结合，提高定位的鲁棒性，避免了定位丢失而导致事故。

采取感知单元感知物体，扫除了盲区。

全局路径生成单元和局部路径跟随单元相结合，先确定路径，再确定沿路径的速度分配，使得无人汽车在行驶中可以提前预判，提高了安全性。

采用hybrid_astar算法，考虑到了物体的实际运动约束，生成的路径满足车辆的非完整约束。启发式函数结合reeds-shepp曲线、dubins曲线、曼哈顿距离三种cost解算出来的最大值作为hybrid_astar的预期花费，使得cost估计更加合理。最后将生成的路径进行平滑处理，更适合车辆运动。

采用pure_pursuit跟踪全局路径，不会破坏车辆的非完整约束，且该算法跟踪偏差较小，确保了车辆行驶的稳定性。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明实施例提供的一种用于车辆模型的路径规划系统的结构示意图；

图2为本发明实施例提供的一种用于车辆模型的路径规划方法的结构示意图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

参见图1，一种用于车辆模型的路径规划系统，包括：地图生成单元，用于生成可供路径规划的栅格地图；感知单元，用于感知并检测车辆模型周围障碍物信息；融合定位单元，用于对车辆模型进行定位；全局路径生成单元，用于生成初始点至目标点的全局路径；所述全局路径由一系列路径点组成，将路径点集成在一起就是一条路径；局部路径跟随单元，用于跟踪上述全局路径并进行车辆模型的速度参数修正值输出；所述速度参数至少包括：车辆模型的前轮偏角、行驶线速度、加速度；车辆模型运动控制单元，用于以上述输出的速度参数修正值对车辆模型进行运动控制。

其中，融合定位单元将定位信息输入给全局路径生成单元；感知单元将障碍物信息输入给全局路径生成单元；地图生成单元将地图信息输入给全局路径生成单元；全局路径单元接受信息后输出全局路径，并将该全局路径输入给局部路径跟随单元；局部路径跟随单元接受全局路径之后生成速度参数修正值；局部路径跟随单元输入所述速度参数修正值给车辆模型运动控制单元以控制车辆模型的运动；

所述感知单元将所述障碍物信息输入给所述融合定位单元，以配合融合定位单元将车辆模型进行定位；

所述感知单元将所述障碍物信息输入给所述地图生成单元，辅助所述地图生成单元进行实时更新。

本发明的实施例所说的车辆模型，不局限于汽车，也可以是基于阿克曼模型的机器人，只要符合阿克曼模型特征就属于该车辆模型的范畴，本发明不对此作出限定。

本实施例中，车辆模型至少自带有gps、里程计、上位机。

前轮偏角是指车辆模型前轮相对于车辆模型纵轴线的夹角，往左为正，往右为负；行驶线速度是指车辆模型的行驶速度；加速度是指车辆模型行驶加速度。上述感知单元至少包括摄像头、激光雷达；所述摄像头检测的信息以及激光雷达扫描到的信息形成点云数据被上传到上位机；所述上位机将该述信息分别输入给所述融合定位单元、所述地图生成单元、全局路径生成单元。

上述激光雷达可装于车辆模型顶部，探测四周环境：摄像头可装于车辆模型前部，探测前方环境。

上述地图生成单元可实时更新上述栅格地图；所述地图生成单元设置有初始的静态地图，后通过输入map_server算法包生成的栅格地图，对地图进行二值化，通过点云数据投影到当前的栅格地图上生成新的占据栅格地图，然后与之前的静态地图叠加。

map_server算法包为基于ros的地图生成算法包。

所述“对地图进行二值化”，即为：map_server算法包生成的栅格地图具有一系列灰度值，通过设置阈值来划分当前栅格是否为可行驶栅格；大于阈值设为1，表示被占据，小于阈值设为0，表示未占据。

上述系统规划路径期间，地图生成单元在一直在实时更新，而全局路径生成单元也在一直在实时更新全局路径，而局部路径跟随单元也跟着更新，并实时输出不停更新的速度参数修正值，以实时控制车辆模型的运动。

所述融合定位单元依据全局卡尔曼滤波算法融合了全局定位信息，输出当前栅格地图的起始点到车辆模型里程计的坐标转换；所述融合定位单元依据局部卡尔曼滤波算法融合了局部定位信息，输出车辆模型里程计到车辆模型本身的坐标转换。

所述全局定位信息由所述点云数据加上gps数据产生；所述全局定位为车辆模型当前位置基于当前栅格地图起始点的一个位置；所述全局定位信息为离散的；

所述局部定位信息由里程计和惯性导航模块imu产生，所述局部定位为根据车辆模型已经走过的距离计算出当前位置基于起始点的哪个位置；所述局部定位信息为连续的。

上述gps数据为车辆模型自带的gps上获得。

所述融合定位单元融合全局定位信息和局部定位信息，该述“融合”，即为将全局定位信息和局部定位信息的每个数据设置协方差来设置权重，然后将各个数据进行滤波，消除跳变点从而得到稳定，连续的数据。

所述全局路径生成单元采用hybrid_astar算法生成初始点至目标点的全局路径，具体包括以下步骤：

首先初始化openlist列表，获取起始点和目标点的车辆模型运动模型(x，y，θ，k，δ)，(x，y)为车辆模型位置坐标，θ为车辆模型的朝向，k为转向曲率；其中，所述openlist列表用来存放车辆模型当前位置周围可以被考虑的全局路径的路径点的数据集合；

从openlist列表中找到代价值cost价值最小的节点作为父节点，并计算g和h的值；其中，g代表从初始结点到当前点的实际代价值；h代表从当前点到目标点的预期花费估计代价值；

然后判断是否到达目标点，如若到达则对路径进行平滑处理，然后输出路径点，没有到达则继续搜索。

其中，g值的计算方法依据寻路算法，通过搜索地图上的一个个点来确定最优路径，如下：

如果车辆前进，则gn+1＝gn+s；

如果车辆后退，则g＝g+p1s；

如果车辆有转向g＝g+p2|δ(c)；

其中，gn代表前一个路径点的g值，gn+1代表当前路径的的g值，g初始值为0，此处gn不一定是初始值，其可能是某个非起始点的路径点对应的g值；s代表车辆当前走过的距离；δ(c)代表车辆模型对应不同的转弯半径的转向角，c代表车辆模型转弯半径；p1代表第一系数，其值根据实际情况调整；p2代表第二系数，其值根据实际情况调整。

在本实施例中，较优地，p1＝-1，p2＝0.5。

h值的计算方法如下：

reeds-shepp曲线、dubins曲线、曼哈顿距离三种cost解算出来的最大值来作为上述hybrid_astar的预期花费估计代价值；

其中reeds-shepp曲线由几段半径固定的圆弧和一段直线段拼接组成，而且圆弧的半径就是车辆模型的最小转向半径，它是车辆模型行驶的最短路径；

dubins曲线和reeds-shepp曲线相比，多了一个约束条件：车辆模型只能朝前开，不能后退。

曼哈顿距离为h(n)＝d*(abs(n.x-goal.x)+abs(n.y-goal.y))，其中d为系数，d值根据实际情况确定；n.x，n.y为车辆当前点的x，y坐标，goal.x，goal.y为目标点的x，y坐标。在本实施例中，较优地，d＝1。

“对路径进行平滑处理”，方法如下：

将路径中各点的曲率、平滑度、与障碍之间的距离进行约束，建立函数后采用共轭梯度下降法对目标函数求极值从而得到更平滑的路径，以对路径进行平滑处理；建立的目标函数为：

该式由三个多项式相加，式中a，b，c为各项所占权重，xi为路径点，oi为xi最近的障碍物的位置，d为节点距离障碍点的安全距离，当|xi-oi|＞d时第一个多项式起作用；第二个多项式对路径进行了平滑，δxi＝xi-xi-1；ki为曲率，当ki＜＝kmax时第三个多项式梯度取0。

在本实施例中，较优地，a＝1，b＝0.2，c＝0.4。

其中，局部路径跟随单元基于pure_pursuit算法跟踪上述全局路径并进行车辆模型的速度参数输出，具体包括以下步骤：

根据车辆模型当前路径和全局路径之间的位置关系，确定预瞄点的距离，从而确定预瞄点的位置，生成前轮偏角控制量表达式控制前轮偏角来追踪全局路径；控制前轮偏转角，再输出各个速度参数的修正值，通过追踪一个个预瞄点来追踪全局路径并控制车辆模型运动；

所述预瞄点即为pure_pursuit算法当前所要追踪的全局路径点。

其中，所述前轮偏角控制量表达式为：

l为车辆模型轴距，l为预瞄距离，α为车辆模型与预瞄点的夹角。

“输出各个速度参数的修正值”的方法如下：

预先设定所述各个速度参数的期望最大值和最小值；所述期望最大值为正数，所述期望最小值为零或负数；这里期望最小值是取0还是负数，以设计者的设计思路来定，如果设计者希望车辆模型能够往后倒着运动，则可设期望最小值为负数；如果设计者希望车辆模型不能倒退只能前进，则可设期望最小值为零。

按上述公式计算得到当前的车辆模型前轮偏角、使用车辆模型的里程计测量当前的车辆模型的行驶线速度、加速度；

上述参数的计算值和测量值分别通过pid算法与其对应的期望最大值和最小值进行比较；

若车辆模型要向前运动，则修正上述计算值或测量值使其以最快时间达到其对应的期望最大值；

若车辆模型要向后或停止运动，则修正上述计算值或测量值使其以最快时间达到其对应的期望最小值；

将上述各个参数的修正值输入给上位机；

所述上位机将上述各个修正值输入给所述车辆模型运动控制单元。

参见图2，一种用于车辆模型的路径规划方法，应用于上述系统，包括以下步骤：

s1：地图生成单元生成可供规划路径的栅格地图；

s2：感知单元感知并检测车辆模型周围的障碍物信息；

s3：感知单元将上述环境信息输送给融合定位单元，所述融合定位单元将车辆模型进行定位；

s4：依据s1步骤的地图信息、s2步骤得到的障碍物信息以及s3步骤得到的车辆模型定位信息，全局路径生成单元对起始点至目标点进行全局路径规划，得到起始点至目标点的全局路径；

s5：将上述全局路径输送给局部路径跟随单元进行路径跟随和速度参数修正值输出；所述速度参数至少包括：车辆模型的前轮偏角、行驶线速度、加速度；

s6：局部路径跟随单元将上述速度参数修正值输入给车辆模型运动控制单元，车辆模型运动控制单元控制车辆模型的运动。

感知单元至少包括摄像头、激光雷达；所述摄像头检测的信息以及激光雷达扫描到的信息形成点云数据被上传到上位机；所述上位机将该述信息分别输入给所述融合定位单元、所述地图生成单元、全局路径生成单元。

步骤s1中，所述地图生成单元可实时更新上述栅格地图；所述地图生成单元设置有初始的静态地图，后通过输入map_server算法包生成的栅格地图，对地图进行二值化，通过点云数据投影到当前的栅格地图上生成新的占据栅格地图，然后与之前的静态地图叠加。

所述“对地图进行二值化”，即为：map_server算法包生成的栅格地图具有一系列灰度值，通过设置阈值来划分当前栅格是否为可行驶栅格；大于阈值设为1，表示被占据，小于阈值设为0，表示未占据。

步骤s3中，所述融合定位单元依据全局卡尔曼滤波算法融合了全局定位信息，输出当前栅格地图的起始点到车辆模型里程计的坐标转换；所述融合定位单元依据局部卡尔曼滤波算法融合了局部定位信息，输出车辆模型里程计到车辆模型本身的坐标转换。

所述全局定位信息由所述点云数据加上gps数据产生；所述全局定位为车辆模型当前位置基于当前栅格地图起始点的一个位置；所述全局定位信息为离散的；

所述局部定位信息由里程计和惯性导航模块imu产生，所述局部定位为根据车辆模型已经走过的距离计算出当前位置基于起始点的哪个位置；所述局部定位信息为连续的。

所述融合定位单元融合全局定位信息和局部定位信息，该述“融合”，即为将全局定位信息和局部定位信息的每个数据设置协方差来设置权重，然后将各个数据进行滤波，消除跳变点从而得到稳定，连续的数据。

步骤s4中，所述全局路径生成单元采用hybrid_astar算法生成初始点至目标点的全局路径，其具体包括以下步骤：

首先初始化openlist列表，获取起始点和目标点的车辆模型运动模型(x，y，θ，k，δ)，(x，y)为车辆模型位置坐标，θ为车辆模型的朝向，k为转向曲率；其中，所述openlist列表用来存放车辆模型当前位置周围可以被考虑的全局路径的路径点的数据集合；

从openlist列表中找到代价值cost价值最小的节点作为父节点，并计算g和h的值；其中，g代表从车辆模型初始点到车辆模型当前点的实际代价值；h代表从车辆模型当前点到车辆模型目标点的预期花费估计代价值；

然后判断是否到达目标点，如若到达则对路径进行平滑处理，然后输出路径点，没有到达则继续搜索。

其中，g值的计算方法依据寻路算法，通过搜索地图上的一个个点来确定最优路径，如下：

如果车辆前进，则gn+1＝gn+s；

如果车辆后退，则g＝g+p1s；

如果车辆有转向g＝g+p2|δ(c)；

其中，gn代表前一个路径点的g值，gn+1代表当前路径的的g值，g初始值为0，此处gn不一定是初始值，其可能是某个非起始点的路径点对应的g值；s代表车辆当前走过的距离；δ(c)代表车辆模型对应不同的转弯半径的转向角，c代表车辆模型转弯半径；p1代表第一系数，其值根据实际情况调整；p2代表第二系数，其值根据实际情况调整。

在本实施例中，较优地，p1＝-1，p2＝0.5。

h值的计算方法如下：

reeds-shepp曲线、dubins曲线、曼哈顿距离三种cost解算出来的最大值来作为上述hybrid_astar的预期花费估计代价值；

其中reeds-shepp曲线由几段半径固定的圆弧和一段直线段拼接组成，而且圆弧的半径就是车辆模型的最小转向半径，它是车辆模型行驶的最短路径；

dubins曲线和reeds-shepp曲线相比，多了一个约束条件：车辆模型只能朝前开，不能后退。

曼哈顿距离为h(n)＝d*(abs(n.x-goal.x)+abs(n.y-goal.y))，其中d为系数，d值根据实际情况确定；n.x，n.y为车辆当前点的x，y坐标，goal.x，goal.y为目标点的x，y坐标。在本实施例中，较优地，d＝1。

“对路径进行平滑处理”，方法如下：

将路径中各点的曲率、平滑度、与障碍之间的距离进行约束，建立函数后采用共轭梯度下降法对目标函数求极值从而得到更平滑的路径，以对路径进行平滑处理；建立的目标函数为：

将路径中各点的曲率、平滑度、与障碍之间的距离进行约束，建立函数后采用共轭梯度下降法对目标函数求极值从而得到更平滑的路径，以对路径进行平滑处理；建立的目标函数为：

该式由三个多项式相加，式中a，b，c为各项所占权重，xi为路径点，oi为xi最近的障碍物的位置，d为节点距离障碍点的安全距离，当|xi-oi|＞d时第一个多项式起作用；第二个多项式对路径进行了平滑，δxi＝xi-xi-1；ki为曲率，当ki＜＝kmax时第三个多项式梯度取0。在本实施例中，较优地，a＝1，b＝0.2，c＝0.4。

步骤s5中，局部路径跟随单元基于pure_pursuit算法跟踪上述全局路径并进行车辆模型的速度参数输出，具体包括以下步骤：

根据车辆模型当前路径和全局路径之间的位置关系，确定预瞄点的距离，从而确定预瞄点的位置，生成前轮偏角控制量表达式控制前轮偏角来追踪全局路径；控制前轮偏转角，再输出各个速度参数的修正值，通过追踪一个个预瞄点来追踪全局路径；

所述预瞄点即为pure_pursuit算法当前所要追踪的全局路径点。

其中，所述前轮偏角控制量表达式为：

l为车辆模型轴距，l为预瞄距离，α为车辆模型与预瞄点的夹角。

“输出各个速度参数的修正值”的方法如下：

预先设定所述各个速度参数的期望最大值和最小值；所述期望最大值为正数，所述期望最小值为零或负数；这里期望最小值是取0还是负数，以设计者的设计思路来定，如果设计者希望车辆模型能够往后倒着运动，则可设期望最小值为负数；如果设计者希望车辆模型能够停下来，则可设期望最小值为零。

按上述公式计算得到当前的车辆模型前轮偏角、使用车辆模型的里程计测量当前的车辆模型的行驶线速度、加速度；

上述参数的计算值和测量值分别通过pid算法与其对应的期望最大值和最小值进行比较；

若车辆模型要向前运动，则修正上述计算值或测量值使其以最快时间达到其对应的期望最大值；

若车辆模型要向后或停止运动，则修正上述计算值或测量值使其以最快时间达到其对应的期望最小值；

将上述各个参数的修正值输入给上位机；

所述上位机将上述各个修正值输入给所述车辆模型运动控制单元。

所述车辆模型运动控制单元至少包括车辆模型驱动器，所述上位机将所述各个速度参数的修正值输入给车辆模型驱动器以控制车辆模型的运动。

本发明通过输入map_server生成的栅格地图，对地图进行二值化，通过点云数据投影到当前的栅格地图上生成新的占据栅格地图，然后与之前的静态地图叠加，这里将躲避静态障碍物和动态障碍物放在一起，更加简洁有效，同时增加了路径预判提高车辆行驶的安全性。

定位方式采用全局定位和局部定位相结合，提高定位的鲁棒性，避免了定位丢失而导致事故。

采取感知单元感知物体，扫除了盲区。

全局路径生成单元和局部路径跟随单元相结合，先确定路径，再确定沿路径的速度分配，使得无人汽车在行驶中可以提前预判，提高了安全性。

采用hybrid_astar算法，考虑到了物体的实际运动约束，生成的路径满足车辆的非完整约束。启发式函数结合reeds-shepp曲线、dubins曲线、曼哈顿距离三种cost解算出来的最大值作为hybrid_astar的预期花费，使得cost估计更加合理。最后将生成的路径进行平滑处理，更适合车辆运动。

采用pure_pursuit跟踪全局路径，不会破坏车辆的非完整约束，且该算法跟踪偏差较小，确保了车辆行驶的稳定性。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。