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

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

To illustrate the motion of the helicopter , Pictured 1 A schematic diagram is shown in . In the working space of the helicopter , Define the world coordinate system and the body coordinate system . The world coordinate system represents a coordinate system that all discussions can refer to , The body coordinate system is a coordinate system attached to the helicopter . In order to keep the four rotor helicopter hovering , The four driving forces $F_i,i=1,2,3,4$ The size of the helicopter is adjusted to a quarter of the original helicopter weight . By changing the speed of four rotors at the same time , Can realize the body along $z$ The vertical movement of the axis . By reversing 1 Number and 3 Speed of rotor No , keep 2 Number and 4 Speed of rotor No , Can realize the body along $x$ Forward movement in axis direction . Reverse change 2 Number and 4 Speed of rotor No , keep 1 Number and 3 Speed of rotor No , The utility model can realize the following functions of the machine body $y$ The lateral movement of the shaft . The yaw motion is related to the difference between the moments produced by the rotor . For clockwise rotation , rotor 2 and 4 Speed should be increased , To overcome the rotor 1 and 3 The speed of . On the other hand , To rotate counterclockwise , rotor 3 and 1 The speed should be increased , To overcome the rotor 2 and 4 Speed of . surface 1 The symbol names used in this paper are summarized .

According to the Euler angle, the rotation transformation matrix between the world coordinate system and the body coordinate system can be obtained .

$$R_{EB}=\left[\begin{array}{ccc}\cos \theta \cos \psi & \sin \theta \cos \psi \sin \varphi -\sin \psi \cos \varphi & \sin \theta \cos \psi \cos \varphi +\sin \psi \sin \varphi \\ \cos \theta \sin \psi & \sin \theta \sin \psi \sin \varphi -\cos \psi \cos \varphi & \sin \theta \sin \psi \cos \varphi -\cos \psi \sin \varphi \\ -\sin \theta & \cos \theta \sin \varphi & \cos \theta \cos \varphi \end{array}\right]\text{\hspace{1em}(1)}$$

The original text is the above formula , There should be a sign error （ Red ）, The correct should be as follows
$$R_{EB}=\left[\begin{array}{ccc}\cos \theta \cos \psi & \sin \theta \cos \psi \sin \varphi -\sin \psi \cos \varphi & \sin \theta \cos \psi \cos \varphi +\sin \psi \sin \varphi \\ \cos \theta \sin \psi & \sin \theta \sin \psi \sin \varphi +\cos \psi \cos \varphi & \sin \theta \sin \psi \cos \varphi -\cos \psi \sin \varphi \\ -\sin \theta & \cos \theta \sin \varphi & \cos \theta \cos \varphi \end{array}\right]\text{\hspace{1em}(1)}$$

Then the velocity transformation between the body coordinate system and the world coordinate system is obtained
$$\left[\begin{array}{c}u\\ v\\ w\end{array}\right]=R_{EB}\left[\begin{array}{c}{u}_B\\ v_B\\ w_B\end{array}\right]\text{\hspace{1em}(2)}$$

Similarly , The acceleration 、 Rotation speed 、 Location 、 Both forces and moments can be based on $R_{EB}$ Convert between coordinate systems .

In the body coordinate system , Force is defined as
$$F_B=\left[\begin{array}{c}{F}_{xB}\\ F_{yB}\\ F_{zB}\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ {\sum }_{k=1}^4{F}_k\end{array}\right]\text{\hspace{1em}(3)}$$

In the world coordinate system , Force can be described as
$$\begin{array}{r}\left[\begin{array}{c}{F}_x\\ F_y\\ F_z\end{array}\right]=R_{EB}\cdot F_B=(\sum _{k=1}^4{F}_k)\left[\begin{array}{c}\sin \theta \cos \psi \cos \varphi +\sin \psi \sin \varphi \\ \sin \theta \sin \psi \cos \varphi -\cos \psi \sin \varphi \\ \cos \theta \cos \varphi \end{array}\right]\end{array}\text{\hspace{1em}(4)}$$

therefore ,UAV The movement equation in the world coordinate system is
$$m\left[\begin{array}{c}\ddot{x}\\ \ddot{y}\\ \ddot{z}\end{array}\right]=\left[\begin{array}{c}{F}_x-K_1\cdot \dot{x}\\ F_y-K_2\cdot \dot{y}\\ F_z-mg-K_3\cdot \dot{z}\end{array}\right]\text{\hspace{1em}(5)}$$

here $K_i,i=1,2,3$ It's the drag coefficient . Be careful , These coefficients are negligible at low speeds .

---

therefore , The equation of motion can be derived from the balance of force and moment .

$$\ddot{\varphi }=\frac{l(F_3-F_1-K_4\dot{\varphi })}{I_x}\text{\hspace{1em}(6)}$$

$$\ddot{\theta }=\frac{l(F_4-F_2-K_5\dot{\theta })}{I_y}\text{\hspace{1em}(7)}$$

$$\ddot{\psi }=\frac{(M_1-M_2+M_3-M_4-K_6\dot{\psi })}{I_z}\text{\hspace{1em}(8)}$$

here $l$ The length from the center of gravity of the UAV to each rotor ,$M_i,i=1,2,3,4$ Is the moment of the rotor ,$I_x,I_y,I_z$ Express $x,y,z$ Moment of inertia in direction . use $F_1,F_2,F_3,F_4$ To express ,（8） It can be rewritten as

$$\ddot{\psi }=\frac{(F_1-F_2+F_3-F_4-K_6^{{}^{\prime }}\dot{\psi })}{I_z^{{}^{\prime }}}\text{\hspace{1em}(9)}$$

here $I_z^{{}^{\prime }}=I_z/C,K_6^{{}^{\prime }}=K_6/C$,$C$ Is a scaling factor .

---

For ease of calculation TOMP problem , Define the input as
$$\begin{array}{rl}{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{array}\text{\hspace{1em}(10)}$$

The input can be expressed as a matrix
$$\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\\ u_4\end{array}\right]=\left[\begin{array}{cccc}1& 1& 1& 1\\ 0& -1& 0& 1\\ -1& 0& 1& 0\\ 1& -1& 1& -1\end{array}\right]\left[\begin{array}{c}{F}_1\\ F_2\\ F_3\\ F_4\end{array}\right]\text{\hspace{1em}(11)}$$

So the single force will be
$$\left[\begin{array}{c}{F}_1\\ F_2\\ F_3\\ F_4\end{array}\right]=\frac{1}{4}\left[\begin{array}{cccc}1& 0& -2& 1\\ 1& -2& 0& -1\\ 1& 0& 2& 1\\ 1& 2& 0& -1\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\\ u_4\end{array}\right]\text{\hspace{1em}(12)}$$

---

therefore , The dynamic equations of the four rotor helicopter are obtained

$$\ddot{x}=\frac{(\sin \psi \sin \varphi +\cos \psi \sin \theta \cos \varphi )u_1-K_1\dot{x}}{m}\text{\hspace{1em}(13)}$$

$$\ddot{y}=\frac{(\sin \psi \sin \theta \cos \varphi -\cos \psi \sin \varphi )u_1-K_2\dot{y}}{m}\text{\hspace{1em}(14)}$$

$$\ddot{z}=\frac{(\cos \varphi \cos \theta )u_1-K_3\dot{z}}{m}-g\text{\hspace{1em}(15)}$$

$$\ddot{\varphi }=\frac{(u_3-K_4\dot{\varphi })l}{I_x}\text{\hspace{1em}(16)}$$

$$\ddot{\theta }=\frac{(u_2-K_5\dot{\theta })l}{I_y}\text{\hspace{1em}(17)}$$

$$\ddot{\psi }=\frac{(u_4-K_6^{{}^{\prime }}\dot{\psi })}{I_z^{{}^{\prime }}}\text{\hspace{1em}(18)}$$

For hovering flight , Speed and roll angle 、 The pitch and yaw angles must be zero , And the four driving forces are $F_1(0)=F_2(0)=F_3(0)=F_4(0)=\frac{mg}{4}$. Substitute these conditions into the formula （13） To formula （18）, It can be found that there will be zero acceleration .

The above sentence further verifies that the formula derivation is correct .