Mobile Robotics (II) posture solution

Four yuan number

Definition and basic operation of quaternion
Quaternion is generally used to express rotation in aircraft attitude calculation , Personal understanding , Rotate matrix with 3*3 Matrix and additional 6 A nonlinear constraint （ The column vectors are orthogonal to each other and the module length is 1） Describes rotation , And quaternions pass 4 Number and a constraint （ The unit is 4 yuan ） Describes the same amount of information , Similar to a system of linear equations , Reduce constraints by substituting elimination , Reduce the number of variables , To reduce the amount of calculation . actually , In inertial navigation system, quaternion is used to realize fast cycle iteration , It can take 1000Hz Update the pose of the moving object at the speed of .

Three dimensional rotation representation

1. The unit is 4 yuan $\tilde{q}=cos(\theta /2)+\hat{w}sin(\theta /2)$ It can represent walking around in three-dimensional space （ Unit vector ）$\hat{w}$ rotate $\theta$ Rotation operator of angle . Be careful ,$\hat{w}$ Here, it is regarded as a vector in three-dimensional space rather than a hypercomplex .
2. The rotation of the vector ：
   a. Vector “ Quaternization ”：$\tilde{x}=0+\overrightarrow{x}$;
   b. Rotate the vector around the axis $\hat{w}$ rotate $\theta$ horn , Get the new vector ${\tilde{x}}^{{}^{\prime }}=\tilde{q}\tilde{x}{\tilde{q}}^{\ast }$.
3. Equivalent rotation matrix ：
   For quaternions $q=q_0+q_{1}i+q_{2}j+q_{3}k$
   take ${\tilde{x}}^{{}^{\prime }}=\tilde{q}\tilde{x}{\tilde{q}}^{\ast }$ an , Merge similar items to get
   $$R=\left[\begin{array}{ccc}2(q_0^2+q_1^2)-1& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& 2(q_0^2+q_2^2)-1& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_0{q}_1+q_2{q}_3)& 2(q_0^2+q_3^2)-1\end{array}\right]$$
   The above is the expression under the left hand system , About left / The distinction between right-handed systems ： Hand to hand “ gun ” The shape of the , Point your index finger at your opponent （You）, representative Y Axis , The thumb is for aiming （x Does it seem to be accurate ）, So it is X Axis , The remaining middle finger represents Z Axis . here , The coordinate system corresponding to the left hand is the left-handed system , The right hand corresponds to the right hand .
   Right hand system expresses ：
   $$R=\left[\begin{array}{ccc}1-2(q_2^2+q_3^2)& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& 1-2(q_1^2+q_3^2)& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_0{q}_1+q_2{q}_3)& 1-2(q_1^2+q_2^2)\end{array}\right]$$
   hinder code It's right-handed , The left hand system in books , It can also be used in other places , Pay attention to distinguish between .

Attitude calculation

Mainly combined with a four axis flight control project .
First, let's explain the navigation coordinate system n The definition of ： x/y/z The axis points east in turn 、 north 、 God , Set the yaw angle as $\psi$ （ Be careful ： Clockwise is positive , Traditionally, north by East is positive ）, The pitch angle is $\theta$, The rolling angle is $\gamma$ , Body coordinate system b And n The relationship is
$$T_n^b=\left[\begin{array}{ccc}c\gamma c\psi +s\gamma s\psi s\theta & -c\gamma s\psi +s\gamma c\psi s\theta & -s\gamma c\theta \\ s\psi c\theta & c\psi c\theta & s\theta \\ s\gamma c\psi -c\gamma s\psi s\theta & -s\gamma s\psi -c\gamma c\psi s\theta & c\gamma c\theta \end{array}\right]$$

1. The acceleration of gravity is normalized to a unit vector ;

	//  Acceleration normalization 
	NormQuat = Q_rsqrt(squa(MPU6050.accX) + squa(MPU6050.accY) + squa(MPU6050.accZ));
	// Q_rsqrt: Fast calculation  1/Sqrt(x)
	Acc.x = pMpu->accX * NormQuat;
	Acc.y = pMpu->accY * NormQuat;
	Acc.z = pMpu->accZ * NormQuat;

2. Extract the gravity component in the quaternion equivalent rotation matrix , Is to rotate the matrix by gravity （ Set to 1, step 1 The acceleration normalization in the ） Convert to the body coordinate system ;

	//  Extract the gravity component in the equivalent rotation matrix 
	Gravity.x = 2 * (NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);
	Gravity.y = 2 * (NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);
	Gravity.z = 1 - 2 * (NumQ.q1 * NumQ.q1 + NumQ.q2 * NumQ.q2);

3. The vector cross product yields the attitude error ,Acc It is measured by accelerometer ,AccGravity It is calculated by gyro integral , In essence, it represents a quantity , The cross product is still in the body coordinate system , And its size is directly proportional to the gyro integration error , Used to correct gyro , This reflects the fusion of sensor data ;

	//  The value of vector cross multiplication 
	AccGravity.x = (Acc.y * Gravity.z - Acc.z * Gravity.y);
	AccGravity.y = (Acc.z * Gravity.x - Acc.x * Gravity.z);
	AccGravity.z = (Acc.x * Gravity.y - Acc.y * Gravity.x);

4. Make compensation ;

	//  Then do the acceleration integral compensation and the compensation value of angular velocity 
	GyroIntegError.x += AccGravity.x * KiDef;
	GyroIntegError.y += AccGravity.y * KiDef;
	GyroIntegError.z += AccGravity.z * KiDef;

	//  Angular velocity fusion acceleration integral compensation value 
	Gyro.x = pMpu->gyroX * Gyro_Gr + KpDef * AccGravity.x + GyroIntegError.x; //  Radian system 
	Gyro.y = pMpu->gyroY * Gyro_Gr + KpDef * AccGravity.y + GyroIntegError.y;
	Gyro.z = pMpu->gyroZ * Gyro_Gr + KpDef * AccGravity.z + GyroIntegError.z;

5. First order Runge-Kutta Update quaternion ;

	//  First order Runge Kutta method ,  Update quaternion 
	q0_t = (-NumQ.q1 * Gyro.x - NumQ.q2 * Gyro.y - NumQ.q3 * Gyro.z) * HalfTime;
	q1_t = (NumQ.q0 * Gyro.x - NumQ.q3 * Gyro.y + NumQ.q2 * Gyro.z) * HalfTime;
	q2_t = (NumQ.q3 * Gyro.x + NumQ.q0 * Gyro.y - NumQ.q1 * Gyro.z) * HalfTime;
	q3_t = (-NumQ.q2 * Gyro.x + NumQ.q1 * Gyro.y + NumQ.q0 * Gyro.z) * HalfTime;

	NumQ.q0 += q0_t;
	NumQ.q1 += q1_t;
	NumQ.q2 += q2_t;
	NumQ.q3 += q3_t;

6. Quaternion normalization ;

	//  Quaternion normalization 
	NormQuat = Q_rsqrt(squa(NumQ.q0) + squa(NumQ.q1) + squa(NumQ.q2) + squa(NumQ.q3));
	NumQ.q0 *= NormQuat;
	NumQ.q1 *= NormQuat;
	NumQ.q2 *= NormQuat;
	NumQ.q3 *= NormQuat;