1. Overview
The description method of the pose (position and direction) of an object is generally represented by two coordinate systems. One is the world coordinate system or the ground coordinate system. Here I call it the ground coordinate system, which belongs to the reference coordinate system; the other is its own Coordinate system, it feels more appropriate to use aircraft as an example to describe some common terms. Here it is called the body coordinate system, which will change with its own changes.
As shown below:
2. Body coordinate system
The body coordinate system refers to a three-dimensional orthogonal rectangular coordinate system that is fixed on the aircraft or aircraft and follows the right-hand rule. Its origin is located at the center of mass of the aircraft.
The X’ axis always points to the nose of the machine, the Y’ axis points to the right of the fuselage, perpendicular to the symmetry plane where the X’ axis is located, and the Z’ axis is perpendicular to the horizontal plane where the X’ axis is located. In other words, the coordinates are fixed relative to the aircraft, and the ground coordinate system above is the entire reference coordinate and remains fixed.
3. Euler angles
Euler angle (attitude angle):
The relationship between the body coordinate system and the ground coordinate system is three angles, which reflect the attitude of the aircraft relative to the ground.
Roll angle Φ (roll): Rotate around the X-axis. The roll angle refers to the angle between the transverse axis of the vehicle and the horizontal line. Also called roll angle, it represents the rotation of the carrier around the longitudinal axis. Clockwise rotation around the longitudinal axis is positive, otherwise it is negative. It can be imagined as an airplane rolling.
Pitch angle θ (pitch): Rotate around the Y axis. The angle between the x-axis of the body coordinate system and the horizontal plane. The pitch angle is positive on the horizontal plane, otherwise it is negative. It can be imagined as the aircraft looking up and diving down.
Yaw angle ψ (yaw): rotation around the Z axis. Relative to the rotation angle of the longitudinal axis, changing the yaw angle can change the flight direction of the aircraft. The nose of the aircraft is pointing to the right. This is the same as driving a car, turning left and right.
4. Euler angle conversion rotation matrix
Of course, the rotation order of the Euler angles is also very relevant. They all describe the change from one coordinate system to another. That is to say, the posture of one coordinate system relative to another coordinate system can be determined using a rotation matrix. express. Let’s take a look at its rotation matrix and derive it as follows:
5. Quaternions
Quaternions, also known as Euler parameters, provide another way to express three-dimensional rotations. The quaternion method is faster when used in most calculations, and can avoid some technical problems, such as universal joint deadlock (that is, when the rotation angle approaches certain specific values, the Euler angle representation will cause an infinite loop. In other words, the two axes coincide, lose one degree of freedom, and then it makes no sense to rotate again) phenomenon. For these reasons, many high-speed three-dimensional graphics programs use quaternions. Of course, quaternions are not as intuitive as Euler angles and are more difficult to understand. This is also its shortcoming.
Euler angles and quaternions are both methods for describing rotation and direction, but they differ in the way they represent rotation and their mathematical structure.
For the situation of gimbal deadlock, you can view the video to understand more intuitively: Euler rotation gimbal deadlock
Quaternions have four degrees of freedom, one more than Euler angles:
Q=a + bi + cj + dk, where a, b, c, d are real numbers, i, j, k are imaginary numbers, i2=j2=k2=-1
6. Python example
6.1. Euler angle rotation rotation matrix
import math
import numpy as np
def euler_to_matrix(theta):
R_x = np.array([[1, 0,0],
[0,math.cos(theta[0]), -math.sin(theta[0])],
[0,math.sin(theta[0]), math.cos(theta[0])]
])
R_y = np.array([[math.cos(theta[1]), 0,math.sin(theta[1])],
[0,1,0],
[-math.sin(theta[1]),0,math.cos(theta[1])]
])
R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]),0],
[math.sin(theta[2]), math.cos(theta[2]),0],
[0,0,1]
])
R = np.dot(R_z, np.dot( R_y, R_x ))
return R
Calculate the rotation matrices around the x, y, and z axes respectively, and then perform dot product operations in a certain order.
print(euler_to_matrix([math.pi/3,0,math.pi/6])) /* [[ 0.8660254 -0.25 0.4330127] [0.5 0.4330127 -0.75] [-0. 0.8660254 0.5 ]] */
Use the libraries in transforms3d or scipy to verify respectively. The answer is correct:
import transforms3d as tfs
print(tfs.euler.euler2mat(math.pi/3,0,math.pi/6,"sxyz"))
print(np.degrees(tfs.euler.mat2euler(euler_to_matrix([math.pi/3,0,math.pi/6]),"sxyz"))) #[60. -0. 30.]
//Use radian system
from scipy.spatial.transform import Rotation as R
print(R.from_euler('xyz', [math.pi/3,0,math.pi/6], degrees=False).as_matrix())
print(np.degrees(R.from_matrix(euler_to_matrix([math.pi/3,0,math.pi/6])).as_euler('xyz'))) #[60. 0. 30.]
//Use angle system
print(R.from_euler('xyz', [60,0,30], degrees=True).as_matrix())
Generally, we use radians as the standard. Of course, in some cases, for the sake of intuition, we can also convert it into angles for calculation.
6.2, Euler angle to quaternion
import math
def euler_to_quaternion(roll, pitch, yaw):
cy = math.cos(yaw * 0.5)
sy = math.sin(yaw * 0.5)
cr = math.cos(roll * 0.5)
sr = math.sin(roll * 0.5)
cp = math.cos(pitch * 0.5)
sp = math.sin(pitch * 0.5)
w = cy * cr * cp + sy * sr * sp
x = cy * sr * cp - sy * cr * sp
y = cy * cr * sp + sy * sr * cp
z = sy * cr * cp - cy * sr * sp
return w, x, y, z
print(euler_to_quaternion(math.pi/3,0,math.pi/6))
#(0.8365163037378079, 0.4829629131445341, 0.12940952255126034, 0.2241438680420134)
print(tfs.euler.euler2quat(math.pi/3,0,math.pi/6,"sxyz"))
#[0.8365163 0.48296291 0.12940952 0.22414387]
print(euler_to_quaternion(math.pi/3,math.pi,math.pi/2))
#(0.3535533905932738, -0.6123724356957946, 0.6123724356957946, -0.3535533905932737)
print(tfs.euler.euler2quat(math.pi/3,math.pi,math.pi/2,"sxyz"))
#array([ 0.35355339, -0.61237244, 0.61237244, -0.35355339])
6.3, Quaternion to rotation matrix
#x, y,z,w
def quaternion_to_rotation_matrix(q):
rot_matrix = np.array(
[[1.0 - 2 * (q[1] * q[1] + q[2] * q[2]), 2 * (q[0] * q[1] - q[3] * q[2] ), 2 * (q[3] * q[1] + q[0] * q[2])],
[2 * (q[0] * q[1] + q[3] * q[2]), 1.0 - 2 * (q[0] * q[0] + q[2] * q[2]) , 2 * (q[1] * q[2] - q[3] * q[0])],
[2 * (q[0] * q[2] - q[3] * q[1]), 2 * (q[1] * q[2] + q[3] * q[0]), 1.0 - 2 * (q[0] * q[0] + q[1] * q[1])]],
dtype=q.dtype)
return rot_matrix
r_matrix=quaternion_to_rotation_matrix(np.array([0.4829629,0.12940952,0.22414387,0.8365163]))
print(r_matrix)
/*
[[ 8.66025403e-01 -2.50000007e-01 4.33012693e-01]
[4.99999996e-01 4.33012726e-01 -7.49999975e-01]
[1.23449401e-09 8.66025378e-01 5.00000027e-01]]
*/
6.4, Rotation matrix to Euler angle
def rotation_matrix_to_euler(R):
sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])
singular = sy < 1e-6
if not singular :
x = math.atan2(R[2,1] , R[2,2])
y = math.atan2(-R[2,0], sy)
z = math.atan2(R[1,0], R[0,0])
else :
x = math.atan2(-R[1,2], R[1,1])
y = math.atan2(-R[2,0], sy)
z = 0
return np.array([x, y, z])
print(rotation_matrix_to_euler(r_matrix))
//radian
#[1.04719751e + 00 -1.23449401e-09 5.23598772e-01]
print(np.degrees(rotation_matrix_to_euler(r_matrix)))
//angle
#[5.99999979e + 01 -7.07312966e-08 2.99999998e + 01]
7. Summary
Here we intuitively understand the relationship between the two coordinate systems through drawing, as well as the various expression methods of the Euler angle generated during the flight of the aircraft. Since the Euler angle has three degrees of freedom, when the two axes overlap, a “universal” will occur. joint deadlock” problem, so we generally use quaternions to replace Euler angles. The four degrees of freedom avoid deadlock at the universal joint.
Since different orders of rotation will produce different rotation matrices, here I also use the knowledge of trigonometric functions to deduce the rotation matrix obtained by rotating the Euler angles along the three axes respectively. I hope it can help everyone better. understand.