CN112147651B - 一种异步多车协同目标状态鲁棒估计方法 - Google Patents

Abstract

本发明公开了一种异步多车协同目标状态鲁棒估计方法，通过主车和协同车分别获得同一辆目标车的观测数据和自身定位数据，主车接收协同车传输的数据，主车在当前感知时刻t_h获得目标车的观测数据并从缓存中取出离t_h时刻最近的t_c时刻的协同车发送的目标车观测数据及协同车的定位数据；将t_h时刻主车定位数据插值到t_c时刻，并计算t_c时刻协同车在主车坐标系中的坐标值应用卡尔曼滤波器预测t_c时刻协同车的状态和协方差矩阵目标车的状态和协方差矩阵根据观测数据利用期望最大化算法计算t_c时刻修正后的协同车和目标车的状态、t_c时刻主车坐标系与协同车坐标系之间的夹角及t_h时刻修正后的目标车的状态。

Description

一种异步多车协同目标状态鲁棒估计方法

技术领域

本发明属于智能车辆定位与目标跟踪技术领域，尤其涉及一种异步多车协同目标状态鲁棒估计方法。

背景技术

环境感知系统是智能汽车的重要组成部分，为了实现车辆自适应巡航、碰撞预警及车辆自动驾驶控制等功能，环境感知系统需要准确地检测并估计智能车辆周围环境中的各类目标的准确位置及运动状态信息。

目前主流的单车环境感知系统主要采用激光雷达、相机等传感器对车辆周围环境进行感知，但是激光雷达和相机传感器容易受目标间相互遮挡及障碍物遮挡的影响，同时对远距离目标车的状态估计准确度会下降。利用多传感器融合可以提升状态估计的准确性，然而会导致单车成本的迅速增加，为自动驾驶汽车的量产带来了巨大的挑战。

随着车联网技术的成熟与普及，越来越多的车辆通过车间通信技术能够在车辆之间完成信息的共享。基于车车通信，主车可以利用协同车辆发布的信息，在不增加自身传感器数量的情况下，提升目标车辆的状态估计精度。因此，可以减少单车传感器的数量，从而降低自动驾驶车辆的制造成本。然而，为利用协同车辆发布的信息，需要知道主车与协同车之间的相对位置。目前广泛使用的GPS定位系统容易受高楼、天气等因素的影响，存在定位精度不高、定位稳定性较差的问题。利用GPS定位系统计算的主车与协同车之间的相对位置不够准确，在定位信息不够准确的情况下融合协同车发布的信息，反而可能导致目标车状态估计精度的下降。同时，由于感知时刻的不同步与通信延迟的影响，主车与协同车对同一目标的感知数据存在异步特性，简单忽略异步性也会显著降低协同状态估计的效果。

发明内容

针对现有技术中的不足，本发明提出了一种异步多车协同目标状态鲁棒估计方法，能够解决当主车与协同车之间相对位置不够准确和感知数据不同步情况下的协同状态估计问题，通过应用期望最大化算法同时估计协同车与目标车的状态，将车车通信与车载传感器相融合，提升周边车辆状态估计的精度和可靠性。

本发明所采用的技术方案如下：

一种异步多车协同目标状态鲁棒估计方法，

步骤1，主车和协同车分别以一定的周期获得同一辆目标车的观测数据和自身定位数据；

步骤2，协同车将其测得的目标车的观测数据和协同车的自身定位数据发送给主车，主车将接收协同车传输的数据并存放在缓存中；

步骤3，主车在当前感知时刻t_h获得目标车的观测数据，并从缓存中取出离t_h时刻最近的t_c时刻的协同车发送的目标车观测数据/>及协同车的定位数据；

步骤4，将t_h时刻主车定位数据插值到t_c时刻，并计算t_c时刻协同车在主车坐标系中的坐标值

步骤5，应用卡尔曼滤波器预测t_c时刻协同车的状态和协方差矩阵/>、目标车的状态/>和协方差矩阵/>

步骤6，基于步骤5中t_c时刻协同车的状态和协方差矩阵/>、目标车的状态/>和协方差矩阵/>根据观测数据/>利用期望最大化算法计算t_c时刻修正后的协同车和目标车的状态、t_c时刻主车与协同车坐标系之间的夹角及t_h时刻修正后的目标车的状态。

进一步，观测数据包括目标车在主车坐标系中的位置和目标车在协同车坐标系中的位置，分别是主车和协同车利用各自的车载传感器获得的；定位数据为主车和协同车各自的GPS坐标。

进一步，所述步骤4包括：

4.1、将主车在t_h时刻的GPS坐标插值到t_c时刻，表示为：

其中，分别是协同车的经度、纬度和高度；/>分别是主车的经度、纬度和高度，Δlon和Δlat分别为纬度和经度插值的偏移量，表示为：

ΔT为主车感知时刻t_h与协同车感知时刻t_c之间的时间间隔，为主车速度，/>为主车航向角，l为同一经度下纬度增加一度的距离；

4.2、将协同车和主车的GPS坐标从WGS-84坐标系转换到地心地固坐标系，协同车GPS坐标的转换公式如下：

其中，为协同车在地球直角坐标系中的坐标，e为椭球偏心率，N为基准椭球体的曲率半径，表示为：

其中，a为基准椭球体的长半径，b为基准椭球体的短半径；

4.3、计算t_c时刻协同车在主车坐标系下的坐标值

进一步，所述步骤6包括：

6.1、设t_c时刻主车坐标系与协同车坐标系之间的初始夹角为r为迭代次数，初始化为1；

6.2、计算t_c时刻协同车的状态目标车的状态/>t_h时刻目标车的状态/>

H为量测方程，R₁为量测噪声协方差矩阵，Q为过程噪声协方差矩阵，为坐标旋转矩阵；

6.3、计算t_c时刻主车与协同车坐标系之间的夹角

其中，err(φ)为误差函数，表示为，

其中，R₁为量测噪声协方差矩阵，为目标车在协同车坐标系下的量测，b(φ)为估计的目标车在协同车坐标系下的量测，表示为：

其中，φ为主车与协同车坐标系之间的夹角，H为量测方程，分别为t_c时刻目标车的状态、协同车的状态；

6.4、若ε为阈值，则迭代停止；否则，令r自增1，转到6.2。

本发明的有益效果：

与现有技术相比，本发明能通过车车通信与车载传感器融合的方法对周边车辆的状态进行估计，尤其是当主车与协同车之间的相对位置不够准确和感知时刻不同步的情况下，通过期望最大化算法对主车和协同车的夹角进行估计，从而修正主车和协同车的相对位置，进而利用修正的相对位置进行滤波，改善对目标车的状态估计。

附图说明

图1为本发明的流程图示意图；

图2是本发明的协同车与目标车状态估计的概率图模型示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用于解释本发明，并不用于限定本发明。

下面将结合附图和具体的实施方式对本发明的技术方案做进一步详细解释，以车车通信与车载传感器融合的多车协同状态估计为例，但本发明保护范围不限于下述实施例。

步骤1，主车和协同车分别以一定的周期利用自身的车载传感器获得同一辆目标车的观测数据和自身定位数据；

步骤2，协同车利用车载定位设备获得自身定位数据，且协同车将其测得的目标车的观测数据和协同车的自身定位数据通过车车通信发送给主车，主车将接收的数据存放在缓存中；协同车发送给主车的目标车的观测数据具体是目标车在协同车坐标系中的位置，协同车的自身定位数据具体是协同车通过车载定位设备获得的GPS坐标

步骤3：主车在当前感知时刻t_h利用车载传感器获得目标车的观测数据并从缓存中取出离t_h时刻最近的t_c时刻(t_c＜t_h)的协同车发送的目标车观测数据/>及协同车的GPS坐标/> 分别是协同车的经度、纬度和高度；

步骤4：主车将t_h时刻根据其车载定位设备获得的自身定位数据插值到t_c时刻，并计算t_c时刻协同车在主车坐标系中的坐标值

4.1.由于主车与协同车感知时刻不同步，同时考虑到车车通信存在延迟，因此需要将主车在t_h时刻的GPS坐标插值到t_c时刻，具体公式如下：

其中，分别是主车的经度，纬度和高度，Δlon和Δlat分别为纬度和经度插值的偏移量，计算公式如下：

ΔT为主车感知时刻t_h与协同车感知时刻t_c之间的时间间隔，为主车速度，/>为主车航向角，/>为主车的纬度，l为同一经度下纬度增加一度的距离，取值为111.32千米。

4.2.将协同车和主车的GPS坐标从WGS-84坐标系(世界大地测量坐标系)转换到地心地固坐标系(ECEF)，协同车GPS坐标的转换公式如下：

其中，分别是协同车在地球直角坐标系中的坐标，e为椭球偏心率，N为基准椭球体的曲率半径，计算方法为：：

其中，a为基准椭球体的长半径，取a＝6378137.0m，b为基准椭球体的短半径，取b＝6356755.0m。对主车GPS坐标也进行同样转换，得到/> 是主车在地球直角坐标系中的坐标。

4.3、计算t_c时刻协同车在主车坐标系下的坐标值

其中，L为旋转方程，计算方法为：

步骤5：应用卡尔曼滤波器分别预测t_c时刻协同车的状态协方差矩阵/>和目标车的状态/>协方差矩阵/>

步骤6：根据观测数据利用期望最大化算法计算t_c时刻修正后的协同车和目标车的状态、t_c时刻主车与协同车坐标系之间的夹角及t_h时刻修正后的目标车的状态；具体过程为：

首先建立t_c时刻协同车的状态、目标车的状态/>t_h时刻目标车的状态/>与观测数据/>的概率图模型，如附图2所示；

根据条件独立性，的联合分布可表示为：

其中，分别为/>和/>的先验高斯分布，由步骤5得到；

均为似然函数，上述概率分布均为高斯分布，F为状态转移矩阵，Q为过程噪声协方差矩阵，H为量测矩阵，R₁为量测噪声协方差矩阵，/>为t_c时刻主车坐标系与协同车坐标系之间的夹角；

在本实施例中，协同车与目标车的状态包括x方向的位置和速度及y方向的位置和速度，状态转移矩阵量测矩阵/>

应用期望最大化算法，交替计算的后验分布及夹角/>；具体过程如下：

6.1.设t_c时刻主车坐标系与协同车坐标系之间的初始夹角为，r为迭代次数，初始化为1；在本实施例中，/>为前一次更新后的主车坐标系与协同车坐标系之间的夹角；

6.2.计算t_c时刻协同车的状态、目标车的状态/>、t_h时刻目标车的状态/>

H为量测方程，为坐标旋转矩阵，R₁为量测噪声协方差矩阵；

6.3.计算t_c时刻主车与协同车坐标系之间的夹角

其中，err(φ)为误差函数，表示为，

其中，R₁为量测噪声协方差矩阵，为目标车在协同车坐标系下的量测，b(φ)为估计的目标车在协同车坐标系下的量测，表示为：

其中，φ为主车与协同车坐标系之间的夹角，H为量测方程，分别为t_c时刻目标车的状态、协同车的状态；

6.4若ε为阈值，则迭代停止；否则，令r自增1，转到6.2。

以上实施例仅用于说明本发明的设计思想和特点，其目的在于使本领域内的技术人员能够了解本发明的内容并据以实施，本发明的保护范围不限于上述实施例。所以，凡依据本发明所揭示的原理、设计思路所作的等同变化或修饰，均在本发明的保护范围之内。

Claims (3)

1.一种异步多车协同目标状态鲁棒估计方法，其特征在于，包括以下步骤：

步骤1，主车和协同车分别以一定的周期获得同一辆目标车的观测数据和自身定位数据；所述观测数据包括目标车在主车坐标系中的位置和目标车在协同车坐标系中的位置，分别是主车和协同车利用各自的车载传感器获得的；定位数据为主车和协同车各自的GPS坐标；

步骤2，协同车将其测得的目标车的观测数据和自身定位数据发送给主车，主车接收协同车传输的数据并存放在缓存中；

步骤3，主车在当前感知时刻t_h获得目标车的观测数据并从缓存中取出离t_h时刻最近的t_c时刻的协同车发送的目标车观测数据/>及协同车的定位数据；

步骤4，将t_h时刻主车定位数据插值到t_c时刻，并计算t_c时刻协同车在主车坐标系中的坐标值

步骤5，应用卡尔曼滤波器预测t_c时刻协同车的状态和协方差矩阵/>目标车的状态/>和协方差矩阵/>

步骤6，基于步骤5中t_c时刻协同车的状态和协方差矩阵/>目标车的状态/>和协方差矩阵/>根据观测数据/>利用期望最大化算法计算t_c时刻修正后的协同车和目标车的状态、t_c时刻主车与协同车坐标系之间的夹角及t_h时刻修正后的目标车的状态。

2.根据权利要求1所述的一种异步多车协同目标状态鲁棒估计方法，其特征在于，所述步骤4包括：

4.1、将主车在t_h时刻的GPS坐标插值到t_c时刻，表示为：

其中，分别是协同车的经度、纬度和高度；/>分别是主车的经度、纬度和高度，Δlon和Δlat分别为纬度和经度插值的偏移量，表示为：

ΔT为主车感知时刻t_h与协同车感知时刻t_c之间的时间间隔，为主车速度，/>为主车航向角，l为同一经度下纬度增加一度的距离；

4.2、将协同车和主车的GPS坐标从WGS-84坐标系转换到地心地固坐标系，协同车GPS坐标的转换公式如下：

其中，为协同车在地球直角坐标系中的坐标，e为椭球偏心率，N为基准椭球体的曲率半径，表示为：

其中，a为基准椭球体的长半径，b为基准椭球体的短半径；

4.3、计算t_c时刻协同车在主车坐标系下的坐标值

3.根据权利要求1所述的一种异步多车协同目标状态鲁棒估计方法，其特征在于，所述步骤6包括：

6.1、设t_c时刻主车坐标系与协同车坐标系之间的初始夹角为r为迭代次数，初始化为1；

6.2、计算t_c时刻协同车的状态目标车的状态/>t_h时刻目标车的状态/>

H为量测方程，R₁为量测噪声协方差矩阵，Q为过程噪声协方差矩阵，为坐标旋转矩阵；

6.3、计算t_c时刻主车与协同车坐标系之间的夹角

其中，err(φ)为误差函数，表示为，

其中，R₁为量测噪声协方差矩阵，为目标车在协同车坐标系下的量测，b(φ)为估计的目标车在协同车坐标系下的量测，表示为：

其中，φ为主车与协同车坐标系之间的夹角，H为量测方程，分别为t_c时刻目标车的状态、协同车的状态；

6.4、若ε为阈值，则迭代停止；否则，令r自增1，转到6.2。