[发明专利]一种适应于挑战环境下的无人艇旋转调制惯导系统及方法

权利要求书

1.一种适应于挑战环境下的无人艇旋转调制惯导系统，其特征在于，包括微机电惯性单元MEMS-IMU、伺服电机、伺服驱动器、单轴旋转台、活动码环、导航定位解算板卡、航空插头和外壳框架；所述微机电惯性单元集成三轴MEMS陀螺仪和MEMS加速度计，其输出的加速度和角速率信息经过单轴旋转调制技术和多源信息融合技术补偿零偏后计算得到无人艇的实时姿态、速度和位置信息；所述伺服电机和伺服驱动器用于驱动微机电惯性单元绕天向轴往复旋转，消除垂直于旋转轴方向的陀螺及加速度计常值漂移、刻度因子误差；所述导航定位解算板卡用于接收输出的加速度、角速率信息以及接收外部全球导航卫星系统的速度、位置信息和水下多普勒测速仪的速度信息，并且通过内设软件算法完成捷联导航解算、组合导航多源信息融合和鲁棒估计抗差后输出导航参数；所述航空插头和外壳框架分别用于电气信息接入和硬件设备装配。

2.根据权利要求1所述的适应于挑战环境下的无人艇旋转调制惯导系统，其特征在于，所述内设软件算法包括如下步骤：

S1：综合考虑MEMS捷联惯导、组合导航特点及多误差源耦合关系，选取姿态角误差[ΔΦ]、速度误差[Δv]、位置误差[Δp]、陀螺仪零偏[ε]、陀螺仪刻度因子误差[δK_G]、加计零偏[▽]、加计刻度因子误差[δK_A]作为组合滤波器24维校正状态量x，选取全球导航卫星系统的速度、位置信息[v_GNSS,p_GNSS]和水下多普勒测速仪的速度信息[v_DVL]作为组合滤波器外部测量y；设计非线性大失准角情况下的捷联惯性误差微分方程f(x)：

其中，f^b表示陀螺仪和加计输出，表示地球旋转角速度矢量、导航系分别相对于地球系和惯性系的旋转矢量，表示导航系旋转计算误差，δg表示重力误差，表示载体系到导航系的转换矩阵，表示计算导航系和理想导航系间的失准角误差矩阵，R_M、R_N、h表示子午圈主曲率半径、卯酉圈主曲率半径以及海拔高度，L表示当地纬度；

S2：引入最大后验估计和非线性最小二乘回归思想，建立迭代容积卡尔曼滤波器ICKF方程；

在非线性sigma点滤波器的框架下，计算含有高斯测量噪声的后验概率密度函数：

其中，H表示测量矩阵，R、P表示测量噪声协方差和最优估计协方差，p(·)表示概率密度函数，exp[·]表示自然常数e为底的指数函数，下标k表示估计时刻；

考虑最大后验估计MAP思想，后验概率密度函数相当于求解非线性回归参数，等效于滤波器更新的过程如下：

根据牛顿-拉夫森算法，将估计状态量x的迭代更新过程构造为形如非线性最小二乘的迭代形式：

x_i+1＝x_i-(▽²V(x_i))^-1▽V(x_i)＝(J^T(x_i)J(x_i))^-1J^T(x_i)r_i(x_i)

其中，i表示当前估计时刻的迭代次数，雅克比矩阵▽V(x_i)＝J^T(x_i)r(x_i)，海森矩阵▽²V(x_i)≈J^T(x_i)J(x_i)，

通过逆矩阵引理，得到迭代容积卡尔曼滤波器ICKF方程最优估计等式：

其中，K＝P_xy,k|k-1(P_yy,k|k-1)^-1表示滤波增益矩阵；在容积卡尔曼滤波器的框架下，滤波增益矩阵K中的协方差表达式被似然近似中的不确定性取代，有：

重新改写，得到当前估计时刻k的第i次迭代容积卡尔曼滤波器ICKF方程最优估计等式：

S3：设计迭代容积卡尔曼滤波器简化迭代更新结构；

简化迭代更新结构如下：

①生成容积点x_j,k-1|k-1

其中，m表本容积点个数，表示基本容积点，[1]表示n维单位向量进行全排列和改变元素符号而生成的完整全对称点集；

②先验预测更新

计算一步预测状态和先验估计协方差

其中，Q_k表示系统噪声协方差；

③后验量测更新

计算量测估计

计算互协方差P_xy,k|k-1和量测协方差P_yy,k|k-1

循环计算第i次迭代后验状态估计x_i+1

直至迭代次数达到设定值N后，令计算1次后验估计协方差

S4：引入惩罚权函数快速降低离群值测量的权重

考虑构造鲁棒核函数，采用鲁棒估计方法的非线性阻尼项代替传统卡尔曼滤波中的原始误差的平方项；为测量异常值建立的稳健估计目标X^*如下：

其中，W_k∈(0，1)表示k时刻的外信息惩罚权矩阵，θ(W_k)表示惩罚权函数，表示向量的马氏距离；

选用GM损失函数作为鲁棒核函数，如下：

其中，s_GM表示GM损失函数的形状控制因子；

由于稳健估计目标函数X^*为凸函数，为了计算最优惩罚权矩阵，对稳健估计目标函数X^*求偏导，并取极小值点：

3.一种适应于GNSS/DVL挑战环境下的鲁棒估计方法，用于输出无人艇旋转调制惯导系统的精确导航参数，包括如下步骤：

S1：综合考虑MEMS捷联惯导、组合导航特点及多误差源耦合关系，选取姿态角误差[ΔΦ]、速度误差[Δv]、位置误差[Δp]、陀螺仪零偏[ε]、陀螺仪刻度因子误差[δK_G]、加计零偏[▽]、加计刻度因子误差[δK_A]作为组合滤波器24维校正状态量x，选取全球导航卫星系统的速度、位置信息[v_GNSS,p_GNSS]和水下多普勒测速仪的速度信息[v_DVL]作为组合滤波器外部测量y；设计非线性大失准角情况下的捷联惯性误差微分方程f(x)：

其中，f^b表示陀螺仪和加计输出，表示地球旋转角速度矢量、导航系分别相对于地球系和惯性系的旋转矢量，表示导航系旋转计算误差，δg表示重力误差，表示载体系到导航系的转换矩阵，表示计算导航系和理想导航系间的失准角误差矩阵，R_M、R_N、h表示子午圈主曲率半径、卯酉圈主曲率半径以及海拔高度，L表示当地纬度；

S2：引入最大后验估计和非线性最小二乘回归思想，建立迭代容积卡尔曼滤波器ICKF方程；

在非线性sigma点滤波器的框架下，计算含有高斯测量噪声的后验概率密度函数：

其中，H表示测量矩阵，R、P表示测量噪声协方差和最优估计协方差，p(·)表示概率密度函数，exp[·]表示自然常数e为底的指数函数，下标k表示估计时刻；

考虑最大后验估计MAP思想，后验概率密度函数相当于求解非线性回归参数，等效于滤波器更新的过程如下：

根据牛顿-拉夫森算法，将估计状态量x的迭代更新过程构造为形如非线性最小二乘的迭代形式：

x_i+1＝x_i-(▽²V(x_i))^-1▽V(x_i)＝(J^T(x_i)J(x_i))^-1J^T(x_i)r_i(x_i)

其中，i表示当前估计时刻的迭代次数，雅克比矩阵▽V(x_i)＝J^T(x_i)r(x_i)，海森矩阵▽²V(x_i)≈J^T(x_i)J(x_i)，

通过逆矩阵引理，得到迭代容积卡尔曼滤波器ICKF方程最优估计等式：

其中，K＝P_xy,k|k-1(P_yy,k|k-1)^-1表示滤波增益矩阵；在容积卡尔曼滤波器的框架下，滤波增益矩阵K中的协方差表达式被似然近似中的不确定性取代，有：

重新改写，得到当前估计时刻k的第i次迭代容积卡尔曼滤波器ICKF方程最优估计等式：

S3：设计迭代容积卡尔曼滤波器简化迭代更新结构；

简化迭代更新结构如下：

④生成容积点x_j,k-1|k-1

其中，m表本容积点个数，表示基本容积点，[1]表示n维单位向量进行全排列和改变元素符号而生成的完整全对称点集；

⑤先验预测更新

计算一步预测状态和先验估计协方差

其中，Q_k表示系统噪声协方差；

⑥后验量测更新

计算量测估计

计算互协方差P_xy,k|k-1和量测协方差P_yy,k|k-1

循环计算第i次迭代后验状态估计x_i+1

直至迭代次数达到设定值N后，令计算1次后验估计协方差

S4：引入惩罚权函数快速降低离群值测量的权重

考虑构造鲁棒核函数，采用鲁棒估计方法的非线性阻尼项代替传统卡尔曼滤波中的原始误差的平方项；为测量异常值建立的稳健估计目标X^*如下：

其中，W_k∈(0，1)表示k时刻的外信息惩罚权矩阵，θ(W_k)表示惩罚权函数，表示向量的马氏距离；

选用GM损失函数作为鲁棒核函数，如下：

其中，s_GM表示GM损失函数的形状控制因子；

由于稳健估计目标函数X^*为凸函数，为了计算最优惩罚权矩阵，对稳健估计目标函数X^*求偏导，并取极小值点：