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中的数学基础 第三篇 李群与李代数