用于车辆状态跟踪过程中的时间对齐方法与流程

1.本发明涉及智能驾驶，具体涉及用于车辆状态跟踪过程中的时间对齐方法。


背景技术：

2.自动驾驶和高级辅助驾驶已经变成汽车智能化的重要发展方向。其中，车载雷达凭借其全天时、全天候、测速精度高以及实时性强等优点而被广泛应用于自动驾驶的感知系统中。然而在车辆自动驾驶的过程中，车载雷达会受到周围环境以及传感器自身的限制，其对车身周围车辆运动状态的检测结果往往带有一定误差，所以在实际的自动驾驶系统中，通常会将周围车辆上一时刻(k
‑
1时刻)的状态跟踪结果通过时间对齐的方法同步到当前时刻(k时刻)，并与当前时刻波雷达实时的检测结果进行状态融合，作为当前时刻的状态跟踪结果，如此迭代重复，实现自动驾驶车辆所在环境中周围车辆行驶状态的稳定跟踪。
3.目前常用的状态融合方法绝大多数是卡尔曼滤波，或者其扩展形式，如扩展卡尔曼滤波，无迹卡尔曼滤波或粒子滤波等。然而无论是哪种滤波方法，都要求k
‑
1时刻的跟踪状态在时间对齐到k时刻后的结果足够准确，而且能够减少时间对齐过程中自动驾驶车辆自身强机动性运动(如加减速或急转弯)的影响。以往的时间对齐方法中，通常将周围车辆的相对位置和相对速度作为状态量并采用cv(constantvelocity，恒速度运动)模型做时间对齐，即假定周围车辆的以恒定的相对速度运动，将k
‑
1时刻周围车辆的跟踪状态预测到k时刻，然而这种方式会导致对齐结果受主车的机动影响较大。故以往的跟踪过程中，自动驾驶车辆需保持足够小的机动性，如避免剧烈转弯或加减速，以达到较好的自动驾驶车辆所在环境中对于周围车辆的跟踪效果。


技术实现要素：

4.本发明要解决的技术问题是：车辆跟踪过程中，将自动驾驶车辆(下文称为自车)所在环境中的周围车辆(下文称为目标车辆)在k
‑
1时刻的状态跟踪结果时间对齐到车载雷达检测的当前时刻(k时刻)，通过解耦目标车辆与自车的运动，克服该过程中自车本身的强机动性对跟踪状态量时间对齐结果的影响，进而提高目标车辆跟踪的精度、灵敏度和安全性。
5.为了解决上述技术问题，本发明的技术方案是提供了以下技术方案：
6.1、用于车辆状态跟踪过程中的时间对齐方法，突出的实质性特点在于：
7.以自车周围的多个或单个车辆作为目标车辆；
8.具体步骤如下：
9.步骤a1、包含子步骤a1.0、子步骤a1.1和子步骤a1.2：
10.子步骤a1.0、获取上一时刻k
‑
1目标车辆的跟踪状态；
11.子步骤a1.1、将子步骤a1.0获得的跟踪状态所包含的相对速度转换为绝对速度，获得目标车辆的中间状态；
12.子步骤a1.2、以a1.1获得的中间状态作为全部参数或部分参数进行预测，获得当
前时刻k目标车辆的预测状态；
13.步骤a2、计算自车从上一时刻k
‑
1到当前时刻k(含上一时刻k
‑
1和当前时刻k)，这段时间内的自车运动轨迹；
14.步骤a3、通过坐标原点的平移和坐标系的旋转，将步骤a2获得的自车运动轨迹和步骤a1.2获得的目标车辆的预测状态，重新结合到当前时刻的坐标系下，完成时间对齐的过程。
15.2、用于车辆状态跟踪过程中的时间对齐方法，突出的实质性特点在于：
16.以自车周围的多个或单个车辆作为目标车辆；
17.具体步骤如下：
18.步骤a1、计算自车从上一时刻k
‑
1到当前时刻k(含上一时刻k
‑
1和当前时刻k)，这段时间内的自车运动轨迹；
19.步骤a2、包含子步骤a2.0、子步骤a2.1和子步骤a2.2：
20.子步骤a2.0、获取上一时刻k
‑
1目标车辆的跟踪状态；
21.子步骤a2.1、将子步骤a2.0获得的目标车辆的跟踪状态所包含的相对速度转换为绝对速度，获得目标车辆的中间状态；
22.子步骤a2.2、以a2.1获得的目标车辆的中间状态作为全部参数或部分参数进行预测操作，获得当前时刻k目标车辆的预测状态；
23.步骤a3、通过坐标原点的平移和坐标系的旋转，将步骤a1获得的自车运动轨迹和步骤a2.2获得的目标车辆的预测状态，重新结合到当前时刻的坐标系下，完成时间对齐的过程。
24.3、如方案1或2所述的用于车辆状态跟踪过程中的时间对齐方法，突出的实质性特点在于：所述的平移的具体的量从自车运动轨迹中获取；所述的旋转的具体的量从自车运动轨迹中获取。
25.4、如方案1或2所述的用于车辆状态跟踪过程中的时间对齐方法，突出的实质性特点在于：步骤a1和步骤a2，并行地进行。
26.5、如方案1或2所述的用于车辆状态跟踪过程中的时间对齐方法，突出的实质性特点在于：
27.为方便表达定义一个坐标系，预先定义自车的一个点为坐标系的原点，坐标系具有x、y两个维度；
28.所述的
‘
目标车辆的跟踪状态所包含的相对速度转换为绝对速度’的具体转换方法如下式(1)～(2)，
[0029][0030][0031]
式中，p
x
表示目标车辆在x方向上的相对于自车的相对位置，p
y
表目标车辆在示y方向上的的相对于自车的相对位置；
[0032]
v
x
表示目标车辆在x方向上的绝对速度、v
y
表示目标车辆在y方向上的绝对速度；
[0033]
表示目标车辆在x方向上的相对于自车的相对速度，表示目标车辆在y方向上的相对于自车的相对速度，yawrate表示自车的偏航率。
[0034]
6、如方案1或2所述的用于车辆状态跟踪过程中的时间对齐方法，突出的实质性特点在于：目标车辆的预测状态的预测的方法，具体为：根据车辆的运动学模型，根据目标车辆的跟踪状态以及协方差矩阵预测得到当前时刻k时的目标车辆的预测状态。
[0035]
7、如方案1或2所述的用于车辆状态跟踪过程中的时间对齐方法，突出的实质性特点在于：自车运动轨迹的获取的方法，具体为：从自动驾驶车辆所配备的轮速传感器以及陀螺仪设备上获取上一时刻k
‑
1与当前时刻k之间的自车的运动信息和自车的姿态信息；根据所获取的自车的运动信息和自车的姿态信息计算时刻k
‑
1与时刻k之间自车运动轨迹。
[0036]
8、如方案7所述的用于车辆状态跟踪过程中的时间对齐方法，突出的实质性特点在于：自车的运动信息包含自车的绝对速度；自车的姿态信息包含自车的车辆偏航率。
[0037]
9、如方案1或2所述的用于车辆状态跟踪过程中的时间对齐方法，突出的实质性特点在于：目标车辆的跟踪状态包括目标车辆相对于自车的相对位置，目标车辆的跟踪状态包括目标车辆的绝对速度；目标车辆的预测状态包括目标车辆相对于自车的相对位置，目标车辆的预测状态包括目标车辆的绝对速度。
[0038]
10、如方案1或2所述的用于车辆状态跟踪过程中的时间对齐方法，突出的实质性特点在于：用于车载毫米波雷达车辆状态跟踪。
[0039]
10、基于方案1的技术思想，用于车辆状态跟踪过程中的时间对齐方法，突出的实质性特点在于，包括以下步骤：
[0040]
步骤1、根据车辆的运动学模型，将自动驾驶车辆(下文称为自车)所在环境中的周围车辆(下文称为目标车辆)上一时刻(下文称k
‑
1时刻)的跟踪状态(包括相对位置p和绝对速度v)以及协方差矩阵预测到当前时刻(下文称k时刻)，使用绝对速度可以不受主车运动的影响，以实现目标车辆的状态的独立预测。
[0041]
步骤2、从自动驾驶车辆所配备的轮速传感器以及陀螺仪设备上获取时刻k
‑
1与时刻k之间的自车的运动信息(v，车速)和姿态信息(yawrate，车辆偏航率)。
[0042]
步骤3、根据所获取的自车信息计算时刻k
‑
1与时刻k之间自车的运动轨迹，轨迹包括自车相对于k
‑
1时刻驶过的横纵向位移与偏航角。
[0043]
步骤4、将步骤1中得到的目标车辆的预测状态经过坐标原点的平移和坐标系的旋转，转换到k时刻自车所在的坐标系中，平移和旋转的量由步骤3中计算所得的自车运动轨迹获取。计算结果即为k
‑
1时刻目标车辆的跟踪状态时间对齐到k时刻自车所在的新的坐标系的结果。值得注意的是，自动驾驶车辆实际行驶的过程中，周围的车辆通常不止一辆，但本发明所提到的方法可以通用的适用到所有的目标车辆中。
[0044]
由于采用了上述的技术方案，与现有技术相比，本发明具有以下的优点和积极效果：
[0045]
其一，在时间对齐的过程中，与传统的基于相对速度时间对齐方法不同，提供了新的技术思路。
[0046]
其二，本发明中目标车辆所有的速度维度信息均为绝对速度，并分别单独计算目标车辆和自车在k
‑
1时刻与k时刻之间的运动轨迹，实现了目标车辆和自车运动状态的解耦，设计巧妙。
[0047]
其三，本发明无需升级硬件即可提高精度和灵敏度，应用成本低廉，容易推广。
[0048]
其后，通过坐标原点的平移和坐标系的旋转将两组独立的运动状态再次结合，实
现了时间对齐过程中不受自动驾驶车辆自身机动性影响的方法，能够极大的提升自动驾驶系统对目标车辆的感知精度和灵敏度，也更加安全，能够适应各种雷达，适应性高于现有技术。
[0049]
根据上述内容，本发明相对现有技术具有设计巧妙，感知精度高，灵敏度高，成本低廉，容易推广，适应性高，更安全等多个有益效果。
[0050]
绝对速度是指相对于地面或地面静止物体的相对速度。
附图说明
[0051]
图1是本发明实施例1的时间对齐方法的流程图；
[0052]
图2是本发明实施例1的雷达的安装位置以及目标车辆k
‑
1时刻跟踪状态示意图；
[0053]
图3是本发明实施例1的目标车辆状态预测的示意图；
[0054]
图4是本发明实施例1的自车运动轨迹的示意图(图中仅展示了自车转弯的情况，直线行驶类似)。
[0055]
图5是本发明实施例1的将目标车辆状态预测值转换到k时刻新的自车坐标系的示意图。
[0056]
图6是本发明实施例2的流程示意图。
具体实施方式
[0057]
下面结合具体实施例，进一步阐述本发明。应理解，这些实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明讲授的内容之后，本领域技术人员可以对本发明作各种改动或修改，这些等价形式同样落于本技术所附方案书所限定的范围。
[0058]
实施例1：
[0059]
如图1至图5所示，用于车辆状态跟踪过程中的时间对齐方法。
[0060]
图1所示为按照本发明实施例的车辆跟踪过程中时间对齐方法的流程图。
[0061]
图2描述了车载雷达的安装示意图以及k
‑
1时刻雷达对周围目标车辆的跟踪结果，其中以自车质心作为笛卡尔坐标系的坐标原点o，车辆质心的正前方垂直方向为笛卡尔坐标系的x轴，车辆质心左侧水平方向定义为笛卡尔坐标系的y轴。车载雷达与自车刚性连接并安装在车辆正前方，安装高度在60～80cm，以尽可能地减少来自高处或地面的噪声干扰，本发明对自车的车型没有具体要求，但通常配备自动驾驶功能。雷达通过向周围发射电磁波来检测周围目标车辆的状态信息，包括位置p和速度v等，但由于受周围环境以及自身测量精度的影响，其检测的结果通常带有很大的误差，通常情况下我们会通过跟踪方法实时记录目标车辆k
‑
1时刻的状态量，并做时间同步到k时刻，与k时刻雷达实时的检测结果做状态融合，并作为k时刻目标车辆的状态跟踪结果，如此迭代重复，实现对于目标车辆行驶状态的稳定跟踪。
[0062]
图3所示为根据k
‑
1时刻目标车辆的跟踪状态进行状态预测的示意图，其中状态预测的过程中使用cv模型，即假设目标车辆按照恒定速度运动到k时刻。
[0063]
图4所示为自车在k
‑
1时刻与k时刻之间的运动轨迹示意图，自车相对于k
‑
1时刻会发生运动与姿态两个维度的变化，其中运动变化指自车相对于k
‑
1时刻所发生的横纵向位
置变化；姿态变化指自车相对于k
‑
1时刻所发生的航向角的变化。
[0064]
图5所示为时间同步最后的过程，将图3与图4所示的两个独立运动的车辆通过坐标原点的平移与坐标系的旋转重新结合在一起，完成整个时间对齐过程。
[0065]
该过程实现了目标车辆和自车运动的解耦，将目标车辆相对自车的运动的过程分解为目标车辆和主车分别运动两个独立的过程，并通过坐标原点的平移和坐标系的旋转将二者在k时刻重新结合在一起。可以减少传统方法中将目标车辆的相对状态作为状态量在时间对齐的过程中引入的自车强机动性运动带来的误差。
[0066]
如图1所示，本发明提供的车辆跟踪过程中时间对齐方法包括以下步骤：
[0067]
首先，在步骤1)中，由跟踪的迭代结果，可以得到目标车辆在k
‑
1时刻的跟踪状态，如图2所示。根据车辆的运动学模型，将目标车辆在k
‑
1时刻的跟踪状态(包括相对位置p和绝对速度v)以及协方差矩阵预测到k时刻，与以往大多数时间对齐过程中使用相对位置和相对速度作为状态量不同，绝对速度在预测过程中仅代表了目标车辆的实际运动，不受主车所做的任何机动行为影响，是实现高精度时间对齐方法的必要步骤，目标车辆相对速度与绝对速度的转换方法如下式(1)～(2)，
[0068][0069][0070]
式中，p
x
、p
y
、v
x
、v
y
表示目标车辆的x方向和y方向的相对位置和绝对速度，表示目标车辆x方向和y方向的相对速度，yawrate表示自车的偏航率。相对现有技术本发明的
‘
目标车辆相对速度与绝对速度的转换方法’所获得的结果更加精确可靠。
[0071]
目标车辆运动状态预测方法过程如下式(3)～(6)；
[0072][0073][0074]
x＝f*x#(5)
[0075]
p＝f*p*f
′
q#(6)
[0076]
式中，f矩阵表示预测过程的状态转移矩阵，代表cv(恒速度运动)模型；dt表示k
‑
1时刻和k时刻的时间差；x向量表示目标车辆的跟踪状态，包括目标车辆的横纵向相对位置p
x
、p
y
和绝对速度v
x
、v
y
；p矩阵表示目标车辆跟踪状态所对应的协方差矩阵，或跟踪状态的不确定性矩阵，不确定性矩阵越大，表示跟踪状态的不确定性越高；q矩阵表示预测过程中的不确定性扰动，与预测时间间隔dt和目标车辆的机动性强弱有关。相对现有技术本发明的
‘
目标车辆运动状态预测方法’所获得的结果更加精确可靠。
[0077]
预测前后目标车辆的跟踪状态对比如示意图3所示，根据cv模型的原理可知，目标
车辆会沿着绝对速度的方向继续运动v*dt的距离，绝对速度v的大小和方向保持不变。
[0078]
在步骤2)中，从自车所配备的轮速传感器以及陀螺仪设备上获取时刻k
‑
1与时刻k之间的自车的运动信息(车速v)和姿态信息(车辆偏航率yawrate)。
[0079]
在步骤3)中，根据所获取的自车信息计算时刻k
‑
1与时刻k之间自车的运动轨迹，轨迹包括相对于k
‑
1时刻自车的横纵向行驶距离以及偏航角，其中自车直行或转弯时的横纵向距离以及偏航角的计算方法分别如下式(7)～(9)和(10)～(13)所示：
[0080]
d
x
＝v
x
*dt 0.5*a
x
*dt*dt#(7)
[0081]
d
y
＝0#(8)
[0082]
d
yaw
＝0#(9)
[0083]
r＝v/yawrate#(10)
[0084]
d
x
＝r*sin(yawrate*dt)#(11)
[0085]
d
y
＝r*(1
‑
cos(yawrate*dt))#(12)
[0086]
d
yaw
＝yawrate*dt#(13)
[0087]
式中，d
x
表示自车在k
‑
1时刻到k时刻运动的纵向距离，或x轴方向距离，x轴方向为自车的前后方向；d
y
表示自动驾驶车辆在k
‑
1时刻到k时刻运动的横向距离，或y轴方向距离，y轴方向为自车的左右方向；d
yaw
表示自车在k
‑
i时刻到k时刻沿x轴方向的偏航角；r表示自车k
‑
1时刻的转弯半径；v和yawrate分别代表自车k
‑
1时刻的车速和偏航率，可从车身传感器直接获取。相对现有技术本发明的
‘
自车直行或转弯时的横纵向距离以及偏航角的计算方法’所获得的结果更加精确可靠。
[0088]
自车运动轨迹如示意图4所示，图中仅展示了自车做转向运动的情况，自车直线运动的情况同理。转向运动的过程中，自车相对于k
‑
1时刻在横向以及纵向均发生了相对位移，且转过了yawrate*dt大小的航向角，自车运动后的位置与姿态即为k时刻以自车质心为原点的新的坐标系。
[0089]
在步骤4)中，将步骤1中得到的k
‑
1时刻目标车辆的预测状态经过坐标原点的平移和坐标系的旋转，转换到k时刻自车所在的坐标系中，平移和旋转的量由步骤3计算得到的自车运动轨迹获取，其中坐标原点的平移和坐标系的旋转方法的过程具体如下式(14)～(16)：
[0090][0091][0092][0093]
式中，p
′
x
、p
′
y
、v
′
x
、v
′
y
代表目标车辆跟踪状态的预测值，由步骤1计算得到；d
x
、d
y
、d
yaw
为自车的运动轨迹，包括横纵向运动距离和偏航角，由步骤2和步骤3计算得到；p
x
、p
y
、v
x
、v
y
代表经过时间对齐到当前时刻(k时刻)的目标车辆的跟踪状态。相对现有技术本发明的
‘
坐标原点的平移和坐标系的旋转方法’所获得的结果更加精确可靠。
[0094]
坐标转换的过程如示意图5所示，该过程将两组独立运动的目标车辆与自车重新
结合到k时刻以自车质心为原点的新的坐标系中，以实现将k
‑
1时刻的目标车辆跟踪状态时间对齐到k时刻的目的。
[0095]
至此，k
‑
1时刻的运动状态已经时间对齐到k时刻。本发明所提出的方法舍弃了传统的基于相对位置和相对速度的cv模型，改用目标车辆的相对位置和绝对速度作为状态量，首先计算目标车辆的运动轨迹，并单独计算自车在该时间段内的运动轨迹，实现了时间对齐过程中的目标车辆和自车的状态解耦；其后，通过坐标原点的平移和坐标系的旋转将两组独立的运动状态再次结合，实现了时间对齐过程中不受自动驾驶车辆自身机动性影响的方法，能够极大的提升跟踪状态经过时间对齐后的精度，进而提升自动驾驶系统对目标车辆实时状态的感知精度。
[0096]
实施例2：
[0097]
如图6、用于车辆状态跟踪过程中的时间对齐方法，突出的实质性特点在于：
[0098]
以自车周围的多个或单个车辆作为目标车辆；
[0099]
具体步骤如下：
[0100]
步骤a1、包含子步骤a1.0、子步骤a1.1和子步骤a1.2：
[0101]
子步骤a1.0、获取上一时刻k
‑
1目标车辆的跟踪状态；
[0102]
子步骤a1.1、将子步骤a1.0获得的跟踪状态所包含的相对速度转换为绝对速度，获得目标车辆的中间状态；
[0103]
子步骤a1.2、以a1.1获得的中间状态作为全部参数或部分参数进行预测，获得当前时刻k目标车辆的预测状态；
[0104]
步骤a2、计算自车从上一时刻k
‑
1到当前时刻k(含上一时刻k
‑
1和当前时刻k)，这段时间内的自车运动轨迹；
[0105]
步骤a3、通过坐标原点的平移和坐标系的旋转，将步骤a2获得的自车运动轨迹和步骤a1.2获得的目标车辆的预测状态，重新结合到当前时刻的坐标系下，完成时间对齐的过程。
[0106]
实施例3：用于车辆状态跟踪过程中的时间对齐方法，其特征在于：
[0107]
以自车周围的多个或单个车辆作为目标车辆；
[0108]
具体步骤如下：
[0109]
步骤a1、计算自车从上一时刻k
‑
1到当前时刻k(含上一时刻k
‑
1和当前时刻k)，这段时间内的自车运动轨迹；
[0110]
步骤a2、包含子步骤a2.0、子步骤a2.1和子步骤a2.2：
[0111]
子步骤a2.0、获取上一时刻k
‑
1目标车辆的跟踪状态；
[0112]
子步骤a2.1、将子步骤a2.0获得的目标车辆的跟踪状态所包含的相对速度转换为绝对速度，获得目标车辆的中间状态；
[0113]
子步骤a2.2、以a2.1获得的目标车辆的中间状态作为全部参数或部分参数进行预测操作，获得当前时刻k目标车辆的预测状态；
[0114]
步骤a3、通过坐标原点的平移和坐标系的旋转，将步骤a1获得的自车运动轨迹和步骤a2.2获得的目标车辆的预测状态，重新结合到当前时刻的坐标系下，完成时间对齐的过程。
[0115]
与实施例2不同之处是，本实施例调节了执行步骤的时序。
[0116]
其他说明：本技术的公式编号位于公式尾部。本发明的方法可以通过设计专用硬件来实现，也可以利用通用芯片(比如单片微控制器)结合软件来实现。