四旋翼运动学

本篇主要写四旋翼运动学，包括坐标系变换，角速度与旋转矩阵导数的关系和角速度与欧拉角导数的关系，这里的角速度均指机体坐标系下的角速度。
由于是从word中复制过来，因此公式格式不太好请谅解。

Coordinate System and Rotation Matrix

Two frames are inevitable when we analyze quadcopter dynamics. The inertial frame is defined by the ground, with gravity pointing in the negative z direction. The body frame is defined by the orientation of quadcopter as e_x and e_y is along the forward and leftward direction respectively and e_z is perpendicular to the quadcopter’s plane.

The reason we build up the body frame is that the dynamics of quadcopter is more easily to describe in this frame because the thrust generated by four propellers is always parallel to the e_z direction respectively. However, in order to determine the position and posture of quadcopter in the real world we should obtain the relationship between the inertial frame and body frame which is called rotation matrix.

Rotation can always be decomposed into three processes, but the way to rotate is not unique. For example, we can rotate around X axis and then Y axis and finally Z axis. It’s obvious that we cannot rotate around the same axis in the two succession stages. So the number of permutation is 12: XYX, XYZ, XZX, XZY, YXY, YXZ, YZX, YZY, ZXY, ZXZ, ZYZ, ZYX. The rotation matrix of them are distinct from each other. First we need to study the rotation matrix of one rotation process. Naturally we need to define the roll, pitch, and yaw angles in the body frame to describe the rotation angle as (ϕ,θ,ψ)^T and the direction of positive rotation is right-handed with coordinate axis.

The rotation matrix of X axis is:

The rotation matrix of Y axis is:

The rotation matrix of Z axis is:

So the rotation matrix of ZYX is:

The rotation matrix of XYZ is:

Where c represents cos and s represents sin. It can be proved that:

The rotation matrix has two meanings. First one is to obtain the coordinates of a vector after rotating. Example: p is a vector in the inertial frame and p’ is the vector formed by the rotation of p. As a result the coordinates of p’ in the inertial frame is:

Another meaning of rotation matrix is to transfer the coordinates in body frame to inertial frame. Example: p is a vector in body frame and it’s coordinates in inertial frame is:

So we can easily get the coordinates of a vector in inertial frame if we know it’s coordinates in body frame and rotation matrix.

Relation between Angular Velocity and Derivation of Rotation Matrix

Angular velocity in body frame is a extremely important parameter for quadcopter control. It is clear that rotation matrix determines the way how quadcopter rotates. However, is the derivation of rotation matrix equals to the angular velocity in body frame? Impossible. The dimension of them are different firstly. There is indeed a relation between them but not a single derivation.

Let’s begin with the definition of the angular velocity and linear velocity.

where S is a skew symmetric matrix which means S^T=-S.

It’s obvious that derivation of a displacement vector is a velocity vector. So let’s differentiate the both sides of this equation:

where p is the displacement vector in inertial frame and p ̅ is the same vector in body frame. As the coordinates of this vector in body frame is constant, we can derive:

If we replace p ̅ with R^(-1) p we can obtain:

As we have mentioned before p ̇=v so we can get:

which means:

To make the physical meaning of the equation more clear we define:

Which demonstrates that S is totally decided by the w and S is just to fit the form of matrix operations. As a result, we can obtain:

It has to be noticed that [w×] represents the angular velocity in inertial frame, so it’s necessary to transfer it into the angular velocity in body frame. We start from this equation:

This time we do not replace p ̅ but p ̇ with R(¯p) ̇ so we get:

Similarly (¯p) ̇ can be also written in this form:

So the result become:

Where ([w×] ) ̅ is the angular velocity in body frame.

Relation between Angular Velocity and Derivation of Euler Angle

As we have proved that the derivation of rotation matrix is related to the angular velocity in body frame, it’s too difficult for us to measure the derivation of rotation matrix in practice. So we need a more specific equation to describe the angular velocity which reminds us of derivation of Euler angle. The first idea is derivation of Euler angle is equal to angular velocity but it is also wrong indeed. Let’s see how the derivation of Euler angle and angular velocity in body frame can be related.

Remember there is one property of rotation matrix is it can transfer the coordinates in the body frame to the coordinates in the inertial frame. So it’s easy for us that if we want to do the opposite process we just need to pre-multiply the coordinates by inverse of rotation matrix which shows:

For example, if the rotation sequence is ZYX. Let’s see [0 0 ψ ̇ ]^T first. At beginning, the coordinate system is rotating around Z axis, so [0 0 ψ ̇ ]^T represents the angular velocity indeed. Then, the coordinate system rotates around Y axis and X axis so although the direction of [0 0 ψ ̇ ]^T is not changed in inertial frame, it’s coordinate in body frame is changed. But as we have a such strong tool as rotation matrix, it’s easy to get that the coordinates of [0 0 ψ ̇ ]^T in body frame is R_x^(-1) R_y^(-1) [0 0 ψ ̇ ]^T. Similarly [0 θ ̇ 0]^T should also be pre-multiply by R_x^(-1) while [ϕ ̇ 0 0]^T can not be changed. Thus the result is:

Which can be written in the following form:

Of course it can also be written as:

So we can use [■(ϕ&θ&ψ)]^T which is relatively easy to be observed to calculate the angular velocity in body frame using above equation:

Similarly we can get the relationship when sequence is XYZ which is useful in our model:

The inverse relationship is: