cousera | Aerial Robotics 四旋翼控制与路径规划 笔记3

写在前面

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

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

四旋翼控制

2维

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

运动方程

惯性坐标系为$\mathcal{A}$，机体坐标系为$\mathcal{B}$，由机体坐标系到惯性坐标系的旋转矩阵为：
$${}^{\mathcal{A}}[R{]}_{\mathcal{B}}=\left[\begin{array}{cc}cos(\varphi )& -sin(\varphi )\\ sin(\varphi )& cos(\varphi )\end{array}\right]$$

设$r=[y,z{]}^T$，作用在四旋翼上的有$\overrightarrow{b_3}$方向的升力和$\overrightarrow{a_3}$方向的重力，进行坐标系转换，可以写出四旋翼的力平衡方程：
$$\begin{gathered} m\ddot{r}=\left[\begin{array}{c}0\\ -mg\end{array}\right]{+}^{\mathcal{A}}[R{]}_{\mathcal{B}}\left[\begin{array}{c}0\\ u_1\end{array}\right] \\ {u}_1=\sum _{i=1}^4{F}_i \end{gathered}$$
力矩平衡方程为，L是旋翼臂长：
$$I_{xx}\ddot{\varphi }=L(F_1-F_2)=u_2$$
则假设俯仰和偏航角均为零，Y-Z平面上的四旋翼的运动方程如下：
$$\left[\begin{array}{c}\ddot{y}\\ \ddot{z}\\ \ddot{\varphi }\end{array}\right]=\left[\begin{array}{c}0\\ -g\\ 0\end{array}\right]+\left[\begin{array}{cc}-\frac{1}{m}sin(\varphi )& 0\\ \frac{1}{m}cos(\varphi )& 0\\ 0& \frac{1}{I_{xx}}\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\end{array}\right]$$

状态变量表示

定义状态变量。
$$x=[y,z,\varphi ,\dot{y},\dot{z},\dot{\varphi }{]}^T$$
则可以将该方程写为状态空间的形式：
$$\dot{x}=\left[\begin{array}{c}\dot{y}\\ \dot{z}\\ \dot{\varphi }\\ 0\\ -g\\ 0\end{array}\right]+\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ -\frac{1}{m}sin(\varphi )& 0\\ \frac{1}{m}cos(\varphi )& 0\\ 0& \frac{1}{I_{xx}}\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\end{array}\right]$$

线性化

由于pd控制器只适用于线性系统，因此在使用之前需要将系统线性化。当四旋翼悬停时，可以将运动线性化，引入小扰动假设，这时有：$sin(\varphi )\approx \varphi ,cos(\varphi )\approx 1$，可以将四旋翼的运动方程线性简化为。
$$\begin{gathered} \ddot{y}=-g\varphi \\ \ddot{z}=\frac{u_1}{m}-g \\ \ddot{\varphi }=\frac{u_2}{I_{xx}} \end{gathered}$$

控制方程

已知$r_T(t),{\dot{r}}_T(t),{\ddot{r}}_T(t)$，${\ddot{r}}_C$为控制器计算出的指令加速度，则控制方程如下：
$$\begin{gathered} e_p=r_T(t)-r \\ {e}_v={\dot{r}}_T(t)-\dot{r} \\ want:({\ddot{r}}_T(t)-{\ddot{r}}_c)+k_d{e}_v+k_p{e}_p=0 \end{gathered}$$
则
$${\ddot{r}}_c={\ddot{r}}_T(t)+k_d{e}_v+k_p{e}_p$$
由控制方程可以计算得到该四旋翼所应输出的$u_1,u_2$ 。
$$\begin{gathered} 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{\varphi }}_T(t)=I_{xx}(\ddot{{\varphi }_c+k_{p,\varphi }({\varphi }_c-\varphi )+k_{d,\varphi }({\dot{\varphi }}_c-\dot{\varphi })}), \\ \ddot{{\varphi }_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)) \end{gathered}$$
在期望轨迹为悬停的特殊情况下（期望位置为常数，期望滚转角为零），有$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

仿真结果

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

3维

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

运动方程

惯性坐标系为$\mathcal{A}$，机体坐标系为$\mathcal{B}$，坐标系表示如下所示：

由机体坐标系到惯性坐标系的旋转矩阵为：
$$R=\left[\begin{array}{ccc}C\psi C\theta -S\varphi S\psi S\theta & -C\varphi S\psi & C\psi S\theta +C\theta S\varphi S\psi \\ C\theta S\psi +C\psi S\varphi S\theta & C\varphi C\psi & S\psi S\theta -C\theta S\varphi C\psi \\ -C\varphi S\theta & S\varphi & C\varphi C\theta \end{array}\right]$$
姿态角变化率和机体角速度的关系如下（某些教材上与课程课件表述不一致，暂时使用课件上的表述，此处存疑）：
$$\left[\begin{array}{c}p\\ q\\ r\end{array}\right]=\left[\begin{array}{ccc}C\theta & 0& -C\varphi S\theta \\ 0& 1& S\theta \\ S\theta & 0& C\varphi C\theta \end{array}\right]\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]$$

平衡方程

力的平衡方程
$$m\ddot{r}=\left[\begin{array}{c}0\\ 0\\ -mg\end{array}\right]+R\left[\begin{array}{c}0\\ 0\\ F_1+F_2+F_3+F_4\end{array}\right]$$
将第一个输出定义为$u_1={\sum }_{i=1}^4{F}_i$
力矩平衡方程
$$I\left[\begin{array}{c}\dot{p}\\ \dot{q}\\ \dot{r}\end{array}\right]=\left[\begin{array}{c}L(F_2-F_4)\\ L(F_3-F_1)\\ M_1-M_2+M_3-M_4\end{array}\right]-\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\times I\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$$
对$u_2$又可以列动量矩定理：
$$u_2=I\left[\begin{array}{c}\ddot{\varphi }\\ \ddot{\theta }\\ \ddot{\psi }\end{array}\right]=\left[\begin{array}{c}L(F_2-F_4)\\ L(F_3-F_1)\\ M_1-M_2+M_3-M_4\end{array}\right]$$

线性化

将力的平衡方程线性化：
$$\begin{gathered} {\ddot{r}}_1=g(\Delta \theta cos({\psi }_0)+\Delta \varphi sin({\psi }_0)) \\ {\ddot{r}}_2=g(\Delta \theta sin({\psi }_0)-\Delta \varphi cos({\psi }_0)) \\ {\ddot{r}}_3=\frac{1}{m}{u}_1-g \end{gathered}$$
将力矩平衡方程线性化，$\gamma =\frac{K_M}{K_F}$：
$$\begin{gathered} \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) \end{gathered}$$

控制方程

3维四旋翼控制问题可以这样描述：以悬停或循迹$z_{des}$为目标，找出四维输入量 { u 1 , u 2 ⃗ } \{u_1,\vec{u_2}\} {u1​,u2​ ​}。
如上图所示，内部的姿态控制器用板载加速度计和陀螺仪来控制滚转、俯仰和偏航姿态角度，运行频率约为1000 Hz。外部的位置控制环则使用速度和位置估计来控制航迹，运行频率约为100-200 Hz。
######姿态控制
联立下列控制方程和动力学方程：
$$\begin{gathered} ({\ddot{r}}_T(t)-{\ddot{r}}_c)+k_d{e}_v+k_p{e}_p=0 \\ {u}_2=I{\left[\begin{array}{ccc}\ddot{\varphi }& \ddot{\theta }& \ddot{\psi }\end{array}\right]}^T \end{gathered}$$
将转动惯量并入增益，可以得到$u_2$的解：
$$u_2=\left[\begin{array}{c}{k}_{p,\varphi }({\varphi }_{des}-\varphi )+k_{d,\varphi }(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{array}\right]$$
又由z方向上的力的平衡方程和控制方程联立，可以得到$u_1$的解：
u 1 = m g + m r ¨ 3 , d e s = m g − m { k d , 3 ( r ˙ 3 − r ˙ 3 , T ) + k p , 3 ( r 3 − r 3 , T ) } 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})\} u1​=mg+mr¨3,des​=mg−m{kd,3​(r˙3​−r˙3,T​)+kp,3​(r3​−r3,T​)}
由x,y方向上的力的平衡方程可得：
$${\varphi }_{des}=\frac{1}{g}({\ddot{r}}_{1,des}sin({\psi }_T)-{\ddot{r}}_{2,des}cos({\psi }_T))\text{\hspace{1em}(1)}$$$${\theta }_{des}=\frac{1}{g}({\ddot{r}}_{1,des}cos({\psi }_T)+{\ddot{r}}_{2,des}sin({\psi }_T))\text{\hspace{1em}(2)}$$
忽略高阶导数项，有：
$$p_{des}=0\text{\hspace{1em}(3)}$$$$q_{des}=0\text{\hspace{1em}(4)}$$
由于偏航角是独立的，因此有：
$${\psi }_{des}={\psi }_T(t)\text{\hspace{1em}(5)}$$$$r_{des}={\dot{\psi }}_T(t)\text{\hspace{1em}(6)}$$

悬停控制和三维循迹控制

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

悬停控制

对于悬停控制，位置误差定义如下：
$$e_i=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}$。
则误差为：
$$\begin{gathered} e_p=((r_T-r)\hat{n})\hat{n}+((r_T-r)\hat{b})\hat{b} \\ {e}_v={\dot{r}}_T-\dot{r} \end{gathered}$$
注意上式中的比例误差忽略了切线方向的误差，并且只考虑了目标航迹上最近的点。

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

航迹规划（planning）

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

数学补充：变分法

$$x^{\ast }(t)=\underset{x(t)}{\min }{\int }_0^T\mathcal{L}(\dot{x},x,t)dt$$
给定起始点$x(0)$和终点$x(T)$，考虑所有的可微曲线$x(t)$。
对于最优函数$x(t)$，它必须遵循欧拉-拉格朗日方程：$$\frac{d}{dt}(\frac{\partial \mathcal{L}}{\partial \dot{x}})-\frac{\partial \mathcal{L}}{\partial x}=0$$

例

• 最短距离 （几何）：$$x^{\ast }(t)=\underset{x(t)}{\min }{\int }_0^T{\dot{x}}^{2}dt$$
• 费马原理（光学）：$$x^{\ast }(t)=\min {\int }_0^{T}1dt$$
• 最小作用量原理（机械）： x ∗ ( t ) = min ⁡ x ( t ) ∫ 0 T { T ( x ˙ , x , t ) − V ( x , t ) } d t x^*(t)=\min_{x(t)}\int_0^T\{T(\dot x,x,t)-V(x,t)\}dt x∗(t)=x(t)min​∫0T​{T(x˙,x,t)−V(x,t)}dt

对一个一阶系统：
$$x^{\ast }(t)=\underset{x(t)}{\min }{\int }_0^T{\dot{x}}^{2}dt$$
代入欧拉-拉格朗日方程得到：$$(\dot{x}{)}^2\to \ddot{x}=0\to x=c_{1}t+c_0$$
由给出的$x(0)$和$x(T)$可以求出$c_1,c_0$

对一个n阶系统：$$x^{\ast }(t)=\underset{x(t)}{\min }{\int }_0^T(x^{(n)}{)}^{2}dt$$
输入为$x^{(n)}$
$$\frac{\partial \mathcal{L}}{\partial x}-\frac{d}{dt}(\frac{\partial \mathcal{L}}{\partial \dot{x}})+\frac{d^2}{d{t}^2}(\frac{\partial \mathcal{L}}{\partial \ddot{x}})+...+(-1{)}^n\frac{d^n}{d{t}^n}(\frac{\partial \mathcal{L}}{\partial x^{(n)}})=0$$
在航迹规划问题中有如下叫法：
n=1为最短距离曲线，同时也是最小速度曲线，数学上可以证明，不再赘述。
n=2为最小加速度曲线
n=3称为min jerk（这俩不知道咋翻译）
n=4称为min snap

minimum jerk

KaTeX parse error: Expected 'EOF', got '\dddot' at position 39: …0^T\mathscr{L}(\̲d̲d̲d̲o̲t̲ ̲x,\ddot x,\dot …
使用欧拉-拉格朗日方程可得：$$x^{(6)}=0$$
积分后可得：$$x=c_5{t}^5+c_4{t}^4+c_3{t}^3+c_2{t}^2+c_{1}t+c_0$$
起始和终止两点的边界条件为：
$$\begin{array}{rrrr}& pos& vel& acc\\ t=0& a& 0& 0\\ t=T& b& 0& 0\end{array}$$
根据边界条件，可以求出方程的六个参数。求出$x(t)$ 后，可以通过求导求得加速度和速度。

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