CN114323034A - 一种卫星遮蔽环境中的基于置信度传播的多车协同定位方法 - Google Patents

Abstract

本发明公开一种卫星遮蔽环境中的基于置信度传播的多车协同定位方法，包括如下步骤：步骤一：对车辆协同网络中的节点(包括定位车辆和路边单元)位置进行初始化，并且设定定位车辆先验概率作为初始置信度，并且建立多车协同定位的因子图模型；步骤二：车辆节点利用车载的惯性导航模块测量得到的车辆加速度与角速度对车辆进行航迹推算，IMU因子节点将置信度传播给相连的变量节点，完成预测过程。本发明适用于当车辆处于诸如峡谷、隧道和森林等卫星遮蔽环境中，卫星定位精度严重下降的场景，依靠基于惯性导航的航迹推算和多车协同，可以实现比传统单车自主定位更高的定位精度。

Description

一种卫星遮蔽环境中的基于置信度传播的多车协同定位方法

技术领域

本发明涉及一种卫星遮蔽环境中的基于置信度传播的多车协同定位方法，属于车辆定位、导航与无线传感器网络节点定位领域。

背景技术

传统的车辆定位方法一般以卫星定位为主，在较为理想的环境下，全球导航卫星系统(Global Navigation Satellite System，GNSS)能够实现米级的定位精度，但是当车辆处于诸如峡谷、隧道和森林等环境中时，由于卫星信号受到遮挡导致定位精度严重下降。近年来随着自动驾驶、智能交通等应用的发展，GNSS定位在复杂环境中的局限性已经无法满足当前车辆定位的需求。

目前解决车辆在GNSS受限环境中定位下降的常用方法主要可以分为车载多传感器融合定位技术和协同定位技术。基于多传感器融合的车辆定位方法在某一车载传感器精度降低甚至失效的情况下，可通过其他正常工作的传感器对定位结果进行修正。惯性导航系统(Inertial Navigation System，INS)是一种不依赖于外部信息的自主导航技术，受周边环境影响较低，在短时间内的定位精度较高，但是定位误差会随时间累积，因此不适用于长距离定位。基于视觉和激光雷达的车辆定位方法是利用即时定位与地图构建(Simultaneous Localization and Mapping，SLAM)技术对车辆周边的环境建图，通过对周边环境的点云信息和地图进行匹配来估计车辆的位置。但是计算复杂度较高，且SLAM同样存在误差累积的问题。

车辆的高机动性车辆周边环境的不确定性导致单一车辆的高精定位具有一定的狭隘性，因此基于车辆之间(Vehicle to Vehicle，V2V)、车辆与路边定位设施之间(Vehicle to Infrastructure，V2I)的多协作感知的技术应运而生。多车协同感知利用车与车、车与路边锚点的交互获得相邻车辆之间的位置信息、和邻居车辆的传感器信息，并将其与自身感知得到的信息进行融合处理，从而可以获得更为准确的定位信息。

发明内容

为解决上述背景技术所提出的问题，本文提供了一种基于置信度传播的多车协同定位方法，车辆可以通过车-车直连通信向附近协同车辆和路边单元发送本车的绝对位置和本车车载传感器测量得到的相对位置信息，利用因子图对车辆网络协同定位建模，然后通过基于置信度传播的迭代算法对车辆节点的位置进行估计。本发明提供如下技术方案：

一种卫星遮蔽环境中的基于置信度传播的多车协同定位方法，包括如下步骤：

步骤一：对车辆协同网络中的节点(包括定位车辆和路边单元)位置进行初始化，定义车辆协同网络中N个节点在第t个采样点的状态信息集合为

X^(0:t)＝{X⁽⁰⁾,X⁽¹⁾,…,X^(t)}表示协同网络中从初始采样点到第t个采样点的所有节点的状态信息集合；

步骤二：车辆节点利用车载的惯性导航模块测量得到的车辆加速度与角速度对车辆进行航迹推算，IMU因子节点将置信度传播给相连的变量节点，完成预测过程；

IMU因子节点表示如下：

式中，H_IMU(·)表示IMU模块的测量函数。

步骤三：车辆通过车-车直连通信向邻居节点发送本车的绝对位置和本车车载传感器测量得到的相对位置信息，所有变量节点，将当前置信度传播给相连的因子节点，并且获取邻居变量节点的置信度对自己的置信度进行更新；

因子节点传输给变量节点的置信度可以表示为：

变量节点传输给因子节点的置信度可以表示为：

BI(x_m,f₁)＝BI(f₂,x_m)·BI(f₃,x_m)…BI(f_j,x_m)

其中，f₂、f₃、f_j表示与变量节点x_m相连的因子节点。

步骤四：重复步骤三，使协同网络中所有节点的置信度达到预设的阈值，此时得到当前采样点所有变量节点的最优状态估计，结束迭代并进入步骤五；

步骤五：所有车辆节点移动到下一个采样点，重复步骤二到步骤四；

步骤六：当所有车辆节点移动到最后一个采样点，迭代结束，此时计算得到的边缘概率即最终的车辆最优位置估计。

节点位置估计方程为：

。

优选地，所述步骤一中的对车辆协同网络中的节点(包括定位置进行初始化，并且设定定位车辆先验概率作为初始置信度，并且建立多车协同定位的因子图模型；具体如下：

车辆协同网络中N个节点(包括车辆节点和RSU节点)在第t个采样点的状态信息集合为

表示m号车辆在t时刻的邻居节点集合，

表示车辆在t时刻得到的观测量集合，

X^(0:t)＝{X⁽⁰⁾,X⁽¹⁾,…,X^(t)}表示协同网络中从初始采样点到第t个采样点的所有节点的状态信息集合，同理Z^(1:t)＝{Z⁽¹⁾,Z⁽²⁾,…,Z^(t)}表示协同网络中从第1个采样点到第t个采样点的所有节点的测量信息集合。根据上述定义，协同网络中的节点状态信息的联合概率分布函数为：

p(X^(0:t)|Z^(1:t))

通过计算一段时间内状态信息的联合概率密度函数的最大后验估计，即可得到状态信息的最优估计。车辆协同网络中节点状态信息的最大后验估计为：

。

优选地，所述步骤二：车辆节点利用车载的惯性导航模块测量得到的车辆加速度与角速度对车辆进行航迹推算，IMU因子节点将置信度传播给相连的变量节点，完成预测过程；具体如下：

用

表示车辆根据前一采样点的状态信息和当前IMU模块测量的自信息做出的当前采样点时刻的状态信息估计。IMU测量值一般包括加速度和角速度，分别记为a和ω。可以得到相邻时刻的状态变量之间有如下关系：

x^(t)＝H_IMU(x^(t-1),a^(t-1),ω^(t-1))+n_IMU

其中，H_IMU(·)表示IMU模块的测量函数，IMU因子可由下式表示：

优选地，所述步骤三：车辆通过车-车直连通信向邻居节点发送本车的绝对位置和本车车载传感器测量得到的相对位置信息，所有变量节点，将当前置信度传播给相连的因子节点，并且获取邻居变量节点的置信度对自己的置信度进行更新；具体如下：

用

表示协同网络中节点根据接收到的邻居节点的状态信息以及测距信息对当前状态信息估计的修正。节点之间的无线距离测量值可表示为如下形式：

距离测量因子可以表示为：

因子节点传输给变量节点的置信度可以表示为：

变量节点传输给因子节点的置信度可以表示为：

BI(x_m,f₁)＝BI(f₂,x_m)·BI(f₃,x_m)…BI(f_j,x_m)

其中，f₂、f₃、f_j表示与变量节点x_m相连的因子节点。

优选地，所述步骤四：重复步骤三，使协同网络中所有节点的置信度达到预设的阈值，此时得到当前采样点所有变量节点的最优状态估计，结束迭代并进入步骤五。

优选地，所述步骤五：所有车辆节点移动到下一个采样点，重复步骤二到步骤四。

优选地，所述步骤六：当所有车辆节点移动到最后一个采样点，迭代结束，此时计算得到的边缘概率即最终的车辆最优位置估计；具体如下：

中节点之间的IMU观测信息相互独立，因此IMU测量部分可以简化为：

而

中节点的观测信息只与当前节点以及相邻节点的状态信息相关，与其他非相邻节点的状态和观测相互独立，因此可以进一步简化为：

综上可得协同网络中定位车辆的状态优化方程为：

。

本发明与现有技术相比的优点如下：

本发明提供了一种卫星遮蔽环境中基于置信度传播的多车协同定位方法，定位车辆首先利用车载的惯性导航模块进行位置估计，然后通过蜂窝车联网技术接收附近车辆和路边单元的信息修正位置估计。利用因子图对多车协同定位建模，然后通过基于置信度传播的迭代算法对车辆节点的位置进行估计。利用因子图模型，将最大后验估计问题转化为非线性最小二乘问题求解，极大的减小了状态优化时的计算量，节省了时间成本。本发明提出的方法相较于传统的单车定位方法具有更高的定位精度，同时受定位车辆的初始误差影响更低。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。

图1为本发明实例提供的卫星遮蔽环境中的基于置信度传播的多车协同定位方法模型示意图；

图2为本发明实例提供的卫星遮蔽环境中的基于置信度传播的多车协同定位方法流程图；

图3为本发明中不同状态节点和因子节点组成的多车协同定位因子图。

具体实施方式

下面结合附图对本发明实施例进行详细描述。

本实例仅限于说明本发明的一种实施方法，不代表对本发明覆盖范围的限制。图1为本发明实例的模型示意图。

如图2所示，本发明所述的一种卫星遮蔽环境中的基于置信度传播的多车协同定位方法，包括如下步骤：

步骤一：对于所有的变量节点获取先验概率

同时将先验概率作为初始置信度。

步骤二：对于所有IMU因子节点，将置信度传播给相连的变量节点，完成预测过程。置信度传播公式如下：

步骤三：对于所有变量节点，将当前置信度传播给相连的因子节点，并且获取邻居变量节点的置信度对自己的置信度进行更新。置信度传播公式如下：

步骤四：根据设定的置信度阈值，重复步骤三直到所有协同节点的置信度达到阈值。

步骤五：迭代结束后计算边缘概率分布即可得到当前采样点所有变量节点的最优状态估计。

步骤六：所有节点移动到下一个采样点，重复步骤二到步骤六。

步骤七：节点移动到最后一个采样点，迭代结束，此时计算得到的最优估计即最终定位结果。

图3为本发明中不同状态节点和因子节点组成的多车协同定位因子图。其中

代表车辆根据前一采样点的状态信息和当前IMU模块测量的自信息做出的当前采样点时刻的状态信息估计。IMU测量值一般包括加速度和角速度，分别记为a和ω。可以得到相邻时刻的状态变量之间有如下关系：

x^(t)＝H_IMU(x^(t-1),a^(t-1),ω^(t-1))+n_IMU

其中，H_IMU(·)表示IMU模块的测量函数。

IMU因子可由下式表示：

其中

代表协同网络中节点根据接收到的邻居节点的状态信息以及测距信息对当前状态信息估计的修正。节点之间的无线距离测量值可表示为如下形式：

距离测量因子可以表示为：

在高斯噪声环境下，设计局部函数因子图表示公式为：

本发明提供的方法在卫星遮蔽环境中相较于传统单一车辆定位具有更高的定位精度，并且对于车辆先验位置的可靠性要求更低，当车辆初始估计位置误差较大的条件下也能通过相邻车辆之间的信息交换得到车辆的位置估计，并且利用因子图模型对联合概率密度函数求解，将最大后验估计问题转化为非线性最小二乘问题的相关求解，极大的减小了计算量。

本领域普通技术人员可以理解实现上述实例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)或随机存储记忆体(Random AccessMemory，RAM)等。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (7)

1.一种卫星遮蔽环境中的基于置信度传播的多车协同定位方法，车辆可以通过车-车直连通信向附近协同车辆和路边单元发送本车的绝对位置和本车车载传感器测量得到的相对位置信息，利用因子图对车辆网络协同定位建模，然后通过基于置信度传播的迭代算法对车辆节点的位置进行估计；其特征在于，包括如下步骤：

步骤一：对车辆协同网络中的节点(包括定位车辆和路边单元)位置进行初始化，定义车辆协同网络中N个节点在第t个采样点的状态信息集合为

X^(0:t)＝{X⁽⁰⁾,X⁽¹⁾,L,X^(t)}表示协同网络中从初始采样点到第t个采样点的所有节点的状态信息集合；

步骤二：车辆节点利用车载的惯性导航模块测量得到的车辆加速度与角速度对车辆进行航迹推算，IMU因子节点将置信度传播给相连的变量节点，完成预测过程；

IMU因子节点表示如下：

式中，H_IMU(g)表示IMU模块的测量函数；

步骤三：车辆通过车-车直连通信向邻居节点发送本车的绝对位置和本车车载传感器测量得到的相对位置信息，所有变量节点，将当前置信度传播给相连的因子节点，并且获取邻居变量节点的置信度对自己的置信度进行更新；

因子节点传输给变量节点的置信度可以表示为：

变量节点传输给因子节点的置信度可以表示为：

BI(x_m,f₁)＝BI(f₂,x_m)·BI(f₃,x_m)L BI(f_j,x_m)

其中，f₂、f₃、f_j表示与变量节点x_m相连的因子节点；

步骤四：重复步骤三，使协同网络中所有节点的置信度达到预设的阈值，此时得到当前采样点所有变量节点的最优状态估计，结束迭代并进入步骤五；

步骤五：所有车辆节点移动到下一个采样点，重复步骤二到步骤四；

步骤六：当所有车辆节点移动到最后一个采样点，迭代结束，此时计算得到的边缘概率即最终的车辆最优位置估计；

节点位置估计方程为：

。

2.根据权利要求1所述的一种卫星遮蔽环境中的基于置信度传播的多车协同定位方法，其特征在于，所述步骤一中的对车辆协同网络中的节点(包括定位置进行初始化，并且设定定位车辆先验概率作为初始置信度，并且建立多车协同定位的因子图模型；具体如下：

车辆协同网络中N个节点(包括车辆节点和RSU节点)在第t个采样点的状态信息集合为

表示m号车辆在t时刻的邻居节点集合，

表示车辆在t时刻得到的观测量集合，X^(0:t)＝{X⁽⁰⁾,X⁽¹⁾,L,X^(t)}表示协同网络中从初始采样点到第t个采样点的所有节点的状态信息集合，同理Z^(1:t)＝{Z⁽¹⁾,Z⁽²⁾,L,Z^(t)}表示协同网络中从第1个采样点到第t个采样点的所有节点的测量信息集合；根据上述定义，协同网络中的节点状态信息的联合概率分布函数为：

p(X^(0:t)|Z^(1:t))

通过计算一段时间内状态信息的联合概率密度函数的最大后验估计，即可得到状态信息的最优估计。车辆协同网络中节点状态信息的最大后验估计为：

3.根据权利要求1所述的一种卫星遮蔽环境中的基于置信度传播的多车协同定位方法，其特征在于，所述步骤二：车辆节点利用车载的惯性导航模块测量得到的车辆加速度与角速度对车辆进行航迹推算，IMU因子节点将置信度传播给相连的变量节点，完成预测过程；具体如下：

用

表示车辆根据前一采样点的状态信息和当前IMU模块测量的自信息做出的当前采样点时刻的状态信息估计。IMU测量值一般包括加速度和角速度，分别记为a和ω；可以得到相邻时刻的状态变量之间有如下关系：

x^(t)＝H_IMU(x^(t-1),a^(t-1),ω^(t-1))+n_IMU

其中，H_IMU(g)表示IMU模块的测量函数，IMU因子可由下式表示：

4.根据权利要求1所述的一种卫星遮蔽环境中的基于置信度传播的多车协同定位方法，其特征在于，所述步骤三：车辆通过车-车直连通信向邻居节点发送本车的绝对位置和本车车载传感器测量得到的相对位置信息，所有变量节点，将当前置信度传播给相连的因子节点，并且获取邻居变量节点的置信度对自己的置信度进行更新；具体如下：

用

表示协同网络中节点根据接收到的邻居节点的状态信息以及测距信息对当前状态信息估计的修正。节点之间的无线距离测量值可表示为如下形式：

距离测量因子可以表示为：

因子节点传输给变量节点的置信度可以表示为：

变量节点传输给因子节点的置信度可以表示为：

BI(x_m,f₁)＝BI(f₂,x_m)·BI(f₃,x_m)L BI(f_j,x_m)

其中，f₂、f₃、f_j表示与变量节点x_m相连的因子节点。

5.根据权利要求1所述的一种卫星遮蔽环境中的基于置信度传播的多车协同定位方法，其特征在于，所述步骤四：重复步骤三，使协同网络中所有节点的置信度达到预设的阈值，此时得到当前采样点所有变量节点的最优状态估计，结束迭代并进入步骤五。

6.根据权利要求1所述的一种卫星遮蔽环境中的基于置信度传播的多车协同定位方法，其特征在于，所述步骤五：所有车辆节点移动到下一个采样点，重复步骤二到步骤四。

7.根据权利要求1所述的一种卫星遮蔽环境中的基于置信度传播的多车协同定位方法，其特征在于，所述步骤六：当所有车辆节点移动到最后一个采样点，迭代结束，此时计算得到的边缘概率即最终的车辆最优位置估计；具体如下：

中节点之间的IMU观测信息相互独立，因此IMU测量部分可以简化为：

而

中节点的观测信息只与当前节点以及相邻节点的状态信息相关，与其他非相邻节点的状态和观测相互独立，因此可以进一步简化为：

综上可得协同网络中定位车辆的状态优化方程为：

。

Images