基于UKF的水下机器人执行器故障检测方法研究

168

文章编号：1001-3997（2011）05-0168-03

机械设计与制造

MachineryDesign＆Manufacture

第5期2011年5月

基于UKF的水下机器人执行器故障检测方法研究*

22

林昌龙1，封锡盛1李一平1，

（1中国科学院沈阳自动化研究所机器人学国家重点实验室，沈阳110016）

（2中国科学院研究生院，北京100049）

Researchonactuatorfaultdetectionofunmannedunderwatervehiclebasedonunscentedkalmanfilter

22

LINChang-long1，，FENGXi-sheng1，LIYi-ping1，

（1ShenyangInstituteofAutomation，ChineseAcademyofScience，StateKeyLaboratoryofRobotics，

Shenyang110016，China）

Beijing100049，China）（2GraduateSchooloftheChineseAcademyofScience，

【摘要】水下机器人需要在无人干预或者少量干预的情况下自主地完成使命，这就要求它在作业

过程中能够自主地检测子系统、传感器和执行器的故障。在确认发生故障的时候，它还需要尽最大可能水下机器人常用的执行器包括推进器、舵、机械手等，针对推进器地进行修复，以确保使命的顺利进行。的故障检测问题，首先建立了水下机器人的运动学和动力学模型，然后采用UKF对系统的状态和参数进行联合估计，最后通过外场试验验证了算法的有效性和正确性。

关键词：故障检测；水下机器人；无色卡尔曼滤波器

【Abstract】Unmannedunderwatervehicleneedstocompletethemissionwithlittleorevenwithoutingthemission.Furthermore，whenidentifyinganyfault,itshouldtrytofixtheproblemtoresumethemis－sion.Generally，theactuatorsforunmannedunderwatervehiclesconsistofthrusters，rudders，manipulatorsthekinematicalandkineticmodelsofanun－andsoon.Concerningtheproblemofactuatorfaultsdetecting，

mannedunderwatervehiclearefirstlyconstructedit.ThenusinganUnscentedKalmanFilter，thestatusandparametersofthesystemareestimated.Finally，fieldexperimentiscarriedoutandtheresultvalidatestheeffectivenessandcorrectnessofthisalgorithm.

Keywords：Faultdetection；Unmannedunderwatervehicle；Unscentedkalmenfilter

文献标识码：A

中图分类号：TH16，TP242.3

itshouldbeabletodetectthefaultsofitssubsystems,sensors，andactuatorsdur－humanintervention.Thus，

1引言

水下机器人（UnmannedUnderwaterVehicles，UUV）是一种

环节，同时也找出了保守设计的部分，为优化工作奠定了基础。（2）用patran/nastran有限元分析软件的优化功能模块对机器人进行优化简单明了，能有效的提高系统的整体性能，节省了人力和财力。

1.19-0021.11-0021.03-0029.53-0038.74-0037.94-0037.15-0036.35-0035.56-003

＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊

表2综合方案

基臂

4

机构板厚（mm）

连杆1

8

上连接板

5

连杆23

＊来稿日期：2010-07-21＊基金项目：国家自然科学基金（60775061），国家基础研究项目（6138102005-4，6138101004-3）

厚度（mm）σmax（MPa）

无人、自治、智能的水下运动平台，自带能源、传感器和执行机构，可以顺利地完成调查、采样、投送等任务，弥补了许多传统水下作

参考文献

［1］薄少军.一种机器人大臂机构的动力学研究［J］.机械设计与制造，2011（3）.［2］夏利娟，等.工程结构的优化设计方法与应用［J］.上海交通大学学报，2002，36

（11）:1572-1575.

［3］王有智.CAE的过去、现在与未来［J］.计算机辅助设计与制造，1998，12

（8）:30-33.

［4］JacobssonH.Aspectsofbrakejudder.Proc.InstnMech.EngrsVol.217Part

D：J.AutomobileEngineering，2003：419-430.

［5］EgglestonD.Thermaljudder.Euractechnicalbulletin0034056，2003.

9

160

［6］WernerOesterle，IngridUrbanThirdbodyformationonbrakepadsand

2006：401-408.rotors.TribologyInternational，

119-0024.77-003

3.97-0033.18-003

图3优化后的变形图

表1连杆1最大应力值随板厚的变化

6

272

7245

8203

基于UKF的水下机器人执行器故障检测方法研究

第5期林昌龙等：基于UKF的水下机器人执行器故障检测方法研究

1ρL2q′2′2′2q（P-B）sinθXuuu+Xvvv+Xwww+Tx-4

169

（3）

业手段的不足，被广泛地应用于海洋资源开发、环境监测、军事等UUV技术逐步发展成领域。经过近几十年的技术积累和进步，

熟，已经在海洋开发、军事活动等领域发挥了重要作用，并显示出不可替代的优势[1]。

传感器和执行器的实时监控和故障自主诊断是水下机器人智能化的重要组成部分。UUV在作业过程中需要对自身健康状况进行评估，将评估结果作为规划决策的依据，保证UUV在出现异常状况时，能够自主识别和判断异常状况，重新规划使命任务与调度潜水器资源，最大限度地完成使命。根据程度的大小可以将故障分为两类：硬故障和软故障[2]。硬故障是指由于结构性损伤变化所造成的不可恢复性故障，即完全丧失能力，一般幅值较大、突然。软故障是指由于元件老化或者受扰动等引起的特性参数变化。例如推进器效率的下降、磁通门罗盘因为磁场扭曲导致测量值的不准确等都属于软故障。硬故障由于具有变化突然、幅值较相比之下，软故障则需要对系统的状大的特点，一般较容易检测。态做出良好的实时估计。

一般的，可以将故障诊断的方法分为依赖于模型和不依赖于

5]

采用的神经网络方法属于前者，某科研模型两类[3]。某科研工作者[4，

7]

工作者[6，采用的扩展卡尔曼滤波器（ExtendedKalmanFilter，EKF）

′

觶+Y′P觶+Y′PP+Y′pq+Y′qr+觶-wp+urmq=1ρLYr觶rv觶pppqqrp′′1ρL3′觶′

Yv觶v+Yvqvq+Ywpwp+Ywrwr+2

′′1ρL3′

Yur+Yup+Yvrrpqq

qq

qv+rq

2

2

1

2

q

q

vr

+q

1

1ρL2′2′′22+

Y0u+Yvuv+Yvvvqv+w2

q

（4）

1ρL2Y′vw+T+P-B）cosθsin准vwy（′

觶′2′2′觶-uq+vpmq=1ρLZq+w觶q+Zppp+Zrrr+Zrprp4

′1ρL3′觶′

+Zw觶w+Zvrvr+Zvpvp′1ρL3′

Zuq+Zwqqqq

qq

q

q

qw

qv+w2

2

1

+q

1

1ρL2′2′′22+

Z*u+Zwuw+Zwwwqv+wq（5）

1

1ρL2Z′v2+1ρL2Z′uw+Z′

22+wwwvvwqv+w和滑膜观测器等方法属于后者。采用基于模型的方法来检测UUV执行器的故障，并用无色卡尔曼滤波器（UnscentedKalmanFilter，UKF）替代EKF以减小线性化误差。

Tz+（P-B）cosθcos准

5

觶+q觶+KP觶Iz-IyqIxpqr=1ρLKr觶r+觶+KppPP+Kpqpq+Kqrqrp′

′

′

′

′

′′′1ρL4′觶′14′

+Kvpvp+Kwpwp+KwrwrqKv觶v+Kpuq+Krur+ρLq221

1ρL3′2′′222+

K*u+Kvuv+Kvvvqv+wqqq

2系统模型的建立

首先建立两个坐标系。大地坐标系以载体的初始位置为原正东和地心；载体坐标系以载体的点，三个坐标轴分别指向正北、正右方和正下重心为原点，三个坐标轴分别指向载体的正前方、方。令：x，y，z—大地坐标系下的坐标；准，θ，φ—载体坐标系相对于u，v，w—载体坐标系下的速大地坐标系的横滚、纵倾和偏航角；

度；p，q，r—载体坐标系下的横滚、纵倾和偏航角速度；nt，nb，αh，αv—载体的主推转速、垂直推进器转速、水平舵角和垂直舵角；P，B，h—载体的重力、浮力和稳心高。于是可以得到：

（1）载体的世界坐标与载体速度的关系：

觶

觶觶觶觶觶觶觶觶觶觶觶

觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶

qq

q

（6）+q

1ρL3K′vw+M+Phcosθsin准

vwTx

2

′′

觶+q觶′2′2′Ix-IzqIyqrp=1ρLMq

觶q+MPPP+Mrrr+Mrprp+Mqqqq2

5

′1ρL4′觶′

+Mw觶w+Mvrvr+Mvpvpq

qqqqq

q

12

1ρL4′′

Mquq+Mwq

qv+wqq

2

2

+q

cosθcosφ-cosθsinφsin准sinθcosφ+cos准sinφ

sinθ

觶觶

x觶觶觶=y觶z

觶觶觶觶觶觶觶觶

觶

-sin准sinθsinφ+cos准cosφ-sin准cosθ-cos准sinθcosφ+sin准sinφ

cos准sinθsinφ+sin准cosφcos准cosθ

觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶

1

1ρL3′2′′222+

M*u+Mwuw+Mwwwqv+wq觶

觶觶觶觶觶觶觶觶

q

（7）

uvw

（1）

1ρL3′

Mwu

w+Mwwwwqv+w

2

5

′

2

MTy+Phsinθqq

1

′′′觶+q觶+N′P觶′Iy-IxqIzppq=1ρLNr觶r觶+Nrrrr+Npqpq+Nqrqr+p′′1ρL4′觶′

Nv觶v+Nvqvq+Nwpwp+Nwrwr+′′1ρL4′

Nur+Nup+Nvrrpqq

）载体姿态角与载体坐标系角速度的关系：（2

觶

觶觶觶觶觶觶觶觶觶觶觶觶

q

qv+wq

2

2

1

觶觶准觶觶1sin准tanθcos准tanθ觶觶觶觶=觶0cos准-sin准觶觶θ觶觶

觶觶觶觶觶0sin准cosθcos准secθ觶觶觶φ觶

4

′

2

觶

觶

觶

觶觶

觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶觶

Pqr

觶觶觶觶觶觶觶觶觶

q

q

vr

+q

（2）

1

1ρL3′2′′22+

N*u+Nvuv+Nvvvqv+wqq

（8）

（3）载体的6自由度动力学方程：觶-vr+wqmq=1ρLq+Xqqq+Xrrr+Xrprpqu′2

′

′1ρL3′觶′

+Xu觶u+Xvrvr+Xwqwq1ρL3N′vw+M

vwTz

式中：（1）、（2）—载体的运动学方程；（3）~（8）—载体的动力学方程；

Tx，Ty，Tz，MTx，MTy，MTz—输入的力和力矩，它们是nt，nb，αh，αv的函数。

qq

基于UKF的水下机器人执行器故障检测方法研究

170机械设计与制造

L—增广状态向量的维数；式中：

γ—缩放比例参数；ωi—权重参数。

No.5

May.2011

3基于UKF的状态和参数估计

选取载体的位置、姿态、速度和角速度作为状态变量，主推和垂直推进器的健康因子kt，kv作为系统的参数，则系统的增广状态变量可以表示为：

X=xyz准θφuvwpqrktkv系统的输入为：ntnvαhαv姨U=姨

根据（1）~（8），可以把离散化的系统状态方程和观测方程简单地表示如下：

Xk，Uk，wk1Xk，vk1Xk+1=f1，Yk=h1

P。差矩阵分别为Q，

采用UKF算法对载体的状态和参数进行联合估计，算法流程如下[8]：

首先，对于非线性系统（9），增广状态向量和状态协方差矩阵：xk=姨=姨xkxkxk姨xkwkvk姨

x′

w′

v′

′

′

′

a

′

′

′

′

4实验结果和分析

在某型自治水下机器人某次外场试验中，主推进器在第220s的时候出现了故障，随后在第（228~244）s期间有几次间断性的恢复，在245s以后，主推进器则一直在故障状态。如图1所示，显示了对主推进器健康因子的估计曲线。

1.210.80.60.40.20

50

100

150200时间（s）

250

300

Kt

（9）

wk、vk—系统的过程噪声向量和观测噪声向量，它们的协方式中：

（10）

P=0Q0

00R

a

000000000

Px00

0000000000

图1Kt估计值曲线图

（11）

如图1所示，在前220s主推进器正常工作期间，其健康因误差主要是受到机器人运行区域水子的估计值一直在1.0附近，

流的影响。在主推进器发生故障后，健康因子的估值快速的下降。随后主推进器间断性的恢复，健康因子也随之回升。在245s后，

3.1初始化

赞0=xx赞0，0，0

a

000000000

a

1

′

′′

1

0000000000

′

（12）

主推进器一直处于故障状态，此时健康因子快速下降到0附近，由此可以判定主推进器出现了严重的故障。可见，健康因子准确地反映了主推进器的工作状态，从而也验证了算法的正确性。

Px00000R

P0=0Q0

（13）

5结论

首先建立了自治水下机器人的模型，包括运动学和动力学

a

3.2Sigma点的计算及时间更新

χk-1=x赞k-1x赞k-1+γ姨Pk-1x赞k-1-γ姨Pk-1a

姨

a

a

姨

（14）（15）（16）

然后采用UKF方法对系统的状态和参数进行联合估计。外方程。

场试验结果显示，这个方法能够准确的检测出执行器的故障。本方法简单易行，具有良好的工程实用性。

χk|k-1=f1χk-1，χk-1，uk-11

x

w

x

赞k=Σωiχi，xk|k-1

i=1

-

2L

（m）x

参考文献

［1］蒋新松，封锡盛，王棣棠．水下机器人［M］．沈阳：辽宁科技出版社，2000.［2］张铭钧，孙瑞琛，王玉甲．基于RBF神经网络的水下机器人传感器状

态检测方法研究［J］．哈尔滨工程大学学报，2005.

［3］Alessandri，A．，Hawkinson，T．，Healey，AJ．，andVeruggio，G．RobustModel-basedFaultDiagnosisforUnmannedUnderwaterVehiclesUsingSliding．ProceedingofInternationalSymposiumUnmannedMode-Observers［C］

UntetheredSubmersibleTechnology，1999.

［4］王玉甲，张铭钧．基于模糊神经网络的水下机器人实时状态监测模型［J］．

中国造船，2005（3）.

［5］王玉甲，张铭钧，金志贤．水下机器人传感器及推进器状态监测系统

．机械工程学报，2006（5）.［J］

［6］Alessandri，A．，Caccia，M．，andVeruggio，G．AModel-basedApproachtoFault

DiagnosisinUnmannedUnderwaterVehicles［C］．OCEANS，1998（10）.［7］Alessandri，A．，Caccia，M．，andVeruggio，G．FaultDetectionofActuatorFaults

inUnmannedUnderwaterVehicles［J］．ControlEngineeringPractice，1999（7）.［8］Merwe，RVD．Sigma-PointKalmanFiltersforProbabilisticInferenceinDynamic

State-SpaceModels［D］．OregonHealth&ScienceUniversity，2004.

Px=Σωi

k

-

2L

（c）

i=0

1χ

x

赞i，k|k-1-xk

-

11χ

x

赞i，k|k-1-xk

-

1

′

（17）

3.3测量更新

vk|k-1=h1χk-1，χk-1

x

n

x

1

（18）（19）

-

赞k=Σωivi，yk|k-1

i=0

-

2L

（m）x

Py赞=Σωi

k

2L

（c）

i=0

1v

x

赞i，k|k-1-yk

x

11v

-

赞i，k|k-1-yk

x

-

1

-

′

（20）（21）（22）

Pxy=Σωi

kk

2L

（c）

i=0

1χ

i，k|k-1

赞k-x

11v

i，k|k-1

赞k-y

1

′

Kk=Pxy·Py赞

kk

-1

k

赞k=x赞k+Kk赞赞xyk-ykPx=P-KkPy赞Kk

k

k

-

11

-′

（23）（24）

-xk