CN116337045A - 一种基于karto和teb的高速建图导航方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明涉及一种基于karto和teb的高速建图导航方法，属于建图导航技术领域。

背景技术

建图导航系统是实现无人车自动规划路径的关键，它允许无人车在不需要人类协助的情况下，自主地通过自身传感器收集到的数据构建出周围的地图信息，并实时进行行进位置定位。随后，根据构建出的地图信息，无人车可以通过路径规划算法实现导航功能，从而能够自主规划前往目标位置的道路并且避开路上的障碍。

在这一领域，建图导航系统主要包括即时定位与地图构建算法(SLAM)与运动规划算法。然而，目前直接在大型无人车上实现实用级别的建图导航系统仍存在许多挑战，目前的技术人员主要针对不同的车辆、不同的具体应用场景设计对应的建图导航系统。在此，以全国大学生智能汽车竞赛室外专项赛的车辆、赛道设置为研究背景条件，无人车为小型的卡尔曼车模，搭载车载里程计、激光雷达与一个算力有限的CPU；赛道则相较于日常中的真实环境有一定的简化，但其仍具有丰富的环境要素，主要包括直道、直角弯道、圆弧弯道三种路线，赛道元素边界由固定尺寸锥桶(28*28*63cm)按一定距离、角度等摆放构成。在上述的背景条件下，实现在高速运行的过程中，车辆使用算力有限的CPU仍能够精准的定位、建图、导航功能。

具体来说，在“建图”方面，锥桶是无人驾驶系统对赛道进行有效识别的重要标志物，通过锥头的摆放位置即可确定赛道的轮廓，因此“建图”的主要目标就是在高速行驶的车辆环境中实现对于锥桶位置的精准定位。在完成了“建图”之后，还需要使用地图数据进行“导航”。实现“导航”的前提是精准地确定车辆自身的位置，随后经过一些路径规划算法结合车辆自身位置与地图数据进行车辆未来的行驶方向、速度、路径的规划，从而真正实现自动驾驶。

目前该方面技术中存在的一些问题与难点有：1.现有的建图方法虽然有很多种，但是往往在车辆高速与算力有限的真实应用环境下表现不佳。2.现有的许多建图方法以及导航方法往往都是割裂开的，并没有形成一个应用系统。为此，提出本发明。

发明内容

针对现有技术的不足，本发明提供一种基于karto和teb的高速建图导航方法，能够在车辆高速、算力受限的情况下进行精准的周围环境扫描与地图搭建，使无人车辆能够仅依靠车载里程计与激光雷达收集的数据，在高速运动中进行准确的定位与地图建立，并以此为基础进行导航，实现自主路径规划，在计算资源受限的情况下提升自动驾驶车辆的行驶速度上限。

本发明的技术方案如下：

一种基于karto和teb的高速建图导航方法，步骤如下：

(1)Karto算法SLAM地图构建步骤：使用karto算法，基于稀疏图优化的方法，实现高速环境下的定位建图；

(2)定位步骤：使用ekf算法综合车辆搭载的车载里程计、激光雷达和惯性测量单元输入的数据，进行车辆定位，从而允许导航进行；

(3)全局规划步骤：通过综合处理车载里程计、激光雷达和惯性测量单元输入与地图数据，为车辆规划最短的通往目标点的路径；

(4)TEB算法局部规划步骤：基于全局规划得到的全局最短路径，使用TEB算法，实时生成局部目标点，并将激光雷达扫描到的环境信息与其实时结合，为车辆规划安全路径，实现避障功能；

(5)路径追踪步骤：抽象车辆的数学模型，采用纯追踪算法，通过输入导航算法规划的路径得到车辆的转角、线速度等数据。

根据本发明优选地，步骤(1)中，Karto算法SLAM地图构建步骤具体如下：

①使用栅格地图技术，将整个地图划分为一系列小的栅格，每个栅格对应一个对数形式的占据概率，即该点有障碍物的概率；

②通过扫描-匹配技术(scan-matching)提取激光雷达传感器收集的数据，生成局部地图，具体做法为：对于一个新输入的激光扫描数据，首先调用车载里程计的数据，作为当前扫描的预测位姿，预测位姿作为扫描-匹配的先验知识，随后，采用滑动窗口法，将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优化完成；

通过以上步骤，优化算法能够时刻优化机器人的位姿，从而得到更精确位姿。

④在长时间的SLAM过程中，由于传感器误差和环境变化等因素，可能会导致地图偏差和姿态估计的不准确性，需要定期对车辆进行回环检测(即车辆是否回到过先前访问过的区域)，纠正地图的偏差和姿态估计的误差，回环检测步骤如下：

特征提取：从激光雷达传回的数据中提取出关键特征点，特征点为显著的角点、边缘等；

特征匹配：将当前帧的特征与历史特征进行匹配，假如当前帧的特征与某一个历史特征类似，则视作车辆进行了一次回环，即车辆到达了过去曾经到过的区域；

当存在回环时，结合历史帧与当前帧的位姿进行优化，使用最小二乘法来进行优化，具体的优化方法在③中说明。值得注意的是，karto算法中的优化算法与回环检测是合并在一个函数中的，回环检测中的优化算法就是③中所述的优化算法；

地图更新：根据优化后的位姿信息，更新地图。

根据本发明优选地，步骤(2)中，定位主要分为预测与更新两个部分，具体步骤如下：

预测部分，算法采用先前的状态估计量与输入控制量来预测现在的状态估计量，具体公式如下：

其中，

为k时刻的状态估计量，f是非线性的运动模型函数，u_k为k时刻的输入控制量；

更新部分，算法使用预测的状态估计量与测量输入来更新当前的状态估计量，具体来说，算法围绕预测状态估计量线性化测量模型，然后应用卡尔曼增益来校正预测状态估计量，数学公式如下：

其中，K为卡尔曼增益，P为误差的协方差矩阵，H为测量模型的雅克比矩阵，R为测量噪音的协方差矩阵，z为测量输入值，h为非线性测量模型。

根据本发明优选地，步骤(3)中，全局规划步骤具体如下：

对于全局路径规划，在已经完成建图与定位的基础上，寻找距离起点与终点最近的轨迹拓扑地图节点，使用A*算法在拓扑地图上找到连接这两个节点的最短路径。A*算法为现有算法，是一种非常经典的、简洁的、启发式的路径规划算法。

根据本发明优选地，步骤(4)中，TEB算法局部规划步骤如下：

首先，建立车辆的状态空间，描述车辆地图中的位置与姿态，公式如下：

X_a＝(x_i,y_i,β_i)^T∈R²×S¹ (9)

其中，x_i,y_i,β_i分别代表车辆在地图中的横坐标、纵坐标和相对于横坐标的倾角。

空间中车辆的一系列位姿的序列如下式描述：

Q＝{X_i}_{i＝0,1,…,n}n∈N (10)

将位姿序列Q与时间序列合并，得到：

B：＝(Q，τ) (11)

通过加权多目标优化，获取最优的路径点，即最优的Q：

f(B)＝Σ_aγ_kf_k(B)

B^*＝arg_Bminf(B) (12)

其中，B^*为最优结果，f(B)为考虑各种约束的目标函数，γ_k为各种约束的权重。随后，引入代价函数来惩罚不满足约束条件的情况，代价函数如下式所示：

其中x代表机器人位姿，x_r为约束值，S表示缩放系数，n表示多项式系数，∈表示约束值附近的一个微小位移量。

然后，根据弹性带(elastic band)的概念，引入2组约束条件，一组为跟踪约束与避障约束，另一组为速度约束；

对于弹性带这一概念，具体来说，TEB算法的核心就是将路径视作一条弹性带，这条弹性带连接起始点、目标点，并且可以变形。变形的外因就是将约束条件视作施加于弹性带的外力。

跟踪约束与避障约束：

这一组的约束主要有2个目的：尽可能贴合全局规划得到的路径，尽可能避障。这两个目的对应的代价函数非常类似，一个是对弹性带施力将其拉向全局路径，一个是对弹性带施力使其远离障碍物，具体公式如下：

其中，d_min，j为位姿序列与全局规划点序列或障碍物z_j的最近距离，

为位姿序列距离全局规划点序列最大允许的距离值，/>

为位姿序列距离障碍物最小允许的距离值，S表示缩放系数，n表示多项式系数，∈表示约束值附近的一个微小位移量。

速度约束

由速度决定的动力学约束可以用约束的惩罚函数表示，车辆运动的平均线速度可以通过相邻的位姿x_i+1，x_i与时间间隔ΔT计算得到；

类似于跟踪约束与避障约束中的情况，线速度的约束表示为：

上述式子中，v_i代表机器人运动的平均线速度，x_i代表机器人位姿序列中第i项的横坐标，y_i代表机器人位姿序列中第i项的纵坐标，β_i代表机器人位姿序列中第i项的倾角，ΔT代表时间间隔(位姿序列中前一项与后一项的时间间隔)。

根据设定好的目标函数与约束条件，该问题已经转变为一个纯粹的优化问题。使用ros系统中g2o这一通用图优化库中的图优化算法，设定线性方程，求解器为PCG，设定迭代策略为GN，屡次迭代优化得到最终的车辆安全轨迹。图优化库是一个非常成熟的用于图优化的算法库。

根据本发明优选地，步骤(5)中，路径追踪步骤如下：

根据车辆采用的车模类型，抽象其转向模型如图3所示。

为了便于分析，将模型进行简化，即在前轮中心和后轮中心各假想一个轮胎来代表前后轮的运动，即所谓的单车模型，如图4所示；

控制量选取后轮速度v及前轮转角δ,状态量选取世界坐标系下的坐标x，y及航向角θ,则阿克曼转向的运动学模型表示为：

对应的里程计模型为：

根据上述式子关系可以得到：

上式的δ是我们要求的转向角，R为末知量，由于曲率k与半径R呈倒数关系可将上式改写为

δ＝tan^-1(kL) (20)

这时求解转换为解等式右边式子,其中L已知,是车辆的轴距,末知量是曲率k，接下来求解k；

由正弦定理可得：

考虑时间，将式(20)表示为：

根据上述数学建模计算，纯追踪导航算法变成每一个时刻求解α的过程；

读取车载里程计传来的，经过滤波的数据，得到车辆的当前位置，并将其作为纯追踪算法的输入；

根据目标物体位置、自身位置，通过上述数学推导所得的计算式(22)，进行舵机转角计算，得到车的行驶角度与角速度，具体来说，给定物体位置与自身位置，即给定了l_d与α，在默认已知车辆自身的轴距L的情况下，即可求得δ；

根据计算得出的δ值，用户设定的行驶速度，将其输入车辆马达与舵机的驱动函数，控制车辆执行相应的行动。

本发明的有益效果在于：

本发明设计了一种基于karto和teb的高速建图导航方法，其主要针对当下无人车算力受限与高速运行下建图精度降低两大问题，使用了karto这一SLAM算法与TEB作为局部规划器，实现了无人车高速运动下的精确建图与导航。有利于提高无人驾驶的安全行驶速度，提高建图、导航系统的精度与可靠性，降低对于CPU性能的要求，使无人车更加可靠、高效。

附图说明

图1为本发明在高速运动、场景较复杂情况下建立的地图；

图2为本发明的导航的系统架构图；

图3为本发明使用的阿克曼车模模型抽象图；

图4为本发明简化的单车模型示意图；

图5为本发明成功配置的tf坐标关系图；

图6为本发明实施例的测试结果图；

图7为本发明的建图导航系统架构图。

具体实施方式

下面通过实施例并结合附图对本发明做进一步说明，但不限于此。

实施例1：

本实施例提供一种基于karto和teb的高速建图导航方法，步骤如下：

(1)Karto算法SLAM地图构建步骤：使用karto算法，基于稀疏图优化的方法，实现高速环境下的定位建图；

(2)定位步骤：使用ekf算法综合车载里程计和激光雷达输入的数据，进行车辆定位，从而允许导航进行；

(3)全局规划步骤：通过综合处理车辆搭载的车载里程计和激光雷达输入与地图数据，为车辆规划最短的通往目标点的路径；

(4)TEB算法局部规划步骤：基于全局规划得到的全局最短路径，使用TEB算法，实时生成局部目标点，并将激光雷达扫描到的环境信息与其实时结合，为车辆规划安全路径，实现避障功能；

(5)路径追踪步骤：抽象车辆的数学模型，采用纯追踪算法，通过输入导航算法规划的路径得到车辆的转角、线速度等数据。

根据本发明优选地，步骤(1)中，Karto算法SLAM地图构建步骤具体如下：

①使用栅格地图技术，将整个地图划分为一系列小的栅格，每个栅格对应一个对数形式的占据概率，即该点有障碍物的概率；

②通过扫描-匹配技术(scan-matching)提取激光雷达传感器收集的数据，生成局部地图，具体做法为：对于一个新输入的激光扫描数据，首先调用车载里程计的数据，作为当前扫描的预测位姿，预测位姿作为扫描-匹配的先验知识，随后，采用滑动窗口法，将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_ncw＝x+Δx (6)

重复上述式(3)到式(6)的步骤，直到Δx小于设定的阈值，则x优化完成；

通过以上步骤，优化算法能够时刻优化机器人的位姿，从而得到更精确位姿。

④在长时间的SLAM过程中，由于传感器误差和环境变化等因素，可能会导致地图偏差和姿态估计的不准确性，需要定期对车辆进行回环检测(即车辆是否回到过先前访问过的区域)，纠正地图的偏差和姿态估计的误差，回环检测步骤如下：

特征提取：从激光雷达传回的数据中提取出关键特征点，特征点为显著的角点、边缘等；

特征匹配：将当前帧的特征与历史特征进行匹配，假如当前帧的特征与某一个历史特征类似，则视作车辆进行了一次回环，即车辆到达了过去曾经到过的区域；

当存在回环时，结合历史帧与当前帧的位姿进行优化，使用最小二乘法来进行优化，具体的优化方法在③中说明。值得注意的是，karto算法中的优化算法与回环检测是合并在一个函数中的，回环检测中的优化算法就是③中所述的优化算法；

地图更新：根据优化后的位姿信息，更新地图。

根据本发明优选地，步骤(2)中，定位主要分为预测与更新两个部分，具体步骤如下：

预测部分，算法采用先前的状态估计量与输入控制量来预测现在的状态估计量，具体公式如下：

其中，

为k时刻的状态估计量，f是非线性的运动模型函数，u_k为k时刻的输入控制量；

更新部分，算法使用预测的状态估计量与测量输入来更新当前的状态估计量，具体来说，算法围绕预测状态估计量线性化测量模型，然后应用卡尔曼增益来校正预测状态估计量，数学公式如下：

其中，K为卡尔曼增益，P为误差的协方差矩阵，H为测量模型的雅克比矩阵，R为测量噪音的协方差矩阵，z为测量输入值，h为非线性测量模型。

根据本发明优选地，步骤(3)中，全局规划步骤具体如下：

对于全局路径规划，在已经完成建图与定位的基础上，寻找距离起点与终点最近的轨迹拓扑地图节点，使用A*算法在拓扑地图上找到连接这两个节点的最短路径。A*算法为现有算法，是一种非常经典的、简洁的、启发式的路径规划算法。

根据本发明优选地，步骤(4)中，TEB算法局部规划步骤如下：

首先，建立车辆的状态空间，描述车辆地图中的位置与姿态，公式如下：

X_a＝(x_i，y_i，β_i)^T∈R²×S¹ (9)

其中，x_i，y_i，β_i分别代表车辆在地图中的横坐标、纵坐标和相对于横坐标的倾角。

空间中车辆的一系列位姿的序列如下式描述：

Q＝{X_i}_{i＝0，1，...，n}n∈N (10)

将位姿序列Q与时间序列合并，得到：

B：＝(Q，τ) (11)

通过加权多目标优化，获取最优的路径点，即最优的Q：

f(B)＝∑_aγ_kf_k(B)

B^*＝arg_Bminf(B) (12)

其中，B^*为最优结果，f(B)为考虑各种约束的目标函数，γ_k为各种约束的权重。随后，引入代价函数来惩罚不满足约束条件的情况，代价函数如下式所示：

其中x代表机器人位姿，x_r为约束值，S表示缩放系数，n表示多项式系数，∈表示约束值附近的一个微小位移量。

然后，根据弹性带(elastic band)的概念，引入2组约束条件，一组为跟踪约束与避障约束，另一组为速度约束；

对于弹性带这一概念，具体来说，TEB算法的核心就是将路径视作一条弹性带，这条弹性带连接起始点、目标点，并且可以变形。变形的外因就是将约束条件视作施加于弹性带的外力。

跟踪约束与避障约束：

这一组的约束主要有2个目的：尽可能贴合全局规划得到的路径，尽可能避障。这两个目的对应的代价函数非常类似，一个是对弹性带施力将其拉向全局路径，一个是对弹性带施力使其远离障碍物，具体公式如下：

其中，d_min，j为位姿序列与全局规划点序列或障碍物z_j的最近距离，

为位姿序列距离全局规划点序列最大允许的距离值，/>

为位姿序列距离障碍物最小允许的距离值，S表示缩放系数，n表示多项式系数，∈表示约束值附近的一个微小位移量。

速度约束

由速度决定的动力学约束可以用约束的惩罚函数表示，车辆运动的平均线速度可以通过相邻的位姿x_i+1，x_i与时间间隔ΔT计算得到；

类似于跟踪约束与避障约束中的情况，线速度的约束表示为：

上述式子中，v_i代表机器人运动的平均线速度，x_i代表机器人位姿序列中第i项的横坐标，y_i代表机器人位姿序列中第i项的纵坐标，β_i代表机器人位姿序列中第i项的倾角，ΔT代表时间间隔(位姿序列中前一项与后一项的时间间隔)。

根据设定好的目标函数与约束条件，该问题已经转变为一个纯粹的优化问题。使用ros系统中g2o这一通用图优化库中的图优化算法，设定线性方程，求解器为PCG，设定迭代策略为GN，屡次迭代优化得到最终的车辆安全轨迹。图优化库是一个非常成熟的用于图优化的算法库。

根据本发明优选地，步骤(5)中，路径追踪步骤如下：

根据车辆采用的车模类型，抽象其转向模型如图4所示。

为了便于分析，将模型进行简化，即在前轮中心和后轮中心各假想一个轮胎来代表前后轮的运动，即所谓的单车模型，如图5所示；

控制量选取后轮速度v及前轮转角δ,状态量选取世界坐标系下的坐标x，y及航向角θ,则阿克曼转向的运动学模型表示为：

对应的里程计模型为：

根据上述式子关系可以得到：

上式的δ是我们要求的转向角，R为末知量，由于曲率k与半径R呈倒数关系可将上式改写为

δ＝tan^-1(kL) (20)

这时求解转换为解等式右边式子,其中L已知,是车辆的轴距,末知量是曲率k，接下来求解k；

由正弦定理可得：

考虑时间，将式(20)表示为：

根据上述数学建模计算，纯追踪导航算法变成每一个时刻求解α的过程；

读取车载里程计传来的，经过滤波的数据，得到车辆的当前位置，并将其作为纯追踪算法的输入；

根据目标物体位置、自身位置，通过上述数学推导所得的计算式(22)，进行舵机转角计算，得到车的行驶角度与角速度，具体来说，给定物体位置与自身位置，即给定了l_d与α，在默认已知车辆自身的轴距L的情况下，即可求得δ；

根据计算得出的δ值，用户设定的行驶速度，将其输入车辆马达与舵机的驱动函数，控制车辆执行相应的行动。

最终，成功配置的tf坐标关系如图5所示。tf坐标关系具体指车辆内部各个传感器读取数据之间的一种互相转换的规则，由于往往同一台车辆上在不同位置会装有多个传感器，而这些传感器各自记录的数据显然不是基于同一个原点，所以需要构建tf坐标关系来协调不同位置传感器的数据。

测试导航算法：

为了验证导航算法能够正确运行，在比赛赛道上进行了实车测试，结果如图6所示。从图6的6个附图中可以看到纯追踪算法驱动下小车一次次到达局部规划的目标点。由图6(c)(d)中转弯时的路径可以看到车辆有避开了弯角以及道路边的障碍物，体现了算法的局部避障功能。

Claims (7)

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)中，全局规划步骤具体如下：

对于全局路径规划，在已经完成建图与定位的基础上，寻找距离起点与终点最近的轨迹拓扑地图节点，使用A*算法在拓扑地图上找到连接这两个节点的最短路径。

6.如权利要求5所述的基于karto和teb的高速建图导航方法，其特征在于，步骤(4)中，TEB算法局部规划步骤如下：

首先，建立车辆的状态空间，描述车辆地图中的位置与姿态，公式如下：

X_a＝(x_i，y_i，β_i)^T∈R²×S¹ (9)

其中，x_i，y_i，β_i分别代表车辆在地图中的横坐标、纵坐标和相对于横坐标的倾角；

空间中车辆的一系列位姿的序列如下式描述：

Q＝{X_i}_{i＝0，1，...，n}n∈N (10)

将位姿序列Q与时间序列合并，得到：

B：＝(Q，τ) (11)

通过加权多目标优化，获取最优的路径点，即最优的Q：

f(B)＝∑_aγ_kf_k(B)

B^*＝arg Bminf(B) (12)

其中，B^*为最优结果，f(B)为考虑各种约束的目标函数，γ_k为各种约束的权重；随后，引入代价函数来惩罚不满足约束条件的情况，代价函数如下式所示：

其中x代表机器人位姿，x_r为约束值，S表示缩放系数，n表示多项式系数，∈表示约束值附近的一个微小位移量；

然后，根据弹性带的概念，引入2组约束条件，一组为跟踪约束与避障约束，另一组为速度约束；

跟踪约束与避障约束：

这一组的约束主要有2个目的：贴合全局规划得到的路径，避障，这两个目的对应的代价函数非常类似，一个是对弹性带施力将其拉向全局路径，一个是对弹性带施力使其远离障碍物，具体公式如下：

其中，d_min，j为位姿序列与全局规划点序列或障碍物z_j的最近距离，

为位姿序列距离全局规划点序列最大允许的距离值，/>

为位姿序列距离障碍物最小允许的距离值，S表示缩放系数，n表示多项式系数，∈表示约束值附近的一个微小位移量；

速度约束

由速度决定的动力学约束可以用约束的惩罚函数表示，车辆运动的平均线速度可以通过相邻的位姿x_i+1，x_i与时间间隔ΔT计算得到；

线速度的约束表示为：

上述式子中，v_i代表机器人运动的平均线速度，x_i代表机器人位姿序列中第i项的横坐标，y_i代表机器人位姿序列中第i项的纵坐标，β_i代表机器人位姿序列中第i项的倾角，ΔT代表时间间隔；

根据设定好的目标函数与约束条件，使用ros系统中g2o这一通用图优化库中的图优化算法，设定线性方程，求解器为PCG，设定迭代策略为GN，屡次迭代优化得到最终的车辆安全轨迹。

7.如权利要求6所述的基于karto和teb的高速建图导航方法，其特征在于，步骤(5)中，路径追踪步骤如下：

为了便于分析，将模型进行简化，即在前轮中心和后轮中心各假想一个轮胎来代表前后轮的运动，即所谓的单车模型；

控制量选取后轮速度v及前轮转角δ，状态量选取世界坐标系下的坐标x，y及航向角θ，则阿克曼转向的运动学模型表示为：

对应的里程计模型为：

根据上述式子关系可以得到：

上式的δ是我们要求的转向角，R为末知量，由于曲率k与半径R呈倒数关系可将上式改写为

δ＝tan^-1(kL) (20)

这时求解转换为解等式右边式子，其中L已知，是车辆的轴距，末知量是曲率k，接下来求解k；

由正弦定理可得：

考虑时间，将式(20)表示为：

根据上述数学建模计算，纯追踪导航算法变成每一个时刻求解α的过程；

读取车载里程计传来的，经过滤波的数据，得到车辆的当前位置，并将其作为纯追踪算法的输入；

根据目标物体位置、自身位置，通过上述数学推导所得的计算式(22)，进行舵机转角计算，得到车的行驶角度与角速度，具体来说，给定物体位置与自身位置，即给定了l_d与α，在默认已知车辆自身的轴距L的情况下，即可求得δ；

根据计算得出的δ值，用户设定的行驶速度，将其输入车辆马达与舵机的驱动函数，控制车辆执行相应的行动。

Images