一、姿態(tài)軸系介紹
1、歐拉角姿態(tài)表示
歐拉(Euler) 角是描述姿態(tài)最直觀的方法，尤其是在描述載體坐標系相對于對應(yīng)的當?shù)貙Ш阶鴺讼档倪\動時。（應(yīng)用在慣性導航中）
姿態(tài)被分解為三個連續(xù)的轉(zhuǎn)動過程，每次旋轉(zhuǎn)所圍繞的軸與前后旋轉(zhuǎn)所圍繞的軸正交。圖1 展示了這種旋轉(zhuǎn)過程，通過兩個過渡坐標系ψ和θ ，將與坐標系β 對齊的坐標系，旋轉(zhuǎn)到與坐標系α 對齊。
圖1 歐拉角的轉(zhuǎn)動
第一次的轉(zhuǎn)動角度ψ是偏航角，繞β 坐標系和第一個過渡坐標系共同的z軸旋轉(zhuǎn)，x 軸和y 軸轉(zhuǎn)動， z 軸保持不變；下一個旋轉(zhuǎn)角為俯仰角θ，繞第一個過渡坐標系和第二個過渡坐標系共同的y 軸旋轉(zhuǎn)， x軸和z 軸轉(zhuǎn)動;最后一個旋轉(zhuǎn)角為滾動角φ御，繞第二個過渡坐標系和α 坐標系共同的z 軸旋轉(zhuǎn)， y 軸和z 軸轉(zhuǎn)動。
通過歐拉角描述從參考坐標系到目標坐標系投影軸的旋轉(zhuǎn)，可方便地表示目標坐標系相對于參考坐標系的方位。因此，滾動、俯仰和偏航歐拉角φ和θ和ψ描述了目標坐標系α 相對于參考坐標系β 的方位。在特定的情況下，用歐拉角描述載體坐標系相對當?shù)貙Ш阶鴺讼档淖藨B(tài)時，滾動角φ被稱作側(cè)傾角(bank) ，俯仰角比被稱作仰角( elevation) ，偏航角ψ呻被稱作航向角(heading) 或方位角( azimuth) 。
2、坐標轉(zhuǎn)換矩陣
坐標轉(zhuǎn)換矩陣(coordinate transformationmatrix) 是一個3 x3 的矩陣，用符號C 表示。通過左乘適當?shù)淖鴺俗儞Q矩陣，一步就可以完成矢量在兩個投影坐標系之間的轉(zhuǎn)換。即，對任意矢量x
歐拉角通過下式就可以轉(zhuǎn)化為坐標轉(zhuǎn)換矩陣:
反之可有矩陣求得歐拉角
2、四元數(shù)姿態(tài)表示
四元數(shù)( quatemion)，即由四個元素組成的超復數(shù):
q = (q0 ,q1,q2 ,q3)
通過下式可完成四元數(shù)與坐標轉(zhuǎn)換矩陣之間的變換：
四元數(shù)和歐拉角之間的變換為
二、姿態(tài)解算步驟及原理
1、讀取加速度計、陀螺儀、磁力計的數(shù)據(jù)；
2、重力加速度、磁力計數(shù)據(jù)歸一化；
norm= sqrt(ax*ax + ay*ay + az*az);
ax= ax /norm;
ay= ay / norm;
az= az / norm;
norm= sqrt(mx*mx + my*my + mz*mz);
mx= mx / norm;
my= my / norm;
mz= mz / norm;
3、重力分量（vx，vy，vz）提取，即將n系中z軸向量（0，0，1）通過坐標轉(zhuǎn)換矩陣變換到b系中，變換后（vx，vy，vz）與（ax，ay，az）同處于b系中；
vx= 2*(q1q3 - q0q2);
vy= 2*(q0q1 + q2q3);
vz= q0q0 - q1q1 - q2q2 + q3q3 ;
4、求重力分量的姿態(tài)誤差，向量（vx，vy，vz）與（ax，ay，az）誤差通過向量叉乘計算獲取；
ex= (ay*vz - az*vy)* Accel_magnitude;
ey= (az*vx - ax*vz)* Accel_magnitude;
ez= (ax*vy - ay*vx)* Accel_magnitude;
Accel_magnitude為加速度數(shù)據(jù)的可靠度
5、在n系中，地磁方向為恒定量，計做（bx，by，bz），其中（bx，by）合成矢量指向地磁場N極，假定bx對準地磁場N極，那么by= 0（頭的定義）。
假如可以求出（bx，0，bz），則可按照與加速度計相同矯正法矯正。
地磁計在b系中的輸出為（mx，my，mz），經(jīng)過坐標轉(zhuǎn)換矩陣（轉(zhuǎn)置）變換到n系中，則有（hx，hy，hz），則有向量（hx，hy）與向量bx在XOY平面上合成矢量相同，n系中z軸向hz與bz相同，則有
bx2 = hx2 + hy2
bz = hz
即利用坐標轉(zhuǎn)換矩陣求出了（bx，0，bz）。
hx = 2*mx*(0.5 - q2q2 - q3q3) +2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 -q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 +q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;
再將（bx，0，bz）通過坐標轉(zhuǎn)換矩陣變換到b系中得（wx，wy，wz），此時有（wx，wy，wz）與 （mx，my，mz）同時處于b系。
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
6、求重力分量的姿態(tài)誤差、磁力計的姿態(tài)誤差，向量（wx，wy，wz）與（mx，my，mz）誤差通過向量叉乘計算獲取；
ex= ex + (my*wz - mz*wy);
ey= ey + (mz*wx - mx*wz);
ez= ez + (mx*wy - my*wx);
7、對重力分量與磁力計的姿態(tài)誤差進行積分
exInt= exInt + ex * Ki;
eyInt= eyInt + ey * Ki;
ezInt= ezInt + ez * Ki;
8、互補濾波、將誤差補償?shù)浇撬俣壬希拚撬俣鹊姆e分漂移
gx= gx + Kp*ex + exInt;
gy= gy + Kp*ey + eyInt;
gz= gz + Kp*ez + ezInt;
9、利用一階龍格庫塔法（畢卡法）解四元數(shù)微分方程，更新四元數(shù)
q0= q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1= q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2= q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3= q3 + (q0*gz + q1*gy - q2*gx)*halfT;
10、四元數(shù)歸一化
norm= sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0= q0 / norm;
q1= q1 / norm;
q2= q2 / norm;
q3= q3 / norm;
11、四元素轉(zhuǎn)換為歐拉角
Roll = -atan2(2 * q2 * q3 + 2 * q0 *q1, -2 * q1 * q1 - 2 * q2* q2 + 1)*57.29578;
Pitch = -asin(-2 * q1 * q3 + 2 * q0*q2)*57.29578;
Mag_Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 *q3* q3 + 1)* 57.29578;
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
