[发明专利]基于激光多普勒测速仪的惯导系统行进间初始对准方法

权利要求书

1.一种基于激光多普勒测速仪的惯导系统行进间初始对准方法，其特征在于，所述方法包括：

在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导系统对准过程的过程模型；

基于比力方程式建立捷联惯导系统对准过程的测量模型；其中，激光多普勒测速仪的速度表示为刻度系数误差和安装角误差的函数；

以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立鲁棒平方根无迹四元数估计器；

使用所述鲁棒平方根无迹四元数估计器得到所述状态向量的预测值，根据所述预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导系统进行行进间初始对准；

所述捷联惯导系统对准过程的过程模型为：

其中，为t时刻基于地心惯性坐标系i系和当地导航坐标系n系的姿态方向余弦矩阵估计，为所述姿态方向余弦矩阵估计的导数，表示基于激光多普勒测速仪的速度得到的在n系中观察到的n系绕i系的转动角速度，表示所述转动角速度的斜对称矩阵；

基于激光多普勒测速仪的速度计算在n系中观察到的n系绕i系的转动角速度的方式包括：

其中，，，表示n系中的东向速度，表示n系中的北向速度，和分别表示载体所在地点地球子午圈和卯酉圈曲率半径，表示载体所在地点的纬度，表示在n系中观察到的e系绕i系的转动角速度，表示在n系中观察到的n系绕e系的转动角速度，表示地球自转角速度。

2.根据权利要求1所述的方法，其特征在于，捷联惯导系统对准过程的测量模型为：

其中，为激光多普勒测速仪在系中的速度，表示为，

其中，m系为基于激光多普勒测速仪建立的坐标系，为从系到系的转换矩阵，为激光多普勒测速仪的刻度系数，为激光多普勒测速仪的刻度系数误差，为激光多普勒测速仪与惯导设备的安装角误差，表示t时刻载体坐标系b系到惯性坐标系i系之间的姿态转换矩阵，表示载体坐标系b系的加速度计测量的比力，表示初始时刻到t时刻载体在导航坐标系n系中姿态变换矩阵，表示导航坐标系中重力加速度矩阵，表示激光多普勒测速仪在基于激光多普勒测速仪建立的坐标系m系中的速度。

3.根据权利要求1所述的方法，其特征在于，鲁棒平方根无迹四元数估计器的k时刻的状态向量为：

其中，

其中，为姿态误差的误差四元数的广义罗德里格斯参数形式，为误差四元数的标量部分，为误差四元数的矢量部分，的取值范围为[0,1]，为比例因子，、、分别为陀螺在n系下x、y、z轴方向上的漂移偏差估计，、、分别为加速度计在n系下x、y、z轴方向上的漂移偏差估计，m系为基于激光多普勒测速仪建立的坐标系，为激光多普勒测速仪在m系的俯仰安装角误差，为激光多普勒测速仪在m系的航向安装角误差，为刻度系数误差。

4.根据权利要求3所述的方法，其特征在于，所述鲁棒平方根无迹四元数估计器对协方差矩阵的平方根处理为：

表示协方差矩阵的权重值，表示QR分解运算，表示Cholesky更新运算，为预测状态协方差矩阵的平方根形式，表示k时刻的过程噪声协方差矩阵，表示求的正负符号，表示鲁棒平方根无迹四元数估计器得到的测量预测均值。

5.根据权利要求3所述的方法，其特征在于，所述鲁棒平方根无迹四元数估计器对测量噪声协方差矩阵的自适应处理为：

其中为新息向量，为由传感器得到的测量值，为鲁棒平方根无迹四元数估计器得到的测量预测均值，为自适应系数，初值为，c称为渐消因子，其中，、为测量噪声协方差矩阵设置的上下限，上标（i）代表对角矩阵的第i个元素。

6.根据权利要求1至5中任意一项所述方法，其特征在于，还包括：

根据所述预测值的俯仰角误差分量、航向角误差分量和刻度系数误差分量，对激光多普勒测速仪的安装角误差和刻度系数误差进行校正。