[发明专利]模值检测动基座鲁棒对准方法

权利要求书

1.一种模值检测动基座鲁棒对准方法，其特征在于，包括：

步骤1：获取惯性传感器实时数据并进行姿态更新；

步骤2：获取辅助传感器信息，构造矢量观测器；

步骤3：对矢量观测器进行模值计算，并利用标准模值进行异常检测；

步骤4：通过模值检测削弱异常噪声影响，构造自适应矢量姿态确定算法；初始对准过程运行时间为M，若姿态变化次数k＝M，则输出对准结果，完成初始对准过程，若姿态变化次数k＜M，表示初始对准过程未完成，则重复上述步骤直到初始对准过程结束；

步骤1具体包括：由惯性测量单元测量模型可知：

式中，表示测量加速度；f^b表示测量真实加速度；表示加速度测量偏置误差；表示加速度计测量噪声；表示陀螺仪测量角速度；表示测量真实角速度；ε^b表示陀螺仪测量偏置；表示陀螺仪测量噪声；

由姿态更新方程可知：

式中，表示测量真实角速度；表示载体系到初始载体系的方向余弦矩阵；表示表示载体系到初始载体系的方向余弦矩阵的微分；表示导航系相对于惯性系的旋转角速度在导航系的投影；表示导航系相对于初始导航系的方向余弦矩阵；表示导航系相对于初始导航系的方向余弦矩阵的微分；

步骤2具体包括：由DVL测量模型可知：

式中，表示DVL测量速度；v^b表示真实速度；表示测量速度误差；

由比力方程可知

式中，表示导航系速度的微分；表示载体系相对于导航系变化的方向余弦矩阵；f^b表示测量真实加速度；表示地球系相对于惯性系的旋转角速度在导航系的投影；表示导航系相对于地球系的转动角速度在导航系的投影；×表示矢量叉乘运算；vⁿ表示导航系速度；gⁿ表示重力加速度在导航系下的投影；

由比力方程及方向余弦变化关系可知：

式中，表示载体系相对于导航系变化的方向余弦矩阵；表示载体系相对于导航系的旋转角速度在载体系的投影；v^b表示载体系速度；表示载体系速度的微分；表示初始导航系相对于导航系的方向余弦矩阵；表示初始载体系到初始导航系之间的方向余弦矩阵；表示载体系到初始载体系之间的方向余弦矩阵；f^b表示测量真实加速度；表示地球系相对于惯性系的旋转角速度在导航系的投影；表示导航系相对于地球系的转动角速度在导航系的投影；×表示矢量叉乘运算；vⁿ表示导航系速度；gⁿ表示重力加速度在导航系下的投影；

对上式进行整理可得

式中，表示载体系到初始载体系之间的方向余弦矩阵；表示地球系相对于惯性系的旋转角速度在载体系的投影；表示测量真实角速度；v^b表示载体系速度；表示载体系速度微分；f^b表示测量真实加速度；表示初始导航系到初始载体系的方向余弦矩阵；表示导航系到初始导航系的方向余弦矩阵；gⁿ表示重力加速度在导航系下的投影；

对上式两边同时进行积分可得

式中，β表示观测矢量；α表示参考矢量；表示载体系到初始载体系之间的方向余弦矩阵；表示地球系相对于惯性系的旋转角速度在载体系的投影；表示测量真实角速度；v^b表示载体系速度；表示载体系速度微分；f^b表示测量真实加速度；表示初始导航系到初始载体系的方向余弦矩阵；表示导航系到初始导航系的方向余弦矩阵；gⁿ表示重力加速度在导航系下的投影；

将DVL测量速度代入观测矢量可得

式中，表示采用DVL测量速度构造的含噪声的观测矢量；β表示观测矢量；表示DVL测量误差在初始载体系的映射；

步骤3具体包括：

对观测矢量进行模值计算可得

式中，表示采用DVL测量速度构造的含噪声的观测矢量；β表示观测矢量；表示DVL测量误差在初始载体系的映射；T表示转置运算；

有观测矢量和参考矢量之间的关系可知

式中，β表示观测矢量；α表示参考矢量；表示初始导航系到初始载体系的方向余弦矩阵；

因此，可以得到DVL测量速度构造的含噪声矢量噪声模值为

式中，χ表示噪声模值；表示采用DVL测量速度构造的含噪声的观测矢量；β表示观测矢量；α表示参考矢量；表示DVL测量误差在初始载体系的映射；T表示转置运算；

噪声模值求解期望和方差可以得到

式中，E[·]表示期望运算；表示DVL测量速度噪声的方差；β表示观测矢量；α表示参考矢量；

步骤4具体包括：由上面噪声模值的方差可以定义权值系数为

式中，表示归一化之后的权值；表示采用DVL测量速度构造的含噪声的观测矢量；α表示参考矢量；σ_v表示DVL测量速度噪声的标准差；

利用OBA姿态确定算法实现自适应矢量姿态确定

式中，K_k表示k时刻的构造矩阵；K_k-1表示k-1时刻的构造矩阵；表示采用DVL测量速度构造的含噪声的观测矢量；α表示参考矢量；表示归一化之后的权值。