[0001]
本发明涉及计量技术领域，尤其涉及一种里程计标度误差标定方法、系统及计算机存储介质。
背景技术：[0002]
在自动驾驶车辆上，往往使用惯导与里程计组合来提供更高的导航精度，避免纯惯导误差随时间的增长。里程计通过输出位置增量信息，利用航位推算算法可实现完全自主导航，但是里程计本身受车辆轮胎磨损度、轮胎气压以及车辆载荷等因素影响，会导致标度误差发生变化，影响组合导航的精度，因此需要对里程计的标度误差进行标定。
[0003]
传统标定方法主要分为零速修正、轨迹相似性校正以及gps辅助标定。零速修正是利用惯导作为位置基准，采用经过零速修正的惯导信息来标定里程计标度因数。但是这种方法需要周期性的停车来实现校准，使用不方便，不能满足实际应用的需求；轨迹相似性标定方法是利用高精度基准轨迹来对比里程计的轨迹从而实现标度因数误差的标定，该方法精度较好，但难点在于高精度基准轨迹的获取；gps辅助标定是利用gps提供的精度很高的位置信息作为参考，但是受限于不可依赖性，在gps信号很弱或者没有的地方标定效果不是很好。
技术实现要素：[0004]
本发明主要目的在于公开一种里程计标度误差标定方法、系统及计算机存储介质，以实现高精度的导航。
[0005]
为达上述目的，本发明公开一种里程计标度误差标定方法，包括：
[0006]
通过惯导和rtk(real-time kinematic，载波相位差分技术)组合获得基准轨迹；
[0007]
通过里程计计算载车水平轨迹；
[0008]
对比基准轨迹和水平轨迹，估算出所述里程计的标度误差。
[0009]
优选地，所述基准轨迹的求解过程包括：
[0010]
定义惯导载体横滚轴为y
b
轴，指向惯导正前方，俯仰轴为x
b
轴，指向惯导右方，航向轴为z
b
轴，沿重力方向指向朝天，x
b
、y
b
、z
b
三个轴符合右手坐标系，将x
b
、y
b
、z
b
记为b系；
[0011]
定义导航坐标系x
n
轴指向东，y
n
轴指向北，z
n
轴指向朝天，将x
n
、y
n
、z
n
记为n系；
[0012]
里程仪测量坐标系是一个与车体固联的“右-前-上”右手直角坐标系并记为m系，oy
m
轴在和载车车轮相接触的地平面内，并且指向车体的正前方；oz
m
轴垂直于地平面向上为正；ox
m
轴指向右方；
[0013]
选取系统状态参数，将状态量记为：
[0014][0015]
δr为惯导位置误差，δv为惯导速度误差，φ是惯导姿态角误差，是陀螺零偏，δf
b
是加速度计零偏；
[0016]
建立惯导姿态误差方程为：其中，是φ的导数，为导航坐标系下陀螺测量误差，为导航坐标系计算误差，表示导航坐标系相对于惯导坐标系的旋转；
[0017]
为地球自转引起的导航坐标系旋转，为系统在地球表面附近移动因地球表面弯曲引起的导航坐标系旋转；
[0018]
速度误差方程为：其中，v
n
、δv
n
分别为导航坐标系下速度及速度误差，是δv
n
的导数，为导航坐标系下加速度计测量值，为导航坐标系下加速度计测量误差，δg
n
分别为导航坐标系下地球自转角速度计算误差、旋转计算误差和重力误差；
[0019]
惯导位置误差方程为：
[0020][0021]
l、λ和h分别表示纬度、经度和高度，δl、δλ和δh分别表示纬度误差、经度误差和高度误差，和分别为δl、δλ和δh的导数，r
m
为子午圈主曲率半径；r
n
为卯酉圈主曲率半径；
[0022]
记惯导速度分量v
n
＝[v
e v
n v
u
]
t
；
[0023]
速度误差分量δv
n
＝[δv
e
δv
n
δv
u
]
t
；
[0024]
其中，下标e、n、u分别代表东向、北向和天向；
[0025]
在导航坐标系下，惯导和rtk组合导航的状态方程表示为：
[0026][0027]
x(t)为所选的组合导航坐标系统状态向量，是x(t)的导数，f(t)为系统状态转移矩阵，w(t)为系统噪声向量，g(t)为系统噪声驱动矩阵；
[0028][0029]
其中，为b系下加速度计的测量值，表示n系到b系的姿态变换矩阵，()
×
表示反对称矩阵；
[0030][0031][0032][0033][0034][0035]
[0036][0037][0038]
其中，w
a
和w
g
分别表示陀螺仪和加速计的噪声，r为地球视为圆球时的半径，w
ie
为地球自转速率；
[0039]
量测方程由rtk测得的位置和速度与惯导测得的位置和速度分别做差得到，即为：
[0040][0041]
上式中z(t)为t时刻的量测矩阵，b代表经度，l代表纬度，h代表高度，v
e
代表东向速度，v
n
代表北向速度，v
u
代表天向速度，下标rtk和ins分别代表rtk和惯导；v(t)代表rtk的位置和速度的噪声序列；
[0042]
采用卡尔曼滤波来进行最优估计，其计算步骤为：
[0043]
一步预测方程：x
k,k―1
＝φ
k,k―1
x
k―1
；其中，φ
k,k―1
是k-1时刻至k时刻的系统转移矩阵，x
k―1
是k-1时刻的系统状态矢量；
[0044]
状态估计方程为：x
k
＝x
k,k―1
+k
k
(z
k
―z
k,k―1
)；其中，x
k
是k时刻的系统状态矢量，k
k
是k时刻的增益矩阵，z
k
是k时刻的量测量，z
k,k―1
是k-1时刻至k时刻的量测量；
[0045]
增益矩阵的求取方法为：其中，p
k,k―1
是k-1时刻至k时刻的均方误差矩阵，h
k
为k时刻的量测矩阵，r
k
为系统k时刻量测噪声方差矩阵；
[0046]
一步预测均方误差：其中，γ
k―1
为k-1时刻的噪声驱动矩阵，γ
k,k―1
是k-1时刻至k时刻的噪声驱动矩阵，q
k-1
为系统k-1时刻噪声方差矩阵；
[0047]
估计均方误差：p
k
＝(i―k
k
h
k
)p
k,k―1
；其中，p
k
为k时刻的均方误差矩阵；
[0048]
联立上述各方程解算出来的位置轨迹作为基准轨迹，记为s1。
[0049]
优选地，所述载车水平轨迹的求解包括：
[0050]
里程仪的速度输出在里程仪坐标系上表示为：其中，v
d
为里程仪测得的前向速度大小，右向和天向速度均为零，视为载车正常行驶时的速度约束条件，上标m、n、b分别表示对应的坐标系；
[0051]
在导航坐标系下里程仪速度输出为：
[0052]
考虑到m系至b系存在小量的安装偏差角，记偏差角矢量α＝[α
θ α
γ α
ψ
]
t
，以及刻度系数误差δk
d
，则实际在导航坐标系上里程计的输出为：
[0053][0054]
其中，φ
d
为航位推算的姿态失准角，下标ψ、γ、θ分别代表m系到b系的俯仰角误差、横滚角误差以及航向角误差，且φ
d
＝[φ
de φ
dn φ
du
]
t
；忽略水平姿态误差影响，即作近似φ
de
≈φ
dn
≈0，得到：
[0055][0056]
α
θ
、φ
du
+α
ψ
和δk
d
均为常值小量，且载车在地理位置变化不大的范围内行驶，即整个导航过程中导航坐标系的旋转变化不大，当作平面处理，上式两边同时积分得：
[0057][0058]
其中，分别表示在时间段[0,t]内的载车真实位移矢量、计算位移矢量和行驶里程；u
u
＝[0 0 1]
t
为天向单位向量；
[0059]
将上式分解为水平和垂直两部分，得：
[0060][0061][0062]
其中，其中，下标h表示在水平面上的投影；
[0063]
真实位移绕天向轴u
u
转动角度φ
du
+α
ψ
，再扩大1+δk
d
倍，得到计算位移从而计算出水平轨迹s2；进而通过对比轨迹s1和s2估算出里程计的标度误差δk
d
。
[0064]
与上述方法相对应的，本发明还公开一种里程计标度误差标定系统，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，其中，所述处理器执行所述计算机程序时实现上述方法的步骤。
[0065]
相对应地，本发明还公开一种计算机存储介质，其上存储有计算机程序，其中，所述程序被处理器执行时实现上述方法中的步骤。
[0066]
本发明能科学地计算出里程计的标度误差，进而有效提高了里程仪的定位精度，为实现高精度导航提供了保障。
[0067]
下面将参照附图，对本发明作进一步详细的说明。
附图说明
[0068]
构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
[0069]
图1是本发明实施例公开的里程计标度误差标定方法流程示意图。
[0070]
图2是本发明实施例公开的惯导与rtk组合导航示意图。
[0071]
图3是本发明实施例公开的基准路线与里程计实际路线对比示意图。
具体实施方式
[0072]
以下结合附图对本发明的实施例进行详细说明，但是本发明可以由权利要求限定和覆盖的多种不同方式实施。
[0073]
实施例1
[0074]
本实施例公开一种里程计标度误差标定方法，分别建立惯导坐标系、导航坐标系和里程仪测量坐标系，并将惯导坐标系和里程仪测量坐标系中的数据映射到导航坐标系进行下述基准轨迹、水平轨迹及标度误差的求解。
[0075]
如图1所示，本发明方法包括：
[0076]
步骤s1、通过惯导和rtk组合获得基准轨迹。
[0077]
在该步骤中，优选的基准轨迹的求解过程包括：
[0078]
定义惯导载体横滚轴为y
b
轴，指向惯导正前方，俯仰轴为x
b
轴，指向惯导右方，航向轴为z
b
轴，沿重力方向指向朝天，x
b
、y
b
、z
b
三个轴符合右手坐标系，将x
b
、y
b
、z
b
记为b系；
[0079]
定义导航坐标系x
n
轴指向东，y
n
轴指向北，z
n
轴指向朝天，将x
n
、y
n
、z
n
记为n系；
[0080]
里程仪测量坐标系是一个与车体固联的“右-前-上”右手直角坐标系并记为m系，oy
m
轴在和载车车轮相接触的地平面内，并且指向车体的正前方；oz
m
轴垂直于地平面向上为正；ox
m
轴指向右方；
[0081]
选取系统状态参数，将状态量记为：
[0082][0083]
δr为惯导位置误差，δv为惯导速度误差，φ是惯导姿态角误差，是陀螺零偏，δf
b
是加速度计零偏；
[0084]
建立惯导姿态误差方程为：其中，是φ的导数，为导航坐标系下陀螺测量误差，为导航坐标系计算误差，表示导航坐标系相对于惯导坐标系的旋转；
[0085]
为地球自转引起的导航坐标系旋转，为系统在地球表面附近移动因地球表面弯曲引起的导航坐标系旋转；
[0086]
速度误差方程为：其中，v
n
、δv
n
分别为导航坐标系下速度及速度误差，是δv
n
的导数，为导航坐标系下加速度计测量值，为导航坐标系下加速度计测量误差，δg
n
分别为导航坐标系下地球自转角速度计算误差、旋转计算误差和重力误差；
[0087]
惯导位置误差方程为：
[0088][0089]
l、λ和h分别表示纬度、经度和高度，δl、δλ和δh分别表示纬度误差、经度误差和高度误差，和分别为δl、δλ和δh的导数，r
m
为子午圈主曲率半径；r
n
为卯酉圈主曲率半径；
[0090]
记惯导速度分量v
n
＝[v
e v
n v
u
]
t
；
[0091]
速度误差分量δv
n
＝[δv
e δv
n δv
u
]
t
；
[0092]
其中，下标e、n、u分别代表东向、北向和天向；
[0093]
在导航坐标系下，惯导和rtk组合导航的状态方程表示为：
[0094][0095]
x(t)为所选的组合导航坐标系统状态向量，是x(t)的导数，f(t)为系统状态转移矩阵，w(t)为系统噪声向量，g(t)为系统噪声驱动矩阵；
[0096][0097]
其中，为b系下加速度计的测量值，表示n系到b系的姿态变换矩阵，()
×
表示反对称矩阵；
[0098][0099]
[0100][0101][0102][0103][0104][0105][0106]
其中，w
a
和w
g
分别表示陀螺仪和加速计的噪声，r为地球视为圆球时的半径，w
ie
为地球自转速率；
[0107]
量测方程由rtk测得的位置和速度与惯导测得的位置和速度分别做差得到，即为：
[0108][0109]
上式中z(t)为t时刻的量测矩阵，b代表经度，l代表纬度，h代表高度，v
e
代表东向速度，v
n
代表北向速度，v
u
代表天向速度，下标rtk和ins分别代表rtk和惯导；v(t)代表rtk的位置和速度的噪声序列；
[0110]
如图2所示，本实施例采用卡尔曼滤波来进行最优估计，其计算步骤为：
[0111]
一步预测方程：x
k,k―1
＝φ
k,k―1
x
k―1
；其中，φ
k,k―1
是k-1时刻至k时刻的系统转移矩阵，x
k―1
是k-1时刻的系统状态矢量；
[0112]
状态估计方程为：x
k
＝x
k,k―1
+k
k
(z
k
―z
k,k―1
)；其中，x
k
是k时刻的系统状态矢量，k
k
是k时刻的增益矩阵，z
k
是k时刻的量测量，z
k,k―1
是k-1时刻至k时刻的量测量；
[0113]
增益矩阵的求取方法为：其中，p
k,k―1
是k-1时刻至k时刻的均方误差矩阵，h
k
为k时刻的量测矩阵，r
k
为系统k时刻量测噪声方差矩阵；
[0114]
一步预测均方误差：其中，γ
k―1
为k-1时刻的噪声驱动矩阵，γ
k,k―1
是k-1时刻至k时刻的噪声驱动矩阵，q
k-1
为系统k-1时刻噪声方差矩阵；
[0115]
估计均方误差：p
k
＝(i―k
k
h
k
)p
k,k―1
；其中，p
k
为k时刻的均方误差矩阵；
[0116]
联立上述各方程解算出来的位置轨迹作为基准轨迹，记为s1。
[0117]
作为一种变形，本实施例在获得基准轨迹的过程中，如图2所示基于卡尔曼滤波进行最优估计的方法也可以呗替换为本领域技术人员容易想到的其他的滤波方法。
[0118]
步骤s2、通过里程计计算载车水平轨迹。
[0119]
优选地，该步骤载车水平轨迹的求解包括：
[0120]
里程仪的速度输出在里程仪坐标系上表示为：其中，v
d
为里程仪测得的前向速度大小，右向和天向速度均为零，视为载车正常行驶时的速度约束条件，上标m、n、b分别表示对应的坐标系；
[0121]
在导航坐标系下里程仪速度输出为：
[0122]
考虑到m系至b系存在小量的安装偏差角，记偏差角矢量α＝[α
θ α
γ α
ψ
]
t
，以及刻度系数误差δk
d
，则实际在导航坐标系上里程计的输出为：
[0123][0124]
其中，φ
d
为航位推算的姿态失准角，下标ψ、γ、θ分别代表m系到b系的俯仰角误差、横滚角误差以及航向角误差，且φ
d
＝[φ
de φ
dn φ
du
]
t
；忽略水平姿态误差影响，即作近似φ
de
≈φ
dn
≈0，得到：
[0125]
[0126]
α
θ
、φ
du
+α
ψ
和δk
d
均为常值小量，且载车在地理位置变化不大的范围内行驶，即整个导航过程中导航坐标系的旋转变化不大，当作平面处理，上式两边同时积分得：
[0127][0128]
其中，分别表示在时间段[0,t]内的载车真实位移矢量、计算位移矢量和行驶里程；u
u
＝[0 0 1]
t
为天向单位向量；
[0129]
将上式分解为水平和垂直两部分，得：
[0130][0131][0132]
其中，其中，下标h表示在水平面上的投影；
[0133]
真实位移绕天向轴u
u
转动角度φ
du
+α
ψ
，再扩大1+δk
d
倍，得到计算位移从而计算出水平轨迹s2；进而通过对比轨迹s1和s2估算出里程计的标度误差δk
d
。
[0134]
步骤s3、对比基准轨迹和水平轨迹，估算出所述里程计的标度误差。
[0135]
在该步骤中，如图3所示，基准轨迹s1如图中的线断oa，水平轨迹记s2如图中的虚线断ob。
[0136]
实施例2
[0137]
与上述方法相对应的，本实施例公开一种里程计标度误差标定系统，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，其中，所述处理器执行所述计算机程序时实现上述方法的步骤。
[0138]
实施例3
[0139]
同理，与上述方法实施例相对应的，本实施例公开一种计算机存储介质，其上存储有计算机程序，其中，所述程序被处理器执行时实现上述方法中的步骤。
[0140]
综上，本发明上述各实施例所分别公开的里程计标度误差标定方法、系统及计算机存储介质，能科学地计算出里程计的标度误差，进而有效提高了里程仪的定位精度，为实现高精度导航提供了保障。
[0141]
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。