coursera | Aerial Robotics 四旋翼导航与控制 笔记4

写在前面

这门课上完花了挺久，晒一个证书，感觉这证书还挺帅的。

本节的内容主要是对四旋翼的很多方面作了导论性质的介绍，但作业挺难的，我最后也没有完全按要求完成作业，但在最后我会把代码放出来让大家参考。

这门课虽然不简单，但上完收获蛮大的，希望以后有时间了能回过头来把没搞懂的地方再想想吧。

Sensing and estimation

实验室中：动作捕捉相机，反光标记。
记载状态估计：
Laser Scanner:测量距障碍物的距离。
RGBD camera:投射红外线，观察红外线图像的变形。
IMU: 测量物体三轴姿态角(或角速率)以及加速度的装置。
另外还会用到：GPS，stero camera(立体相机)， 气压计，机载惯量测量仪。

SLAM

SLAM问题即将一个机器人放入未知环境中的未知位置，是否有办法让机器人一边对自己的位置进行定位，一边逐步描绘出此环境完全的地图。
在SLAM问题中，IMU提供机器人的姿态和方向以及$x_{i-1}$~$x_i$之间的运动信息。laser scanner提供$f_i$相对$x_i$的方位和距离。

一些笔记

为什么用2D的laser scanner而不用3D的？
答：商用大小的3D laser scanner精度不够

在室内进行地图构建的时候，GPS往往不太好用。

系统设计：
四旋翼的耗电量级约为200W/kg，越重耗电越大。
大的飞行器可以带更多的传感器，更大的电池因此续航也更久。小的飞行器可以在更逼仄的环境中飞行，同时更加敏捷，操纵性也更好。

非线性控制

线性控制的假设：滚转和俯仰角接近于0。$r_c$为command acceleration。
$$\begin{gathered} Given:r_T(t),{\dot{r}}_T(t),{\ddot{r}}_T(t),r_T(t)=[x(t),y(t),z(t),\psi (t){]}^T \\ {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}$$

$$\begin{gathered} u1=({\ddot{r}}^{des}+k{k}_v{e}_{\dot{r}}+k_p{e}_r+mg\overrightarrow{a_3})R\overrightarrow{b_3} \\ {R}^{des}\overrightarrow{b_3}=\frac{t}{\Vert \vec{t}\Vert } \end{gathered}$$
又已知${\psi }^{des},\psi ={\psi }^{des}$，联立上式得：$e_R(R^{des},R)$
$$u_2=\omega \times I\omega +I(-K_R{e}_R-k_w{e}_w)$$

几道计算

1.如何计算$R_{des}$
这也是课程里的一道题目，给出$\vec{t}$和${\psi }^{des}$要求$R_{des}$。我们知道$R^{des}\overrightarrow{b_3}=\vec{t}/\Vert \vec{t}\Vert$，又有$\vec{b}=[0,0,1{]}^T$，将旋转矩阵和欧拉角之间的转换公式代入，即可求得$R_{des}$。
这里很简单，用matlab得solve函数带入一下就行了，不上程序了。

2.如何计算$$e_R(R^{des},R),R\to R_{des}$$
由下式可得
$$\Delta R=R^T{R}^{des}$$

稳定性：large basin of attraction

下式为能收敛为悬停的R：
$$tr[I-((R^{des}{)}^{T}R)]<2$$
下式为收敛为悬停的$\omega$的范围，其中${\lambda }_{min}(I)$是惯量矩阵特征值的最小值。
$$\Vert e_w(o){\Vert }^2\le \frac{2}{{\lambda }_{m}in(I)}{k}_R(1-\frac{1}{2}tr[I-(R_{des}{)}^{T}R])$$
从该式终我们不难看出，惯量越小，飞行器能收敛为悬停的$\omega$的范围就越大，这也是我们为什么要制造更小的飞行器的原因之一，他们很容易就能完成大角度机动飞行动作。有经验公式：basin of attraction ~$L^{-\frac{5}{2}}$

路径规划

输入：
$$\begin{gathered} u1=\sum _{i=1}^4{F}_i \\ u2=\left[\begin{array}{cccc}0& 1& 0& -1\\ -1& 0& 1& 0\\ \mu & -\mu & \mu & -\mu \end{array}\right]\left[\begin{array}{c}{F}_1\\ F_2\\ F_3\\ F_4\end{array}\right] \end{gathered}$$
状态：
$$\begin{gathered} \left[\begin{array}{cccccccc}r& v& a& j& s& \psi & \dot{\psi }& \ddot{\psi }\end{array}\right] \\ \left[\begin{array}{cccccc}q& \dot{q}& u_1& {\dot{u}}_1& {\ddot{u}}_1& u_2\end{array}\right] \end{gathered}$$

无人机集群控制

合作行为的一些原则：
1.每个个体都独立地行动。
2.基于本地信息的行动（没有某种机制是的每个个体都通过信息共享拥有全局的地图）。
3.匿名合作。

复杂度分析

设有n个机器，m个障碍物
状态空间的维度：$O(n)$
和邻居的潜在碰撞可能：$O(n^2)$
和障碍物的潜在碰撞数：$O(mn)$
任务分配问题：$O(n!)$（谁去哪个地方）
$${\varphi }_{i,j}=\{\begin{array}{ll}1& \text{i is assigned to j}\\ 0& \text{otherwise}\end{array}$$

轨迹规划问题

已知起始点，终点和障碍物的位置和大小，要规划出各个飞行器的轨迹。最优解可以表述如下：
$$r^{\ast }(t)=\underset{r(t)}{\min }{\int }_{t_0}^{t_f}\mathcal{L}(r(t))dt$$

解决该问题的4个关键：

• 任务分配和轨迹规划同步进行
• Leader - follower networks
• 匿名性（不同的robot可以交换）
• 分享信息

1.Concurrent Assignment and Planning of Trajectory($C_{APT}$):
定理：分配任务规划航迹：
$$\underset{\varphi ,r(t)}{\min }{\int }_{t_0}^{t_f}\dot{x}(t{)}^T\dot{x}(t)dt\text{ will be safe }\Vert x_i(t)-x_j(t)\Vert >2R$$
2.Hungarian algorithm
首先找到每个起始点到所有目标点的优化轨迹，接着找到总代价最小的任务分配。
使用该算法解决$C_{APT}$问题的复杂度在$O(n^2)$~$O(n^3)$之间。
3.leader- follower networks
每个robot和他的另据保持固定的位置向量，其中一个或多个leader领航。
3.匿名性
即使某个成员被取走，系统依然正常的保持运作。
4.通信
通过信息共享不同的robot可以合作。
$$x_T^{\ast }=\underset{x_T\in X}{\min }\frac{I_{cs}[m;z_T|x_T]}{D(x_T)}$$
m为map，X为path，$D(x_T)$为耗时，$z_T$为measurement，$I_{cs}$为imformation。

作业

本章的作业是四旋翼的3D控制。
controller.m

function [F, M] = controller(t, state, des_state, params)
%CONTROLLER  Controller for the quadrotor
%
%   state: The current state of the robot with the following fields:
%   state.pos = [x; y; z], state.vel = [x_dot; y_dot; z_dot],
%   state.rot = [phi; theta; psi], state.omega = [p; q; r]
%
%   des_state: The desired states are:
%   des_state.pos = [x; y; z], des_state.vel = [x_dot; y_dot; z_dot],
%   des_state.acc = [x_ddot; y_ddot; z_ddot], des_state.yaw,
%   des_state.yawdot
%
%   params: robot parameters

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


% =================== Your code goes here ===================
% t = 0:0.01:10;
% H = zeros(size(t,2),3);
% for i =1:size(t,2)
%     x = trajhandle(i);
%     H(i,:) = x.acc;
% end



% % Thrust
% F = 0;
% 
% % Moment
% M = zeros(3,1);

kd_f = 10;
kp_f = 100;

F = params.mass*(params.gravity + ...
    kd_f*(des_state.vel(3) - state.vel(3)) + ...
    kp_f*(des_state.pos(3) - state.pos(3)));

kd_r1 = 25; 
kp_r1 = 200;
kd_r2 = 25;
kp_r2 = 200;

des_acc_1 = des_state.acc(1) + ...
    kd_r1*(des_state.vel(1) - state.vel(1)) + ...
    kp_r1*(des_state.pos(1) - state.pos(1));
des_acc_2 = des_state.acc(2) + ...
    kd_r2*(des_state.vel(2) - state.vel(2)) + ...
    kp_r2*(des_state.pos(2) - state.pos(2));


phi_des = 1/params.gravity*(des_acc_1*sin(des_state.yaw) - ...
    des_acc_2*cos(des_state.yaw));
theta_des = 1/params.gravity*(des_acc_1*cos(des_state.yaw) + ...
    des_acc_2*sin(des_state.yaw));
p_des = 0;
q_des = 0;

kp_phi = 200;
kd_phi = 20;
kp_theta = 200;
kd_theta = 20;
kp_psi = 10;
kd_psi = 5;

M = [kp_phi*(phi_des - state.rot(1)) + kd_phi*(p_des - state.omega(1));...
     kp_theta*(theta_des - state.rot(2)) + kd_theta*(q_des - state.omega(2));...
     kp_psi*(des_state.yaw - state.rot(3)) + kd_psi*(des_state.yawdot - state.omega(3))];

if F > params.maxF
    F = params.maxF;
end

% =================== Your code ends here ===================

end

traj_generator.m这个题我解平滑曲线的参数解到最后是无解，于是用的硬切，但硬切只要控制器调好了，最后效果也还行，也能拿满分。

如果有人按他的要求写出了平滑控制器参数的，还希望能私聊博主。

以下是我解曲线系数的程序，但失败了：

waypoints = [0    0   0;
             1    1   1;
             2    0   2;
             3    -1  1;
             4    0   0]';
d = waypoints(:,2:end) - waypoints(:,1:end-1);
d0 = 2 * sqrt(d(1,:).^2 + d(2,:).^2 + d(3,:).^2);
%计算出以0.5m/s的速度走到每个waypoints需要的时间
traj_time = [0, cumsum(d0)];
%cumsum：累加，计算出走到各个wayoints所需要的时间
waypoints0 = waypoints;
    
n = size(waypoints,2) - 1;%共有0,1,2...n个目标点
S = sym(traj_time);%Si = S(i+1)到达点i所花的时间
T = sym(d0);%Ti = T(i)从点i-1到达点i所花的时间
alpha = sym('alpha',[n,8],'real');%alpha_ij = alpha(i-1,j-1)
tt = sym('tt','positive');
p = sym('p',[n,1]);
for i = 1:n
    p(i) = alpha(i,1);
    for j = 2:8
        p(i) = p(i) +alpha(i,j)*((tt-S(i))/T(i))^(j-1); 
    end
end

pd1 = diff(p,tt,1);
pd2 = diff(p,tt,2);
pd3 = diff(p,tt,3);
pd4 = diff(p,tt,4);
pd5 = diff(p,tt,5);
pd6 = diff(p,tt,6);
pd = {pd1,pd2,pd3,pd4,pd5,pd6};

p_8n = sym('p_8n',[8*n,3]);

% sym_waypoints = sym(waypoints);

for i = 1:8*n
    if     i>0 && i<=n
        p_8n(i,:) = (subs(p(i),tt,S(i)) == waypoints(:,i))';
    elseif i>n && i<=2*n
        k = i-n;
        p_8n(i,:) = (subs(p(k),tt,S(k)) == waypoints(:,k+1))';
    elseif i>2*n && i<=(2*n + 3)
        k = i-2*n;
        p_8n(i,:) =  (subs(pd{k}(1),tt,S(1)) == 0)';
    elseif i>(2*n+3)&&i<=(2*n+6)
        k = i-(2*n+3);
        p_8n(i,:) = (subs(pd{k}(n),tt,S(n+1)) == 0);
    else
        temp = 0;
        for kk = 1:6
            for jj = 1:(n-1)
                temp = temp+1;
                p_8n(2*n+6+temp,:) = ...
                    (subs(pd{kk}(jj),S(jj+1)) == subs(pd{kk}(jj+1),S(jj+1)))';
            end
        end
        break;
    end
end
eqns_x = p_8n(:,1)';
var_x = alpha(:)';
var_y = alpha(:)';
var_z = alpha(:)';
solve_x = solve(eqns_x,var_x)
solve_y = solve(eqns_y,var_y)
solve_z = solve(eqns_z,var_z)

这时他提供的教程，如果有大神知道怎么搞，还望赐教。