第9卷第2期 智能系统学报 Vol.9 No.2 2014年4月 CAAI Transactions on Intelligent Systems Apr.2014 D0I:10.3969/j.issn.1673-4785.201210058 网络出版地址：http://www.cnki.net/kcms/doi/CNKI:23-1538/TP.20131107.1716.002.html 基于四元数的欠驱动AUV的镇定控制设计 吴琪，李晔 (哈尔滨工程大学船舶工程学院，黑龙江哈尔滨150001) 摘要：镇定控制是指在控制输入作用于水下机器人，使水下机器人能够从任意的初始位置和姿态到达，并且稳定 在设计的位置和姿态，在深潜救生艇的水下对接、定位跟踪中应用广泛。通过对欠驱动AUV的六自由度空间运动方 程和轨迹方程简化，采用四元数方法进行了模型变换，提出了一种具有4个控制输人的连续时变镇定控制律，证明 了提出的控制律的收敛性，并且进行了镇定仿真实验演示。仿真结果表明，控制律是有效的，并且能够在任意初始 条件下实现镇定控制。 关键词：智能水下机器人：镇定控制：三维：轨迹：线性控制系统：时变系统：运动控制：仿真 中图分类号：TP24文献标志码：A文章编号：1673-4785(2014)02-0186-06 中文引用格式：吴琪，李晔.基于四元数的欠驱动AUV的镇定控制设计[J].智能系统学报，2014,9(2)：186-191. 英文引用格式：WUQi,LlYe.Stabilization design of underactuated AUV based on quaternion[J].CAAI Transactions on Intelli- gent Systems,.2014,9(2):186-191. Stabilization design of underactuated AUV based on quaternion WU Qi,LI Ye Clledge of Shipbuilding Engineering,Harbin Engineering University,Harbin 150001,China) Abstract:Stabilization control aims at stabilizing the AUV to the designed position and attitude from any initial po- sition under the controlling input.Stabilization is widely used in the abutment of deep-water submarines and location tracking.This paper describes a new time-variable stabilization law with four inputs which was designed based on the simplification of the 6-DOF (degree of freedom)space motion and trace equations of an underactuated AUV and the conversion of the model which was realized by quaternion.The control trial was done to prove that the con- trol law was convergent.The simulation results prove that the control method is effective and the stabilization can be achieved under any initial conditions. Keywords:autonomous underwater vehicles:stabilization:three dimensional:trajectories:linear control systems: time varying systems;motion control;simulation 智能水下机器人(autonomous underwater vehicle, 源采样、海底地形勘测、沉物打捞、地震地热活动监 AUV)作为船舶与海洋工程和机器人技术的学科交叉测、海洋环境监测等。在AUV进行作业任务的过程 领域，得到了世界各国的广泛重视。AUV在军事上 中，轨迹跟踪能力是必不可少的技术条件之一。尤其 可广泛应用于水域侦查、中继通讯、区域探雷、智能攻 是在执行海区搜索、海洋测绘等任务的时候，轨迹跟 击等：在民用方面，可用于海底生物资源探查、矿产资 踪的精度直接影响到区域覆盖和探测数据的完整 性-2)。文献[3]采用微分同胚变化进行了二维平面 收稿日期：2012-10-27.网络出版日期：2013-11-07. 的水面无人艇的慎定控制研究，引进扩大形式函数8 基金项目：国家自然科学基金资助项目(51279221,51179035)：黑龙江 省自然科学基金资助项目(E201121). 进行了二维镇定的证明，仿真结果表明可以实现无人 通信作者：吴琪.E-mail:wugi0609@163.com. 艇的镇定控制。本文基于文献[3]方法，针对欠驱动
第 ９ 卷第 ２ 期 智 能 系 统 学 报 Ｖｏｌ．９ №．２ ２０１４ 年 ４ 月 ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ Ａｐｒ． ２０１４ ＤＯＩ：１０．３９６９ ／ ｊ．ｉｓｓｎ．１６７３⁃４７８５．２０１２１００５８ 网络出版地址：ｈｔｔｐ： ／ ／ ｗｗｗ．ｃｎｋｉ．ｎｅｔ ／ ｋｃｍｓ／ ｄｏｉ ／ ＣＮＫＩ：２３⁃１５３８ ／ ＴＰ．２０１３１１０７．１７１６．００２．ｈｔｍｌ 基于四元数的欠驱动 ＡＵＶ 的镇定控制设计 吴琪， 李晔 （哈尔滨工程大学 船舶工程学院，黑龙江 哈尔滨 １５０００１） 摘 要：镇定控制是指在控制输入作用于水下机器人，使水下机器人能够从任意的初始位置和姿态到达，并且稳定 在设计的位置和姿态，在深潜救生艇的水下对接、定位跟踪中应用广泛。 通过对欠驱动 ＡＵＶ 的六自由度空间运动方 程和轨迹方程简化，采用四元数方法进行了模型变换，提出了一种具有 ４ 个控制输入的连续时变镇定控制律，证明 了提出的控制律的收敛性，并且进行了镇定仿真实验演示。 仿真结果表明，控制律是有效的，并且能够在任意初始 条件下实现镇定控制。 关键词：智能水下机器人；镇定控制；三维；轨迹；线性控制系统；时变系统；运动控制；仿真 中图分类号： ＴＰ２４ 文献标志码：Ａ 文章编号：１６７３⁃４７８５（２０１４）０２⁃０１８６⁃０６ 中文引用格式：吴琪， 李晔． 基于四元数的欠驱动 ＡＵＶ 的镇定控制设计［Ｊ］． 智能系统学报， ２０１４， ９（２）： １８６⁃１９１． 英文引用格式：ＷＵ Ｑｉ， ＬＩ Ｙｅ． Ｓｔａｂｉｌｉｚａｔｉｏｎ ｄｅｓｉｇｎ ｏｆ ｕｎｄｅｒａｃｔｕａｔｅｄ ＡＵＶ ｂａｓｅｄ ｏｎ ｑｕａｔｅｒｎｉｏｎ［ Ｊ］． ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉ⁃ ｇｅｎｔ Ｓｙｓｔｅｍｓ， ２０１４， ９（２）： １８６⁃１９１． Ｓｔａｂｉｌｉｚａｔｉｏｎ ｄｅｓｉｇｎ ｏｆ ｕｎｄｅｒａｃｔｕａｔｅｄ ＡＵＶ ｂａｓｅｄ ｏｎ ｑｕａｔｅｒｎｉｏｎ ＷＵ Ｑｉ， ＬＩ Ｙｅ （Ｃｌｌｅｄｇｅ ｏｆ Ｓｈｉｐｂｕｉｌｄｉｎｇ Ｅｎｇｉｎｅｅｒｉｎｇ，Ｈａｒｂｉｎ Ｅｎｇｉｎｅｅｒｉｎｇ Ｕｎｉｖｅｒｓｉｔｙ， Ｈａｒｂｉｎ １５０００１， Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ： Ｓｔａｂｉｌｉｚａｔｉｏｎ ｃｏｎｔｒｏｌ ａｉｍｓ ａｔ ｓｔａｂｉｌｉｚｉｎｇ ｔｈｅ ＡＵＶ ｔｏ ｔｈｅ ｄｅｓｉｇｎｅｄ ｐｏｓｉｔｉｏｎ ａｎｄ ａｔｔｉｔｕｄｅ ｆｒｏｍ ａｎｙ ｉｎｉｔｉａｌ ｐｏ⁃ ｓｉｔｉｏｎ ｕｎｄｅｒ ｔｈｅ ｃｏｎｔｒｏｌｌｉｎｇ ｉｎｐｕｔ． Ｓｔａｂｉｌｉｚａｔｉｏｎ ｉｓ ｗｉｄｅｌｙ ｕｓｅｄ ｉｎ ｔｈｅ ａｂｕｔｍｅｎｔ ｏｆ ｄｅｅｐ⁃ｗａｔｅｒ ｓｕｂｍａｒｉｎｅｓ ａｎｄ ｌｏｃａｔｉｏｎ ｔｒａｃｋｉｎｇ． Ｔｈｉｓ ｐａｐｅｒ ｄｅｓｃｒｉｂｅｓ ａ ｎｅｗ ｔｉｍｅ⁃ｖａｒｉａｂｌｅ ｓｔａｂｉｌｉｚａｔｉｏｎ ｌａｗ ｗｉｔｈ ｆｏｕｒ ｉｎｐｕｔｓ ｗｈｉｃｈ ｗａｓ ｄｅｓｉｇｎｅｄ ｂａｓｅｄ ｏｎ ｔｈｅ ｓｉｍｐｌｉｆｉｃａｔｉｏｎ ｏｆ ｔｈｅ ６⁃ＤＯＦ （ ｄｅｇｒｅｅ ｏｆ ｆｒｅｅｄｏｍ） ｓｐａｃｅ ｍｏｔｉｏｎ ａｎｄ ｔｒａｃｅ ｅｑｕａｔｉｏｎｓ ｏｆ ａｎ ｕｎｄｅｒａｃｔｕａｔｅｄ ＡＵＶ ａｎｄ ｔｈｅ ｃｏｎｖｅｒｓｉｏｎ ｏｆ ｔｈｅ ｍｏｄｅｌ ｗｈｉｃｈ ｗａｓ ｒｅａｌｉｚｅｄ ｂｙ ｑｕａｔｅｒｎｉｏｎ． Ｔｈｅ ｃｏｎｔｒｏｌ ｔｒｉａｌ ｗａｓ ｄｏｎｅ ｔｏ ｐｒｏｖｅ ｔｈａｔ ｔｈｅ ｃｏｎ⁃ ｔｒｏｌ ｌａｗ ｗａｓ ｃｏｎｖｅｒｇｅｎｔ． Ｔｈｅ ｓｉｍｕｌａｔｉｏｎ ｒｅｓｕｌｔｓ ｐｒｏｖｅ ｔｈａｔ ｔｈｅ ｃｏｎｔｒｏｌ ｍｅｔｈｏｄ ｉｓ ｅｆｆｅｃｔｉｖｅ ａｎｄ ｔｈｅ ｓｔａｂｉｌｉｚａｔｉｏｎ ｃａｎ ｂｅ ａｃｈｉｅｖｅｄ ｕｎｄｅｒ ａｎｙ ｉｎｉｔｉａｌ ｃｏｎｄｉｔｉｏｎｓ． Ｋｅｙｗｏｒｄｓ：ａｕｔｏｎｏｍｏｕｓ ｕｎｄｅｒｗａｔｅｒ ｖｅｈｉｃｌｅｓ； ｓｔａｂｉｌｉｚａｔｉｏｎ； ｔｈｒｅｅ ｄｉｍｅｎｓｉｏｎａｌ； ｔｒａｊｅｃｔｏｒｉｅｓ； ｌｉｎｅａｒ ｃｏｎｔｒｏｌ ｓｙｓｔｅｍｓ； ｔｉｍｅ ｖａｒｙｉｎｇ ｓｙｓｔｅｍｓ； ｍｏｔｉｏｎ ｃｏｎｔｒｏｌ； ｓｉｍｕｌａｔｉｏｎ 收稿日期：２０１２⁃１０⁃２７． 网络出版日期：２０１３⁃１１⁃０７． 基金项目：国家自然科学基金资助项目（ ５１２７９２２１，５１１７９０３５）；黑龙江 省自然科学基金资助项目（Ｅ２０１１２１）． 通信作者：吴琪．Ｅ⁃ｍａｉｌ： ｗｕｑｉ０６０９＠ １６３．ｃｏｍ． 智能水下机器人（ａｕｔｏｎｏｍｏｕｓ ｕｎｄｅｒｗａｔｅｒ ｖｅｈｉｃｌｅ， ＡＵＶ）作为船舶与海洋工程和机器人技术的学科交叉 领域，得到了世界各国的广泛重视。 ＡＵＶ 在军事上 可广泛应用于水域侦查、中继通讯、区域探雷、智能攻 击等；在民用方面，可用于海底生物资源探查、矿产资 源采样、海底地形勘测、沉物打捞、地震地热活动监 测、海洋环境监测等。 在 ＡＵＶ 进行作业任务的过程 中，轨迹跟踪能力是必不可少的技术条件之一。 尤其 是在执行海区搜索、海洋测绘等任务的时候，轨迹跟 踪的精度直接影响到区域覆盖和探测数据的完整 性［１－２］ 。 文献［３］采用微分同胚变化进行了二维平面 的水面无人艇的镇定控制研究，引进扩大形式函数 δ ａ λ 进行了二维镇定的证明，仿真结果表明可以实现无人 艇的镇定控制。 本文基于文献［３］方法，针对欠驱动
第2期 吴琪，等：基于四元数的欠驱动AV的镇定控制设计 ·187· AUV的空间六自由度运动方程，采用四元数进行模 在大地坐标系和联体坐标系中，当给定物体的 型变换，提出一种连续时变控制律，引进扩大形式函 姿态用p、0、中表示，并且分别以x、y、z旋转，则实现 数8证明了这种控制律的收敛性。 从四元数到欧拉角的转换公式为) 1四元数 9=arctan[2(9o91+9293)]/[1-2(qi+q2)] 0=arcsin[-2(q193-9o42）] 四元数向量写法为 中=arctan[2(g,92+qo93)]/[1-2(q2+gi)] Q=t+xi+yj+zk 式中：p、0、p分别为横滚角、纵倾角和偏航角，90、 以原点为旋转中心，旋转的轴分别是αB、 91、92、9:分别为四元数的各个分量。实现上述坐标 y(a2+B2+y2=1),用右手系的坐标定义，望向向量 转换的方向余弦矩阵为 &B、Y的前进方向反时针旋转0，用四元数表示为 8 Q=(cos 2,asin 2Bsin 2,rsin2) Ag=A(p)A,(8)A.(中)= cos dcos 0 sinΦcos6 sin cosφsin Osin-sin中cos o sin osin Osin+cosφcos cos Osin cos中sin0cosp+sin中sin o sin中sin Ocos o-cosφsin o cos 0cos o 实现相应坐标转换的四元数矩阵为 「g6+g斤-92-952(9192+909)2(9193-4092) A,= 2(9192-9093） q6-97+q2-qi2(9293+9041） 2(9193+9092）2(9293-9091)96-9i-92+9i」 2水下机器人空间运动方程 u=T1/m1-[(m33/m11)wg-（m2/m11)tr+ (X./m1)u] 水下机器人6自由度空间运动方程可以写为 i-[(m,/m22)ur-(m33/m22)wp+(Y,/m22)v] (Mi+C(v)v+D(v)v+g()=T (1) w=-[(mz/ma)up-(ma/ms)uq +(Z./ma)w] n=J(n)v 式中：M为机器人总的质量矩阵，C()为科氏力矩 n=/mu-〔8-m 20m+mwm -gr- m44 m44 阵，D(v)为阻尼矩阵，g(η)为重力以及恢复力 矩，J(?)为位移转换矩阵，T为控制力。为速度 dp,Weos @coWco m44 向量，v=[u。wpq小T,分别代表纵向、横 m11-m33 m4-m66 向、垂向速度和横滚、纵倾、首摇角速度；？为位移 9=Ts/m55-[ 一u10+ pr- mss mss 向量，？=[xyzp]T,代表纵向、横向、垂 dss 向位移和横滚、纵倾、首摇角度。 q BG.Wsin 0+BG,Wcose cos mss 欠驱动水下机器人一般采用桨一舵一翼形式， 因此外部作用力轴向推进力T1、横滚力矩T4、纵倾 广=76/m6-【- 1ms5-m44 —Pq一 m的 m66 力矩ī5和偏航力矩T6,忽略二阶非线性水动力细数 的影响，将方程(1)展开为式(2)： d“,-BG.Wcos 0sin中-BG,Wsin](2） m66 x=ucos中cos0+v(cos中sin Osin-sin中cosp）+ 为了获得合适的同类型的运动模型，选择单位 (cosΦsin dcos+sin中sinp) 四元数来参数化模型(2)，单位四元数定义为：在单 y=usin中cosB+v(sin中sin Osin+cos中cosp)+ 位向量k附近旋转角度中，则4个参数有如下形式： to(sin中sin Ocos-cos中sinp) =-usin 0 vcos Osin wcos Ocos p [e1e2e3]'=ksin(中/2) =p qsin otan 0 rcos otan E4=c0s(中/2) 0=gcos o-rsin o 假设中|0,单位四元数满足： o gsin o/cos 0 rcos o/cos 6 e+e3+e号+ei=1 (3)
ＡＵＶ 的空间六自由度运动方程，采用四元数进行模 型变换，提出一种连续时变控制律，引进扩大形式函 数 δ ａ λ 证明了这种控制律的收敛性。 １ 四元数 四元数向量写法为 Ｑ ＝ ｔ ＋ ｘｉ ＋ ｙｊ ＋ ｚｋ 以原点为旋转中心，旋转的轴分别是 α、β、 γ（α ２ ＋β ２ ＋ γ ２ ＝ １），用右手系的坐标定义，望向向量 α、β、γ 的前进方向反时针旋转 θ ，用四元数表示为［４］ Ｑ ＝ （ｃｏｓ θ ２ ，αｓｉｎ θ ２ ，βｓｉｎ θ ２ ，γｓｉｎ θ ２ ） 在大地坐标系和联体坐标系中，当给定物体的 姿态用 φ、θ、ϕ 表示，并且分别以 ｘ、ｙ、ｚ 旋转，则实现 从四元数到欧拉角的转换公式为［５］ φ ＝ ａｒｃｔａｎ［２（ｑ０ ｑ１ ＋ ｑ２ ｑ３ ）］ ／ ［１ － ２（ｑ ２ １ ＋ ｑ ２ ２ ）］ θ ＝ ａｒｃｓｉｎ［ － ２（ｑ１ ｑ３ － ｑ０ ｑ２ ）］ ϕ ＝ ａｒｃｔａｎ［２（ｑ１ ｑ２ ＋ ｑ０ ｑ３ ）］ ／ ［１ － ２（ｑ ２ ２ ＋ ｑ ２ ３ ）］ 式中： φ、θ、φ 分别为横滚角、纵倾角和偏航角， ｑ０ 、 ｑ１ 、ｑ２ 、ｑ３ 分别为四元数的各个分量。 实现上述坐标 转换的方向余弦矩阵为 Ａｂｇ ＝ Ａｘ（φ）Ａｙ（θ）Ａｚ（ϕ） ＝ ｃｏｓ ϕｃｏｓ θ ｓｉｎ ϕｃｏｓ θ － ｓｉｎ θ ｃｏｓ ϕｓｉｎ θｓｉｎ φ － ｓｉｎ ϕｃｏｓ φ ｓｉｎ ϕｓｉｎ θｓｉｎ φ ＋ ｃｏｓ ϕｃｏｓ φ ｃｏｓ θｓｉｎ φ ｃｏｓ ϕｓｉｎ θｃｏｓ φ ＋ ｓｉｎ ϕｓｉｎ φ ｓｉｎ ϕｓｉｎ θｃｏｓ φ － ｃｏｓ ϕｓｉｎ φ ｃｏｓ θｃｏｓ φ é ë ê ê êê ù û ú ú úú 实现相应坐标转换的四元数矩阵为 Ａｑ ＝ ｑ ２ ０ ＋ ｑ ２ １ － ｑ ２ ２ － ｑ ２ ３ ２（ｑ１ ｑ２ ＋ ｑ０ ｑ３ ） ２（ｑ１ ｑ３ － ｑ０ ｑ２ ） ２（ｑ１ ｑ２ － ｑ０ ｑ３ ） ｑ ２ ０ － ｑ ２ １ ＋ ｑ ２ ２ － ｑ ２ ３ ２（ｑ２ ｑ３ ＋ ｑ０ ｑ１ ） ２（ｑ１ ｑ３ ＋ ｑ０ ｑ２ ） ２（ｑ２ ｑ３ － ｑ０ ｑ１ ） ｑ ２ ０ － ｑ ２ １ － ｑ ２ ２ ＋ ｑ ２ ３ é ë ê ê ê ê ù û ú ú ú ú ２ 水下机器人空间运动方程 水下机器人６ 自由度空间运动方程可以写为［１］ Ｍｖ · ＋ Ｃ（ｖ）ｖ ＋ Ｄ（ｖ）ｖ ＋ ｇ（η） ＝ τ η · ＝ Ｊ（η）ｖ { （１） 式中： Ｍ 为机器人总的质量矩阵， Ｃ（ｖ） 为科氏力矩 阵， Ｄ（ｖ） 为阻尼矩阵， ｇ（η） 为重力以及恢复力 矩， Ｊ（η） 为位移转换矩阵， τ 为控制力。 ｖ 为速度 向量， ｖ ＝ [ｕ ｖ ｗ ｐ ｑ ｒ] Ｔ ，分别代表纵向、横 向、垂向速度和横滚、纵倾、首摇角速度； η 为位移 ψ 向量， η ＝ [ｘ ｙ ｚ φ θ ] Ｔ ，代表纵向、横向、垂 向位移和横滚、纵倾、首摇角度。 欠驱动水下机器人一般采用桨—舵—翼形式， 因此外部作用力轴向推进力 τ １ 、横滚力矩 τ ４ 、纵倾 力矩 τ ５ 和偏航力矩 τ ６ ，忽略二阶非线性水动力细数 的影响，将方程（１）展开为式（２）： ｘ · ＝ ｕｃｏｓ ϕｃｏｓ θ ＋ ｖ（ｃｏｓ ϕｓｉｎ θｓｉｎ φ － ｓｉｎ ϕｃｏｓ φ） ＋ ｗ（ｃｏｓ ϕｓｉｎ θｃｏｓ φ ＋ ｓｉｎ ϕｓｉｎ φ） ｙ · ＝ ｕｓｉｎ ϕｃｏｓ θ ＋ ｖ（ｓｉｎ ϕｓｉｎ θｓｉｎ φ ＋ ｃｏｓ ϕｃｏｓ φ） ＋ ｗ（ｓｉｎ ϕｓｉｎ θｃｏｓ φ － ｃｏｓ ϕｓｉｎ φ） ｚ · ＝ － ｕｓｉｎ θ ＋ ｖｃｏｓ θｓｉｎ φ ＋ ｗｃｏｓ θｃｏｓ φ φ · ＝ ｐ ＋ ｑｓｉｎ φｔａｎ θ ＋ ｒｃｏｓ φｔａｎ θ θ · ＝ ｑｃｏｓ φ － ｒｓｉｎ φ ϕ · ＝ ｑｓｉｎ φ／ ｃｏｓ θ ＋ ｒｃｏｓ φ／ ｃｏｓ θ ｕ · ＝ τ１ ／ ｍ１１ － ［（ｍ３３ ／ ｍ１１ ）ｗｑ － （ｍ２２ ／ ｍ１１ ）ｖｒ ＋ （Ｘｕ ／ ｍ１１ ）ｕ］ ｖ · － ［（ｍ１１ ／ ｍ２２ ）ｕｒ － （ｍ３３ ／ ｍ２２ ）ｗｐ ＋ （Ｙｖ ／ ｍ２２ ）ｖ］ ｗ · ＝ － ［（ｍ２２ ／ ｍ３３）ｖｐ － （ｍ１１ ／ ｍ３３）ｕｑ ＋ （Ｚｗ ／ ｍ３３）ｗ］ ｐ · ＝ τ４ ／ ｍ４４ － ［ ｍ３３ － ｍ２２ ｍ４４ ｗｖ ＋ ｍ６６ － ｍ５５ ｍ４４ ｑｒ － ｄ４４ ｍ４４ ｐ － ＢＧｙＷｃｏｓ θｃｏｓ ϕ ＋ ＢＧｚＷｃｏｓ θｓｉｎ ϕ］ ｑ · ＝ τ５ ／ ｍ５５ － ［ ｍ１１ － ｍ３３ ｍ５５ ｕｗ ＋ ｍ４４ － ｍ６６ ｍ５５ ｐｒ － ｄ５５ ｍ５５ ｑ ＋ ＢＧｚＷｓｉｎ θ ＋ ＢＧｘＷｃｏｓθ ｃｏｓ ϕ］ ｒ · ＝ τ６ ／ ｍ６６ － ［ ｍ２２ － ｍ１１ ｍ６６ ｕｖ ＋ ｍ５５ － ｍ４４ ｍ６６ ｐｑ － ｄ６６ ｍ６６ ｒ － ＢＧｘＷｃｏｓ θｓｉｎ ϕ － ＢＧｙＷｓｉｎ θ］ （２） 为了获得合适的同类型的运动模型，选择单位 四元数来参数化模型（２），单位四元数定义为：在单 位向量 ｋ 附近旋转角度 ϕ ，则 ４ 个参数有如下形式： ［ε１ ε２ ε３ ］ Ｔ ＝ ｋｓｉｎ（ϕ／ ２） ε４ ＝ ｃｏｓ（ϕ／ ２） 假设 ϕ ＜ π ，且 ε４ ＞ ０，单位四元数满足： ε ２ １ ＋ ε ２ ２ ＋ ε ２ ３ ＋ ε ２ ４ ＝ １ （３） 第 ２ 期 吴琪，等： 基于四元数的欠驱动 ＡＵＶ 的镇定控制设计 ·１８７·
·188· 智能系统学报 第9卷 方程(2)可以写为 p"(n,s,v,w)sin(t/s) (5) x=(1-2e1-2e)u+2(e62-63V1-e-e-6)· pa(n,s,v,w,t)=-kis (6) v+2(e1e3+e2√1-e-e3-e3)w 9(n,E,D,0,t)=-k2e2-2(kz+ y=2(6162+e3√1-e-6-6)u+(1-2s7- du)/p()si t (7) 2s)u+2(6263-61√1-e-6-6)0 T(n,E,D,0,t)=-k3e3+2(ky+ 2=2(e163-62√-e-6号-e)u+2(6263+ do)/p(n,e,,o)sin号 (8) 61√1-61-83-e3)v+(1-2s-2e)0 式中：k、k1、k2、3、k、dE均大于零。p(n,E,, e,=(p1-e7-e3-e-E39+e2r)/2 心)是一个任意可微的同类型的标准函数)，考虑到 82=(e3P+9W1-6-e3-e-61r）/2 其形式为 8(7,E,v,10）= e3=(-62P+619+r√1-e1-63-e3)/2 (Ax,A'y,A2z,A61,A62,A3,A'v,A'w,t) i=T1-[(m33/m1)wg-(m2/m1)r- 进一步定义控制输入： (X./m1)u] T(7,t,t)=-k,m(u-u(7,,0,t)）(9) i=-[(m1/m2)r-（m3s/m2）wp- T(7,v,t)=-km4a(p-Pa(7,v,0,t)）(10) (Y,/m2)v] T(n,,t)=-k5m5s(q-9a(n,t,w,t)(11) =-[(m2/m3)p-（m1,/m3）g- T6(m,v,t)=-k6m6(r-r(7,v,0,t)(12) (Z./m3）o] 则存在e。>0,对于任意的e∈(0，Eo),当k1、k2、 m33-m22 1m66-m k、k,足够大且全为正数，则控制律(9)~(12)可使 p=T4-[ -10D+ -gr m44 m44 系统(4)局部指数镇。考虑如下的形式： d 2W BG. 8说(7,6，"，0）= 一卫一 (E2E3+E1√1-e-e-e)] m44 (Ax,A'y,A'z,AE1.A62,A63,A2v,A'w,t) m 证明：AUV的空间运动方程模型(1)在控制输 9=75-【"m!~m m44-m6 二u10+ pr- 入T2=0,T3=0的情况下，可写为 mss [)T=fn,心，)+h(n,,t) (13) dss 2W BG. (e2√1-e-e-e-e163)] 式中： mss mss 「u,D+2e3u,0-2e2u, i=76-[ mz mu 1ms5-m44 一uU+ p/2,9/2,r/2, m66 m66 (duu+T)/mn (4) （-d2v-mur)m2 定义向量7=[m1n2]',其中n1-[xyz]T是 f(n,v,t)= (-d3sv muuq)m33 惯性坐标系下的位置向量，72=[e1e26,]T。定义 T=[T1T2T3T4T5T6]分别代表纵向、横向、垂向、 （-d4P-2WBGE1+Ta)m4 横滚、纵倾、首摇方向的控制输入。 (-dssq -2W BG.62 +Ts)mss 3AUV的镇定设计 (-d6r+T6)m6 h(),v,t)是余下的部分。相比于标准函数8识、r1、 这一部分只考虑AUV的镇定控制输入只有4 T4、T5、T6是同类的一阶形式，且处处连续。向量场f 个方向：轴向推进力T1、横滚力矩T4、纵倾力矩T5和 是连续的时间周期的，且相对于标准函数说，∫是 偏航力矩r6。根据Brockett定理的第3个条件o], 同类型的零阶形式。向量场h(n,v,t)也是连续的， 可知六自由度空间运动方程不存在任何光滑时不变 是相对于8严格的正阶的同类型函数形式的总和。 反馈控制使系统在平衡点附近渐进稳定。因此本文 因此，如果（η，）=(0,0)是系统(14)局部渐近指 提出一个连续时变反馈控制律。 数稳定的稳定点，则系统(13)的解(1，)=(0,0) 考虑虚拟设计控制输入： 是局部p一指数镇定的。 ua(7,E,v,0,t)=-kx+ [i i]=f(n,v,t) (14)
方程（２）可以写为 ｘ · ＝ （１ － ２ε ２ １ － ２ε ２ ２）ｕ ＋ ２（ε１ε２ － ε３ １ － ε ２ １ － ε ２ ２ － ε ２ ３ ）· ｖ ＋ ２（ε１ε３ ＋ ε２ １ － ε ２ １ － ε ２ ２ － ε ２ ３ ）ｗ ｙ · ＝ ２（ε１ε２ ＋ ε３ １ － ε ２ １ － ε ２ ２ － ε ２ ３ ）ｕ ＋ （１ － ２ε ２ １ － ２ε ２ ２ ）ｖ ＋ ２（ε２ε３ － ε１ １ － ε ２ １ － ε ２ ２ － ε ２ ３ ）ｗ ｚ · ＝ ２（ε１ε３ － ε２ １ － ε ２ １ － ε ２ ２ － ε ２ ３ ）ｕ ＋ ２（ε２ε３ ＋ ε１ １ － ε ２ １ － ε ２ ２ － ε ２ ３ ）ｖ ＋ （１ － ２ε ２ １ － ２ε ２ ２ ）ｗ ε · １ ＝ （ｐ １ － ε ２ １ － ε ２ ２ － ε ２ ３ － ε３ ｑ ＋ ε２ ｒ） ／ ２ ε · ２ ＝ （ε３ ｐ ＋ ｑ １ － ε ２ １ － ε ２ ２ － ε ２ ３ － ε１ ｒ） ／ ２ ε · ３ ＝ （ － ε２ ｐ ＋ ε１ ｑ ＋ ｒ １ － ε ２ １ － ε ２ ２ － ε ２ ３ ） ／ ２ ｕ · ＝ τ１ － ［（ｍ３３ ／ ｍ１１ ）ｗｑ － （ｍ２２ ／ ｍ１１ ）ｖｒ － （Ｘｕ ／ ｍ１１ ）ｕ］ ｖ · ＝ － ［（ｍ１１ ／ ｍ２２ ）ｕｒ － （ｍ３３ ／ ｍ２２ ）ｗｐ － （Ｙｖ ／ ｍ２２ ）ｖ］ ｗ · ＝ － ［（ｍ２２ ／ ｍ３３ ）ｖｐ － （ｍ１１ ／ ｍ３３ ）ｕｑ － （Ｚｗ ／ ｍ３３ ）ｗ］ ｐ · ＝ τ４ － ［ ｍ３３ － ｍ２２ ｍ４４ ｗｖ ＋ ｍ６６ － ｍ５５ ｍ４４ ｑｒ － ｄ４４ ｍ４４ ｐ － ２Ｗ ＢＧｚ ｍ４４ （ε２ε３ ＋ ε１ １ － ε ２ １ － ε ２ ２ － ε ２ ３ ）］ ｑ · ＝ τ５ － ［ ｍ１１ － ｍ３３ ｍ５５ ｕｗ ＋ ｍ４４ － ｍ６６ ｍ５５ ｐｒ － ｄ５５ ｍ５５ ｑ － ２Ｗ ＢＧｚ ｍ５５ （ε２ １ － ε ２ １ － ε ２ ２ － ε ２ ３ － ε１ε３ ）］ ｒ · ＝ τ６ － ［ ｍ２２ － ｍ１１ ｍ６６ ｕｖ ＋ ｍ５５ － ｍ４４ ｍ６６ ｐｑ － ｄ６６ ｍ６６ ｒ］ （４） 定义向量 η ＝ ［η１ η２ ］ Ｔ ，其中 η１ ＝ ［ｘ ｙ ｚ］ Ｔ 是 惯性坐标系下的位置向量， η２ ＝ ［ε１ ε２ ε３ ］ Ｔ 。 定义 τ ＝ ［τ １ τ ２ τ ３ τ ４ τ ５ τ ６ ］ Ｔ 分别代表纵向、横向、垂向、 横滚、纵倾、首摇方向的控制输入。 ３ ＡＵＶ 的镇定设计 这一部分只考虑 ＡＵＶ 的镇定控制输入只有 ４ 个方向：轴向推进力 τ １ 、横滚力矩 τ ４ 、纵倾力矩 τ ５ 和 偏航力矩 τ ６ 。 根据 Ｂｒｏｃｋｅｔｔ 定理的第 ３ 个条件［６］ ， 可知六自由度空间运动方程不存在任何光滑时不变 反馈控制使系统在平衡点附近渐进稳定。 因此本文 提出一个连续时变反馈控制律。 考虑虚拟设计控制输入： ｕｄ（η，ε，ｖ，ｗ，ｔ） ＝ － ｋ ｘ ｘ ＋ ρ ａ （η，ε，ｖ，ｗ）ｓｉｎ（ｔ ／ ε） （５） ｐｄ（η，ε，ｖ，ｗ，ｔ） ＝ － ｋ ε１ε１ （６） ｑｄ（η，ε，ｖ，ｗ，ｔ） ＝ － ｋ ε２ε２ － ２（ｋｚ ＋ ｄｗ） ／ ρ ａ （η，ε，ｖ，ｗ）ｓｉｎ ｔ ε （７） ｒｄ（η，ε，ｖ，ｗ，ｔ） ＝ － ｋ ε３ε３ ＋ ２（ｋｙ ＋ ｄｖ） ／ ρ ａ （η，ε，ｖ，ｗ）ｓｉｎ ｔ ε （８） 式中： ｋ ｘ 、ｋ ε１ 、 ｋ ε２ 、ｋ ε３ 、ｋ、ｄ、ε 均大于零。 ρ ａ （η，ε，ｖ， ｗ） 是一个任意可微的同类型的标准函数［３］ ，考虑到 其形式为 δ ａ λ（η，ε，ｖ，ｗ） ＝ （λｘ，λ ２ ｙ，λ ２ ｚ，λε１ ，λε２ ，λε３ ，λ ２ ｖ，λ ２ｗ，ｔ） 进一步定义控制输入： τ１（η，ｖ，ｔ） ＝ － ｋ１ｍ１１（ｕ － ｕｄ（η，ｖ，ｗ，ｔ）） （９） τ４（η，ｖ，ｔ） ＝ － ｋ４ｍ４４（ｐ － ｐｄ（η，ｖ，ｗ，ｔ）） （１０） τ５（η，ｖ，ｔ） ＝ － ｋ５ｍ５５（ｑ － ｑｄ（η，ｖ，ｗ，ｔ）） （１１） τ６（η，ｖ，ｔ） ＝ － ｋ６ｍ６６（ｒ － ｒｄ（η，ｖ，ｗ，ｔ）） （１２） 则存在 ε０ ＞ ０，对于任意的 ε ∈ （０，ε０ ） ，当 ｋ１ 、ｋ２ 、 ｋ３ 、ｋ４ 足够大且全为正数，则控制律（９） ～ （１２）可使 系统（４）局部指数镇。 考虑如下的形式： δ β λ（η，ε，ｖ，ｗ） ＝ （λｘ，λ ２ ｙ，λ ２ ｚ，λε１ ，λε２ ，λε３ ，λ ２ ｖ，λ ２ｗ，ｔ） 证明：ＡＵＶ 的空间运动方程模型（１）在控制输 入 τ ２ ＝０，τ ３ ＝ ０ 的情况下，可写为 η · ｖ · [ ] Ｔ ＝ ｆ（η，ｖ，ｔ） ＋ ｈ（η，ｖ，ｔ） （１３） 式中： ｆ（η，ｖ，ｔ） ＝ ｕ，ｖ ＋ ２ε３ ｕ，ｗ － ２ε２ ｕ， ｐ ／ ２，ｑ ／ ２，ｒ／ ２， （ － ｄ１１ ｕ ＋ τ１ ） ／ ｍ１１ （ － ｄ２２ ｖ － ｍ１１ ｕｒ）ｍ２２ （ － ｄ３３ ｖ ＋ ｍ１１ ｕｑ）ｍ３３ （ － ｄ４４ ｐ － ２Ｗ ＢＧｚε１ ＋ τ４ ）ｍ４４ （ － ｄ５５ ｑ － ２Ｗ ＢＧｚε２ ＋ τ５ ）ｍ５５ （ － ｄ６６ ｒ ＋ τ６ ）ｍ６６ é ë ê ê ê ê ê ê ê ê ê ê ê ê ê ù û ú ú ú ú ú ú ú ú ú ú ú ú ú Ｔ ｈ（η，ｖ，ｔ） 是余下的部分。 相比于标准函数 δ β λ 、τ １ 、 τ ４ 、τ ５ 、τ ６ 是同类的一阶形式，且处处连续。 向量场 ｆ 是连续的时间周期的，且相对于标准函数 δ β λ ， ｆ 是 同类型的零阶形式。 向量场 ｈ（η，ｖ，ｔ） 也是连续的， 是相对于 δ β λ 严格的正阶的同类型函数形式的总和。 因此，如果 （η，ｖ） ＝ （０，０） 是系统（１４）局部渐近指 数稳定的稳定点，则系统（１３）的解 （η，ｖ） ＝ （０，０） 是局部 ρ β —指数镇定的。 η · ｖ · [ ] ＝ ｆ（η，ｖ，ｔ） Ｔ （１４） ·１８８· 智 能 系 统 学 报 第 ９ 卷
第2期 吴琪，等：基于四元数的欠驱动AV的镇定控制设计 .189. 由于e4没有包含在f(n,,t)中，且e4可由式(11) E=g(5,t/e) (16) 惟一定义，因此可以将系统(14)中的第7个方程去 取用自治的平均系统近似的代替系统(16)，方 除。如果简化后的方程局部渐进稳定，则原系统方 程(16)的平均系统为 程(13)也渐近稳定。 =go() def 进一步定义u兰山4，p当p4,g兰94，r兰r,将 式中：g()是其平均值。将控制输人(13)~(16) 其作为输入变量，简化后有 带入到(15)，系统(15)的平均系统可写为 ud -Kx v+283ud v-2k'83x w 22ua y w+2k82x 1/2pa -1/2k1E1 B1 1/2qa 9, -1/2k2e2 (15) = 1/2ra 令 -1/2k3E3 今 E3 ua一 m(kkx8,+y+d）- m22 m22 mn m22 m da _w -uaqa- —10 m mu(kk2x62-kz-de)- m33 I m33 33 方程组(5)~(8)定义了(15)的控制输入。鉴于其 (17) 是周期时变输入，因此产生的系统结果也是周期时 方程(17)可以写成如下的级联系统： 变的，可写为 2k'63x y 10 2kBxx · dn mu(ky+dv)-m + m22 mn mukkxes m22 m(-kz-d）- ds m33 m33 mukkxe2 m33 0 0 √ 0 -2k83x 0 0 0 1 2kBxx 0 ud一 n 0 m22 m22 z mukkxEs (18) m22 0 以水 0 d d 10 m33 m m3sJ mukkx6: m33 -k'x [4]命题，存在k1k2、k3k4足够大且全为正数，合理 -1/2k4E1 地选择参数k、d,能够使得系(18)渐近稳定。 2 -1/2k2e2 (19) 4 镇定仿真 83J 1/2ke3」 根据线性理论知识容易得到式(19)是全局稳定 AUV的基本参数为)：m1=215,m2=265, 的。当系统(19)全局稳定时，其状态向量全部趋近于 m33=265,m44=40,m55=80,m6=80,d,=70, 零。且由(18)可以看出，关联项是高阶小的形式，因 d22=100,d3=100,d44=30,d5=50,d6=50,且 此只要设计证明系统(18)的名义系统稳定即可。根 AUV是中性浮力，不计入重力和浮力（假设重力和 据Hurwitz稳定性定理，当名义系统矩阵的所有特征 浮力平衡，且重心浮心重合)。 值具有负实部，则系统可以实现稳定。因此根据文献 采用式(9)~(12)的控制律进行控制输入，从 上一节可知T4=0,满足渐近收敛。相关控制参数
由于 ε４ 没有包含在 ｆ（η，ｖ，ｔ） 中，且 ε４ 可由式（１１） 惟一定义，因此可以将系统（１４）中的第 ７ 个方程去 除。 如果简化后的方程局部渐进稳定，则原系统方 程（１３）也渐近稳定。 进一步定义 ｕ ＝ ｄｅｆ ｕｄ ， ｐ ＝ ｄｅｆ ｐｄ ， ｑ ＝ ｄｅｆ ｑｄ ， ｒ ＝ ｄｅｆ ｒｄ ，将 其作为输入变量，简化后有 ｘ · ｙ · ｚ · ε１ ε２ ε３ ｖ · ｗ · é ë ê ê ê ê ê ê ê ê ê ê ê ê ù û ú ú ú ú ú ú ú ú ú ú ú ú ＝ ｕｄ ｖ ＋ ２ε３ ｕｄ ｗ － ２ε２ ｕｄ １ ／ ２ｐｄ １ ／ ２ｑｄ １ ／ ２ｒｄ － ｍ１１ ｍ２２ ｕｄ ｒｄ － ｄ２２ ｍ２２ ｖ － ｍ１１ ｍ３３ ｕｄ ｑｄ － ｄ３３ ｍ３３ ｗ é ë ê ê ê ê ê ê ê ê ê ê ê ê ê ê ê ù û ú ú ú ú ú ú ú ú ú ú ú ú ú ú ú （１５） 方程组（５） ～ （８）定义了（１５）的控制输入。 鉴于其 是周期时变输入，因此产生的系统结果也是周期时 变的，可写为 ξ · ＝ ｇ（ξ，ｔ ／ ε） （１６） 取用自治的平均系统近似的代替系统（１６），方 程（１６）的平均系统为 ξ · ＝ ｇ０（ξ） 式中： ｇ０（ξ） 是其平均值。 将控制输入（１３） ～ （１６） 带入到（１５），系统（１５）的平均系统可写为 ｘ · ｙ · ｚ · ε１ ε２ ε３ ｖ · ｗ · é ë ê ê ê ê ê ê ê ê ê ê ê ê ù û ú ú ú ú ú ú ú ú ú ú ú ú ＝ － ｋ ｘ ｘ ｖ － ２ｋ ｘ ε３ ｘ ｗ ＋ ２ｋ ｘ ε２ ｘ － １ ／ ２ｋ ε１ε１ － １ ／ ２ｋ ε２ε２ － １ ／ ２ｋ ε３ε３ － ｍ１１ ｍ２２ （ｋ ｘ ｋ ε３ ｘε３ ＋ ｋｙ ＋ ｄｖ） － ｄ２２ ｍ２２ ｖ ｍ１１ ｍ３３ （ｋ ｘ ｋ ε２ ｘε２ － ｋｚ － ｄｗ） － ｄ３３ ｍ３３ ｗ é ë ê ê ê ê ê ê ê ê ê ê ê ê ê ê ê ê ù û ú ú ú ú ú ú ú ú ú ú ú ú ú ú ú ú （１７） 方程（１７）可以写成如下的级联系统： ｙ · ｚ · ｖ · ｗ · é ë ê ê ê ê êê ù û ú ú ú ú úú ＝ ｖ ｗ － ｍ１１ ｍ２２ （ｋｙ ＋ ｄｖ） － ｄ２２ ｍ２２ ｖ ｍ１１ ｍ３３ （ － ｋｚ － ｄｗ） － ｄ３３ ｍ３３ ｗ é ë ê ê ê ê ê ê ê ê ù û ú ú ú ú ú ú ú ú ＋ － ２ｋ ｘ ε３ ｘ ２ｋ ｘ ε２ ｘ － ｍ１１ ｍ２２ ｋ ｘ ｋ ε３ ｘε３ ｍ１１ ｍ３３ ｋ ｘ ｋ ε２ ｘε２ é ë ê ê ê ê ê ê ê ê ê ù û ú ú ú ú ú ú ú ú ú ＝ ０ ０ １ ０ ０ ０ ０ １ － ｍ１１ ｍ２２ ｋ ０ － ｍ１１ ｍ２２ ｄ － ｄ２２ ｍ２２ ０ ０ － ｍ１１ ｍ３３ ｋ ０ － ｍ１１ ｍ３３ ｄ － ｄ３３ ｍ３３ é ë ê ê ê ê ê ê ê ê ù û ú ú ú ú ú ú ú ú ｙ ｚ ｖ ｗ é ë ê ê ê ê ê ù û ú ú ú ú ú ＋ － ２ｋ ｘ ε３ ｘ ２ｋ ｘ ε２ ｘ － ｍ１１ ｍ２２ ｋ ｘ ｋ ε３ ｘε３ ｍ１１ ｍ３３ ｋ ｘ ｋ ε２ ｘε２ é ë ê ê ê ê ê ê ê ê ê ù û ú ú ú ú ú ú ú ú ú （１８） ｘ · ε · １ ε · ２ ε · ３ é ë ê ê ê ê ê ê ù û ú ú ú ú ú ú ＝ － ｋ ｘ ｘ － １ ／ ２ｋ ε１ε１ － １ ／ ２ｋ ε２ε２ － １ ／ ２ｋ ε３ε３ é ë ê ê ê ê ê ê ù û ú ú ú ú ú ú （１９） 根据线性理论知识容易得到式（１９）是全局稳定 的。 当系统（１９）全局稳定时，其状态向量全部趋近于 零。 且由（１８）可以看出，关联项是高阶小的形式，因 此只要设计证明系统（１８）的名义系统稳定即可。 根 据 Ｈｕｒｗｉｔｚ 稳定性定理，当名义系统矩阵的所有特征 值具有负实部，则系统可以实现稳定。 因此根据文献 ［４］命题，存在 ｋ１ 、ｋ２ 、ｋ３ 、ｋ４ 足够大且全为正数，合理 地选择参数 ｋ、ｄ ，能够使得系（１８）渐近稳定。 ４ 镇定仿真 ＡＵＶ 的基本参数为［７］ ： ｍ１１ ＝ ２１５， ｍ２２ ＝ ２６５， ｍ３３ ＝ ２６５， ｍ４４ ＝ ４０， ｍ５５ ＝８０， ｍ６６ ＝ ８０， ｄ１１ ＝ ７０， ｄ２２ ＝１００， ｄ３３ ＝ １００， ｄ４４ ＝３０， ｄ５５ ＝ ５０， ｄ６６ ＝ ５０，且 ＡＵＶ 是中性浮力，不计入重力和浮力（假设重力和 浮力平衡，且重心浮心重合）。 采用式（９） ～ （１２） 的控制律进行控制输入，从 上一节可知 τ ４ ＝ ０，满足渐近收敛。 相关控制参数 第 ２ 期 吴琪，等： 基于四元数的欠驱动 ＡＵＶ 的镇定控制设计 ·１８９·
·190 智能系统学报 第9卷 如下：k1=3,k=3,k6=3,k=2,k2=2,k3=2, 0.6 k=0.3,d=0.3,e=2.5,名义准则取为 0.4 p°=(x+y2+2+61+8+6+02+w2)1/4 0.2 (30) 初始位置为 -0.2 [x y 2 61 62 63 u v w p q r]= -0.4 [045000000000]。 -0.6 仿真结果如图1~6所示，图1表示平移速度u、 -0.8 20406080100120140160180 v,w变化情况，图2表示转动角速度P、q、r的变化情 t/s 况，图3表示位移x、yz的变化情况，图4表示转换 图4转换变量e1,62、63 变量e1、62、e3曲线，图5反映机器人的实际镇定运 Fig.4 Transformation variables,, 动轨迹，图6代表名义标准p“变化曲线。 3.5 3.0 2.5 (s.w)/Ma 2.0 1.5 1.0 0.5 020406080100120140160180 020406080100120140160180 t/s t/s 图1平移速度u、、0 图5实际轨迹图 Fig.1 Velocity of vehicle u,v, Fig.5 Actual path of vehicle 1.5 1.0 三9 0.5 4 3 -0.5 -1.0 020406080100120140160180 0 t/s 图2旋转速度p、q、T 0 0 Fig.2 Rotational speed of vehicle p,q,r y/m -2-1 x/m 10 图6名义标准p响应 Fig.6 Response of the dilation p" 日5 图1中速度在初始时刻具有较大的变化率和 超调量，说明初始时刻有较大的控制输入以及输入 变化，即输入的力和力矩是突然加人的，引起欠驱动 AUV有较大的速度变化，在仿真中这是允许的。位 020406080100120140160180 移图3在稳定点附近存在震荡，因此实际的运动轨 t/s 图3位移xy、 迹在稳定点附近会比较密集，仿真结果与此相符合。 因此在实际的AUV操控中，就需要考虑物理量的变 Fig.3 Displacement of vehiclex,y,z
如下： ｋ１ ＝ ３， ｋ５ ＝ ３， ｋ６ ＝ ３， ｋ ｘ ＝ ２， ｋ ε２ ＝ ２， ｋ ε３ ＝ ２， ｋ ＝０．３， ｄ ＝ ０．３，ε ＝ ２．５，名义准则取为 ρ α ＝ （ｘ ４ ＋ ｙ ２ ＋ ｚ ２ ＋ ε ４ １ ＋ ε ４ ２ ＋ ε ４ ３ ＋ ｖ ２ ＋ ｗ ２ ） １ ／ ４ （３０） 初始位置为 [ｘ ｙ ｚ ε１ ε２ ε３ ｕ ｖ ｗ ｐ ｑ ｒ] ＝ [０ ４ ５ ０ ０ ０ ０ ０ ０ ０ ０ ０] 。 仿真结果如图 １～６ 所示，图 １ 表示平移速度 ｕ、 ｖ、ｗ 变化情况，图 ２ 表示转动角速度 ｐ、ｑ、ｒ 的变化情 况，图 ３ 表示位移 ｘ、ｙ、ｚ 的变化情况，图 ４ 表示转换 变量 ε１ 、ε２ 、ε３ 曲线，图 ５ 反映机器人的实际镇定运 动轨迹，图 ６ 代表名义标准 ρ α 变化曲线。 图 １ 平移速度 ｕ、 ｖ、 ｗ Ｆｉｇ．１ Ｖｅｌｏｃｉｔｙ ｏｆ ｖｅｈｉｃｌｅ ｕ， ｖ， ｗ 图 ２ 旋转速度 ｐ、 ｑ、 ｒ Ｆｉｇ．２ Ｒｏｔａｔｉｏｎａｌ ｓｐｅｅｄ ｏｆ ｖｅｈｉｃｌｅ ｐ， ｑ， ｒ 图 ３ 位移 ｘ、ｙ 、ｚ Ｆｉｇ．３ Ｄｉｓｐｌａｃｅｍｅｎｔ ｏｆ ｖｅｈｉｃｌｅ ｘ， ｙ， ｚ 图 ４ 转换变量 ε１ ，ε２ 、ε３ Ｆｉｇ．４ Ｔｒａｎｓｆｏｒｍａｔｉｏｎ ｖａｒｉａｂｌｅｓ ε１ ，ε２ ，ε３ 图 ５ 实际轨迹图 Ｆｉｇ．５ Ａｃｔｕａｌ ｐａｔｈ ｏｆ ｖｅｈｉｃｌｅ 图 ６ 名义标准 ρ α 响应 Ｆｉｇ．６ Ｒｅｓｐｏｎｓｅ ｏｆ ｔｈｅ ｄｉｌａｔｉｏｎ ρ α 图 １ 中速度在初始时刻具有较大的变化率和 超调量，说明初始时刻有较大的控制输入以及输入 变化，即输入的力和力矩是突然加入的，引起欠驱动 ＡＵＶ 有较大的速度变化，在仿真中这是允许的。 位 移图 ３ 在稳定点附近存在震荡，因此实际的运动轨 迹在稳定点附近会比较密集，仿真结果与此相符合。 因此在实际的 ＡＵＶ 操控中，就需要考虑物理量的变 ·１９０· 智 能 系 统 学 报 第 ９ 卷
第2期 吴琪，等：基于四元数的欠驱动AV的镇定控制设计 .191. 化快慢，不能急剧地改变AUV的运动参数，使得 [2]张铭钧，宋炜胥，褚振忠.自主式水下机器人模糊定性建 AUV发生抖动，影响实际的轨迹跟踪。 模方法研究[J].哈尔滨工程大学学报，2013,34(1)： 由图1可以看出，纵向速度相比于横向和垂向 116-122. 速度收敛较慢，图3中纵向位移相比于横向和垂向 ZHANG Mingjun,SONG Weixu,CHU Zhenzhong.Re- search on the method of fuzzy qualitative modeling for AUV 位移收敛也较慢，因为纵向存在控制输入，而控制输 [J].Journal of Harbin Engineering University,2013,34 入是各个变量的综合函数，其收敛要滞后于单个变 (1):116-122. 量，因此纵向方向的变量收敛会比较慢。 3]PETTERSEN K Y,EGELAND O.Robust control of an un- 转换变量E1、62、63代表了横滚、纵倾、首摇的 deractuated surface vessel with thruster dynamics[C]//Proc 角速度，由于初始控制输入的较大加入，其变化震荡 1997 Amer Control Conf.Albuquerque,NM,USA,1997: 也比较明显，与平移速度相一致需要考虑实际情况 3411-3416. [4]倪振松，廖启征，魏世民，等.空间6R机器人位置反解的 的变化快慢。名义标准“是各个变量的偶次方之 对偶四元数法[J刀.机械工程学报，2009,45(11)：25-29. 和，因此必大于零，随着各个变量的收敛到零，p“必 NI Zhensong,LIAO Qizheng,WEI Shimin,et al.Dual four 趋于零。初始时刻其变化较快，是因为各物理量收 element method for inverse kinematics analysis of spatial 6R 敛得较快，与之相对应p“也较快减小。 manipulator[J].Joural of Mechanical Engineering,2009, 从仿真结果曲线可以看出，采用四元数进行变 45(11）：25-29 量转换以后，设计的控制输入能够实现水下机器人 [5]刘忠，梁晓庚，曹秉刚等.基于四元数的空间全方位算法 的点镇定控制，欠驱动AUV的状态变量速度和位移 研究[J].西安交通大学学报，2006,40(5)：618-620. 均可以收敛到零。仿真结果证明在3个控制输入的 LIU Zhong,LIANG Xiaogeng,CAO Binggang,et al.Space omnidircetional algorithm study based on quaternion[J]. 情况下，设计的控制器能够实现三维镇定。 Journal of Xi'an Jiaotong University,2006.40(5):618- 5结束语 620. [6]贾鹤鸣，程相勤，张利军，等.基于自适应Backstepping 本文讨论了欠驱动AUV的三维镇定控制问题。 的AUV的三维航迹跟踪控制[J].控制与决策，2012,5 通过对欠驱动AUV的六自由度空间运动方程和轨 (27):652-657. 迹方程的简化，采用四元数方法进行了模型变换，提 JIA Heming,CHENG Xiangqin,ZHANG Lijun,et al. Three-dimensional path tracking control for underactuated 出一种具有4个控制输入的连续时变镇定控制律， AUV based on adaptive backstepping[J].Control and Deci- 证明提出的控制律的收敛性，并且进行镇定仿真实 si0n,2012,5(27）:652-657. 验演示。如果在横滚方向的水动力恢复力足够大的 作者简介： 情况下，在3个控制输入的情况下AUV可以实现指 吴琪，男，1989年生，硕士研究生， 数镇定。仿真结果表明控制律的有效性，能够在任 主要研究方向为水下机器人的运动控 意初始条件下实现镇定控制。 制、路径规划以及数据融合技术。 参考文献： [1]毕凤阳.欠驱动自主水下航行器的非线性鲁棒控制策略 研究[D].哈尔滨：哈尔滨工业大学，2010：15-18 李晔，1978年生，教授，博士，主要 BI Fengyang.Research on nonlinear robust control strategies 研究方向为水下机器人技术，主持国家 of underactuated autonomous undewater vehicle[D].Harbin: 自然科学基金等国家和省部级以上项 Harbin Industrial of Technology University,2010:15-18. 目10余项，发表学术论文80余篇
化快慢，不能急剧地改变 ＡＵＶ 的运动参数，使得 ＡＵＶ 发生抖动，影响实际的轨迹跟踪。 由图 １ 可以看出，纵向速度相比于横向和垂向 速度收敛较慢，图 ３ 中纵向位移相比于横向和垂向 位移收敛也较慢，因为纵向存在控制输入，而控制输 入是各个变量的综合函数，其收敛要滞后于单个变 量，因此纵向方向的变量收敛会比较慢。 转换变量 ε１ 、ε２ 、ε３ 代表了横滚、纵倾、首摇的 角速度，由于初始控制输入的较大加入，其变化震荡 也比较明显，与平移速度相一致需要考虑实际情况 的变化快慢。 名义标准 ρ α 是各个变量的偶次方之 和，因此必大于零，随着各个变量的收敛到零， ρ α 必 趋于零。 初始时刻其变化较快，是因为各物理量收 敛得较快，与之相对应 ρ α 也较快减小。 从仿真结果曲线可以看出，采用四元数进行变 量转换以后，设计的控制输入能够实现水下机器人 的点镇定控制，欠驱动 ＡＵＶ 的状态变量速度和位移 均可以收敛到零。 仿真结果证明在 ３ 个控制输入的 情况下，设计的控制器能够实现三维镇定。 ５ 结束语 本文讨论了欠驱动 ＡＵＶ 的三维镇定控制问题。 通过对欠驱动 ＡＵＶ 的六自由度空间运动方程和轨 迹方程的简化，采用四元数方法进行了模型变换，提 出一种具有 ４ 个控制输入的连续时变镇定控制律， 证明提出的控制律的收敛性，并且进行镇定仿真实 验演示。 如果在横滚方向的水动力恢复力足够大的 情况下，在 ３ 个控制输入的情况下 ＡＵＶ 可以实现指 数镇定。 仿真结果表明控制律的有效性，能够在任 意初始条件下实现镇定控制。 参考文献： ［１］毕凤阳． 欠驱动自主水下航行器的非线性鲁棒控制策略 研究［Ｄ］． 哈尔滨：哈尔滨工业大学，２０１０： １５⁃１８． ＢＩ Ｆｅｎｇｙａｎｇ． Ｒｅｓｅａｒｃｈ ｏｎ ｎｏｎｌｉｎｅａｒ ｒｏｂｕｓｔ ｃｏｎｔｒｏｌ ｓｔｒａｔｅｇｉｅｓ ｏｆ ｕｎｄｅｒａｃｔｕａｔｅｄ ａｕｔｏｎｏｍｏｕｓ ｕｎｄｅｗａｔｅｒ ｖｅｈｉｃｌｅ［Ｄ］． Ｈａｒｂｉｎ： Ｈａｒｂｉｎ Ｉｎｄｕｓｔｒｉａｌ ｏｆ Ｔｅｃｈｎｏｌｏｇｙ Ｕｎｉｖｅｒｓｉｔｙ， ２０１０： １５⁃１８． ［２］张铭钧，宋炜胥，褚振忠．自主式水下机器人模糊定性建 模方法研究［ Ｊ］． 哈尔滨工程大学学报，２０１３， ３４（ １）： １１６⁃１２２． ＺＨＡＮＧ Ｍｉｎｇｊｕｎ， ＳＯＮＧ Ｗｅｉｘｕ， ＣＨＵ Ｚｈｅｎｚｈｏｎｇ． Ｒｅ⁃ ｓｅａｒｃｈ ｏｎ ｔｈｅ ｍｅｔｈｏｄ ｏｆ ｆｕｚｚｙ ｑｕａｌｉｔａｔｉｖｅ ｍｏｄｅｌｉｎｇ ｆｏｒ ＡＵＶ ［Ｊ］． Ｊｏｕｒｎａｌ ｏｆ Ｈａｒｂｉｎ Ｅｎｇｉｎｅｅｒｉｎｇ Ｕｎｉｖｅｒｓｉｔｙ， ２０１３， ３４ （１）： １１６⁃１２２． ［３］ＰＥＴＴＥＲＳＥＮ Ｋ Ｙ， ＥＧＥＬＡＮＤ Ｏ． Ｒｏｂｕｓｔ ｃｏｎｔｒｏｌ ｏｆ ａｎ ｕｎ⁃ ｄｅｒａｃｔｕａｔｅｄ ｓｕｒｆａｃｅ ｖｅｓｓｅｌ ｗｉｔｈ ｔｈｒｕｓｔｅｒ ｄｙｎａｍｉｃｓ［Ｃ］ ／ ／ Ｐｒｏｃ １９９７ Ａｍｅｒ Ｃｏｎｔｒｏｌ Ｃｏｎｆ． Ａｌｂｕｑｕｅｒｑｕｅ， ＮＭ， ＵＳＡ， １９９７： ３４１１⁃３４１６． ［４］倪振松，廖启征，魏世民，等． 空间 ６Ｒ 机器人位置反解的 对偶四元数法［Ｊ］． 机械工程学报， ２００９， ４５（１１） ： ２５⁃２９． ＮＩ Ｚｈｅｎｓｏｎｇ， ＬＩＡＯ Ｑｉｚｈｅｎｇ， ＷＥＩ Ｓｈｉｍｉｎ， ｅｔ ａｌ． Ｄｕａｌ ｆｏｕｒ ｅｌｅｍｅｎｔ ｍｅｔｈｏｄ ｆｏｒ ｉｎｖｅｒｓｅ ｋｉｎｅｍａｔｉｃｓ ａｎａｌｙｓｉｓ ｏｆ ｓｐａｔｉａｌ ６Ｒ ｍａｎｉｐｕｌａｔｏｒ［Ｊ］． Ｊｏｕｒｎａｌ ｏｆ Ｍｅｃｈａｎｉｃａｌ Ｅｎｇｉｎｅｅｒｉｎｇ， ２００９， ４５（１１） ： ２５⁃２９． ［５］刘忠，梁晓庚，曹秉刚等． 基于四元数的空间全方位算法 研究［Ｊ］． 西安交通大学学报， ２００６， ４０（５） ： ６１８⁃６２０． ＬＩＵ Ｚｈｏｎｇ， ＬＩＡＮＧ Ｘｉａｏｇｅｎｇ， ＣＡＯ Ｂｉｎｇｇａｎｇ， ｅｔ ａｌ． Ｓｐａｃｅ ｏｍｎｉｄｉｒｃｅｔｉｏｎａｌ ａｌｇｏｒｉｔｈｍ ｓｔｕｄｙ ｂａｓｅｄ ｏｎ ｑｕａｔｅｒｎｉｏｎ ［ Ｊ ］． Ｊｏｕｒｎａｌ ｏｆ Ｘｉ′ａｎ Ｊｉａｏｔｏｎｇ Ｕｎｉｖｅｒｓｉｔｙ， ２００６， ４０（ ５） ： ６１８⁃ ６２０． ［６］贾鹤鸣， 程相勤， 张利军，等． 基于自适应 Ｂａｃｋｓｔｅｐｐｉｎｇ 的 ＡＵＶ 的三维航迹跟踪控制［Ｊ］． 控制与决策， ２０１２， ５ （２７）： ６５２⁃６５７． ＪＩＡ Ｈｅｍｉｎｇ， ＣＨＥＮＧ Ｘｉａｎｇｑｉｎ， ＺＨＡＮＧ Ｌｉｊｕｎ， ｅｔ ａｌ． Ｔｈｒｅｅ⁃ｄｉｍｅｎｓｉｏｎａｌ ｐａｔｈ ｔｒａｃｋｉｎｇ ｃｏｎｔｒｏｌ ｆｏｒ ｕｎｄｅｒａｃｔｕａｔｅｄ ＡＵＶ ｂａｓｅｄ ｏｎ ａｄａｐｔｉｖｅ ｂａｃｋｓｔｅｐｐｉｎｇ［Ｊ］． Ｃｏｎｔｒｏｌ ａｎｄ Ｄｅｃｉ⁃ ｓｉｏｎ， ２０１２， ５（２７）： ６５２⁃６５７． 作者简介： 吴琪，男，１９８９ 年生，硕士研究生， 主要研究方向为水下机器人的运动控 制、路径规划以及数据融合技术 。 李晔，１９７８ 年生，教授，博士，主要 研究方向为水下机器人技术，主持国家 自然科学基金等国家和省部级以上项 目 １０ 余项，发表学术论文 ８０ 余篇。 第 ２ 期 吴琪，等： 基于四元数的欠驱动 ＡＵＶ 的镇定控制设计 ·１９１·
