一种利用道路平面约束RTK定位的方法

一种利用道路平面约束rtk定位的方法
技术领域
1.本发明设计属于卫星导航领域，具体涉及到一种利用道路平面约束rtk定位的方法。


背景技术：

2.全球卫星导航系统精密动态实时相对定位，简称rtk(real time kinematic)，通过对接收机间和星间做差消除卫星轨道误差、大气传播延迟误差以及卫星和接收机钟差等误差源，再利用高精度的载波相位观测量，可以实现厘米级甚至毫米级精度的定位。rtk定位具有高精度、高可靠性、以及高实时性的特点，在民用和军事领域具有广泛的应用。例如rtk可应用于大地测量、建筑物和基础设施变形监测、车道级高精度汽车导航、无人驾驶汽车导航、飞行器定姿、飞机精密进近着陆、舰载机精密进近着舰等。实现rtk定位的关键是正确解算载波相位整周模糊度。
3.协同定位是随着车联网兴起的一种基于车辆自组网的定位技术。车辆在进行协同定位时不仅可以利用本地的测量数据，还可以利用协同车辆共享的测量数据。相比于单车多传感器的定位方法，协同定位方法具有以下优势：来自协同节点的测量值能够改善几何精度因子，提升定位的精度；额外的测量值可以增加观测的维度和定位方程的数量，提升定位的可用性；大多协同定位方法无需激光雷达等昂贵设备，主要利用车载通信及低成本的测距单元，实现成本很低；不涉及复杂的图像处理和数据匹配，占用的车载资源较小，可保证定位的实时性。现有协同定位方法均是在车间测量的基础上实现的，目前尚没有协同定位方法能够脱离车间测量实现。然而由于安全、隐私、权限以及设备成本等因素约束，并非所有协同车辆均具备提供原始测量信息的能力，待定位车辆可能无法获得协同车辆的测量数据。另外，若协同测量数据在传输过程中存在较大的延迟，测量数据也不能用于当前时刻的定位解算。
4.不同于测量域协同定位方法，定位域协同定位方法只利用协同车辆的位置数据来实现定位解算。因此提出一种定位域的协同定位方法，利用协同车辆的定位解来约束rtk定位的结果，进而解决车间测量失效时协同定位方法不可用的问题具有重要现实意义。


技术实现要素：

5.本发明的目的在于利用协同车辆的定位解来构建道路平面，以此约束rtk定位的结果。基于gnss载波相位测量的rtk以及n-rtk定位技术在成功固定载波相位模糊度的情况下可以为车辆用户提供厘米级定位解，然而该技术依赖于数量充足且观测质量良好的卫星伪距和载波相位测量值，在城市密集环境下，高楼、桥梁甚至树木的遮挡都将导致卫星观测的降级，当健康卫星测量值的数量变少时，rtk以及n-rtk的浮点解精度会降低，并且难以固定载波相位模糊度，定位解中浮点解占比的提升以及浮点解精度的下降使得车辆整体定位精度降级，难以满足高级别车辆应用的定位精度需求。
6.本发明提出一种定位域的协同定位方法以解决车间测量失效时协同定位方法不
可用的问题，利用协同车辆的定位解来构建道路平面，以此约束rtk定位的结果。
7.本发明提出的利用道路平面约束rtk定位的方法，其实施步骤如下：
8.步骤一：提出基于协同车辆位置信息的道路平面构建方法，包括基于奇异值分解的平面拟合方法以及基于拟合残差的平面检测方法。
9.步骤二：根据步骤一所得到的拟合道路平面，提出平面约束的高精度定位方法，设计基于平面约束的浮点估计器，利用平面约束rtk浮点估计过程。
10.步骤三：利用lambda算法解算载波相位模糊度以得到高精度的固定解。
11.其中，在步骤一中所述的“基于协同车辆位置信息的道路平面构建方法”，包括道路平面构建、平面检测以及车辆天线高补偿，其做法如下：
12.s11、对协同车辆定位数据及天线高度进行处理。平面构建过程以及定位解算过程统一使用ecef坐标系。为消除天线高的影响，在平面拟合前先将车辆定位结果转化到enu坐标系下，计算得到协同车辆gnss天线相位中心于enu坐标系下的坐标，获得车辆天线在道路上的投影坐标；
13.s12、计算车辆在路面的投影位置与基站位置之间的基线。利用坐标系转化矩阵s
′
将enu坐标转为ecef坐标，将b
″
i,k
表示为ecef坐标系下车辆在路面的投影位置与基站位置之间的基线，其中k表示第k个历元，i表示可获得第i辆协同车的位置信息；
14.s13、选取合适的车辆数据。在s12中计算得到的基线位置信息，生成一组用于拟合平面的点集set
fitting,k
＝{b
″
1,k
,...,b
″
n,k
}。在本发明中采用连续多个历元的协同车辆定位数据来拟合平面，具体地，默认选用最接近当前时刻的连续5个历元的数据，用set
fitting
来表示由这些历元的定位数据所构成的点集；
15.s14、拟合车辆行驶所在的平面α。本发明采用经典的最小二乘法对车辆行驶的平面进行拟合。
16.s15、选取拟合平面的最优解。本发明提出一种利用奇异值分解的方法来寻找最优解；
17.s15、为了后续目标车的gnss定位，需要将目标车的天线高引入道路平面方程中，进而构建一个平行于路面并通过目标车gnss天线相位中心的平面β；
18.s16、为验证拟合的平面能否满足车辆行驶于同一路面并且定位解足够精确这一要求，采用平面拟合的平均残差来检测平面的可用性，一个拟合的平面是否可用通过比较拟合残差和设定的门限值来判定；
19.其中，在步骤二中所述的“设计基于平面约束的浮点估计器，利用平面约束rtk浮点估计过程”，其做法如下：
20.s21、将利用协同车位置信息得到的平面的参数视为新的观测量，与gnss双差观测模型结合；
21.s22、构建卡尔曼滤波待估计的状态向量，除了三维位置、速度、加速度，还有双差载波相位的模糊度，即其中前三项代表ecef坐标系下的基线、速度和加速度，最后一项是双差模糊度向量，其维度取决于双差载波相位测量值的数量；
22.s23、构建卡尔曼滤波的观测向量，包括gnss的伪距双差测量值、载波相位双差测量值以及平面方程的参数；
23.s23、构建观测矩阵h，h0和hg均是与双差测量值有关的观测子矩阵，而h
p
是与平面约束有关的观测子矩阵，h
p
中的元素pa、pb和pc是步骤1得到的平面法向量参数；
24.s24、估计过程噪声矩阵q和测量噪声矩阵r，矩阵q为常矩阵，估计并更新其中一个噪声矩阵，即r矩阵；
25.s25、估计器存在发散的风险，即当q矩阵是负定的或者r矩阵是半负定时，且偶尔发生于多径严重区域。当出现上述情况时，本发明采用传统的卡尔曼滤波噪声矩阵设定方法
26.其中，在步骤三中所述的“利用lambda算法解算载波相位模糊度以得到高精度的固定解”属于经典方法，在本发明中不再赘述。
27.通过上述步骤，利用协同车辆的定位解来构建道路平面，以此约束rtk定位的结果，引入的路面信息可同时对车辆水平和垂直方向的定位结果进行约束，在定位域上对浮点解进行修正，提高浮点解的精度，进而提升载波相位模糊度固定的成功率。
28.依据本发明的设计，本发明实现了只利用车间通信设备以获取协同车辆的位置数据，实现成本低，同时该方法可扩展性较强，可以与多传感器方法相结合，进一步提升定位精度与可用性。
29.依据本发明的设计，本发明无需协同车辆的gnss原始数据以及车间无线测距值等需要时间同步的车间测量数据，只利用协同车辆的历史位置数据即可实现协同定位，相比现有测量域协同定位方法，适用范围更广，可用性更高。
30.依据本发明的设计，本发明所提方法无需存储并处理数据量庞大的地图信息，且利用的是最近时刻的协同车辆定位数据。高精度地图也具备提供道路平面模型数据的能力，但是高精度地图信息需要不断地维护和更新以修正随时间推移道路产生的变化，因此本发明环境适应性强，资源消耗小。
附图说明
31.图1是本发明实施例的定位过程图。
32.图2是本发明实施例的方法架构图。
具体实施方式
33.为了能对本发明的特征、目的及功能有更进一步的认知与了解，现结合具体实施实例和附图对本发明进行更详细的描述。
34.首先概述本发明的基本场景和核心原理。多辆车行驶在一条路面平整的道路上，车辆间可以通过v2x链路共享各自的实时位置信息，具体地，这里的位置指的是车辆gnss主天线相位中心的位置，除了车辆位置外，车辆间还会共享各自gnss天线距地面的高度信息。当一辆车接收到协同车辆发送的位置信息和天线高度信息后，该车可以利用这些数据来拟合一个车辆行驶的平面。为了便于理解所提方法的实现流程，在本发明中定义了一个目标车辆作为研究对象，其余车辆被视为它的协作者，由于该方法是分布式的，在实际应用中每辆车均可以使用本发明所提方法。
35.如图2所示，是本次发明的方法架构图，可分为基于协同数据的平面构建和平面约束的高精度定位两部分，其中平面约束的高精度定位又可分为浮点估计和整周模糊度解
算。平面构建是所提方法实现的基础，包括道路平面构建、平面检测以及车辆天线高补偿，这一步的输出为平面的三个参数。平面约束下的浮点估计是所提方法的核心，采用自适应卡尔曼滤波来实现浮点解算，通过构建混合观测模型，平面拟合中得到的平面参数将作为一种新的测量值与gnss双差观测量进行融合，输出为待定位车辆的浮点解。最后的模糊度解算采用经典lambda算法，在通过比率检测的情况下，定位系统将输出高精度的固定解，输出结果可作为历史定位解用于其他车辆的协同定位。具体实施如下：
36.第一步：利用协同车辆位置信息构建道路平面。如图2所示，基于协同数据平面构建主要包括协同车辆定位数据及天线高度处理和目标车辆天线距离地面高度处理这两部分。本发明涉及到数据的坐标系有两种，一个是地心地固坐标系(ecef)，另一个是东北天坐标系(eastnorthup，enu)。平面构建过程以及定位解算过程统一使用ecef坐标系，具体地，本发明使用wgs84坐标系。
37.(1)将车辆定位结果转化到enu坐标系下
38.将ecef至enu的坐标系转化矩阵定义为s，enu的坐标原点为基准站的gnss天线相位中心，基准站的坐标精确已知。s的计算过程如下：
[0039][0040]
这里，代表格林威治参考子午面与经过基准站天线相位中心的子午面之间的夹角。φ为赤道面与经过基准站天线相位中心的椭球面法线的夹角，可以由迭代计算得到：
[0041][0042][0043][0044][0045]
其中a和b分别代表地球基准椭球体的长半轴和短半轴长度，e为地球的椭球偏心率，n是基准椭球体的卯酉圆曲率半径。计算φ时，初值设为0，然后按照上述公式计算并更新，直至其收敛。
[0046]
(2)获取可拟合平面的车辆数据集合
[0047]
通过转化矩阵s，可以计算得到协同车辆gnss天线相位中心于enu坐标系下的坐标，进而获得车辆天线在道路上的投影坐标：
[0048]b′
i,k
＝s
·bi,k-[0；0；hi]
[0049]
这里b
i,k
＝x
i,k-xr代表ecef坐标系下车辆i与基准站之间的基线，协同车辆i在第k个历元的ecef坐标记为x
i,k
，基准站在ecef下的坐标为xr，hi为车i的gnss天线距路面的高
度。通过上述公式，即可获得协同车i在道路平面投影位置，接下来利用坐标系转化矩阵s
′
将enu坐标转为ecef坐标：
[0050]b″
i,k
＝s
′
·b′
i,k
[0051]
这里b
″
i,k
代表ecef坐标系下车辆在路面的投影位置与基站位置之间的基线，在后续平面拟合和定位过程中，我们均采用车辆与基站之间的相对位置(即基线)。
[0052]
假设目标车辆在第k个历元可以获得来自n辆协同车的位置信息，这样就得到了一组用于拟合平面的点集set
fitting,k
＝{b
″
1,k
,...,b
″
n,k
}。通常，只用一个历元的数据来拟合平面是不够的，本发明采用连续多个历元的协同车辆定位数据来拟合平面，具体地，默认选用了最接近当前时刻的连续5个历元的数据，并用set
fitting
来表示由这些历元的定位数据所构成的点集，默认使用这个集合内所有的定位解来拟合平面。
[0053]
(3)利用已知数据拟合平面
[0054]
本发明采用了经典的最小二乘法来拟合车辆行驶所在的平面α。这里假设若干用于拟合平面的点坐标b
″
i,k
＝(x
″
i,k
,y
″
i,k
,z
″
i,k
)，假设拟合的平面为
[0055]
α:pax pby pcz pd＝0
[0056]
其中pa、pb和pc为平面法向量。给定一个约束条件，将平面法向量坐标归一化：
[0057]
p
a2
p
b2
p
c2
＝1
[0058]
假设所有用于平面拟合的定位解的平均坐标为那么
[0059][0060][0061]
设定一个矩阵a为：
[0062][0063]
这里，n表示拟合点的数量，x＝[pa；pb；pc]，方程等价于ax＝0。
[0064]
在实际中由于道路不是严格意义的平面，而且定位数据存在误差甚至较大偏差，上式并不成立，因此拟合的本质就是寻找一个平面，使得所有用于拟合的点与该平面距离之和最小，即设定一个目标函数min||ax||，其中||x||＝1。本发明采用了奇异值分解的方法来寻找最优解，假设a可做奇异值分解：
[0065]
a＝udv
t
[0066]
其中u和v是酉矩阵，d属于对角阵，那么||ax||＝||udv
t
x||＝||dv
t
x||，这里v
t
x是列矩阵，并满足||v
t
x||＝||x||＝1。
[0067]
由于d的对角元素是奇异值，假设其最后一个对角元素对应最小奇异值，那么当且仅当v
t
x＝[0；0；
…
；1]时，||ax||取值最小，这时
[0068]
[0069]
因此，min||ax||的最优解是最小奇异值对应的特征向量，这个特征向量即为拟合平面的法向量：
[0070][0071]
接下来计算pd：
[0072][0073]
(4)构建平行于路面并通过目标车gnss天线相位中心的平面β
[0074]
目标车天线所在平面可表示为：
[0075]
β:pax pby pcz p
′d＝0
[0076]
其中，
[0077][0078]
这里h
target
代表目标车的天线高。
[0079]
第二步：采用平面拟合的平均残差来检测平面的可用性
[0080]
具体计算方式如下：
[0081][0082]
其中，
[0083][0084]
这里n表示协同车数量，k
start
和k
end
分别表示拟合时所用定位解的开始和结束历元，d
i,k
代表一个点b
″
i,k
＝(x
″
i,k
,y
″
i,k
,z
″
i,k
)与拟合平面之间的距离，如果所有协同车都行驶于一个平面内并且它们的定位解足够准确，那么拟合平面的平均残差值将会非常小，否则残差将会变大。因此，一个拟合的平面是否可用可以通过比较拟合残差和设定的门限值来判定：
[0085][0086]
其中t
fitting
代表平面检测的阈值，阈值的大小取决于平面拟合残差的分布和虚警率。
[0087]
第三步：设计rtk的浮点估计器
[0088]
(1)确定卡尔曼滤波待估计的状态向量
[0089]
卡尔曼滤波状态向量包括待估计车辆的三维位置、速度、加速度，还有双差载波相位的模糊度：
[0090]
[0091]
其中，前三项分别代表ecef坐标系下的基线、速度和加速度，最后一项是双差模糊度向量，其维度取决于双差载波相位测量值的数量。
[0092]
(2)预测
[0093]
浮点估计的预测过程表述如下：
[0094]
x
k,k-1
＝fx
k-1
ωk,ωk～n(0,q)
[0095]
p
k,k-1
＝fp
k-1ft
q
[0096]
其中，x
k,k-1
是由上一历元预测得到的当前历元的状态向量，x
k-1
是上一历元估计的状态向量，f是状态转换矩阵。ωk和q代表预测过程的噪声和过程噪声的协方差矩阵，p
k,k-1
代表预测误差的协方差矩阵，p
k-1
为上一历元的估计误差矩阵。
[0097]
浮点估计的状态转移矩阵可展开如下：
[0098]
f＝diag(f1,f2)
[0099][0100]
f2＝diag(im)
[0101]
其中τ代表卡尔曼滤波的更新周期，im是维度为m的单位矩阵。
[0102]
(3)测量更新
[0103]
卡尔曼滤波的观测模型可表示为：
[0104]
zk＝hxk vk,vk～n(0,r)
[0105]
其中，zk代表当前历元的观测向量，h代表观测矩阵，vk和r分别代表测量噪声和测量噪声的协方差矩阵。根据上一小节推导的gnss双差观测模型以及平面约束模型，观测矩阵可表示为：
[0106][0107]
其中，
[0108][0109]
hg＝diag([...,λ,...]1×m)
[0110]hp
＝[p
a p
b pc]
[0111]
上述公式中ui代表由车辆指向卫星i的单位方向矢量，u
p
中的下角标p指的是双差选定的主卫星。m为当前历元双差测量值的数量，这里假定双差伪距测量值的数量与双差载波相位测量值的数量一致，h0和hg均是与双差测量值有关的观测子矩阵，而h
p
是与平面约束有关的观测子矩阵，h
p
中的元素pa、pb和pc是上一节计算得到的平面法向量参数。
[0112]
这样，卡尔曼滤波增益可表示为：
[0113]
[0114]
测量更新如下：
[0115]
xk＝x
k,k-1
kk(z
k-hkx
k,k-1
)
[0116]
pk＝(i-k
khk
)p
k,k-1
[0117]
其中，xk为当前历元估计的状态向量，pk为当前历元的估计误差协方差矩阵。至此本发明实现了对浮点解的估计，获得了浮点状态向量以及浮点模糊度，下一步是固定整周模糊度并修正浮点解。
[0118]
第四步：噪声矩阵选取
[0119]
本发明采用基于信息的自适应卡尔曼滤波来更新测量噪声矩阵r，值得注意的是，如果同时估计过程噪声矩阵q和测量噪声矩阵r，则估计器可能被它们之间的相关性所误导而发散，为了避免这个问题，在实例中只估计并更新其中一个噪声矩阵，即r矩阵，而另一个矩阵q仍未常矩阵。
[0120]
新息被定义为观测值与预测值之差，具体的新息向量可表示为：
[0121]ek
＝z
k-hkx
k,k-1
[0122]
新息序列的协方差矩阵可表示为：
[0123][0124]
在实际使用时，上述矩阵的计算方式如下：
[0125][0126][0127]
其中k表示当前历元，l是新息序列的窗口长度。如果用于估计自适应噪声矩阵的方程数量小于噪声矩阵中未知参数的数量，估计过程将面临着发散的风险。因此，窗口长度不能过小，当更新测量噪声矩阵r时，窗口长度应大于测量值的数量，当更新过程噪声矩阵q时，窗口长度应大于滤波器状态向量维度。
[0128]
理论上，新息协方差矩阵应满足如下关系：
[0129][0130]
q和r矩阵可由下式计算得到：
[0131][0132][0133]
尽管这里同时给出了q和r的估计方法，在使用自适应滤波时本发明只更新r矩阵而q矩阵仍设定为常矩阵。值得注意的是，如果q矩阵是负定的或者r矩阵是半负定的，估计器则有发散的风险，这种现象偶尔发生于多径严重区域。当出现上述情况时，本发明仍采用传统的卡尔曼滤波噪声矩阵设定方法。
[0134]
以上所述仅为本技术的实施例而已，并不用于限制本技术。对于本领域技术人员来说，本技术可以有各种更改和变化。凡在本技术的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本技术的权利要求范围之内。