一种深水机器人多模型EKF组合导航装置及方法制造方法及图纸

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

【技术实现步骤摘要】
一种深水机器人多模型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＝[vevn]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+1T·(Hk+1·Pk+1/k·Hk+1T+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本文档来自技高网...

【技术保护点】
一种深水机器人多模型EKF组合导航装置，其特征在于，包括：测距仪，安装于深水机器人壳体外部，连接深水机器人内部的导航计算机，利用声信号测量深水机器人到各个信标的距离；信标，至少三个，布设于海底，坐标为已知；传感器，包括航向传感器、航速传感器和深度传感器，均安装于深水机器人中并连接导航计算机，分别用于测量深水机器人的航向、航速和所处深度。导航计算机，用于采集测距仪和传感器的输出信号，并采用多模型EKF组合导航方法实时计算深水机器人当前时刻的位置。

【技术特征摘要】
1.一种深水机器人多模型EKF组合导航装置，其特征在于，包括：测距仪，安装于深水机器人壳体外部，连接深水机器人内部的导航计算机，利用声信号测量深水机器人到各个信标的距离；信标，至少三个，布设于海底，坐标为已知；传感器，包括航向传感器、航速传感器和深度传感器，均安装于深水机器人中并连接导航计算机，分别用于测量深水机器人的航向、航速和所处深度。导航计算机，用于采集测距仪和传感器的输出信号，并采用多模型EKF组合导航方法实时计算深水机器人当前时刻的位置。2.根据权利要求1所述的一种深水机器人多模型EKF组合导航装置，其特征在于，所述测距仪按固定周期进行测距，周期在3秒～60秒之间，量测数据内容包括信标的编号，信号双程传播时间，并将时间量测转化为距离量测。3.根据权利要求1所述的一种深水机器人多模型EKF组合导航装置，其特征在于，所述导航计算机按固定周期采集数据，周期在0.2秒～2秒之间。4.一种深水机器人多模型EKF组合导航方法，其特征在于，包括以下步骤：深水机器人下水后，导航计算机对测距仪、航向传感器、航速传感器和深度传感器进行数据采集；深水机器人依据航向传感器和航速传感器的量测值，通过航位推算方法估计深水机器人的位置；当导航计算机采集到测距仪的距离量测数据时，采用EKF算法估计深水机器人的位置；判断导航计算机采集到的距离量测数据是否同时满足数量和质量上的要求；如果导航计算机采集到的距离量测数据同时满足数量和质量上的要求，则通过空间位置几何解算获得深水机器人的初始位置，并设置深水机器人处于组合导航阶段，并将此空间解算位置设置为深水机器人可靠的初始值；如果不满足要求，则此时深水机器人的导航状态处于组合导航初期阶段，组合导航初期阶段目的是通过空间位置解算确定深水机器人的初始位置；在组合导航阶段，判断距离量测数据是否有效，如果距离量测数据无效，则此量测值为野值，舍弃该距离量测数据，返回通过航位推算方法估计深水机器人的位置步骤；如果距离量测数据有效，则首先确定深水机器人运动系统的模型集，然后根据距离量测数据采用多模型EKF算法对深水机器人的位置进行在线修正。5.根据权利要求4所述的一种深水机器人多模型EKF组合导航方法，其特征在于，所述深水机器人依据航向传感器和航速传感器的量测值，通过航位推算方法估计深水机器人的位置，具体为：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＝[vevn]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+1T·(Hk+1·Pk+1/k·Hk+1T+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)式中，

【专利技术属性】
技术研发人员：李冬冬，张瑶，陈巩，
申请(专利权)人：中国科学院沈阳自动化研究所，
类型：发明
国别省市：辽宁,21