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

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

A method for planning and control of unmanned vehicle based on topological map

The invention discloses a method for planning and controlling unmanned vehicles based on topological map. In order to establish a map mode to run multiple unmanned vehicles, each run to get different key frame coordinates. The Euclidean distance between the coordinates of key frames are calculated in different geographical proximity based on the intersection of different operation sequence in order to identify the key frame of these intersections as nodes of the topology map, multiple path together to form a whole map. This topology map execution path planning based on heuristic graph search algorithm, can obtain the path intersection sequence, using this path intersection sequence index dense key frame coordinate primitive to these coordinates as the result of the trajectory planning, input to the unmanned vehicle control execution system, realize unmanned vehicle position tracking.

【技术实现步骤摘要】

本专利技术属于移动机器人自主导航
，特别是基于稀疏视觉特征地图的无人车位置跟踪方法。
技术介绍
随着移动机器人技术的发展，针对非结构化场景自主建立环境地图，并基于所建立的环境地图实现安全的导航控制，成为越来越迫切的核心需求，是达成移动机器人高层次作业任务的基础支撑。为了提高建立地图的效率，一般的做法是提取环境的稀疏特征信息进行运算，最终生成的地图也是稀疏的表达形式，从而难以直接用于自主移动平台的路径规划和控制。
技术实现思路
本专利技术所要解决的技术问题是提供一种基于拓扑地图的无人车规划控制方法，以解决稀疏环境地图难以集成到规划控制系统的问题。为此，本专利技术提供以下技术方案：一种基于拓扑地图的无人车规划控制方法，包括以下步骤：步骤1，无人车多次建立地图，并存储关键帧坐标序列，每次建立地图的过程如下：启动无人车，所述无人车内设有双目相机，设置无人车为随机探索环境的自动运行模式；启动双目相机，采集环境图像，建立稀疏视觉特征地图，并以文本文件的形式存储关键帧位姿序列至车载计算机本地磁盘；每次建立地图的过程如下：1-1、无人车为随机探索环境，利用无人车搭载的2D激光雷达探测周边的障碍分布信息，计算出安全的行驶方向，根据此安全行驶方向，得到角度控制量和速度控制量，驱动无人车以安全的运动轨迹在环境中探索；1-2、建立稀疏视觉特征地图，在自动运行模式下，利用搭载的双目相机采集环境的图像，提取图像的特征点，并跟踪这些特征点，以计算无人车的位姿变换矩阵；1-3、存储关键帧序列，按行存储关键帧位姿序列至车载计算机本地磁盘的文本文件，文本文件每行的数据格式为：xpypzpqwqxqyqz；其中，为无人车的位置，为无人车的姿态；步骤2，从车载计算机的本地磁盘读取上述按行存储的关键帧位姿序列数据xpypzpqwqxqyqz至内存，基于欧式距离，在不同关键帧序列间寻找路径的交叉点，以交叉点为图节点，建立有向加权拓扑地图，有向加权拓扑地图的边权重设置为交叉点间关键帧的数量；2-1、基于欧式距离寻找路径交叉点，欧式距离小于某一阈值的两个坐标点是同一地理位置；d(p1,p2)=(xp1-xp2)2+(yp1-yp2)2+(zp1-zp2)2]]>若d＜dthresh，则判定两个坐标点p1和p2属于同一个地理位置；2-2、建立有向图，并为图边设置权重权重的计算方式为：wedge＝numfrms×w1+d(p1,p2)×w2；其中，numfrms是节点间关键帧的数目；d(p1,p2)是节点间的欧式距离；且有，w1+w2＝1.0，其中，w1是关键帧数目所占的权重，w2是节点间欧式距离的大小所占的权重；步骤3，在上述有向加权拓扑地图上执行基于图搜索的路径规划算法A*算法，A*(A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。得到若干的路径交叉点，这些交叉点顺次连接构成了无人车可通行的最短路径；基于A*算法路径搜索；这种在静态离散栅格地图中求解最短路径的启发式搜索算法，其基本公式为：f(n)＝g(n)+h(n)；其中，f(n)是从初始状态经由状态n到达目标状态的代价值估计；g(n)是从初始状态到状态n的最短路径代价值估计；h(n)是从状态n到达目标状态的最短路径代价值估计；在前述的有向加权拓扑地图中的节点空间设定好起始点和目标点后，即可执行A*算法进行路径搜索，得到一条最短的可通行路径；步骤4，生成的交叉点序列，索引原始的多条关键帧位置序列，得到稠密的关键帧坐标，这些关键帧坐标连同交叉点序列，构成无人车的可执行轨迹；4-1、由索引到的分段路径拼合成整条轨迹，依次将相邻交叉点之间的若干关键帧取出，并构成一条向量，这条向量即构成了无人车的可通行轨迹；当规划出路径{A,B,E,C本文档来自技高网...

【技术保护点】
一种基于拓扑地图的无人车规划控制方法，其特征在于，包括以下步骤：步骤1，无人车多次建立地图，并存储关键帧坐标序列，每次建立地图的过程如下：启动无人车，所述无人车前方安装有双目相机，设置无人车为随机探索环境的自动运行模式；启动双目相机，采集环境图像，建立稀疏视觉特征地图，并以文本文件的形式存储关键帧位姿序列至车载计算机本地磁盘；每次建立地图的过程如下：1‑1、无人车为随机探索环境，利用无人车搭载的2D激光雷达探测周边的障碍分布信息，计算出安全的行驶方向，根据此安全行驶方向，得到角度控制量和速度控制量，驱动无人车以安全的运动轨迹在环境中探索；1‑2、建立稀疏视觉特征地图，在自动运行模式下，利用搭载的双目相机采集环境的图像，提取图像的特征点，并跟踪这些特征点，以计算无人车的位姿变换矩阵；1‑3、存储关键帧序列，按行存储关键帧位姿序列至车载计算机本地磁盘的文本文件，文本文件每行的数据格式为：xp yp zp qw qx qy qz；其中，为无人车的位置，为无人车的姿态；步骤2，从车载计算机的本地磁盘读取上述按行存储的关键帧位姿序列数据xp yp zp qw qx qy qz至内存，基于欧式距离，在不同关键帧序列间寻找路径的交叉点，以交叉点为图节点，建立有向加权拓扑地图，有向加权拓扑地图的边权重设置为交叉点间关键帧的数量；2‑1、基于欧式距离寻找路径交叉点，欧式距离小于某一阈值的两个坐标点是同一地理位置；d(p1,p2)=(xp1-xp2)2+(yp1-yp2)2+(zp1-zp2)2]]>若d＜dthresh，则判定两个坐标点p1和p2属于同一个地理位置；2‑2、建立有向图，并为图边设置权重权重的计算方式为：wedge＝numfrms×w1+d(p1,p2)×w2；其中，numfrms是节点间关键帧的数目；d(p1,p2)是节点间的欧式距离；且有，w1+w2＝1.0，其中，w1是关键帧数目所占的权重，w2是节点间欧式距离的大小所占的权重；步骤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、接收无人车当前位置xcurr，zcurr和朝向hcurr，目标位置xt和zt；4‑2‑2、将目标位置变换至本体坐标系，计算公式如下：xin_cam＝(xin_global‑xcam)·sin(θcam)‑(zin_global‑zcam)·cos(θcam)zin_cam＝(xin_global‑xcam)·cos(θcam)+(zin_global‑zcam)·sin(θcam)其中，本体坐标系的含义是以无人车中心为原点，前方为z轴正方向，右侧为x轴正方向的坐标系。xin_cam、zin_cam是目标位置在本体坐标系下的坐标；xin_global、zin_global是目标位置在地图中的位置；xcam、zcam、θcam是无人车在地图中的位置。4‑2‑3、计算参考向量，计算公式如下：vx＝xin_cam‑0vz＝zin_cam‑0其中，vx、vz是目标位置在本体坐标系下的参考向量；4‑2‑4、计算控制角θctrl，计算公式如下：θctrl＝tan‑1(vx,vz)。...

【技术特征摘要】
1.一种基于拓扑地图的无人车规划控制方法，其特征在于，包括以下步骤：步骤1，无人车多次建立地图，并存储关键帧坐标序列，每次建立地图的过程如下：启动无人车，所述无人车前方安装有双目相机，设置无人车为随机探索环境的自动运行模式；启动双目相机，采集环境图像，建立稀疏视觉特征地图，并以文本文件的形式存储关键帧位姿序列至车载计算机本地磁盘；每次建立地图的过程如下：1-1、无人车为随机探索环境，利用无人车搭载的2D激光雷达探测周边的障碍分布信息，计算出安全的行驶方向，根据此安全行驶方向，得到角度控制量和速度控制量，驱动无人车以安全的运动轨迹在环境中探索；1-2、建立稀疏视觉特征地图，在自动运行模式下，利用搭载的双目相机采集环境的图像，提取图像的特征点，并跟踪这些特征点，以计算无人车的位姿变换矩阵；1-3、存储关键帧序列，按行存储关键帧位姿序列至车载计算机本地磁盘的文本文件，文本文件每行的数据格式为：xpypzpqwqxqyqz；其中，为无人车的位置，为无人车的姿态；步骤2，从车载计算机的本地磁盘读取上述按行存储的关键帧位姿序列数据xpypzpqwqxqyqz至内存，基于欧式距离，在不同关键帧序列间寻找路径的交叉点，以交叉点为图节点，建立有向加权拓扑地图，有向加权拓扑地图的边权重设置为交叉点间关键帧的数量；2-1、基于欧式距离寻找路径交叉点，欧式距离小于某一阈值的两个坐标点是同一地理位置；d(p1,p2)=(xp1-xp2)2+(yp...

【专利技术属性】
技术研发人员：刘勇，张高明，张涛，
申请(专利权)人：浙江大学，
类型：发明
国别省市：浙江;33