Aerial Robotics 第 3 讲

写在前面

本周主要介绍了四旋翼的控制理论和路径规划算法。

课后作业是四旋翼的二维pid控制,需要沿着规定的路线对四旋翼的控制进行仿真,并达到一定的精度。

欢迎关注我的个人公众号「我不是药丸」,查看更多原创内容。

四旋翼控制

2维

y-z平面上的四旋翼运动和控制。

运动方程

惯性坐标系为$\mathcal{A}$,机体坐标系为$\mathcal{B}$,由机体坐标系到惯性坐标系的旋转矩阵为:

$$^{\mathcal{A}}[R]_{\mathcal{B}}=\begin{bmatrix}cos(\phi)&-sin(\phi)\\sin(\phi)&cos(\phi)\end{bmatrix}$$

file
设$r=[y,z]^T$,作用在四旋翼上的有$\vec{b_3}$方向的升力和$\vec{a_3}$方向的重力,进行坐标系转换,可以写出四旋翼的力平衡方程:

$$m\ddot{r}=\begin{bmatrix}0\\-mg\end{bmatrix}+^{\mathcal{A}}[R]_{\mathcal{B}}\begin{bmatrix}0\\u_1\end{bmatrix}\\ u_1=\sum^4_{i=1}F_i$$

力矩平衡方程为,L是旋翼臂长:

$$I_{xx}\ddot{\phi}=L(F_1-F_2)=u_2$$

则假设俯仰和偏航角均为零,Y-Z平面上的四旋翼的运动方程如下:

$$\begin{bmatrix}\ddot{y}\\\ddot{z}\\\ddot{\phi}\end{bmatrix}= \begin{bmatrix}0\\-g\\0\end{bmatrix}+ \begin{bmatrix}-\frac{1}{m}sin(\phi)&0\\\frac{1}{m}cos(\phi)&0\\0&\frac{1}{I_{xx}}\end{bmatrix} \begin{bmatrix}u_1\\u_2\end{bmatrix}\\$$

状态变量表示

定义状态变量。

$$x=[y,z,\phi,\dot{y},\dot{z},\dot{\phi}]^T$$

则可以将该方程写为状态空间的形式:

$$\dot{x}=\begin{bmatrix}\dot{y}\\\dot{z}\\\dot{\phi}\\0\\-g\\0\end{bmatrix}+ \begin{bmatrix}0&0\\0&0\\0&0\\-\frac{1}{m}sin(\phi)&0\\\frac{1}{m}cos(\phi)&0\\0&\frac{1}{I_{xx}}\end{bmatrix} \begin{bmatrix}u_1\\u_2\end{bmatrix}$$

线性化

由于pd控制器只适用于线性系统,因此在使用之前需要将系统线性化。当四旋翼悬停时,可以将运动线性化,引入小扰动假设,这时有:$sin(\phi)\approx \phi,cos(\phi)\approx1$,可以将四旋翼的运动方程线性简化为。

$$\ddot{y}=-g\phi\\ \ddot{z}=\frac{u_1}{m}-g \\ \ddot{\phi}=\frac{u_2}{I_{xx}}$$

控制方程

file
已知$r_T(t),\dot{r}_T(t),\ddot{r}_T(t)$,$\ddot{r}_C$为控制器计算出的指令加速度,则控制方程如下:

$$e_p=r_T(t)-r\\ e_v=\dot{r}_T(t)-\dot{r}\\ want:(\ddot{r}_T(t)-\ddot{r}_c)+k_de_v+k_pe_p=0$$

$$\ddot{r}_c=\ddot{r}_T(t)+k_de_v+k_pe_p$$

由控制方程可以计算得到该四旋翼所应输出的$u_1,u_2$ 。

$$u_1=mg+m\ddot{z_c}=m(g+\ddot{z}_{des}+k_{d,z}(\dot{z}_{des}-\dot{z})+k_{p,z}(z_{des}-z))\\ u_2=I_{xx}\ddot{\phi}_T(t)=I_{xx}(\ddot{\phi_c+k_{p,\phi}(\phi_c-\phi)+k_{d,\phi}(\dot\phi_c-\dot\phi)}),\\ \ddot{\phi_c}=-\frac{\ddot{y_c}}{g}=\frac{1}{g}(\ddot{y_T}(t)+k_{v,y}(\dot{y}_T(t)-\dot{y})+k_{p,y}(y_T(t)-y))$$

在期望轨迹为悬停的特殊情况下(期望位置为常数,期望滚转角为零),有$R_T(t)=[y_0,z_0]^T,\dot{r}_T(t)=\ddot{r}_T(t)=0$。

作业

本周作业为二维控制,目标是控制四旋翼走一个直线和一个sin形曲线,需达到一定精度要求方为作业合格。

本周的作业编程本身其实并不难,比较耗时间的是调参,如果四旋翼的飞行不理想,应该首先观察自己四旋翼的飞行过程,看看是哪里出了问题,在这周的课程论坛里,还有一篇文章讲了调参的一些技巧。

你在调参的时候会发现,滚转角太大的时候,四旋翼容易失稳,这是因为我们用的是简化过的线性控制器,角度太大的时候会引入非线性因素,控制器容易失稳。

如果实在调不出来了,可以参考我调的参数。

my code
function [ u1, u2 ] = controller(~, state, des_state, params)
%CONTROLLER  Controller for the planar quadrotor
%
%   state: The current state of the robot with the following fields:
%   state.pos = [y; z], state.vel = [y_dot; z_dot], state.rot = [phi],
%   state.omega = [phi_dot]
%
%   des_state: The desired states are:
%   des_state.pos = [y; z], des_state.vel = [y_dot; z_dot], des_state.acc =
%   [y_ddot; z_ddot]
%
%   params: robot parameters

%   Using these current and desired states, you have to compute the desired
%   controls

% u1 = 0;
% u2 = 0;
kp_z = 30;%达到des_z的时间
kv_z = 10;%减慢z轴的晃动
kp_phi = 200;%将该参数放得更大,可以使飞行时的phi更接近0,这时飞行更加稳定
kv_phi = 20;%使四旋翼晃动更慢
kp_y = 1;%达到des_y的时间
kv_y = 10;%减小y轴的晃动
phi_c = -1/params.gravity*(des_state.acc(1) + ...
    kv_y*(des_state.vel(1)-state.vel(1)) + ...
    kp_y*(des_state.pos(1)-state.pos(1)));
phi_dc = 0;
phi_ddc = 0;
u1 =params.mass*(params.gravity + des_state.acc(2) + ...
    kv_z*(des_state.vel(2) - state.vel(2)) + ...
    kp_z*(des_state.pos(2) - state.pos(2)));
u2 =params.Ixx*(phi_ddc + ...
    kv_phi*(phi_dc - state.omega) + ...
    kp_phi*(phi_c - state.rot));
% debug code
% for i = 1:500
% x = trajhandle(i);
% H(i,:) = [x.pos(1),x.pos(2),x.vel(1),x.vel(2),x.acc(1),x.acc(2)];
% end

if(u1 > params.maxF)
    u1=params.maxF;
end

M_max = params.maxF*params.arm_length;

if(u2 > M_max)
    u2 = M_max;
end
% FILL IN YOUR CODE HERE

end
仿真结果

file
可以看出,最后的结果还是比较令人满意的。

3维

第四周的编程作业中有3维控制的内容,对其编程细节感兴趣的读者可以看后续第四周的笔记。

运动方程

惯性坐标系为$\mathcal{A}$,机体坐标系为$\mathcal{B}$,坐标系表示如下所示:
file
由机体坐标系到惯性坐标系的旋转矩阵为:

$$R = \begin{bmatrix} C\psi C\theta - S\phi S\psi S\theta&-C\phi S\psi&C\psi S\theta + C\theta S\phi S\psi\\C\theta S\psi + C\psi S\phi S\theta&C\phi C\psi &S\psi S\theta -C\theta S\phi C\psi\\-C\phi S\theta&S\phi &C\phi C\theta \end{bmatrix}$$

姿态角变化率和机体角速度的关系如下(某些教材上与课程课件表述不一致,暂时使用课件上的表述,此处存疑):

$$\begin{bmatrix}p\\q\\r\end{bmatrix}= \begin{bmatrix}C\theta&0&-C\phi S\theta\\0&1&S\theta\\S\theta&0&C\phi C\theta\end{bmatrix} \begin{bmatrix}\dot\phi\\\dot\theta\\\dot\psi\end{bmatrix}$$

平衡方程

力的平衡方程

$$m\ddot r=\begin{bmatrix}0\\0\\-mg\end{bmatrix}+ R\begin{bmatrix}0\\0\\F_1+F_2+F_3+F_4\end{bmatrix}$$

将第一个输出定义为$u1=\sum{i=1}^4F_i$
力矩平衡方程

$$I\begin{bmatrix}\dot p\\\dot q \\\dot r\end{bmatrix}= \begin{bmatrix}L(F_2-F_4)\\L(F_3-F_1)\\M_1-M_2+M_3-M_4\end{bmatrix}- \begin{bmatrix}p\\q\\r\end{bmatrix}\times I\begin{bmatrix}p\\q\\r\end{bmatrix}$$

对$u_2$又可以列动量矩定理:

$$u_2=I\begin{bmatrix}\ddot \phi\\ \ddot \theta \\ \ddot\psi \end{bmatrix}=\begin{bmatrix}L(F_2-F_4)\\L(F_3-F_1)\\M_1-M_2+M_3-M_4\end{bmatrix}$$

线性化

将力的平衡方程线性化:

$$\ddot r_1=g(\Delta\theta cos(\psi_0)+\Delta\phi sin(\psi_0))\\ \ddot r_2=g(\Delta \theta sin(\psi_0)-\Delta\phi cos(\psi_0))\\ \ddot r_3 = \frac{1}{m}u_1-g$$

将力矩平衡方程线性化,$\gamma=\frac{K_M}{K_F}$:

$$\dot p = \frac{u_{2,x}}{I_{xx}}(F_2-F_4)\\ \dot q = \frac{u_{2,y}}{I_{yy}}(F_3-F_1)\\ \dot r = \frac{u_{2,z}}{I_{zz}}=\frac{\gamma}{I_{zz}}(F_1-F_2+F_3-F_4)$$

控制方程

file
3维四旋翼控制问题可以这样描述:以悬停或循迹$z_{des}$为目标,找出四维输入量${u_1,\vec{u_2}}$。
如上图所示,内部的姿态控制器用板载加速度计和陀螺仪来控制滚转、俯仰和偏航姿态角度,运行频率约为1000 Hz。外部的位置控制环则使用速度和位置估计来控制航迹,运行频率约为100-200 Hz。

姿态控制

联立下列控制方程和动力学方程:

$$(\ddot{r}_T(t)-\ddot{r}_c)+k_de_v+k_pe_p=0\\ u_2=I\begin{bmatrix}\ddot \phi& \ddot \theta & \ddot\psi \end{bmatrix}^T$$

将转动惯量并入增益,可以得到$u_2$的解:

$$u_2=\begin{bmatrix} k_{p,\phi}(\phi_{des}-\phi)+k_{d,\phi}(p_{des}-p)\\ k_{p,\theta}(\theta_{des}-\theta)+k_{d,\theta}(q_{des}-q)\\ k_{p,\psi}(\psi_{des}-\psi)+k_{d,\psi}(r_{des}-r) \end{bmatrix}$$

又由z方向上的力的平衡方程和控制方程联立,可以得到$u_1$的解:

$$u_1=mg+m\ddot r_{3,des}=mg-m\{k_{d,3} (\dot r_3-\dot r_{3,T})+k_{p,3}(r_3-r_{3,T})\}$$

由x,y方向上的力的平衡方程可得:

$$\phi_{des}=\frac{1}{g}(\ddot r_{1,des}sin(\psi_T)-\ddot r_{2,des}cos(\psi_T))\tag{1}$$$$ \theta_{des}=\frac{1}{g}(\ddot r_{1,des}cos(\psi_T)+\ddot r_{2,des}sin(\psi_T))\tag{2}$$

忽略高阶导数项,有:

$$p_{des}=0\tag{3}$$$$ q_{des}=0\tag{4}$$

由于偏航角是独立的,因此有:

$$\psi_{des}=\psi_T(t)\tag{5}$$$$ r_{des}=\dot \psi_T(t)\tag{6}$$

悬停控制和三维循迹控制

悬停控制用于将位置保持在某个位置,而三维循迹控制可以用于跟踪指定的轨迹。

悬停控制

对于悬停控制,位置误差定义如下:

$$ei=r{i,T}-r_i$$

联立控制方程如下,即可求得$\ddot r_{i,des}$:

$$(\ddot r_{i,T}-\ddot r_{i,des})+k_{d,i}\dot e_i+ k_{p,i}e_i=0$$

对于悬停状态,有$\dot r{i,T}=\ddot r{i,T}=0$
再联立(1)-(6)式,即可求得$u_1,u_2$

三维循迹控制

三维循迹控制控制的前提是准悬停假设成立(near-hover assumptions),以保证系统仍是线性的,但 $ \dot r{i,T} $ 和 $\ddot r{i,T}$ 不再为零,而是由具体的航迹来决定。

设$r_T$ 表示目标航迹上距离当前位置 $r$ 最近的点, $\dot r_T,\ddot r_T$ 分别是目标速度和目标加速度。轨迹 $r_T$ 的单位切向量为 $\hat t$ ,单位法向量为 $\hat n$ ,单位副法向量为 $\hat b =\hat t \times \hat n$。

则误差为:

$$e_p=((r_T-r)\hat n)\hat n+ ((r_T-r)\hat b)\hat b\e_v=\dot r_T -\dot r$$

注意上式中的比例误差忽略了切线方向的误差,并且只考虑了目标航迹上最近的点。

据新定义的误差,我们联立控制方程,即可求得 $ \ddot r_{i,des}$ ,再联立(1)-(6)式,即可求得 $u_1,u_2$。

航迹规划(planning)

  • 起始点位、终止点位
  • 航路点位
  • smoothness原则:将输入的变化率减到最小
  • 系统阶数:系统阶数决定了输入的边界条件
    数学补充:变分法

$$x^*(t)=\min_{x(t)} \int_0^T\mathscr{L}(\dot x,x,t)dt$$

给定起始点$x(0)$和终点$x(T)$,考虑所有的可微曲线$x(t)$。
对于最优函数 $ x(t) $ ,它必须遵循欧拉-拉格朗日方程:

$$\frac{d}{dt}(\frac{\partial \mathscr{L}}{\partial \dot x})-\frac{\partial \mathscr{L}}{\partial x}=0$$

  • 最短距离 (几何):

$$x^*(t)=\min_{x(t)}\int_0^T\dot x^2 dt$$

  • 费马原理(光学):

$$x^*(t)=\min \int_0^T 1 dt$$

  • 最小作用量原理(机械):

$$x^*(t)=\min_{x(t)}\int_0^T{T(\dot x,x,t)-V(x,t)}dt$$

对一个一阶系统:

$$x^*(t)=\min_{x(t)}\int_0^T\dot x^2dt$$

代入欧拉-拉格朗日方程得到:

$$(\dot x)^2 \rightarrow \ddot {x}=0 \rightarrow x=c_1t+c_0$$

由给出的$x(0)$和$x(T)$可以求出$c_1,c_0$

对一个n阶系统:

$$x^*(t)=\min_{x(t)}\int_0^T (x^{(n)})^2dt$$

输入为$x^{(n)}$

$$\frac{\partial \mathscr{L}}{\partial x}- \frac{d}{dt}(\frac{\partial \mathscr{L}}{\partial \dot x})+\frac{d^2}{dt^2}(\frac{\partial \mathscr{L}}{\partial \ddot x})+...+(-1)^n\frac{d^n}{dt^n}(\frac{\partial \mathscr{L}}{\partial x^{(n)}})=0$$

在航迹规划问题中有如下叫法:
n=1为最短距离曲线,同时也是最小速度曲线,数学上可以证明,不再赘述。
n=2为最小加速度曲线
n=3称为min jerk(这俩不知道咋翻译)
n=4称为min snap

minimum jerk

$$x^*(t)=\min_{x(t)}\int_0^T\mathscr{L}(\dddot x,\ddot x,\dot x,x,t)dt,\mathscr{L}=(\dddot x)^2$$

使用欧拉-拉格朗日方程可得:

$$x^{(6)}=0$$

积分后可得:

$$x=c_5t^5+c_4t^4+c_3t^3+c_2t^2+c_1t+c_0$$

起始和终止两点的边界条件为:

$$\begin{array}{r|rrr} & pos & vel & acc\\ \hline t=0 & a &0 &0\\ t=T & b &0&0 \end{array}$$

根据边界条件,可以求出方程的六个参数。求出$x(t)$ 后,可以通过求导求得加速度和速度。

而对于途经多点的航迹规划问题,在下一周的笔记中有详细的原理和代码实现。