里程计(odo)量测模型

里程计量测模型学习记录

1.概要

本文主要讨论如何利用车载里程计的输出信息构建滤波量测方程，并以此估计安装角$C_i^v$

2.符号约定

符号	意义
$V$	速度，用角标表示是哪个系的投影。一般给出的是导航系下的速度。
$C$	旋转矩阵
$\tilde{x}$	$ 某个量对应的测量值
$\tilde{x}$	某个量对应的近似值
$C_i^n$	imu到导航系（东北天）的旋转矩阵，真值
$C_i^{n^{\prime }}$	实际估计的imu到导航系的旋转矩阵。此时认为导航系不准，用n’来表示这个不准的导航系。
$\varphi$	姿态失准角。
$C_i^v$	安装角。imu到车体系的旋转矩阵。
$\lambda$	安装角误差。
$S_o$	里程计的刻度误差（比例因子系数）

3.基础知识

1. 哥氏定理
2. 卡尔曼滤波

4.问题描述

利用车辆前后轮里程计的输出信息，构建滤波量测方程，确定各误差状态量的系数以及新息的计算。

5. 解算过程

5.1 后轮里程计模型
后轮里程计输出为标量$v_o$，据此可以构建车轮中心点O在车体坐标系v系下的速度矢量$V_o^v=[0,v_o,0{]}^T$。首先给出以下涉及的一些基本公式：
$$\begin{array}{rl}{V}_o^v& =V_i^v+{\omega }^v\times l^v\\ {\overline{V}}_i^v& =C_{n^{\prime }}^{v^{\prime }}\cdot V_i^n\\ {\overline{V}}_i^n& =V_i^n+\delta v_i^n\\ \overline{V_i^v}& =C_{n^{\prime }}^{v^{\prime }}\cdot \overline{V_i^n}\\ \overline{V_i^n}& =V_i^n+\delta v_i^n\\ \overline{{\omega }^v}& =C_v^{v^{\prime }}\cdot C_i^v\cdot ({\omega }^i+{ϵ}_g)\\ & =(I-\lambda \times )\cdot C_i^v\cdot ({\omega }^i+{ϵ}_g)\\ C_{n^{\prime }}^{v^{\prime }}& =C_v^{v^{\prime }}\cdot C_n^v\cdot C_{n^{\prime }}^n\\ & =(I-\lambda \times )\cdot C_n^v\cdot (I+\varphi \times )\end{array}$$
根据以上公式，推导预测更新后的$\overline{V_o^v}$与真值以及误差状态量之间的关系如下：
$$\begin{array}{rl}\overline{V_o^v}& =(\overline{V_i^v}+\overline{{\omega }^v}\times l^v)\cdot (1+{\overline{S}}_o)\\ & =((I-\lambda \times )\cdot C_n^v\cdot (I+\varphi \times )\cdot (V_i^n+\delta v_i^n)+(I-\lambda \times )\cdot C_i^v\cdot ({\omega }^i+{ϵ}_g)\times l^v)\cdot (1+\overline{S_o})\\ & =((C_n^v-\lambda \times C_n^v)\cdot (I+\varphi \times )\cdot (V_i^n+\delta v_i^n)+(C_i^v-\lambda \times C_i^v)\cdot ({\omega }^i+{ϵ}_g)\times l^v)\cdot (1+\overline{S_o})\\ & =((C_n^v+C_n^v\varphi \times -\lambda \times C_n^v)\cdot (V_i^n+\delta v_i^n)+({\omega }^v+C_i^v\cdot {ϵ}_g-\lambda \times {\omega }^v)\times l^v)\cdot (1+\overline{S_o})\\ & =(V_i^v+{\omega }^v\times l^v-C_n^v[V_i^n\times ]\varphi +[V_i^v\times ]\lambda +C_n^v\delta v_i^n-[l^v\times ]C_i^v{ϵ}_g+[{\omega }^v\times ][l^v\times ]\lambda )\cdot (1+\overline{S_o})\\ & =(V_i^v+{\omega }^v\times l^v-C_n^v[V_i^n\times ]\varphi +([V_i^v\times ]+[{\omega }^v\times ][l^v\times ])\lambda +C_n^v\delta v_i^n-[l^v\times ]C_i^v{ϵ}_g)\cdot (1+\overline{S_o})\\ & =(V_o^v-C_n^v[V_i^n\times ]\varphi +([V_i^v\times ]+[{\omega }^v\times ][l^v\times ])\lambda +C_n^v\delta v_i^n-[l^v\times ]C_i^v{ϵ}_g)\cdot (1+\overline{S_o})\\ & =V_o^v-C_n^v[V_i^n\times ]\varphi +([V_i^v\times ]+[{\omega }^v\times ][l^v\times ])\lambda +C_n^v\delta v_i^n-[l^v\times ]C_i^v{ϵ}_g+V_o^v\cdot \overline{S_o}\end{array}$$
对应量测信息与真值之间的关系表达如下：
$$\begin{array}{r}\widetilde{V_o^v}=V_o^v\cdot (1+S_o)+{\xi }_o\end{array}$$
其中${\xi }_n$由实际里程计的测量噪声确定。
5.2 前轮里程计模型
由于车辆前轮为转向轮，在转向行驶的过程中，存在前向和侧向的速度，根据前轮里程计输出的标量$v_o$，确定速度矢量$V_o^v$不同于后轮，根据哥氏定理有：
$$\overline{V_{of}^v}=\overline{V_{or}^v}+\overline{{\omega }^v}\times l^v$$
其中of表示前轮、or表示后轮，即：
$$\left[\begin{array}{c}{v}_{ofx}^v\\ v_{ofy}^v\\ v_{ofz}^v\end{array}\right]=\left[\begin{array}{c}0\\ v_{ory}^v\\ 0\end{array}\right]+\left[\begin{array}{c}{\omega }_x^v\\ {\omega }_y^v\\ {\omega }_z^v\end{array}\right]\times \left[\begin{array}{c}{l}_x^v\\ l_y^v\\ l_z^v\end{array}\right]$$
所以有$v_{ofx}^v={\omega }_y^v\cdot l_z^v-{\omega }_z^v\cdot l_y^v$， 结合里程计输出值则有$v_{ofy}^v=\sqrt{{v_o}^2-{v_{ofx}^v}^2}$

至此可以获得前轮速度矢量$V_o^v=[{\omega }_y^v\cdot l_z^v-{\omega }_z^v\cdot l_y^v,\sqrt{{v_o}^2-{v_{ofx}^v}^2},0{]}^T$，量测方程与后轮里程计则一致

$$\begin{array}{rl}A& ={\tilde{C}}_i^n\cdot [{\tilde{l}}_{ai}^i\times ]{\tilde{\omega }}^i\\ & \approx (1-\varphi \times )C_i^n\cdot [l_{ai}^i\times ]\cdot ({\omega }^i+\delta {\omega }^i)\\ & =(C_i^n[l_{ai}^i\times ]-[\varphi \times ]C_i^n[l_{ai}^i\times ])\cdot ({\omega }^i+\delta {\omega }^i)\\ & =C_i^n[l_{ai}^i\times ]{\omega }^i+C_i^n[l_{ai}^i\times ]\delta {\omega }^i-[\varphi \times ]C_i^n[l_{ai}^i\times ]{\omega }^i\\ & =C_i^n[l_{ai}^i\times ]{\omega }^i+C_i^n[l_{ai}^i\times ]\delta {\omega }^i+C_i^n[l_{ai}^i\times ][{\omega }^i\times ]\varphi \end{array}$$
$$\begin{array}{rl}B& =[{\omega }_{In}^n\times ]{\tilde{C}}_v^n\cdot l_{ai}^v\\ & =[{\omega }_{In}^n\times ]\cdot (1-\varphi \times )C_v^n(1+\lambda \times )\cdot l_{ai}^v\\ & =[{\omega }_{In}^n\times ]\cdot (C_v^n+C_v^n[\lambda \times ]-\varphi \times C_v^n)\cdot l_{ai}^v\\ & =[{\omega }_{In}^n\times ]C_v^n{l}_{ai}^v+[{\omega }_{In}^n\times ]C_v^n[\lambda \times ]l_{ai}^v-[{\omega }_{In}^n\times ]\varphi \times l_{ai}^n\\ & =[{\omega }_{In}^n\times ]C_v^n{l}_{ai}^v-[{\omega }_{In}^n\times ]C_v^n[l_{ai}^v\times ]\lambda +[{\omega }_{In}^n\times ][l_{ai}^n\times ]\varphi \end{array}$$