四軸飛行器1.1 Matlab 姿態顯示

?開始做四軸了，一步一步來，東西實在很多，比較雜。先做matlab上位機，主要用來做數據分析，等板子到了可以寫飛控的程序了，從底層一層一層開始寫。。希望能好好的完成它。。。關于matlab上位機，首先做個姿態顯示，然后等板子來了，把板子底層程序寫好后，加上matlab的串口接收部分，基本的環境就算搭建好了。。。。

? ?? ???這個代碼寫了一天，寫到最后出現戲劇性的一幕，實在是太惡心了哈。。開始自己的想法就是通過輸入pitch roll yaw三個歐拉角，然后在空間中現實飛機的姿態，為了學習matlab翻了matlab的書，還看了線性代數，為了畫這個姿態圖，看了高中的立體解析幾何，向量運算等。。。都是淚啊，說回正題，首先計算xOy平面中的轉動，也就是yaw軸，這個相對比較簡單，讓三角形的三個點分別在圖中的大圓和小圓上，如圖所示：

yaw解決了之后就需要解決pitch了，就是俯仰角，約定是以坐標的（0 0 0）點進行旋轉的，也是兩個圓的圓心，所以算pitch只需要在xOz平面內計算，通過sin（pitch）可以算出來A B C三個點在Z軸上的坐標了，這里需要注意下，A點變換后，相對應的X軸變化是cos(pitch),y軸也是，算到這里會發現一個問題，用matlab算B C連個點的時候，只需算B或者C，解出來是有兩個解的，一個B一個C，B和C必須分辨清楚，否則在計算roll的時候因為 B C沒有分清楚會導致roll旋轉方向不確定，后面再說B C怎么分辨。
? ?? ???接下來是計算 roll了，需要計算B 點和C點在Z軸上的坐標，因為我們是繞著（0 0 0）轉的，而不是繞著BC的終點轉，所以無法通過BC的長度乘以sin（roll）計算，所以通過圓心做一條直線與BC平行，假設與AC交與F點，

%? ?? ?? ? A

%? ?? ? E??O??F

%? ?B? ?? ?D? ???C
無論pitch和yaw怎么轉，OF都是在xOy平面的，方便計算，通過sin（roll）*OF的長度就可以得到F在Z軸的變化，從而通過等比可以的到C在Z軸的變化，B點變化和C是一樣的，方向相反，之后將B C的坐標在xOy平面做cos（roll）縮放就可以的到最終的三角形的三個坐標了。
? ?? ? 接著講BC的分辨問題，想來想去只想到一個比較簡單的方法，我們算出來BC并不知道哪個是B，哪個是C，不過我們可以制定一個B‘ 點，那就是我們取一個DB方向的方向向量n，跟隨三角形旋轉，讓它始終指向定義的DB方向，然后可以計算OB OC分別和向量n的內積，因為n與OB為銳角，與OB為鈍角，so，n 與OB點乘為負數，與OC點乘為正數，從而區分出B點和C點 。
? ?? ???上面想法看起來不錯，但是怎么讓向量n隨著yaw角轉動呢，靈機一閃，線性代數書的矩陣里面有個旋轉矩陣啊，立馬拿過來驗證，發現可以很好的運行，然后想到一個問題，如果某種情況三角形roll為90度，DB的分量在xOy平面為0，這個方法就無效了啊（其實這個問題應該不會出現，因為我們是線計算yaw 然后計算pitch，在計算pitch的時候分辨BC亮點，壓根就還沒開始計算roll），那用三維旋轉矩陣就可以解決這個問題啊，嗯嗯，又靈機一閃，之前看過捷聯慣性導航書上講了方向旋轉矩陣啊，應該可以用。把方向余弦拿過來計算一下，和用xOy平面的旋轉舉證效果一樣，到此忽然想到一個非常十分傻逼的事情，媽蛋，三角形三個點全部用這個方向余弦矩陣旋轉就可以了啊，立馬改程序，不到十分鐘就改完了，程序運行良好，都是淚。。。。。。不過自己的算法不能半途而廢啊，后面還是把自己的算法完成，并且也可以很好的運行。。。不過因為用了matlab的符號運算，速度和用方向余弦計算比起來慢很多，后面還是用方向余弦算吧。。。。。。。
下面貼代碼：

%%

%2014.7.19 由 sky.zhou 編寫

function DrawAttitude(pitch,roll,yaw)

%%

%用于顯示飛機姿態，輸入為pitch，roll，yaw。

%自己的2B算法算的太慢了，我勒個去。。。還是用方向余弦吧

mode = 2? ?? ? %標記用那種方法進行計算，1：表示用自己寫的2B算法進行計算，2表示用方向余弦矩陣進行計算

? ?

%pitch = 60;

%roll = 45;

%yaw = 35;

r1 =3;? ?? ???%大圓半徑

r2 = 0.618*r1;? ? %小圓半徑

??

if mode == 2

? ???pitch = -pitch;? ?%角度定義不一樣，改一下

? ???roll = -roll;? ???%角度定義方式不一樣，自己習慣改就好，看你希望是以怎樣的方向轉

end

dc = [cosd(yaw)*cosd(pitch)-sind(yaw)*sind(roll)*sind(pitch)? ?sind(yaw)*cosd(pitch)+cosd(yaw)*sind(roll)*sind(pitch)? ?cosd(roll)*(-sind(pitch));

? ?? ? sind(yaw)*(-cosd(roll))? ?? ?? ?? ?? ?? ?? ?? ???cosd(yaw)*cosd(roll)? ?? ?? ?? ?? ?? ?? ?? ?? ? sind(roll)? ?? ???;

? ?? ? cosd(yaw)*sind(pitch)+sind(yaw)*sind(roll)*cosd(pitch)? ?sind(yaw)*sind(pitch)-cosd(yaw)*sind(roll)*cosd(pitch)? ?? ?cosd(roll)*cosd(pitch) ]

%三角形規約：A為定點，B C為兩邊的角，具體方位如下

%? ?? ? A

%? ???B? ?C

t_fpa = 35;? ?? ?%三角形定點角度設置為40度，fpa On behalf of Fixed point angle

t_b = (180 - t_fpa) / 2;

t_c = t_b;

??

if t_fpa > asind((r2/r1))*2

? ???t_fpa = asind((r2/r1))*2

end

??

%xd,yd,zd存放真是數值，與符號xyz區分開來

%約定 xd yd zd 第 1 2 3 4位分別代表三角形ABC的 A、B、A、C坐標

if mode == 2

? ? xd=[3 -1.2735;3 -1.2735];

? ? yd=[0??1.3474;0??-1.3474];

? ? zd=[0 0;0 0];

? ? %上面幾個初始化的點是根據 定義的。

? ? %pitch = 0;

? ? %roll = 0;

? ? %yaw = 0;

? ? %r1 =3;? ?? ???%大圓半徑

? ? %r2 = 0.618*r1;? ? %小圓半徑

else

? ? xd=[];

? ? yd=[];

? ? zd=[];

? ? tempA =[];? ???%保存中間計算角度，目前之用來保存角BOA

end

? ? temp = [];

if mode == 2

? ? temp = [xd(1,1) yd(1,1) zd(1,1);

? ?? ?? ?? ?xd(1,2) yd(1,2) zd(1,2);

? ?? ?? ?? ?xd(2,2) yd(2,2) zd(2,2)];

? ? temp = temp*dc;

? ? xd = [temp(1:2,1)';temp(1,1),temp(3,1)]

? ? yd = [temp(1:2,2)';temp(1,2),temp(3,2)]

? ? zd = [temp(1:2,3)';temp(1,3),temp(3,3)]

? ? %到此位置，方向余弦矩陣已經計算完畢，可以直接用后面的函數進行顯示

end

??

if mode == 1? ?? ?%執行自己的2B算法

%xs ys zs分別問記錄方程的解 xs 為sysm縮寫

syms x y z r xs ys zs;? ?? ?? ?? ?? ?? ?? ?? ?? ?? ?? ?? ???%x y z 慣性坐標系中三個正交基，r為xOy平面中的大圓和小圓半徑

%定義各點的坐標符號參數

syms xa ya za xb yb zb za zb zc ;

??

%%

c1 = sym('x^2+y^2 = r^2');? ?? ?? ?? ?? ?? ?? ?? ? %大圓方程

c1 = subs(c1,'r',r1)? ?? ?? ?? ?? ?? ?? ?? ?? ?? ? %換成實際數值

??

c2 = sym('x^2+y^2 = r^2');? ?? ?? ?? ?? ?? ?? ?? ? %校園方程，可以表達為：c2 = 'x^2+y^2 = r^2'，效果是一樣的

c2 = subs(c2,'r',r2)

??

l1 = sym('cosd(yaw)*y=sind(yaw)*x')

%l1 = sym('y=tand(yaw)*x')? ?? ?? ?? ?? ?? ?? ?? ? %不用這個公式是因為這個公式有零點，90和-90無法使用

%l1 = subs(l1, 'yaw', yaw)? ?? ?? ?? ?? ?? ?? ?? ? %換成實際數值,這里不要轉成實際數值，為了方便subs的運算

%%

[xs ys] = solve(c1,l1,'x','y')? ?? ?? ?? ?? ?? ?? ?%注意，這里算出來的xd yd是符號變量，matlab自動轉換了，下面重新對其賦值，可以變回數值變量

? ?

%雙百分號還可以類似于分類的作用，挺好。

temp = subs([xs;ys])

??

%%

%計算A點坐標

if yaw > -90 && yaw < 90? ?? ?? ?? ?? ?? ?? ?? ?? ???%判斷角度的范圍，用來選擇在坐標中三角形的頂點是正還是負

? ???%這個可能有點難理解，角度確定了，就可以知道焦點在x軸的正負，從前兩個數值中取對應的X解后，然后取對應的Y的解

? ???temp = temp([temp(1:2)>0;temp(1:2)>0])

elseif yaw == -90

? ???temp = [ 0 ;temp(temp<0)]

elseif yaw == 90

? ???temp = [ 0 ;temp(temp>0)]

else

? ???temp = temp([temp(1:2)<0;temp(1:2)<0])

end

??

%得到在XOY平面中三角形定點的第一個解

xd = [xd temp(1)]

yd = [yd temp(2)]

??

%%

%計算B點坐標

??

%temp計算出來表示的是 AB段的長度，

%? ?? ? A

%? ?? ? O

%? ? B??D??C

%其中 sind(t_b/2)*r2 表示的是OD段的長度，cosd(t_b/2)*r2是BD段的長度，

%temp計算的最終結果是AB的長度

%利用三角形邊與對面角正弦成比例進行運算

%? ?AB? ???BC? ?? ?? ? A0? ?? ?? ?? ?? ? B0

% ----- = -----? ?? ? -----? ???=? ?---------

% sin(C)??sin(A)? ???sin(角ABO)? ?? ?sin(角OAB)（ps:A的一半）

%? ?可以求出角ABO，然后通過內角和可以求出角AOB

%? ?AB? ?? ?? ?? ?? ? BO? ?? ?? ?

% -----? ?? ? =? ???--------? ?? ?可以求出AB長度，簡化代碼如下? ??

% sin(角AOB)? ?? ???sin(角OAB)? ??

% (180 - asind((r1/r2)*sind(t_fpa/2)) - (t_fpa/2)) 為角BOA的大小

tempA = sym('(180 - asind((r1/r2)*sind(t_fpa/2)) - (t_fpa/2))');

temp = sym('(r2/sind(t_fpa/2))*sind(tempA)');

tempA = subs(tempA);

temp = subs(temp);

??

??

%temp = subs(sym('sqrt(((sind(t_b/2)*r2)+r1)^2 + (cosd(t_b/2)*r2)^2)'));

??

%假設 符號 xa ya 為 A點的坐標,x,y為要求的B點坐標

temp = subs(sym('(x-xa)^2 + (y-ya)^2 = temp^2'),'temp',temp);

%將xa和ya換成數值xa和ya，嵌套換的

temp = subs(subs(temp,'xa',xd(1)),'ya',yd(1))

[xs ys] = solve(temp,c2,'x','y')? ??

??

%通過下面的計算就已經可以得到 B C的坐標了

temp = subs([xs;ys])

??

%下面需要做的是區別哪個點是A，哪個點是B。

%%

%? ? 下面是在xOy平面內的旋轉

%? ? B??

%? ? D??O??A? ?? ? yaw=0度的時候三角型在X0Y平面的方位，其中水平位置為x軸豎直方向為Y軸

%? ? C

%? ? 取一個與DB方向一樣的方向向量n（0，1）

%? ? 用旋轉矩陣讓它跟三角形同步旋轉

%? ? 因為n與OB為銳角，與OB為鈍角，so，n與OB點乘為負數，與OC點乘為正數，從而區分出B點和C點

%%

%? ? 為了避免roll為90度的時候按照之前的定義方向向量n=（0，0），區分不出來B和C點，所以用方向余弦矩陣進行計算

%方向余弦矩陣定義

%dc = [cosd(yaw)*cosd(pitch)-sind(yaw)*sind(roll)*sind(pitch)? ?sind(yaw)*cosd(pitch)+cosd(yaw)*sind(roll)*sind(pitch)? ?cosd(roll)*(-sind(pitch));

%? ?? ?sind(yaw)*(-cosd(roll))? ?? ?? ?? ?? ?? ?? ?? ???cosd(yaw)*cosd(roll)? ?? ?? ?? ?? ?? ?? ?? ?? ? sind(roll)? ?? ???;

%? ?? ?cosd(yaw)*sind(pitch)+sind(yaw)*sind(roll)*cosd(pitch)? ?sind(yaw)*sind(pitch)-cosd(yaw)*sind(roll)*cosd(pitch)? ?? ?cosd(roll)*cosd(pitch) ]

%%算到這里的時候我發現只要在xOy平面內將三角形的初始化坐標ABC三個點輸入后，用方向余弦矩陣算就可以了，然后花了10分鐘不到的時間就實現了

%不過這里還是決定把這個方法寫完。。。都是淚。。。。。。。。。。。。。。。。。

%%

n??= [0??1??0]? ?? ?? ?? ?? ?? ?? ?? ?? ?? ?? ?? ?? ???%方向向量

n??= n*dc? ?? ?? ?? ?? ?? ?? ?? ?? ?? ?? ?? ?? ?? ?? ???%對方向向量進行旋轉

%約定 xd yd zd 第 1 2 3 4位分別代表三角形ABC的 A、B、A、C坐標

n = n*[temp(1);temp(3);0]

if n > 0? ?? ? %說明夾角是銳角，該角是B點

? ???xd = [ xd temp(1) xd temp(2)]

? ???yd = [ yd temp(3) yd temp(4)]

else

? ???xd = [ xd temp(2) xd temp(1)]

? ???yd = [ yd temp(4) yd temp(3)]

end

??

%處理成變成矩陣形式

xd = [xd(1:2);xd(3:4)]

yd = [yd(1:2);yd(3:4)]

??

%當存在pitch角度的時候，X坐標做相印調整

xd = xd.*cosd(pitch)

yd = yd.*cosd(pitch)

??

??

%%

%約定 xd yd zd 第 1 2 3 4位分別代表三角形ABC的 A、B、A、C坐標

%計算z中A的坐標，其中B和C是相等的

zd = [zd sind(pitch)*r1]

??

%下面OD的長度，然后可以計算出B和C在Z軸上的坐標,也就是D點的坐標

od = (sind(tempA - 90)*r2)

%zd = [zd temp;zd temp]

??

%計算roll狀態下B和C的坐標

%? ?? ?? ? A

%? ?? ? E??O??F

%? ???B? ? D? ? C

%? ? 先計算在roll下OF的長度，然后算F在Z軸的高度，然后等比后算B和C在Z軸的高度

%下面計算OF的長度

l2 = tand(t_fpa/2)*r1

%下面計算F在Z軸上的變化高度

l2 = sind(roll)*l2

%下面計算C點在Z軸上的變化高度,通過相似三角形計算

l2 = l2*(r1+od)/r1

??

zd = [zd -l2;zd l2]

%x,y軸根據picth角度縮放

yd(:,2) = yd(:,2).*cosd(roll)

xd(:,2) = xd(:,2).*cosd(roll)

??

%額。。這方法寫的心力交瘁。。。。。。。還是方向余弦好。。。四元素再學。。。。。。

??

??

end

surf(xd,yd,zd)

axis([-3 3 -3 3 -3 3])

xlabel('X')

ylabel('Y')

zlabel('Z')

text(xd(1,1),yd(1,1),zd(1,1),'A點')

text(xd(1,2),yd(1,2),zd(1,2),'B點')

text(xd(2,2),yd(2,2),zd(2,2),'C點')

%%

%測試用圓

hold on

alpha=0:pi/20:2*pi;

x=r1*cos(alpha);

y=r1*sin(alpha);

plot(x,y);

??

hold on

x=r2*cos(alpha);

y=r2*sin(alpha);

plot(x,y);

??

hold off

end

總結

以上是生活随笔為你收集整理的四轴飞行器1.1 Matlab 姿态显示的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。