|
|
一、姿態(tài)軸系介紹
1、歐拉角姿態(tài)表示
歐拉(Euler) 角是描述姿態(tài)最直觀的方法，尤其是在描述載體坐標(biāo)系相對(duì)于對(duì)應(yīng)的當(dāng)?shù)貙?dǎo)航坐標(biāo)系的運(yùn)動(dòng)時(shí)。（應(yīng)用在慣性導(dǎo)航中）
姿態(tài)被分解為三個(gè)連續(xù)的轉(zhuǎn)動(dòng)過(guò)程，每次旋轉(zhuǎn)所圍繞的軸與前后旋轉(zhuǎn)所圍繞的軸正交。圖1 展示了這種旋轉(zhuǎn)過(guò)程，通過(guò)兩個(gè)過(guò)渡坐標(biāo)系ψ和θ ，將與坐標(biāo)系β 對(duì)齊的坐標(biāo)系，旋轉(zhuǎn)到與坐標(biāo)系α 對(duì)齊。
圖1 歐拉角的轉(zhuǎn)動(dòng)
第一次的轉(zhuǎn)動(dòng)角度ψ是偏航角，繞β 坐標(biāo)系和第一個(gè)過(guò)渡坐標(biāo)系共同的z軸旋轉(zhuǎn)，x 軸和y 軸轉(zhuǎn)動(dòng)， z 軸保持不變；下一個(gè)旋轉(zhuǎn)角為俯仰角θ，繞第一個(gè)過(guò)渡坐標(biāo)系和第二個(gè)過(guò)渡坐標(biāo)系共同的y 軸旋轉(zhuǎn)， x軸和z 軸轉(zhuǎn)動(dòng);最后一個(gè)旋轉(zhuǎn)角為滾動(dòng)角φ御，繞第二個(gè)過(guò)渡坐標(biāo)系和α 坐標(biāo)系共同的z 軸旋轉(zhuǎn)， y 軸和z 軸轉(zhuǎn)動(dòng)。
通過(guò)歐拉角描述從參考坐標(biāo)系到目標(biāo)坐標(biāo)系投影軸的旋轉(zhuǎn)，可方便地表示目標(biāo)坐標(biāo)系相對(duì)于參考坐標(biāo)系的方位。因此，滾動(dòng)、俯仰和偏航歐拉角φ和θ和ψ描述了目標(biāo)坐標(biāo)系α 相對(duì)于參考坐標(biāo)系β 的方位。在特定的情況下，用歐拉角描述載體坐標(biāo)系相對(duì)當(dāng)?shù)貙?dǎo)航坐標(biāo)系的姿態(tài)時(shí)，滾動(dòng)角φ被稱作側(cè)傾角(bank) ，俯仰角比被稱作仰角( elevation) ，偏航角ψ呻被稱作航向角(heading) 或方位角( azimuth) 。
2、坐標(biāo)轉(zhuǎn)換矩陣
坐標(biāo)轉(zhuǎn)換矩陣(coordinate transformationmatrix) 是一個(gè)3 x3 的矩陣，用符號(hào)C 表示。通過(guò)左乘適當(dāng)?shù)淖鴺?biāo)變換矩陣，一步就可以完成矢量在兩個(gè)投影坐標(biāo)系之間的轉(zhuǎn)換。即，對(duì)任意矢量x
歐拉角通過(guò)下式就可以轉(zhuǎn)化為坐標(biāo)轉(zhuǎn)換矩陣:
反之可有矩陣求得歐拉角
2、四元數(shù)姿態(tài)表示
四元數(shù)( quatemion)，即由四個(gè)元素組成的超復(fù)數(shù):
q = (q0 ,q1,q2 ,q3)
通過(guò)下式可完成四元數(shù)與坐標(biāo)轉(zhuǎn)換矩陣之間的變換：
四元數(shù)和歐拉角之間的變換為
二、姿態(tài)解算步驟及原理
1、讀取加速度計(jì)、陀螺儀、磁力計(jì)的數(shù)據(jù)；
2、重力加速度、磁力計(jì)數(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）通過(guò)坐標(biāo)轉(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）誤差通過(guò)向量叉乘計(jì)算獲取；
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系中，地磁方向?yàn)楹愣�，�?jì)做（bx，by，bz），其中（bx，by）合成矢量指向地磁場(chǎng)N極，假定bx對(duì)準(zhǔn)地磁場(chǎng)N極，那么by= 0（頭的定義）。
假如可以求出（bx，0，bz），則可按照與加速度計(jì)相同矯正法矯正。
地磁計(jì)在b系中的輸出為（mx，my，mz），經(jīng)過(guò)坐標(biāo)轉(zhuǎn)換矩陣（轉(zhuǎn)置）變換到n系中，則有（hx，hy，hz），則有向量（hx，hy）與向量bx在XOY平面上合成矢量相同，n系中z軸向hz與bz相同，則有
bx2 = hx2 + hy2
bz = hz
即利用坐標(biāo)轉(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）通過(guò)坐標(biāo)轉(zhuǎn)換矩陣變換到b系中得（wx，wy，wz），此時(shí)有（wx，wy，wz）與 （mx，my，mz）同時(shí)處于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)誤差、磁力計(jì)的姿態(tài)誤差，向量（wx，wy，wz）與（mx，my，mz）誤差通過(guò)向量叉乘計(jì)算獲�。�
ex= ex + (my*wz - mz*wy);
ey= ey + (mz*wx - mx*wz);
ez= ez + (mx*wy - my*wx);
7、對(duì)重力分量與磁力計(jì)的姿態(tài)誤差進(jìn)行積分
exInt= exInt + ex * Ki;
eyInt= eyInt + ey * Ki;
ezInt= ezInt + ez * Ki;
8、互補(bǔ)濾波、將誤差補(bǔ)償?shù)浇撬俣壬�，修正角速度的積分漂移
gx= gx + Kp*ex + exInt;
gy= gy + Kp*ey + eyInt;
gz= gz + Kp*ez + ezInt;
9、利用一階龍格庫(kù)塔法（畢卡法）解四元數(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空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
