CN114200926B - 一种无人驾驶车辆的局部路径规划方法及系统 - Google Patents

Abstract

本发明公开了一种无人驾驶车辆的局部路径规划方法及系统，基于组合导航传感器实现，所述方法包括：步骤1)在世界坐标系中建立运动学方程，以车辆运动中的速度和转角为自变量，建立运动模型；步骤2)基于实时载波相位动态差分技术，将组合导航传感器获取的数据转换至导航坐标系，为车辆运行提供定位信息；步骤3)从时间和位姿两个方面，通过实施加权多目标优化的方法，建立多目标代价函数；步骤4)通过确定车辆的运行状态，融合时间分辨率参数dt_ref进行动态调节，实现车辆稳定运行。本发明的方法提高了车辆在转弯过程中的速度和转角的稳定性，实现了车辆在不同弯道处的柔性、准确运动的目的，从应用角度，扩展了无人驾驶车辆的使用范围。

Description

一种无人驾驶车辆的局部路径规划方法及系统

技术领域

本发明为无人驾驶技术、路径规划技术以及车辆控制技术领域，尤其涉及一种无人驾驶车辆的局部路径规划方法及系统。

背景技术

由于具有无人驾驶功能的智能汽车能够从多方面解决因驾驶疲劳而产生的交通事故和大气污染等问题，智能无人汽车逐渐成为人们关注和研究的重点。从发展趋势来看，无人驾驶将是未来一个重要的发展方向，由于无人驾驶涉及到一个庞大的产业链，所以在推进无人驾驶的过程中，不仅会促进汽车产业的可持续发展和绿色发展进程，同时也会带动更多的投资和就业。无人驾驶智能汽车技术可以分为三部分，环境感知技术、智能决策技术、控制执行技术三个部分。在无人驾驶系统中，无人驾驶车辆路径规划可分为全局路径规划和局部路径规划。全局路径规划是一种已知所有的障碍信息的路径规划方法，在实际应用程序中属于静态路径规划，在障碍信息多样化的情况下，全局路径规划需要基于传感器信息，依靠局部路径规划信息完成运动控制。局部路径规划对动态避障动态障碍物规避、轨迹跟踪以及车辆稳定控制具有重要的作用，提高了规划系统在动态未知环境中的稳定运行的能力。

现阶段常用的局部路径规划方法时间弹性带算法，在无人驾驶汽车中运用极广。时间弹性带算法又称TEB算法，TEB算法是通过对全局规划器生成的初始轨迹进行后续修正来优化轨迹的方法。轨迹优化考虑的目标包括总体路径长度、轨迹执行时间、与障碍物的分离，通过中间路径点，是否符合机器人的动力学、运动学和几何约束。起始点、目标点状态由用户/全局规划器指定，中间插入N个控制橡皮筋形状的控制点(无人车姿态)，为了显示轨迹的运动学信息，在点与点之间定义运动时间Time，即为Timed-Elastic-Band算法。在车辆直行和转弯的稳定性控制方面，有多个参数对性能影响较大，其中时间分辨率dt_ref，也是点与点之间定义运动时间Time，通过无人车实测，动态修改该参数，能够有效改善车辆直行过程中摆动等情况。但一般在运用TEB算法进行局部路径规划时，时间分辨率是直接设定的一个固定的值。由于无人车在直行和拐弯时，时间分辨率不变，车辆有直行摆动、转弯不平顺的现象。

发明内容

现有技术存在直行摆动、转弯不平顺的问题，本发明的目的在于克服上述现有技术缺陷，提出了一种无人驾驶车辆的局部路径规划方法及系统。

为了实现上述目的，本发明提出了一种无人驾驶车辆的局部路径规划方法，基于组合导航传感器实现，所述方法包括：

步骤1)在世界坐标系中建立运动学方程，以车辆运动中的速度和转角为自变量，建立运动模型；

步骤2)基于实时载波相位动态差分技术，将组合导航传感器获取的数据转换至导航坐标系，为车辆运行提供定位信息；

步骤3)从时间和位姿两个方面，通过实施加权多目标优化的方法，建立多目标代价函数；

步骤4)通过确定车辆的运行状态，融合时间分辨率参数dt_ref进行动态调节，实现车辆稳定运行。

作为上述方法的一种改进，所述步骤1)具体包括：

将世界坐标系OwXwYw的基坐标系设置在车辆简化的运行模型后轴中心上，设置O为运动模型的瞬时旋转中心；

建立自动驾驶车辆运动学状态方程为：

其中，s(t)＝[x(t),y(t),β(t)]^T为车辆的位姿状态量，

为车辆位姿状态的导数，x(t)和y(t)分别为车辆在世界坐标系的X轴和Y轴的位移量，β(t)为t时刻车辆质心处的转向角，v(t)为t时刻车辆后轴中心速度，θ(t)为t时刻车辆的横摆角，l_r为车辆后轮到车辆质心的距离。

作为上述方法的一种改进，所述步骤2)具体包括：

步骤2-1)将组合导航传感器获取的大地坐标系下的地理经度λ、地理纬度φ、大地高度H，转换至地理直角坐标系下的坐标值(x,y,z)；

其中，N为椭球面卯酉圈的曲率半径，满足下式：

e为椭球的第一偏心率，a、b分别为椭球的长、短半径，均为常数，W为第一辅助系数；

步骤2-2)选取P₀＝[x₀，y₀，z₀]^T作为导航坐标系的原点，P₀点表示车辆的初始位置在空间直角坐标系下的坐标，T表示转置，P＝[x,y,z]^T表示地理直角坐标系下的坐标值；[Δx，Δy，Δz]^T表示在空间直角坐标系下当前位置与初始位置之间的差值：

通过坐标旋转，计算点P＝[x,y,z]^T在以点P₀为坐标原点的导航坐标系下的位置(E,N,U)为：

为坐标转换矩阵，满足下式：

作为上述方法的一种改进，所述步骤3)具体包括：

从时间和位姿两个方面，通过实施加权多目标优化的方法，建立多目标代价函数为：

B^*＝argminf(B)

其中，B表示由姿序列信息和时间间隔序列信息的TEB轨迹模型；f(B)表示各种约束目标函数之和，γ_k为第k个目标函数f_k(B)的权重系数，1≤k≤K，B^*为被优化的TEB序列结果；所述目标函数包括用惩罚函数表示的速度和加速度约束以及与轨迹有关的目标函数，所述TEB序列包括位姿序列信息和时间间隔序列信息。

作为上述方法的一种改进，所述步骤4)的确定车辆的运行状态，具体包括：

根据设定时间内的转向角度，确定车辆为直行状态或转弯状态；

根据下式确定时间分辨率参数dt_ref：

其中，d为预先指定的时间间隔，Δd为转弯时的变化量。

作为上述方法的一种改进，所述步骤4)的时间分辨率参数dt_ref动态调节方法具体包括：

在初始化阶段，根据A-Star算法生成的全局路径，生成TEB序列；

在轨迹修正阶段，根据下式计算两个相邻位姿之间的时间间隔dt：

其中，p_i和p_i-1分别为两个相邻的位姿点，max_vel_x为车辆最大速度；

设定dt_ref为的两个点之间的指定时间间隔，dt_hysteresis为滞后时间，dt_hysteresis∈(-0.1*dt_ref，0.1*dt_ref)；

当dt>dt_ref+dt_hysteresis，在P_i-1点和P_i点的中间位置插入点P_i，P_i点变为P_i+1点，P_i-1点和P_i点之间的时间间隔为原来P_i-1点和P_i点之间的时间间隔的一半；

当dt<dt_ref+dt_hysteresis时，删除P_i点，P_i+1点成为新的P_i点，P_i-1点和P_i点之间的时间间隔为原来P_i-1点至P_i+1点时间间隔之和；

在轨迹修正阶段，重复进行增加和删除点的操作，直至整个TEB序列任意两个点之间的时间间隔都在指定的dt_ref范围内或达到最大迭代次数，从而得到动态调整好的局部路径。

一种无人驾驶车辆的局部路径规划系统，基于组合导航传感器实现，所述系统包括：运动模型建立模块、定位信息获取模块、代价函数建立模块和动态调节模块；其中，

所述运动模型建立模块，用于在世界坐标系中建立运动学方程，以车辆运动中的速度和转角为自变量，建立运动模型；

所述定位信息获取模块，用于基于实时载波相位动态差分技术，将组合导航传感器获取的数据转换至导航坐标系，为车辆运行提供定位信息；

所述代价函数建立模块，用于从时间和位姿两个方面，通过实施加权多目标优化的方法，建立多目标代价函数；

所述动态调节模块，用于通过确定车辆的运行状态，融合时间分辨率参数dt_ref进行动态调节，实现车辆稳定运行

与现有技术相比，本发明的优势在于：

1、采用本发明的方法无人驾驶车辆在直行过程中加速度和速度变化稳定，不会出现突然的跳变，车辆运行状态平滑；

2、本发明采用时间分辨率动态调节与匹配方法，提高了车辆在转弯过程中的速度和转角的稳定性，实现了车辆在不同弯道处的柔性、准确运动的目的，从应用角度，扩展了无人驾驶车辆的使用范围。

附图说明

图1是简化的运行学模型示意图；

图2是地理坐标系和导航坐标系示意图；

图3是本发明无人驾驶车辆的局部路径规划方法控制流程图。

具体实施方式

一种针对无人驾驶车辆的局部路径方法可以简述如下：首先在世界坐标系中建立系统的运动学方程，考虑到车辆运动中速度和转角两个参数对稳定性影响较大，因此以这两个量为自变量，得到运动模型；其次基于实施载波动态相位差分技术，将组合导航数据高精度的位置、航向数据转换导航坐标系中，为车辆运行提供可靠的定位信息；再次根据经典的时间弹性带算法，从时间和位姿两个方面，通过实施加权多目标优化的方法，建立多目标代价函数，为最优路径点的获取提供理论基础；通过确定车辆的运行状态，直行或者转弯，融合时间分辨率参数dt_ref动态调节方法，最终实现车辆稳定运行的目的。

下面结合附图和实施例对本发明的技术方案进行详细的说明。

实施例1

本发明的实施例1提出了一种无人驾驶车辆的局部路径规划方法。具体步骤如下：

一、建立运动学方程

如图1所示，世界坐标系为X_WO_WY_W，基坐标系设置在简化的运行模型后轴中心上，x,y为车辆在世界坐标系中的坐标，O为运动模型的瞬时旋转中心，or为后轮中心，v为车辆后轴中心速度，θ为车辆的横摆角(航向角)，α为前轮虚拟转向角，β为质心处的转向角，R为转弯半径，前轮和后轮到车辆质心的距离分别为l_f和l_r，L为轴距，等于l_f和l_r之和。

由运动学关系，得

t表示当前运动时刻，R_min表示最小转弯半径，ω为前轮转角速度。

根据几何关系，与前后轮到车辆质心的距离、，前轮虚拟转向角α满足如下关系：

在无人驾驶车辆的路径跟踪过程中，阿克曼车辆的控制输入量为u(t)＝[v(t),β(t)]^T。

建立自动驾驶车辆运动学状态方程建立如下：

其中，s(t)＝[x(t),y(t),β(t)]^T为车辆的位姿状态量，

为车辆位姿状态的导数，x(t)和y(t)分别为车辆在世界坐标系的X轴和Y轴的位移量，T表示转置。

二、坐标变换

通过组合导航传感器，直接获取的数据是大地坐标系下的经度、纬度、高度信息，需要将坐标转换到统一的导航坐标系下。过程为先将大地坐标系的经度纬度高度新，转换为地理直角坐标系的XYZ，再将地理直角坐标系转换为导航坐标系(东北天坐标系)，如图2所示，为路径规划提供高精度的定位数据。

(1)大地坐标系与地理坐标系的转换

地理坐标系随地球自转而不断旋转，其原点在地球中心，参考面同样为地球赤道平面，其中X_e轴指向赤道面与格林尼治子午面(Greenwich)的交点；Z_e轴与地球赤道平面垂直，指向北极；Y_e轴与X_e轴、Z_e轴构成右手直角坐标系。该坐标系的转动角速度与地球自转角速度相同。组合导航的数据是以地理经度λ、地理纬度φ、大地高度H表示。

坐标转换关系为：

其中，a＝6378137，b＝635672.3142，

上式中，XYZ为地理坐标系中的坐标值。N为椭球面卯酉圈的曲率半径，e为椭球的第一偏心率，a、b为椭球的长短半径，W为第一辅助系数。

(2)地理坐标系与导航坐标系之间的转换

坐标转换矩阵为：

这里选取P₀＝[x₀，y₀，z₀]^T作为导航坐标系的原点，P₀点表示载体的初始位置在空间直角坐标系下的坐标，P＝[x,y,z]^T表示载体的当前位置在地理坐标系下的坐标；用[Δx，Δy，Δz]^T表示在空间直角坐标系下载体当前位置与初始位置之间的差值。

通过坐标旋转，计算点P＝[x,y,z]^T在以点P₀为坐标原点的导航坐标系下的位置(E,N,U)。

三、建立时间弹性带方法的代价函数

时间弹性带(TEB)在弹性带(EB)的基础上，增加了相邻两个位姿之间的时间间隔ΔT_i，ΔT_i表示从位姿p_i运动到位姿p_i+1所需的时间，n个位姿序列生成n-1个时间间隔序列，可表示为：

τ＝{ΔT_i}_{i＝0,1,…,n-1}n∈N

TEB轨迹模型包含位姿序列信息和时间间隔序列信息，可表示为：

B＝(Q,τ)＝{(p₀,ΔT₀),(p₁,ΔT₁),…(p_n-1,ΔT_n-1)}

TEB算法的关键思想是通过实时加权多目标优化的方法对机器人的位姿和时间差进行调整和优化，以获得最优的路径点。

B^*＝argminf(B)

其中f(B)表示各种约束目标函数之和，它是各目标函数乘以权重系数之和，其目标函数属于两种类型:用惩罚函数表示的速度和加速度约束以及与轨迹有关的目标函数，比如最短路径、最快路径轨或避障；γ_k为某个目标函数的权重系数，B^*为被优化的TEB序列结果。

TEB被表述为一个多目标优化问题，但目标函数的大部分位姿都是局部的，只依赖于几个连续的机器人状态，而不是整条全局轨迹上的位姿，TEB的这种局部性导致了稀疏系统矩阵，对于这种矩阵，可以使用专门快速有效的大规模数值优化方法去求解。

四、时间分辨率参数动态调节

在初始化阶段，根据A*算法(也叫A-Star算法)生成的全局路径，生成由位姿序列和时间序列所构成的初始化轨迹序列B(Q,τ)。在轨迹修正阶段，为了遵循初始的dt_ref，该序列函数B(Q,τ)可以新插入或删除路径中的点。

式中，dt为初始轨迹B(Q,τ)中两个相邻位姿之间的时间间隔，p_i和p_i-1为两个相邻的位姿点，max_vel_x为车辆最大速度。

当dt>dt_ref+dt_hysteresis，dt_hysteresis为滞后时间，范围为±0.1*dt_ref，在P_i-1点和P_i点的中间位置插入点P_i，P_i点变为P_i+1点，P_i-1点和P_i点之间的时间间隔为原来P_i-1点和P_i点之间的时间间隔的一半。

当dt<dt_ref+dt_hysteresis时，删除P_i点，P_i+1点成为新的P_i点，P_i-1点和P_i点之间的时间间隔为原来P_i-1点至P_i+1点时间间隔之和。

如此往复的增加和删除，最终能够保证整个TEB路径两个点之间的时间间隔都在指定的dt_ref范围内。有时难以同时满足上述两个条件，程序中定义了最大迭代次数，以避免卡死。

非直角转弯情况时，dt_ref参数越小，单位距离内的规划的路径点越多，车辆行驶就越平稳，可以有效降低车辆的大幅度摆动问题，提高车辆运行的稳定性。直角转弯时，由于非完整运动学的约束，两个相邻的位姿需要位于一个恒定曲率的公共弧上，有：

d_i＝dt_ref*max_vel_x

式中，d_i为运动方向向量，r_i为转弯半径，α_i为前轮转向角度。dt_ref越大，α_i的值越大，每个位姿间调整的角度越大，车辆平滑、准确完成转弯的概率越高。

设定以下条件：

式中的d为非直角转弯时的dt_ref大小，Δd为转弯时的变化量。通过上述条件的设定，能够保证车辆的转角和速度不会发生突变或者跳变，从而实现了稳定运行的目的。图3是本实施例的控制流程图。

实施例2

本发明的实施例2提出了一种无人驾驶车辆的局部路径规划系统，基于组合导航传感器实现，采用实施例1的方法实现，所述系统包括：运动模型建立模块、定位信息获取模块、代价函数建立模块和动态调节模块；其中，

所述运动模型建立模块，用于在世界坐标系中建立运动学方程，以车辆运动中的速度和转角为自变量，建立运动模型；

所述定位信息获取模块，用于基于实时载波相位动态差分技术，将组合导航传感器获取的数据转换至导航坐标系，为车辆运行提供定位信息；

所述代价函数建立模块，用于从时间和位姿两个方面，通过实施加权多目标优化的方法，建立多目标代价函数；

所述动态调节模块，用于通过确定车辆的运行状态，融合时间分辨率参数dt_ref进行动态调节，实现车辆稳定运行。

本申请的技术效果

本发明提出了一种改进型的时间弹性带方法，增加对时间分辨率参数(dt_ref)的动态和自适应调节方法，使得轨迹分辨率可以在直行和转弯处进行动态匹配。降低了因车辆动力学模型精度太低而出现左右摆动的问题，最大限度的保证车辆直行和转弯过程中的稳定性和可靠性。

本发明针对无人驾驶车辆，基于车载传感器，公开了一种局部路径规划方法，可以完成规划目标点平滑、顺利达到，同时能够有效改善车辆直行运行和转弯时的稳定性，避免出现多次倒车、大角度转动以及速度突变的情况。基于自动驾驶车辆的多次测试结果，发现时间弹性带算法虽然可以达到对导航路径的规划能力，但是存在规划结果不平滑、加速度以及速度突变等问题，直接导致车辆自动运行状态不合理。本发明通过对车辆运动学模型、动力学模型、路径规划以及导航控制算法的分析，对规划算法进行了优化和改进，增加了对车辆运行过程中角速度和加速度突变的抑制方法。同时对于转弯指令，由于组合导航传感器的辅助，能够得到车辆当前位置与弯道边界的距离信息，利用改进的规划算法，完成平滑、准确的拐弯效果。

通过本发明装置在无人驾驶车辆中的应用，能够有效改善自动运行车辆的稳定性和可靠性，拓展无人驾驶技术的应用范围。

最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (2)

1.一种无人驾驶车辆的局部路径规划方法，基于组合导航传感器实现，所述方法包括：

步骤1)在世界坐标系中建立运动学方程，以车辆运动中的速度和转角为自变量，建立运动模型；

步骤2)基于实时载波相位动态差分技术，将组合导航传感器获取的数据转换至导航坐标系，为车辆运行提供定位信息；

步骤3)从时间和位姿两个方面，通过实施加权多目标优化的方法，建立多目标代价函数；

步骤4)通过确定车辆的运行状态，融合时间分辨率参数dt_ref进行动态调节，实现车辆稳定运行；

所述步骤1)具体包括：

将世界坐标系OwXwYw的基坐标系设置在车辆简化的运行模型后轴中心上，设置O为运动模型的瞬时旋转中心；

建立自动驾驶车辆运动学状态方程为：

其中，s(t)＝[x(t),y(t),β(t)]^T为车辆的位姿状态量，

为车辆位姿状态的导数，x(t)和y(t)分别为车辆在世界坐标系的X轴和Y轴的位移量，β(t)为t时刻车辆质心处的转向角，v(t)为t时刻车辆后轴中心速度，θ(t)为t时刻车辆的横摆角，l_r为车辆后轮到车辆质心的距离；

所述步骤2)具体包括：

步骤2-1)将组合导航传感器获取的大地坐标系下的地理经度λ、地理纬度φ、大地高度H，转换至地理直角坐标系下的坐标值(x,y,z)；

其中，N为椭球面卯酉圈的曲率半径，满足下式：

e为椭球的第一偏心率，a、b分别为椭球的长、短半径，均为常数，W为第一辅助系数；

步骤2-2)选取P₀＝[x₀，y₀，z₀]^T作为导航坐标系的原点，P₀点表示车辆的初始位置在空间直角坐标系下的坐标，T表示转置，P＝[x,y,z]^T表示地理直角坐标系下的坐标值；[Δx，Δy，Δz]^T表示在空间直角坐标系下当前位置与初始位置之间的差值：

通过坐标旋转，计算点P＝[x,y,z]^T在以点P₀为坐标原点的导航坐标系下的位置(E,N,U)为：

为坐标转换矩阵，满足下式：

所述步骤3)具体包括：

从时间和位姿两个方面，通过实施加权多目标优化的方法，建立多目标代价函数为：

B^*＝argminf(B)

其中，B表示由姿序列信息和时间间隔序列信息的TEB轨迹模型；f(B)表示各种约束目标函数之和，γ_k为第k个目标函数f_k(B)的权重系数，1≤k≤K，B^*为被优化的TEB序列结果；所述目标函数包括用惩罚函数表示的速度和加速度约束以及与轨迹有关的目标函数，所述TEB序列包括位姿序列信息和时间间隔序列信息；

所述步骤4)的确定车辆的运行状态，具体包括：

根据设定时间内的转向角度，确定车辆为直行状态或转弯状态；

根据下式确定时间分辨率参数dt_ref：

其中，d为预先指定的时间间隔，Δd为转弯时的变化量；

所述步骤4)的时间分辨率参数dt_ref动态调节方法具体包括：

在初始化阶段，根据A-Star算法生成的全局路径，生成TEB序列；

在轨迹修正阶段，根据下式计算两个相邻位姿之间的时间间隔dt：

其中，p_i和p_i-1分别为两个相邻的位姿点，max_vel_x为车辆最大速度；

设定dt_ref为的两个点之间的指定时间间隔，dt_hysteresis为滞后时间，dt_hysteresis∈(-0.1*dt_ref，0.1*dt_ref)；

当dt>dt_ref+dt_hysteresis，在P_i-1点和P_i点的中间位置插入点P_i，P_i点变为P_i+1点，P_i-1点和P_i点之间的时间间隔为原来P_i-1点和P_i点之间的时间间隔的一半；

当dt<dt_ref+dt_hysteresis时，删除P_i点，P_i+1点成为新的P_i点，P_i-1点和P_i点之间的时间间隔为原来P_i-1点至P_i+1点时间间隔之和；

在轨迹修正阶段，重复进行增加和删除点的操作，直至整个TEB序列任意两个点之间的时间间隔都在指定的dt_ref范围内或达到最大迭代次数，从而得到动态调整好的局部路径。

2.一种无人驾驶车辆的局部路径规划系统，基于组合导航传感器实现，其特征在于，所述系统包括：运动模型建立模块、定位信息获取模块、代价函数建立模块和动态调节模块；其中，

所述运动模型建立模块，用于在世界坐标系中建立运动学方程，以车辆运动中的速度和转角为自变量，建立运动模型；

所述定位信息获取模块，用于基于实时载波相位动态差分技术，将组合导航传感器获取的数据转换至导航坐标系，为车辆运行提供定位信息；

所述代价函数建立模块，用于从时间和位姿两个方面，通过实施加权多目标优化的方法，建立多目标代价函数；

所述动态调节模块，用于通过确定车辆的运行状态，融合时间分辨率参数dt_ref进行动态调节，实现车辆稳定运行；

所述运动模型建立模块的处理过程具体包括：

将世界坐标系OwXwYw的基坐标系设置在车辆简化的运行模型后轴中心上，设置O为运动模型的瞬时旋转中心；

建立自动驾驶车辆运动学状态方程为：

其中，s(t)＝[x(t),y(t),β(t)]^T为车辆的位姿状态量，

为车辆位姿状态的导数，x(t)和y(t)分别为车辆在世界坐标系的X轴和Y轴的位移量，β(t)为t时刻车辆质心处的转向角，v(t)为t时刻车辆后轴中心速度，θ(t)为t时刻车辆的横摆角，l_r为车辆后轮到车辆质心的距离；

所述定位信息获取模块的处理过程具体包括：

步骤2-1)将组合导航传感器获取的大地坐标系下的地理经度λ、地理纬度φ、大地高度H，转换至地理直角坐标系下的坐标值(x,y,z)；

其中，N为椭球面卯酉圈的曲率半径，满足下式：

e为椭球的第一偏心率，a、b分别为椭球的长、短半径，均为常数，W为第一辅助系数；

步骤2-2)选取P₀＝[x₀，y₀，z₀]^T作为导航坐标系的原点，P₀点表示车辆的初始位置在空间直角坐标系下的坐标，T表示转置，P＝[x,y,z]^T表示地理直角坐标系下的坐标值；[Δx，Δy，Δz]^T表示在空间直角坐标系下当前位置与初始位置之间的差值：

通过坐标旋转，计算点P＝[x,y,z]^T在以点P₀为坐标原点的导航坐标系下的位置(E,N,U)为：

为坐标转换矩阵，满足下式：

所述代价函数建立模块的处理过程具体包括：

从时间和位姿两个方面，通过实施加权多目标优化的方法，建立多目标代价函数为：

B^*＝argminf(B)

其中，B表示由姿序列信息和时间间隔序列信息的TEB轨迹模型；f(B)表示各种约束目标函数之和，γ_k为第k个目标函数f_k(B)的权重系数，1≤k≤K，B^*为被优化的TEB序列结果；所述目标函数包括用惩罚函数表示的速度和加速度约束以及与轨迹有关的目标函数，所述TEB序列包括位姿序列信息和时间间隔序列信息；

动态调节模块的处理过程具体包括：

根据设定时间内的转向角度，确定车辆为直行状态或转弯状态；

根据下式确定时间分辨率参数dt_ref：

其中，d为预先指定的时间间隔，Δd为转弯时的变化量；

在初始化阶段，根据A-Star算法生成的全局路径，生成TEB序列；

在轨迹修正阶段，根据下式计算两个相邻位姿之间的时间间隔dt：

其中，p_i和p_i-1分别为两个相邻的位姿点，max_vel_x为车辆最大速度；

设定dt_ref为的两个点之间的指定时间间隔，dt_hysteresis为滞后时间，dt_hysteresis∈(-0.1*dt_ref，0.1*dt_ref)；

当dt>dt_ref+dt_hysteresis，在P_i-1点和P_i点的中间位置插入点P_i，P_i点变为P_i+1点，P_i-1点和P_i点之间的时间间隔为原来P_i-1点和P_i点之间的时间间隔的一半；

当dt<dt_ref+dt_hysteresis时，删除P_i点，P_i+1点成为新的P_i点，P_i-1点和P_i点之间的时间间隔为原来P_i-1点至P_i+1点时间间隔之和；

在轨迹修正阶段，重复进行增加和删除点的操作，直至整个TEB序列任意两个点之间的时间间隔都在指定的dt_ref范围内或达到最大迭代次数，从而得到动态调整好的局部路径。

Images