写在前面

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

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

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

Sensing and estimation

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

SLAM

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

一些笔记

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

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

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

非线性控制

线性控制的假设：滚转和俯仰角接近于0。$r_c$rc​为command acceleration。
$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$Given:rT​(t),r˙T​(t),r¨T​(t),rT​(t)=[x(t),y(t),z(t),ψ(t)]Tep​=rT​(t)−r,,ev​=r˙T​(t)−r˙want:(r¨T​(t)−r¨c​)+kd​ev​+kp​ep​=0

$u1 = (\ddot r^{des}+kk_ve_{\dot r} + k_p e_r + mg\vec{a_3})R\vec{b_3}\\ R^{des} \vec{b_3} = \frac{t}{\Vert\vec t \Vert}$u1=(r¨des+kkv​er˙​+kp​er​+mga3​​)Rb3​​Rdesb3​​=∥t∥t​
又已知$\psi^{des},\psi = \psi^{des}$ψdes,ψ=ψdes，联立上式得：$e_R(R^{des}, R)$eR​(Rdes,R)
$u_2 = \omega \times I \omega + I(-K_Re_R-k_we_w)$u2​=ω×Iω+I(−KR​eR​−kw​ew​)

几道计算

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

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

稳定性：large basin of attraction

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

路径规划

输入：
$u1 = \sum_{i=1}^4 F_i\\u2 = \begin{bmatrix}0&amp;1&amp;0&amp;-1\\-1&amp;0&amp;1&amp;0\\\mu&amp; -\mu &amp;\mu &amp;-\mu \end{bmatrix}\begin{bmatrix}F_1\\F_2\\F_3\\F_4\end{bmatrix}$u1=i=1∑4​Fi​u2=⎣⎡​0−1μ​10−μ​01μ​−10−μ​⎦⎤​⎣⎢⎢⎡​F1​F2​F3​F4​​⎦⎥⎥⎤​
状态：
$\begin{bmatrix}r&amp;v&amp;a&amp;j&amp;s&amp;\psi&amp;\dot \psi&amp; \ddot \psi\end{bmatrix}\\ \begin{bmatrix}q&amp; \dot q &amp;u_1 &amp;\dot u_1 &amp;\ddot u_1 &amp;u_2\end{bmatrix}$[r​v​a​j​s​ψ​ψ˙​​ψ¨​​][q​q˙​​u1​​u˙1​​u¨1​​u2​​]

无人机集群控制

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

复杂度分析

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

轨迹规划问题

已知起始点，终点和障碍物的位置和大小，要规划出各个飞行器的轨迹。最优解可以表述如下：
$r^*(t) = \min_{r(t)}\int_{t_0}^{t_f}\mathscr{L}(r(t))dt$r∗(t)=r(t)min​∫t0​tf​​L(r(t))dt

解决该问题的4个关键：

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

1.Concurrent Assignment and Planning of Trajectory($C_{APT}$CAPT​):
定理：分配任务规划航迹：
$\min_{\phi ,r(t)}\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 &gt;2R$ϕ,r(t)min​∫t0​tf​​x˙(t)Tx˙(t)dt will be safe ∥xi​(t)−xj​(t)∥>2R
2.Hungarian algorithm
首先找到每个起始点到所有目标点的优化轨迹，接着找到总代价最小的任务分配。
使用该算法解决$C_{APT}$CAPT​问题的复杂度在$O(n^2)$O(n2)~$O(n^3)$O(n3)之间。
3.leader- follower networks
每个robot和他的另据保持固定的位置向量，其中一个或多个leader领航。
3.匿名性
即使某个成员被取走，系统依然正常的保持运作。
4.通信
通过信息共享不同的robot可以合作。
$x^*_T=\min_{x_T \in X} \frac{I_{cs}[m; z_T | x_T]}{D(x_T)}$xT∗​=xT​∈Xmin​D(xT​)Ics​[m;zT​∣xT​]​
m为map，X为path，$D(x_T)$D(xT​)为耗时，$z_T$zT​为measurement，$I_{cs}$Ics​为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)

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