本发明公开了一种基于改进AR模型和平滑滤波的陀螺误差抑制方法，属于水下机器人陀螺仪误差补偿技术领域，该方法基于已知量MEMS IMU陀螺输出的设备系下的三维角速度信息，具体包括如下步骤：首先采用滑动窗3阶拉盖尔多项式抗差拟合处理方法对陀螺输出信息进行抗差处理；然后建立改进的实时AR模型；并使用以IGGIII法为权函数的抗差最小二乘估计参数；再进行后向平滑卡尔曼滤波补偿。本发明可以提高陀螺误差建模和误差抑制能力。相比于传统MEMS IMU陀螺误差建模抑制方法，本方法能有效减小水下机器人运动过程中剧烈晃动对陀螺误差抑制的影响，进一步提高陀螺提供信息的准确性。
1.一种基于改进AR模型和平滑滤波的陀螺误差抑制方法，该方法基于已知量MEMSIMU陀螺输出的设备系下的三维角速度信息Ωgyro＝[Ωx、Ωy、Ωz]，其中Ωx、Ωy、Ωz分别表示x方向、y方向、z方向的角速度信息，上标T表示矩阵的转置；其特征在于，该方法包括如下步骤：(1)采用滑动窗3阶拉盖尔多项式抗差拟合处理方法对陀螺输出信息进行抗差处理；
(2)使用步骤(1)得到的陀螺角速度信息建立改进的实时AR模型；
(3)使用以IGGIII法为权函数的抗差最小二乘估计参数；
设置滑动窗口长度为N＝10，当陀螺采样个数，即N＜10时不做处理，当N≥10时分别对陀螺的测量信息处理如下：其中，Ωx，t表示t时刻陀螺输出的x方向的角速度信息，Ωy，t表示t时刻陀螺输出的y方向的角速度信息，Ωz，t表示t时刻陀螺输出的z方向的角速度信息，定义陀螺的输出频率为
100Hz，即0.01s输出一次数据，按照N＝10进行滑动窗处理，即t的取值范围如下：经过该抗差处理后可得到一系列序列号为k的陀螺信息为：其中， 表示经过抗差处理后的k时刻的陀螺角速度信息，Ωx，k、Ωy，k、Ωz，k分别表示k时刻x方向、y方向、z方向的角速度信息；
设置窗口长度N＝20的滑动窗，首先对每一个滑动窗取均值为：其中 为k时刻滑动窗内陀螺信息均值；
其中n为AR模型阶数， 分别表示k‑1时刻、k‑2时刻、…、k‑n时刻陀螺测得的角速度信息，θ1…θn为传统AR模型待估参数， 代表k时刻白噪声序列， 代表AR模型改进后的待估参数，且 矩阵形式为：其中Y为k时刻陀螺测得的角速度信息，θ为传统AR模型待估参数， 表示k时刻之前陀螺测得的角速度信息，a代表k时刻白噪声序列。
2.根据权利要求1所述的一种基于改进AR模型和平滑滤波的陀螺误差抑制方法，其特征在于，步骤(3)的具体方法是：使用以IGGIII法为权函数的抗差最小二乘估计，IGGIII法的权因子为：其中 为观测量的抗差等价权，Pt为原始权因子， 为标准化残差向量，k0，k1为控制常量，k0取1.0～1.5，k1取2.5～3.0，则可求得传统AR模型待估参数θ为：
3.根据权利要求2所述的一种基于改进AR模型和平滑滤波的陀螺误差抑制方法，其特征在于，步骤(4)的具体方法是：卡尔曼滤波过程如下：
其中Xk为k时刻的系统状态， 由步骤2和步骤3获得；A，B为系统参数，Wk为过程噪声，系统的测量值为：Y＝HXk+Vk
其中H为测量系统参数，Vk表示测量噪声，卡尔曼滤波过程如下：Xk，k‑1＝AXk‑1
Xk＝Xk，k‑1+ΦK(Y‑HXk，k‑1)T T ‑1
ΦK＝Pk，k‑1H(HPk，k‑1H+R)T T
其中Xk，k‑1是利用上一状态预测的结果，ΦK为卡尔曼增益，Pk，k‑1为Xk，k‑1对应的协方差，Pk‑1为Xk‑1对应的协方差，Pk为Xk对应的协方差，Q表示系统过程的方差阵，I表示单位矩阵；R为量测噪声方差阵；
下面进入后向平滑过程更新k‑1时刻的估计值：Xk‑1，k＝Xk‑1+KS[Xk‑Xk，k‑1]T
Pk‑1，k＝Pk‑1+KS[Pk‑Pk，k‑1]KS其中Xk‑1，k为后向平滑后的状态估计，Xk‑1为上一状态最优的结果，Pk‑1，k为后向平滑后的状态估计方差，KS为后向平滑增益；
将所得Xk‑1，k，Pk‑1，k重新代回卡尔曼滤波过程，得到最终结果。
一种基于改进AR模型和平滑滤波的陀螺误差抑制方法
技术领域
[0001] 本发明属于水下机器人陀螺仪误差补偿技术领域，具体涉及一种基于改进AR模型和平滑滤波的陀螺误差抑制方法。
背景技术
[0002] 水下机器人作为水下作业的重要载体，因其体积小，灵活度高，在海洋资源勘测、水下设备检测等领域发挥着重要作用。水下机器人在水下作业需要时刻获得自身的位置和姿态信息。近年来小型低成本的水下机器人运用越来越普遍，相对应的需要一种小型低成本的惯性器件为机器人提供位姿信息。随着微机电系统的快速发展，基于微机电系统的水下导航传感器也应运而生，微机械惯性测量单元（MEMS IMU）具有体积小、成本低等优点，但是相对而言，其随机误差较大。
[0003] 水下环境相较于地面更为复杂，因此水下机器人在水下航行时会出现由于剧烈晃动及陀螺受到随机扰动而导致误差补偿能力下降问题。
发明内容
[0004] 针对上述问题，本发明提出一种基于改进AR模型和平滑滤波的陀螺误差抑制方法。首先，为提高陀螺仪角速度信息的抗干扰能力，提出一种滑动窗拉盖尔多项式抗差拟合处理方法，使用此方法对陀螺数据进行预处理，去除掉扰动项；其次，为提高水下机器人航行过程中陀螺输出的信息的稳定性性，本发明将滑动窗算法引入改进AR模型中，提高陀螺仪输出数据剧烈波动情况下对误差精确建模的能力；最后，本发明给出一种基于后向平滑卡尔曼滤波的误差抑制方法。
[0005] 为达到上述目的，本发明的技术方案如下：
[0006] 一种基于改进AR模型和平滑滤波的陀螺误差抑制方法，该方法基于已知量MEMS IMU陀螺输出的设备系下的三维角速度信息 ，其中 分别表示x方向、y方向、z方向的角速度信息，上标T表示矩阵的转置；其特征在于，该方法包括如下步骤：
[0007] （1）采用滑动窗3阶拉盖尔多项式抗差拟合处理方法对陀螺输出信息进行抗差处理；
[0008] （2）使用步骤（1）得到的陀螺角速度信息建立改进的实时AR模型；
[0009] （3）使用以IGGIII法为权函数的抗差最小二乘估计参数；
[0010] （4）后向平滑卡尔曼滤波补偿。
[0011] 进一步地，步骤（1）的具体方法是：
[0012] 设置滑动窗口长度为N=10，当陀螺采样个数，即N＜10时不做处理，当N≥10时分别对陀螺的测量信息处理如下：
[0013]
[0014] 其中， 表示t时刻陀螺输出的x方向的角速度信息， 表示t时刻陀螺输出的y方向的角速度信息， 表示t时刻陀螺输出的z方向的角速度信息，定义陀螺的输出频率为100Hz，即0.01s输出一次数据，按照N=10进行滑动窗处理，即t的取值范围如下：
[0015]
[0016] 经过该抗差处理后可得到一系列序列号为k的陀螺信息为：
[0017]
[0018] 其中， 表示经过抗差处理后的k时刻的陀螺角速度信息， 分别表示k时刻x方向、y方向、z方向的角速度信息。
[0019] 进一步地，步骤（2）的具体方法是：
[0020] 设置窗口长度N=20的滑动窗，首先对每一个滑动窗取均值为：
[0021]
[0022] 其中 为k时刻滑动窗内陀螺信息均值；
[0023] 随后建立改进的AR模型如下：
[0024]
[0025] 其中n为AR模型阶数， 分别表示k‑1时刻、k‑2时刻、…、k‑n时刻陀螺测得的角速度信息， 为传统AR模型待估参数， 代表k时刻白噪声序列， 代表AR模型改进后的待估参数，且 ，矩阵形式
为：
[0026]
[0027] 其中Y为k时刻陀螺测得的角速度信息，为传统AR模型待估参数待估参数， 表示k时刻之前陀螺测得的角速度信息， 代表k时刻白噪声序列。
[0028] 进一步地，步骤（3）的具体方法是：
[0029] 使用以IGGIII法为权函数的抗差最小二乘估计，IGGIII法的权因子为：
[0030]
[0031] 其中 为观测量的抗差等价权， 为原始权因子， 为标准化残差向量，为控制常量， 取1.0 1.5， 取2.5 3.0，则可求得传统AR模型待估参数 为：~ ~
[0032] 。
[0033] 进一步地，步骤（4）的具体方法是：
[0034] 卡尔曼滤波过程如下：
[0035] 系统的状态方程为：
[0036]
[0037] 其中 为k时刻的系统状态， ，由步骤2和步骤3获得；为系统参数， 为过程噪声，系统的测量值为：
[0038]
[0039] 其中H为测量系统参数， 表示测量噪声，卡尔曼滤波过程如下：
[0040]
[0041] 其中 是利用上一状态预测的结果， 为卡尔曼增益， 为 对应的协方差， 为 对应的协方差， 为 对应的协方差， 表示系统过程的方差阵，I表示单位矩阵；R为量测噪声方差阵；
[0042] 下面进入后向平滑过程更新k‑1时刻的估计值：
[0043]
[0044] 其中 为后向平滑后的状态估计， 为上一状态最优的结果， 为后向平滑后的状态估计方差， 为后向平滑增益；
[0045] 将所得 重新代回卡尔曼滤波过程，得到最终结果。
[0046] 与现有技术相比，本发明具有如下优点：
[0047] (1) 本发明针对水下航行器陀螺仪输出角速度信息受水下环境干扰而易产生非正常扰动项的问题，引入拉盖尔多项式对陀螺信息进行预处理，提出一种滑动窗3阶拉盖尔多项式抗差拟合处理方法，进一步提高了水下航行器装载的陀螺仪测量角速度的抗干扰能力。
[0048] (2) 本发明针对水下航行器行驶时容易剧烈晃动而使陀螺输出信息波动的问题，本发明引入滑动窗改进AR模型，通过滑动窗减小陀螺输出波动影响，进一步提高了陀螺误差建模的准确性。
[0049] (3) 本发明针对传统卡尔曼滤波利用上一时刻的状态量递推当前时刻的状态时，上一时刻的状态估计对当前时刻状态估计精度影响很大的问题，本发明引入后向平滑滤波算法，对当前时刻的估计结果进行递归计算，提高了误差补偿精度。
附图说明
[0050] 图1为本发明所描述的基于改进AR模型和平滑滤波的陀螺误差抑制方法流程图；
[0051] 图2实验车辆运动轨迹曲线；
[0052] 图3不同方法的位置误差曲线。
具体实施方式
[0053] 下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
[0054] 一种基于改进AR模型和平滑滤波的陀螺误差抑制方法，该方法基于已知量MEMS IMU陀螺输出的设备系下的三维角速度信息 ，其中 分别表示x方向、y方向、z方向的角速度信息，上标T表示矩阵的转置；如图1所示，本发明的方法包括如下步骤：
[0055] （1）为减小陀螺输出数据时的不正常扰动，提高陀螺测量信息的鲁棒性，本发明设计一种采用滑动窗3阶拉盖尔多项式抗差拟合处理方法对陀螺输出信息进行抗差处理；
[0056] 设置滑动窗口长度为N=10，当陀螺采样个数，即N＜10时不做处理，当N≥10时分别对陀螺的测量信息处理如下：
[0057]
[0058] 其中， 表示t时刻陀螺输出的x方向的角速度信息， 表示t时刻陀螺输出的y方向的角速度信息， 表示t时刻陀螺输出的z方向的角速度信息，定义陀螺的输出频率为100Hz，即0.01s输出一次数据，按照N=10进行滑动窗处理，即t的取值范围如下：
[0059]
[0060] 经过该抗差处理后可得到一系列序列号为k的陀螺信息为：
[0061]
[0062] 其中， 表示经过抗差处理后的k时刻的陀螺角速度信息， 分别表示k时刻x方向、y方向、z方向的角速度信息。
[0063] （2）针对水下航行器运动过程中出现剧烈晃动而导致传统AR模型对误差无法精准建模的问题，使用步骤（1）得到的陀螺角速度信息建立改进的实时AR模型；
[0064] 设置窗口长度N=20的滑动窗，首先对每一个滑动窗取均值为：
[0065]
[0066] 其中 为k时刻滑动窗内陀螺信息均值；
[0067] 随后建立改进的AR模型如下：
[0068]
[0069] 其中n为AR模型阶数， 分别表示k‑1时刻、k‑2时刻、…、k‑n时刻陀螺测得的角速度信息， 为传统AR模型待估参数， 代表k时刻白噪声序列， 代表AR模型改进后的待估参数，且 ，矩阵形式
为：
[0070]
[0071] 其中Y为k时刻陀螺测得的角速度信息，为传统AR模型待估参数待估参数， 表示k时刻之前陀螺测得的角速度信息， 代表k时刻白噪声序列。
[0072] （3）使用以IGGIII法为权函数的抗差最小二乘估计参数；
[0073] 使用以IGGIII法为权函数的抗差最小二乘估计，IGGIII法的权因子为：
[0074]
[0075] 其中 为观测量的抗差等价权， 为原始权因子， 为标准化残差向量，为控制常量， 取1.0 1.5， 取2.5 3.0，则可求得传统AR模型待估参数 为：~ ~
[0076] 。
[0077] （4）后向平滑卡尔曼滤波补偿。
[0078] 卡尔曼滤波过程如下：
[0079] 系统的状态方程为：
[0080]
[0081] 其中 为k时刻的系统状态， ，由步骤2和步骤3获得；为系统参数， 为过程噪声，系统的测量值为：
[0082]
[0083] 其中H为测量系统参数， 表示测量噪声，卡尔曼滤波过程如下：
[0084]
[0085] 其中 是利用上一状态预测的结果， 为卡尔曼增益， 为 对应的协方差， 为 对应的协方差， 为 对应的协方差， 表示系统过程的方差阵，I表示单位矩阵；R为量测噪声方差阵；
[0086] 下面进入后向平滑过程更新k‑1时刻的估计值：
[0087]
[0088] 其中 为后向平滑后的状态估计， 为上一状态最优的结果， 为后向平滑后的状态估计方差， 为后向平滑增益；
[0089] 将所得 重新代回卡尔曼滤波过程，得到最终结果。
[0090] 为了验证本发明方法的有效性，开展车载惯性导航实验。使用光纤陀螺惯性测量单元设备。其中图2给出了实验过程中车辆的运动轨迹路线。图3是对应的位置误差曲线，其中，纯惯性方法误差最大；本发明方法的定位误差小于传统AR模型方法。
[0091] 需要说明的是，以上内容仅仅说明了本发明的技术思想，不能以此限定本发明的保护范围，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰均落入本发明权利要求书的保护范围之内。
