笔记-NonlinearQuadrocopterAttitudeControl

1、非线性四元数姿态控制摘要这篇文章解决了四旋翼非线性姿态控制的问题。一个基于单位四元数的全局姿态稳 定控制器被提出，而且它的稳定性和鲁棒性都得到了证明。另外，一个优先考虑关键的 滚转和俯仰角度的控制再考虑偏航控制的方法被引入进来。最终，对这个被提出的非线 性控制器进行实验，并论证了它的性能。.背景适用于小角度线性化的控制器已经不适用于高机动性，高性能的要求。为了挖掘四 旋翼飞行器的所有潜能，全局角度控制器应该被采用。欧拉角产生的奇异点也应该避免， 因为从纯数学的角度它限制了飞行器一些可能的动作，不适用于全天候角度控制。而四 元数和余弦矩阵可以做到这一点。这篇文章的内容为：第二节提供了关于姿态表示

2、和四轴飞行器设置的背景材料。第三节提出了全局渐近稳定控制了律。最后，所提出控制器的实验结果见第四节，报告总结见第五节。.预备知识角度表示刚体的角度可以用从参考坐标系到机体坐标系的旋转表示。 这些旋转参数可以是欧 拉角（三个参数），单位四元数（四个参数）或旋转矩阵（九个参数）。而，所有的三参 数表示法会产生奇异点，所有的k参数表示法会有（k-3）个约束。旋转矩阵旋转矩阵Rbi是一个从一个线性空间 山_ 3到另一个线性空间BLL3的线性变换， 同时保留长度和右手螺旋准则。由以上特性，可以直接得出Rbi亡SO(3)。设正交基M W I 和eB w b ,其中e1 =&,封)(1)向量，在坐标系I和B

3、下的坐标分别是力和七。那么，从向量I到向量七的线性变 换Rbi满足下式:rB = RBIrI而Rbi是一个3 M3的矩阵BIBI BI BIRBI = (rx ryrz )矩阵Rbi的三个列向量是坐标系I的单位正交基在坐标系B中的表示。同理，该矩阵的三个行向量是是坐标系B的单位正交基在坐标系I中的表示。2.1.2欧拉角欧拉角基于以下事实：可以通过连续地应用三个基本旋转矩阵来构造任何旋转矩阵。然后可以通过三个基本旋转的旋转角度来参数化旋转矩阵的九个元素。一个用于飞行器的非常普遍的旋转顺序是ZYX顺序。如图1所示图1 ZYX顺序的欧拉角.绕初始I系中z轴旋转的角为偏航角中(I系T K系)。.绕K系

4、中y轴旋转的角为俯仰角0 (K系TL系)。.绕L系中x轴旋转的角为滚转角* ( L系t B系)。因此Rbi可表小为 TOC o 1-5 h z Rbi =RblRlkRki(4)100IJ0-SCy Sr0|=0cs010即即0(5)0-Scs00CQ001一c四旃-se:= -c砰 + s4s0cg c 唧 + s 苹觌s 炉e(6) s帆 + cjs-sjQg + 冲觎 c炉8 一其中c(.)和s(.)分别是cos(.)和sin(.)的缩写。从旋转矩阵到欧拉角的逆映射是仁二arctan2(R23, R33)(7)a a arcsin(R3)(8)中=arctan2(R2, R11)(9)其

5、中，Rj表示矩阵Rbi中的第i行，第j列的元素。2.1.3单位四元数一人、一一一E ，k，、八*，、每个旋转行为都可以用由单包向重k和旋转角u描述的绕一个固止轴的一次旋转进行参数化表示。这被称为特征轴旋转，它体现了两个方向之间的最短旋转。在此基础上，定义了一个单位四元数_ cOs 一1T ,q0 1 I2q=qq1q2q3 =1= 4,、(10)q1:3 电 )-ksin-12人四元数q的伴随，范数和逆分别是0q1:3 J|q| = Jq2 +q； +q2 +q2两个四元数p和q的乘法可定义为q p =Q(q) p其中q-牝-q31q1qO73q2q2q3q。7*-q2q1q。-Q(q)=注意

6、，对应于旋转矩阵I的四元数由下式给出-11qi向量r通过四元数q旋转的计算公式如下p(r)=q p(r) q其中，rr是旋转后的向量，p（.）是一个向量的四元数表示:o;-TGp从欧拉角到单位四元数的ZYX顺序的映射为一与2（2屯2十射222即/2S/2C-/2C./2 C/2j2 s.72C/2SN2C./2 S/2J/2S.722c% 即/2 S/ S&2 Cg/22.1.4角度表示附录旋转矩阵和单位四元数都提供了角度的非奇异的特征表示。但是，需要说明的是， 单位四元数空间S3两倍覆盖了物理角度空间SO(3) o因此，单位四元数不是唯一的。实 际上，每对相反的单位四元数 土qwS3正对应相

7、同的姿态。对于空间 S3中的姿态控制任 务，这意味着必须设计一个控制器来稳定一组断开的平衡点。然而，单位四元数经常被 选择作为姿态控制的参数，因为他们是最小的全局非奇异参数。另外，单位四元数可以 让我们轻松地理解几何级别上发生的情况。2.2四旋翼飞行器卜面，我们考虑一个具有机体坐标系和惯性坐标系的四旋翼无人机，如图2所示图2机体坐标系B和惯性坐标系I下的四旋翼无人机这个四旋翼的输入是四个电机的拉，输出是位置，速度，加速度以及机体的姿态和角速 率。因为相比于输入模型由更多的输出，所以显而易见，并不是所有的输出能够被独立 地控制。在FMA中，使用一个级联控制结构来控制四旋翼，它将位置控制作为最外环

8、 控制(图3)。这个控制方式的内外环时间尺度是分离的，只要内环明显快于外环，它就 是有效的。在这篇报告中，我们只考虑角度控制环。假设机上控制回路和四旋翼的动态 特性非常快以至于被控系统可以被看做一个以期望角速率 A作为直接输入的刚体。那么四旋翼动态模型可以由下式表示1IJ:3O 1q V-5 21 、, 2(S(q1：3)+qI p(20)其中S（q：3）表示qi：3的斜矩阵S( qi:3) -一。q3.一q2-q30qiq2q10(21)FeedforwardJIDesiredTrajectoryPositionControllerOnboard ControllerAttitude Con

9、troLler以匕血duadrocaptcrState feedback (position, velocity： acceleration, attitude, angular body rate)图3级联控制结构框图证明:由于载体的运动，四元数 q是变量，即*q1,q2,q3是时间的函数。这里设刚体绕瞬时转轴n转过日角，其角速度为(22)设这个机体坐标系（b系）和惯性坐标系（n系）之间的变换四元数的三角形式为e e 4q = cos sin n22(23)对公式（22）进行求导，得dq1 , - d-1-d -4二-sin cos ndt22 dt22dt这里假设旋转轴未变化，即 如=0,

10、则 dtsin-电 2 dt(24)(25)(26) TOC o 1-5 h z dq1 ch1cbiq =二-sin -cosndt22dt22dt又因为q为单位四元数，所以n n=_i,则有dq1cd1c d4=-sin -cosn dt22dt22 dtdi f f*n cos sin - ndt22 1 n二 2-nb至此，公式(20)前半部分得证，后半部分将q和p展开相乘即可得到3.姿态控制设计首先第一步，位置环产生的期望加速度和期望偏航角必须转换成期望的姿态qcmd。之后的目标就是设计一个能够将四旋翼稳定在任意期望的物理姿态角的反馈法则。因为在空间SO(3)中的任意物理角度对应空间

11、S3中的两个对映四元数，可以通过将q稳定在 士qcmd上实现稳定性。如果忽视这个因素，基于四元数的控制器可能会导致不良现象比 如，unwinding,刚体不必要的全旋转(a full rotation) 0为了解决unwinding的问题，控 制器必须满足cmd(q) ”cmd(-q)(27)显而易见，因为q和-q表示相同的物理角度，控制器的输出也应该相同。这个问题 可以通过改变q0 0时q的符号解决。但是改变了符号会导致控制器不连续。有趣的是， 这不是一个弊端，因为可以证明所有的连续状态反馈控制法则至多是几乎全局稳定。控制法则考虑下面的控制法则：21， qe,0 - 0cmd(q) = sg

12、n(qe，0)qe,1：3，sgn(qe，0)(28)-1，qe,0 : 0其中E是一阶系统时间常数S,qe =q,，qcmd为衡量误差，表示从q到q0md的旋转。然后，土qcmd是一个全局渐进稳定平衡点证明：不失一般性，设置qcmd =qI。定义一个自主的混合装置，H =(Z,Q, f ,Init,Dom, E,G,R),其中Z=z1,z2,两个离散状态，对映空间S3的上下两个半球。Q=S3u14,连续状态q,位于三位球面上，表示目前的角度。向量场1,、-q p -1cmd(z,q) , if z = 4f(z,q)=、.(29)1-q p -1cmd(z,q) , if z = z22其中

13、 TOC o 1-5 h z 224qe,i：3 二 一 qi：3,if z = zimd(z,q)= 。(30)22 qe,1:3 =-q：3, if z = z2LTT上述公式分别表示四元数在上半球 z1或下半球z2时，q随时间的变化率。域 Dom(z1) = Dom z2 = S3。边界E =(42),(22,乙),表示从4到z2的可能性，反之亦然。保护函数 G(4,z2)S3 q0 o,G(z21z1) q = S3 q0 0 Vq,(z,q) w Dom(z)q，*V(z,q) =(z,q) f (z,q) W0 Vq,(z,q) w Dom(z)。二 q此外，V(z,q)0 Vz=

14、 Z , Vqw Dom(z)qJ而且 V(z,q ) = 0 Vz= Z 0 我们注意 到当z改变状态时V(z,q)并不跳变，因此V(z,q)是严格递减的，这意味着q是一个 全局渐进稳定平衡点。备注1.偏差qe表示从当前角度q到期望角度qcmd的旋转。通过取qe的符号，可以 保证旋转角总是小于或等于180,。这就意味着控制器总是以最小旋转角旋转刚体。 但是注意，一个最小的旋转角度通常并不意味着一个时间最优的旋转操作。如果旋 转角速度被限制这是显而易见的。例如，|Qj EQ, i wx,y,z andfor i # j。备注2.控制法则（28）关于qcmd =qi的形象解释如图4所示。图4:上

15、述控制法则将角度稳定在 %=1或%=1。但是，在q0=0处有一个间断点如果不考虑qe,。的符号，控制器将只能是几乎全局渐进稳定，因为 q = -1是一个不稳定平衡点。备注3.由定义知，何=1, VqwS3,因此，辰：M1。则最大可能输出被限制为 15/_2。这对于饱和的系统尤其具有优势。+ q； +q； Iqoq1一qq2-qoq3备注4.不失一般T假设qcmd =q 。将控制法则（28）代入状态方程（20）中(32)(33)忽略平衡点q的小偏差（q。比1,勺：3 1_ 1）,式（32）可简化为飞。10 1一qq1q2一qq2-qoq31这是一个无耦合的时间常数为 七的一阶系统。备注，七表示四

16、元数误差衰减到 1/e的时间，而不是欧拉角误差衰减到 1/e的时间。3.1.1鲁棒性尽管所提出的控制器实现了任何所需姿态的全局渐近稳定性,但可以证明控制器对于任意小的测量噪声不稳健。通过参考文献8的严密证明，可以构建一个噪声信号qnoise,使得当q任意地开始任意接近不连续性时，能始终保持接近不连续性。令当前角度表示为仅)1 cos 一I12)ik sin 1 2 I2人(34)定义qnoise为qnoise (q)losgI 2 Jksin(酗)-I 2力(35)其中k是角度q的本征轴，而且-P：(q) =|-p0,0，0,f 0 0( Hf n ot -nf n 之a 2冗with 0 0

17、, Vm(M) = 0,然后，当a|wB0时，Vm (q) = 2qq。1q0 qi:：3qi:3(39) =|q0|kTksin；sini工I 2 J 0因此，从任何在卜-可W P范围内开始的角度，系统不会收敛到期望角度。？ ？ ？在文献8中，提出了一个带有亢奋记忆状态的不连续控制器能够避免这种有害的平 衡。这种亢奋记忆状态定义了一个旋转方向，他可以使机体沿着最短过渡的方向旋转。 如果|na|EB。，亢奋记忆状态没有获得更新旋转状态仍然保持目前状态，则会留下一 个任意小的噪声信号都有可能使控制器不稳定的区域。在实践中，如果控制法则用在一 个离散时间控制器上，亢奋记忆状态是不需要的。由于离散时

18、间控制器的输出在两次更 新之间是不变的，旋转方向在这个时间段也是不变的，因此离散时间控制器的行为就像 带滞后的连续时间控制器。期望姿态考虑图2所示的带有机体坐标系B的四旋翼飞行器，可以很明显地看出，四旋翼仅 仅在能方向有加速度。因此，期望加速度 Icmd必须转化为目标定位qcmd, Z轴才能与期 望加速度方向对齐。与每个指向应用一样，围绕指向方向的旋转是无关紧要的，比如， 关于推力方向段的旋转对四旋翼的平移行为没有影响。因此，把控制任务分为两部分是 合理的：简化的姿态控制：只有推力的关键指向方向是受控制的。偏航角不被直接控制，但 是始终选择qcmd都不会引起围绕偏航轴的旋转完整的姿态控制：推力

19、变量的指向和偏航角都是受控制的。选择 qcmd所以对应的z轴和既取是一致的，而且中小cmd简化的姿态控制给定任意期望加速度Imd ,期望推力和方向都能计算得到(40)(41)1MB _ Iacmd0mdcmdCOllcmd = IScmd11简化的偏差四元数qe,red（图5）是将四旋翼从当前姿态旋转到期望姿态，公式如下qe,red 值Yicos 一I 12H fa ) k sin I 22)f 0()cos I 一2sin - 12 MleB BIe Ie -Ieze cmd, zzB Ie(42)(43)(44)其中IeB是当前姿态q对应的z轴，口是当前推力方向和期望推力方向的夹角:=ar

20、ccos IezB - IeLhz最终，期望姿态可由下式得出qcmd,red q qe, red注：qe,red的最后一个元素总是为零，因为k 1 6 ,这表明Ccmd,z=0。由于欧拉角的结构，这并不意味着偏航角 中是恒定的。3图5 qe,red的几何表示完整的姿态控制一个期望偏航角中,和推力方向&，：可以完整的表示姿态。俯仰和滚转角，“md和%d,可以通过将eL,z映射到中间坐标系K和L上得到。首先，将期望推力方向向量言，工在K坐标系中表示，即沿着初始Z轴旋转mdKecmd ,z二 RKI lecmd,zCOS# cmd )sin(中 cmd )0Sin(- cmd)cos( cmd )0

21、011(45)(46)(Kecmd ,z,3 J期望俯仰角8cmd可以通过把方向向量e，z投影到34平面进行重构（图6）： am,、，使用公式（17）进行限制。首先，计算旋转轴 Blezlecmd ,z Ie(54)然后，定义倾斜旋转为cJ%iit cos (55)(56)12)Ik sin i%t-I 2人坐落在孔径为21 max的椎体中的新的目标推力方向是BB片P lenew cmd,z 一 qtiltP lecmd ,zqtilt值得一提的是，这并不能阻止四轴飞行器翻转过来。虽然现在可以到达的角度收到 了限制，飞行器还是可能通过一些外部的动力旋转到一些极端的角度。然后，恢复到命 令姿态的

22、最短角度旋转仍然可以涉及到执行翻转。启发式算法*前面提到的控制法则（28）总是通过尽可能短的旋转将无人机稳定在期望的方向。 这是一个非常令人满意的结果，但是，它假设角速度A可以被直接地，无穷快地控制，这在实践中是不可能的。想象飞行器有几乎180一偏航误差的情况。此时，飞行器无论绕k轴旋转还是绕-k轴旋转并没有大的区别。但是，如果向很大，很明显沿着和Cz相 同的方向旋转比沿着相反的方向加速要快得多。旋转时间越长越快与否的问题，并不是 微不足道的？ ？。实际上，角速度 8的上升时间依赖于多个参数，比如，推力大小和角 速度的值，但是不存在解决这个问题的代数表达式。为了保持小的计算量，这里简单 的启发式的方法。之0,显然什么都不用改这里，只引入偏航角误差的启发式算法，但是，偏航角和滚转角使用它的工作原理 是一样的。假设思四旋翼的角速度为 Q o qmix的旋转轴是:;0勤1或:二0-1丁。如