[发明专利]一种园区快递配送车定位与最优路径的规划控制方法

权利要求书

1.一种用于园区的快递配送车定位与最优路径规划的方法，基于高精地图，所述高精地图包括高精度矢量化语义地图，高精度矢量化语义地图包括车道边界信息和车道中心线信息，车道边界信息包括车道边界的几何描述、车道边界编号、起始节点编号、末端节点编号、车道边界连接节点编号；车道中心线信息包括车道中心线的几何描述、车道中心线编号、起始节点编号、末端节点编号、车道中心线连接节点编号；

所述方法基于两套硬件系统和工控机；

第一硬件系统为地图构建硬件系统，包括第一相机、半固态激光雷达和组合导航系统；所述第一相机用于采集环境信息并提取特征图像点；所述固态或半固态激光雷达用于对提取的特征图像点获得其深度信息以计算特征图像点在三维坐标系下的坐标；所述组合导航系统具有厘米级精度，用于获得高精度GPS坐标和姿态信息；第一硬件系统进行标定完成，在道路上设置采集点，得到采集点处第一相机的高精度位置、姿态以及在该点处检测到图像特征点在三维空间下的坐标；

第二硬件系统为快递配送车定位系统，包括第二相机、GPS定位模块、惯性导航单元(IMU)；所述第二相机用于获得周围图像，用于和地图作匹配；所述GPS定位模块用于得到车辆在地图中的粗定位信息，缩小图像匹配的搜索范围；所述惯性导航单元(IMU)用于和第二相机构成视觉惯性里程计(VIO)，连续预测快递配送车的位置和姿态；实施场景为无杂乱车辆停放的园区道路；

所述姿态包航向角、俯仰角、翻滚角；

所述园区道路均为双向车道；

所述工控机为计算平台，用于所有算法的计算；

其特征在于，所述方法的操作步骤如下：

(1)构建高精地图的高精度图像特征点地图

由第一硬件系统构建高精度图像特征点地图，所述高精度图像特征点地图包括采集特征点的三维空间坐标、第一相机的GPS位置和第一相机姿态信息；具体构建操作如下：

(1.1)在园区道路上设置环境特征采集点：在双向车道的分界线处，以1m为间距，设置采集点，布满园区道路；

(1.2)在步骤(1.1)所有设置的采集点上，用第一硬件系统采集并存储每个采集点处八个方向的图像特征点的三维空间坐标、第一相机的GPS位置和第一相机姿态信息；所述八个方向为在第二相机水平面上的前向、后向、左向、右向及每个相邻方向之间的45度角方向；构成高精度视觉特征点地图，并将其作为第二硬件系统中的定位地图使用；

(2)基于高精度图像特征点地图对快递配送车的定位

由步骤(1)构建的高精度图像特征点地图，实现道路上各环境特征采集点处的环境特征点信息获取，以进行快递配送车在高精度图像特征点地图中的高精度定位；

(2.1)由第二硬件系统的GPS定位模块获取快递配送车的粗定位信息，所述粗定位信息的定位精度为10m级；根据GPS定位模块获取的粗定位信息，在高精度图像特征点地图中，选取GPS定位模块粗定位点周围10m范围内采集的图像特征点信息作为局部高精度地图用于快递配送车的匹配定位，得到低频率全局位姿信息；

(2.2)为了得到连续高精度定位，将所述匹配获得的快递配送车的低频率全局位姿信息作为初始位姿输入，融合第二硬件系统中的第二相机和惯性导航单元组成视觉惯性里程计(VIO)，得到连续的位姿估计；

(2.3)由步骤(2.1)得到的低频率全局位姿信息，对步骤(2.2)得到的连续的位姿估计进行误差修正，得到修正后的连续的高精度全局位姿信息；高精度全局位姿信息用于快递配送车自身位置和姿态的确定，用于步骤(3)生成的路径进行轨迹跟踪；

(3)确定快递配送车的最短行驶路径

基于高精度矢量化语义地图，确定快递配送车的最短行驶路径，最短行驶路径由车道中心线编号连接成序列表示；

(3.1)为了确保快递配送车能够到达指定位置，根据实际配送站点的地理坐标信息，将配送点和仓库位置进行编号，并与最近的高精度矢量化语义地图上的车道中心线相关联；

(3.2)确定需要途径的配送站点编号，以快递配送车从起点出发并经过所有需要途径的站点后回到起点为目标，规划全局最短路径；计算过程分为构建代价矩阵和状态转移方程，根据代价矩阵和状态转移方程采用动态规划算法迭代计算，得到按照行驶顺序排列的车道中心线编号，由行驶顺序排列的车道中心线编号组成快递配送车的最短行驶路径；

(4)优化行驶轨迹

(4.1)由步骤(3)生成的车道中心线编号的序列表，从高精度矢量化语义地图中得到车道中心线的几何描述信息，车道中心线的几何描述信息由一系列高精度GPS坐标点构成；通过对高精度GPS坐标点进行分段线性插值，使得相邻坐标点之间的间距不超过1m；得到高密度的车道中心线高精度GPS坐标点；

(4.2)通过工控机采用贝塞尔曲线对高密度的车道中心线高精度GPS坐标点进行平滑处理，生成具有连续曲率的优化行驶轨迹；得到快递配送车的最优路径。