Aerial Robotics 第 4 讲

写在前面

这门课上完花了挺久,晒一个证书,感觉这证书还挺帅的。
file
本节的内容主要是对四旋翼的很多方面作了导论性质的介绍,但作业挺难的,我最后也没有完全按要求完成作业,但在最后我会把代码放出来让大家参考。

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

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

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。

$$
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
$$

file

$$
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}
$$

又已知$\psi^{des},\psi = \psi^{des}$,联立上式得:$e_R(R^{des}, R)$

$$
u_2 = \omega \times I \omega + I(-K_Re_R-k_we_w)
$$
几道计算

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

2.如何计算$$eR(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_min(I)}k_R(1-\frac{1}{2}tr[I-(R_{des})^TR])$$

从该式终我们不难看出,惯量越小,飞行器能收敛为悬停的$\omega$的范围就越大,这也是我们为什么要制造更小的飞行器的原因之一,他们很容易就能完成大角度机动飞行动作。有经验公式:basin of attraction ~$L^{-\frac{5}{2}}$

路径规划

输入:

$$u1 = \sum_{i=1}^4 F_i\\u2 = \begin{bmatrix}0&1&0&-1\\-1&0&1&0\\\mu& -\mu &\mu &-\mu \end{bmatrix}\begin{bmatrix}F_1\\F_2\\F_3\\F_4\end{bmatrix}$$

状态:

$$
\begin{bmatrix}r&v&a&j&s&\psi&\dot \psi& \ddot \psi\end{bmatrix}\\
\begin{bmatrix}q& \dot q &u_1 &\dot u_1 &\ddot u_1 &u_2\end{bmatrix}
$$

无人机集群控制

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

复杂度分析

设有n个机器,m个障碍物
状态空间的维度:$O(n)$
和邻居的潜在碰撞可能:$O(n^2)$
和障碍物的潜在碰撞数:$O(mn)$
任务分配问题:$O(n!)$(谁去哪个地方)

$$
\phi_{i,j} = \begin{cases}
1&\text{i is assigned to j}\\
0&\text{otherwise}
 \end{cases}
$$

轨迹规划问题

已知起始点,终点和障碍物的位置和大小,要规划出各个飞行器的轨迹。最优解可以表述如下:

$$
r^*(t) = \min_{r(t)}\int_{t_0}^{t_f}\mathscr{L}(r(t))dt
$$

解决该问题的4个关键:

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

1.Concurrent Assignment and Planning of Trajectory($C_{APT}$):
定理:分配任务规划航迹:

$$
\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 >2R
$$

2.Hungarian algorithm
首先找到每个起始点到所有目标点的优化轨迹,接着找到总代价最小的任务分配。
使用该算法解决$C_{APT}$问题的复杂度在$O(n^2)$~$O(n^3)$之间。
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)}
$$

m为map,X为path,$D(x_T)$为耗时,$zT$为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)

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