一种基于拓扑地图的无人车规划控制方法

摘要

本发明公开了一种基于拓扑地图的无人车规划控制方法。以建立地图模式多次运行无人车，每一次运行得到不同的关键帧坐标序列。基于欧式距离计算不同关键帧坐标间的地理接近性，以此识别不同次运行关键帧序列的交叉点，这些交叉点作为拓扑地图的节点，将多条轨迹联结在一起，构成一整个地图。在此拓扑地图上执行基于启发式图搜索算法的路径规划，即可获得路径交叉点序列，用这个路径交叉点序列可以索引得到原始的稠密关键帧坐标，这些坐标作为轨迹规划的结果，输入到无人车控制执行系统，实现无人车的位置跟踪。

主权项

一种基于拓扑地图的无人车规划控制方法，其特征在于，包括以下步骤：步骤1，无人车多次建立地图，并存储关键帧坐标序列，每次建立地图的过程如下：启动无人车，所述无人车前方安装有双目相机，设置无人车为随机探索环境的自动运行模式；启动双目相机，采集环境图像，建立稀疏视觉特征地图，并以文本文件的形式存储关键帧位姿序列至车载计算机本地磁盘；每次建立地图的过程如下：1‑1、无人车为随机探索环境，利用无人车搭载的2D激光雷达探测周边的障碍分布信息，计算出安全的行驶方向，根据此安全行驶方向，得到角度控制量和速度控制量，驱动无人车以安全的运动轨迹在环境中探索；1‑2、建立稀疏视觉特征地图，在自动运行模式下，利用搭载的双目相机采集环境的图像，提取图像的特征点，并跟踪这些特征点，以计算无人车的位姿变换矩阵；1‑3、存储关键帧序列，按行存储关键帧位姿序列至车载计算机本地磁盘的文本文件，文本文件每行的数据格式为：x_p y_p z_p q_w q_x q_y q_z；其中，为无人车的位置，为无人车的姿态；步骤2，从车载计算机的本地磁盘读取上述按行存储的关键帧位姿序列数据x_p y_p z_p q_w q_x q_y q_z至内存，基于欧式距离，在不同关键帧序列间寻找路径的交叉点，以交叉点为图节点，建立有向加权拓扑地图，有向加权拓扑地图的边权重设置为交叉点间关键帧的数量；2‑1、基于欧式距离寻找路径交叉点，欧式距离小于某一阈值的两个坐标点是同一地理位置；$d(p_1,p_2)=\sqrt{{(x_{p1}-x_{p2})}^2+{(y_{p1}-y_{p2})}^2+{(z_{p1}-z_{p2})}^2}$若d＜d_thresh，则判定两个坐标点p₁和p₂属于同一个地理位置；2‑2、建立有向图，并为图边设置权重权重的计算方式为：w_edge＝num_frms×w₁+d(p₁,p₂)×w₂；其中，num_frms是节点间关键帧的数目；d(p₁,p₂)是节点间的欧式距离；且有，w₁+w₂＝1.0，其中，w₁是关键帧数目所占的权重，w₂是节点间欧式距离的大小所占的权重；步骤3，在上述有向加权拓扑地图上执行基于启发式图搜索的路径规划算法A*算法，得到若干的路径交叉点，这些交叉点顺次连接构成了无人车可通行的最短路径；基于A*算法执行路径搜索；这种在静态离散栅格地图中求解最短路径的启发式搜索算法，其基本公式为：f(n)＝g(n)+h(n)；其中，f(n)是从初始状态经由状态n到达目标状态的代价值估计；g(n)是从初始状态到状态n的最短路径代价值估计；h(n)是从状态n到达目标状态的最短路径代价值估计；在前述的有向加权拓扑地图中的节点空间设定好起始点和目标点后，即可执行A*算法进行路径搜索，得到一条最短的可通行路径；步骤4，生成的交叉点序列，索引原始的多条关键帧位置序列，得到稠密的关键帧坐标，这些关键帧坐标连同交叉点序列，构成无人车的可执行轨迹；4‑1、由索引到的分段路径拼合成整条轨迹，依次将相邻交叉点之间的若干关键帧取出，并构成一条向量，这条向量即构成了无人车的可通行轨迹；当规划出路径{A,B,E,C}后，为了获得完整的航点序列，需要利用索引区间[A,B]，[B,E]，[E,C]在前述的关键帧序列中索引得到完整的路径{A,a,b,c,B,d,e,f,g,E,h,i,C}；其中，{a,b,c,d,e,f,g,h,i}是交叉点间的关键帧位姿；4‑2、角度控制量的计算，得到航点序列航点序列后，就可以计算角度控制量依次抵达每个航点，直至到达最终的目标点，计算每两个相邻航点间的角度控制量的过程如下：4‑2‑1、接收无人车当前位置x_curr，z_curr和朝向h_curr，目标位置x_t和z_t；4‑2‑2、将目标位置变换至本体坐标系，计算公式如下：x_{in_cam}＝(x_{in_global}‑x_cam)·sin(θ_cam)‑(z_{in_global}‑z_cam)·cos(θ_cam)z_{in_cam}＝(x_{in_global}‑x_cam)·cos(θ_cam)+(z_{in_global}‑z_cam)·sin(θ_cam)其中，本体坐标系的含义是以无人车中心为原点，前方为z轴正方向，右侧为x轴正方向的坐标系。x_{in_cam}、z_{in_cam}是目标位置在本体坐标系下的坐标；x_{in_global}、z_{in_global}是目标位置在地图中的位置；x_cam、z_cam、θ_cam是无人车在地图中的位置。4‑2‑3、计算参考向量，计算公式如下：v_x＝x_{in_cam}‑0v_z＝z_{in_cam}‑0其中，v_x、v_z是目标位置在本体坐标系下的参考向量；4‑2‑4、计算控制角θ_ctrl，计算公式如下：θ_ctrl＝tan^‑1(v_x,v_z)。