admin管理员组文章数量:1660165
车辆安装角标定
- 前言
- 一、基本流程图
- 二、基本原理
- 1.安装角的卡尔曼滤波器
- 2.航位推算
- 3.状态模型
- 4.量测模型
- 三、结果
- 总结
前言
纯IMU解算位置发散太快,IMU/ODO能够一定程度上减缓发散的速度,本质是利用IMU的姿态和里程计的速度进行航位推算,但实际中惯组坐标系与车体坐标系存在安装偏角,必须进行安装角的标定。
一、基本流程图
最初的想法是在直线段标定安装角,IMU可以获得自身姿态,全站仪全程跟踪获得轨迹不同时刻的航向角、俯仰角,然后将两者的数值相减得到安装角。但这样算出来的结果很粗糙,误差很大。最后查找文献,确定了现在的安装角标定方法,基本流程如下图。
二、基本原理
算法部分原理参考陈起金的论文,
CHEN Q, ZHANG Q, NIU X. Estimate the Pitch and Heading Mounting Angles of the IMU for Land Vehicular GNSS/INS Integrated System[J]. IEEE Transactions on Intelligent Transportation Systems, 2021, 22(10): 6503-6515.
论文中有开源代码地址。开源代码仅有估计安装角的部分,双向平滑部分需要自己编写,可以参考PSINS工具箱。
http://www.psins/
这里我将估计的维数降为15维。另外两部分开源代码坐标系是不同的,需要进行调整,这里我按照自己的习惯将导航系统一转换到了东北天坐标系。调整部分可以参考这篇博文:
https://zhuanlan.zhihu/p/464385688
下面我参考论文给出东北天坐标系下具体公式。
1.安装角的卡尔曼滤波器
待估的状态向量共有9维:
δ x = [ δ r n α ϕ δ k ] T \delta {\bf{x}} = \left[ \begin{array}{cccc} \delta {{\bf{r}}^n} & {\bf{\alpha }} & \phi & \delta k \end{array} \right]^{\rm{T}} δx=[δrnαϕδk]T
其中,
δ
r
n
=
[
δ
r
E
δ
r
N
δ
r
U
]
T
\delta {{\bf{r}}^n} = \left[ \begin{array}{ccc} \delta {r_E} & \delta {r_N} & \delta {r_U} \end{array} \right]^{\rm{T}}
δrn=[δrEδrNδrU]T表示东、北、天方向上的位置误差,单位为米。这里定义为航位推算位置向量
r
^
n
{{\bf{\hat r}}^n}
r^n与真实位置向量
r
n
r^n
rn之间的差异。
ϕ
=
[
ϕ
p
ϕ
r
ϕ
y
]
T
\phi = \left[ \begin{array}{ccc} \phi_p & \phi_r & \phi_y \end{array} \right]^T
ϕ=[ϕpϕrϕy]T表示GNSS/INS姿态解的偏差。其中,
ϕ
p
、
ϕ
r
、
ϕ
y
\phi_p、 \phi_r、\phi_y
ϕp、ϕr、ϕy分别表示俯仰、横滚和航向角的误差。类似地,
α
=
[
δ
Δ
θ
δ
Δ
ψ
]
T
\alpha = \left[ \begin{array}{cc} \delta\Delta\theta & \delta\Delta\psi \end{array} \right]^T
α=[δΔθδΔψ]T是IMU安装角度的残差。其实,
α
=
[
δ
Δ
θ
δ
Δ
γ
δ
Δ
ψ
]
T
\alpha = \left[ \begin{array}{ccc} \delta\Delta\theta & \delta\Delta\gamma & \delta\Delta\psi \end{array} \right]^T
α=[δΔθδΔγδΔψ]T只是最后发现误差
δ
Δ
γ
\delta\Delta\gamma
δΔγ与无关,去掉了
δ
Δ
γ
\delta\Delta\gamma
δΔγ。
δ
k
δk
δk是行进距离测量的比例因子误差。
滤波器的设计需要描述误差状态动态的方程。接下来,将介绍航位推算算法,然后推导并详细讨论误差状态的动态特性。
注: C b n C_b^n Cbn表示从 b b b系到 n n n系的旋转矩阵,如果仅存在右上角,表示在某个坐标下的变量,如 v n v^n vn表示 n n n系下的速度向量; Δ s k v \Delta s_k^v Δskv表示 k k k时刻 v v v系下的里程增量。大体如此。
2.航位推算
航位推算位置可以用一组关于地理纬度φ、经度λ和高度h的微分方程描述。
φ ˙ = v N R M + h \dot{φ} = \frac{v_N}{R_M + h} φ˙=RM+hvN λ ˙ = v E ( R N + h ) cos φ \dot{\lambda} = \frac{v_E}{(R_N + h)\cos{φ}} λ˙=(RN+h)cosφvE h ˙ = v U \dot{h} = v_U h˙=vU
其中,
R
M
R_M
RM 和
R
N
R_N
RN分别指代沿着经度和纬度线的曲率半径。
v
E
、
v
N
、
v
U
v_E、v_N、v_U
vE、vN、vU分别是在
n
n
n系中
v
n
v^n
vn东、北、天方向上速度分量。
在实际应用中,里程计的车辆速度测量参考系为
v
v
v系,然后通过以下转换将其转换为
n
n
n系,
v n = C b n C v b v v v^n=C_b^n C_v^b v^v vn=CbnCvbvv
其中,车辆参考速度
v
v
v^v
vv的第二个元素(y/前向分量)具有非零值
v
v
v,其他元素为零,即,
v
v
=
[
0
v
0
]
T
v^v=[0\quad v\quad 0]^T
vv=[0v0]T
由里程计测量或由GNSS/INS轨迹导出的增量行驶距离
Δ
s
k
v
Δs_k^v
Δskv参考系为
v
v
v系,并定义为
Δ s k v = ∫ t k − 1 t k v v ( t ) d t \Delta s_k^v = \int_{t_{k-1}}^{t_k} v^v(t) \, dt Δskv=∫tk−1tkvv(t)dt
其中,
t
k
−
1
t_{k-1}
tk−1和
t
k
t_{k}
tk表示离散时间戳。
Δ
s
k
v
\Delta s_k^v
Δskv的第二个元素(y分量)
∆
s
∆s
∆s 也具有非零值,其他元素为零;即,
Δ
s
k
v
=
[
0
∆
s
0
]
T
\Delta s_k^v=[0\quad ∆s\quad 0]^T
Δskv=[0∆s0]T
航位推算位置在通过数值积分使用地理速度分量进行更新,
r k n = r k − 1 n + ∫ t k − 1 t k C b n ( t ) C v b ( t ) v v ( t ) d t r_k^n = r_{k-1}^n + \int_{t_{k-1}}^{t_k} C_b^n(t) C_v^b(t) v^v(t) \, dt rkn=rk−1n+∫tk−1tkCbn(t)Cvb(t)vv(t)dt
略去一些参数介绍…
r k n = r k − 1 n + C b n ( t k − 1 ) C v b ∫ t k − 1 t k v v ( t ) d t = r k − 1 n + C b n ( t k − 1 ) C v b Δ s k v r_k^n = r_{k-1}^n + C_b^n(t_{k-1})C_v^b \int_{t_{k-1}}^{t_k} v^v(t) \, dt = r_{k-1}^n + C_b^n(t_{k-1})C_v^b \Delta s_k^v rkn=rk−1n+Cbn(tk−1)Cvb∫tk−1tkvv(t)dt=rk−1n+Cbn(tk−1)CvbΔskv
略去一些参数介绍…
Δ s k n = C b n ( t k − 1 ) C v b Δ s k v \Delta s_k^n = C_b^n(t_{k-1})C_v^b \Delta s_k^v Δskn=Cbn(tk−1)CvbΔskv
其中, Δ s k n = [ Δ s E , k Δ s N , k Δ s U , k ] T \Delta s_k^n = \begin{bmatrix} \Delta s_{E,k} \quad \Delta s_{N,k} \quad \Delta s_{U,k} \end{bmatrix}^T Δskn=[ΔsE,kΔsN,kΔsU,k]T
ϕ k = ϕ k − 1 + Δ s N , k R M + h \phi_k = \phi_{k-1} + \frac{\Delta s_{N,k}}{R_M + h} ϕk=ϕk−1+RM+hΔsN,k
λ k = λ k − 1 + Δ s E , k ( R N + h ) cos ϕ \lambda_k= \lambda_{k-1} + \frac{\Delta s_{E,k}}{(R_N + h) \cos \phi} λk=λk−1+(RN+h)cosϕΔsE,k
h k = h k − 1 + Δ s U , k h_k = h_{k-1} + \Delta s_{U,k} hk=hk−1+ΔsU,k
接下来,推导航位推算位置误差的离散时间传播模型。
航位推算与初始位置误差或前一时段的误差、姿态误差、补偿后的安装角度的残余误差以及距离测量误差有关。因此,航位推算的位置表示为,
r ^ k n = r ^ k − 1 n + C ^ b , k − 1 n C ^ v b Δ s ^ k v \hat{r}_k^n = \hat{r}_{k-1}^n + \hat{C}_{b,k-1}^n \hat{C}_v^b {\Delta}\hat{s}_k^v r^kn=r^k−1n+C^b,k−1nC^vbΔs^kv
车辆增量距离测量误差被假设为由比例因子误差 δ k δk δk引起,
Δ s ^ v = ( 1 + δ k ) Δ s v = Δ s v + Δ s v δ k {\Delta}\hat{s}^v = (1 + \delta_k) {\Delta}s^v = {\Delta}s^v + {\Delta}s^v \delta_k Δs^v=(1+δk)Δsv=Δsv+Δsvδk
结合上两式,并考虑到
δ
r
n
=
r
^
n
−
r
n
{\delta}r^n =\hat r^n-r^n
δrn=r^n−rn
C ^ b n = [ I − ( ϕ × ) ] C b n \hat C_b^n=[I-(ϕ×)]C_b^n C^bn=[I−(ϕ×)]Cbn
C
^
b
v
=
[
I
−
(
α
×
)
]
C
b
v
\hat C_b^v=[I-(α×)]C_b^v
C^bv=[I−(α×)]Cbv
可得
δ r k n = δ r k − 1 n − C b n C v b ( Δ s v × α ) + Δ s n × ϕ + Δ s n δ k \delta r_k^n = \delta r_{k-1}^n - C_b^n C_v^b (\Delta s^v \times \alpha)+ \Delta s^n \times \phi + \Delta s^n \delta_k δrkn=δrk−1n−CbnCvb(Δsv×α)+Δsn×ϕ+Δsnδk
Δ s v × α = [ 0 0 Δ s 0 0 0 − Δ s 0 0 ] [ δ Δ θ δ Δ γ δ Δ ψ ] = Δ s [ δ Δ ψ 0 − δ Δ θ ] \Delta s^v \times \alpha = \begin{bmatrix} 0 & 0 & \Delta s \\ 0 & 0 & 0 \\ -\Delta s & 0 & 0 \end{bmatrix} \begin{bmatrix} \delta \Delta \theta \\ \delta \Delta \gamma \\ \delta \Delta \psi \end{bmatrix} = \Delta s \begin{bmatrix} \delta \Delta \psi \\ 0 \\ -\delta \Delta \theta \end{bmatrix} Δsv×α=
00−Δs000Δs00 δΔθδΔγδΔψ =Δs δΔψ0−δΔθ
上述方程表明位置误差与 δ Δ γ \delta \Delta \gamma δΔγ无关,整理一下:
δ r k n = δ r k − 1 n − C b n C v b M [ δ Δ θ δ Δ ψ ] + Δ s n × ϕ + Δ s n δ k \delta r_k^n = \delta r_{k-1}^n - C_b^n C_v^b M \begin{bmatrix} \delta \Delta \theta \\ \delta \Delta \psi \end{bmatrix} + \Delta s^n \times \phi + \Delta s^n \delta_k δrkn=δrk−1n−CbnCvbM[δΔθδΔψ]+Δsn×ϕ+Δsnδk
M = [ 0 Δ s 0 0 − Δ s 0 ] M = \begin{bmatrix} 0 & \Delta s \\ 0 & 0 \\ -\Delta s & 0 \end{bmatrix} M=00−ΔsΔs00
只要以一定的精度观测到航位推算定位误差,就可以估计俯仰和航向安装角的残差
3.状态模型
在离散时间下,安装角度估计滤波器的系统模型,
δ x k = Φ k / k − 1 δ x k − 1 + G k − 1 w k − 1 \delta x_k = \Phi_{k/k-1} \delta x_{k-1} + G_{k-1} w_{k-1} δxk=Φk/k−1δxk−1+Gk−1wk−1
其中, w k − 1 w_{k-1} wk−1表示状态转移矩阵, Φ \Phi Φ是状态转移矩阵, G G G是离散时间系统噪声分布矩阵, w w w 是系统噪声向量。
Φ k / k − 1 = [ I 3 × 3 − C b n C v b M ( Δ s n × ) Δ s n 0 I 2 × 2 0 0 0 0 I 3 × 3 0 0 0 0 1 ] \Phi_{k/k-1} = \begin{bmatrix} I_{3 \times 3} & -C_b^n C_v^b M & (\Delta s^n \times) & \Delta s^n \\ 0 & I_{2 \times 2} & 0 & 0 \\ 0 & 0 & I_{3 \times 3} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} Φk/k−1=
I3×3000−CbnCvbMI2×200(Δsn×)0I3×30Δsn001
G k − 1 = [ 0 3 × 6 I 6 × 6 ] G_{k-1} = \begin{bmatrix} 0_{3 \times 6} \\ I_{6 \times 6} \end{bmatrix} Gk−1=[03×6I6×6]
w k − 1 = [ w Δ θ w Δ ψ w θ w ϕ w ψ w S F ] T w_{k-1} = \begin{bmatrix} w_{\Delta\theta} & w_{\Delta\psi} & w_{\theta} & w_{\phi} & w_{\psi} & w_{SF} \end{bmatrix}^T wk−1=[wΔθwΔψwθwϕwψwSF]T
变量说明参考上文,写公式真累……
4.量测模型
KF滤波器的量测是航位推算位置和GNSS/INS位置之间的差异
实在懒得打公式了,大家凑合看吧。
三、结果
取了某次的实测数据,往返采集了两次数据。
第一次
第二次
结果基本吻合。
总结
希望对大家有所帮助,有所纰漏在所难免,望斧正。
ps:矩阵不清楚怎么加粗,并未与普通变量区分,注意辨识。
本文标签: 车辆
版权声明:本文标题:车辆安装角标定 内容由热心网友自发贡献,该文观点仅代表作者本人, 转载请联系作者并注明出处:https://m.elefans.com/dongtai/1729851589a1215488.html, 本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容,一经查实,本站将立刻删除。
发表评论