专利名称：一种基于自适应ukf的水下机器人状态和参数联合估计方法
技术领域：
本发明涉及一种基于自适应无色卡尔曼滤波(UKF,Unscented Kalman Filter)的水下机器人状态和參数联合估计方法，特别涉及一种基于自适应UKF的水下机器人状态和推进器故障參数联合估计方法。
背景技术：
随着海洋开发事业的迅速发展，水下施工和建设项目越来越多，对于水下作用手段的性能要求也越来越高。由于水下机器人能够在水下进行观察、摄影、打捞和施工作业，因此在海洋开发中得到广泛的应用。而推进器的健康状况决定着水下机器人能否很好完成预期的任务。传统预编程的水下机器人在检测到推进器有故障发生时一般采取抛载上浮等 措施，其实在很多情况下，水下机器人的推进器并非完全失效，如果能够实时的辨识出其效率损失因子，然后对推力进行重新分配，水下机器人仍可能完成预期的任务。在水下机器人系统的噪声信息已知的情况下，采用UKF算法能够达到很好的估计效果，而在系统噪声信息未知吋，如果设置的噪声參数与实际的噪声相差过大，就会导致有偏估计甚至滤波算法的发散，因此要设计和采用更适合于估计系统噪声信息的滤波算法，以达到实时的估计水下机器人系统的噪声信息，在噪声信息未知的情况下仍可以达到很好的估计效果。
发明内容
针对上述存在的问题，本发明提供了一种能实时估计推进器故障信息和系统噪声信息，并具有实用性好、估算精度高的基于自适应UKF的水下机器人状态和參数联合估计方法为实现本发明的目的，本发明采用如下方案一种基于自适应UKF的水下机器人状态和參数联合估计方法，其特征在于包括以下步骤在水下机器人动力学模型的基础上，对推进器故障进行建模，得到离线的扩展參考模型；根据位姿传感器探測的在线位姿信息yk，采用自适应UKF的主滤波器为标准UKF滤波估计算法，结合水下机器人离线的扩展參考模型为扩展的系统状态方程，对系统状态和推进器故障组成的扩展状态传递和更新，实时估计出系统的状态和參数信息，得到系统估算的结果值均值ち和方差Pk ;这里系统的状态信息包括经过滤波后的位姿信息Sk和实时估计出来的速度信息も，參数信息为系统推进器的故障參数bk，同时，依据主滤波器计算估计的新息信息I k计算出辅助滤波器的测量值Sqk ;建立UKF辅助滤波器的状态方程和測量方程，依据测量值Sqk作为系统的观测信号，采用自适应UKF的辅助滤波器KF对水下机器人系统的噪声信息进行实时估计，得到系统噪声协方差阵D的对角线元素も和方^的估算值，这里的噪声信息包括过程噪声Wk和测量噪声vk，它们的协方差矩阵分别为Dw和Dv。
主滤波器扩展系统方程形成过程扩展非线性系统时，含有未知/时变推进器的故障參数bk的系统状态空间方程为xk+1 = f (xk, bk, uk) +wk_] bk+1 = fb(xk, bk, uk)+wkb式中，bk是k时刻的未知/时变推进器故障參数，Wkb为故障參数模型的系统推进器故障噪声矢量，是零均值的高斯噪声；当故障參数bk变化规律未知，则可将bk视为不相关的随机漂移矢量，其递归表达 式为bk = bk_!+wbk基于自适应UKF的联合估计，将系统推进器的故障參数追加到系统的真实状态后，组成扩展的状态矩阵，即<=(4,把系统方程组成为扩展系统状态方程为 xak+l=f(xak,uk) + wak式中，!为扩展的系统过程噪声矢量，其中！<为系统过程噪声矢量， 为估计推进器故障噪声矢量。扩展參考模型中故障參数bk的％表示为k时刻第i个推进器的效率损失因子，取O I数值表示推进器故障发生失效的程度。自适应UKF算法中辅助滤波器状态方程的构建由水下机器人系统噪声方差矩阵对角线元素あ可将辅助滤波器的状态方程表示为^k=fq(^k-0 + Wqk其中ル%为辅助滤波器中的系统噪声矢量，是零均值高斯白噪声；由于あ的变化规律未知，可将其视为噪声驱动的不相关的随机漂移向量，所以可得辅助滤波器的状态方程qk = qk~\ + wqk。自适应UKF算法中辅助滤波器測量方程的构建以主滤波器的新息ξ k方差矩阵的测量值Sqk作为系统的观测信号，依据主滤波器扩展状态的ー步估计的方差PkiH，主滤波器扩展状态的方差Pk，主滤波器UKF的增益矩阵Kk组成辅助滤波器的測量方程为Sqk =g(qk) = Vdiag^(KrkKk) (P叫—,-Pk、K人KTk Kk)其中vdiag为中括号内矩阵的主对角线元素组成的ー个向量。主滤波器对扩展的状态方程滤波估计的实现过程对上ー时刻随机向量的被估计量均值和方差Plrl经非线性无色变换后，得到的一组离散Sigma点X Jrf,经系统状态方程计算更新Sigma点,然后计算出更新Sigma点的扩展状态均值^*-1和方差PkIJrf，对扩展状态均值·5^-1和方差Pkik-i计算得到一步估计的状态均值和方差；对一步估计的状态均值和方差再经无色变换和用測量方程计算更新，得到水下机器人可观测状态的均值J7H、其方差な及协方差ち的估计值；读取传感器采集可观测位姿信息yk同扩展状态均值和方差Pk^和可观测状态的估计值及増益矩阵Kk经计算估计，得到系统均值ち、方差Pk的估算值，从中得到新息Ik;依主滤波器的新息Ik经计算得到测量值Sqk作为辅助滤波器的观测信号，然后接续辅助滤波器程序步骤。辅助滤波器KF对系统过程噪声进行滤波估计的实现过程上一时刻估计得到辅助滤波器的系统噪声方差矩阵的对角线元素も—i和其方差Pqkハ，用辅助滤波器的状态方程对も—丨和^L1计算更新后得到对角线元素和其方差ち^的一歩估计值；用辅助滤波器的測量方程依主滤波器的一步估计的方差pkM、扩展状态方差Pk和増益矩阵Kk经计算更新后得到估计的观测值，所得到的一歩估计值和观测值5 同主滤波器的测量值Sqk经计算估计得到系统过程噪声方差阵D的对角线元素的估算值も和其方差ちt，当有新观测位姿信息yk时，接续下ー控制周期主滤波器程序步骤。本发明与现有技术相比有益效果如下
I.本发明实现简单，可以在现有的水下机器人载体上直接使用该方法，不需要对水下机器人的硬件系统进行任何的改动，也不需要増加任何的硬件设备。该发明可以嵌入水下机器人现有的线程中，对水下机器人来说，仅仅是增加了系统的计算量。2.本发明估计方法，是根据水下机器人传感器的探測信息，采用自适应UKF算法，实时的估计水下机器人状态和推进器的故障參数信息，这些信息为水下机器人的容错控制提供了依据，能够实时的辨识出其效率损失因子，然后对推力进行重新分配，以保证水下机器人仍可能完成预期的任务。3.本发明估计精度高，可在线对系统的状态和參数进行联合估计。由于UT直接作用在非线性动力学模型上，不需要对其进行线性化，所以避免了非线性系统线性化过程中产生的线性化误差。4.在系统的噪声先验信息包括过程噪声和测量噪声未知的情况下，如果设置的噪声和实际的噪声有较大偏差，采用UKF滤波估计算法会产生有偏误差或者滤波算法的发散，而自适应UKF算法中由于辅助滤波器可以对系统的噪声信息进行实时估计，且能够较快的收敛，所以能够达到较高的估计精度。
图I为该发明方法的实现原理图；图2为该发明的滤波估计流程图。
具体实施例方式下面结合附图和实施例对本发明方案进ー步详细描述參见附图I 2，为本发明方法实现原理图和其滤波估计流程图。一种基于自适应UKF的水下机器人状态和參数联合估计方法，其特征在于包括以下步骤在水下机器人动力学模型的基础上，对推进器故障进行建模，得到离线的扩展參考模型；根据位姿传感器探測的在线位姿信息yk，采用自适应UKF的主滤波器为标准UKF滤波估计算法，结合水下机器人离线的扩展參考模型为扩展的系统状态方程，对系统状态和推进器故障组成的扩展状态传递和更新，实时的估计出系统的状态和參数信息，得到系统估算的结果值均值ち和方差Pk ;这里系统的状态信息包括经过滤波后的位姿信息Sk和实时估计出来的速度信息ち，參数信息为系统推进器的故障參数bk，同时，依据主滤波器计算估计的新息信息ξ k计算出辅助滤波器的测量值Sqk ;建立UKF辅助滤波器的状态方程和測量方程，依据测量值Sqk作为系统的观测信号，采用自适应UKF的辅助滤波器KF对水下机器人系统的噪声信息进行实时估计，得到系统噪声协方差阵D的对角线元素も和方差^^的估算值；这里的噪声信息包括过程噪声Wk和测量噪声vk，它们的协方差矩阵分别为Dw和Dv。扩展參考模型中故障參数bk的％表示为k时刻第i个推进器的效率损失因子，取O I数值表示推进器故障发生失效的程度。离线的扩展參考模型的够建过程水下机器人的动力学模型表示如下Ms + C(s)s + D(s)s + G(s) = u式中，M为6X6维的惯性矩阵，包含离心カ和哥氏カ的6X6维矩阵，Z)问为·6X6维的水动カ矩阵，G(S)包含重力和浮力的6X1维矩阵，s和u分别为6X1维的状态和电机转矩向量；根据推进器的故障表现为系统电机转矩向量输入的变化，得到水下机器人的扩展參考模型表示如下Ms + C (i)i + D(s)s + G(s) = F{u)式中F(u)为推进器的故障函数，定义如下Fk(Uk) = uk+Ukbk
O…Ob\
O-U2k ··· Ob2kUk= . . . . A=；
_ ο ο --<」k上式中，Uk为6X1维电机转矩向量矩阵，bk为6X1维推进器故障參数矩阵，％表示为k时刻推进器的效率损失因子，上标i表示第i个推进器，当K = O吋，表示第i个推进器正常；当ぢ=埘，表示第i个推进器发生了硬性故障，即损失因子为100%;当0<% <埘，表示第i个推进器发生了软性故障，即部分失效。主滤波器扩展系统状态方程的形成过程扩展非线性系统时，含有未知/时变推进器的故障參数bk的系统状态空间方程为xk+1 = f (xk, bk, uk) +wkbk+1 = fb (xk, bk, uk) +Wbk式中，bk是k时刻的未知/时变推进器故障參数矢量，Wkb为故障參数模型的系统推进器故障噪声，是零均值的高斯噪声；因为故障參数bk变化规律未知，则可将bk视为不相关的随机漂移矢量，其递归表达式为bk = bk_!+wbk在基于自适应UKF的联合估计中，将系统推进器的故障參数追加到系统的真实状态后，组成扩展的状态矩阵，即< =OUff把系统方程组成为扩展系统状态方程的形式为
权利要求
1.一种基于自适应UKF的水下机器人状态和參数联合估计方法，其特征在于包括以下步骤 在水下 机器人动力学模型的基础上，对推进器故障进行建模，得到离线的扩展參考模型；根据位姿传感器探測的在线位姿信息yk，采用自适应UKF的主滤波器为标准UKF滤波估计算法，结合水下机器人离线的扩展參考模型为扩展的系统状态方程，对系统状态和推进器故障组成的扩展状态传递和更新，实时的估计出系统的状态和參数信息，得到系统估算的结果值均值ち和方差Pk ;这里系统的状态信息包括经过滤波后的位姿信息Sk和实时估计出来的速度信息ち，參数信息为系统推进器的故障參数bk，同吋，依据主滤波器计算估计的新息信息ξ k计算出辅助滤波器的测量值Sqk ;建立UKF辅助滤波器的状态方程和測量方程，依据测量值Sqk作为系统的观测信号，采用自适应UKF的辅助滤波器KF对水下机器人系统的噪声信息进行实时估计，得到系统噪声协方差阵D的对角线元素も和方差^^的估算值；这里的噪声信息包括过程噪声Wk和测量噪声vk，它们的协方差矩阵分别为Dw和Dv。
2.根据权利要求I所述的ー种基于自适应UKF的水下机器人状态和參数联合估计方法，其特征在于主滤波器扩展系统方程形成过程 扩展非线性系统时，含有未知/时变推进器的故障參数bk的系统状态空间方程为 Xk+1 — f (Xk，bk，Uk； +Wk bk+1 — fb、xk，bk，uk) +wkb 式中，bk是k时刻的未知/时变推进器故障參数，Wkb为故障參数模型的系统推进器故障噪声矢量，是零均值的高斯噪声； 当故障參数bk变化规律未知，则可将bk视为不相关的随机漂移矢量，其递归表达式为bk = K-Jwbk 基于自适应UKF的联合估计，将系统推进器的故障參数追加到系统的真实状态后，组成扩展的状态矩阵，即把系统方程组成为扩展系统状态方程为 xii=f(K^k)+wi 式中，!< =OKf为扩展的系统过程噪声矢量，其中W为系统过程噪声矢量，为估计推进器故障噪声矢量。
3.根据权利要求I或2所述的ー种基于自适应UKF的水下机器人状态和參数联合估计方法，其特征在于扩展參考模型中故障參数bk的％表示为k时刻第i个推进器的效率损失因子，取O I数值表示推进器故障发生失效的程度。
4.根据权利要求I所述的ー种基于自适应UKF的水下机器人状态和參数联合估计方法，其特征在干自适应UKF算法中辅助滤波器状态方程的构建 由水下机器人系统噪声方差矩阵对角线元素あ可将辅助滤波器的状态方程表示为 ^k=/M-O +wqk 其中… 为辅助滤波器中的系统噪声矢量，是零均值高斯白噪声； 由于あ的变化规律未知，可将其视为噪声驱动的不相关的随机漂移向量，所以可得辅助滤波器的状态方程 qk=qk-i+^qk。
5.根据权利要求I所述的ー种基于自适应UKF的水下机器人状态和參数联合估计方法，其特征在干自适应UKF算法中辅助滤波器測量方程的构建 以主滤波器的新息I k方差矩阵的测量值Sqk作为系统的观测信号，依据主滤波器扩展状态的ー步估计的方差Pklk+主滤波器扩展状态的方差Pk，主滤波器UKF的增益矩阵Kk组成辅助滤波器的測量方程为Sqk =g(qk) = Vdiag^(KrkKk) (Pk^l-Pk)Kk [KrkKk) 其中vdiag为取中括号内矩阵的主对角线元素组成的ー个向量。
6.根据权利要求I所述的ー种基于自适应UKF的水下机器人状态和參数联合估计方法，其特征在于主滤波器对扩展的状态方程滤波估计的实现过程对上ー时刻随机向量的被估计量均值和方差Plri经非线性无色变换后，得到的一组离散Sigma点X ,ベ，经系统状态方程计算更新Sigma点,然后计算出更新Sigma点的扩展状态均值和方差PkH1,对扩展状态均值和方差PkM计算得到一步估计的状态均值和方差；对一步估计的状态均值和方差再经无色变换和用測量方程计算更新，得到水下机器人可观测状态的均值3V-i、其方差AfcFi及协方差的估计值；读取传感器米集可观测位姿彳目息yk同扩展状态均值ち^和方差Pkllri和可观测状态的估计值及増益矩阵Kk经计算估计，得到系统均值ち、方差Pk的估算值，从中得到新息Ik;依主滤波器的新息Ik经计算得到测量值Sqk作为辅助滤波器的观测信号，然后接续辅助滤波器程序步骤。
7.根据权利要求I或6所述的ー种基于自适应UKF的水下机器人状态和參数联合估计方法，其特征在于辅助滤波器KF对系统过程噪声进行滤波估计的实现过程 上一时刻估计得到辅助滤波器的系统噪声方差矩阵的对角线元素も—,P其方差ちμ，用辅助滤波器的状态方程对和^L1计算更新后得到对角线元素和其方差ち·ェ的一歩估计值；用辅助滤波器的測量方程依主滤波器的一步估计的方差pkM、扩展状态方差Pk和增益矩阵Kk经计算更新后得到估计的观测值，所得到的一歩估计值和观测值5 同主滤波器的测量值Sqk经计算估计得到系统过程噪声方差阵D的对角线元素的估算值も和其方差Pqk，当有新观测位姿信息yk时，接续下ー控制周期主滤波器程序步骤。
全文摘要
本发明公开一种基于自适应UKF的水下机器人状态和参数联合估计方法，该方法首先建立了水下机器人的扩展参考模型，该参考模型有水下机器人的动力学模型和推进器的故障模型；依据位置传感器探测到的位姿信息，采用自适应UKF的主滤波器对水下机器人状态包括位姿和速度及推进器故障组成的扩展状态传递和更新，实时估计出水下机器人的速度信息和推进器故障信息；同时，依据主滤波器的新息信息，采用自适应UKF的辅助滤波器对系统的噪声信息进行实时的估计。该方法具有很好的实时性，可在线对系统的状态和参数进行联合估计，且当过程噪声和测量噪声的先验信息未知的情况下，该方法也能够达到较的估计精度。
文档编号B63C11/52GK102862666SQ20111019051
公开日2013年1月9日 申请日期2011年7月8日 优先权日2011年7月8日
发明者刘开周, 程大军, 李一平, 封锡盛 申请人:中国科学院沈阳自动化研究所