沈 圆 王 勇 倪福生

（1.河海大学机电工程学院 常州 213022）（2.江苏省水利机械制造有限公司 扬州 225000）（3.疏浚技术教育部工程研究中心 常州 213022）

1 引言

水库清淤方法有泄洪冲刷、吸力挖泥船、抓斗挖泥船等方法。挖泥船由于漂浮在水面，使用范围受到水深的制约，而清淤机器人能够不受水深影响，在水库底部行走、清淤及向地面排沙，因此能够保证水库库容、避免泥沙对水量调节和水电站的运转带来不利影响。在水下进行精确的定位［1］是清淤机器人运动和操控的必要条件，在深海环境下，电磁波衰减很快，传统的陆上定位方式［2］不再适用。相比而言，惯性导航能够不依赖于外界信息，进行自主定位，是清淤机器人理想的定位方式。

惯性导航分为平台式以及捷联式，由于捷联惯导具有可靠性高、成本低以及使用方便的特点，本文选取捷联惯导作为清淤机器人的定位方式。捷联惯导解算前，需要先求得姿态的初始值，即为载体坐标系与导航坐标系的相对关系，包括横滚角γ、俯仰角θ和航向角ψ。载体坐标系O-XbYbZb和导航坐标系O-XnYnZn之间的变换关系如下：

将上面的转换关系表示成矩阵形式即为

即为姿态矩阵，初始姿态对准［3］即为了求得高精度的初始姿态矩阵，也就是初始姿态角，横滚角γ0、俯仰角θ0和航向角ψ0，利用粒子滤波法改进初始对准结果，将其与滤波前的结果进行对比研究，流程图如图1所示。

图1 初始姿态对准流程

2 初始姿态对准难题

初始姿态对准是捷联惯导中一项难题，初始对准精度直接影响最终的定位精度［4］。滤波技术在初始对准中具有重要作用。

陀螺仪和加速度计提供3个相互垂直方向上的三轴角速率信息和三轴比力信息。一般来说，陀螺仪和加速度计的所在的三轴都是对齐的。对准过程主要是由相对于给定参考坐标系的加速度计轴定义的笛卡尔坐标系的三个角度来确定。

由惯导基本公式：

从而

地球自转角速度ωnie与陀螺仪输出值之间的关系：

从而

根据加速度计和陀螺仪的输出值就可以按照式（4）和（6）求出姿态角的初始值，但是由于加速度计和陀螺仪噪声的存在，得到的值始终在一个范围内波动，无法得到精确值，因此，陀螺仪和加速度计的误差会导致对准误差；而且对准时载体受到的干扰也会产生对准误差。

针对初始姿态对准问题，前人做了大量的研究。L Zhao［5］等将高斯逼近法和蒙特卡洛方法应用到初始姿态对准中，比较了两者的优缺点，高斯法速度更快，蒙特卡洛方法精度更高。李辰淑［6］详细分析了初始姿态对准全过程，以及误差的成因。Li J［7］等提出了一种基于固定积分区间滑动的优化对准方法，减少了对准时间。李宏成［8］将卡尔曼滤波方法应用到初始姿态对准中，并将其优化，达到了对准精度要求。

惯性传感器是惯性导航的核心技术，在积分运算中，惯性导航系统和对惯性传感器误差极为敏感，而惯性传感器存在误差，所以计算出的姿态角也会偏离真实值，当其偏差过大时，定位结果将会完全失真，而这个误差将不会被消除，持续累积，惯导系统失去定位能力。因此必须对误差进行补偿，得到一个理想的初始姿态角，滤波技术对于捷联惯导尤其重要。

3 基于粒子滤波法的初始姿态对准模型

捷联惯导的误差主要是由于惯性传感器引起的，而错误的误差消除方法将会放大缺点，因此，建立合适的初始姿态对准误差纠正模型是非常有必要的，本小节主要分析了失准角以及速度误差，建立了相应的状态方程以及观测方程［9］。

3.1 状态方程

在本文中采用地理坐标系n（即东北天坐标系）为导航坐标系［10］，计算导航坐标系为 n̂，惯性坐标系为i。n系绕zn轴转动得到n1系，n1系绕yn轴转动得到n2系，n2系绕xn转动得到 n̂系，写成矩阵形式即为

在真实的导航坐标系中，根据SINS原理可知捷联矩阵的微分方程为

在没有进行初始姿态对准时，实际上导航坐标系n系是由计算导航坐标系n̑系来代替的。所以要想求得真实的姿态矩阵，必须要乘以一个误差矩阵，对其进行补偿。

其中∇n̑是加速度计零偏在计算导航坐标系上的投影。在静基座初始对准中，当地位置不变，所以、、Vn和 δgn皆为0，整理上式得到：

3.2 观测方程

本文以东北向的速度差作为系统的观测量建立观测方程［11］，Z=[δVE，δVN]为观测量。根据捷联惯导比力方程：

可以求取当前时刻的速度，因为清淤机器人静止不动，所以真实速度为0，所以速度差即为当前时刻的速度，所以有观测方程为

η =[ηδVE，ηδVN]即为观测噪声。

4 算法设计及实现

本文选用粒子滤波法进行求解，可以直接应用非线性的状态方程，不需要对其进行线性处理［12］。

5 仿真计算及分析

仿真模拟中，采样间隔为0.02s，粒子数为1000，输出次数为1500次。

加速度的启动漂移为5×10-5m/s2，快变漂移为10-4m/s2；陀螺仪的启动漂移为0.02°/h，快变漂移为0.01°/h。

由于对准时间较短，惯性传感器的慢变漂移［13～16］相对于启动漂移以及快变漂移非常小，所以在仿真过程中仅仅考虑启动漂移以及快变漂移。

状态噪声：

观测噪声：

理想的三个姿态角皆为0度，仿真结果如下：

图2 横滚角

图3 俯仰角

图4 航向角

图2～4可以看出，横滚角、俯仰角以及航向角逐渐向理想值逼近，最终结果为0.29''、3.57''和-0.39'。滤波刚开始时，角度波动幅度较大，特别是航向角。这是因为在根据概率分布进行真实值的估计时，尽量选择更多的可能性，使粒子分布更广，在经过一段时间以后，选择出最接近真实值的粒子。粒子滤波对于非线性问题的处理比较方便［17］，不需要进行线性化处理，而且降低了状态量的个数。

6 结语

本文从捷联惯导初始姿态对准的基本原理出发，在惯性导航系统初始阶段，就姿态对准流程，待处理的疑难点进行了相关的分析，在静基座条件下，分析了捷联惯导的失准角和速度差，建立了状态以及观测方程。简单依据地球自转角速率和重力加速度进行初始姿态角计算，精度差之甚远。在初始姿态对准过程中，滤波后的结果大为改善，与理想值十分接近，证明了基于粒子滤波法的初始对准误差校正模型的有效性。