CN106767783A - 基于车载通信的定位修正方法和装置 - Google Patents

Abstract

本公开涉及一种基于车载通信的定位修正方法和装置，涉及定位技术领域，应用于目标车辆，该方法包括：获取至少两个邻居节点的相对距离、相对角度、通过导航获取的定位位置；邻居节点为与目标车辆的距离小于预设距离的其他车辆或者路边单元；根据至少两个邻居节点的相对距离、相对角度、定位位置利用三角测量法确定目标车辆的测量定位位置；通过惯性导航获取目标车辆在大地坐标系的X轴和Y轴的速度和加速度；以目标车辆的测量定位位置、目标车辆在大地坐标系的X轴和Y轴的速度作为卡尔曼滤波器的输入进行卡尔曼滤波；根据卡尔曼滤波的输出结果得到目标车辆的定位位置。本公开具有延长惯性导航的可信时间、提高定位精度的效果。

Description

基于车载通信的定位修正方法和装置

技术领域

本公开涉及定位技术领域，具体涉及一种基于车载通信的定位修正方法和装置。

背景技术

随着无线通信技术的发展，移动定位技术得到了广泛的应用，尤其是在车辆导航的领域。现有技术中，车辆利用GNSS(Global Navigation Satellite System，全球导航卫星系统)进行导航定位，但车辆在行驶过程中会通过多种地形，GNSS信号会受到信号衰减、多径干扰和障碍物等多种因素的影响，导致不能提供稳定、准确的定位信息。在极端情况下，当车辆进入隧道、涵洞等区域时，因为车辆对导航卫星不可见，不能接收GNSS信号，导致不能提供定位信息。此时只能依赖于惯性导航对车辆位置进行估计，但惯性导航的可信时间短，在单独运行超过30秒时已不具有可信度。

发明内容

本公开提供一种基于车载通信的定位修正方法和装置，用以解决车辆在行驶过程中由于导航卫星信号减弱或消失导致的定位精度低、惯性导航可信时间短的问题。

为了实现上述目的，根据本公开实施例的第一方面，提供一种基于车载通信的定位修正方法，应用于目标车辆，所述方法包括：

获取至少两个邻居节点的相对距离、相对角度、通过导航获取的定位位置；所述邻居节点为与所述目标车辆的距离小于预设距离的其他车辆或者路边单元；

根据所述至少两个邻居节点的相对距离、相对角度、定位位置利用三角测量法确定所述目标车辆的测量定位位置；

通过惯性导航获取所述目标车辆在大地坐标系的X轴和Y轴的速度和加速度；

以所述目标车辆的测量定位位置、所述目标车辆在大地坐标系的X轴和Y轴的速度作为卡尔曼滤波器的输入进行卡尔曼滤波；

根据所述卡尔曼滤波的输出结果得到所述目标车辆的定位位置。

可选的，在进行卡尔曼滤波之前，所述方法还包括：

确定所述卡尔曼滤波器的状态转移矩阵、单位观测矩阵以及所述卡尔曼滤波器的状态方程的噪声的协方差矩阵。

可选的，所述获取至少两个邻居节点的相对距离、相对角度、通过导航获取的定位位置，包括：

通过专用短程通信技术DSRC从所述至少两个邻居节点获取所述至少两个邻居节点的相对距离、相对角度、通过导航获取的定位位置；

其中，所述相对距离包括每个邻居节点与所述目标车辆的相对距离以及每个邻居节点之间的相对距离，所述相对角度包括任意两个邻居节点与所述目标车辆所构成的三角形中以邻居节点为顶点的角的角度。

可选的，所述确定卡尔曼滤波器的状态转移矩阵、单位观测矩阵以及所述卡尔曼滤波器的状态方程的噪声的协方差矩阵，包括：

根据预先设置的单位测量时间确定所述状态转移矩阵和所述单位观测矩阵；

根据所述单位观测矩阵以及所述目标车辆在大地坐标系的X轴和Y轴的加速度确定所述卡尔曼滤波器的状态方程的噪声的协方差矩阵；

其中，所述状态转移矩阵包括：

所述单位观测矩阵包括：

其中，Δt表示所述单位测量时间；

所述卡尔曼滤波器的状态方程的噪声的协方差矩阵，包括：

Q＝cov(Bu(n))

其中，u(n)＝(a_x，a_y)，a_x表示所述目标车辆在大地坐标系的X轴的加速度，a_y表示所述目标车辆在大地坐标系的Y轴的加速度。

可选的，所述以所述目标车辆的测量定位位置，所述目标车辆在大地坐标系的X轴和Y轴的速度作为卡尔曼滤波器的输入进行卡尔曼滤波，包括：

获取所述卡尔曼滤波器的协方差矩阵的初始状态；

根据所述卡尔曼滤波器的输入，以及所述卡尔曼滤波器的协方差矩阵的初始状态，分别利用所述卡尔曼滤波器的系统状态计算公式以及所述卡尔曼滤波器的协方差计算公式，获取所述卡尔曼滤波器的状态矩阵的预测值和协方差矩阵的预测值；

根据所述协方差矩阵的预测值，利用卡尔曼增益计算公式获取卡尔曼增益；

根据所述卡尔曼增益，利用系统状态修正公式对所述状态矩阵的预测值进行修正，得到所述状态矩阵的修正后的预测值；

根据所述卡尔曼增益，利用协方差更新公式对所述协方差矩阵的预测值进行更新，得到所述协方差矩阵的更新后的预测值；

根据所述状态矩阵的修正后的预测值，以及所述协方差矩阵的更新后的预测值，再次执行所述分别利用所述卡尔曼滤波器的系统状态计算公式以及所述卡尔曼滤波器的协方差计算公式，获取所述卡尔曼滤波器的状态矩阵的预测值和协方差矩阵的预测值至所述根据所述卡尔曼增益对所述协方差矩阵的预测值进行更新的步骤，直至得到的所述状态矩阵的修正后的预测值满足所述卡尔曼滤波器的预设收敛条件；

将满足所述预设收敛条件的所述状态矩阵的修正后的预测值作为所述卡尔曼滤波器的输出结果。

可选的，所述系统状态计算公式包括：

所述协方差计算公式包括：

p(n|n-1)＝A(n-1)p(n-1|n-1)A(n-1)^T+Q

所述卡尔曼增益计算公式包括：

K(n)＝p(n|n-1)A^T(n)[R(n)+A(n)p(n|n-1)A^T(n)]^-1

所述系统状态修正公式包括：

所述协方差更新公式包括：

p(n|n)＝[I-K(n)A(n)]p(n|n-1)

其中，R(n)表示所述目标车辆的测量定位位置的协方差，R(n)＝cov(Z(n))，Z(n)表示所述目标车辆的测量定位位置，I表示单位矩阵。

根据本公开实施例的第二方面，提供一种基于车载通信的定位修正装置，应用于目标车辆，所述装置包括：第一信息获取模块、测量定位模块、第二信息获取模块、滤波模块和定位模块；

所述第一信息获取模块，用于获取至少两个邻居节点的相对距离、相对角度、通过导航获取的定位位置；所述邻居节点为与所述目标车辆的距离小于预设距离的其他车辆或者路边单元；

所述测量定位模块，用于根据所述至少两个邻居节点的相对距离、相对角度、定位位置利用三角测量法确定所述目标车辆的测量定位位置；

所述第二信息获取模块，用于通过惯性导航获取所述目标车辆在大地坐标系的X轴和Y轴的速度和加速度；

所述滤波模块，用于以所述目标车辆的测量定位位置、所述目标车辆在大地坐标系的X轴和Y轴的速度作为卡尔曼滤波器的输入进行卡尔曼滤波；

所述定位模块，用于根据所述卡尔曼滤波的输出结果得到所述目标车辆的定位位置。

可选的，所述装置还包括：矩阵获取模块；

所述矩阵获取模块，用于确定所述卡尔曼滤波器的状态转移矩阵、单位观测矩阵以及所述卡尔曼滤波器的状态方程的噪声的协方差矩阵。

可选的，所述第一信息获取模块用于：

通过专用短程通信技术DSRC从所述至少两个邻居节点获取所述至少两个邻居节点的相对距离、相对角度、通过导航获取的定位位置；其中，所述相对距离包括每个邻居节点与所述目标车辆的相对距离以及每个邻居节点之间的相对距离，所述相对角度包括任意两个邻居节点与所述目标车辆所构成的三角形中以邻居节点为顶点的角的角度。

可选的，所述矩阵获取模块包括：第一矩阵获取子模块和第二矩阵获取子模块；

所述第一矩阵获取子模块，用于根据预先设置的单位测量时间确定所述状态转移矩阵和所述单位观测矩阵；

所述第二矩阵获取子模块，用于根据所述单位观测矩阵以及所述目标车辆在大地坐标系的X轴和Y轴的加速度确定所述卡尔曼滤波器的状态方程的噪声的协方差矩阵；

其中，所述状态转移矩阵包括：

所述单位观测矩阵包括：

其中，Δt表示所述单位测量时间；

所述卡尔曼滤波器的状态方程的噪声的协方差矩阵，包括：

Q＝cov(Bu(n))

其中，u(n)＝(a_x，a_y)，a_x表示所述目标车辆在大地坐标系的X轴的加速度，a_y表示所述目标车辆在大地坐标系的Y轴的加速度。

可选的，所述滤波模块包括：初值获取子模块、预测子模块、增益获取子模块、修正子模块、更新子模块和输出子模块；

所述初值获取子模块，用于获取所述卡尔曼滤波器的协方差矩阵的初始状态；

所述预测子模块，用于根据所述卡尔曼滤波器的输入，以及所述卡尔曼滤波器的协方差矩阵的初始状态，分别利用所述卡尔曼滤波器的系统状态计算公式以及所述卡尔曼滤波器的协方差计算公式，获取所述卡尔曼滤波器的状态矩阵的预测值和协方差矩阵的预测值；

所述增益获取子模块，用于根据所述协方差矩阵的预测值，利用卡尔曼增益计算公式获取卡尔曼增益；

所述修正子模块，用于根据所述卡尔曼增益，利用系统状态修正公式对所述状态矩阵的预测值进行修正，得到所述状态矩阵的修正后的预测值；

所述更新子模块，用于根据所述卡尔曼增益，利用协方差更新公式对所述协方差矩阵的预测值进行更新，得到所述协方差矩阵的更新后的预测值；

所述输出子模块，用于根据所述状态矩阵的修正后的预测值，以及所述协方差矩阵的更新后的预测值，再次执行所述分别利用所述卡尔曼滤波器的系统状态计算公式以及所述卡尔曼滤波器的协方差计算公式，获取所述卡尔曼滤波器的状态矩阵的预测值和协方差矩阵的预测值至所述根据所述卡尔曼增益对所述协方差矩阵的预测值进行更新的步骤，直至得到的所述状态矩阵的修正后的预测值满足所述卡尔曼滤波器的预设收敛条件；将满足所述预设收敛条件的所述状态矩阵的修正后的预测值作为所述卡尔曼滤波器的输出结果。

可选的，所述系统状态计算公式包括：

所述协方差计算公式包括：

p(n|n-1)＝A(n-1)p(n-1|n-1)A(n-1)^T+Q

所述卡尔曼增益计算公式包括：

K(n)＝p(n|n-1)A^T(n)[R(n)+A(n)p(n|n-1)A^T(n)]^-1

所述系统状态修正公式包括：

所述协方差更新公式包括：

p(n|n)＝[I-K(n)A(n)]p(n|n-1)

其中，R(n)表示所述目标车辆的测量定位位置的协方差，R(n)＝cov(Z(n))，Z(n)表示所述目标车辆的测量定位位置，I表示单位矩阵。

通过上述技术方案，本公开根据在目标车辆没有GNSS信号的情况下，利用邻居节点获得的测量定位位置和目标车辆的速度、加速度作为卡尔曼滤波器的输入值，对测量定位位置进行动态修正，用以解决车辆在行驶过程中由于导航卫星信号减弱或消失导致的定位精度低、惯性导航可信时间短的问题，具有延长惯性导航的可信时间、提高定位精度的效果。

应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本公开。

附图说明

附图是用来提供对本公开的进一步理解，并且构成说明书的一部分，与下面的具体实施方式一起用于解释本公开，但并不构成对本公开的限制。在附图中：

图1是根据一示例性实施例示出的一种基于车载通信的定位修正方法的流程图；

图2是根据图1所示实施例示出的一种三角测量法的示意图；

图3是根据一示例性实施例示出的另一种基于车载通信的定位修正方法的流程图；

图4是根据一示例性实施例示出的一种卡尔曼滤波的矩阵确定方法的流程图；

图5是根据一示例性实施例示出的一种卡尔曼滤波方法的流程图；

图6是根据一示例性实施例示出的一种基于车载通信的定位修正装置的框图；

图7是根据一示例性实施例示出的另一种基于车载通信的定位修正装置的框图；

图8是根据一示例性实施例示出的又一种基于车载通信的定位修正装置的框图；

图9是根据一示例性实施例示出的再一种基于车载通信的定位修正装置的框图。

具体实施方式

这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本公开相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本公开的一些方面相一致的装置和方法的例子。

在介绍本公开提供的车载通信的定位修正方法和装置之前，首先对本公开各个实施例所涉及的应用场景进行介绍。该应用场景可以包括支持DSRC(英文：Dedicated ShortRange Communications中文：专用短程通信技术)网络的车辆和路边单元，该车辆和路边单元均能通过DSRC网络进行短距离的无线通信，以无线信号为载体传递报文，可以利用无线信号在无线环境下的传播计算得出各车辆和路边单元之间的相对距离和相对角度，对车辆本身的导航定位系统进行修正。本公开以车辆和路边单元均配置有能够发射UWB(英文：Ultra Wideband，中文：超宽带)信号的UWB装置为例，车辆和路边单元均可以通过其自身的UWB装置以UWB信号进行通信，UWB信号是高频短脉冲信号，利用UWB信号进行定位能够达到分米级的精度。并且，UWB信号具有很宽的带宽，因此能够适应车辆高速的行驶状态，能够解决多路径问题、信号反射问题，非视线内绕射问题，在高速下定位精度基本不变，可在无GNSS信号的情况下单独工作。上述的车辆还可以配置有GNSS(英文：Global NavigationSatellite System，中文：全球导航卫星系统)装置和惯性导航装置。其中，在本公开的各个实施例中，GNSS可以包括但不限于：GPS(Global Positioning System，全球定位系统)、北斗卫星导航系统、GLONASS导航系统、伽利略卫星导航系统等，惯性导航是指利用三轴加速度计和三轴陀螺仪的变化，不断校准GNSS定位数据的技术，可以提供更高精度和稳定度的修正数据，在短时间GNSS信号丢失状况下，惯性导航能持续预测车辆绝对位置，常见的惯性导航如ADR导航技术。该车辆可以是汽车，该汽车不限于传统汽车、纯电动汽车或是混动汽车，除此之外还可以适用于其他类型的机动车或非机动车。

图1是根据一示例性实施例示出的一种基于车载通信的定位修正方法的流程图，如图1所示，该方法包括：

步骤101，获取至少两个邻居节点的相对距离、相对角度、通过导航获取的定位位置；邻居节点为与目标车辆的距离小于预设距离的其他车辆或者路边单元。

可选的，可以通过专用短程通信技术DSRC从至少两个邻居节点获取至少两个邻居节点的相对距离、相对角度、通过导航获取的定位位置。其中，相对距离包括每个邻居节点与目标车辆的相对距离以及每个邻居节点之间的相对距离，相对角度包括任意两个邻居节点与目标车辆所构成的三角形中以邻居节点为顶点的角的角度。

示例的，当目标车辆在行驶过程中，接收到的GNSS信号的强度小于预设强度时，可能导致定位结果的误差过大或者无法定位。此时，目标车辆可以搜索附近所有能够通过DSRC网络接入的邻居节点，这些邻居节点可以是目标车辆附近的其他车辆，也可以是安装在固定位置辅助车辆定位的路边单元。其中无论是目标车辆或是其他车辆均可以为上述应用场景中所述的车辆，路边单元为上述应用场景中所述的具有UWB装置的路边单元，该目标车辆可以通过上述的UWB装置与这些邻居节点进行通信。邻居节点可以通过UWB定位技术测量与目标车辆之间，以及与其他邻居节点之间的相对距离、相对角度，并可以通过GNSS获取自身的定位位置，从而可以将相对距离、相对角度、通过导航获取的定位位置以及定位位置确信度发送给目标车辆，用于该目标车辆计算自身的位置。

另外，目标车辆获取的与每个邻居节点的相对距离可以包括每个邻居节点与目标车辆的相对距离以及每个邻居节点之间的相对距离，相对角度可以包括任意两个邻居节点与目标车辆所构成的三角形中以邻居节点为顶点的角的角度。值得一提的是，关于相对距离和相对角度的获取方法，除了上述的可以由邻居节点测量并计算获得后直接发送给目标车辆的方式外，也可以由目标车辆根据邻居节点和目标车辆在DSRC网络中的坐标计算获得。

步骤102，根据至少两个邻居节点的相对距离、相对角度、定位位置利用三角测量法确定目标车辆的测量定位位置。

举例说明，图2是三角测量法的示意图，如图2所示，其中D表示目标车辆，A、B、C和E表示在D周围的四个邻居节点，邻居节点可以是目标车辆D附近的其他车辆或者路边单元，并且邻居节点的GNSS坐标可以通过GNSS定位直接得到或固定位置的坐标。在B和C与D所构成的三角形中，D与B、C之间的相对距离为d2和d3、B和C之间的相对距离为d1、分别以B、C为顶点的角为a1和b1和以D为顶点的角为c1。在A和B与D所构成的三角形中，D与A、B之间的相对距离为d4和d2、A和B之间的相对距离为d6、分别以A、B为顶点的角为a2和b2和以D为顶点的角为c2。在C和E与D所构成的三角形中，D与C、E之间的相对距离为d3和d5、C和E之间的相对距离为d7、分别以C、E为顶点的角为a3和b3和以D为顶点的角为c3。三个三角形构成三角锁。

可以利用以下公式对上述的三角锁中的每个三角形进行校准：

首先可以通过计算三角形的角度闭合差，对三角形的顶角进行校准：

其中，i为1至3的自然数。

其次，可以计算上述的三角锁的基线条件闭合差W_D：

其中，W_D越接近零表示上述三角锁校准的越准确，因此可以预先设置一个W_D的阈值，并以此为标准对三角形的顶角进行校准，直到W_D小于或者等于该阈值时，确定完成了对三角锁的校准。此时，选择三角锁中任意一个三角形，可以求得D点的坐标。以B和C与D所构成的三角形为例，根据B和C与D所构成的三角形中三边长度(相对距离d1、d2、d3)，和三个顶角的角度(相对角度a1、b1、c1)，利用锐角三角形公式可以求出D点的坐标，该坐标就可以作为目标车辆的测量定位位置。还可以根据B和C的GNSS坐标和a1和b1角的角度，以及BC之间的距离d1，根据正弦定理有：

其中，假设B的GNSS坐标为(X1,Y1)，C的GNSS坐标为(X2,Y2)，需要测量的目标车辆D的GNSS坐标表示为(X_P,Y_P)，则根据上述的正弦定理公式有一下关系：

其中，由于B的GNSS坐标为(X₁,Y₁)，C的GNSS坐标为(X₂,Y₂)，BC之间的距离d1已知，因此可以计算出D的GNSS坐标为(X_P,Y_P)，从而得到了目标车辆的测量定位位置(X_P,Y_P)。

另外，值得一提的是，图3所示的三角锁的形状是示例性的，包括但不限于此，三角锁具体的拟合形式可以根据周边环境的地形来设置，例如在隧道内，在高大的建筑物周边，构建的三角锁的形状并不相同，但是对三角锁的校准，均可以采用上述的校准方法。

步骤103，通过惯性导航获取目标车辆在大地坐标系的X轴和Y轴的速度和加速度。

示例的，根据惯性导航中的三轴加速度计和三轴陀螺仪可以获取目标车辆的实时加速度a和目标车辆的行驶方向，其中目标车辆的行驶方向可以用目标车辆与大地坐标系的X轴的夹角θ(大地坐标系是以该目标车辆为原点，Y轴指向正北的坐标系)，再结合目标车辆的速度V，通过投影的方式，就可以根据以下公式得到目标车辆在大地坐标系的X轴和Y轴的速度V_X、V_Y和加速度a_X、a_Y：

V_X＝V*cosθ

V_Y＝V*sinθ

a_X＝a*cosθ

a_Y＝a*sinθ

需要说明的是，在本公开的各个实施例中，大地坐标系为GNSS坐标系，即X轴为纬度方向，Y轴为经度方向的坐标系。

步骤104，以目标车辆的测量定位位置、目标车辆在大地坐标系的X轴和Y轴的速度作为卡尔曼滤波器的输入进行卡尔曼滤波。

示例的，以目标车辆的测量定位位置(X_P,Y_P)和目标车辆在大地坐标系的X轴和Y轴的速度(V_X,V_Y)作为尔曼滤波器的输入Z：

Z＝(X_P,Y_P,V_X,V_Y)

步骤105，根据卡尔曼滤波的输出结果得到目标车辆的定位位置。

进一步的，图3是根据一示例性实施例示出的另一种基于车载通信的定位修正方法的流程图，如图3所示，在步骤104所述的进行卡尔曼滤波之前，该方法还可以包括：

步骤106，确定卡尔曼滤波器的状态转移矩阵、单位观测矩阵以及所述卡尔曼滤波器的状态方程的噪声的协方差矩阵。

举例来说，确定建立卡尔曼滤波器所需要的几个矩阵，可以按照图4所示的一种卡尔曼滤波的矩阵确定方法来获得，如图4所示，步骤106可以包括以下步骤：

步骤1061，根据预先设置的单位测量时间确定状态转移矩阵和单位观测矩阵。

步骤1062，根据单位观测矩阵以及目标车辆在大地坐标系的X轴和Y轴的加速度确定卡尔曼滤波器的状态方程的噪声的协方差矩阵。

其中，状态转移矩阵包括：

单位观测矩阵包括：

其中，Δt表示单位测量时间。单位测量时间是预先设定的，根据DSRC网络的技术特点，可以将单位测量时间控制在100ms以内，并且能够将误差控制在10ms以下。

卡尔曼滤波器的状态方程的噪声的协方差矩阵，包括：

Q＝cov(Bu(n))

其中，u(n)＝(a_x，a_y)，a_x表示目标车辆在大地坐标系的X轴的加速度，a_y表示目标车辆在大地坐标系的Y轴的加速度。

图5是根据一示例性实施例示出的一种卡尔曼滤波方法的流程图，如图5所示，步骤104包括：

步骤1041，获取卡尔曼滤波器的协方差矩阵的初始状态。

示例地，当目标车辆的位置已知时，协方差矩阵P的初始状态可以是：

但是通常情况下，目标车辆的准确位置是不确定的，所以我们可以初始化协方差矩阵P为：

P(0|0)＝Q

步骤1042，根据卡尔曼滤波器的输入，以及卡尔曼滤波器的协方差矩阵的初始状态，分别利用卡尔曼滤波器的系统状态计算公式以及卡尔曼滤波器的协方差计算公式，获取卡尔曼滤波器的状态矩阵的预测值和协方差矩阵的预测值。

其中根据系统状态计算公式获得状态矩阵的预测值

根据协方差计算公式获得协方差矩阵的预测值p(n|n-1)包括：

p(n|n-1)＝A(n-1)p(n-1|n-1)A(n-1)^T+Q

步骤1043，根据协方差矩阵的预测值，利用卡尔曼增益计算公式获取卡尔曼增益。

其中，卡尔曼增益计算公式包括：

K(n)＝p(n|n-1)A^T(n)[R(n)+A(n)p(n|n-1)A^T(n)]^-1

其中，R(n)表示目标车辆的测量定位位置的协方差，R(n)＝cov(Z(n))。

步骤1044，根据卡尔曼增益，利用系统状态修正公式对状态矩阵的预测值进行修正，得到状态矩阵的修正后的预测值。

其中，系统状态修正公式包括：

其中，Z(n)表示所述目标车辆的测量定位位置。

步骤1045，根据卡尔曼增益，利用协方差更新公式对协方差矩阵的预测值进行更新，得到协方差矩阵的更新后的预测值。

其中协方差更新公式包括：

p(n|n)＝[I-K(n)A(n)]p(n|n-1)

其中，I表示单位矩阵。

步骤1046，根据状态矩阵的修正后的预测值，以及协方差矩阵的更新后的预测值，再次执行步骤1042至1046，直至得到的状态矩阵的修正后的预测值满足卡尔曼滤波器的预设收敛条件。将满足预设收敛条件的状态矩阵的修正后的预测值作为卡尔曼滤波器的输出结果。

综上所述，本公开根据在目标车辆没有GNSS信号的情况下，利用邻居节点获得的测量定位位置和目标车辆的速度、加速度作为卡尔曼滤波器的输入值，对测量定位位置进行动态修正，用以解决车辆在行驶过程中由于导航卫星信号减弱或消失导致的定位精度低、惯性导航可信时间短的问题，具有延长惯性导航的可信时间、提高定位精度的效果。

图6是根据一示例性实施例示出的一种基于车载通信的定位修正装置的框图，如图6所示，该装置包括：第一信息获取模块601、测量定位模块602、第二信息获取模块603、滤波模块604和定位模块605。

第一信息获取模块601，用于获取至少两个邻居节点的相对距离、相对角度、通过导航获取的定位位置；邻居节点为与目标车辆的距离小于预设距离的其他车辆或者路边单元。

测量定位模块602，用于根据至少两个邻居节点的相对距离、相对角度、定位位置利用三角测量法确定目标车辆的测量定位位置。

第二信息获取模块603，用于通过惯性导航获取目标车辆在大地坐标系的X轴和Y轴的速度和加速度。

滤波模块604，用于以目标车辆的测量定位位置、目标车辆在大地坐标系的X轴和Y轴的速度作为卡尔曼滤波器的输入进行卡尔曼滤波。

定位模块605，用于根据卡尔曼滤波的输出结果得到目标车辆的定位位置。

图7是根据一示例性实施例示出的另一种基于车载通信的定位修正装置的框图，如图7所示，该装置还包括矩阵获取模块606。

矩阵获取模块606，用于确定卡尔曼滤波器的状态转移矩阵、单位观测矩阵以及卡尔曼滤波器的状态方程的噪声的协方差矩阵。

可选的，第一信息获取模块601用于：

通过专用短程通信技术DSRC从至少两个邻居节点获取至少两个邻居节点的相对距离、相对角度、通过导航获取的定位位置。其中，相对距离包括每个邻居节点与目标车辆的相对距离以及每个邻居节点之间的相对距离，相对角度包括任意两个邻居节点与目标车辆所构成的三角形中以邻居节点为顶点的角的角度。

图8是根据一示例性实施例示出的又一种基于车载通信的定位修正装置的框图，如图8所示，矩阵获取模块606包括：第一矩阵获取子模块6061和第二矩阵获取子模块6062。

第一矩阵获取子模块6061，用于根据预先设置的单位测量时间确定状态转移矩阵和单位观测矩阵。

第二矩阵获取子模块6062，用于根据单位观测矩阵以及目标车辆在大地坐标系的X轴和Y轴的加速度确定卡尔曼滤波器的状态方程的噪声的协方差矩阵。

其中，状态转移矩阵包括：

单位观测矩阵包括：

其中，Δt表示单位测量时间。

卡尔曼滤波器的状态方程的噪声的协方差矩阵，包括：

Q＝cov(Bu(n))

其中，u(n)＝(a_x，a_y)，a_x表示目标车辆在大地坐标系的X轴的加速度，a_y表示目标车辆在大地坐标系的Y轴的加速度。

图9是根据一示例性实施例示出的再一种基于车载通信的定位修正装置的框图，如图9所示，滤波模块604包括：初值获取子模块6041、预测子模块6042、增益获取子模块6043、修正子模块6044、更新子模块6045和输出子模块6046；

初值获取子模块6041，用于获取卡尔曼滤波器的协方差矩阵的初始状态。

预测子模块6042，用于根据卡尔曼滤波器的输入，以及卡尔曼滤波器的协方差矩阵的初始状态，分别利用卡尔曼滤波器的系统状态计算公式以及卡尔曼滤波器的协方差计算公式，获取卡尔曼滤波器的状态矩阵的预测值和协方差矩阵的预测值。

增益获取子模块6043，用于根据协方差矩阵的预测值，利用卡尔曼增益计算公式获取卡尔曼增益。

修正子模块6044，用于根据卡尔曼增益，利用系统状态修正公式对状态矩阵的预测值进行修正，得到状态矩阵的修正后的预测值。

更新子模块6045，用于根据卡尔曼增益，利用协方差更新公式对协方差矩阵的预测值进行更新，得到协方差矩阵的更新后的预测值。

输出子模块6046，用于根据状态矩阵的修正后的预测值，以及协方差矩阵的更新后的预测值，再次执行分别利用卡尔曼滤波器的系统状态计算公式以及卡尔曼滤波器的协方差计算公式，获取卡尔曼滤波器的状态矩阵的预测值和协方差矩阵的预测值至根据所述卡尔曼增益对协方差矩阵的预测值进行更新的步骤，直至得到的状态矩阵的修正后的预测值满足卡尔曼滤波器的预设收敛条件；将满足预设收敛条件的状态矩阵的修正后的预测值作为卡尔曼滤波器的输出结果。

可选的，系统状态计算公式包括：

协方差计算公式包括：

p(n|n-1)＝A(n-1)p(n-1|n-1)A(n-1)^T+Q

卡尔曼增益计算公式包括：

K(n)＝p(n|n-1)A^T(n)[R(n)+A(n)p(n|n-1)A^T(n)]^-1

系统状态修正公式包括：

协方差更新公式包括：

p(n|n)＝[I-K(n)A(n)]p(n|n-1)

其中，R(n)表示所述目标车辆的测量定位位置的协方差，R(n)＝cov(Z(n))，Z(n)表示所述目标车辆的测量定位位置，I表示单位矩阵。

其中，上述各个模块所实现功能的具体说明已经在上述方法实施例中进行了详细描述，此处不再赘述。

综上所述，本公开根据在目标车辆没有GNSS信号的情况下，利用邻居节点获得的测量定位位置和目标车辆的速度、加速度作为卡尔曼滤波器的输入值，对测量定位位置进行动态修正，用以解决车辆在行驶过程中由于导航卫星信号减弱或消失导致的定位精度低、惯性导航可信时间短的问题，具有延长惯性导航的可信时间、提高定位精度的效果。

以上结合附图详细描述了本公开的优选实施方式，但是，本公开并不限于上述实施方式中的具体细节，在本公开的技术构思范围内，本领域技术人员在考虑说明书及实践本公开后，容易想到本公开的其它实施方案，均属于本公开的保护范围。

另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合。同时本公开的各种不同的实施方式之间也可以进行任意组合，只要其不违背本公开的思想，其同样应当视为本公开所公开的内容。本公开并不局限于上面已经描述出的精确结构，本公开的范围仅由所附的权利要求来限制。

Claims (10)

1.一种基于车载通信的定位修正方法，其特征在于，应用于目标车辆，所述方法包括：

获取至少两个邻居节点的相对距离、相对角度、通过导航获取的定位位置；所述邻居节点为与所述目标车辆的距离小于预设距离的其他车辆或者路边单元；

根据所述至少两个邻居节点的相对距离、相对角度、定位位置利用三角测量法确定所述目标车辆的测量定位位置；

通过惯性导航获取所述目标车辆在大地坐标系的X轴和Y轴的速度和加速度；

以所述目标车辆的测量定位位置、所述目标车辆在大地坐标系的X轴和Y轴的速度作为卡尔曼滤波器的输入进行卡尔曼滤波；

根据所述卡尔曼滤波的输出结果得到所述目标车辆的定位位置。

2.根据权利要求1所述的方法，其特征在于，在进行卡尔曼滤波之前，所述方法还包括：

确定所述卡尔曼滤波器的状态转移矩阵、单位观测矩阵以及所述卡尔曼滤波器的状态方程的噪声的协方差矩阵。

3.根据权利要求1或2所述的方法，其特征在于，所述获取至少两个邻居节点的相对距离、相对角度、通过导航获取的定位位置，包括：

通过专用短程通信技术DSRC从所述至少两个邻居节点获取所述至少两个邻居节点的相对距离、相对角度、通过导航获取的定位位置；

其中，所述相对距离包括每个邻居节点与所述目标车辆的相对距离以及每个邻居节点之间的相对距离，所述相对角度包括任意两个邻居节点与所述目标车辆所构成的三角形中以邻居节点为顶点的角的角度。

4.根据权利要求2所述的方法，其特征在于，所述确定卡尔曼滤波器的状态转移矩阵、单位观测矩阵以及所述卡尔曼滤波器的状态方程的噪声的协方差矩阵，包括：

根据预先设置的单位测量时间确定所述状态转移矩阵和所述单位观测矩阵；

根据所述单位观测矩阵以及所述目标车辆在大地坐标系的X轴和Y轴的加速度确定所述卡尔曼滤波器的状态方程的噪声的协方差矩阵；

其中，所述状态转移矩阵包括：

$A\left(n\right)=\left[\begin{array}{cccc}1& 0& \mathrm{\Δ}t& 0\\ 0& 1& 0& \mathrm{\Δ}t\\ 0& 0& \mathrm{\Δ}t& 0\\ 0& 0& 0& \mathrm{\Δ}t\end{array}\right]$

所述单位观测矩阵包括：

$B=\left[\begin{array}{cc}{\mathrm{\Δt}}^2/2& 0\\ 0& {\mathrm{\Δt}}^2/2\\ \mathrm{\Δ}t& 0\\ 0& \mathrm{\Δ}t\end{array}\right]$

其中，Δt表示所述单位测量时间；

所述卡尔曼滤波器的状态方程的噪声的协方差矩阵，包括：

Q＝cov(Bu(n))

其中，u(n)＝(a_x，a_y)，a_x表示所述目标车辆在大地坐标系的X轴的加速度，a_y表示所述目标车辆在大地坐标系的Y轴的加速度。

5.根据权利要求1或2所述的方法，其特征在于，所述以所述目标车辆的测量定位位置，所述目标车辆在大地坐标系的X轴和Y轴的速度作为卡尔曼滤波器的输入进行卡尔曼滤波，包括：

获取所述卡尔曼滤波器的协方差矩阵的初始状态；

根据所述卡尔曼滤波器的输入，以及所述卡尔曼滤波器的协方差矩阵的初始状态，分别利用所述卡尔曼滤波器的系统状态计算公式以及所述卡尔曼滤波器的协方差计算公式，获取所述卡尔曼滤波器的状态矩阵的预测值和协方差矩阵的预测值；

根据所述协方差矩阵的预测值，利用卡尔曼增益计算公式获取卡尔曼增益；

根据所述卡尔曼增益，利用系统状态修正公式对所述状态矩阵的预测值进行修正，得到所述状态矩阵的修正后的预测值；

根据所述卡尔曼增益，利用协方差更新公式对所述协方差矩阵的预测值进行更新，得到所述协方差矩阵的更新后的预测值；

根据所述状态矩阵的修正后的预测值，以及所述协方差矩阵的更新后的预测值，再次执行所述分别利用所述卡尔曼滤波器的系统状态计算公式以及所述卡尔曼滤波器的协方差计算公式，获取所述卡尔曼滤波器的状态矩阵的预测值和协方差矩阵的预测值至所述根据所述卡尔曼增益对所述协方差矩阵的预测值进行更新的步骤，直至得到的所述状态矩阵的修正后的预测值满足所述卡尔曼滤波器的预设收敛条件；

将满足所述预设收敛条件的所述状态矩阵的修正后的预测值作为所述卡尔曼滤波器的输出结果。

6.根据权利要求5所述的方法，其特征在于，所述系统状态计算公式包括：

$\hat{x}\left(n\right|n-1)=A(n-1)\hat{x}(n-1|n-1)+Bu\left(n\right)$

所述协方差计算公式包括：

p(n|n-1)＝A(n-1)p(n-1|n-1)A(n-1)^T+Q

所述卡尔曼增益计算公式包括：

K(n)＝p(n|n-1)A^T(n)[R(n)+A(n)p(n|n-1)A^T(n)]^-1

所述系统状态修正公式包括：

$\hat{x}\left(n\right|n)=\hat{x}\left(n\right|n-1)+K\left(n\right)\[Z\left(n\right)-A\left(n\right)\hat{x}\left(n\right|n-1)\]$

所述协方差更新公式包括：

p(n|n)＝[I-K(n)A(n)]p(n|n-1)

其中，R(n)表示所述目标车辆的测量定位位置的协方差，R(n)＝cov(Z(n))，Z(n)表示所述目标车辆的测量定位位置，I表示单位矩阵。

7.一种基于车载通信的定位修正装置，其特征在于，应用于目标车辆，所述装置包括：第一信息获取模块、测量定位模块、第二信息获取模块、滤波模块和定位模块；

所述第一信息获取模块，用于获取至少两个邻居节点的相对距离、相对角度、通过导航获取的定位位置；所述邻居节点为与所述目标车辆的距离小于预设距离的其他车辆或者路边单元；

所述测量定位模块，用于根据所述至少两个邻居节点的相对距离、相对角度、定位位置利用三角测量法确定所述目标车辆的测量定位位置；

所述第二信息获取模块，用于通过惯性导航获取所述目标车辆在大地坐标系的X轴和Y轴的速度和加速度；

所述滤波模块，用于以所述目标车辆的测量定位位置、所述目标车辆在大地坐标系的X轴和Y轴的速度作为卡尔曼滤波器的输入进行卡尔曼滤波；

所述定位模块，用于根据所述卡尔曼滤波的输出结果得到所述目标车辆的定位位置。

8.根据权利要求7所述装置，其特征在于，所述装置还包括：矩阵获取模块；

所述矩阵获取模块，用于确定所述卡尔曼滤波器的状态转移矩阵、单位观测矩阵以及所述卡尔曼滤波器的状态方程的噪声的协方差矩阵。

9.根据权利要求7或8所述装置，其特征在于，所述第一信息获取模块用于：

通过专用短程通信技术DSRC从所述至少两个邻居节点获取所述至少两个邻居节点的相对距离、相对角度、通过导航获取的定位位置；其中，所述相对距离包括每个邻居节点与所述目标车辆的相对距离以及每个邻居节点之间的相对距离，所述相对角度包括任意两个邻居节点与所述目标车辆所构成的三角形中以邻居节点为顶点的角的角度。

10.根据权利要求8所述装置，其特征在于，所述矩阵获取模块包括：第一矩阵获取子模块和第二矩阵获取子模块；

所述第一矩阵获取子模块，用于根据预先设置的单位测量时间确定所述状态转移矩阵和所述单位观测矩阵；

所述第二矩阵获取子模块，用于根据所述单位观测矩阵以及所述目标车辆在大地坐标系的X轴和Y轴的加速度确定所述卡尔曼滤波器的状态方程的噪声的协方差矩阵；

其中，所述状态转移矩阵包括：

$A\left(n\right)=\left[\begin{array}{cccc}1& 0& \mathrm{\Δ}t& 0\\ 0& 1& 0& \mathrm{\Δ}t\\ 0& 0& \mathrm{\Δ}t& 0\\ 0& 0& 0& \mathrm{\Δ}t\end{array}\right]$

所述单位观测矩阵包括：

$B=\left[\begin{array}{cc}{\mathrm{\Δt}}^2/2& 0\\ 0& {\mathrm{\Δt}}^2/2\\ \mathrm{\Δ}t& 0\\ 0& \mathrm{\Δ}t\end{array}\right]$

其中，Δt表示所述单位测量时间；

所述卡尔曼滤波器的状态方程的噪声的协方差矩阵，包括：

Q＝cov(Bu(n))

其中，u(n)＝(a_x，a_y)，a_x表示所述目标车辆在大地坐标系的X轴的加速度，a_y表示所述目标车辆在大地坐标系的Y轴的加速度。