CN106896361A - 一种深水机器人多模型ekf组合导航装置及方法 - Google Patents

一种深水机器人多模型ekf组合导航装置及方法 Download PDF

Info

Publication number
CN106896361A
CN106896361A CN201510953833.4A CN201510953833A CN106896361A CN 106896361 A CN106896361 A CN 106896361A CN 201510953833 A CN201510953833 A CN 201510953833A CN 106896361 A CN106896361 A CN 106896361A
Authority
CN
China
Prior art keywords
deep water
water robot
model
state
robot
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201510953833.4A
Other languages
English (en)
Inventor
李冬冬
张瑶
陈巩
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenyang Institute of Automation of CAS
Original Assignee
Shenyang Institute of Automation of CAS
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenyang Institute of Automation of CAS filed Critical Shenyang Institute of Automation of CAS
Priority to CN201510953833.4A priority Critical patent/CN106896361A/zh
Publication of CN106896361A publication Critical patent/CN106896361A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
    • G01S15/06—Systems determining the position data of a target
    • G01S15/08—Systems for measuring distance only
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves
    • G01S5/30—Determining absolute distances from a plurality of spaced points of known location

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Acoustics & Sound (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Feedback Control In General (AREA)

Abstract

本发明涉及深水机器人技术领域，尤其涉及一种用于深水机器人组合导航方法。其中该方法相关装置包括测距仪、声信标、惯性导航传感器和导航计算机。方法为：深水机器人下水后，导航计算机对测距仪、航向传感器、航速传感器和深度传感器进行数据采集；依据距离量测数据的数量和质量确定深水机器人的导航阶段；当深水机器人在组合导航阶段时，建立多个深水机器人运动学模型，并在距离量测数据更新时，确定每个模型对应的权重，进而求得所有导航模型的加权输出。此加权输出便是组合导航算法的输出值。本方法稳定可靠，深水环境导航精度高，应用范围广，而且所使用的装置和方法简单，继承性好。

Description

一种深水机器人多模型EKF组合导航装置及方法
技术领域
本发明涉及深水机器人技术领域，尤其涉及一种深水机器人多模型EKF组合导航装置及方法。
背景技术
深水机器人航行深度可达数千米。在水下航行过程中，导航误差会随着路程和时间的增加而逐渐增大。当深水机器人航行一段距离后，需要对其位置进行修正，以提高导航精度。
当前对深水机器人进行位置修正的方法主要是：通过声学定位***输出的定位信息对深水机器人位置进行直接修正。但这种方法存在一定的缺陷：
1)无法消除声学传播时延引起的定位误差；
2)水下机器人标定存在偏差，会导致导航偏差随时间累积；
3)深水机器人航行轨迹不平滑；
4)无法有效地克服野值的干扰。
发明内容
为了克服现有方法的不足，本发明要解决的技术问题是提供一种深水机器人多模型EKF组合导航装置及方法，使得航行轨迹平滑且定位误差小。
本发明为实现上述目的所采用的技术方案是：一种深水机器人多模型EKF组合导航装置，包括：
测距仪，安装于深水机器人壳体外部，连接深水机器人内部的导航计算机，利用声信号测量深水机器人到各个信标的距离；
信标，至少三个，布设于海底，坐标为已知；
传感器，包括航向传感器、航速传感器和深度传感器，均安装于深水机器人中并连接导航计算机，分别用于测量深水机器人的航向、航速和所处深度。
导航计算机，用于采集测距仪和传感器的输出信号，并采用多模型EKF组合导航方法实时计算深水机器人当前时刻的位置。
所述测距仪按固定周期进行测距，周期在3秒～60秒之间，量测数据内容包括信标的编号，信号双程传播时间，并将时间量测转化为距离量测。
所述导航计算机按固定周期采集数据，周期在0.2秒～2秒之间。
当导航计算机采集到测距仪的距离量测数据时，采用EKF算法估计深水机器人的位置；
判断导航计算机采集到的距离量测数据是否同时满足数量和质量上的要求；如果导航计算机采集到的距离量测数据同时满足数量和质量上的要求，则通过空间位置几何解算获得深水机器人的初始位置，并设置深水机器人处于组合导航阶段，并将此空间解算位置设置为深水机器人可靠的初始值；如果不满足要求，则此时深水机器人的导航状态处于组合导航初期阶段，组合导航初期阶段目的是通过空间位置解算确定深水机器人的初始位置；
在组合导航阶段，判断距离量测数据是否有效，如果距离量测数据无效，则此量测值为野值，舍弃该距离量测数据，返回通过航位推算方法估计深水机器人的位置步骤；如果距离量测数据有效，则首先确定深水机器人运动***的模型集，然后根据距离量测数据采用多模型EKF算法对深水机器人的位置进行在线修正。
所述深水机器人依据航向传感器和航速传感器的量测值，通过航位推算方法估计深水机器人的位置，具体为：
ve＝vksinθk (3)
vn＝vkcosθk (4)
式中，(x10,x20)为深水机器人的初始点，Δt为导航计算机的采集周期，(x1t,x2t)为推算航位，θk为航向传感器量测值，vk为航速传感器量测值，ve为深水机器人向东速度，vn为深水机器人向北速度。
所述当导航计算机采集到测距仪的距离量测数据时，采用EKF算法估计深水机器人的位置，包括以下步骤：
步骤1：设置AUV的初始状态为X0/0和初始协方差矩阵为P0/0，默认为深水机器人刚刚开始执行任务时候的经纬度位置；
步骤2：时间更新：通过以下时间更新方程，更新深水机器人的状态Xk+1/k和协方差矩阵Pk+1/k：
Xk+1/k＝A·Xk/k+B·uk (5)
Pk+1/k＝A·Pk/k·AT+B·Qk·BT (6)
其中，uk是由航向传感器量测值θk和航速传感器量测值vk计算而来，uk＝[ve vn]T，A为状态转移矩阵，AT为矩阵A的转置，B为控制输入矩阵，BT为B的转置，Xk/k为量测更新之后深水机器人的状态，Pk/k为量测更新之后深水机器人的状态协方差矩阵，Xk+1/k为深水机器人的一步预测状态，Pk+1/k为深水机器人的一步预测状态协方差矩阵；
步骤3：量测更新：当深水机器人获得测距仪的量测数据时，则采用量测更新方程矫正深水机器人的状态Xk+1/k和协方差矩阵Pk+1/k；
Kk+1＝Pk+1/k·Hk+1 T·(Hk+1·Pk+1/k·Hk+1 T+Rk+1)-1 (7)
Xk+1/k+1＝Xk+1/k+Kk+1·(y-yk+1) (8)
Pk+1/k+1＝(I-Kk+1·Hk+1)·Pk+1/k (9)
其中，Pk+1/k为深水机器人的一步预测状态协方差矩阵，Hk+1为量测矩阵，Rk+1为量测协方差矩阵，Kk+1为卡尔曼增益矩阵，y为实际量测值，yk+1为预测量测值，I为单位阵，Xk+1/k为深水机器人的一步预测状态，Xk+1/k+1为k+1时刻状态更新值，Pk+1/k+1为k+1时刻状态协方差矩阵更新值。
所述判断导航计算机采集到的距离量测数据是否同时满足数量和质量上的要求，具体为：
在下一个测距周期到来时，当一个测距周期内的测距数据数量大于或等于3时，对深水机器人水平面坐标进行解算，解算公式如下：
Ax＝v (10)
式中，
为第i号信标的三维坐标，(x1,x2,x3)为深水机器人水平面坐标和深度，为第i号信标到坐标系原点的水平间距，为第i号信标到深水机器人的水平距离，其中Ri表示第i号信标的单程传播距离；
将深水机器人水平面坐标与所述推算航位作差，然后求取均方根误差，即：
如果e≤Bias，则量测数据质量达到要求，并令空间位置解算标志变量N＝N+1，N的初始值为0，直到N≥4时，则量测数据数量和质量同时达到要求，则设置此时导航状态为组合导航阶段，并确定当前深水机器人水平面的初始位置是(x1,x2)；如果e＞Bias并且N＜4，则设置N＝0，其中，Bias范围为1米～100米，Bias是预设值。
所述在组合导航阶段，判断距离量测数据是否有效，具体为：
v(k+1)＝y-yk+1/k (12)
v(k+1)T·Sk -1·v(k+1)≤γ (13)
其中，y是实际量测值，yk+1/k是预测量测值，v(k+1)是新息，Sk是新息协方差矩阵，γ是阈值，γ的建议值为9.2。
判断公式(13)是否成立，如果成立，则使用当前测量值y；否则，此量测值为野值，无效，给予舍弃。
所述确定深水机器人运动***的模型集，根据***过程噪声Q的范围，确定合理的过程噪声最小协方差矩阵Qmin和过程噪声最大协方差矩阵Qmax，Qmin≤Q≤Qmax，取N组运动模型，模型集的确定方法是：
为每一个Qi建立一个EKF滤波器，每一个滤波器独立估计深水机器人的状态，i表示模型索引，N是预设值，表示模型的个数，当水下机器人的最大速度小于或等于3m/s时，N取值为10。
所述根据距离量测数据采用多模型EKF算法对深水机器人的位置进行在线修正，包括以下步骤：
步骤1：对每一个模型对应的滤波器进行时间更新，得到Xk+1/k，
Xi,k+1/k＝A·Xi,k/k+B·uk (15)
Pi,k+1/k＝A·Pi,k/k·AT+B·Qi,k·BT (16)
yi,k+1/k＝h(Xi,k+1/k) (17)
Xi,k+1/k+1＝Xi,k+1/k+Ki,k+1·(y-yi,k+1/k) (19)
Pi,k+1/k+1＝(I-Ki,k+1·Hi,k+1)·Pi,k+1/k (20)
其中，A是状态转移矩阵，Xi,k/k是第i个模型对应的状态，B是速度控制输入矩阵，uk是速度矢量，Xi,k+1/k表示第i个模型对应的预测状态，Pi,k+1/k表示第i个模型对应的预测状态协方差矩阵，Pi,k/k表示量测更新之后第i个模型对应的状态协方差矩阵，Qi,k表示第i个模型对应的过程噪声协方差矩阵，h(·)表示量测方程，yi,k+1/k表示第i个模型对应的预测量测值，Ki,k+1表示第i个模型对应的卡尔曼增益，Hi,k+1表示第i个模型对应的线性化之后量测矩阵，Rk+1是量测协方差矩阵，y是量测值，Xi,k+1/k+1表示第i个模型对应的状态更新值，Pi,k+1/k+1表示第i个模型对应的状态协方差矩阵更新值，I表示单位阵；
步骤2：当检测到距离量测信息时，计算深水机器人到信标的距离；并且依据量测，计算每一个模型对应的权重：
vi(k+1)＝y-yi,k+1/k,i＝1,...,N (21)
Sk+1＝H·Pk+1/k·HT+Rk+1 (22)
其中，y是量测值，yi,k+1/k表示第i个模型对应的预测量测值，vi(k+1)表示第i个模型对应的新息，Rk+1是量测协方差矩阵，H表示线性化之后的量测矩阵，Sk+1表示新息协方差矩阵，Pk+1/k是预测状态协方差矩阵，e(i)是权重，β(i)是归一化权重；
步骤3：更新深水机器人的状态和协方差矩阵：
Xk+1/k+1＝∑β(i)Xi,k+1/k+1 (25)
Pk+1/k+1＝∑β(i)Pi,k+1/k+1 (26)
其中，β(i)是归一化权重，Xi,k+1/k+1是第i个模型量测对应的状态更新值，Pi,k+1/k+1是第i个模型量测对应的状态协方差矩阵更新值，Xk+1/k+1是我们所需要估计的状态，Pk+1/k+1是我们所需要估计的状态协方差矩阵；
则Xk+1/k+1便是所求的深水机器人此时此刻的状态，Pk+1/k+1是对应的协方差矩阵。
本发明具有以下优点：
1.本发明稳定可靠，修正结果准确。本发明设计了确定初始位置算法和测距波门算法，保证了初始值和测量值的可靠、有效，位置估计能够稳定收敛，结果准确。
2.本发明所需装置简单，继承性好。该方法所需装置仅仅是几台信标，一台测距仪和一台计算机，无需其它辅助装置，测距仪安装简便，修正算法程序移植性好，可方便移植到各个深水机器人。
3.本发明应用范围广。本发明不但可以应用于深水机器人，还可以用于其它海洋相关设备，可适用于全海深导航。
附图说明
图1是本发明的深水机器人导航传感器配置图；
图2组合导航算法流程图；
图3测距仪获得量测数据示意图；
图4多模型EKF算法流程图。
具体实施方式
下面结合附图及实施例对本发明做进一步的详细说明。
本发明由不少于3台的信标和深水机器人组成。深水机器人导航传感器配置如图1所示，由测距仪、航向传感器、航速传感器、深度传感器组成。
测距仪，安装于深水机器人壳体外部，连接深水机器人内部的导航计算机，利用声信号测量深水机器人到各个信标的距离；
信标，至少三个，布设于海底，坐标为已知；
传感器，包括航向传感器、航速传感器和深度传感器，均安装于深水机器人中并连接导航计算机，分别用于测量深水机器人的航向、航速和所处深度。
导航计算机，用于采集测距仪和传感器的输出信号，并采用多模型EKF组合导航方法实时计算深水机器人当前时刻的位置。
所述测距仪按固定周期进行测距，周期在3秒～60秒之间，量测数据内容包括信标的编号，信号双程传播时间，并将时间量测转化为距离量测。
所述导航计算机按固定周期采集数据，周期在0.2秒～2秒之间。
多模型EKF组合导航算法如图2所示，深水机器人下水后，导航计算机对测距仪、航向传感器、航速传感器和深度传感器进行数据采集；首先深水机器人依据航向传感器和航速传感器的量测值，通过航位推算方法估计深水机器人的初始位置(初始位置不准确没有关系，在随后的滤波过程中，其导航精度不断提高)。
然后开启组合导航***，深水机器人依据惯性导航***和航位推算算法不断对自身进行定位；同时通过长基线***周期性地以广播方式向四周发射信号。
深水机器人依据航向传感器和航速传感器的量测值，通过航位推算方法估计深水机器人的位置；当导航计算机采集到测距仪的距离量测数据时，采用EKF算法估计深水机器人的位置；
如果深水机器人没有收到测距量测信号，则继续依据惯性导航***进行航位推算；如果收到测距量测信号，则进入组合导航初期阶段。此外，在检测到量测信号时，需要将对量测数据进行修正，如图3所示，因为一般情况下深水机器人在运动，则深水机器人在其发射信号与检测到信号时，并不是在同一个位置，因此需要对其修正。修正方法如下：
假设深水机器人在t0时刻发射信号，在t1时刻声信标m检测到信号，并给深水机器人回馈一个信号，深水机器人在t2时刻检测到源自信标m的信号，则
判断导航计算机采集到的距离量测数据是否同时满足数量和质量上的要求；如果导航计算机采集到的距离量测数据同时满足数量和质量上的要求，则通过空间位置几何解算获得深水机器人的初始位置，并设置深水机器人处于组合导航阶段，并将此空间解算位置设置为深水机器人可靠的初始值；如果不满足要求，则此时深水机器人的导航状态处于组合导航初期阶段，组合导航初期阶段目的是通过空间位置解算确定深水机器人的初始位置；
在组合导航初期阶段，深水机器人一方面进行航位推算估计自身的位置，一方面则计算空间位置解算的个数与质量。首先如果在一个测距采样周期内，有量测更新，则采用EKF更新深水机器人的位置。如果在一个测距采样周期内，有大于或者等于3个量测信号，则可以通过空间位置解算，求解深水机器人的位置。如果在连续4个测距采样周期内，均可以获得深水机器人的空间解算位置，那么则设置深水机器人处于组合导航阶段。
在组合导航阶段，深水机器人通过航位推算预测自身的位置，获得量测数据后，采用多模型EKF算法矫正自身的位置，提高导航精度；同时记录空间位置解算点，用于离线分析。多模型EKF算法流程图如图4所示。
在组合导航阶段，判断距离量测数据是否有效，如果距离量测数据无效，则此量测值为野值，舍弃该距离量测数据，返回通过航位推算方法估计深水机器人的位置步骤；如果距离量测数据有效，则首先确定深水机器人运动***的模型集，然后根据距离量测数据采用多模型EKF算法对深水机器人的位置进行在线修正。
所述深水机器人依据航向传感器和航速传感器的量测值，通过航位推算方法估计深水机器人的位置，具体为：
ve＝vksinθk (3)
vn＝vkcosθk (4)
式中，(x10,x20)为深水机器人的初始点，Δt为导航计算机的采集周期，(x1t,x2t)为推算航位，θk为航向传感器量测值，vk为航速传感器量测值，ve为深水机器人向东速度，vn为深水机器人向北速度。
所述当导航计算机采集到测距仪的距离量测数据时，采用EKF算法估计深水机器人的位置，包括以下步骤：
步骤1：设置AUV的初始状态为X0/0和初始协方差矩阵为P0/0，默认为深水机器人刚刚开始执行任务时候的经纬度位置；
步骤2：时间更新：通过以下时间更新方程，更新深水机器人的状态Xk+1/k和协方差矩阵Pk+1/k：
Xk+1/k＝A·Xk/k+B·uk (5)
Pk+1/k＝A·Pk/k·AT+B·Qk·BT (6)
其中，uk是由航向传感器量测值θk和航速传感器量测值vk计算而来，uk＝[ve vn]T，A为状态转移矩阵，AT为矩阵A的转置，B为控制输入矩阵，BT为B的转置，Xk/k为量测更新之后深水机器人的状态，Pk/k为量测更新之后深水机器人的状态协方差矩阵，Xk+1/k为深水机器人的一步预测状态，Pk+1/k为深水机器人的一步预测状态协方差矩阵；
步骤3：量测更新：当深水机器人获得测距仪的量测数据时，则采用量测更新方程矫正深水机器人的状态Xk+1/k和协方差矩阵Pk+1/k；
Xk+1/k+1＝Xk+1/k+Kk+1·(y-yk+1) (8)
Pk+1/k+1＝(I-Kk+1·Hk+1)·Pk+1/k (9)其中，Pk+1/k为深水机器人的一步预测状态协方差矩阵，Hk+1为量测矩阵，Rk+1为量测协方差矩阵，Kk+1为卡尔曼增益矩阵，y为实际量测值，yk+1为预测量测值，I为单位阵，Xk+1/k为深水机器人的一步预测状态，Xk+1/k+1为k+1时刻状态更新值，Pk+1/k+1为k+1时刻状态协方差矩阵更新值。
所述判断导航计算机采集到的距离量测数据是否同时满足数量和质量上的要求，具体为：
在下一个测距周期到来时，当一个测距周期内的测距数据数量大于或等于3时，对深水机器人水平面坐标进行解算，解算公式如下：
Ax＝v (10)
式中，
为第i号信标的三维坐标，(x1,x2,x3)为深水机器人水平面坐标和深度，为第i号信标到坐标系原点的水平间距，为第i号信标到深水机器人的水平距离，其中Ri表示第i号信标的单程传播距离；
将深水机器人水平面坐标与所述推算航位作差，然后求取均方根误差，即：
如果e≤Bias，则量测数据质量达到要求，并令空间位置解算标志变量N＝N+1，N的初始值为0，直到N≥4时，则量测数据数量和质量同时达到要求，则设置此时导航状态为组合导航阶段，并确定当前深水机器人水平面的初始位置是(x1,x2)；如果e＞Bias并且N＜4，则设置N＝0，其中，Bias范围为1米～100米，Bias是预设值。
所述在组合导航阶段，判断距离量测数据是否有效，具体为：
v(k+1)＝y-yk+1/k (12)
v(k+1)T·Sk -1·v(k+1)≤γ (13)
其中，y是实际量测值，yk+1/k是预测量测值，v(k+1)是新息，Sk是新息协方差矩阵，γ是阈值，γ的建议值为9.2。
判断公式(13)是否成立，如果成立，则使用当前测量值y；否则，此量测值为野值，无效，给予舍弃。
所述确定深水机器人运动***的模型集，根据***过程噪声Q的范围，确定合理的过程噪声最小协方差矩阵Qmin和过程噪声最大协方差矩阵Qmax，Qmin≤Q≤Qmax，取N组运动模型，模型集的确定方法是：
为每一个Qi建立一个EKF滤波器，每一个滤波器独立估计深水机器人的状态，i表示模型索引，N是预设值，表示模型的个数，当水下机器人的最大速度小于或等于3m/s时，N取值为10。
所述根据距离量测数据采用多模型EKF算法对深水机器人的位置进行在线修正，包括以下步骤：
步骤1：对每一个模型对应的滤波器进行时间更新，得到Xk+1/k，
Xi,k+1/k＝A·Xi,k/k+B·uk (15)
Pi,k+1/k＝A·Pi,k/k·AT+B·Qi,k·BT (16)
yi,k+1/k＝h(Xi,k+1/k) (17)
Xi,k+1/k+1＝Xi,k+1/k+Ki,k+1·(y-yi,k+1/k) (19)
Pi,k+1/k+1＝(I-Ki,k+1·Hi,k+1)·Pi,k+1/k (20)
其中，A是状态转移矩阵，Xi,k/k是第i个模型对应的状态，B是速度控制输入矩阵，uk是速度矢量，Xi,k+1/k表示第i个模型对应的预测状态，Pi,k+1/k表示第i个模型对应的预测状态协方差矩阵，Pi,k/k表示量测更新之后第i个模型对应的状态协方差矩阵，Qi,k表示第i个模型对应的过程噪声协方差矩阵，h(·)表示量测方程，yi,k+1/k表示第i个模型对应的预测量测值，Ki,k+1表示第i个模型对应的卡尔曼增益，Hi,k+1表示第i个模型对应的线性化之后量测矩阵，Rk+1是量测协方差矩阵，y是量测值，Xi,k+1/k+1表示第i个模型对应的状态更新值，Pi,k+1/k+1表示第i个模型对应的状态协方差矩阵更新值，I表示单位阵；
步骤2：当检测到距离量测信息时，计算深水机器人到信标的距离；并且依据量测，计算每一个模型对应的权重：
vi(k+1)＝y-yi,k+1/k,i＝1,...,N (21)
Sk+1＝H·Pk+1/k·HT+Rk+1 (22)
其中，y是量测值，yi,k+1/k表示第i个模型对应的预测量测值，vi(k+1)表示第i个模型对应的新息，Rk+1是量测协方差矩阵，H表示线性化之后的量测矩阵，Sk+1表示新息协方差矩阵，Pk+1/k是预测状态协方差矩阵，e(i)是权重，β(i)是归一化权重；
步骤3：更新深水机器人的状态和协方差矩阵：
Xk+1/k+1＝∑β(i)Xi,k+1/k+1 (25)
Pk+1/k+1＝∑β(i)Pi,k+1/k+1 (26)
其中，β(i)是归一化权重，Xi,k+1/k+1是第i个模型量测对应的状态更新值，Pi,k+1/k+1是第i个模型量测对应的状态协方差矩阵更新值，Xk+1/k+1是我们所需要估计的状态，Pk+1/k+1是我们所需要估计的状态协方差矩阵；
则Xk+1/k+1便是所求的深水机器人此时此刻的状态，Pk+1/k+1是对应的协方差矩阵。
表1变量以及变量介绍

Claims (10)

1.一种深水机器人多模型EKF组合导航装置，其特征在于，包括：
测距仪，安装于深水机器人壳体外部，连接深水机器人内部的导航计算机，利用声信号测量深水机器人到各个信标的距离；
信标，至少三个，布设于海底，坐标为已知；
传感器，包括航向传感器、航速传感器和深度传感器，均安装于深水机器人中并连接导航计算机，分别用于测量深水机器人的航向、航速和所处深度。
导航计算机，用于采集测距仪和传感器的输出信号，并采用多模型EKF组合导航方法实时计算深水机器人当前时刻的位置。
2.根据权利要求1所述的一种深水机器人多模型EKF组合导航装置，其特征在于，所述测距仪按固定周期进行测距，周期在3秒～60秒之间，量测数据内容包括信标的编号，信号双程传播时间，并将时间量测转化为距离量测。
3.根据权利要求1所述的一种深水机器人多模型EKF组合导航装置，其特征在于，所述导航计算机按固定周期采集数据，周期在0.2秒～2秒之间。
4.一种深水机器人多模型EKF组合导航方法，其特征在于，包括以下步骤：
深水机器人下水后，导航计算机对测距仪、航向传感器、航速传感器和深度传感器进行数据采集；
深水机器人依据航向传感器和航速传感器的量测值，通过航位推算方法估计深水机器人的位置；当导航计算机采集到测距仪的距离量测数据时，采用EKF算法估计深水机器人的位置；
判断导航计算机采集到的距离量测数据是否同时满足数量和质量上的要求；如果导航计算机采集到的距离量测数据同时满足数量和质量上的要求，则通过空间位置几何解算获得深水机器人的初始位置，并设置深水机器人处于组合导航阶段，并将此空间解算位置设置为深水机器人可靠的初始值；如果不满足要求，则此时深水机器人的导航状态处于组合导航初期阶段，组合导航初期阶段目的是通过空间位置解算确定深水机器人的初始位置；
在组合导航阶段，判断距离量测数据是否有效，如果距离量测数据无效，则此量测值为野值，舍弃该距离量测数据，返回通过航位推算方法估计深水机器人的位置步骤；如果距离量测数据有效，则首先确定深水机器人运动***的模型集，然后根据距离量测数据采用多模型EKF算法对深水机器人的位置进行在线修正。
5.根据权利要求4所述的一种深水机器人多模型EKF组合导航方法，其特征在于，所述深水机器人依据航向传感器和航速传感器的量测值，通过航位推算方法估计深水机器人的位置，具体为：
x 1 t = x 10 + Σ k = 1 v e · Δ t - - - ( 1 )
x 2 t = x 20 + Σ k = 1 v n · Δ t - - - ( 2 )
ve＝vksinθk (3)
vn＝vkcosθk (4)
式中，(x10,x20)为深水机器人的初始点，Δt为导航计算机的采集周期，(x1t,x2t)为推算航位，θk为航向传感器量测值，vk为航速传感器量测值，ve为深水机器人向东速度，vn为深水机器人向北速度。
6.根据权利要求4所述的一种深水机器人多模型EKF组合导航方法，其特征在于，所述当导航计算机采集到测距仪的距离量测数据时，采用EKF算法估计深水机器人的位置，包括以下步骤：
步骤1：设置AUV的初始状态为X0/0和初始协方差矩阵为P0/0，默认为深水机器人刚刚开始执行任务时候的经纬度位置；
步骤2：时间更新：通过以下时间更新方程，更新深水机器人的状态Xk+1/k和协方差矩阵Pk+1/k：
Xk+1/k＝A·Xk/k+B·uk (5)
Pk+1/k＝A·Pk/k·AT+B·Qk·BT (6)
其中，uk是由航向传感器量测值θk和航速传感器量测值vk计算而来，uk＝[ve vn]T，A为状态转移矩阵，AT为矩阵A的转置，B为控制输入矩阵，BT为B的转置，Xk/k为量测更新之后深水机器人的状态，Pk/k为量测更新之后深水机器人的状态协方差矩阵，Xk+1/k为深水机器人的一步预测状态，Pk+1/k为深水机器人的一步预测状态协方差矩阵；
步骤3：量测更新：当深水机器人获得测距仪的量测数据时，则采用量测更新方程矫正深水机器人的状态Xk+1/k和协方差矩阵Pk+1/k；
Kk+1＝Pk+1/k·Hk+1 T·(Hk+1·Pk+1/k·Hk+1 T+Rk+1)-1 (7)
Xk+1/k+1＝Xk+1/k+Kk+1·(y-yk+1) (8)
Pk+1/k+1＝(I-Kk+1·Hk+1)·Pk+1/k (9)
其中，Pk+1/k为深水机器人的一步预测状态协方差矩阵，Hk+1为量测矩阵，Rk+1为量测协方差矩阵，Kk+1为卡尔曼增益矩阵，y为实际量测值，yk+1为预测量测值，I为单位阵，Xk+1/k为深水机器人的一步预测状态，Xk+1/k+1为k+1时刻状态更新值，Pk+1/k+1为k+1时刻状态协方差矩阵更新值。
7.根据权利要求4所述的一种深水机器人多模型EKF组合导航方法，其特征在于，所述判断导航计算机采集到的距离量测数据是否同时满足数量和质量上的要求，具体为：
在下一个测距周期到来时，当一个测距周期内的测距数据数量大于或等于3时，对深水机器人水平面坐标进行解算，解算公式如下：
Ax＝v (10)
式中，
A = ( x 1 2 - x 1 1 ) ( x 2 2 - x 2 1 ) ( x 1 3 - x 1 1 ) ( x 2 3 - x 2 1 ) , x = x 1 x 2 , v = 1 2 [ ( r 1 2 - r 2 2 ) - ( d 1 2 - d 2 2 ) ] - ( x 3 2 - x 3 1 ) · x 3 1 2 [ ( r 1 2 - r 3 2 ) - ( d 1 2 - d 3 2 ) ] - ( x 3 3 - x 3 1 ) · x 3
为第i号信标的三维坐标，(x1,x2,x3)为深水机器人水平面坐标和深度，为第i号信标到坐标系原点的水平间距，为第i号信标到深水机器人的水平距离，其中Ri表示第i号信标的单程传播距离；
将深水机器人水平面坐标与所述推算航位作差，然后求取均方根误差，即：
e = ( x 1 - x 1 t ) 2 + ( x 2 - x 2 t ) 2 - - - ( 11 )
如果e≤Bias，则量测数据质量达到要求，并令空间位置解算标志变量N＝N+1，N的初始值为0，直到N≥4时，则量测数据数量和质量同时达到要求，则设置此时导航状态为组合导航阶段，并确定当前深水机器人水平面的初始位置是(x1,x2)；如果e＞Bias并且N＜4，则设置N＝0，其中，Bias范围为1米～100米，Bias是预设值。
8.根据权利要求4所述的一种深水机器人多模型EKF组合导航方法，其特征在于，所述在组合导航阶段，判断距离量测数据是否有效，具体为：
v(k+1)＝y-yk+1/k (12)
v(k+1)T·Sk -1·v(k+1)≤γ (13)
其中，y是实际量测值，yk+1/k是预测量测值，v(k+1)是新息，Sk是新息协方差矩阵，γ是阈值，γ的建议值为9.2。
判断公式(13)是否成立，如果成立，则使用当前测量值y；否则，此量测值为野值，无效，给予舍弃。
9.根据权利要求4所述的一种深水机器人多模型EKF组合导航方法，其特征在于，所述确定深水机器人运动***的模型集，根据***过程噪声Q的范围，确定合理的过程噪声最小协方差矩阵Qmin和过程噪声最大协方差矩阵Qmax，Qmin≤Q≤Qmax，取N组运动模型，模型集的确定方法是：
Q i = N + 1 - i N Q m i n + i - 1 N Q m a x , i = 1 , 2 , ... , N - - - ( 1 )
为每一个Qi建立一个EKF滤波器，每一个滤波器独立估计深水机器人的状态，i表示模型索引，N是预设值，表示模型的个数，当水下机器人的最大速度小于或等于3m/s时，N取值为10。
10.根据权利要求4所述的一种深水机器人多模型EKF组合导航方法，其特征在于，所述根据距离量测数据采用多模型EKF算法对深水机器人的位置进行在线修正，包括以下步骤：
步骤1：对每一个模型对应的滤波器进行时间更新，得到Xk+1/k，
Xi,k+1/k＝A·Xi,k/k+B·uk (15)
Pi,k+1/k＝A·Pi,k/k·AT+B·Qi,k·BT (16)
yi,k+1/k＝h(Xi,k+1/k) (17)
K i , k + 1 = P i , k + 1 / k · H i , k + 1 T · ( H i , k + 1 · P i , k + 1 / k · H i , k + 1 T + R k + 1 ) - 1 - - - ( 18 )
Xi,k+1/k+1＝Xi,k+1/k+Ki,k+1·(y-yi,k+1/k) (19)
Pi,k+1/k+1＝(I-Ki,k+1·Hi,k+1)·Pi,k+1/k (20)
其中，A是状态转移矩阵，Xi,k/k是第i个模型对应的状态，B是速度控制输入矩阵，uk是速度矢量，Xi,k+1/k表示第i个模型对应的预测状态，Pi,k+1/k表示第i个模型对应的预测状态协方差矩阵，Pi,k/k表示量测更新之后第i个模型对应的状态协方差矩阵，Qi,k表示第i个模型对应的过程噪声协方差矩阵，h(·)表示量测方程，yi,k+1/k表示第i个模型对应的预测量测值，Ki,k+1表示第i个模型对应的卡尔曼增益，Hi,k+1表示第i个模型对应的线性化之后量测矩阵，Rk+1是量测协方差矩阵，y是量测值，Xi,k+1/k+1表示第i个模型对应的状态更新值，Pi,k+1/k+1表示第i个模型对应的状态协方差矩阵更新值，I表示单位阵；
步骤2：当检测到距离量测信息时，计算深水机器人到信标的距离；并且依据量测，计算每一个模型对应的权重：
vi(k+1)＝y-yi,k+1/k,i＝1,...,N (21)
Sk+1＝H·Pk+1/k·HT+Rk+1 (22)
e ( i ) = exp { - 1 2 v i ( k + 1 ) T S k + 1 - 1 v i ( k + 1 ) } - - - ( 23 )
β ( i ) = e ( i ) Σ e ( i ) - - - ( 24 )
其中，y是量测值，yi,k+1/k表示第i个模型对应的预测量测值，vi(k+1)表示第i个模型对应的新息，Rk+1是量测协方差矩阵，H表示线性化之后的量测矩阵，Sk+1表示新息协方差矩阵，Pk+1/k是预测状态协方差矩阵，e(i)是权重，β(i)是归一化权重；
步骤3：更新深水机器人的状态和协方差矩阵：
Xk+1/k+1＝Σβ(i)Xi,k+1/k+1 (25)
Pk+1/k+1＝Σβ(i)Pi,k+1/k+1 (26)
其中，β(i)是归一化权重，Xi,k+1/k+1是第i个模型量测对应的状态更新值，Pi,k+1/k+1是第i个模型量测对应的状态协方差矩阵更新值，Xk+1/k+1是我们所需要估计的状态，Pk+1/k+1是我们所需要估计的状态协方差矩阵；
则Xk+1/k+1便是所求的深水机器人此时此刻的状态，Pk+1/k+1是对应的协方差矩阵。
CN201510953833.4A 2015-12-17 2015-12-17 一种深水机器人多模型ekf组合导航装置及方法 Pending CN106896361A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510953833.4A CN106896361A (zh) 2015-12-17 2015-12-17 一种深水机器人多模型ekf组合导航装置及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510953833.4A CN106896361A (zh) 2015-12-17 2015-12-17 一种深水机器人多模型ekf组合导航装置及方法

Publications (1)

Publication Number Publication Date
CN106896361A true CN106896361A (zh) 2017-06-27

Family

ID=59189765

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510953833.4A Pending CN106896361A (zh) 2015-12-17 2015-12-17 一种深水机器人多模型ekf组合导航装置及方法

Country Status (1)

Country Link
CN (1) CN106896361A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108827313A (zh) * 2018-08-10 2018-11-16 哈尔滨工业大学 基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法
CN109116845A (zh) * 2018-08-17 2019-01-01 华晟（青岛）智能装备科技有限公司 自动导引运输车定位方法、定位***及自动导引运输***
CN109489668A (zh) * 2018-11-16 2019-03-19 上海瀚界科技发展有限公司 单兵水下导航方法及装置
CN112445244A (zh) * 2020-11-09 2021-03-05 中国科学院沈阳自动化研究所 一种多自主水下机器人的目标搜寻方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6819984B1 (en) * 2001-05-11 2004-11-16 The United States Of America As Represented By The Secretary Of The Navy LOST 2—a positioning system for under water vessels
CN101078936A (zh) * 2007-06-08 2007-11-28 北京航空航天大学 一种基于遗传最优request和gupf的高精度组合定姿方法
CN103338168A (zh) * 2013-05-28 2013-10-02 哈尔滨工业大学 基于加权分数傅立叶变换的双弥散信道下的迭代时域最小均方误差均衡方法
CN104280024A (zh) * 2013-07-05 2015-01-14 中国科学院沈阳自动化研究所 一种深水机器人组合导航装置和方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6819984B1 (en) * 2001-05-11 2004-11-16 The United States Of America As Represented By The Secretary Of The Navy LOST 2—a positioning system for under water vessels
CN101078936A (zh) * 2007-06-08 2007-11-28 北京航空航天大学 一种基于遗传最优request和gupf的高精度组合定姿方法
CN103338168A (zh) * 2013-05-28 2013-10-02 哈尔滨工业大学 基于加权分数傅立叶变换的双弥散信道下的迭代时域最小均方误差均衡方法
CN104280024A (zh) * 2013-07-05 2015-01-14 中国科学院沈阳自动化研究所 一种深水机器人组合导航装置和方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
许昭霞等: ""水下航行器导航***的模糊自适应多模型滤波方法"", 《舰船科学技术》 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108827313A (zh) * 2018-08-10 2018-11-16 哈尔滨工业大学 基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法
CN109116845A (zh) * 2018-08-17 2019-01-01 华晟（青岛）智能装备科技有限公司 自动导引运输车定位方法、定位***及自动导引运输***
CN109116845B (zh) * 2018-08-17 2021-09-17 华晟（青岛）智能装备科技有限公司 自动导引运输车定位方法、定位***及自动导引运输***
CN109489668A (zh) * 2018-11-16 2019-03-19 上海瀚界科技发展有限公司 单兵水下导航方法及装置
CN112445244A (zh) * 2020-11-09 2021-03-05 中国科学院沈阳自动化研究所 一种多自主水下机器人的目标搜寻方法

Similar Documents

Publication Publication Date Title
CN109459040B (zh) 基于rbf神经网络辅助容积卡尔曼滤波的多auv协同定位方法
CN103487050B (zh) 一种室内移动机器人定位方法
KR100561855B1 (ko) 로봇용 로컬라이제이션 시스템
CN108614258B (zh) 一种基于单水声信标距离量测的水下定位方法
CN110057365A (zh) 一种大潜深auv下潜定位方法
CN102749622B (zh) 基于多波束测深的声速剖面及海底地形的联合反演方法
CN111948602A (zh) 基于改进Taylor级数的二维UWB室内定位方法
CN106896361A (zh) 一种深水机器人多模型ekf组合导航装置及方法
CN109901205B (zh) 一种水下机器人多传感器融合与运动轨迹预测方法
CN110954132B (zh) Grnn辅助自适应卡尔曼滤波进行导航故障识别的方法
CN104360315A (zh) 基于LabVIEW的麦克风阵列声源定位方法及装置
CN106054135B (zh) 一种基于周期移动时间窗的被动水声定位方法
CN105628016B (zh) 一种基于超短基线的导航定位方法
CN104280024B (zh) 一种深水机器人组合导航装置和方法
CN110061716A (zh) 一种基于最小二乘和多重渐消因子的改进kalman滤波方法
CN103323815A (zh) 一种基于等效声速的水下声学定位方法
CN109540154B (zh) 一种基于粒子滤波算法的水声导航定位方法
CN108827305A (zh) 一种基于鲁棒信息滤波的auv协同导航方法
CN108332756B (zh) 一种基于拓扑信息的水下航行器协同定位方法
CN110132308A (zh) 一种基于姿态确定的usbl安装误差角标定方法
CN106501774A (zh) 一种水下声传感器网络节点定位方法
CN110906933A (zh) 一种基于深度神经网络的auv辅助导航方法
KR102082263B1 (ko) 수중위치 추정 시스템 및 방법
CN105445722A (zh) 一种适用于多auv协同导航的动态条件下水声双程测距误差补偿方法
CN111174774A (zh) 一种某深度水层面模式下的导航信息融合方法及***

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20170627