从卫星接收机到组合导航【九】（单位置对准、标准卡尔曼滤波）

difudan4777

已于 2023-10-23 23:53:54 修改

阅读量174

点赞数

分类专栏：从卫星导航接收机到组合导航文章标签：算法嵌入式硬件

于 2023-10-23 23:53:02 首次发布

本文链接：https://blog.csdn.net/difudan4777/article/details/133973178

版权

从卫星导航接收机到组合导航专栏收录该内容

10 篇文章 7 订阅

订阅专栏

文章目录

对准结果

对准实验按照如下操作进行：

先将惯导安装在转台。
转台旋转到指定位置，惯导进行上电。
等待对准7分钟后，记录下航向、俯仰、滚动值。
转台旋转180度。
等待第二位置对准8分钟后，记录下双位置对准结果。
关闭惯导电源。
后续反复进行2~6过程，直至对准结束

表1 单位置对准7分钟结果
转台（°）	航向（°）		俯仰（°）		滚动（°）
	实测	误差	实测	误差	实测	误差
0	0.06846	-0.06846	0.27875	-0.27875	0.12206	-0.12206
0	359.62836	0.37164	0.27989	-0.27989	0.11619	-0.11619
90	88.81884	1.18116	0.27515	-0.27515	0.11256	-0.11256
90	88.81527	1.18473	0.27543	-0.27543	0.11128	-0.11128
180	179.24097	0.75903	0.27682	-0.27682	0.10582	-0.10582
180	179.33724	0.66276	0.27692	-0.27692	0.10491	-0.10491
270	269.76143	0.23857	0.28199	-0.28199	0.10554	-0.10554
270	269.69401	0.30599	0.28178	-0.28178	0.10498	-0.10498
45	44.3558	0.6442	0.27726	-0.27726	0.10999	-0.10999
45	44.44199	0.55801	0.2769	-0.2769	0.10976	-0.10976
135	134.08621	0.91379	0.2745	-0.2745	0.10508	-0.10508
135	134.15093	0.84907	0.27444	-0.27444	0.10493	-0.10493
225	224.31309	0.68691	0.27823	-0.27823	0.10222	-0.10222
225	224.44878	0.55122	0.27815	-0.27815	0.10187	-0.10187
315	314.50551	0.49449	0.28113	-0.28113	0.10599	-0.10599
315	314.63951	0.36049	0.28088	-0.28088	0.1055	-0.1055
标准差		0.322785297		0.002455532		0.005196391

表2 双位置总共对准15分钟结果
转台（°）	航向（°）		俯仰（°）		滚动（°）
	实测	误差	实测	误差	实测	误差
180	179.52461	0.47539	0.28534	-0.28534	0.11052	-0.11052
180	179.41716	0.58284	0.28187	-0.28187	0.108668	-0.108668
270	269.19857	0.80143	0.28553	-0.28553	0.10921	-0.10921
270	269.21908	0.78092	0.28455	-0.28455	0.10816	-0.10816
0	359.32432	0.67568	0.28177	-0.28177	0.11207	-0.11207
0	359.34896	0.65104	0.28113	-0.28113	0.11185	-0.11185
90	89.45742	0.54258	0.27611	-0.27611	0.11053	-0.11053
90	89.4285	0.5715	0.2756	-0.2756	0.10993	-0.10993
225	224.36746	0.63254	0.27897	-0.27897	0.10376	-0.10376
225	224.41883	0.58117	0.2787	-0.2787	0.10249	-0.10249
315	314.30663	0.69337	0.28155	-0.28155	0.10757	-0.10757
315	314.32008	0.67992	0.28118	-0.28118	0.10736	-0.10736
45	44.33777	0.66223	0.27671	-0.27671	0.10954	-0.10954
45	44.44186	0.55814	0.27668	-0.27668	0.10958	-0.10958
135	134.41085	0.58915	0.27318	-0.27318	0.10516	-0.10516
135	134.47416	0.52584	0.27307	-0.27307	0.10523	-0.10523
标准差		0.086292754		0.003881585		0.002721621

解析式粗对准

本节实现参考文献为《船用捷联惯导系统解析粗对准的误差分析》
重力矢量和地球自转角速度在导航坐标系上投影为
$\begin{array}{l}{{\rm{g}}^n} = \left[ {\begin{array}{ccccccccccccccc}0\\0\\{ - g}\end{array}} \right]\\{\bf{\omega }}_{ie}^n = \left[ {\begin{array}{ccccccccccccccc}0\\{{{\bf{\omega }}_{ie}}\cos L}\\{{{\bf{\omega }}_{ie}}\sin L}\end{array}} \right]\end{array}\tag{1}$
构造矢量一般使用两种方案：
第一种方案： ${\rm{g}}$ 、 ${{{\bf{\omega }}_{ie}}}$ 进行双矢量定姿，计算公式如下
$\hat C_b^n = {\left[ {\begin{array}{ccccccccccccccc}{{g^n}}&{{\bf{\omega }}_{ie}^n}&{{g^n} \times {\bf{\omega }}_{ie}^n}\end{array}} \right]^{ - 1}}\left[ {\begin{array}{ccccccccccccccc}{{{\left( {\tilde f_{sf}^b} \right)}^T}}\\{{{\left( {{\bf{\tilde \omega }}_{ib}^b} \right)}^T}}\\{{{\left( {\tilde f_{sf}^b \times {\bf{\tilde \omega }}_{ib}^b} \right)}^T}}\end{array}} \right]\tag{2}$

通过推导可以得到对准误差为

$\left\{ {\begin{array}{ccccccccccccccc}{{\phi _E} = \frac{1}{2}\left( {\frac{{{\nabla _{\rm N}}}}{g} + \frac{{{\nabla _U}}}{g}\tan L + \frac{{{\varepsilon _U}}}{{{{\bf{\omega }}_{ie}}\cos L}}} \right)}\\{{\phi _N} = \frac{{{\nabla _{\rm E}}}}{g}}\\{{\phi _U} = - \frac{{{\nabla _E}}}{g}\tan L - \frac{{{\varepsilon _{\rm{E}}}}}{{{{\bf{\omega }}_{ie}}\cos L}}}\end{array}} \right.\tag{4}$

第一种方案： ${\rm{g}}$ 、 ${\rm{g}} \times {{\bf{\omega }}_{ie}}$ 进行双矢量定姿，计算公式如下
$\hat C_b^n = {\left[ {\begin{array}{ccccccccccccccc}{{g^n}}&{{g^n} \times {\bf{\omega }}_{ie}^n}&{({g^n} \times {\bf{\omega }}_{ie}^n) \times {g^n}}\end{array}} \right]^{ - 1}}\left[ {\begin{array}{ccccccccccccccc}{{{\left( {\tilde f_{sf}^b} \right)}^T}}\\{{{\left( {\tilde f_{sf}^b \times {\bf{\tilde \omega }}_{ib}^b} \right)}^T}}\\{{{\left[ {\left( {\tilde f_{sf}^b \times {\bf{\tilde \omega }}_{ib}^b} \right) \times \tilde f_{sf}^b} \right]}^T}}\end{array}} \right]\tag{5}$
通过推导可以得到对准误差为
$\left\{ {\begin{array}{ccccccccccccccc}{{\phi _E} = \frac{{{\nabla _{\rm N}}}}{g}}\\{{\phi _N} = - \frac{{{\nabla _{\rm E}}}}{g}}\\{{\phi _U} = - \frac{{{\nabla _E}}}{g}\tan L - \frac{{{\varepsilon _{\rm{E}}}}}{{{{\bf{\omega }}_{ie}}\cos L}}}\end{array}} \right.\tag{6}$
一般情况下 ${{\phi _U}}$ 中的误差陀螺引起的误差远大于加速度计引起误差，即
$\left| { - \frac{{{\varepsilon _{\rm{E}}}}}{{{{\bf{\omega }}_{ie}}\cos L}}} \right| \gg \left| { - \frac{{{\nabla _E}}}{g}\tan L} \right|$
故误差可以写成如下式
$\left\{ {\begin{array}{ccccccccccccccc}{{\phi _E} = \frac{{{\nabla _{\rm N}}}}{g}}\\{{\phi _N} = - \frac{{{\nabla _{\rm E}}}}{g}}\\{{\phi _U} \approx - \frac{{{\varepsilon _{\rm{E}}}}}{{{{\bf{\omega }}_{ie}}\cos L}}}\end{array}} \right.\tag{8}$

通过表1可以看到，航向误差基本都在0.5°-0.7°（0.00872rad-0.00124rad）之内波动（去掉一个0位置），俯仰和滚动峰峰值分别为0.00755°（1.5219e-4rad）、0.01432°（2.4993e-4）（去掉一个0位置），再由公式（8），大概得出东向陀螺漂移为0.1003°/h-0.01426°/h，东向加速度零偏1.5219e-4g，北向2.4993e-4g，可以看到这个结论还是不是非常符合IMU精度（陀螺零偏大概是0.05°/h，加速度计为50ug）。
我个人认为计算出来的零偏大可能原因有如下：
1、标定之后残差比较大
2、卡尔曼滤波参数不合适
3、对准时间太短

卡尔曼滤波

粗对准使用的是抗摇晃粗对准，精对准采用的是15（状态量）+6（测量量），状态量分别为3个失准角、3个速度误差、3个位置误差、3个陀螺零偏、3个加速度零偏，测量量为3个速度误差、3个位置误差。
粗对准和精对准分别可以参考PSINS中的test_align_methods_compare_lgimu.m、test_SINS_GPS_186.m代码，使用test_SINS_GPS_186.m代码时，需要将杆臂的P阵初始值设置成0（不需要估计杆臂）。

difudan4777

关注

0
点赞
踩
1

收藏

觉得还不错? 一键收藏
0
评论
从卫星接收机到组合导航【九】（单位置对准、标准卡尔曼滤波）

通过表1可以看到，航向误差基本都在0.5°-0.7°（0.00872rad-0.00124rad）之内波动（去掉一个0位置），俯仰和滚动峰峰值分别为0.00755°（1.5219e-4rad）、0.01432°（2.4993e-4）（去掉一个0位置），再由公式（8），大概得出东向陀螺漂移为0.1003°/h-0.01426°/h，东向加速度零偏1.5219e-4g，北向2.4993e-4g，可以看到这个结论还是不是非常符合IMU精度（陀螺零偏大概是0.05°/h，加速度计为50ug）。
复制链接

扫一扫