一种自动驾驶车辆在规划轨迹中的相对定位方法技术

一种新的车辆在规划轨迹中的相对定位方法。该方法通过“平面坐标(x,y)+行驶距离s+时间t”方式定义的“时空轨迹离散点”为规划轨迹输入，以车辆绝对位置坐标P(x,y)和绝对时刻t为当前状态输入，通过一系列数学方法和运算步骤，输出车辆在规划轨迹中的“时间最近参考点”和“空间最近参考点”作为相对定位结果。相对定位模块输出的“时间最近参考点”和“空间最近参考点”具有时空连续性和光滑性，为后续自动驾驶控制执行模块提供高品质的相对定位输入。驶控制执行模块提供高品质的相对定位输入。驶控制执行模块提供高品质的相对定位输入。

【技术实现步骤摘要】
一种自动驾驶车辆在规划轨迹中的相对定位方法


[0001]本专利技术属于自动定位
，具体涉及一种自动驾驶车辆在规划轨迹中的相对定位方法。

技术介绍

[0002]自动驾驶定位技术可以分为绝对定位和相对定位两种，绝对定位通过融合卫星定位、惯导积分和感知定位等技术对车辆在全局坐标系中的位置进行观测或估计，相对定位技术主要是基于绝对定位的结果，计算车辆在局部规划路径中的相对位置，为控制模块提供参考点输入，包括时间最近参考点和空间最近参考点等。自动驾驶在局部规划路径中的相对定位是自动驾驶控制算法实现的重要环节之一。本专利技术聚焦于自动驾驶中的相对定位技术，提出一种自动驾驶车辆在规划轨迹中的相对定位方法及装置。现有的自动驾驶规划轨迹相对定位流程如图1所示。
[0003]具体执行步骤可分解如下：
[0004]步骤1：
[0005]相当定位系统接受以“时空轨迹离散点”方式描述的规划轨迹、车辆绝对位置坐标和当前绝对时刻作为输入。其中，“时空轨迹离散点”为一组按等时间间隔或等空间间隔给出离散轨迹点序列，每个离散点至少包括二维平面坐标值(x,y)、时间坐标t和距离坐标s四维信息，另外也可能伴随着给出对应的航向角h、曲率c、曲率相对距离的变化率dcds、速度v、加速度a等附加信息。
[0006]步骤2：
[0007]采用二分法查询当前绝对时刻t在“时空轨迹离散点”中的对应的时间区间t，在该时间区间的前后两点之间，采用关于时间的线性内插算法，求解二维平面坐标值、距离坐标等作为当前车辆在规划轨迹中的“时间最近参考点”信息。
[0008]步骤3：
[0009]从前往后遍历所有“时空轨迹离散点”，求解离车辆当前实际绝对定位点P最近的参考轨迹点O，然后取O点的前一参考离散点A和后一参考离散点B，组成折线段AOB，该折线段将作为后续“空间最近参考点”搜索区间，图2展示了车辆位置P与折线段AOB间的6种典型相对位置关系。
[0010]步骤4：
[0011]分别求解车辆当前绝对位置P与线段AO和线段OB之间的最近点C和D，其中C在线段AO上，D在线段OB上，然后比较距离PC和PD，若PC<PD,则取C点为“空间最近参考点”Q，并以线段AO为线性内插区间求解Q点对应的距离坐标和时间坐标，否则，取D点为“空间最近参考点”Q，并以线段OB为线性内插区间求解Q点对应的距离坐标和时间坐标。从图2可以看出，P在两条线段上的最近点C或D在很多情况下会与端点O重合。
[0012]最终，相对定位装置将计算得到的“时间最近参考点”和“空间定位参考点”输出给控制模块，控制模块结合“时空轨迹离散点”代表的参考轨迹以及算法预设的预瞄时间和预
瞄距离参数，分别对车辆纵向和横向运动实现跟踪控制。
[0013]然而现有技术存在以下问题：
[0014](1)现有方法的“时间最近参考点”与“空间最近参考点”的求解过程相互独立，采用遍历搜索方法得到的全局“空间最近参考点”可能由于参考路径的非凸问题发生跳变，与“时间最近参考点”相离甚远，脱离物理实际。
[0015](2)现有方法通过求解车辆与折线段的最近点来确定最终的“空间最近参考点”，这种方法可能存在如图3所示的奇异现象，当车辆绝对定位点P位于折线段内侧的角平分线上时，P点到两个线段的距离相等，即PC＝PD，此时的“空间最近参考点”在C或D之间难以取舍，当P点在角平分线附近运动，会导致“空间最近参考点”在两个线段上来回跳变，不连续的“空间最近参考点”传递到控制模块，可能引起车辆运动响应的抖动。
[0016](3)现有方法求解“空间最近参考点”，可能存在P点在一定范围内维持运动，而所求得到的“空间最近参考点”维持不变的情况，如图4所示，当P在阴影区域任意运动时，所求“空间最近参考点”始终位于O点不变，在车辆运动过程中，将这种停滞的“空间最近参考点”传递给控制模块，也可能引起不平滑的车辆运动响应。
[0017](4)现有方法将“时空轨迹离散点”之间通过直线段顺次连接构成参考空间轨线，导致相邻线段之间的航向角发生突变，且直线段无法反映规划轨迹的曲率信息，不光滑的参考空间轨线必然会导致控制执行响应的抖动，具体表现为方向盘转角和加速度响应的高频振荡，严重影响整车舒适性体验。

技术实现思路

[0018]本专利技术所要解决的技术问题是克服现有技术的不足，提供一种自动驾驶车辆在规划轨迹中的相对定位方法及装置，通过“平面坐标(x,y)+行驶距离s+时间t”方式定义的“时空轨迹离散点”为规划轨迹输入，以车辆绝对位置坐标P(x,y)和绝对时刻t为当前状态输入，再通过一系列数学方法和运算步骤，输出车辆在规划轨迹中的“时间最近参考点”和“空间最近参考点”作为相对定位结果。
[0019]本专利技术提供一种自动驾驶车辆在规划轨迹中的相对定位方法，包括如下步骤，
[0020]步骤S1.相对定位系统接受以“时空轨迹离散点”方式描述的规划轨迹、车辆绝对位置坐标(x
p
,y
p
)和当前绝对时刻t
p
作为输入。其中，“时空轨迹离散点”为一组按等时间间隔或等空间间隔给出的离散轨迹点序列；
[0021]步骤S2.基于三次样条插值算法，构建“时空轨迹离散点”中距离坐标s关于时间坐标t之间的分段三次多项式函数f
s
(t)；
[0022]步骤S3.基于三次样条插值算法，构建“时空轨迹离散点”中平面直角坐标(x,y)关于距离坐标s之间的分段三次多项式函数f
x
(t),f
y
(t)；
[0023]步骤S4.采用二分索引法查询当前绝对时刻t
p
在“时空轨迹离散点”中的对应的时间区间t
m
≤t
p
＜t
m+1
，当前车辆在规划轨迹中的“时间最近参考点”被认为在第m和第m+1个“时空轨迹离散点”之间；
[0024]步骤S5.根据搜索得到的“时间最近参考点”所在区间t
m
≤t
p
＜t
m+1
，结合时间域三次样条曲线f
s
(t)和空间域三次样条曲线f
x
(s),f
y
(s)，求解最终“时间最近参考点”的距离坐标s
t
和空间直角坐标(x
t
,y
t
)。
[0025]作为本专利技术的进一步技术方案，步骤S1中，相对定位系统接受每个离散点的四维信息，包括二维平面坐标值(x
i
,y
i
)、时间坐标t
i
和距离坐标s
i
，距离坐标s
i
通过空间曲线积分获取，根据两点之间直线距离最短原则，相邻离散点间的距离坐标间隔不小于相邻空间点之间的欧式距离。
[0026]进一步的，步骤S2中，构建过程采用“非扭结”边界条件，前两个插值点本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种自动驾驶车辆在规划轨迹中的相对定位方法，其特征在于，包括如下步骤，步骤S1.相对定位系统接受以“时空轨迹离散点”方式描述的规划轨迹、车辆绝对位置坐标(x
p
,y
p
)和当前绝对时刻t
p
作为输入。其中，“时空轨迹离散点”为一组按等时间间隔或等空间间隔给出的离散轨迹点序列；步骤S2.基于三次样条插值算法，构建“时空轨迹离散点”中距离坐标s关于时间坐标t之间的分段三次多项式函数f
s
(t)；步骤S3.基于三次样条插值算法，构建“时空轨迹离散点”中平面直角坐标(x,y)关于距离坐标s之间的分段三次多项式函数f
x
(t),f
y
(t)；步骤S4.采用二分索引法查询当前绝对时刻t
p
在“时空轨迹离散点”中的对应的时间区间t
m
≤t
p
＜t
m+1
，当前车辆在规划轨迹中的“时间最近参考点”被认为在第m和第m+1个“时空轨迹离散点”之间；步骤S5.根据搜索得到的“时间最近参考点”所在区间t
m
≤t
p
＜t
m+1
，结合时间域三次样条曲线f
s
(t)和空间域三次样条曲线f
x
(s),f
y
(s)，求解最终“时间最近参考点”的距离坐标s
t
和空间直角坐标(x
t
,y
t
)。2.根据权利要求1所述的一种自动驾驶车辆在规划轨迹中的相对定位方法，其特征在于，所述步骤S1中，相对定位系统接受每个离散点的四维信息，包括二维平面坐标值(x
i
,y
i
)、时间坐标t
i
和距离坐标s
i
，距离坐标s
i
通过空间曲线积分获取，根据两点之间直线距离最短原则，相邻离散点间的距离坐标间隔不小于相邻空间点之间的欧式距离。3.根据权利要求1所述的一种自动驾驶车辆在规划轨迹中的相对定位方法，其特征在于，所述步骤S2中，构建过程采用“非扭结”边界条件，前两个插值点的三阶导数值相等，最后两个插值点的三阶导数值相等，三次样条曲线f
s
(t)的计算公式为其中，三次样条曲线f
s
(t)满足如下约束：
其中，4.根据权利要求1所述的一种自动驾驶车辆在规划轨迹中的相对定位方法，其特征在于，所述步骤S3中，三次样条曲线f
x
(s),f
y
(s)的公式为其中，三次样条曲线f
x
(s),f
y
(s)满足如下约束：其中，5.根据权利要求1所述的一种自动驾驶车辆在规划轨迹中的相对定位方法，其特征在于，所述步骤S5中，最终“时间最近参考点”的距离坐标s
t
和空间直角坐标(x
t
,y
t
)的计算公式为，6.根据权利要求1所述的一种自动驾驶车辆在规划轨迹中的相对定位方法，其特征在于，所述步骤S4中，搜索得到的“时间最近参考点”所在区间t
m
≤t
p
＜t
m+1
所对应的第m个“时空轨迹离散点”为起点，确定“空间最近参考点”对应的折线段搜索区间AOB的方法包括如下
步骤，步骤S41.先向前遍历后继离散点，逐个计算后继离散点与当前车辆绝对位置P之间的欧式距离，直至距离首次出现由小变大的情况为止，计前向后继局部最近离散...

【专利技术属性】
技术研发人员：刘伟，奚浩晨，杨俊，章楠，荀航，白炳仁，刘振斌，张显宏，
申请(专利权)人：上海友道智途科技有限公司，
类型：发明
国别省市：