mc_att_control基础知识:向量运算和罗德里格斯旋转

向量的叉乘和点乘

在我们的mc_att_control中有我们的向量的点乘和叉乘,一般遇到的都是三维的运算( SO(3) S O ( 3 ) $SO(3)$李群).
向量点乘:假设向量 a⃗ =[a1,a2,a3] a → = [ a 1 , a 2 , a 3 ] $\vec{a}=[a_1,a_2,a_3]$和 b⃗ =[b1,b2,b3] b → = [ b 1 , b 2 , b 3 ] $\vec{b}=[b_1,b_2,b_3]$,则有: a⃗ ⋅b⃗ =a1b1+a2b2+a3b3=|a⃗ ||b⃗ |cosθ a → ⋅ b → = a 1 b 1 + a 2 b 2 + a 3 b 3 = | a → | | b → | c o s θ $\vec{a}\cdot\vec{b}=a_1b_1+a_2b_2+a_3b_3=|\vec{a}||\vec{b}|cos\theta$
向量叉乘:假设向量 a⃗ =[a1,a2,a3] a → = [ a 1 , a 2 , a 3 ] $\vec{a}=[a_1,a_2,a_3]$和 b⃗ =[b1,b2,b3] b → = [ b 1 , b 2 , b 3 ] $\vec{b}=[b_1,b_2,b_3]$,则有: a⃗ ×b⃗ =(a2b3−a3b2)i+(a3b1−a1b3)j+(a1b2−a2b1)k=|a⃗ ||b⃗ |sinθ a → × b → = ( a 2 b 3 − a 3 b 2 ) i + ( a 3 b 1 − a 1 b 3 ) j + ( a 1 b 2 − a 2 b 1 ) k = | a → | | b → | s i n θ $\vec{a}\times\vec{b}=(a_2b_3-a_3b_2)\mathbf{i}+(a_3b_1-a_1b_3)\mathbf{j}+(a_1b_2-a_2b_1)\mathbf{k}=|\vec{a}||\vec{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 π 2 $\frac{\pi}{2}$弧度（或90°）。将轴角表示看作有序对，这将是:

$$(axis,angle)= \begin{pmatrix} \begin{bmatrix} e_x\\e_y\\e_z \end{bmatrix}, \theta \end{pmatrix}= \begin{pmatrix} \begin{bmatrix} 0\\0\\1 \end{bmatrix}, \frac{\pi}{2} \end{pmatrix}=\begin{bmatrix} 0\\0\\ \frac{\pi}{2} \end{bmatrix}$$
实际上,轴角法就是利用旋转向量表示旋转,而这与旋转矩阵有很大的关系
罗德里格斯旋转
关于罗德里格斯公式的推导,我曾在一篇博客中做过: 罗德里格斯旋转推导,我们看一下罗德里格斯公式的定义就明白二者的关系了:
已知单位向量 ω=(wx,wy,wz)∈R3 ω = ( w x , w y , w z ) ∈ R 3 $\omega=(w_x,w_y,w_z)\in R^3$,将其旋转角度 θ θ $\theta$得到 Rω^(θ) R ω ^ ( θ ) $R_{\hat{\omega}}(\theta)$,则有:
$$R_{\hat{\omega}}(\theta)=\mathtt{e}^{\hat{\omega}\theta}=\mathbf{I}+\hat{\omega}sin\theta+\hat{\omega}^2(1-cos\theta)$$
其中 ω^ ω ^ $\hat{\omega}$是一个反对称矩阵:
$$\begin{bmatrix} 0&-w_z&w_y\\w_z&0&-w_x\\-w_y&w_x&0 \end{bmatrix}$$
从这也就很好的揭示了旋转向量和旋转矩阵的关系了,这里的 ω ω $\omega$就是旋转轴, θ θ $\theta$就是我们的旋转角度.一般而言,轴角法和罗德里格斯旋转的转换如下:

1. 旋转角度 θ=r.length() θ = r . l e n g t h ( ) $\theta=r.length()$,其中 length() l e n g t h ( ) $length()$函数为求向量的模长
2. 单位向量 (rx,ry,rz)=r/θ ( r x , r y , r z ) = r / θ $(r_x,r_y,r_z)=r/\theta$;
3. 反对称矩阵 ⎡⎣⎢0rz−ry−rz0rxry−rx0⎤⎦⎥ [ 0 − r z r y r z 0 − r x − r y r x 0 ] $\begin{bmatrix}0&-r_z&r_y\\r_z&0&-r_x\\-r_y&r_x&0\end{bmatrix}$
4. 罗德里格斯旋转公式: Vtarget=RV V t a r g e t = R V $\mathbf{V}_{target}=\mathbf{R}\mathbf{V}$

---

李群李代数基础知识介绍

此部分主要参考高翔博士的slam数学基础博客,可以直接看大佬的博客,来理解反对称矩阵的导数性和正切空间,以及李代数的性质,此处不做过多介绍,直接上链接:高博士对李群李代数的介绍

---

特技模式的几个曲线

在mc_att_control中,有几个在特技模式下对油门曲线的调整,原理暂时不清楚,用python把图画出来了.
exp:

supexp:

---

参考资料

PID Attenuation and scaling
轴角法-维基
罗德里格斯公式推导
视觉SLAM中的数学基础 第三篇 李群与李代数