[发明专利]一种无人车队与路侧单元协同工作的相对定位方法

权利要求书

1.一种无人车队与路侧单元协同工作的相对定位方法，其特征在于，包括以下步骤：

步骤1、在无人车行驶区域的道路网中部署两个路侧单元，作为已知固定点，在无人车队的每一辆车上安装车载单元，建立基于车载惯导系统、车载数据链以及相对定位处理器的无人车队相对定位系统；

步骤2、确定相对定位时无人车队相邻车辆之间、车辆与路侧单元之间的信息交互内容，将车载惯导系统的数据传送至车载数据链，车载数据链利用无线电测距的方法测量无人车队中车辆之间的距离，同时利用无线电通信实时通报惯导数据和测距信息，并将测距信息和惯导数据传送给相对定位处理器；

步骤3、以各个无人车的惯性导航系统的误差作为待估计量，实测的车辆之间的距离与由惯导导航信息计算得出的距离之差为量测量，建立在无GNSS的情况下无人车队的相对定位方法的数学模型；然后用非线性最小二乘法与牛顿迭代法，求出各个惯导系统误差的估计值，再利用车辆惯导位置误差估计值反馈，修正惯导输出位置，完成车辆定位；

步骤3中所述的建立在无GNSS的情况下无人车队的相对定位方法的数学模型，具体如下：

设定无人车队中车的数量为n，[x_am,y_am]为车辆m惯导系统的输出位置坐标，[x_tm,y_tm]为车辆m的真实坐标，x_em,y_em为车辆m的惯导位置误差，m＝1,…,n，那么[x_am,y_am]与[x_tm,y_tm]以及[x_em,y_em]的关系为：

车辆i与车辆j之间相互测距值为l_i,j，i,j∈{1,…,n}，由惯性导航系统输出的车辆坐标计算得到的计算距离d_i,j表示为：

将(1)带入(2)式得到：

令在同一时刻对任意两无人车或路侧单元之间的距离进行测定，一共组数据，这些数据集中在一起组成以下的个方程式，即为无人车队相对定位的数学模型：

设定解x＝(x_e1,y_e1,x_e2,y_e2,…,…,x_en,y_en)的初始值为将式中的每一方程在处线性化；在参数x的估算中，两侧方程组共有2n个未知参数，根据数学原理，至少需要2n个非线性相关的量测方程，而在无人车队相对定位的数学模型中，最多有个相互独立的量测方程，因此，在没有绝对定位数据的情况下，无人车队中至少需要5个车辆节点才能实现相对定位；在有两个位置已知且固定的路侧单元的情况下，两侧方程组共有2n-4个未知参数，根据数学原理，至少需要2n-4个非线性相关的量测方程，而在无人车队相对定位的数学模型中，最多有个相互独立的量测方程，因此，无人车的数量要大于1辆，即车队中至少应有两辆无人车才能进行准确定位；

方程组中的第个方程的泰勒展开式为：

式中，为函数对x_en-1的偏导在处的值，即

非线性方程组(4)近似转化为用以下矩阵形式表达的线性方程组：

G·Δx＝b (7)

式中，

x_k＝x_k-1+Δx (11)

Δx＝(G^TG)^-1G^Tb (12)

式中Δx即为矩阵方程式(7)的非线性最小二乘法解，目标节点的真实坐标即为[x_am-x_em,y_am-y_em]，从而完成惯导定位的误差修正。

2.根据权利要求1所述的无人车队与路侧单元协同工作的相对定位方法，其特征在于，步骤1中所述的建立基于车载惯导系统、车载数据链以及相对定位处理器的无人车队相对定位系统，具体如下：

在无人车行驶区域的道路网中部署两个路侧单元，在无人车队的每一辆车上安装车载单元，车载单元和路侧单元都具有无线通信模块，路侧单元覆盖范围内的系统网络有两种基本的通信，一是无人车与无人车之间的通信，二是无人车与路侧单元这种路旁基础设施之间的通信，利用无线通信技术，使行驶中的车辆互相通信，并通过路侧单元访问外部网络。