GVINS理解

snowflakeeeee

已于 2024-06-13 21:08:35 修改

阅读量853

点赞数 5

文章标签： ubuntu linux

于 2024-06-13 20:50:41 首次发布

本文链接：https://blog.youkuaiyun.com/qq_44749244/article/details/138484143

版权

原文中Global坐标系为：第0帧IMU坐标系旋转到与导航坐标系z轴重合时的坐标系。本文中的定义与其相差一个 $R_G^h=R_y R_x$

系统状态参量

引入GNSS后系统状态参量如下，其中 $\varphi$ 为导航坐标系 $n$ 系到 $G$ 系的航向角，此处所有参数均定义在Global坐标系下，即第0帧IMU坐标系：
$^GX= [^Gp,^GR,^Gv,^Gb_a,^Gb_g,^Gg,d t_{r},\dot{d t_{r}},\varphi ]$

GNSS提供的观测方程

伪距观测
伪距观测方程如下，其中 $E$ 为ECI坐标系 (地心惯性坐标系，不随地球旋转)
$\tilde{P}_{r, i}^{s}=||^Ep_r-^Ep_s||+c\left(d t_{r}-d t_{s}\right)+I_{r, i}^{s}+T_{r}^{s}+\varepsilon_{P} \tag{1}$
因为：
$^Ep_r=R_{n}^{e} R_{G}^{n} ({}^Gp+{}^Gl)$
其中 ${}^Gl$ 为杆臂； $e$ 为ECEF坐标系； $E$ 为ECI坐标系，定义为接收机接收到信号时的ECEF坐标系，则ECEF相对于 $E$ 系z轴以地球自转角速度 $w_E$ 旋转；由于信号传播时间 $t_f$ 的存在，得到的卫星坐标并不在 $e$ 系下，而在 $e^{'}$ 坐标系， $KaTeX parse error: Double superscript at position 16: ^Ep_s=R_{e'}^e ^̲{e'}p_s$ 且 $R_{e'}^e=R_z(-w_Et_f)$ ； $R_{n}^{e}$ 由第0帧IMU的经纬度计算； $R_{G}^{n}$ 由初始化确定并与 $\varphi$ 角相关
观测噪声建模为零均值高斯分布 $\varepsilon_{P}\sim N(0,\delta_{P})$ ，其方差为：
$\delta_{P}=\frac{n_s\times n_{pr}}{sin^{2}\theta_{el} }$
其中 $\theta_{el}$ 为从接收机查看的卫星高度角； $n_s$ 为广播卫星空间精密指数，由广播星历给出； $n_{pr}$ 为接收机报告的码伪距测量噪声指数
分析易知，该观测对 $^Gp、d t_{r}、\varphi$ 角可观
多普勒观测
多普勒观测方程如下，其中 $E$ 为ECI坐标系 (地心惯性坐标系，不随地球旋转)：
$\tilde{D}_{r, i}^{s}=^E\kappa_{r}^{sT} (^Ev_s-^Ev_r)+c\left(\dot{d t_{r}}-\dot{d t_{s}}\right)+\varepsilon_{D} \tag{2}$
因为：
$^Ev_r=R_{n}^{e}R_{G}^{n}{}^Gv$ $^Ev_s=R_{e'}^e{}^{e'}v_s$
观测噪声建模为零均值高斯分布 $\varepsilon_{D}\sim N(0,\delta_{D})$ ，其中 $n_{dp}$ 为接收机报告的测量噪声指数
$\delta_{P}=\frac{n_s\times n_{dp}}{sin^{2}\theta_{el} }$
分析易知，该观测对 $^Gp、^Gv、\dot{d t_{r}}、\varphi$ 角可观
接收机零偏观测
$t_{r}^{t_{k}}=d t_{r}^{t_{k-1}}+\int_{t_{k-1}}^{t_k}\dot{d t_{r}}$
其中 $t_k$ 表示第 $k$ 次观测的时间， $\tau_{t_{k-1}}^{t_k}$ 为第 $k - 1$ 次观测与第 $k$ 次观测之间的时间间隔
$t_{r}^{t_{k}}=d t_{r}^{t_{k-1}}+\dot{d} t_{r}^{t_{k-1}} \tau_{t_{k-1}}^{t_k} \tag{3}$
低成本接收机的时钟通常为TCXO，则零漂常被描述为一个随机游走过程，故有
$\dot{d} t_{r}^{t_{k}}=\dot{d} t_{r}^{t_{k-1}} \tag{4}$
相应的方差 $\delta_{dt,k}$ 由零漂稳定性决定

GNSS初始化

初始化的过程主要是要求解 $R_{G}^{n}$ ，具体可以分解为 $R_{G}^{n}=R_z R_y R_x$ 分别表示航向、俯仰、横滚角对应的旋转矩阵

依据里程计估出的重力加速度 $^Gg$ 及当地重力加速度 $^ng=[0,0,-9.81]$ 构建约束求得旋转矩阵 $R_G^n$ ：
$R_G^n)^T {}^ng=^Gg$
因为 $^ng$ 与 $z$ 轴重合，绕 $n$ 系 $z$ 轴转动的方位角不可观。实际求得的旋转矩阵为 $R_G^h=R_y R_x$ ，其中 $h$ 为水平坐标系，有 $R_G^n=R_h^n R_G^h$ ，其中 $R_h^n$ 只与方位角 $\varphi$ 相关
$R_h^n=\begin{pmatrix}-sin\phi&cos\phi&0\\-cos\phi&sin\phi&0\\0&0&0\\\end{pmatrix}$
使用SPP单点定位获取第0帧IMU的粗略位置 $^ep^0$ 及接收机钟差 $d t_{r}^0$
利用上节中的2.多普勒观测约束优化 $\varphi$ 角及钟差漂移 $\dot{d t_{r}}^0$
使用上节中的1.伪距观测及3.接收机零偏观测优化位置 $^ep^0$ 及接收机钟差 $d t_{r}^0$