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

向量的叉乘和点乘

在我们的mc_att_control中有我们的向量的点乘和叉乘,一般遇到的都是三维的运算( $SO(3)$ 李群).
向量点乘:假设向量 $\vec{a}=[a_1,a_2,a_3]$ 和 $\vec{b}=[b_1,b_2,b_3]$ ,则有: $\vec{a}\cdot\vec{b}=a_1b_1+a_2b_2+a_3b_3=|\vec{a}||\vec{b}|cos\theta$
向量叉乘:假设向量 $\vec{a}=[a_1,a_2,a_3]$ 和 $\vec{b}=[b_1,b_2,b_3]$ ,则有: $\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之间),如下图所示:

当油门>1500时开始衰减
当油门到达1750时,PID衰减25%
当油门到达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轴旋转 $\frac{\pi}{2}$ 弧度（或90°）。将轴角表示看作有序对，这将是:

(a x i s, a n g l e) = (\begin{matrix} [\begin{matrix} e_{x} \\ e_{y} \\ e_{z} \end{matrix}], θ \end{matrix}) = (\begin{matrix} [\begin{matrix} 0 \\ 0 \\ 1 \end{matrix}], \frac{π}{2} \end{matrix}) = [\begin{matrix} 0 \\ 0 \\ \frac{π}{2} \end{matrix}]

$(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}$
实际上,轴角法就是利用旋转向量表示旋转,而这与旋转矩阵有很大的关系
罗德里格斯旋转
关于罗德里格斯公式的推导,我曾在一篇博客中做过: 罗德里格斯旋转推导,我们看一下罗德里格斯公式的定义就明白二者的关系了:
已知单位向量

ω = (w_{x}, w_{y}, w_{z}) \in R^{3}

$\omega=(w_x,w_y,w_z)\in R^3$ ,将其旋转角度

θ

$\theta$ 得到

R_{\hat{ω}} (θ)

$R_{\hat{\omega}}(\theta)$ ,则有:

R_{\hat{ω}} (θ) = e^{\hat{ω} θ} = I + \hat{ω} s i n θ + {\hat{ω}}^{2} (1 - c o s θ)

$R_{\hat{\omega}}(\theta)=\mathtt{e}^{\hat{\omega}\theta}=\mathbf{I}+\hat{\omega}sin\theta+\hat{\omega}^2(1-cos\theta)$
其中

\hat{ω}

$\hat{\omega}$ 是一个反对称矩阵:

[\begin{matrix} 0 & - w_{z} & w_{y} \\ w_{z} & 0 & - w_{x} \\ - w_{y} & w_{x} & 0 \end{matrix}]

$\begin{bmatrix} 0&-w_z&w_y\\w_z&0&-w_x\\-w_y&w_x&0 \end{bmatrix}$
从这也就很好的揭示了旋转向量和旋转矩阵的关系了,这里的

ω

$\omega$ 就是旋转轴,

θ

$\theta$ 就是我们的旋转角度.一般而言,轴角法和罗德里格斯旋转的转换如下:

旋转角度 $\theta=r.length()$ ,其中 $length()$ 函数为求向量的模长
单位向量 $(r_x,r_y,r_z)=r/\theta$ ;
反对称矩阵 $\begin{bmatrix}0&-r_z&r_y\\r_z&0&-r_x\\-r_y&r_x&0\end{bmatrix}$
罗德里格斯旋转公式: $\mathbf{V}_{target}=\mathbf{R}\mathbf{V}$

李群李代数基础知识介绍

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

特技模式的几个曲线

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

参考资料

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