CN102862666B

CN102862666B - Underwater robot state and parameter joint estimation method based on self-adaption unscented Kalman filtering (UKF)

Info

Publication number: CN102862666B
Application number: CN201110190512.5A
Authority: CN
Inventors: 刘开周; 程大军; 李一平; 封锡盛
Original assignee: Shenyang Institute of Automation of CAS
Current assignee: Shenyang Institute of Automation of CAS
Priority date: 2011-07-08
Filing date: 2011-07-08
Publication date: 2014-12-10
Anticipated expiration: 2031-07-08
Also published as: CN102862666A

Abstract

The invention discloses a method for jointly estimating the state and parameters of an underwater robot based on self-adaptive UKF. The method first establishes an extended reference model of the underwater robot, and the reference model includes a dynamic model of the underwater robot and a fault model of the thruster. ; According to the position and orientation information detected by the position sensor, the main filter of the adaptive UKF is used to transmit and update the extended state of the underwater robot state, including the position, velocity and thruster failure, and estimate the velocity information and Thruster fault information; at the same time, according to the innovation information of the main filter, the auxiliary filter of adaptive UKF is used to estimate the noise information of the system in real time. This method has good real-time performance, and can jointly estimate the state and parameters of the system online, and when the prior information of process noise and measurement noise is unknown, this method can also achieve relatively high estimation accuracy.

Description

A Joint Estimation Method of State and Parameters of Underwater Robot Based on Adaptive UKF

技术领域 technical field

本发明涉及一种基于自适应无色卡尔曼滤波(UKF，Unscented KalmanFilter)的水下机器人状态和参数联合估计方法，特别涉及一种基于自适应UKF的水下机器人状态和推进器故障参数联合估计方法。The present invention relates to a kind of joint estimation method of state and parameter of underwater robot based on self-adaptive colorless Kalman filter (UKF, Unscented KalmanFilter), particularly relate to a kind of joint estimation method of state of underwater robot and thruster failure parameter based on self-adaptive UKF method.

技术背景 technical background

随着海洋开发事业的迅速发展，水下施工和建设项目越来越多，对于水下作用手段的性能要求也越来越高。由于水下机器人能够在水下进行观察、摄影、打捞和施工作业，因此在海洋开发中得到广泛的应用。而推进器的健康状况决定着水下机器人能否很好完成预期的任务。传统预编程的水下机器人在检测到推进器有故障发生时一般采取抛载上浮等措施，其实在很多情况下，水下机器人的推进器并非完全失效，如果能够实时的辨识出其效率损失因子，然后对推力进行重新分配，水下机器人仍可能完成预期的任务。With the rapid development of marine development, there are more and more underwater construction and construction projects, and the performance requirements for underwater means of action are also getting higher and higher. Because underwater robots can perform observation, photography, salvage and construction operations underwater, they are widely used in ocean development. The health of the propeller determines whether the underwater robot can complete the expected tasks well. Traditional pre-programmed underwater robots generally take measures such as dumping and floating when detecting a fault in the propeller. In fact, in many cases, the propeller of the underwater robot is not completely invalid. If the efficiency loss factor can be identified in real time , and then redistribute the thrust, the underwater robot may still complete the expected tasks.

在水下机器人系统的噪声信息已知的情况下，采用UKF算法能够达到很好的估计效果，而在系统噪声信息未知时，如果设置的噪声参数与实际的噪声相差过大，就会导致有偏估计甚至滤波算法的发散，因此要设计和采用更适合于估计系统噪声信息的滤波算法，以达到实时的估计水下机器人系统的噪声信息，在噪声信息未知的情况下仍可以达到很好的估计效果。When the noise information of the underwater robot system is known, the UKF algorithm can achieve a good estimation effect, but when the noise information of the system is unknown, if the set noise parameters are too different from the actual noise, it will lead to some problems. Therefore, it is necessary to design and adopt a filtering algorithm that is more suitable for estimating the noise information of the system, so as to achieve real-time estimation of the noise information of the underwater robot system, and it can still achieve good accuracy when the noise information is unknown. estimated effect.

发明内容 Contents of the invention

针对上述存在的问题，本发明提供了一种能实时估计推进器故障信息和系统噪声信息，并具有实用性好、估算精度高的基于自适应UKF的水下机器人状态和参数联合估计方法In view of the above existing problems, the present invention provides a joint estimation method of state and parameters of underwater robot based on self-adaptive UKF, which can estimate thruster fault information and system noise information in real time, and has good practicability and high estimation accuracy

为实现本发明的目的，本发明采用如下方案：For realizing the purpose of the present invention, the present invention adopts following scheme:

一种基于自适应UKF的水下机器人状态和参数联合估计方法，其特征在于包括以下步骤：A method for jointly estimating the state and parameters of an underwater robot based on adaptive UKF is characterized in that it comprises the following steps:

在水下机器人动力学模型的基础上，对推进器故障进行建模，得到离线的扩展参考模型；根据位姿传感器探测的在线位姿信息y_k，采用自适应UKF的主滤波器为标准UKF滤波估计算法，结合水下机器人离线的扩展参考模型为扩展的系统状态方程，对系统状态和推进器故障组成的扩展状态传递和更新，实时估计出系统的状态和参数信息，得到系统估算的结果值均值和方差P_k；这里系统的状态信息包括经过滤波后的位姿信息s_k和实时估计出来的速度信息参数信息为系统推进器的故障参数b_k，同时，依据主滤波器计算估计的新息信息ξ_k计算出辅助滤波器的测量值Sq_k；建立UKF辅助滤波器的状态方程和测量方程，依据测量值Sq_k作为系统的观测信号，采用自适应UKF的辅助滤波器KF对水下机器人系统的噪声信息进行实时估计，得到系统噪声协方差阵D的对角线元素和方的估算值，这里的噪声信息包括过程噪声w_k和测量噪声v_k，它们的协方差矩阵分别为D^w和D^v。On the basis of the dynamic model of the underwater robot, the propeller fault is modeled to obtain an offline extended reference model; according to the online pose information y _k detected by the pose sensor, the main filter of the adaptive UKF is used as the standard UKF The filter estimation algorithm, combined with the offline extended reference model of the underwater robot, is the extended system state equation, which transmits and updates the extended state composed of the system state and thruster failure, estimates the state and parameter information of the system in real time, and obtains the result of system estimation value mean and variance P _k ; here the state information of the system includes filtered pose information _sk and real-time estimated speed information The parameter information is the fault parameter b _k of the propeller of the system. At the same time, the measurement value Sq _k of the auxiliary filter is calculated according to the new information ξ _k calculated and estimated by the main filter; the state equation and measurement equation of the UKF auxiliary filter are established, based on The measured value Sq _k is used as the observation signal of the system, and the auxiliary filter KF of the adaptive UKF is used to estimate the noise information of the underwater vehicle system in real time, and the diagonal elements of the system noise covariance matrix D are obtained Peaceful The estimated value of , the noise information here includes process noise w _k and measurement noise v _k , and their covariance matrices are D ^w and D ^v respectively.

主滤波器扩展系统方程形成过程：Formation process of main filter extended system equation:

扩展非线性系统时，含有未知/时变推进器的故障参数b_k的系统状态空间方程为：When expanding the nonlinear system, the state space equation of the system with unknown/time-varying thruster fault parameter _bk is:

x_k+1＝f(x_k，b_k，u_k)+w_k x _k+1 ＝f(x _k ，b _k ，u _k )+w _k

b_k+1＝f_b(x_k，b_k，u_k)+w_kb b _k+1 ＝f _b (x _k ，b _k ，u _k )+w _kb

式中，b_k是k时刻的未知/时变推进器故障参数，w_kb为故障参数模型的系统推进器故障噪声矢量，是零均值的高斯噪声；where b _k is the unknown/time-varying thruster fault parameter at time k, and w _kb is the system thruster fault noise vector of the fault parameter model, which is Gaussian noise with zero mean value;

当故障参数b_k变化规律未知，则可将b_k视为不相关的随机漂移矢量，其递归表达式为：When the change law of the fault parameter b _k is unknown, b _k can be regarded as an uncorrelated random drift vector, and its recursive expression is:

b_k＝b_k-1+w_bk b _k ＝b _k-1 +w _{b k}

基于自适应UKF的联合估计，将系统推进器的故障参数追加到系统的真实状态后，组成扩展的状态矩阵，即把系统方程组成为扩展系统状态方程为：Based on the joint estimation of adaptive UKF, the fault parameters of the propellers of the system are added to the real state of the system to form an extended state matrix, namely Composing the system equations into an extended system state equation is:

${x x}_{k k + + 11}^{a a} = = \overset{~ ~}{f f} (({x x}_{k k}^{a a},, {u u}_{k k})) + + {w w}_{k k}^{a a}$

式中，为扩展的系统过程噪声矢量，其中为系统过程噪声矢量，为估计推进器故障噪声矢量。In the formula, is the extended system process noise vector, where is the system process noise vector, To estimate the thruster fault noise vector.

扩展参考模型中故障参数b_k的表示为k时刻第i个推进器的效率损失因子，取0～1数值表示推进器故障发生失效的程度。The fault parameter b _k in the extended reference model Expressed as the efficiency loss factor of the i-th propeller at time k, the value of 0 to 1 indicates the degree of propeller failure.

自适应UKF算法中辅助滤波器状态方程的构建：The construction of the auxiliary filter state equation in the adaptive UKF algorithm:

由水下机器人系统噪声方差矩阵对角线元素可将辅助滤波器的状态方程表示为：The diagonal elements of the noise variance matrix of the underwater vehicle system The state equation of the auxiliary filter can be expressed as:

${\overset{&OverBar; &OverBar;}{q q}}_{k k} = = {f f}_{q q} (({\overset{&OverBar; &OverBar;}{q q}}_{k k - - 11})) + + {w w}_{{q q}_{k k}}$

其中为辅助滤波器中的系统噪声矢量，是零均值高斯白噪声；in is the system noise vector in the auxiliary filter, which is zero-mean Gaussian white noise;

由于的变化规律未知，可将其视为噪声驱动的不相关的随机漂移向量，所以可得辅助滤波器的状态方程：because The change law of is unknown, and it can be regarded as an uncorrelated random drift vector driven by noise, so the state equation of the auxiliary filter can be obtained:

${\overset{&OverBar; &OverBar;}{q q}}_{k k} = = {\overset{&OverBar; &OverBar;}{q q}}_{k k - - 11} + + {w w}_{{q q}_{k k}} . .$

自适应UKF算法中辅助滤波器测量方程的构建：Construction of auxiliary filter measurement equation in adaptive UKF algorithm:

以主滤波器的新息ξ_k方差矩阵的测量值Sq_k作为系统的观测信号，依据主滤波器扩展状态的一步估计的方差P_k|k-1，主滤波器扩展状态的方差P_k，主滤波器UKF的增益矩阵K_k组成辅助滤波器的测量方程为：Taking the measured value Sq _k of the variance matrix of the innovation ξ _k of the main filter as the observed signal of the system, according to the variance P _k|k-1 of the one-step estimation of the extended state of the main filter, the variance P _k of the extended state of the main filter, The measurement equation of the auxiliary filter composed of the gain matrix K _k of the main filter UKF is:

$\overset{&OverBar; &OverBar;}{S S} {q q}_{k k} = = g g (({\overset{&OverBar; &OverBar;}{q q}}_{k k})) = = vdiag vdiag [[{(({K K}_{k k}^{T T} {K K}_{k k}))}^{- - 11} {K K}_{k k}^{T T} (({P P}_{k k | | k k - - 11} - - {P P}_{k k})) {K K}_{k k} {(({K K}_{k k}^{T T} {K K}_{k k}))}^{- - 11}]]$

其中vdiag为中括号内矩阵的主对角线元素组成的一个向量。Where vdiag is a vector composed of the main diagonal elements of the matrix in square brackets.

主滤波器对扩展的状态方程滤波估计的实现过程：The implementation process of the main filter for the extended state equation filter estimation:

对上一时刻随机向量的被估计量均值和方差P_k-1经非线性无色变换后，得到的一组离散Sigma点χ_k-1，经系统状态方程计算更新Sigma点，然后计算出更新Sigma点的扩展状态均值和方差P_k|k-1，对扩展状态均值和方差P_k|k-1计算得到一步估计的状态均值和方差；对一步估计的状态均值和方差再经无色变换和用测量方程计算更新，得到水下机器人可观测状态的均值其方差及协方差的估计值；读取传感器采集可观测位姿信息y_k同扩展状态均值和方差P_k|k-1和可观测状态的估计值及增益矩阵K_k经计算估计，得到系统均值方差P_k的估算值，从中得到新息ξ_k；依主滤波器的新息ξ_k经计算得到测量值Sq_k作为辅助滤波器的观测信号，然后接续辅助滤波器程序步骤。The mean value of the estimator of the random vector at the previous moment A set of discrete Sigma points χ _k-1 obtained after the sum variance P _k-1 undergoes nonlinear colorless transformation, the updated Sigma points are calculated by the system state equation, and then the extended state mean value of the updated Sigma points is calculated and variance P _k|k-1 , for the extended state mean Calculate the state mean and variance of the one-step estimate with the variance P _k|k-1 ; the state mean and variance of the one-step estimate are then updated by colorless transformation and calculated with the measurement equation to obtain the mean value of the observable state of the underwater robot its variance and covariance The estimated value of ; read the sensor to collect the observable pose information y _k and the mean value of the extended state The sum variance P _k|k-1 and the estimated value of the observable state and the gain matrix K _k are calculated and estimated to obtain the system mean The estimated value of the variance P _k is obtained from it to obtain the innovation ξ _k ; the measured value Sq _k is calculated according to the innovation ξ _k of the main filter as the observation signal of the auxiliary filter, and then the auxiliary filter program steps are continued.

辅助滤波器KF对系统过程噪声进行滤波估计的实现过程：The realization process of the auxiliary filter KF for filtering and estimating the system process noise:

上一时刻估计得到辅助滤波器的系统噪声方差矩阵的对角线元素和其方差用辅助滤波器的状态方程对和计算更新后得到对角线元素和其方差的一步估计值；用辅助滤波器的测量方程依主滤波器的一步估计的方差P_k|k-1、扩展状态方差P_k和增益矩阵K_k经计算更新后得到估计的观测值所得到的一步估计值和观测值同主滤波器的测量值Sq_k经计算估计得到系统过程噪声方差阵D的对角线元素的估算值和其方差当有新观测位姿信息y_k时，接续下一控制周期主滤波器程序步骤。The diagonal elements of the system noise variance matrix of the auxiliary filter estimated at the previous moment and its variance Using the state equation of the auxiliary filter for and Calculate the diagonal elements after updating and its variance One-step estimated value of ; use the measurement equation of the auxiliary filter to calculate and update the estimated observed value according to the one-step estimated variance P _k|k-1 of the main filter, the extended state variance P _k and the gain matrix K _k The resulting one-step estimates and observations The measured value Sq _k of the same main filter is calculated and estimated to obtain the estimated value of the diagonal elements of the system process noise variance matrix D and its variance When there is new observed pose information y _k , the main filter program step of the next control cycle is continued.

本发明与现有技术相比有益效果如下：Compared with the prior art, the present invention has the following beneficial effects:

1.本发明实现简单，可以在现有的水下机器人载体上直接使用该方法，不需要对水下机器人的硬件系统进行任何的改动，也不需要增加任何的硬件设备。该发明可以嵌入水下机器人现有的线程中，对水下机器人来说，仅仅是增加了系统的计算量。1. The present invention is simple to realize, and the method can be used directly on the existing underwater robot carrier, without any modification to the hardware system of the underwater robot, and no need to add any hardware equipment. The invention can be embedded in the existing thread of the underwater robot, and for the underwater robot, it only increases the calculation amount of the system.

2.本发明估计方法，是根据水下机器人传感器的探测信息，采用自适应UKF算法，实时的估计水下机器人状态和推进器的故障参数信息，这些信息为水下机器人的容错控制提供了依据，能够实时的辨识出其效率损失因子，然后对推力进行重新分配，以保证水下机器人仍可能完成预期的任务。2. The estimation method of the present invention is based on the detection information of the underwater robot sensor, adopts the adaptive UKF algorithm, estimates the fault parameter information of the underwater robot state and the propeller in real time, and these information provide the basis for the fault-tolerant control of the underwater robot , can identify its efficiency loss factor in real time, and then redistribute the thrust to ensure that the underwater robot can still complete the expected tasks.

3.本发明估计精度高，可在线对系统的状态和参数进行联合估计。由于UT直接作用在非线性动力学模型上，不需要对其进行线性化，所以避免了非线性系统线性化过程中产生的线性化误差。3. The present invention has high estimation accuracy, and can jointly estimate the state and parameters of the system online. Since the UT directly acts on the nonlinear dynamic model, it does not need to be linearized, so the linearization error generated during the linearization of the nonlinear system is avoided.

4.在系统的噪声先验信息包括过程噪声和测量噪声未知的情况下，如果设置的噪声和实际的噪声有较大偏差，采用UKF滤波估计算法会产生有偏误差或者滤波算法的发散，而自适应UKF算法中由于辅助滤波器可以对系统的噪声信息进行实时估计，且能够较快的收敛，所以能够达到较高的估计精度。4. In the case where the noise prior information of the system includes unknown process noise and measurement noise, if there is a large deviation between the set noise and the actual noise, the UKF filter estimation algorithm will produce biased errors or divergence of the filter algorithm, while In the adaptive UKF algorithm, because the auxiliary filter can estimate the noise information of the system in real time, and can converge quickly, it can achieve higher estimation accuracy.

附图说明 Description of drawings

图1为该发明方法的实现原理图；Fig. 1 is the realization schematic diagram of this inventive method;

图2为该发明的滤波估计流程图。Fig. 2 is a flow chart of filtering estimation of the invention.

具体实施方式 Detailed ways

下面结合附图和实施例对本发明方案进一步详细描述：Below in conjunction with accompanying drawing and embodiment the scheme of the present invention is described in further detail:

参见附图1～2，为本发明方法实现原理图和其滤波估计流程图。一种基于自适应UKF的水下机器人状态和参数联合估计方法，其特征在于包括以下步骤：Referring to accompanying drawings 1-2, it is the realization schematic diagram of the method of the present invention and its filter estimation flow chart. A method for jointly estimating the state and parameters of an underwater robot based on adaptive UKF is characterized in that it comprises the following steps:

在水下机器人动力学模型的基础上，对推进器故障进行建模，得到离线的扩展参考模型；根据位姿传感器探测的在线位姿信息y_k，采用自适应UKF的主滤波器为标准UKF滤波估计算法，结合水下机器人离线的扩展参考模型为扩展的系统状态方程，对系统状态和推进器故障组成的扩展状态传递和更新，实时的估计出系统的状态和参数信息，得到系统估算的结果值均值和方差P_k；这里系统的状态信息包括经过滤波后的位姿信息s_k和实时估计出来的速度信息参数信息为系统推进器的故障参数b_k，同时，依据主滤波器计算估计的新息信息ξ_k计算出辅助滤波器的测量值Sq_k；建立UKF辅助滤波器的状态方程和测量方程，依据测量值Sq_k作为系统的观测信号，采用自适应UKF的辅助滤波器KF对水下机器人系统的噪声信息进行实时估计，得到系统噪声协方差阵D的对角线元素和方差的估算值；这里的噪声信息包括过程噪声w_k和测量噪声v_k，它们的协方差矩阵分别为D^w和D^v。On the basis of the dynamic model of the underwater robot, the propeller fault is modeled to obtain an offline extended reference model; according to the online pose information y _k detected by the pose sensor, the main filter of the adaptive UKF is used as the standard UKF The filter estimation algorithm, combined with the offline extended reference model of the underwater robot, is the extended system state equation, which transmits and updates the extended state composed of the system state and thruster faults, estimates the state and parameter information of the system in real time, and obtains the estimated value of the system. result mean and variance P _k ; here the state information of the system includes filtered pose information _sk and real-time estimated speed information The parameter information is the fault parameter b _k of the propeller of the system. At the same time, the measurement value Sq _k of the auxiliary filter is calculated according to the new information ξ _k calculated and estimated by the main filter; the state equation and measurement equation of the UKF auxiliary filter are established, based on The measured value Sq _k is used as the observation signal of the system, and the auxiliary filter KF of the adaptive UKF is used to estimate the noise information of the underwater vehicle system in real time, and the diagonal elements of the system noise covariance matrix D are obtained and variance The estimated value of ; the noise information here includes process noise w _k and measurement noise v _k , and their covariance matrices are D ^w and D ^v respectively.

离线的扩展参考模型的够建过程：The process of building an offline extended reference model:

水下机器人的动力学模型表示如下：The dynamic model of the underwater robot is expressed as follows:

$M m \overset{\cdot &Center Dot; \cdot &Center Dot;}{s the s} + + C C ((\overset{\cdot \cdot}{s the s})) \overset{\cdot &Center Dot;}{s the s} + + D D. ((\overset{\cdot &Center Dot;}{s the s})) \overset{\cdot &Center Dot;}{s the s} + + G G ((s the s)) = = u u$

式中，M为6×6维的惯性矩阵，包含离心力和哥氏力的6×6维矩阵，为6×6维的水动力矩阵，G(s)包含重力和浮力的6×1维矩阵，s和u分别为6×1维的状态和电机转矩向量；In the formula, M is a 6×6 dimensional inertia matrix, A 6×6 dimensional matrix containing centrifugal and Coriolis forces, is a 6×6 dimensional hydrodynamic matrix, G(s) contains a 6×1 dimensional matrix of gravity and buoyancy, s and u are 6×1 dimensional state and motor torque vectors, respectively;

根据推进器的故障表现为系统电机转矩向量输入的变化，得到水下机器人的扩展参考模型表示如下：According to the failure of the thruster as the change of the torque vector input of the system motor, the extended reference model of the underwater robot is expressed as follows:

$M m \overset{\cdot &Center Dot; \cdot &Center Dot;}{s the s} + + C C ((\overset{\cdot &Center Dot;}{s the s})) \overset{\cdot &Center Dot;}{s the s} + + D D. ((\overset{\cdot \cdot}{s the s})) \overset{\cdot &Center Dot;}{s the s} + + G G ((s the s)) = = F f ((u u))$

式中F(u)为推进器的故障函数，定义如下：where F(u) is the fault function of the thruster, defined as follows:

F_k(u_k)＝u_k+U_kb_k F _k (u _k )＝u _k +U _k b _k

$b_{k} = [\begin{matrix} b_{k}^{1} \\ b_{k}^{2} \\ . \\ . \\ . \\ b_{k}^{6} \end{matrix}]$ $b_{k} = [\begin{matrix} b_{k}^{1} \\ b_{k}^{2} \\ . \\ . \\ . \\ b_{k}^{6} \end{matrix}]$

上式中，u_k为6×1维电机转矩向量矩阵，b_k为6×1维推进器故障参数矩阵，表示为k时刻推进器的效率损失因子，上标i表示第i个推进器，当时，表示第i个推进器正常；当时，表示第i个推进器发生了硬性故障，即损失因子为100％；当时，表示第i个推进器发生了软性故障，即部分失效。In the above formula, u _k is a 6×1-dimensional motor torque vector matrix, b _k is a 6×1-dimensional thruster fault parameter matrix, Expressed as the efficiency loss factor of the propeller at time k, the superscript i represents the ith propeller, when When , it means that the i-th propeller is normal; when When When , it means that the i-th thruster has a soft fault, that is, a partial failure.

主滤波器扩展系统状态方程的形成过程：The formation process of the main filter extended system state equation:

x_k+1＝f(x_k，b_k，u_k)+w_k x _k+1 ＝f(x _k ，b _k ，u _k )+w _k

b_k+1＝f_b(x_k，b_k，u_k)+w_bk b _k+1 ＝f _b (x _k ，b _k ，u _k )+w _bk

式中，b_k是k时刻的未知/时变推进器故障参数矢量，w_kb为故障参数模型的系统推进器故障噪声，是零均值的高斯噪声；In the formula, b _k is the unknown/time-varying thruster fault parameter vector at time k, and w _kb is the system thruster fault noise of the fault parameter model, which is Gaussian noise with zero mean value;

因为故障参数b_k变化规律未知，则可将b_k视为不相关的随机漂移矢量，其递归表达式为：Because the change law of the fault parameter b _k is unknown, b _k can be regarded as an uncorrelated random drift vector, and its recursive expression is:

b_k＝b_k-1+w_bk b _k ＝b _k-1 +w _{b k}

在基于自适应UKF的联合估计中，将系统推进器的故障参数追加到系统的真实状态后，组成扩展的状态矩阵，即把系统方程组成为扩展系统状态方程的形式为：In the joint estimation based on adaptive UKF, the fault parameters of the system thrusters are added to the real state of the system to form an extended state matrix, namely Composing the system equations into an extended system state equation is in the form:

式中，为扩展的系统过程噪声矢量，其中为系统过程噪声矢量，表示估计推进器的噪声矢量。In the formula, is the extended system process noise vector, where is the system process noise vector, represents the noise vector of the estimated thruster.

对上一时刻随机向量的被估计量的均值和方差P_k-1经非线性无色变换后，得到的一组离散Sigma点χ_k-1，经系统状态方程计算更新Sigma点，然后计算出更新Sigma点的扩展状态均值和方差P_k|k-1，对扩展状态均值和方差P_k|k-1计算得到一步估计的状态均值和方差；对一步估计的状态均值和方差再经无色变换和用测量方程计算更新，得到水下机器人可观测状态的均值其方差及协方差的估计值；读取传感器采集可观测位姿信息y_k同扩展状态均值和方差P_k|k-1和可观测状态的估计值及增益矩阵K_k经计算估计，得到系统均值方差P_k估算值，从中得到新息ξ_k；依主滤波器的新息ξ_k经计算得到测量值Sq_k作为辅助滤波器的观测信号，然后接续辅助滤波器程序步骤。The mean value of the estimated quantity of the random vector at the last moment A set of discrete Sigma points χ _k-1 obtained after the sum variance P _k-1 undergoes nonlinear colorless transformation, the updated Sigma points are calculated by the system state equation, and then the extended state mean value of the updated Sigma points is calculated and variance P _k|k-1 , for the extended state mean Calculate the state mean and variance of the one-step estimate with the variance P _k|k-1 ; the state mean and variance of the one-step estimate are then updated by colorless transformation and calculated with the measurement equation to obtain the mean value of the observable state of the underwater robot its variance and covariance The estimated value of ; read the sensor to collect the observable pose information y _k and the mean value of the extended state The sum variance P _k|k-1 and the estimated value of the observable state and the gain matrix K _k are calculated and estimated to obtain the system mean Estimated value of variance P _k , from which the innovation ξ _k is obtained; calculated according to the innovation ξ _k of the main filter, the measured value Sq _k is obtained as the observation signal of the auxiliary filter, and then the auxiliary filter program steps are continued.

参见附图2，所述的自适应滤波估计算法是以基本的卡尔曼滤波算法为框架，通过非线性的无色变换来进行非线性系统的状态和误差协方差的递推和更新。Referring to Figure 2, the adaptive filter estimation algorithm is based on the basic Kalman filter algorithm, and performs recursion and update of the state and error covariance of the nonlinear system through nonlinear colorless transformation.

非线性系统的无色变换：Colorless transformations for nonlinear systems:

无色变换是自适应UKF算法实现的基础，也是区别于其它非线性滤波的本质特点。无色变换(UT，Unscented Transform)的基础原理是用采样点的分布来近似随机变量的概率分布，由被估计量的“先验”均值和方差，产生一批离散的与被估计量具有相同概率统计特性的采样点，称为Sigma点，在根据经过非线性方程传递后的Sigma点，生成“后验”的均值和方差。Colorless transformation is the basis of the adaptive UKF algorithm, and it is also the essential feature that distinguishes it from other nonlinear filters. The basic principle of colorless transformation (UT, Unscented Transform) is to use the distribution of sampling points to approximate the probability distribution of random variables. From the "prior" mean and variance of the estimated quantity, a batch of discrete values with the same The sampling point of the probability and statistical characteristics is called the Sigma point. According to the Sigma point passed through the nonlinear equation, the "posterior" mean and variance are generated.

如果已知非线性函数y＝j(x)，其中为一随机向量，它的均值和方差分别为和P_x，求y的均值和方差P_y，其无色变换的步骤如下：If the nonlinear function y=j(x) is known, where is a random vector whose mean and variance are and P _x , find the mean of y and variance P _y , the steps of its colorless transformation are as follows:

Sigma点的生成Generation of Sigma points

根据随机向量x的均值和方差P_x，可以构造一组关于对称求分布于其附近的离散Sigma点，可以记为χ_i，i＝1，…，2l分别对应各个Sigma点。可以用这一组Sigma点近似表示随机向量x的近似分布，具体如下：According to the mean of the random vector x and variance P _x , you can construct a set of The discrete Sigma points distributed in its vicinity can be obtained symmetrically, which can be recorded as χ _i , where i=1,..., 2l correspond to each Sigma point respectively. This set of Sigma points can be used to approximate the approximate distribution of the random vector x, as follows:

${χ χ}_{00} = = \overset{&OverBar; &OverBar;}{x x}$

${χ χ}_{i i} = = \overset{&OverBar; &OverBar;}{x x} - - {((\sqrt{((l l + + λ λ)) {P P}_{x x}}))}_{i i} i i = = 11,, . . . . . .,, l l$

${χ χ}_{i i} = = \overset{&OverBar; &OverBar;}{x x} + + {((\sqrt{((l l + + λ λ)) {P P}_{x x}}))}_{i i - - 11} i i = = l l + + 11,, . . . . . .,, 22 l l$

式中，χ_i为经过无色变换后的Sigma点；l为状态和噪声组成的扩展状态的维数，λ为控制Sigma点到均值距离的尺度常数。用这一组Sigma点近似表示随机向量的分布，Sigma点具有与x相同的均值和方差。In the formula, _χi is the Sigma point after the colorless transformation; l is the dimension of the extended state composed of state and noise, and λ is the scale constant controlling the distance from the Sigma point to the mean. The distribution of the random vector is approximated by this set of sigma points, which have the same mean and variance as x.

计算非线性函数Compute Nonlinear Functions

γ_i＝j(χ_i)i＝0，1，...，2lγ _i = j(χ _i ) i = 0, 1, . . . , 2l

用γ_i近似非线性函数的分布。Approximate the distribution of a nonlinear function with _γi .

计算非线性函数y的均值和方差：Compute the mean and variance of a nonlinear function y:

$\overset{&OverBar; &OverBar;}{y the y} = = {Σ Σ}_{i i = = 00}^{22 l l} {d d}_{i i}^{m m} {γ γ}_{i i}$

${P P}_{y the y} = = {Σ Σ}_{i i = = 00}^{22 l l} {d d}_{i i}^{c c} (({γ γ}_{i i} - - \overset{&OverBar; &OverBar;}{y the y})) {(({γ γ}_{i i} - - \overset{&OverBar; &OverBar;}{y the y}))}^{T T}$

式中为权系数，且In the formula is the weight coefficient, and

${d d}_{00}^{m m} = = \frac{λ λ}{λ λ + + l l}$

${d d}_{00}^{c c} = = \frac{λ λ}{λ λ + + l l} + + ((11 - - {α α}^{22} + + β β))$

${d d}_{i i}^{m m} = = {d d}_{i i}^{c c} = = \frac{11}{22 ((l l + + λ λ))} i i = = 11,, . . . . . .,, 22 l l$

其中，α控制Sigma点分布的范围，取值区域为0≤α≤1，一般取1；β是非负常数，其作用是使变换后的方差含有部分的高阶信息，对于高斯分布有β＝2；滤波器参数λ＝α²(l+e)-l，其中e为常数，它与估算的高阶项有关，它等于或高于4阶的高阶项为其函数，通常状态估算取0，参数估算取3-l。Among them, α controls the range of Sigma point distribution, and the value range is 0≤α≤1, generally 1; β is a non-negative constant, and its function is to make the transformed variance contain some high-order information. For Gaussian distribution, β= 2; filter parameter λ=α ² (l+e)-l, where e is a constant, which is related to the estimated high-order item, which is equal to or higher than the fourth-order high-order item as its function, and the state estimation usually takes 0, parameter estimation takes 3-l.

参见附图2，为该发明的流程图；在本实施中该方案的实现流程：Referring to accompanying drawing 2, be the flowchart of this invention; In this implementation, the realization process of this scheme:

自适应UKF主滤波器-标准UKF对扩展的状态方程滤波估计的实现流程：滤波初始化：Adaptive UKF main filter - the implementation process of standard UKF for the extended state equation filter estimation: filter initialization:

读取水下机器人系统的状态和推进器故障参数的初始值，组成扩展的初始状态的矩阵，其初始条件为：Read the state of the underwater robot system and the initial value of the thruster failure parameters to form a matrix of the extended initial state, and its initial condition is:

${\overset{&OverBar; &OverBar;}{x x}}_{00} = = E E. [[{x x}_{00}]]$

${P P}_{00} = = E E. [[(({x x}_{00} - - {\overset{&OverBar; &OverBar;}{x x}}_{00})) {(({x x}_{00} - - {\overset{&OverBar; &OverBar;}{x x}}_{00}))}^{T T}]]$

扩展的状态矩阵s₀表示大地坐标系下的六维水下机器人的初始位姿信息，表示六维水下机器人载体坐标系下的速度和角速度信息，b₀表示六个自由度推进器的故障信息。同时还要设置初始系统过程噪声w₀和测量噪声v₀，D^w和D^v分别为过程噪声和测量噪声协方差矩阵。Extended state matrix s ₀ represents the initial pose information of the six-dimensional underwater robot in the earth coordinate system, Indicates the velocity and angular velocity information in the six-dimensional underwater vehicle carrier coordinate system, and b ₀ indicates the fault information of the six-degree-of-freedom thruster. At the same time, the initial system process noise w ₀ and measurement noise v ₀ should be set, and D ^w and D ^v are the process noise and measurement noise covariance matrices, respectively.

通过滤波初始化得到水下机器人扩展状态和初始均值和方差P₀。Obtain the extended state and initial mean value of the underwater robot by filtering initialization and variance P ₀ .

随着滤波算法的进行，这里要得到上一时刻k-1可估计滤波的均值和方差P_k-1；同时要得到上一时刻k-1的系统噪声w_k-1和测量噪声v_k-1，系统噪声w_k-1和测量噪声v_k-1是通过辅助滤波器估计得到的系统噪声方差阵D的对角线元素有了水下机器人扩展状态的均值和方差P_k-1，通过无色变换构造一组关于对称且位于其附近的离散Sigma点，如式所示：As the filtering algorithm proceeds, here we need to get the mean value of k-1 estimable filtering at the last moment and variance P _k-1 ; at the same time, to obtain the system noise w _k-1 and measurement noise v _k-1 of k-1 at the last moment, the system noise w _k-1 and measurement noise v _k-1 are estimated by the auxiliary filter The diagonal elements of the resulting system noise variance matrix D With the mean value of the AUV extended state and variance P _k-1 , construct a set of about Discrete Sigma points that are symmetrical and located near it, as shown in the formula:

${χ χ}_{k k - - 11} = = [[{\overset{&OverBar; &OverBar;}{x x}}_{k k - - 11},, {\overset{&OverBar; &OverBar;}{x x}}_{k k - - 11} - - \sqrt{((l l + + λ λ)) {P P}_{k k - - 11}},, {\overset{&OverBar; &OverBar;}{x x}}_{k k - - 11} + + \sqrt{((l l + + λ λ)) {P P}_{k k - - 11}}]]$

其中，l为系统状态和推进器故障参数组成的扩展状态的维数，λ为控制Sigma点到均值距离的尺度常数。where l is the dimension of the extended state composed of the system state and thruster fault parameters, and λ is the scaling constant controlling the distance from the Sigma point to the mean.

离散Sigma点经系统方程计算更新Sigma点，由于要对水下机器人的状态和推进器的故障参数进行联合估计，所以水下机器人的系统方程中必须包括推进器的故障模型。即扩展参考模型的系统方程公式所示：The discrete Sigma points are updated by the system equation calculation. Since the state of the underwater robot and the failure parameters of the thruster are to be jointly estimated, the system equation of the underwater robot must include the failure model of the thruster. That is, the system equation formula of the extended reference model is shown as:

$M m \overset{\cdot &Center Dot; \cdot &Center Dot;}{s the s} + + C C ((\overset{\cdot &Center Dot;}{s the s})) \overset{\cdot &Center Dot;}{s the s} + + D D. ((\overset{\cdot &Center Dot;}{s the s})) \overset{\cdot &Center Dot;}{s the s} + + G G ((s the s)) = = F f ((u u))$

滤波进行中形成的扩展的系统方程公式：The extended system of equations formula formed during the filtering process:

经过上述系统方程计算更新后的Sigma点公式为：The updated Sigma point formula calculated by the above system equation is:

${χ χ}_{k k | | k k - - 11}^{* *} = = f f (({χ χ}_{k k - - 11},, {u u}_{k k}))$

然后计算更新后Sigma点的均值和方差公式为：Then calculate the mean and variance formula of the updated Sigma point as:

${\overset{&OverBar; &OverBar;}{x x}}_{k k | | k k - - 11} = = {Σ Σ}_{i i = = 00}^{22 l l} {d d}_{i i}^{m m} {χ χ}_{i i,, k k | | k k - - 11}^{* *}$

${P P}_{k k | | k k - - 11} = = {Σ Σ}_{i i = = 00}^{22 l l} {d d}_{i i}^{c c} (({χ χ}_{i i,, k k | | k k - - 11}^{* *} - - {\overset{&OverBar; &OverBar;}{x x}}_{k k | | k k - - 11})) {(({χ χ}_{i i,, k k | | k k - - 11}^{* *} - - {\overset{&OverBar; &OverBar;}{x x}}_{k k | | k k - - 11}))}^{T T} {+ + Q Q}^{w w}$

计算系统方程计算更新的扩展状态均值和方差的Sigma点，得到一步估计的状态均值和方差：Calculate the Sigma points of the updated extended state mean and variance to obtain the one-step estimated state mean and variance by computing the system equations:

${χ χ}_{k k | | k k - - 11} = = [[{\overset{&OverBar; &OverBar;}{x x}}_{k k | | k k - - 11},, {\overset{&OverBar; &OverBar;}{x x}}_{k k | | k k - - 11} - - \sqrt{((l l + + λ λ)) {P P}_{k k | | k k - - 11}},, {\overset{&OverBar; &OverBar;}{x x}}_{k k | | k k - - 11} + + \sqrt{((l l + + λ λ)) {P P}_{k k | | k k - - 11}}]]$

测量方程计算更新Sigma点：The measurement equation calculates the updated Sigma point:

对一步估计的状态均值和方差再进行无色变换和经测量方程计算更新后，得到水下机器人可观测位姿信息：After the colorless transformation is performed on the state mean and variance estimated in one step, and the measurement equation is calculated and updated, the observable pose information of the underwater robot is obtained:

由于水下机器人扩展状态中仅前六维的位姿信息可观测，所以对上步生成Sigma点的前六维进行计算更新。测量方程如下公式所示：Since only the pose information of the first six dimensions is observable in the extended state of the underwater robot, the first six dimensions of the Sigma points generated in the previous step are calculated and updated. The measurement equation is shown in the following formula:

y_k＝x_k(1:6)+v_k y _k ＝x _k (1:6)+v _k

其中x_k(1:6)表示扩展状态的前六维的位姿信息，v_k为不相关的白噪声；经过测量方程计算更新后的Sigma点公式为：Among them, x _k (1:6) represents the pose information of the first six dimensions of the extended state, and v _k is irrelevant white noise; the updated Sigma point formula after calculation of the measurement equation is:

γ_k|k-1＝h(χ_k|k-1)γ _k|k-1 ＝h(χ _k|k-1 )

计算得到测量方程计算更新的可观测状态均值和方差的Sigma点公式为：Calculate the Sigma point formula of the measurement equation to calculate the updated observable state mean and variance:

${\overset{&OverBar; &OverBar;}{y the y}}_{k k | | k k - - 11} = = {Σ Σ}_{i i = = 00}^{22 l l} {d d}_{i i}^{m m} {γ γ}_{i i,, k k | | k k - - 11}$

${P P}_{{\overset{&OverBar; &OverBar;}{y the y}}_{k k} {\overset{&OverBar; &OverBar;}{y the y}}_{k k}} = = {Σ Σ}_{i i = = 00}^{22 l l} {d d}_{i i}^{c c} (({γ γ}_{i i,, k k | | k k - - 11} {- - \overset{&OverBar; &OverBar;}{y the y}}_{k k | | k k - - 11})) {(({γ γ}_{i i,, k k | | k k - - 11} - - {\overset{&OverBar; &OverBar;}{y the y}}_{k k | | k k - - 11}))}^{T T} + + {Q Q}^{v v}$

${P P}_{{\overset{&OverBar; &OverBar;}{x x}}_{k k} {\overset{&OverBar; &OverBar;}{y the y}}_{k k}} = = {Σ Σ}_{i i = = 00}^{22 l l} {d d}_{i i}^{c c} (({χ χ}_{i i,, k k | | k k - - 11} {- - \overset{&OverBar; &OverBar;}{x x}}_{k k | | k k - - 11})) {(({γ γ}_{i i,, k k | | k k - - 11} - - {\overset{&OverBar; &OverBar;}{y the y}}_{k k | | k k - - 11}))}^{T T}$

计算UKF滤波器的增益矩阵：Compute the gain matrix of the UKF filter:

${K K}_{k k} = = {P P}_{{\overset{&OverBar; &OverBar;}{x x}}_{k k} {\overset{&OverBar; &OverBar;}{y the y}}_{k k}} {P P}_{{\overset{&OverBar; &OverBar;}{y the y}}_{k k} {\overset{&OverBar; &OverBar;}{y the y}}_{k k}}^{- - 11}$

估计系统扩展状态的均值和方差：Estimate the mean and variance of the extended state of the system:

读取经过数据处理后的传感器观测位姿信息y_k，以此为基础估计水下机器人扩展状态的均值和方差，如式所示：Read the sensor observation pose information y _k after data processing, and estimate the mean value and variance of the extended state of the underwater robot based on this, as shown in the formula:

${P P}_{k k} = = {P P}_{k k | | k k - - 11} - - {K K}_{k k} {P P}_{{\overset{&OverBar; &OverBar;}{y the y}}_{k k} {\overset{&OverBar; &OverBar;}{y the y}}_{k k}} {K K}_{k k}^{T T}$

${\overset{&OverBar; &OverBar;}{x x}}_{k k} = = {\overset{&OverBar; &OverBar;}{x x}}_{k k | | k k - - 11} + + {K K}_{k k} (({y the y}_{k k} - - {\overset{&OverBar; &OverBar;}{y the y}}_{k k | | k k - - 11}))$

在下一个时刻，即k+1时刻，如果有新的测量值，接续辅助滤波器程序步骤。At the next moment, that is, moment k+1, if there is a new measurement value, continue with the auxiliary filter program steps.

由水下机器人系统噪声方差矩阵D对角线元素可将辅助滤波器的状态方程表示为：Diagonal elements of the noise variance matrix D of the underwater vehicle system The state equation of the auxiliary filter can be expressed as:

其中为辅助滤波器中的系统噪声，是零均值高斯白噪声为零均值高斯白噪声；in is the system noise in the auxiliary filter, which is zero-mean Gaussian white noise is zero-mean Gaussian white noise;

辅助滤波器是以主滤波器的新息ξ_k方差矩阵的对角线元素作为系统的观测信号，依据主滤波器扩展状态的一步估计的方差P_k|k-1，主滤波器扩展状态的方差P_k，主滤波器UKF的增益矩阵K_k组成辅助滤波器的观测方程为：The auxiliary filter is the diagonal element of the innovation ξ _k variance matrix of the main filter As the observation signal of the system, according to the variance P _k|k-1 of the one-step estimation of the extended state of the main filter, the variance P _k of the extended state of the main filter, and the gain matrix K _k of the main filter UKF form the observation equation of the auxiliary filter for:

辅助滤波器KF对系统的过程噪声进行滤波估计的实现过程：The realization process of the auxiliary filter KF filtering and estimating the process noise of the system:

上一时刻估计得到辅助滤波器的系统噪声方差阵的对角线元素和其方差用辅助滤波器的状态方程对和计算更新后得到对角线元素和其方差的一步估计值；用辅助滤波器的测量方程依主滤波器的一步估计的方差P_k|k-1、扩展状态方差P_k和增益矩阵K_k经计算更新后得到估计的观测值所得到的一步估计值和观测值同主滤波器的测量值经计算估计得到系统过程噪声方差阵D的对角线元素的估算值和其方差当有新观测位姿信息y_k时，接续下一控制周期主滤波器程序步骤。The diagonal elements of the system noise variance matrix of the auxiliary filter estimated at the previous moment and its variance Using the state equation of the auxiliary filter for and Calculate the diagonal elements after updating and its variance One-step estimated value of ; use the measurement equation of the auxiliary filter to calculate and update the estimated observed value according to the one-step estimated variance P _k|k-1 of the main filter, the extended state variance P _k and the gain matrix K _k The resulting one-step estimates and observations The measured value of the same main filter is calculated and estimated to obtain the estimated value of the diagonal element of the system process noise variance matrix D and its variance When there is new observed pose information y _k , the main filter program step of the next control cycle is continued.

参见附图2，See attached drawing 2,

自适应UKF辅助滤波器KF的实现流程(以估计系统的过程噪声为例)：The implementation process of the adaptive UKF auxiliary filter KF (taking the process noise of the estimated system as an example):

滤波初始化：Filter initialization:

读取水下机器人系统过程噪声方差矩阵对角线元素的初始值极其方差，初始条件为：Read the initial values and variances of the diagonal elements of the process noise variance matrix of the underwater robot system, and the initial conditions are:

${\overset{&OverBar; &OverBar;}{q q}}_{00} = = E E. [[{q q}_{00}]]$

${P P}_{{q q}_{00}} = = E E. [[(({q q}_{00} - - {\overset{&OverBar; &OverBar;}{q q}}_{00})) {(({q q}_{00} - - {\overset{&OverBar; &OverBar;}{q q}}_{00}))}^{T T}]]$

通过滤波初始化得到水下机器人系统过程噪声方差矩阵对角线元素的初始值及其方差随着滤波算法的进行，这里要得到上一时刻k-1可估计滤波的和方差 Obtain the initial value of the diagonal elements of the process noise variance matrix of the underwater robot system by filtering initialization and its variance As the filtering algorithm proceeds, it is necessary to obtain the k-1 estimable filtering at the last moment and variance

由水下机器人系统过程噪声方差矩阵对角线元素可将辅助滤波器的状态方程表示为：The diagonal elements of the variance matrix of the AUV system process noise The state equation of the auxiliary filter can be expressed as:

其中为辅助滤波器中的系统噪声，表示零均值高斯白噪声；in is the system noise in the auxiliary filter, representing zero-mean Gaussian white noise;

由于的变化规律未知，可将其视为噪声驱动的不相关的随机漂移向量，所以上时刻估计的水下机器人系统的噪声方差矩阵D对角线元素及其方差经过辅助滤波器状态方程计算更新的公式为：because The change law of is unknown, and it can be regarded as an uncorrelated random drift vector driven by noise, so the diagonal element of the noise variance matrix D of the underwater robot system estimated at the last moment and its variance The updated formula calculated by the auxiliary filter state equation is:

${\overset{&OverBar; &OverBar;}{q q}}_{k k | | k k - - 11} = = {\overset{&OverBar; &OverBar;}{q q}}_{k k - - 11} + + {w w}_{{q q}_{k k}}$

${P P}_{{q q}_{k k | | k k - - 11}} = = {P P}_{{q q}_{k k - - 11}} + + {Q Q}_{q q}^{w w};;$

其中，是辅助滤波器中的噪声方差矩阵，通过估计主滤波器的D^w而设置。in, is the noise variance matrix in the auxiliary filter, via It is set by estimating the ^Dw of the main filter.

辅助滤波器的测量方程为：auxiliary filter The measurement equation is:

其中vdiag表示取中括号内矩阵的主对角线元素组成的一个向量，设可以得到：Where vdiag represents a vector composed of the main diagonal elements of the matrix in square brackets, set can get:

$\overset{&OverBar; &OverBar;}{S S} {q q}_{k k} = = vdiag vdiag [[{KP KP}_{k k} (({Σ Σ}_{i i = = 00}^{22 n no} {d d}_{i i}^{c c} (({χ χ}_{i i,, k k | | k k - - 11}^{* *} - - {\overset{&OverBar; &OverBar;}{x x}}_{k k | | k k - - 11})) {(({χ χ}_{i i,, k k | | k k - - 11}^{* *} - - {\overset{&OverBar; &OverBar;}{x x}}_{k k | | k k - - 11}))}^{T T} + + {Q Q}^{w w} {- - P P}_{k k}))]]$

$= = vdiag vdiag [[{KP KP}_{k k} (({Σ Σ}_{i i = = 00}^{22 n no} {d d}_{i i}^{c c} (({χ χ}_{i i,, k k | | k k - - 11}^{* *} - - {\overset{&OverBar; &OverBar;}{x x}}_{k k | | k k - - 11})) {(({χ χ}_{i i,, k k | | k k - - 11}^{* *} - - {\overset{&OverBar; &OverBar;}{x x}}_{k k | | k k - - 11}))}^{T T} - - {P P}_{k k}))]] + + vdiag vdiag (({KP KP}_{k k} {Q Q}^{w w} {KP KP}_{k k}^{T T}))$

$= = {Bp bp}_{k k} + + vdiag vdiag (({KP KP}_{k k} {Q Q}^{w w} {KP KP}_{k k}^{T T}))$

$= = {Bp bp}_{k k} + + {HP HP}_{k k} \cdot \cdot {q q}_{k k}$

其中，Bp_k为常值向量；HP_k为常值矩阵。若KP_k∈R^mq×nq，且，Among them, Bp _k is a constant value vector; HP _k is a constant value matrix. If KP _k ∈ ^{R mq×nq} , and,

由于Q^w为对角矩阵，则可得HP_k∈R^mq×nq，且有Since Q ^w is a diagonal matrix, HP _k ∈ ^{R mq×nq} can be obtained, and

计算辅助滤波器的增益矩阵：Compute the gain matrix of the auxiliary filter:

${K K}_{{q q}_{k k}} = = {P P}_{{q q}_{k k | | k k - - 11}} \cdot \cdot {HP HP}_{k k}^{T T} {(({HP HP}_{k k} \cdot \cdot {P P}_{{q q}_{k k | | k k - - 11}} \cdot \cdot {HP HP}_{k k}^{T T} + + {Q Q}_{q q}^{v v}))}^{- - 11}$

根据主滤波器的实时新息信息ξ_k，计算辅助滤波器的测量值：According to the real-time innovation information ξ _k of the main filter, calculate the measurement value of the auxiliary filter:

${Sq Sq}_{k k} = = g g (({q q}_{k k})) = = vdiag vdiag ((\frac{11}{N N} {Σ Σ}_{i i = = k k - - N N + + 11}^{k k} {ξ ξ}_{k k} {ξ ξ}_{k k}^{T T}))$

其中，y_k为主滤波器k时刻的观测位姿信息，为可观测状态均值。in, y _k is the observed pose information at time k of the main filter, is the mean value of the observable states.

估计k时刻水下机器人系统的过程噪声方差矩阵对角线元素和方差：Estimate the process noise variance matrix diagonal elements and variances of the AUV system at time k:

读取辅助滤波器的测量值Sq_k，以此为基础估计水下机器人系统的过程噪声方差矩阵对角线元素和方差如式所示：Read the measured value Sq _k of the auxiliary filter, based on which the diagonal elements of the process noise variance matrix of the underwater vehicle system are estimated and variance As shown in the formula:

${P P}_{{q q}_{k k}} = = ((I I - - {K K}_{{q q}_{k k}} \cdot \cdot {HP HP}_{k k})) {P P}_{{q q}_{k k | | k k - - 11}}$

${\overset{&OverBar; &OverBar;}{q q}}_{k k} = = {\overset{&OverBar; &OverBar;}{q q}}_{k k | | k k - - 11} + + {K K}_{{q q}_{k k}} (({Sq Sq}_{k k} - - {\overset{&OverBar; &OverBar;}{S S} q q}_{k k | | k k - - 11}))$

在下一个时刻，如果有新息信息ξ_k+1，重复上述辅助滤波器的步骤；当读取新观测值位姿信息时，接续下一控制周期程序步骤。At the next moment, if there is new information ξ _k+1 , repeat the above steps of the auxiliary filter; when reading the pose information of the new observation value, continue the program steps of the next control cycle.

Claims

1. a method for joint estimation of underwater robot state and parameters based on self-adaptive UKF, is characterized in that comprising the following steps:

On the basis of the dynamic model of the underwater robot, the propeller fault is modeled to obtain an offline extended reference model; according to the online pose information y _k detected by the pose sensor, the main filter of the adaptive UKF is used as the standard UKF The filter estimation algorithm, combined with the offline extended reference model of the underwater robot, is the extended system state equation, which transmits and updates the extended state composed of the system state and thruster faults, estimates the state and parameter information of the system in real time, and obtains the estimated value of the system. result mean and variance P _k ; here the state information of the system includes filtered pose information _sk and real-time estimated speed information The parameter information is the fault parameter b _k of the propeller of the system. At the same time, the measurement value Sq _k of the auxiliary filter is calculated according to the new information ξ _k calculated and estimated by the main filter; the state equation and measurement equation of the UKF auxiliary filter are established, according to The measured value Sq _k is used as the observation signal of the system, and the auxiliary filter KF of the adaptive UKF is used to estimate the noise information of the underwater vehicle system in real time, and the diagonal elements of the system noise covariance matrix D are obtained and variance The estimated value of ; the noise information here includes process noise w _k and measurement noise v _k , and their covariance matrices are D ^w and D ^v respectively;

Formation process of main filter extended system equation:

When expanding the nonlinear system, the state space equation of the system with unknown/time-varying thruster fault parameter _bk is:

x _k+1 ＝f(x _k ,b _k ,u _k )+w _k

b _k+1 ＝f _b (x _k ,b _k ,u _k )+w _kb

where b _k is the unknown/time-varying thruster fault parameter at time k, and w _kb is the system thruster fault noise vector of the fault parameter model, which is Gaussian noise with zero mean value;

When the change law of the fault parameter b _k is unknown, b _k is regarded as an irrelevant random drift vector, and its recursive expression is:

b _k ＝b _k-1 +w _{b k}

Based on the joint estimation of adaptive UKF, the fault parameters of the propellers of the system are added to the real state of the system to form an extended state matrix, namely Composing the system equations into an extended system state equation is:

{x x}_{k k + + 11}^{a a} = = \overset{~ ~}{f f} (({x x}_{k k}^{a a},, {u u}_{k k})) + + {w w}_{k k}^{a a}

In the formula, is the extended system process noise vector, where is the system process noise vector, To estimate the thruster fault noise vector.

2. a kind of underwater vehicle state and parameter joint estimation method based on self-adaptive UKF according to claim 1, is characterized in that: in the extended reference model, the fault parameter b _k Expressed as the efficiency loss factor of the i-th propeller at time k, the value between 0 and 1 represents the degree of propeller failure.

3. a kind of underwater robot state and parameter joint estimation method based on adaptive UKF according to claim 1, is characterized in that: the construction of auxiliary filter state equation in adaptive UKF algorithm:

The diagonal elements of the noise variance matrix of the underwater vehicle system The state equation of the auxiliary filter can be expressed as:

{\overset{&OverBar; &OverBar;}{q q}}_{k k} = = {f f}_{q q} (({\overset{&OverBar; &OverBar;}{q q}}_{k k - - 11})) + + {w w}_{{q q}_{k k}}

in is the system noise vector in the auxiliary filter, which is zero-mean Gaussian white noise;

because The change law of is unknown, and it is regarded as an uncorrelated random drift vector driven by noise, so the state equation of the auxiliary filter can be obtained:

{\overset{&OverBar; &OverBar;}{q q}}_{k k} = = {\overset{&OverBar; &OverBar;}{q q}}_{k k - - 11} + + {w w}_{{q q}_{k k}} . .

4. a kind of underwater robot state and parameter joint estimation method based on adaptive UKF according to claim 1 is characterized in that: the construction of auxiliary filter measurement equation in adaptive UKF algorithm:

Taking the measured value Sq _k of the variance matrix of the innovation ξ _k of the main filter as the observed signal of the system, according to the variance P _k|k-1 of the one-step estimation of the extended state of the main filter, the variance P _k of the extended state of the main filter, The measurement equation of the auxiliary filter composed of the gain matrix K _k of the main filter UKF is:

{\overset{&OverBar; &OverBar;}{S S}}_{{q q}_{k k}} = = g g (({\overset{&OverBar; &OverBar;}{q q}}_{k k})) = = vdiag vdiag [[{(({K K}_{k k}^{T T} {K K}_{k k}))}^{- - 11} {K K}_{k k}^{T T} (({P P}_{k k | | k k - - 11} - - {P P}_{k k})) {K K}_{k k} {(({K K}_{k k}^{T T} {K K}_{k k}))}^{- - 11}]]

Where vdiag is a vector composed of the main diagonal elements of the matrix in square brackets.

5. a kind of underwater robot state and parameter joint estimation method based on self-adaptive UKF according to claim 1, is characterized in that: main filter is to the realization process of the extended state equation filter estimation:

The mean value of the estimator of the random vector at the previous moment A set of discrete Sigma points χ _k-1 obtained after the sum variance P _k-1 is subjected to nonlinear colorless transformation, the updated Sigma points are calculated by the system state equation, and then the extended state mean value of the updated Sigma points is calculated and variance P _k|k-1 , for the extended state mean Calculate the state mean and variance of one-step estimation with the variance P _k|k-1 ; the state mean and variance of the one-step estimation are then updated by colorless transformation and calculation with the measurement equation to obtain the mean value of the observable state of the underwater robot variance and covariance The estimated value of ; read the sensor to collect the observable pose information y _k and the mean value of the extended state The sum variance P _k|k-1 and the estimated value of the observable state and the gain matrix K _k are calculated and estimated to obtain the system mean The estimated value of the variance P _k is obtained from it to obtain the innovation ξ _k ; the measured value Sq _k is calculated according to the innovation ξ _k of the main filter as the observation signal of the auxiliary filter, and then the auxiliary filter program steps are continued.

6. according to claim 1 or 5, a kind of underwater robot state and parameter joint estimation method based on self-adaptive UKF, it is characterized in that: auxiliary filter KF carries out the realization process of filter estimation to system process noise:

The diagonal elements of the system noise variance matrix of the auxiliary filter estimated at the previous moment and variance Using the state equation of the auxiliary filter for and Calculate the diagonal elements after updating and variance One-step estimated value of ; use the measurement equation of the auxiliary filter to calculate and update the estimated observed value according to the one-step estimated variance P _k|k-1 of the main filter, the extended state variance P _k and the gain matrix K _k The resulting one-step estimates and observations The measured value Sq _k of the same main filter is calculated and estimated to obtain the estimated value of the diagonal elements of the system process noise variance matrix D and variance When there is new observed pose information y _k , the main filter program step of the next control cycle is continued.