人类的孤独像是一种与生俱来的残疾。
> C语言 > 关于四轴飞行器的姿态解算

关于四轴飞行器的姿态解算

C语言 lihuixiang 2673℃

作者：年少如你

1.四元数与欧拉角
四轴最重要的就是确定飞机的姿态，也就是所谓的姿态解算，什么是姿态？试想一下如果我们人分不清上下左右会怎么样，可能在路上走着自己头向下脚向上都不知道位。飞机在空中飞行它也需要知道自己的状态信息，到底自己在往哪个方向飞，机身的是不是倾斜的，是向左还是向右倾，倾了多少度等等，要不然可就是一只无头苍蝇到处乱撞了。
要知道机体的状态那么必然要有一个参考，机体的所有状态都是相对这个参考系的状态。如图一般选正北为机头正向（Y轴），正东为X轴，Z轴指向上。

图1 参考系

图2 机体状态描述
有了参考系统就可以描述机体状态信息，比如要知道机头是向上还是向下（俯仰角pitch）这个角度可以看成机体绕x轴转动一个θ角得到，一般向上为正向下为负（顺时针为负，逆时针为正）。要知道机头偏向东还是向西（偏航角yaw）就可以看成机体绕z轴旋转一个角度Ψ得到，一般选向东偏为正。机体是向左倾还是向右倾（横滚角roll）是绕y轴旋转一个角度γ得到，顺时针为负，逆时针为正。绕坐标轴转动的角度也叫欧拉角。那么问题来了，上面都是只绕一个轴转动的情况，如果既有偏航，又有信仰，还有横滚那么怎么得到这三个角度呢？这个就可以通过坐标轴旋转得到。
先看绕轴转动的坐标变换情况。如图3坐标系O-x1y1z1绕Z轴逆时针转动角度Ψ得到O-x2y2z2坐标系，在x1oy1中的向量在平面xoy中，由于是绕Z轴转动所以Z坐标不变。F在O-x1y1z1中坐标为(a,b,z)下面计算F在O-x2y2z2下的坐标。

由图可以得到：

[latex]\vec{AF} = \vec{AB} + \vec{BF} = \vec{OE} + \vec{BF} = \vec{\alpha}\cos\psi + \vec{\beta}\sin\psi[/latex]              (1)

[latex]\vec{OA}=\vec{BD}-\vec{DE}=\vec{\beta}\sin\psi - \vec{\alpha}\cos\psi[/latex]            (2)

用矩阵表示为：

 [latex]\left[\begin{matrix} r_{x2}\cr r_{y2}\cr r_{z2}\end{matrix}\right]=\left[\begin{matrix} \cos\psi & \sin\psi & 0\cr -\sin\psi & \cos\psi & 0\cr 0 & 0 & 1\end{matrix}\right]\cdot\left[\begin{matrix} r_{x1}\cr r_{y1}\cr r_{z1}\end{matrix}\right][/latex]                          (3)

图3 坐标变换关系

绕其他轴旋转也是同理。绕每个轴旋转就都可以得到一个矩阵，将这三个矩阵连乘就可以得到整体的由参考系到机体系的旋转矩阵[latex]C^b_n[/latex] 。对就的旋转顺序为先Z（角度顺时针为正）、再X（角度逆时针为正）、最后是Y（角度逆时针为正），需要注意的就是角度符号。具体过程可以参考秦永元《惯性导航》。

[latex]C^b_n=C^b_2C^2_1C^1_n=\left[\begin{matrix} \cos\gamma & 0 & -\sin\gamma\cr 0 & 1 & 0\cr \sin\gamma & 0 & \cos\gamma\end{matrix}\right]\cdot
\left[\begin{matrix} 1 & 0 & 0\cr 0 & \cos\theta & \sin\theta\cr 0 & -\sin\theta & \cos\theta\end{matrix}\right]\cdot
\left[\begin{matrix} \cos\psi & -\sin\psi & 0\cr \sin\psi & \cos\psi & 0\cr 0 & 0 & 1\end{matrix}\right][/latex]

[latex]=
\left[\begin{matrix} \cos\gamma\cos\psi+\sin\gamma\sin\psi\sin\theta & -\cos\theta\sin\psi+\sin\gamma\cos\psi\sin\theta & -\sin\gamma\cos\theta\cr
\sin\psi\cos\theta & \cos\psi\cos\theta & \sin\theta\cr
\sin\gamma\cos\psi-\cos\gamma\sin\psi\sin\theta & -\sin\gamma\sin\psi-\cos\gamma\cos\psi\sin\theta & \cos\gamma\cos\theta\end{matrix}\right][/latex]

在解算中最常用的是四元数，四元数的定义就不说了，要具体了解还是参考秦永元《惯性导航》。为什么要用四元数，主要是因为用欧拉角计算都是三角函数比较消耗CPU时间，而四元数是普通的加减乘除。还有一点欧拉角会有万向节死锁问题导致旋转失效。
四元数是一个复数，它可以把空间中绕三个轴的三次旋转看成是绕空间中的一个向量r[q0,q1,q2,q3]的单次旋转实现的。通过推导可以得到四元数矩阵，这是从机体系到参考系的矩阵。因为是正交矩阵，它与前面的是互为转置的关系，也就是说，的对应行是的对应列。（注意n和R系都是指参考系，公式里没有统一写）

记：                              

则为：

根据以上对应可以得到三个角度：

2.姿态解算
要进行姿态解算就要知道初始时刻也就是飞机起飞时的机体状态信息，以这个初始信息确定旋转四元素初值。姿态信息可以通过加速度计和地磁计得到。
在参考系下重力是竖直向下的也就是X、Y分量为0只有Z轴为-1，为什么是-1呢？因为前面选定的坐标系是Z轴向上为正，向下为负。现在传感器是装在机体上的，也就是说测出的重力加速度[ax,ay,az]是在机体系下的值。而机体坐标与参考坐标系下的向量通过左乘对应的旋转矩阵就可以得到，如要知道参考重力[0,0,-1]在当前机体系下对应的x、y、z分量可以用公式：

从这里可以得到俯仰角:                       

横滚角：                                                  

而对于偏航角由于初始机体水平，我们可以由地磁传感器得到:

这样就得到了三个角的初始值，下面可以利用这三个角度根据前面的与的元素对应关系可以初始化四无数。

由对应关系可以得到下面的方程，T11到T33定义在上文中。

可以解得:

它们的符号是由下式确定，其中q0的符号可以任意：

姿态初始化程序如下：

void IMUInit(void)
{
    float norm,T11,T12,T13,T21,T22,T23,T31,T32,T33;
    ReadIMU();				//读取数据

    //covert gravity [0,0,-1] vector to quadrotor's aixs ,than slove the inital angle
    IMU.Pitch=asin(-IMU.ay);					//弧度
    IMU.angleP=IMU.Pitch*RADIAN;				//转成角度
    IMU.Roll=atan(-IMU.ax/IMU.az);
    IMU.angleR=IMU.Roll*RADIAN;
    IMU.Yaw=atan2(IMU.my,IMU.mx)-PI/2;		
    if(IMU.Yaw<-PI){
        IMU.Yaw+=2*PI;
    }
    IMU.angleY=IMU.Yaw*RADIAN;
    T11=cos(IMU.Roll)*cos(IMU.Yaw) + sin(IMU.Roll)*sin(IMU.Yaw)*sin(IMU.Pitch);
    T12=sin(IMU.Yaw)*cos(IMU.Pitch);
    T13=sin(IMU.Roll)*cos(IMU.Yaw) - cos(IMU.Roll)*sin(IMU.Yaw)*sin(IMU.Pitch);
    T21=sin(IMU.Roll)*cos(IMU.Yaw) * sin(IMU.Pitch)-cos(IMU.Roll)*sin(IMU.Yaw);
    T22=cos(IMU.Yaw)*cos(IMU.Pitch);
    T23=-sin(IMU.Roll)*sin(IMU.Yaw) - cos(IMU.Roll)*cos(IMU.Yaw)*sin(IMU.Pitch);
    T31=-sin(IMU.Roll)*cos(IMU.Pitch);
    T32=sin(IMU.Pitch);
    T33=cos(IMU.Roll)*cos(IMU.Pitch);

    IMU.q0=0.5*sqrt(1+T11+T22+T33);						//得到初始四元数
    IMU.q1=0.5*sqrt(1+T11-T22-T33)*Sign(T32-T23)*Sign(IMU.q0);
    IMU.q2=0.5*sqrt(1-T11+T22-T33)*Sign(T13-T31)*Sign(IMU.q0);
    IMU.q3=0.5*sqrt(1-T11-T22+T33)*Sign(T21-T12)*Sign(IMU.q0);
//正规化
    norm = sqrt(IMU.q0*IMU.q0 + IMU.q1*IMU.q1 + IMU.q2*IMU.q2 + IMU.q3*IMU.q3);
    IMU.q0 = IMU.q0 / norm;	
    IMU.q1 = IMU.q1 / norm;
    IMU.q2 = IMU.q2 / norm;
    IMU.q3 = IMU.q3 / norm;	
}

从上面的分析我们可以在初始情况下得到姿态角，那用上面的方法也可以得到运动状态的角度为什么还用四元素矩阵来解算？从前面可以知道通过重力可以知道俯仰角和横滚角，但是对于偏航角是不精确的，因为在机体有倾斜角时测量到的my,mx因mz的存在合向量大小是不同的所以arctan求解的值也不能代表偏航角。地球磁场是由南向北指向，并且除了在赤道，其他地方都会有两个量一个是y轴分量，一个是z轴分量，所以在参考系下地磁向量为[0,by,bz]。要用阵解算而不是用上面的方法还有一个重要的原因就是加速度计与磁场计它们的数值在长期来看是值得相信的，但是在短期内不可信，因为机体振动还有运动的原因加速度计在短期内是不定的，为了克服这个情况就引入了陀螺仪。陀螺仪是测量角度变化的传感器，它可以测量单位时间绕一个轴转动的角度，这个角度就是转动的欧拉角。所以要让得到的欧拉角更稳定就要结合陀螺仪与加速度计和地磁计的数据。

那要如何运用陀螺仪的数据呢？运用陀螺仪数据也就是要想办法把角增量用进去。将四元素Q对时间求导可以得到一个与角度增量有关的微分方程，而通过毕卡解法可以得到四元素方程的解法，在这个解法中就用到了角增量（具体推导参考《惯性导航》）。最后可以得到的一阶近似解为：

其中I为单位矩阵， 为前一次的矩阵，为最新矩阵。而如下所示就可以看成是陀螺仪测量的角度增量。


在程序中有一个用向量叉积计算角度误差的方法，为什么向量叉积是角度误差或说与角度差有关。回想下向量叉积公式 axb=|a||b|sinθ， θ就是向量的夹角。所以当我们将参考重力向量通过四元数向机体系转换时得到了[vx,vy,vz]，与机体下传感器测量的值[ax,ay,ax]叉乘时就得到各轴的与角度误差有关的量。
下面是角度代码：

void AttitudeUpdate(void)
{
    float norm;
    float ex,ey,ez,vx,vy,vz,gx,gy,gz,mx,my,mz,wx,wy,wz;
    float q0,q1,q2,q3;

//先计算好数值
    float q0q0=IMU.q0*IMU.q0;
    float q0q1=IMU.q0*IMU.q1;
    float q0q2=IMU.q0*IMU.q2;
    float q0q3=IMU.q0*IMU.q3;
    float q1q1=IMU.q1*IMU.q1;
    float q1q2=IMU.q1*IMU.q2;
    float q1q3=IMU.q1*IMU.q3;
    float q2q2=IMU.q2*IMU.q2;
    float q2q3=IMU.q2*IMU.q3;
    float q3q3=IMU.q3*IMU.q3;
    //计算地磁场方向,将测量到的值通过四元素矩阵转到参考系统下，得到参考系下的磁场，在参考系统下是没有x分量的只有y与z分量
    mx=2*(0.5-q2q2 - q3q3)*IMU.mx + 2*(q1q2 - q0q3)*IMU.my + 2*(q1q3 + q0q2)*IMU.mz;
    my=2*(q1q2 + q0q3)*IMU.mx + 2*(0.5-q1q1 - q3q3)*IMU.my +2*(q2q3 - q0q1)*IMU.mz;
    mz=2*(q1q3-q0q2)*IMU.mx+2*(q2q3+q0q1)*IMU.my+ 2*(0.5-q1q1-q2q2)*IMU.mz;
    my=sqrt(mx*mx+my*my);	//得到参考磁场y分量

//将参考系下的重力[0,0,-1]用四元素向机体系投影
    vx = 2*(q0q2 - q1q3);				
    vy = -2*(q0q1 + q2q3);
    vz = -q0q0 + q1q1 + q2q2 - q3q3;
//将参考系下的磁场[0,my,mz]用四元素向机体系投影	
    wx=2*(q1q2+q0q3)*my + 2*(q1q3-q0q2)*mz;
    wy=2*(0.5-q1q1-q3q3)*my + 2*(q2q3+q0q1)*mz;
    wz=2*(q2q3-q0q1)*my + 2*(0.5-q1q1-q2q2)*mz;
    
    //计算误差角度，这里用的是向量差乘。前一部分是重力得到的误差，后一部分是磁场得到的误差
ex = (IMU.ay*vz - IMU.az*vy) + (IMU.my*wz - IMU.mz*wy);  
    ey = (IMU.az*vx - IMU.ax*vz) + (IMU.mz*wx - IMU.mx*wz);
    ez = (IMU.ax*vy - IMU.ay*vx) + (IMU.mx*wy - IMU.my*wx);

    //陀螺仪的测量乘上分辨率得到角度增量
    gx= IMU.GyroX*0.0010653;//逆时针为正	
    gy= IMU.GyroY*0.0010653;//逆时针为正
    gz= -IMU.GyroZ*0.0010653;//由于z轴顺时针为正所以要将测量值乘负号
//误差的积分
    exInt = exInt + ex * KI;			
    eyInt = eyInt + ey * KI;
    ezInt = ezInt + ez * KI;
    //误差互补融合=KI=0.001f，KP=5.0f。KI和KP参数是调试得到的，可以看PID调试方法
    gx = gx + KP*ex + exInt;						
    gy = gy + KP*ey + eyInt;
    gz = gz + KP*ez + ezInt;		
//解四元数微分方程，其中0.005是采样周期一半						
    q0 = IMU.q0 - (IMU.q1*gx + IMU.q2*gy + IMU.q3*gz)*0.005;
    q1 = IMU.q1 + (IMU.q0*gx + IMU.q2*gz - IMU.q3*gy)*0.005;
    q2 = IMU.q2 + (IMU.q0*gy - IMU.q1*gz + IMU.q3*gx)*0.005;
    q3 = IMU.q3 + (IMU.q0*gz + IMU.q1*gy - IMU.q2*gx)*0.005;

    //正规化四元数，因为计算会因CPU精度使四元数失去正规性
    norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    IMU.q0 = q0 / norm;
    IMU.q1 = q1 / norm;
    IMU.q2 = q2 / norm;
    IMU.q3 = q3 / norm;
    
    q0q1=IMU.q0* IMU.q1;
    q0q3=IMU.q0*IMU.q3;
    q1q1=IMU.q1*IMU.q1;
    q1q2=IMU.q1*IMU.q2;
    q2q2=IMU.q2*IMU.q2;
    q2q3=IMU.q2*IMU.q3;
    q0q2=IMU.q0*IMU.q2;
    q1q3=IMU.q1*IMU.q3;
    q3q3=IMU.q3*IMU.q3;
    //计算姿态角度
    IMU.Pitch=asin(2*(q2q3 + q0q1));
    IMU.angleP=IMU.Pitch*RADIAN;
    IMU.Roll=atan2(2*(q0q2 - q1q3),1 - 2*(q1q1 + q2q2));
    IMU.angleR=IMU.Roll*RADIAN;
    IMU.Yaw=atan2(2*(q1q2-q0q3),1-2*(q1q1+q3q3));
    IMU.angleY=IMU.Yaw*RADIAN;
}

 

转载请注明：OpenMind » 关于四轴飞行器的姿态解算

喜欢 (1)