专利名称：载人潜水器位置测量延时处理方法
技术领域：
本发明涉及载人潜水器在海洋中定位技术，具体地说是一种载人潜水器位置测量延时处理方法。
背景技术：
载人潜水器在海洋中运动时，需要实时确定自己目前的坐标。目前多采用位置传感器进行实时测量，提供载人潜水器的位置信息。通过载人潜水器导航处理系统的计算，综合这些位置信息，得到一个最优的载人潜水器在海洋中的位置信息。
载人潜水器的导航处理系统在进行载人潜水器位置估计时，最基本的可以实时测得的状态量包括航向角θ、纵倾角ψ、水平面的速度值u，v。载人潜水器的水平面位置数据(X方向、Y方向)可以通过两种方法获得，一是直接方法，即利用位置传感器(如超短基线水声定位系统)直接测量；二是间接方法，即根据其它传感器测量数据(速度、航向角)来推测位置数据。其中超短基线水声定位系统是在母船上安装一个水声接收基阵，水声接收基阵上有5个接收换能器，按圆环型布置(相互间隔1厘米左右)，在圆环的中间有一个发射换能器，再在载人潜水器上安装一只水声应答信标；水声接收基阵收到水声应答信标的信号，就可以确定载人潜水器与母船的相对位置。
载人潜水器获得超短基线定位系统的位置信息过程为首先超短基线水声定位系统的发射换能器定时地发出声脉冲询问信号，载人潜水器上的应答信标接到询问信号后，发射应答声脉冲信号。水声接收基阵收到应答信标的脉冲信号后，超短基线数据处理系统计算出载人潜水器位置值，然后通过母船上的水声通讯机将获得的载人潜水器的位置信息发送给水下载人潜水器。这一过程由于载人潜水器与母船的距离不同，声传播需要几秒甚至几十秒的时间，因此每次载人潜水器收到的位置信息都将是过时的信息。
在现有技术中，无缆潜水器位置估算中，只靠间接的方法推算位置数据，不综合超短基线的数据，超短基线的数据只作为起始数据使用。由于传感器的数据漂移，长时间的位置推算，无缆潜水器的位置数据会偏离实际的位置值。
发明内容
为了解决载人潜水器在海洋航行中，导航系统得到的水声定位系统数据延时问题，本发明的目的是提供一种载人潜水器位置测量延时处理方法。通过当前得到的过时超短基线位置测量信息，实现对当前的载人潜水器位置信息进行优化处理，解决位置信息滞后造成的估计误差问题，本发明把超短基线的测量值融合到载人潜水器的位置推算中，避免长时间的载人潜水器位置推算，导致载人潜水器位置推算数据偏离实际位置值。
为了实现上述目的，本发明的技术方案是把当前得到的过时超短基线位置测量值通过卡尔曼滤波算法融合到载人潜水器的位置推算中，实现对当前的载人潜水器位置信息进行优化处理；所述推算算法具体是假设得到超短基线水声定位系统测量的载人潜水器位置信息过程中，声波在水中的传播时间长度为t＝d*Ts，在t＝k*Ts时刻水下水声应答器向水声接收基阵发出应答信号，由于声波在水中传输的时延，载人潜水器要在t＝(k+d)*Ts才能接收到k时刻的超短基线测量的载人潜水器位置信息Xkm，Ykm；此时，在t＝(k+d)*Ts时刻，载人潜水器的位置估计为Xk+d=Xk+Σi=1d[u(k+i-1)cosθ(k+i-1)cosψ(k+i-1)-v(k+i-1)sin(k+i-1)cosψ(k+i-1]Ts;]]>Yk+d=Yk+Σi=1d[u(k+i-1)sinθ(k+i-1)cosψ(k+i-1)+v(k+i-1)cosθ(k+i-1)cosψ(k+i-1)]Ts]]>其中，d为时间间隔，Ts为时间计量单位；k为第k次测量；Xk，Yk为k时刻载人潜水器位置的估计，u、v是载人潜水器在水平面的两维运动速度，θ是载人潜水器的航向角，ψ是载人潜水器的纵倾角；令ΔXd=Σi=1d[u(k+i-1)cosθ(k+i-1)cosψ(k+i-1)-v(k+i-1)sin(k+i-1)cosψ(k+i-1)]Ts;]]>ΔYd=Σi=1d[u(k+i-1)sinθ(k+i-1)cosψ(k+i-1)+v(k+i-1)cosθ(k+i-1)cosψ(k+i-1)]Ts]]>则根据k时刻超短基线测量值Xkm，Ykm和k时刻估计值Xk，Yk，应用卡尔曼滤波公式，得到k时刻的最优估计 则可得(k+d)时刻载人潜水器位置的最优估计值为X^(k+d|k)=X^(k|k)+ΔXd.]]>Y^(k+d|k)=Y^(k|k)+ΔYd]]>所述(k+d)时刻载人潜水器位置的最优估计值中X，Y每隔d*Ts秒刷新一次，其余变量如载人潜水器的运动速度和航向角，每隔Ts秒刷新一次；所述载人潜水器位置测量延时处理推算算法流程为先进行数据采集，然后对潜水器X方向位置和潜水器Y方向位置进行推算，再接收k时刻潜水器的位置值，运用卡尔曼滤波器估算K时刻位置值，更新k+d时刻的位置值，构成一个算法过程。
本发明具有如下优点1.本发明解决了以前因超短基线的延迟造成数据无法利用的问题。
在现有技术中，无缆潜水器位置只根据其它传感器数据(速度、航向角)来推测位置数据。由于测量误差的存在，位置估计误差呈现增大趋势，并随着时间增长，最终使得位置估计误差趋于无穷大。超短基线水声定位系统直接进行位置测量可以保证测量误差有界，但是其测量噪声是很大的。本发明把超短基线测量数据，通过卡尔曼滤波器融合到潜水器位置推算中，综合了间接测量(推算)的数据平滑性和直接测量过程误差的有界性，获得了最佳的估计效果。
2.本发明提出的载人潜水器位置测量延时处理方法，同样适用于其它的水下机器人，包括自治水下机器人(AUV)和遥控水下机器人(ROV)。
图1为本发明载人潜水器位置测量延时处理算法软件编程流程图。
图2为现有技术中传统的根据速度测量值进行位置推算结果示意图。
图3为现有技术中另一种传统的直接位置测量效果图。
图4为本发明采用卡尔曼滤波进行数据融合的效果图。
具体实施例方式
下面结合附图对本发明作进一步详细说明。
把当前得到的过时超短基线位置测量值融合到载人潜水器的位置推算中，实现对当前的载人潜水器位置信息进行优化处理；具体是假设得到超短基线水声定位系统测量的载人潜水器位置信息过程中，声波在水中的传播时间长度为t＝d*Ts，其中d为时间间隔，Ts为时间计量单位；k为第k次测量；时刻水下水声应答器向水声接收基阵发出应答信号，由于声波在水中传输的时延，载人潜水器要在t＝(k+d)*Ts才能接收到k时刻的超短基线测量的载人潜水器位置信息Xkm，Ykm；此时，在t＝(k+d)*Ts时刻，载人潜水器的位置估计为Xk+d=Xk+Σi=1d[u(k+i-1)cosθ(k+i-1)cosψ(k+i-1)-v(k+i-1)sin(k+i-1)cosψ(k+i-1]Ts---(1);]]>Yk+d=Yk+Σi=1d[u(k+i-1)sinθ(k+i-1)cosψ(k+i-1)+v(k+i-1)cosθ(k+i-1)cosψ(k+i-1)]Ts]]>其中，Xk，Yk为k时刻载人潜水器位置的估计，u、v是载人潜水器在水平面的两维运动速度，θ是载人潜水器的航向角，ψ是载人潜水器的纵倾角。令ΔXd=Σi=1d[u(k+i-1)cosθ(k+i-1)cosψ(k+i-1)-v(k+i-1)sin(k+i-1)cosψ(k+i-1)]Ts---(2);]]>ΔYd=Σi=1d[u(k+i-1)sinθ(k+i-1)cosψ(k+i-1)+v(k+i-1)cosθ(k+i-1)cosψ(k+i-1)]Ts]]>则根据k时刻超短基线测量值Xkm，Ykm和k时刻估计值Xk，Yk，应用(10)～(13)的卡尔曼滤波公式，得到k时刻的最优估计 则可得(k+d)时刻载人潜水器位置的最优估计值为
X^(k+d|k)=X^(k|k)+ΔXd---(3);]]>Y^(k+d|k)=Y^(k|k)+ΔYd]]>其中，X，Y每隔d*Ts秒刷新一次，其余变量如载人潜水器的运动速度和航向角，每隔Ts秒刷新一次；如图1所示，本发明载人潜水器位置测量延时处理算法流程为先进行数据采集，然后对潜水器X方向位置和潜水器Y方向位置进行推算，再接收k时刻潜水器的位置值，运用卡尔曼滤波器估算K时刻位置值，更新k+d时刻的位置值，构成一个算法过程。
卡尔曼滤波是一种基于迭代和递推过程的最优线性估计器，它可以分解成预测、测量、校正三个子过程；设离散时间系统的状态方程和输出方程分别为x(k+1)＝Φ(k+1，k)x(k)+Γ(k+1，k)w(k) (4)；y(k)＝C(k)x(k)+v(k)其中，Φ(k+1，k)为状态转移矩阵，C(k)为观测矩阵，w(k)，v(k)分别为模型噪声和观测噪声，且有E[w(k)]＝0，Cov[w(k)，w(j)]＝Q(k)δkj，k，j≥0(5)；E[v(k)]＝0，Cov[w(k)，w(j)]＝R(k)δkj，k，j≥0(6)；假设根据观测数据y＝{y(1)，y(2)，...，y(k-1)}，已经求得k-1时刻状态x(k-1)的最优滤波估计 则在没有获得新的观测数据y(k)以前，根据y(k-1)求得k时刻状态x(k)的预测值为x^(k|k-1)=Φ(k,k-1)x^(k-1|k-1)---(7);]]>观测值的预测估计为y^(k|k-1)=C(k)x^(k|k-1)---(8);]]>当获得新的观测数据y(k)时，可得出由(6-5)式求得的预测误差为y~(k|k-1)=y(k)-y^(k|k-1)---(9);]]>可以证明，当式(6-2)(6-3)满足时， 是一个白噪声序列，即后来的与先前的无关，因此， 就可以视为k时刻获得的新信息，可以用来对可状态预测值 进行校正，则k时刻状态的估计值则变为x~(k|k)=x^(k|k-1)+K(k)y~(k|k-1)---(10);]]>其中K(k)为滤波增益矩阵，它根据估计误差的方差达到极小的准则来确定的。表达式为K(k)＝P(k|k-1)CT(k)[C(k)P(k|k-1)CT(k)+R(k)]-1(11)；其中，P(k|k-1)为预测估计误差的方差，表示为P(k|k-1)＝Φ(k，k-1)P(k-1|k-1)ΦT(k，k-1)+Q(k) (12)；P(k|k)＝[I-K(k)C(k)]P(k|k-1)[I-K(k)C(k)]T+K(k)R(k)KT(k)(13)。
在载人潜水器导航中，上述卡尔曼滤波算法实际上可理解为一种数据融合过程。
在现有技术中，无缆潜水器位置只根据其它传感器数据(速度、航向角)来推测位置数据。由于测量误差的存在，位置估计误差呈现增大趋势，并随着时间增长，最终使得位置估计误差趋于无穷大(如图2)。超短基线水声定位系统直接进行位置测量可以保证测量误差有界，但是其测量噪声是很大的(如图3)。把超短基线测量数据，通过卡尔曼滤波器融合到潜水器位置推算中，综合了间接测量(推算)的数据平滑性和直接测量过程误差的有界性，获得了最佳的估计效果(如图4)。
本发明提出的载人潜水器位置测量延时处理方法，同样适用于其它的水下机器人，包括自治水下机器人(AUV)和遥控水下机器人(ROV)。
在载人潜水器导航中，上述卡尔曼滤波算法实际上可理解为一种数据融合过程。
权利要求
1.一种载人潜水器位置测量延时处理方法，其特征在于把当前得到的过时超短基线位置测量值通过卡尔曼滤波算法融合到载人潜水器的位置推算中，实现对当前的载人潜水器位置信息进行优化处理。
2.按照权利要求1所述载人潜水器位置测量延时处理方法，其特征在于所述推算算法具体是假设得到超短基线水声定位系统测量的载人潜水器位置信息过程中，声波在水中的传播时间长度为t＝d*Ts，在t＝k*Ts时刻水下水声应答器向水声接收基阵发出应答信号，由于声波在水中传输的时延，载人潜水器要在t＝(k+d)*Ts才能接收到k时刻的超短基线测量的载人潜水器位置信息Xkm，Ykm；此时，在t＝(k+d)*Ts时刻，载人潜水器的位置估计为Xk+d=Xk+Σi=1d[u(k+i-1)cosθ(k+i-1)cosψ(k+i-1)-v(k+i-1)sin(k+i-1)cosψ(k+i-1)]Ts;]]>Yk+d=Yk+Σi=1d[u(k+i-1)sinθ(k+i-1)cosψ(k+i-1)+v(k+i-1)cosθ(k+i-1)cosψ(k+i-1)]Ts]]>其中，d为时间间隔，Ts为时间计量单位；k为第k次测量；Xk，Yk为k时刻载人潜水器位置的估计，u、v是载人潜水器在水平面的两维运动速度，θ是载人潜水器的航向角，φ是载人潜水器的纵倾角；令ΔXd=Σi=1d[u(k+i-1)cosθ(k+i-1)cosψ(k+i-1)-v(k+i-1)sin(k+i-1)cosψ(k+i-1)]Ts;]]>ΔYd=Σi=1d[u(k+i-1)sinθ(k+i-1)cosψ(k+i-1)+v(k+i-1)cosθ(k+i-1)cosψ(k+i-1)]Ts]]>则根据k时刻超短基线测量值Xkm，Ykm和k时刻估计值Xk，Yk，应用卡尔曼滤波公式，得到k时刻的最优估计X^(k|k),Y^(k|k),]]>则可得(k+d)时刻载人潜水器位置的最优估计值为X^(k+d|k)=X^(k|k)+ΔXd.]]>Y^(k+d|k)=Y^(k|k)+ΔYd]]>
3.按照权利要求2所述载人潜水器位置测量延时处理方法，其特征在于所述(k+d)时刻载人潜水器位置的最优估计值中X，Y每隔d*Ts秒刷新一次，其余变量如载人潜水器的运动速度和航向角，每隔Ts秒刷新一次。
4.按照权利要求1所述载人潜水器位置测量延时处理方法，其特征在于所述载人潜水器位置测量延时处理推算算法流程为先进行数据采集，然后对潜水器X方向位置和潜水器Y方向位置进行推算，再接收k时刻潜水器的位置值，运用卡尔曼滤波器估算K时刻位置值，更新k+d时刻的位置值，构成一个算法过程。
全文摘要
本发明涉及载人潜水器在海洋中定位技术，具体地说是一种载人潜水器位置测量延时处理方法。本发明是把当前得到的过时超短基线位置测量值通过卡尔曼滤波算法融合到载人潜水器的位置推算中，实现对当前的载人潜水器位置信息进行优化处理。本发明通过当前得到的过时超短基线位置测量信息，实现对当前的载人潜水器位置信息进行优化处理，解决位置信息滞后造成的估计误差问题，本发明把超短基线的测量值融合到载人潜水器的位置推算中，避免长时间的载人潜水器位置推算，导致载人潜水器位置推算数据偏离实际位置值。
文档编号G01C21/00GK1719198SQ20041002092
公开日2006年1月11日 申请日期2004年7月7日 优先权日2004年7月7日
发明者于开洋 申请人:中国科学院沈阳自动化研究所