Lai L C, Yang C C, Wu C J. Time-optimal control of a hovering quad-rotor helicopter[J]. Journal of Intelligent and Robotic Systems, 2006, 45(2): 115-135.

文章目录

1. Introduction
2. Dynamical Equations of a Quad-Rotor Helicopter

1. Introduction

2. Dynamical Equations of a Quad-Rotor Helicopter

为了说明直升机的运动，如图 1 所示给出了一个示意图。在直升机的工作空间内，定义世界坐标系和机体坐标系。世界坐标系表示一个所有讨论都可以参考的坐标系，而机体坐标系是一个附加到直升机上的坐标系。为了保持四旋翼直升机的悬停飞行，将四个驱动力 Fi,i=1,2,3,4F_i, i = 1,2,3,4Fi,i=1,2,3,4 的大小分别调整为最初直升机重量的四分之一。通过同时改变四个转子的转速，可以实现机体沿 zzz 轴的垂直运动。通过反向改变1号和3号转子的转速，保持2号和4号转子的转速，可以实现机体沿 xxx 轴方向的向前运动。反向改变2号和4号转子的速度，保持1号和3号转子的速度，可实现机体沿 yyy 轴的横向运动。偏航运动与转子产生的力矩之间的差异有关。为顺时针旋转，转子2和4应提高速度，以克服转子1和3的速度。另一方面，若要逆时针旋转，转子3和1应提高转速，以克服转子2和4的转速。表1总结了本文使用的符号命名。

根据欧拉角可以得到世界坐标系与机体坐标系之间的旋转变换矩阵。

REB=[cos⁡θcos⁡ψsin⁡θcos⁡ψsin⁡ϕ−sin⁡ψcos⁡ϕsin⁡θcos⁡ψcos⁡ϕ+sin⁡ψsin⁡ϕcos⁡θsin⁡ψsin⁡θsin⁡ψsin⁡ϕ−cos⁡ψcos⁡ϕsin⁡θsin⁡ψcos⁡ϕ−cos⁡ψsin⁡ϕ−sin⁡θcos⁡θsin⁡ϕcos⁡θcos⁡ϕ](1)R_{EB} = \left[\begin{matrix} \cos\theta \cos\psi & \sin\theta \cos\psi \sin\phi - \sin\psi \cos\phi & \sin\theta \cos\psi \cos\phi + \sin\psi \sin\phi \\ \cos\theta \sin\psi & \sin\theta \sin\psi \sin\phi \red{-} \cos\psi \cos\phi & \sin\theta \sin\psi \cos\phi - \cos\psi \sin\phi \\ -\sin\theta & \cos\theta \sin\phi & \cos\theta \cos\phi \\ \end{matrix}\right] \tag{1}REB=⎣⎡cosθcosψcosθsinψ−sinθsinθcosψsinϕ−sinψcosϕsinθsinψsinϕ−cosψcosϕcosθsinϕsinθcosψcosϕ+sinψsinϕsinθsinψcosϕ−cosψsinϕcosθcosϕ⎦⎤(1)

原文中是上式，应该有个符号错误（红色处），正确的应该如下
REB=[cos⁡θcos⁡ψsin⁡θcos⁡ψsin⁡ϕ−sin⁡ψcos⁡ϕsin⁡θcos⁡ψcos⁡ϕ+sin⁡ψsin⁡ϕcos⁡θsin⁡ψsin⁡θsin⁡ψsin⁡ϕ+cos⁡ψcos⁡ϕsin⁡θsin⁡ψcos⁡ϕ−cos⁡ψsin⁡ϕ−sin⁡θcos⁡θsin⁡ϕcos⁡θcos⁡ϕ](1)R_{EB} = \left[\begin{matrix} \cos\theta \cos\psi & \sin\theta \cos\psi \sin\phi - \sin\psi \cos\phi & \sin\theta \cos\psi \cos\phi + \sin\psi \sin\phi \\ \cos\theta \sin\psi & \sin\theta \sin\psi \sin\phi + \cos\psi \cos\phi & \sin\theta \sin\psi \cos\phi - \cos\psi \sin\phi \\ -\sin\theta & \cos\theta \sin\phi & \cos\theta \cos\phi \\ \end{matrix}\right] \tag{1}REB=⎣⎡cosθcosψcosθsinψ−sinθsinθcosψsinϕ−sinψcosϕsinθsinψsinϕ+cosψcosϕcosθsinϕsinθcosψcosϕ+sinψsinϕsinθsinψcosϕ−cosψsinϕcosθcosϕ⎦⎤(1)

然后得到机体坐标系与世界坐标系之间的速度变换
[uvw]=REB[uBvBwB](2)\left[\begin{matrix} u \\ v \\ w \\ \end{matrix}\right] =R_{EB} \left[\begin{matrix} u_B \\ v_B \\ w_B \\ \end{matrix}\right] \tag{2}⎣⎡uvw⎦⎤=REB⎣⎡uBvBwB⎦⎤(2)

类似地，加速度、旋转速度、位置、力和力矩都可以基于 REBR_{EB}REB 在坐标系之间转换。

在机体坐标系中，力被定义为
FB=[FxBFyBFzB]=[00∑k=14Fk](3)F_{B}= \left[\begin{matrix} F_{xB} \\ F_{yB} \\ F_{zB} \\ \end{matrix}\right]= \left[\begin{matrix} 0 \\ 0 \\ \sum_{k=1}^{4} F_k \\ \end{matrix}\right] \tag{3}FB=⎣⎡FxBFyBFzB⎦⎤=⎣⎡00∑k=14Fk⎦⎤(3)

在世界坐标系中，力可以描述为
[FxFyFz]=REB⋅FB=(∑k=14Fk)[sin⁡θcos⁡ψcos⁡ϕ+sin⁡ψsin⁡ϕsin⁡θsin⁡ψcos⁡ϕ−cos⁡ψsin⁡ϕcos⁡θcos⁡ϕ](4)\begin{aligned} \left[\begin{matrix} F_{x} \\ F_{y} \\ F_{z} \\ \end{matrix}\right]= R_{EB} \cdot F_B = (\sum_{k=1}^{4} F_k) \left[\begin{matrix} \sin\theta \cos\psi \cos\phi + \sin\psi \sin\phi \\ \sin\theta \sin\psi \cos\phi - \cos\psi \sin\phi \\ \cos\theta \cos\phi \\ \end{matrix}\right] \end{aligned} \tag{4}⎣⎡FxFyFz⎦⎤=REB⋅FB=(k=1∑4Fk)⎣⎡sinθcosψcosϕ+sinψsinϕsinθsinψcosϕ−cosψsinϕcosθcosϕ⎦⎤(4)

因此，UAV在世界坐标系中的移动方程为
m[x¨y¨z¨]=[Fx−K1⋅x˙Fy−K2⋅y˙Fz−mg−K3⋅z˙](5)m \left[\begin{matrix} \ddot{x} \\ \ddot{y} \\ \ddot{z} \\ \end{matrix}\right]= \left[\begin{matrix} F_{x} - K_1 \cdot \dot{x} \\ F_{y} - K_2 \cdot \dot{y} \\ F_{z} - mg - K_3 \cdot \dot{z} \\ \end{matrix}\right] \tag{5}m⎣⎡x¨y¨z¨⎦⎤=⎣⎡Fx−K1⋅x˙Fy−K2⋅y˙Fz−mg−K3⋅z˙⎦⎤(5)

这里 Ki,i=1,2,3K_i, i= 1,2,3Ki,i=1,2,3 是阻力系数。注意，这些系数在低速时可以忽略不计。

因此，可以根据力和力矩的平衡推导出运动方程。

ϕ¨=l(F3−F1−K4ϕ˙)Ix(6)\ddot{\phi} = \frac{l (F_3 - F_1 - K_4 \dot{\phi})}{I_x} \tag{6}ϕ¨=Ixl(F3−F1−K4ϕ˙)(6)

θ¨=l(F4−F2−K5θ˙)Iy(7)\ddot{\theta} = \frac{l (F_4 - F_2 - K_5 \dot{\theta})}{I_y} \tag{7}θ¨=Iyl(F4−F2−K5θ˙)(7)

ψ¨=(M1−M2+M3−M4−K6ψ˙)Iz(8)\ddot{\psi} = \frac{(M_1 - M_2 + M_3 - M_4 - K_6\dot{\psi})}{I_z} \tag{8}ψ¨=Iz(M1−M2+M3−M4−K6ψ˙)(8)

这里 lll 无人机重心到各旋翼的长度，Mi,i=1,2,3,4M_i, i=1,2,3,4Mi,i=1,2,3,4 是旋翼的力矩，Ix,Iy,IzI_x, I_y, I_zIx,Iy,Iz 表示 x,y,zx,y,zx,y,z 方向的惯性矩。用 F1,F2,F3,F4F_1, F_2, F_3, F_4F1,F2,F3,F4 来表示，（8）可以改写成

ψ¨=(F1−F2+F3−F4−K6′ψ˙)Iz′(9)\ddot{\psi} = \frac{(F_1 - F_2 + F_3 - F_4 - K_6^{'} \dot{\psi})}{I_z^{'}} \tag{9}ψ¨=Iz′(F1−F2+F3−F4−K6′ψ˙)(9)

这里 Iz′=Iz/C,K6′=K6/CI_z^{'} = I_z /C, K_6^{'}=K_6/CIz′=Iz/C,K6′=K6/C，CCC 是一个缩放因子。

为方便计算 TOMP 问题，将输入定义为
u1=F1+F2+F3+F4u2=−F2+F4u3=−F1+F3u4=F1−F2+F3−F4(10)\begin{aligned} u_1 =& F_1 + F_2 + F_3 + F_4 \\ u_2 =& - F_2 + F_4 \\ u_3 =& - F_1 + F_3 \\ u_4 =& F_1 - F_2 + F_3 - F_4 \\ \end{aligned} \tag{10}u1=u2=u3=u4=F1+F2+F3+F4−F2+F4−F1+F3F1−F2+F3−F4(10)

输入可以表示成矩阵的形式
[u1u2u3u4]=[11110−101−10101−11−1][F1F2F3F4](11)\left[\begin{matrix} u_1 \\ u_2 \\ u_3 \\ u_4 \\ \end{matrix}\right]= \left[\begin{matrix} 1 & 1 & 1 & 1 \\ 0 & -1 & 0 & 1 \\ -1 & 0 & 1 & 0 \\ 1 & -1 & 1 & -1 \\ \end{matrix}\right] \left[\begin{matrix} F_1 \\ F_2 \\ F_3 \\ F_4 \\ \end{matrix}\right] \tag{11}⎣⎢⎢⎡u1u2u3u4⎦⎥⎥⎤=⎣⎢⎢⎡10−111−10−11011110−1⎦⎥⎥⎤⎣⎢⎢⎡F1F2F3F4⎦⎥⎥⎤(11)

那么单个的力将是
[F1F2F3F4]=14[10−211−20−11021120−1][u1u2u3u4](12)\left[\begin{matrix} F_1 \\ F_2 \\ F_3 \\ F_4 \\ \end{matrix}\right]= \frac{1}{4} \left[\begin{matrix} 1 & 0 & -2 & 1 \\ 1 & -2 & 0 & -1 \\ 1 & 0 & 2 & 1 \\ 1 & 2 & 0 & -1 \\ \end{matrix}\right] \left[\begin{matrix} u_1 \\ u_2 \\ u_3 \\ u_4 \\ \end{matrix}\right] \tag{12}⎣⎢⎢⎡F1F2F3F4⎦⎥⎥⎤=41⎣⎢⎢⎡11110−202−20201−11−1⎦⎥⎥⎤⎣⎢⎢⎡u1u2u3u4⎦⎥⎥⎤(12)

因此，得到了四旋翼直升机的动力学方程

x¨=(sin⁡ψsin⁡ϕ+cos⁡ψsin⁡θcos⁡ϕ)u1−K1x˙m(13)\ddot{x}= \frac{(\sin \psi \sin \phi + \cos \psi \sin \theta \cos \phi) u_1 - K_1 \dot{x}}{m} \tag{13}x¨=m(sinψsinϕ+cosψsinθcosϕ)u1−K1x˙(13)

y¨=(sin⁡ψsin⁡θcos⁡ϕ−cos⁡ψsin⁡ϕ)u1−K2y˙m(14)\ddot{y}= \frac{(\sin \psi \sin \theta \cos \phi - \cos \psi \sin \phi) u_1 - K_2 \dot{y}}{m} \tag{14}y¨=m(sinψsinθcosϕ−cosψsinϕ)u1−K2y˙(14)

z¨=(cos⁡ϕcos⁡θ)u1−K3z˙m−g(15)\ddot{z}= \frac{(\cos \phi \cos \theta) u_1 - K_3 \dot{z}}{m} -g \tag{15}z¨=m(cosϕcosθ)u1−K3z˙−g(15)

ϕ¨=(u3−K4ϕ˙)lIx(16)\ddot{\phi} = \frac{(u_3 - K_4 \dot{\phi})~ l}{I_x} \tag{16}ϕ¨=Ix(u3−K4ϕ˙) l(16)

θ¨=(u2−K5θ˙)lIy(17)\ddot{\theta} = \frac{(u_2 - K_5 \dot{\theta}) ~l}{I_y} \tag{17}θ¨=Iy(u2−K5θ˙) l(17)

ψ¨=(u4−K6′ψ˙)Iz′(18)\ddot{\psi} = \frac{(u_4 - K_6^{'} \dot{\psi})}{I_z^{'}} \tag{18}ψ¨=Iz′(u4−K6′ψ˙)(18)

对于悬停飞行，速度和横摇角、俯仰角和偏航角必须为零，且四个驱动力为 F1(0)=F2(0)=F3(0)=F4(0)=mg4F_1(0)=F_2(0)=F_3(0)=F_4(0)=\frac{mg}{4}F1(0)=F2(0)=F3(0)=F4(0)=4mg。将这些条件代入式（13）至式（18），可以发现将产生零加速度。

上一句话进一步验证了公式推导的是正确的。

【Paper】2006_Time-Optimal Control of a Hovering Quad-Rotor Helicopter相关推荐

【Paper】2019_Distributed Optimal Control of Energy Storages in a DC Microgrid with Communication Dela
M. Shi, X. Chen, J. Zhou, Y. Chen, J. Wen and H. He, "Distributed Optimal Control of Energy Sto ...
【Paper】2010_Distributed optimal control of multiple systems
Dong W. Distributed optimal control of multiple systems[J]. International Journal of Control, 2010, ...
【Paper】2015_El H_Decentralized Control Architecture for UAV-UGV Cooperation
Decentralized Control Architecture for UAV-UGV Cooperation 1 Introduction 2 Problem Statement and Ar ...
【Paper】2020_Distributed optimal consensus with obstacle avoidance algorithm of mixed-order UAVs
Yang X, Wang W, Huang P. Distributed optimal consensus with obstacle avoidance algorithm of mixed-or ...
【Paper】2019_Bearing-only circumnavigation control of the multi-agent system around a moving target
Yu Y, Li Z, Wang X, et al. Bearing-only circumnavigation control of the multi-agent system around a ...
【Paper】2019_Distributed Cooperative Control of a High-speed Train
2019_Distributed Cooperative Control of a High-speed Train 文章目录 1. Introduction 2. Modeling of a hig ...
【Paper】2015_Active fault-tolerant control system design with trajectory re-planning against actuator
Chamseddine A, Theilliol D, Zhang Y M, et al. Active fault‐tolerant control system design with traje ...
【Paper】2022_Adaptive Formation Control of Unmanned Underwater Vehicles with Collision Avoidance unde
Yan Z, Jiang A, Lai C. Adaptive Formation Control of Unmanned Underwater Vehicles with Collision Avo ...
【Paper】2019_Consensus Control of Multiple AUVs Recovery System Under Switching Topologies and Time D
Zhang W, Zeng J, Yan Z, et al. Consensus control of multiple AUVs recovery system under switching to ...
【Paper】2021_Distributed Consensus Tracking of Networked Agent Systems Under Denial-of-Service Attack
Y. Wan, G. Wen, X. Yu and T. Huang, "Distributed Consensus Tracking of Networked Agent Systems ...

【Paper】2006_Time-Optimal Control of a Hovering Quad-Rotor Helicopter

文章目录

1. Introduction

2. Dynamical Equations of a Quad-Rotor Helicopter

【Paper】2006_Time-Optimal Control of a Hovering Quad-Rotor Helicopter相关推荐

最新文章

热门文章