本发明提供一种基于AHRS和DVL的水下机器人自主航位推算方法，其主要目的在于提高水下机器人基于AHRS和DVL的自主航位推算系统的抗干扰能力。本发明将小型AHRS和DVL用于水下机器人自主航位推算系统中，整个过程主要步骤包括：DVL输出速度信息抗差处理、DVL安装误差及刻度因子误差补偿、基于AHRS的坐标系转换矩阵计算、基于航位推算的速度更新以及水下机器人位置推算。本发明可以解决水下机器人自主定位系统干扰问题，提高自主航位推算精度。相比传统航位推算方法。本方法能提高系统的够抗干扰能力，进一步提高水下机器人的定位精度。
1.一种基于AHRS和DVL的水下机器人自主航位推算方法，基于AHRS和DVL的水下机器人的己知量：多普勒计程仪，缩写为DVL，输出的载体系下的三维速度信息其中， 表示载体坐标系下x方向的速度信息， 表示载体坐标系下y方向的速度信息，表示载体坐标系下z方向的速度信息，上标b表示载体坐标系；
航姿参考系统，缩写为AHRS，输出的导航坐标系下的姿态信息包括俯仰角θ、横滚角γ、航向角ψ；
DVL和AHRS之间的安装误差角：φ＝[φx φy φz]，其中φx表示x方向安装误差角，φy表示y方向安装误差角，φz表示z方向安装误差角；
水下机器人初始位置：P0＝[L0 λ0 h0]，其中，L0表示初始位置经度，λ0表示初始位置纬度，h0表示初始位置高度；
步骤1.采用滑动窗5阶切比雪夫多项式抗差拟合处理方法对DVL输出速度信息进行抗差处理：步骤2.在步骤1的基础上对DVL安装误差及刻度因子误差补偿处理得到载体坐标系下t时刻的DVL速度步骤3.引入滑动窗中值滤波算法对AHRS输出的姿态信息进行预处理后计算载体坐标系到导航坐标系的姿态转移矩阵 其中上标n表示导航坐标系；
步骤4.在步骤3得到的姿态转移矩阵 和步骤2得到的DVL载体系下速度的基础上，可得基于航位推算的速度更新；
设置滑动窗口长度，即DVL采样个数N＝12；当N＜12时不做处理，当N≥12时分别对DVL的测量信息处理如下：其中， 表示t时刻DVL输出的x方向的速度信息， 表示t时刻DVL输出的y方向的速度信息， 表示t时刻DVL输出的z方向的速度信息，上标d表示设备坐标系；定义DVL的输出频率为1Hz，即1s输出一次数据；
按照N＝12进行滑动窗处理，即DVL的测量信息处理公式中t的取值范围如下：t＝t‑N+1，…，t
经过DVL的测量信息处理后可得到t时刻的DVL信息为：其中， 表示经过处理后的t时刻的DVL速度信息，上标T表示矩阵的转置；
引入滑动窗中值滤波算法对姿态信息进行预处理，具体如下：θ＝med{θt‑M+1，…，θt}
其中，M＝5为窗口长度，med{·}表示取中间值，θt表示AHRS输出的t时刻的俯仰角信息，γt表示AHRS输出的t时刻的横滚角信息，ψt表示AHRS输出的t时刻的航向角信息，当AHRS输出的数量长度小于5时不做处理；当AHRS输出的数据长度大于等于5时，进行滑动窗中值滤波算法处理；
经过滑动窗中值滤波算法处理后计算载体系到导航系的姿态转移矩阵，以方便后续使用，具体计算如下：其中， 表示载体系到导航系的姿态转移矩阵。
2.根据权利要求1所述的基于AHRS和DVL的水下机器人自主航位推算方法，其特征在于，步骤2具体包括如下步骤：对DVL误差进行补偿处理，误差补偿公式如下：其中， 表示安装误差角姿态转移矩阵，为己知量，表示如下：
3.根据权利要求1或2所述的基于AHRS和DVL的水下机器人自主航位推算方法，其特征在于，步骤4具体包括如下步骤：基于航位推算的速度更新如下：
其中， 表示t时刻航位推算得到的导航系下的速度信息，该速度包括三个方向的速度信息，描述如下：其中， 表示东向速度； 表示北向速度； 表示天向速度。
4.根据权利要求1或2所述的基于AHRS和DVL的水下机器人自主航位推算方法，其特征在于，步骤5具体包括如下步骤：当t＝1时：
其中，P0表示水下机器人的初始位置，Pt表示水下机器人t时刻的位置，ΔLt表示t时刻经度增量，Δλt表示t时刻纬度增量，Δht表示t时刻高度增量，计算公式为：其中，Ts表示DVL采样周期；
Rn＝RE·(1‑2×AEE+3×AEE×sin(λ0)×sin(λ0))Ree＝RE×(1+AEE×sin(L0)×sin(L0))当t＞1时：
其中，Pt‑1表示水下机器人上一时刻的位置，ΔLt表示t时刻经度增量，Δλt表示t时刻纬度增量，Δht表示t时刻高度增量，计算公式为：Rn＝RE·(1‑2×AEE+3×AEE×sin(λt‑1)×sin(λt‑1))Ree＝RE×(1+AEE×sin(Lt‑1)×sin(Lt‑1))其中，RE表示地球半径，AEE表示地球扁率，Rn表示地球子午圈，Ree表示曲率半径，ht‑1表示t‑1时刻的高度信息，Lt‑1表示t‑1时刻的经度信息，λt‑1表示t‑1时刻的纬度信息。
基于AHRS和DVL的水下机器人自主航位推算方法
技术领域
[0001] 本发明属于水下机器人导航定位技术，具体涉及基于AHRS和DVL的水下机器人自主航位推算方法。
背景技术
[0002] 水下机器人作为水下作业的重要载体，在海洋资源勘测、水下设备检测等领域发挥着重要作用。水下机器人的顺利作业离不开高精度的定位系统。近年来特别是随着小型机器人、机器人集群的出现，迫切需要一种小型低成本水下机器人自主定位系统。随着微机电系统的快速发展，基于微机电系统的水下导航传感器也应运而生，其中航姿参考系统(AHRS)便是重要的组成部分。AHRS能够为载体提供准确的姿态信息，具有价格低、体积小的特点。本发明结合AHRS和小型多普勒计程仪(DVL)，给出一种自主航位推算方法。
[0003] 不同于地面环境，水下环境复杂多变。因此，基于DVL的声学信号传播存在不确定性和干扰性。因此，基于AHRS和DVL的自主航位推算系统需要重点考虑其鲁棒性和抗干扰能力问题。
发明内容
[0004] 发明目的：针对上述问题，本发明提出一种基于AHRS和DVL的水下机器人自主航位推算方法，首先，为提高DVL速度信息的抗干扰能力，提出一种滑动窗5阶切比雪夫多项式抗差拟合处理方法，并将该方法用于DVL输出的速度信息与处理中；其次，为提高AHRS输出的姿态信息的稳定性性，本发明引入滑动窗中值滤波算法对姿态信息进行预处理；最后，本发明给出一种考虑地球模型参数的高精度的位置推算方法。
[0005] 本发明的基于AHRS和DVL的水下机器人自主航位推算方法，基于AHRS和DVL的水下机器人的已知量：
[0006] 多普勒计程 仪，缩写为D VL，输出的载体 系下的三维速度 信息其中， 表示载体坐标系下x方向的速度信息， 表示载体坐标
b
系下y方向的速度信息，Vz表示载体坐标系下z方向的速度信息，上标b表示载体坐标系；
[0007] 航姿参考系统，缩写为AHRS输出的导航坐标系下的姿态信息包括俯仰角θ、横滚角γ、航向角ψ；
[0008] DVL和AHRS之间的安装误差角：φ＝[φx φy φz]，其中φx表示x方向安装误差角，φy表示y方向安装误差角，φz表示z方向安装误差角；
[0009] DVL的刻度因子误差：δK；
[0010] 水下机器人初始位置：P0＝[L0 λ0 h0]，其中，L0表示初始位置经度；λ0表示初始位置纬度；h0表示初始位置高度；
[0011] 该方法包括如下步骤：
[0012] 步骤1.采用滑动窗5阶切比雪夫多项式抗差拟合处理方法对DVL输出速度信息进行抗差处理；
[0013] 步骤2.在步骤1的基础上对DVL安装误差及刻度因子误差补偿处理得到载体坐标系下t时刻的DVL速度
[0014] 步骤3.引入滑动窗中值滤波算法对AHRS输出的姿态信息进行预处理后计算载体坐标系到导航坐标系的姿态转移矩阵 其中上标n表示导航坐标系；
[0015] 步骤4.在步骤3得到的姿态转移矩阵 和步骤2得到的DVL载体系下速度的基础上，可得基于航位推算的速度更新；
[0016] 步骤5.进行水下机器人位置推算。
[0017] 进一步地，步骤1具体包括如下步骤：
[0018] 设置滑动窗口长度，即DVL采样个数N＝12；当N＜12时不做处理，当N≥12时分别对DVL的测量信息处理如下：
[0019]
[0020]
[0021]
[0022] 其中， 表示t时刻DVL输出的x方向的速度信息， 表示t时刻DVL输出的y方向的速度信息， 表示t时刻DVL输出的z方向的速度信息，上标d表示设备坐标系；定义DVL的输出频率为1Hz，即1s输出一次数据；
[0023] 按照N＝12进行滑动窗处理，即DVL的测量信息处理公式中t的取值范围如下：
[0024] t＝t‑N+1，...，t
[0025] 经过DVL的测量信息处理后可得到t时刻的DVL信息为：
[0026]
[0027] 其中， 表示经过处理后的t时刻的DVL速度信息，上标T表示矩阵的转置。
[0028] 进一步地，步骤2具体包括如下步骤：
[0029] 对DVL误差进行补偿处理，误差补偿公式如下：
[0030]
[0031] 其中， 表示安装误差角姿态转移矩阵，为己知量，表示如下：
[0032]
[0033] 进一步地，步骤3具体包括如下步骤：
[0034] 引入滑动窗中值滤波算法对姿态信息进行预处理，具体如下：
[0035] θ＝med{θt‑M+1，…，θt}
[0036] γ＝med{γt‑M+1，…，γt}
[0037] ψ＝med{ψt‑M+1，…，ψt}
[0038] 其中，M＝5为窗口长度，med{·}表示取中间值，θt表示AHRS输出的t时刻的俯仰角信息，γt表示AHRS输出的t时刻的横滚角信息，ψt表示AHRS输出的t时刻的航向角信息，当AHRS输出的数量长度小于5时不做处理；当AHRS输出的数据长度大于等于5时，进行滑动窗中值滤波处理；
[0039] 经过滑动窗中值滤波算法处理后计算载体系到导航系的姿态转移矩阵，以方便后续使用，具体计算如下：
[0040]
[0041] 其中， 表示载体系到导航系的姿态转移矩阵。
[0042] 进一步地，步骤4具体包括如下步骤：
[0043] 基于航位推算的速度更新如下：
[0044]
[0045] 其中， 表示t时刻航位推算得到的导航系下的速度信息，该速度包括三个方向的速度信息，描述如下：
[0046]
[0047] 其中， 表示东向速度， 表示北向速度， 表示天向速度。
[0048] 进一步地，步骤5具体包括如下步骤：
[0049] 当t＝1时：
[0050]
[0051] 其中，P0表示水下机器人的初始位置，Pt表示水下机器人t时刻的位置，ΔLt表示t时刻经度增量，Δλt表示t时刻纬度增量，Δht表示t时刻高度增量，计算公式为：
[0052]
[0053] 其中，Ts表示DVL采样周期；
[0054] Rn＝RE·(1‑2×AEE+3×AEE×sin(λ0)×sin(λ0))
[0055] Ree＝RE×(1+AEE×sin(L0)×sin(L0))
[0056] 当t＞1时：
[0057]
[0058] 其中，Pt‑1表示水下机器人上一时刻的位置，ΔLt表示t时刻经度增量，Δλt表示t时刻纬度增量，Δht表示t时刻高度增量，计算公式为：
[0059]
[0060] Rn＝RE·(1‑2×AEE+3×AEE×sin(λt‑1)×sin(λt‑1))
[0061] Ree＝RE×(1+AEE×sin(Lt‑1)×sin(Lt‑1))
[0062] 其中，RE表示地球半径，AEE表示地球扁率，Rn表示地球子午圈，Ree表示曲率半径，ht‑1表示t‑1时刻的高度信息，Lt‑1表示t‑1时刻的经度信息，λt‑1表示t‑1时刻的纬度信息。
[0063] 与现有技术相比，本发明具有如下优点：
[0064] (1)本发明针对DVL输出速度信息受水下环境干扰问题，引入切比雪夫多项式进行预处理，提出一种滑动窗5阶切比雪夫多项式抗差拟合处理方法，进一步提高了水下DVL测速的抗干扰能力。
[0065] (2)本发明针对AHRS输出的姿态信息的稳定性问题，本发明引入滑动窗中值滤波算法对AHRS输出的姿态信息进行预处理，进一步提高了机器人姿态测量的稳定性。
[0066] (3)本发明为解决水下机器人小型化低成本定位问题，给出了一种基于AHRS和DVL的小型自主航位推算解决方案，考虑地球模型参数，提出一种高精度位置推算方法。
附图说明
[0067] 图1为本发明所描述的基于AHRS和DVL的水下机器人自主航位推算方法流程图；
[0068] 图2为本发明方法的仿真航位推算轨迹图；
[0069] 图3为本发明方法的仿真航位推算的东向位置误差、北向位置误差和天向位置误差曲线。
具体实施方式
[0070] 下面对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。
[0071] 一种基于AHRS和DVL的水下机器人自主航位推算方法，该方法基于AHRS和DVL的水下机器人的己知量：
[0072] 多普勒计程 仪，缩写为D VL，输出的载体 系下的三维速度 信息其中， 表示载体坐标系下x方向的速度信息， 表示载体坐
b
标系下y方向的速度信息，Vz表示载体坐标系下z方向的速度信息，上标b表示载体坐标系；
[0073] 航姿参考系统，缩写为AHRS输出的导航坐标系下的姿态信息包括俯仰角θ、横滚角γ、航向角ψ；
[0074] DVL和AHRS之间的安装误差角：φ＝[φx φy φz]，其中φx表示x方向安装误差角；φy表示y方向安装误差角；φz表示z方向安装误差角；
[0075] DVL的刻度因子误差：δK；
[0076] 水下机器人初始位置：P0＝[L0 λ0 h0]，其中，L0表示初始位置经度；λ0表示初始位置纬度；h0表示初始位置高度；
[0077] 如图1所示，该方法具体包括如下步骤：
[0078] 步骤1：DVL输出速度信息抗差处理
[0079] 为提高DVL测量数据的鲁棒性，本发明设计一种滑动窗5阶切比雪夫多项式抗差拟合处理方法。设置滑动窗口长度，即DVL采样个数N＝12；当N＜12时不做处理，当N≥12时分别对DVL的测量信息处理如下：
[0080]
[0081]
[0082]
[0083] 其中， 表示t时刻DVL输出的x方向的速度信息， 表示t时刻DVL输出的y方向的速度信息， 表示t时刻DVL输出的z方向的速度信息，上标d表示设备坐标系；定义DVL的输出频率为1Hz，即1s输出一次数据；
[0084] 按照N＝12进行滑动窗处理，即DVL的测量信息处理公式中t的取值范围如下：
[0085] t＝t‑N+1，...，t
[0086] 经过DVL的测量信息处理后可得到t时刻的DVL信息为：
[0087]
[0088] 其中， 表示经过处理后的t时刻的DVL速度信息，上标T表示矩阵的转置。
[0089] 步骤2：DVL安装误差及刻度因子误差补偿
[0090] 接着，在上述获得的DVL信息的基础上对DVL误差进行补偿处理。考虑DVL和AHRS之间的安装误差角以及DVL自身刻度因子误差，本发明对DVL误差进行补偿处理。误差补偿公式如下：
[0091]
[0092] 其中， 表示安装误差角姿态转移矩阵，为已知量，表示如下：
[0093]
[0094] 步骤3：基于AHRS的坐标系转换矩阵计算
[0095] 为提高AHRS输出的姿态信息的稳定性，本发明引入滑动窗中值滤波算法对姿态信息进行预处理，具体如下：
[0096] θ＝med{θt‑M+1，…，θt}
[0097] γ＝med{γt‑M+1，…，γt}
[0098] ψ＝med{ψt‑M+1，…，ψt}
[0099] 其中，M＝5为窗口长度，med{·}表示取中间值，θt表示AHRS输出的t时刻的俯仰角信息，γt表示AHRS输出的t时刻的横滚角信息，ψt表示AHRS输出的t时刻的航向角信息，当AHRS输出的数量长度小于5时不做处理；当AHRS输出的数据长度大于等于5时，进行滑动窗中值滤波处理。
[0100] 经过滑动窗中值滤波算法处理后计算载体系到导航系的姿态转移矩阵，以方便后续使用，具体计算如下：
[0101]
[0102] 其中， 表示载体系到导航系的姿态转移矩阵，其中上标n表示导航坐标系。
[0103] 步骤4：基于航位推算的速度更新
[0104] 在上述姿态转移矩阵 和DVL载体系下速度 的基础上，可得基于航位推算的速度更新如下：
[0105]
[0106] 其中， 表示t时刻航位推算得到的导航系下的速度信息，该速度包括三个方向的速度信息，描述如下：
[0107]
[0108] 其中， 表示东向速度， 表示北向速度， 表示天向速度。
[0109] 步骤5：水下机器人位置推算
[0110] 当t＝1时：
[0111]
[0112] 其中，P0表示水下机器人的初始位置，Pt表示水下机器人t时刻的位置，ΔLt表示t时刻经度增量，Δλt表示t时刻纬度增量，Δht表示t时刻高度增量，计算公式为：
[0113]
[0114] 其中，Ts表示DVL采样周期；
[0115] Rn＝RE·(1‑2×AEE+3×AEE×sin(λ0)×sin(λ0))
[0116] Ree＝RE×(1+AEE×sin(L0)×sin(L0))
[0117] 当t＞1时：
[0118]
[0119] 其中，Pt‑1表示水下机器人上一时刻的位置，ΔLt表示t时刻经度增量，Δλt表示t时刻纬度增量，Δht表示t时刻高度增量，计算公式为：
[0120]
[0121] Rn＝RE·(1‑2×AEE+3×AEE×sin(λt‑1)×sin(λt‑1))
[0122] Ree＝RE×(1+AEE×sin(Lt‑1)×sin(Lt‑1))
[0123] 其中，RE表示地球半径，AEE表示地球扁率，Rn表示地球子午圈，Ree表示曲率半径，ht‑1表示t‑1时刻的高度信息，Lt‑1表示t‑1时刻的经度信息，λt‑1表示t‑1时刻的纬度信息。
[0124] 下面结合具体仿真实验，进一步阐明本发明，仿真参数设置如下：
[0125] 仿真AHRS系统的航向角误差为：0.3°、俯仰角误差为0.1°、横滚角误差为0.1度；AHRS的数据更新频率为100Hz；DVL刻度因子误差0.005；DVL输出频率1Hz；AHRS和DVL之间的安装误差为：0.2°、‑0.1°和0.3°。其中图2给出了本发明的航位推算方法的轨迹图，从图2中可以看出本发明轨迹紧紧跟随参考轨迹，误差发散较小；图3给出了东向位置误差、北向位置误差和天向位置误差曲线，可见本发明具有较好的鲁棒性。
