四軸飛行器1.1 Matlab 姿態顯示

                                                             四軸飛行器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