专利名称：一种基于ukf的水下机器人状态和参数联合估计方法
技术领域：
本发明涉及一种基于无色卡尔曼滤波(UKF,Unscented Kalman Filter)的水下机器人状态和参数联合估计方法，特别涉及一种基于UKF的水下机器人状态和推进器故障参数联合估计方法。
背景技术：
随着海洋开发事业的迅速发展，水下施工和建设项目越来越多，对于水下作用手段的性能要求也越来越高。由于水下机器人能够在水下进行观察、摄影、打捞和施工作业，因此在海洋开发中得到广泛的应用。随着水下机器人技术的不断发展成熟，其任务使命也越来越复杂，在很多情况下水下机器人所处的环境主要包括自身行为环境、外部自然环境、目标环境等是复杂未知的，所以要提高水下机器人的自主能力，以保证水下机器人顺利地完成任务使命，同时又可以适应复杂多变的动态环境。推进器的健康状况决定着水下机器人能否很好完成预期的任务。传统预编程的水下机器人在检测到推进器有故障发生时一般采取抛载上浮等措施，其实在很多情况下，水下机器人的推进器并非完全失效，如果能够实时的辨识出其效率损失因子，然后对推力进行重新分配，水下机器人仍可能完成预期的任务。申请号为200810042803. 8的中国专利文件(
公开日2010年3月17日)中公开的“水下机器人推进器信息融合故障诊断方法及装置”提供了依据推进器电机的转速电流信号和电压信号来对水下机器人推进器故障诊断和故障大小识别的装置。
发明内容
针对上述存在的问题，本发明提供了一种能实时自动探测信息并对其高精度估算的基于UKF的水下机器人状态和参数联合估计方法。为实现本发明的目的，本发明采用如下方案一种基于UKF的水下机器人状态和参数联合估计方法，包括如下步骤建立水下 机器人离线的扩展参考模型；根据位姿传感器探测的在线位姿信息yk，采用UKF滤波估计算法，结合水下机器人离线的扩展参考模型作为扩展的系统状态方程，对系统状态和推进器故障组成的扩展状态传递和更新，实时地估计出系统的状态和参数信息，得到系统估算的结果值4和Pk ;这里系统的状态信息包括经过滤波后的位姿信息qk和实时估计出来的速度信息％，参数信息为系统推进器的故障参数bk。离线的扩展参考模型的构成水下机器人的动力学模型表示如下Mq + C(q)q + D{q)q + G(q) = u式中，M为6X6维的惯性矩阵，包含离心力和哥氏力的6X6维矩阵，/Y办为6X6维的水动力矩阵，G(q)包含重力和浮力的6X1维矩阵，q和u分别为6X1维的状态和电机转矩向量；
根据推进器的故障表现为系统电机转矩向量U输入的变化，得到水下机器人的扩展参考模型表示如下Mq + C(q)q + D(q)q + G(q) = F{u)式中F(u)为推进器的故障函数，定义如下Fk(Uk) = uk+Ukbk
0…0 ] \b\
0—m, . 0b,Uk= . 'k . : ，bk= f
_ o o…-<」k 上式中，Uk为6X1维电机转矩向量矩阵，bk为6X1维故障参数矩阵，6丨表示为k时刻推进器的效率损失因子，上标i表示第i个推进器，色K = 0时，表示第i个推进器正常；当％ =埘，表示第i个推进器发生了硬性故障，即损失因子为100 % ;当0 < % < I时，表示第i个推进器发生了软性故障，即部分失效；所述的UKF滤波估计算法是以基本的卡尔曼滤波算法为框架，通过非线性的无色变换来进行非线性系统的状态和误差协方差的递推和更新。采用UKF滤波估计的实现过程对上一时刻即k_l由被估计量“先验”的均值和方差Plrl经非线性无色变换得到的一组离散Sigma点Xk+ Xk^1经系统状态方程计算更新后得到离散的Sigma点;，对
计算得到一步估计的扩展状态均值和方差PklH ;对^-郝PkIH再进行无色变化得到一组离散的Sugma点XkM，XklH经测量方程计算更新后得到离散的Sigma点Ykm，对YkIk-!计算得到水下机器人可观测状态的均值和方差的估计值；所得到的一步估计值和得到的可观测状态的估计值与传感器采集观测值yk，经计算估计得到系统估算值&和Pk;如果有新的测量值yk，重复上述步骤。对水下机器人参数的估计是通过联合估计来实现，其步骤如下联合估计即采用同一种方法对系统的状态和推进器的故障参数bk同时进行联合估计，在联合估计中，将推进器的故障参数bk作为系统的动态矢量，追加在实际的状态矢量[见，么『后，组成扩展的状态矢量，再使用UKF对其扩展参考模型进行估计，得到扩展的状态矢量的估计结果，进而可得到推进器故障参数bk的估计值。本发明与现有技术相比有益效果如下I.本发明实现简单，可以在现有的水下机器人载体上直接使用该方法，不需要对水下机器人的硬件系统进行任何的改动，也不需要增加任何的硬件设备。该发明可以嵌入水下机器人现有的线程中，对水下机器人来说，仅仅是增加了系统的计算量。2.本发明估计方法，是根据水下机器人传感器的探测信息，采用UKF算法，实时的估计水下机器人状态和推进器的故障参数信息，这些信息为水下机器人的容错控制提供了依据，能够实时的辨识出其效率损失因子，进而对推力进行重新分配，以保证水下机器人仍可能完成预期的任务。3.本发明估计精度高，可在线对系统的状态和参数进行联合估计。当过程噪声和测量噪声的先验信息已知的情况下，该方法能够达到较高的估计精度；由于UT直接作用在非线性动力学模型上，不需要对其进行线性化，所以避免了非线性系统线性化过程中产生的线性化误差。
图I为该发明方法的实现原理图；图2为UKF算法的滤波估计过程图；图3为该发明的估计流程图。
具体实施方式
下面结合附图和实施例对本发明方案进一步详细描述参见附图1，为本发明方法实现原理图。基于UKF的水下机器人状态和参数联合估计方法包括如下步骤建立水下机器人离线的扩展参考模型；根据位姿传感器探测的在线位姿信息yk，采用UKF滤波估计算法，结合水下机器人离线的扩展参考模型为扩展的系统状态方程，对系统状态和推进器故障组成的扩展状态传递和更新，实时的估计出系统的状态和参数信息，得到系统估算的结果值巧和Pk ;这里系统的状态信息包括经过滤波后的位姿信息qk和实时估计出来的速度信息A，参数信息为系统推进器的故障参数bk。离线的扩展参考模型的构成水下机器人的动力学模型表示如下Mq + C(q)q + D{q)q + G(q) = u式中，M为6X6维的惯性矩阵，包含离心力和哥氏力的6X6维矩阵，/Y办为6X6维的水动力矩阵，G(q)包含重力和浮力的6X1维矩阵，q和u分别为6X1维的状态和电机转矩向量；在水下机器人的动力学模型的基础上，根据推进器的故障表现为系统电机转矩向量u输入的变化，得到水下机器人的扩展参考模型表示如下Mq + C(q)q + D(q)q + G(q) = F{u)式中F(u)为推进器的故障函数，定义如下
权利要求
1.一种基于UKF的水下机器人状态和参数联合估计方法，其特征是包括如下步骤建立水下机器人离线的扩展参考模型；根据位姿传感器探测的在线位姿信息yk，采用UKF滤波估计算法，结合水下机器人离线的扩展参考模型为扩展的系统状态方程，对系统状态和推进器故障组成的扩展状态传递和更新，实时的估计出系统的状态和参数信息，得到系统估算的结果值4和Pk ;这里系统的状态信息包括经过滤波后的位姿信息qk和实时估计出来的速度信息％，参数信息为系统推进器的故障参数bk。
2.根据权利要求I所述的一种基于UKF的水下机器人状态和参数联合估计方法，其特征是水下机器人参考模型的构建 水下机器人的动力学模型表示如下
3.根据权利要求I所述的一种基于UKF的水下机器人状态和参数联合估计方法，其特征是所述的UKF滤波估计算法是以基本的卡尔曼滤波算法为框架，通过非线性的无色变换来进行非线性系统的状态和误差协方差的递推和更新。
4.根据权利要求I或3所述的一种基于UKF的水下机器人状态和参数联合估计方法，其特征是，采用UKF滤波估计的实现过程 对上一时刻即k-1由被估计量“先验”的均值:和方差Plri经非线性无色变换得到的一组离散Sigma点XkfXlri经系统状态方程计算更新后得到离散的Sigma点，对3十算得到一步估计的扩展状态均值^^和方差Pkllrf和Pkllrf再进行无色变化得到一组离散的Sigma点Xklk-PXkllrf经测量方程计算更新后得到离散的Sigma点Y ^，对Y k|k-!计算得到水下机器人可观测状态的均值和方差仏&的估计值；所得到的一步估计值和得到的可观测状态的估计值与传感器采集观测值yk，经计算估计得到系统估算值4和Pk ;如果有新的测量值yk，重复上述步骤。
5.根据权利要求I或3所述的一种基于UKF的水下机器人状态和参数联合估计方法，其特征是对水下机器人参数的估计是通过联合估计来实现步骤如下 联合估计即采用同一种方法对系统的状态[%, Ar和推进器的故障参数bk同时进行联合估计，在联合估计中，将推进器的故障参数bk作为系统的动态矢量，追加在实际的状态矢量[%，Ar后，组成扩展的状态矢量，再使用UKF对其扩展参考模型进行估计，得到扩展的状态矢的估计结果，进而可得到推进器故障参数bk的估计值。
全文摘要
本发明公开一种基于UKF的水下机器人状态和参数联合估计方法，该方法首先建立了水下机器人的扩展参考模型包括水下机器人的动力学模型和推进器的故障模型。本发明依据位置传感器探测到的位姿信息，采用UKF算法，以水下机器人状态包括位姿和速度及推进器故障参数，对扩展参考模型进行在线联合估计，实时估计出水下机器人的速度信息和推进器故障信息。该方法具有很好的实时性，可在线对系统的状态和参数进行联合估计；当过程噪声和测量噪声的先验信息已知的情况下，该方法能够达到较高的估计精度。
文档编号B63C11/52GK102795323SQ201110137339
公开日2012年11月28日 申请日期2011年5月25日 优先权日2011年5月25日
发明者刘开周, 程大军, 李一平, 封锡盛 申请人:中国科学院沈阳自动化研究所