一种基于karto和teb的高速建图导航方法技术

本发明专利技术涉及一种基于karto和teb的高速建图导航方法，属于建图导航技术领域。车辆控制系统通过车载里程计和激光雷达的输入数据调整车辆运行姿态，调整过程包括Karto算法SLAM地图构建步骤、定位步骤、全局规划步骤、TEB算法局部规划步骤和路径追踪步骤。本发明专利技术能够在车辆高速、算力受限的情况下进行精准的周围环境扫描与地图搭建，使无人车辆能够仅依靠车载里程计与激光雷达收集的数据，在高速运动中进行准确的定位与地图建立，并以此为基础进行导航，实现自主路径规划，在计算资源受限的情况下提升自动驾驶车辆的行驶速度上限。下提升自动驾驶车辆的行驶速度上限。下提升自动驾驶车辆的行驶速度上限。

【技术实现步骤摘要】
一种基于karto和teb的高速建图导航方法


[0001]本专利技术涉及一种基于karto和teb的高速建图导航方法，属于建图导航


技术介绍

[0002]建图导航系统是实现无人车自动规划路径的关键，它允许无人车在不需要人类协助的情况下，自主地通过自身传感器收集到的数据构建出周围的地图信息，并实时进行行进位置定位。随后，根据构建出的地图信息，无人车可以通过路径规划算法实现导航功能，从而能够自主规划前往目标位置的道路并且避开路上的障碍。
[0003]在这一领域，建图导航系统主要包括即时定位与地图构建算法(SLAM)与运动规划算法。然而，目前直接在大型无人车上实现实用级别的建图导航系统仍存在许多挑战，目前的技术人员主要针对不同的车辆、不同的具体应用场景设计对应的建图导航系统。在此，以全国大学生智能汽车竞赛室外专项赛的车辆、赛道设置为研究背景条件，无人车为小型的卡尔曼车模，搭载车载里程计、激光雷达与一个算力有限的CPU；赛道则相较于日常中的真实环境有一定的简化，但其仍具有丰富的环境要素，主要包括直道、直角弯道、圆弧弯道三种路线，赛道元本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于karto和teb的高速建图导航方法，其特征在于，步骤如下：(1)Karto算法SLAM地图构建步骤：使用karto算法，基于稀疏图优化的方法，实现高速环境下的定位建图；(2)定位步骤：使用ekf算法综合车载里程计、激光雷达和惯性测量单元输入的数据，进行车辆定位，从而允许导航进行；(3)全局规划步骤：通过综合处理车辆搭载的车载里程计、激光雷达和惯性测量单元输入与地图数据，为车辆规划最短的通往目标点的路径；(4)TEB算法局部规划步骤：基于全局规划得到的全局最短路径，使用TEB算法，实时生成局部目标点，并将激光雷达扫描到的环境信息与其实时结合，为车辆规划安全路径，实现避障功能；(5)路径追踪步骤：抽象车辆的数学模型，采用纯追踪算法，通过输入导航算法规划的路径得到车辆的转角、线速度数据。2.如权利要求1所述的基于karto和teb的高速建图导航方法，其特征在于，步骤(1)中，Karto算法SLAM地图构建步骤具体如下：
①
使用栅格地图技术，将整个地图划分为一系列小的栅格，每个栅格对应一个对数形式的占据概率，即该点有障碍物的概率；
②
通过扫描
‑
匹配技术提取激光雷达传感器收集的数据，生成局部地图，具体做法为：对于一个新输入的激光扫描数据，首先调用车载里程计的数据，作为当前扫描的预测位姿，预测位姿作为扫描
‑
匹配的先验知识，随后，采用滑动窗口法，将karto保存的最近24米的所有雷达数据转换成分辨率为0.01米的栅格地图，即生成局部地图，随后，遍历一定范围内所有的平移与旋转的位姿，选出得分最大的一个位姿，其中位姿的得分通过雷达点对应于栅格地图中的格子的占用概率来确定，公式如下：其中，P
l
表示所有雷达点的集合，O
p
表示雷达点p对应与栅格地图中的格子的占用概率；最后，对于与先验位姿差别大的位姿进行惩罚；
③
使用优化算法提高姿态估计的准确性，具体来说，首先，可以构建一个误差函数如下：其中，x为位姿的参数向量，e
i
(x)是第i个激光雷达扫描的测量误差；随后，对误差函数进行线性优化，得到误差函数的雅克比矩阵J(x)和残差向量r(x)，具体公式为：r(x)＝e(x)
‑
J(x)
·
Δx
ꢀꢀꢀꢀꢀꢀ
(4)其中，Δx是参数向量的更新量，公式如下：Δx＝(J(x)
T
·
J(x))―(J(x)
T
·
r(x))
ꢀꢀꢀꢀ
(5)然后，将更新量Δx添加到当前的参数向量x上，得到新的参数向量：x
new
＝x+Δx
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
重复上述式(3)到式(6)的步骤，直到Δx小于设定的阈值，则x优化完成；
④
对车辆进行回环检测，纠正地图的偏差和姿态估计的误差。3.如权利要求2所述的基于karto和teb的高速建图导航方法，其特征在于，步骤
④
中，回环检测步骤如下：特征提取：从激光雷达传回的数据中提取出关键特征点，特征点为显著的角点或边缘；特征匹配：将当前帧的特征与历史特征进行匹配，假如当前帧的特征与某一个历史特征类似，则视作车辆进行了一次回环，即车辆到达了过去曾经到过的区域；当存在回环时，结合历史帧与当前帧的位姿进行优化，使用最小二乘法来进行优化；地图更新：根据优化后的位姿信息，更新地图。4.如权利要求3所述的基于karto和teb的高速建图导航方法，其特征在于，步骤(2)中，定位主要分为预测与更新两个部分，具体步骤如下：预测部分，算法采用先前的状态估计量与输入控制量来预测现在的状态估计量，具体公式如下：其中，为k时刻的状态估计量，f是非线性的运动模型函数，u
k
为k时刻的输入控制量；更新部分，算法使用预测的状态估计量与测量输入来更新当前的状态估计量，具体来说，算法围绕预测状态估计量线性化测量模型，然后应用卡尔曼增益来校正预测状态估计量，数学公式如下：其中，K为卡尔曼增益，P为误差的协方差矩阵，H为测量模型的雅克比矩阵，R为测量噪音的协方差矩阵，z为测量输入值，h为非线性测量模型。5.如权利要求4所述的基于karto和teb的高速建图导航方法，其特征在于，步骤(3)中，全局规划步骤具体如下：对于全局路径规划，在已经完成建图与定位的基础上，寻找距离起点与终点最近的轨迹拓扑地图节点，使...

【专利技术属性】
技术研发人员：房明，吴承臻，王庚霖，王文涛，刘益，贺泽宇，
申请(专利权)人：山东大学，
类型：发明
国别省市：