In this chapter, we will explore the kinematics of a quadrotor drone. This chapter will talk about coordinate transformation (Transformation), rotation (Rotation), Euler Angles (Euler Angles), angle-axis rotation representation (Axis/Angle Representations for Rotations), angular velocity (Angular Velocity) and other knowledge in robotics.

Previous chapter link:

Robotics: Aerial Robotics (Aerial Robot) Notes (2): UAV Energy and System Design

Coordinate transformations (Transformations)

In this part, we will gain a deep understanding of the basic concepts required to describe the motion of a rigid body in 3D and the basic tools for describing displacement in 3D.

reference frame

First, let's talk about the reference coordinate system:

Because it is to describe the motion of a three-dimensional rigid body, the reference coordinate system is three mutually orthogonal (although not necessarily orthogonal, but orthogonal will be more convenient) vectors. Usually for quadrotor drones, these three directions are generally shown in the figure, one is the forward direction of the drone, one is perpendicular to the forward direction, and the last one is the intersection plane perpendicular to these two axes pointing upward.

Then the speed of the quadrotor UAV can be determined:

Rigid Body Displacemen

When this rigid body is displaced, the point $p$ is displaced to a new point, which we call $g(p)$ the displacement, which is essentially the transformation of the point. Of course, if there is a point $q$ , then the displaced point is $g(q)$ . The vector $p$ obtained after displacement is , because one input is a point and the other is a vector, so the names are different. $q$ $v$ $g_*(p)$

So what are the characteristics of the displacement of a rigid body?

(1) The distance between any two points in the rigid body remains unchanged, which is easy to understand:

(2) The outer product of any vector in the rigid body is equal to the outer product of the displaced vector after displacement:

(3) Orthogonal vectors also map to each other:

(4) The inner product also satisfies:

Rotation Matrix:

We introduce a reference coordinate system (vectors are orthogonal to each other) in the rigid body displacement, so how to describe these two reference coordinate systems?

We can know through the study of linear algebra that each vector can be described by mutually orthogonal vectors, in three-dimensional space:

Among them, such a matrix is defined as a rotation matrix:

Properties of a Rotation Matrix (Properties of a Rotation Matrix):

(1) The rotation matrix is an orthogonal matrix , that is, multiplying it with its transpose is the identity matrix.

(2) Its determinant is equal to 1.

(3) The multiplication of any two rotation matrices is also a rotation matrix, that is, when the rigid body moves multiple times, we only need to multiply each rotation matrix to get the final coordinate transformation.

(4) The inverse matrix of the rotation matrix is also a rotation matrix. Let me mention here a little bit. If we rotate a matrix, we need to restore it back. In fact, the rotation matrix that is rotated back is the original inverse matrix (turning back is the inverse ) . The transpose of an orthogonal matrix is its inverse, so the inverse of a rotation matrix is its transpose.

So how should the point after rigid body displacement $g(Q)$ be described?

We put the displaced $Q$ (called $Q'$ ) into the reference coordinate system before the displacement, and the distance from the origin to the reference coordinate system in the reference coordinate system $Q'$

A vector of can be described as:

in:

Rotation

We discussed the rotation matrix before, the rotation matrix is a 3X3 matrix, which is an orthogonal matrix, and the value of the determinant is 1.

These types of matrices form special orthogonal groups. We refer to this group by SO(3) . It refers to the special orthogonal in three dimensions

群（Special Orthogonal group in 3 dimensions ）：

There are actually many ways to describe rotation. The rotation matrix is one of them, but it is not intuitive enough. Next we will talk about some other methods of describing rotation, some of which will not be mentioned in this course:

(1) Rotation matrices

(2) Euler angles

（３）Axis angle parameterization

(4) Index coordinates (Exponential coordinates, do not speak)

(5) Quaternions (Quaternions, do not speak)

It's also mentioned here that we often use latitude and longitude to describe a point on the Earth's surface. That is to say, there is a one-to-one correspondence between this point and the longitude and latitude of this point. There is no other point that can be described by this pair of longitude and latitude, and this pair of longitude and latitude can only refer to this point. But in fact not all points can be described by a unique latitude and longitude, these points are the north and south poles of the earth. The poles have a unique latitude, but the longitude is not well defined. In fact, any longitude can be used to describe the North or South Pole.

Why make this point in particular? This will be covered later.

Euler Angles _

Euler proved that three coordinates are required to describe a general rotation, and these coordinates are called Euler angles. If we want to describe the rotation from the A coordinate system to the D coordinate system $^{A}R_D$ , we can decompose it into three processes, which are the rotation $x$ angle of the A coordinate system around the axis $\psi$ , the $y$ rotation angle around the axis $\phi$ , and $z$ the rotation angle around the axis $\theta$ . These three angles It is the roll angle (roll), the pitch angle (pitch) and the yaw angle (yaw) , and we call this description XYZ Euler angle :

There is another way to describe it, which we call ZYZ Euler angles $z$ . In fact, it rotates around the axis first $\phi$ , then around $y$ the axis $\theta$ , and finally around $z$ the axis $\psi$ :

We found that when $\theta$ it is not 0, $z$ the axes of the other two rotations are not collinear, and the combination of these three rotations can correspond to a specific rotation one-to-one. But $\theta$ when it is 0, the other two $z$ axes of rotation are collinear, and the situation at this time is similar to the north and south poles of the earth. For example, when $\phi$ = $\psi$ = 45 degrees, the actual step is $z$ rotated 90 degrees around the axis, so in fact, it can also be described by $\phi$ = 30 degrees and $\psi$ = 60 degrees, which means that the description of this rotation combination is not unique.

Let's take a closer look, the rotation matrix corresponding to the ZYZ Euler angle is as follows:

How to push it, here I use matlab code to demonstrate:

syms psi theta phi real; %定义三个角度的符号变量并设置成实数

R1 = [cos(phi) -sin(phi) 0; sin(phi) cos(phi) 0; 0 0 1];
R2 = [cos(theta) 0 sin(theta); 0 1 0; -sin(theta) 0 cos(theta)];
R3 = [cos(psi) -sin(psi) 0; sin(psi) cos(psi) 0; 0 0 1];

simplify(R1*R2*R3)

The result is the above formula. As for why R1, R2, and R3 are set in this way, it is recommended to see the following link:

3D rotation matrix derivation and mnemonics

Interested students can also try to use the code to see what the rotation matrix of the XYZ Euler angle looks like.

If we already know the specific value of the rotation matrix, then we can get the value through the green box , and then we can get the value through the blue box , and at the same time we can get it through the yellow box Value: The calculation formula at that time is as follows: $R_{33}$ $\theta$ $\psi$ $\phi$ $\left | R_{33}\right | <1$

Note that the atan2 function is used here. This is because if you use an ordinary arctangent, inputting a tangent value will cause ambiguous results, which can be avoided by using the atan2 function:

At that time , we will find that each element in the rotation matrix is actually a summation formula of sine and cosine angles, so the rotation matrix at this time is actually a function about + . Assuming + =90, then there are countless combinations of sums that satisfy this condition. Just like what we said before when the ZYZ Euler angle rotates around the axis is 0, the axes of the other two rotations are collinear, so the description of this rotation combination is not unique. $\left | R_{33}\right | =\pm 1$ $\phi$ $\psi$ $\phi$ $\psi$ $\phi$ $\psi$ $\phi$ $\psi$ $y$ $\theta$ $z$

Finally, it was mentioned that the commonly used Euler angle description method for quadrotor drones used in their laboratory is ZXY Euler angle, as shown in the figure below:

Angle-axis rotation representation (Axis/Angle Representations for Rotations)

Now let's look at another way of describing rotation, this way we can describe a rotation given an axis of rotation and an angle of rotation.

Assuming that there is a point O on the rigid body, if the point O remains unchanged after the displacement, it is equivalent to the rigid body rotating around the axis passing through the point O.

How to prove this point?

Through the previous discussion, we can know that a certain point after the rigid body has been rotated can be represented as follows:

So we have two 3 X 1 vector sums $\textbf{p}$ , $\textbf{q}$ related to the rotation matrix:

So what we want to know is: is there a point $\textbf{p}$ that remains unchanged after R's rotation? That is:

Students who have studied linear algebra in this form will be familiar with it. Isn’t this the eigenvector and eigenvalue:

So when the eigenvalue, that is, $\lambda=1$ is the solution to this problem, that is to say, the result exists, that is, the direction and size of p remain unchanged after rotation, which is equivalent to rotating around the passed $\textbf{p}$ axis.

Next, let's think in reverse, given a specific axis $\textbf{u}$ and rotation direction $\phi$ , how do we calculate the rotation matrix? As shown in the figure below, we create a $\textbf{u}$ vector perpendicular to the rotation axis $\textbf{v}$ , and $\textbf{p}$ decompose it into the amount of these two vectors, then $\textbf{u}$ the component in the direction is $(\textbf{p} \cdot \textbf{u})\textbf{u}$ (because the dot product is to find the size of the projection of a vector in another direction). The component in $\textbf{v}$ the direction is $\textbf{p}$ itself minus $\textbf{u}$ the component in the direction $(\textbf{p} \cdot \textbf{u})\textbf{u}$ . Since it is in three-dimensional space, we also built an $\textbf{w}=$ $\textbf{u}$ X $\textbf{v}$ axis, which means it is orthogonal to the other two axes:

Next, we can consider the situation around a specific axis $\textbf{u}$ and rotation $\phi$ . The related formulas are shown in the figure below. Let’s deduce them one by one:

The first is the vector after the vector $\textbf{v}$ is $\textbf{u}$ rotated, we can use its components $\textbf{w}$ on $\textbf{v}$ the sum to get, that is:

$\text{v}cos\phi + \text{u} \times \text{v}sin\phi$

Then $\textbf{p}$ the projection of the vector on this vector is to $\textbf{p}-(\textbf{p}\cdot \textbf{u})\textbf{u}$ substitute into $\textbf{v}$ :

$(\textbf{p}-(\textbf{p}\cdot \textbf{u})\textbf{u})cos\phi + \textbf{u} \times (\textbf{p}-(\textbf{p }\cdot \textbf{u})\textbf{u})sin\phi$

Since the components of the vector $\textbf{p}$ on $\textbf{u}$ the axis $(\textbf{p}\cdot \textbf{u})\textbf{u}$ will not change after rotation, $\textbf{p}$ the vector after the rotation of the vector is the addition of these two vectors:

$(\textbf{p}-(\textbf{p}\cdot \textbf{u})\textbf{u})cos\phi + \textbf{u} \times (\textbf{p}-(\textbf{p }\cdot \textbf{u})\textbf{u})sin\phi + (\textbf{p}\cdot \textbf{u})\textbf{u}$

$\textbf{p}cos\phi + (\textbf{p}\cdot \textbf{u})\textbf{u}(1-cos\phi) + \textbf{u} \times \textbf{p}sin\ phi$ in $\textbf{u} \times (\textbf{p}\cdot \textbf{u})\textbf{u} = 0$

We $(\textbf{p}\cdot \textbf{u})\textbf{u}$ rewrite it in the form of vector multiplication:

$(\textbf{p}\cdot \textbf{u})\textbf{u} =\textbf{u} (\textbf{p}\cdot \textbf{u}) = \textbf{u}(\textbf{u }^T\textbf{p}) = \textbf{u}\textbf{u}^T\textbf{p}$

Then you get:

$\textbf{p}cos\phi + \textbf{u} \textbf{u}^T(1-cos\phi) \textbf{p}+ \textbf{u} \times \textbf{p}sin\phi$

Since the form of the cross product can be expressed in this way, we use the hat operator to represent an antisymmetric matrix:

So there is:

$\textbf{a} \times \textbf{b} =\hat{\textbf{a}}\textbf{b}$

So we get the final form and the corresponding rotation matrix:

Through the formula of this rotation matrix, we can know that $(u,\phi)$ and $(-u,2\pi - \phi)$ can describe the same rotation, so it is not a one-to-one relationship, so if it is $\phi$ limited $[0,\pi]$ , we will discuss this later.

After getting this rotation matrix, how do we find the corresponding axis $\textbf{u}$ and rotation angle $\phi$ ? The formula is as follows:

where $\ can$ is the trace of the rotation matrix:

$\tau = tr(I)cos\phi + tr(\textbf{u}\textbf{u}^T)(1-cos\phi) + tr(\hat{\textbf{u}})sin\phi$

$=3cos\phi + 1-cos\phi$

$= 2cos\phi +1$

Due to the nature of the anti-symmetric matrix, we can know $\hat{a}^T= -\hat{a}$ that the transposition of the rotation matrix and the other two added quantities will not change, so we have:

$R-R^T = 2\hat{u}sin\phi$

So get certified. It can be known from the formula $= 3$ at that time $\phi=0$ , so it is equivalent to no rotation, so there is no need to discuss the specific axis to be around. In other words, any axis can be used to describe. That is, $(u,\phi)$ there is not a one-to-one relationship with the rotation matrix at this time. $\tau=-1$ When $\phi=\pi$ , it can be described by $\textbf{u}$ sum $\textbf{-u}$ , and we can substitute it into the formula to see, so it is not a one-to-one relationship. So when $\phi$ restricting to $[0,\pi]$ , except for these two cases, the other is a one-to-one relationship.

Angular Velocity

After discussing the concept of rotation, we can talk about the rate of change of rotation, which is the angular velocity. We know that the linear velocity is the derivative of the displacement with respect to time, so naturally, the angular velocity is the derivative of the rotation angle with respect to time.

Now let's take a derivative of a 3 X 3 rotation matrix. We know that the rotation matrix is orthogonal, so it has the following properties, and derives the following identity:

Then we can get the sum and anti-symmetric matrix , simple derivation , the other is the same: $R^T\dot{R}$ $\dot{R}R^T$ $R^T\dot{R}$

$R^T\dot{R} = -\dot{R^T}R = -(R^T\dot{R})^T$

Let us consider the following rotation again:

This time we have to imagine that this rotation changes over time, that is:

Note that the vector $\textbf{p}$ is a constant, because we $\textbf{p}$ think in a body-fixed frame, that is, this coordinate system will rotate with the rotation of the rigid body, that is, $\textbf{p}$ the position on this coordinate system will not change, And q is considered in the inertial frame, that is, there is an absolute coordinate system, which will not change with the rotation of the rigid body. So we have the above formula.

We take the derivative of this formula, so the left part $\dot{q}$ is the velocity under the inertial system, and the right part $p$ is the position under the fixed connection system:

Then we multiply both sides of the formula to the left at the same time $R^T$ , then the speed on the right side of the formula is the speed under the fixed connection system. This is because we can know from the definition of the above two coordinate systems that the solid-connected system coincides with the inertial system when it is rotated back, and the following formula is obtained. The variables in the purple box are deduced from the above. matrix, so it can be $\hat{\omega}^b$ represented by:

The expression in the inertial system is as follows, which can be obtained by $q = Rp$ multiplying the two equations to the left and substituting them into the above equation to obtain. Similarly, the formula in the orange box is represented by: $R^T$ $p = R^Tq$ $\hat{\omega}^s$

With these two formulas, we can obtain the angular velocities in the inertial system and the solid-connected system respectively.

Next chapter link:

Robotics: Aerial Robotics (air robot) notes (4): UAV dynamics

Robotics: Aerial Robotics (air robot) notes (3): UAV kinematics modeling