mc_att_control基礎知識:向量運算和羅德里格斯旋轉

向量的叉乘和點乘

在我們的mc_att_control中有我們的向量的點乘和叉乘,一般遇到的都是三維的運算(SO(3)$SO(3)$ 李羣).
向量點乘:假設向量a⃗ =[a1,a2,a3]$\overrightarrow{a}=[a_1,a_2,a_3]$ 和b⃗ =[b1,b2,b3]$\overrightarrow{b}=[b_1,b_2,b_3]$ ,則有:a⃗ ⋅b⃗ =a1b1+a2b2+a3b3=|a⃗ ||b⃗ |cosθ$\overrightarrow{a}\cdot \overrightarrow{b}=a_1{b}_1+a_2{b}_2+a_3{b}_3=|\overrightarrow{a}||\overrightarrow{b}|cos\theta$
向量叉乘:假設向量a⃗ =[a1,a2,a3]$\overrightarrow{a}=[a_1,a_2,a_3]$ 和b⃗ =[b1,b2,b3]$\overrightarrow{b}=[b_1,b_2,b_3]$ ,則有:a⃗ ×b⃗ =(a2b3−a3b2)i+(a3b1−a1b3)j+(a1b2−a2b1)k=|a⃗ ||b⃗ |sinθ$\overrightarrow{a}\times \overrightarrow{b}=(a_2{b}_3-a_3{b}_2)\mathbf{i}+(a_3{b}_1-a_1{b}_3)\mathbf{j}+(a_1{b}_2-a_2{b}_1)\mathbf{k}=|\overrightarrow{a}||\overrightarrow{b}|sin\theta$
從上面的定義式也可以理解爲什麼我們常用向量的叉乘表示偏差,假設兩個向量沒有偏差,則其夾角爲0,那麼其正弦也就爲0.三維向量叉乘也可由下面的圖計算:

---

Throttle PID Attenuation(TPA)

簡單的來說,相對於全油門，TPA應用PID值降低。當達到全油門時，它用於應用PID值的衰減,以此消除振盪。其有兩個最重要的參數:tpa和tpa_breakpoint,其中tpa是縮放係數,tpa_breakpoint是開始縮放的閾值.
舉個例子,tpa=50(%),tpa_breakpoint=1500(我們知道飛機的PWM在1000-2000之間),如下圖所示:

1. 當油門>1500時開始衰減
2. 當油門到達1750時,PID衰減25%
3. 當油門到達2000(全油門)時,PID衰減50%
   
   對於TPA能消除振盪,簡單的寫個matlab就可以直觀的看出來,運行結果如下:
   
   
   程序如下:

        %設一被控對象G（s）=50/(0.125s^2+7s),  
    %用增量式PID控制算法編寫仿真程序  
    %（輸入分別爲單位階躍、正弦信號，採樣時間爲1ms，控制器輸出限幅：[-5,5],  
    %  仿真曲線包括系統輸出及誤差曲線，並加上註釋、圖例）。  

    clear all;  
    close all; 
    tpa=0.5,tpa_breakpoint=0,25;

    kp_cst=100;ki_cst=0.1;kd_cst=10; 
    kpp=kp_cst;kii=ki_cst;kdd=kd_cst;
    ts=0.001;                 %採樣時間  
    sys=tf(50,[0.125,7, 0]); %tf是傳遞函數  即被控對象函數G（）;  
    dsys=c2d(sys,ts,'z');    %把控制函數離散化  
    [num,den]=tfdata(dsys,'v');% 離散化後提取分子、分母    
    u_1=0.0;  
    u_2=0.0;  
    y_1=0.0;  
    y_2=0.0;  
    x=[0,0,0]';  
    error_1=0;  
    error_2=0;  
    for k=1:1:1000  
    time(k)=k*ts;                        %採樣次數  
    S=2;  
    if S==1  
        kp=50;ki=0.1;kd=15;             %初始化PID    
        rin(k)=1;                       %Step Signal   
    elseif S==2  
        kp=kpp;ki=kii;kd=kdd;               
        rin(k)=0.5*sin(2*pi*k*ts);       %Sine Signal     即實際輸入      
    end   
    du(k)=kp*x(1)+kd*x(2)+ki*x(3);      %PID Controller   控制係數    
    u(k)=u_1+du(k);                     %Restricting the output of controller  
    if u(k)>=5         
       u(k)=5;  
    end  
    if u(k)<=-5  
       u(k)=-5;  
    end  
    %Linear model  
    yout(k)=-den(2)*y_1-den(3)*y_2+num(2)*u_1+num(3)*u_2;          %實際輸出  
    error(k)=rin(k)-yout(k);    %Return of parameters 誤差  
    if yout(k)>0.25
         %kpp=kp_cst;kii=ki_cst;kdd=kd_cst;
      kpp=kp_cst-2*kp_cst*(yout(k)-0.25);kii=ki_cst ;kdd=kd_cst ;
    else
       kpp=kp_cst;kii=ki_cst;kdd=kd_cst;
    end
    u_2=u_1;                                                       %保存上上次輸入   爲下次計算  
    u_1=u(k);                                                      %保存上一次控制係數   爲下次計算  
    y_2=y_1;                                                       %保存上上次次輸出   爲下次計算  
    y_1=yout(k);                                                   %保存上一次輸出   爲下次計算  
    x(1)=error(k)-error_1;                                         %Calculating P  
    x(2)=error(k)-2*error_1+error_2;                               %Calculating D  
    x(3)=error(k);                                                 %Calculating I   
    error_2=error_1;                      
    error_1=error(k);                      
    end  
    figure(1);  
    plot(time,rin,'b',time,yout,'r');                        %輸入 和實際控制輸出  
    xlabel('time(s)'),ylabel('rin,yout');   

---

軸角法和羅德里格斯旋轉推導

軸角法:簡單的來說,用單位向量表示旋轉方向,用向量的模長代表旋轉的角度θ$\theta$ ,舉個例子:
假設你站在地面上，並選擇重力方向作爲負z方向。然後，如果轉向左側，則會圍繞z軸旋轉π2$\frac{\pi }{2}$ 弧度（或90°）。將軸角表示看作有序對，這將是:

(axis,angle)=⎛⎝⎜⎡⎣⎢exeyez⎤⎦⎥,θ⎞⎠⎟=⎛⎝⎜⎡⎣⎢001⎤⎦⎥,π2⎞⎠⎟=⎡⎣⎢00π2⎤⎦⎥$$(axis,angle)=\left(\begin{array}{c}\left[\begin{array}{c}{e}_x\\ e_y\\ e_z\end{array}\right],\theta \end{array}\right)=\left(\begin{array}{c}\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right],\frac{\pi }{2}\end{array}\right)=\left[\begin{array}{c}0\\ 0\\ \frac{\pi }{2}\end{array}\right]$$

實際上,軸角法就是利用旋轉向量表示旋轉,而這與旋轉矩陣有很大的關係
羅德里格斯旋轉
關於羅德里格斯公式的推導,我曾在一篇博客中做過:羅德里格斯旋轉推導,我們看一下羅德里格斯公式的定義就明白二者的關係了:
已知單位向量ω=(wx,wy,wz)∈R3$\omega =(w_x,w_y,w_z)\in R^3$ ,將其旋轉角度θ$\theta$ 得到Rω^(θ)$R_{\hat{\omega }}(\theta )$ ,則有:

Rω^(θ)=eω^θ=I+ω^sinθ+ω^2(1−cosθ)$$R_{\hat{\omega }}(\theta )={\mathtt{e}}^{\hat{\omega }\theta }=\mathbf{I}+\hat{\omega }sin\theta +{\hat{\omega }}^2(1-cos\theta )$$

其中ω^$\hat{\omega }$ 是一個反對稱矩陣:

⎡⎣⎢0wz−wy−wz0wxwy−wx0⎤⎦⎥$$\left[\begin{array}{ccc}0& -w_z& w_y\\ w_z& 0& -w_x\\ -w_y& w_x& 0\end{array}\right]$$

從這也就很好的揭示了旋轉向量和旋轉矩陣的關係了,這裏的ω$\omega$ 就是旋轉軸,θ$\theta$ 就是我們的旋轉角度.一般而言,軸角法和羅德里格斯旋轉的轉換如下:

1. 旋轉角度θ=r.length()$\theta =r.length()$ ,其中length()$length()$ 函數爲求向量的模長
2. 單位向量(rx,ry,rz)=r/θ$(r_x,r_y,r_z)=r/\theta$ ;
3. 反對稱矩陣⎡⎣⎢0rz−ry−rz0rxry−rx0⎤⎦⎥$\left[\begin{array}{ccc}0& -r_z& r_y\\ r_z& 0& -r_x\\ -r_y& r_x& 0\end{array}\right]$
4. 羅德里格斯旋轉公式:Vtarget=RV${\mathbf{V}}_{target}=\mathbf{R}\mathbf{V}$

---

李羣李代數基礎知識介紹

此部分主要參考高翔博士的slam數學基礎博客,可以直接看大佬的博客,來理解反對稱矩陣的導數性和正切空間,以及李代數的性質,此處不做過多介紹,直接上鍊接:高博士對李羣李代數的介紹

---

特技模式的幾個曲線

在mc_att_control中,有幾個在特技模式下對油門曲線的調整,原理暫時不清楚,用python把圖畫出來了.
exp:

supexp:

---

參考資料

PID Attenuation and scaling
軸角法-維基
羅德里格斯公式推導
視覺SLAM中的數學基礎 第三篇 李羣與李代數