1 Robotics: Aerial Robotics 第3+4周课程学习记录及课后习题解答

最新推荐文章于 2020-11-02 11:23:14 发布

Kin-Zhang

最新推荐文章于 2020-11-02 11:23:14 发布

阅读量8.5k

点赞数 10

分类专栏： Robotics Specialization

本文链接：https://blog.csdn.net/qq_39537898/article/details/104199327

版权

Robotics Specialization 专栏收录该内容

8 篇文章 64 订阅

订阅专栏

Robotics: Aerial Robotics 第3+4周

WEEK - 3
- Quiz
- Programming Assignment: 2-D Quadrotor Control
WEEK - 4
- Quiz
- Programming Assignment: 3-D Quadrotor Control

第1和2周在另一篇帖子下… 分开好写一点
附上链接： 1 Robotics: Aerial Robotics 第1+2周课程学习记录及课后习题解答

此课程在Coursera需要科学上网才能观看，但是b站有人搬运，只是无中英字幕，放一下B站和Coursera的课程链接

UP主博主自己做的字幕版本（只是没更新完而且空中机器人是打算放最后慢慢更新…）
Coursera的链接介绍

建议将Coursera自身翻译的中文对着b站看，虽然视频播不了，但是能看字幕的txt文件。
此文仅为听课记录以及做题思考，可以的话我会将题目和代码题都搬运一下
VS 为视频字幕，MI为个人想法，有所错误或是好的想法欢迎评论区交流。

2.7：明天开始学习并更新吧，这么一看，我进度有点迅猛…
2.8：急性之下… 都看完了，感想都在文字里了… 3D的调着真累
2.9：大晚上… 一看怎么这么多阅读量，感觉万一误人子弟了咋办 /狗头，终于我看懂了最后一个代码的意思了… 回来把后面补充完整（我觉得今天补充不完，最后那个轨迹的生成写起来需要时间… 所以… 大家看看注释？），顺便附上这一个单元课完结证书。然后… 发现自己的笔记都没上传（其实没啥看的，我听的也云里雾里的，论文值得一看，大家下载下来看看吧~）
论文标题如下：
1 AUTONOMOUS NAVIGATION IN COMPLEX INDOOR AND OUTDOOR
2 INFORMATION-THEORETIC ACTIVE PERCEPTION FOR MULTI-ROBOT TEAMS
3 Minimum Jerk Trajectory Planning for Trajectory Constrained Redun
4 The GRASP Multiple Micro-UAV
5 Trajectory Generation and Control for Quadrotors

WEEK - 3

Quiz

1.In the nested feedback control loop
The inner loop corresponds to orientation and the outer loop corresponds to position.

2. $argmin_{x(t)}{\int\limits_0^T {\left\| {{x^{(5)}}(t)} \right\|} ^2}dt$ is an $k$ th degree polynomial trajectory where $k =$
9
解释：这里的 ${{x^{(5)}}(t)}$ 也就是等于 $c_4x^4+c_3x^3+c_2x^2+c_1x^1+c_0x^0$ 所以平方后最高阶是8，再一次积分成为9所以 $k = 9$

3.Which of the following are simplifying assumptions we made when designing the controller in this module?
Quadrotor is near equilibrium
Angular velocities are close to zero
Roll and pitch angles are close to zero

Programming Assignment: 2-D Quadrotor Control

太认真看pdf也会… /被坑，PS题目中，pdf最后一页是，阈值小于最小值能得满分，一开始… 我以为要在那个范围内，还说，我设计的好了还不行，现在想来是有点搞笑，\笑哭。正题：两个没反应过来的点：

还记得我第2周作业里说到无法得到连续时间内，所以角度求导… 是0，一开始一直没反应过来，想着怎么求这个 $\ddot \phi_c$ 和 $\dot \phi_c$ 这两个是理想情况下，他们应该都是0，也就是四旋翼没有横滚…
还有就是三个！PD控制器，PD值的测试。

直接贴一下没有三个参数的代码：

%这里参数有错噢，且看下文
Kpz=50;Kvz=2;
Kpf=50;Kvf=2;
Kpy=50;Kvy=2;
%这里参数有错噢，且看下文

%% 这里是公式求推理，看下文公式对应
u1=params.mass*(params.gravity+des_state.acc(2)+Kvz*(des_state.vel(2)-state.vel(2))+Kpz*(des_state.pos(2)-state.pos(2)));
fc=-(des_state.acc(1)+Kvy*(des_state.vel(1)-state.vel(1))+Kpy*(des_state.pos(1)-state.pos(1)))/params.gravity;
u2=params.Ixx*(0+Kvf*(0-state.omega)+Kpf*(fc-state.rot));
%最大的u1,u2限制
if u1>params.maxF
    u1=params.maxF;
end

接下来看图，仔细看图，需要的平面是y-z哦！2D的。

剩下三个公式我也直接截图pdf的贴进来了，
公式1
现在应该就能对应上代码中 $u_1,\phi_c,u_2$ 了，这个pdf的公式顺序如果按求的来说其实有点问题，不过大家都能看懂了。
==然后！==最磨人的来了，找3个PD控制的值！（z平面，y平面， $\phi$ 角度）
首先对于跟随直线来说，z平面只要保持在z=1即可，所以测试过程中，我截几张图给大家一起分析一下：
这是我随便设的值（一般调PID都是先P后D再I，详情转PID控制器即可）

Kpz=50;Kvz=0;
Kpf=50;Kvf=0;
Kpy=50;Kvy=0;

这个状态我们可以得知：1.角度的PD控制器不行，连基本的保持0°都没有，2.z无法维持在z=1，有一点维持迹象但是又下去了，y就是… 往右状态。那么！我们应该先解决问题1，加大对角度的控制！加！

Kpz=50;Kvz=0;
Kpf=500;Kvf=0;%直接来个500
Kpy=50;Kvy=0;

在这里插入图片描述
是不是至少到终点了。但是！由于我们没有加微分，所以很明显右边第3幅图抖的厉害，那么，接下来就很明显了，加大Kvf使角度先稳定。
一般来说PD的倍数有时候相差几倍的，按你自己感觉来吧（这里再次：如果以前做过电设可能调这个会得心应手，一下就知道了）

Kpz=50;Kvz=0;
Kpf=500;Kvf=10;%直接来个500
Kpy=50;Kvy=0;

再一看是不是好多了，但是，咦咦咦，为啥我的小四旋翼掉下去了… 没错我们对z,y的D都没有调呢，人家只有冲上去的劲，当然无法稳定来，那就随意点，都加上D也就是Kv（z,y我都加了5）

Kpz=50;Kvz=5;
Kpf=500;Kvf=10;%直接来个500
Kpy=50;Kvy=5;

在这里插入图片描述
哦呦！他终于能稳稳当当的停下来，而不掉下去了，接下来又有发现问题了吧，对！角度又有抖动了->Kfv（角度的D），后面过程我就不贴图了，找到值都在最低阈值下的，直接贴了，但是大家一点要自己理解哦：（那么正确参数的完整代码,hoho）当然这个参数答案不唯一，位置误差越小越好啦~

% FILL IN YOUR CODE HERE
Kpz=80;Kvz=12;
Kpf=1000;Kvf=30;
Kpy=20;Kvy=5;
u1=params.mass*(params.gravity+des_state.acc(2)+Kvz*(des_state.vel(2)-state.vel(2))+Kpz*(des_state.pos(2)-state.pos(2)));
fc=-(des_state.acc(1)+Kvy*(des_state.vel(1)-state.vel(1))+Kpy*(des_state.pos(1)-state.pos(1)))/params.gravity;
u2=params.Ixx*(0+Kvf*(0-state.omega)+Kpf*(fc-state.rot));
%最大的u1,u2限制
if u1>params.maxF
    u1=params.maxF;
end

这个仅为以上参数得出的图：
在这里插入图片描述
误差和如下：

Line trajectory:
Cumulative position error: 0.017743

Sine trajectory:
Cumulative position error: 0.081424

然后submit，把生成的.mat上传就能知道自己的分数合格啦~

WEEK - 4

Quiz

1.What sensors would you rely on for state estimation in an office building with vertical walls without too much clutter due to furniture when the lighting is poor?
IMU
Laser Scanners
解释：这些传感器的使用失效如果自己做过东西用过，就特别好悬念，例如光线暗（摄像头就不要了），垂直的墙（墙内会有钢筋影响GPS的定位，一般室内就GPS不要了…）

2.Given a desired thrust vector $t=sin(30^∘)cos(45^∘)a_1+sin(30^∘)sin(45^∘)a_2+cos(30^∘)a_3$ and a desired yaw angle, $ψ_{des}=45^∘$ . Compute the desired rotation matrix, $R_{des}$ .
[0.6124 -0.7071 0.3536;0.6124,0.7071,0.3536;-0.5,0,0.8866]

3.What is the rotation matrix that describes the attitude error if the current rotation matrix is given by RR and the desired rotation matrix is $R_{des}$
[0.6424 0.25 0.7244;-0.483 0.866 0.1294;-0.595 -0.433 0.6771]

4.What sensors are most likely to fail when operating indoors in a building with glass walls?
GPS
Laser Scanners
Cameras
解释：此处解释玻璃，反射问题所以导致激光雷达基本gg

5.What sensors are most likely to fail when the robot is flying outdoors, close to the ground near the wall of a tall building?
GPS

Programming Assignment: 3-D Quadrotor Control

直接先贴代码了，这次… 我就不演示了怎么调参的过程了… 因为没有三幅图一起看着了，PS经常坠机，主要是稳住自身的角度后，也就是Moment那块先ok，这样不容易坠机，一看这参数就是我又是P=50，D=10开调的，大家可以试试改里面的数据。接下来就是公式代码说明贴了
第一个文件：controller.m如下

% =================== Your code goes here ===================
%   state.rot = [phi; theta; psi
%   state.omega = [p; q; r]

% Thrust
Kp=[500;500;500];Kd=[20;20;20];
rdes_ddot=des_state.acc+Kp.*(des_state.pos-state.pos)+Kd.*(des_state.vel-state.vel);
F=params.mass*(params.gravity+rdes_ddot(3));

% Moment
M = zeros(3,1);
Kp_ang=[100;100;100];
Kd_ang=[2;2;2];
phi_des=(rdes_ddot(1)*sin(des_state.yaw)-rdes_ddot(2)*cos(des_state.yaw))/params.gravity;
theta_des=(rdes_ddot(1)*cos(des_state.yaw)+rdes_ddot(2)*sin(des_state.yaw))/params.gravity;
rot_des=[phi_des;theta_des;des_state.yaw];%角度
omega_des=[0;0;des_state.yawdot];%角速度
M=Kp_ang.*(rot_des-state.rot)+Kd_ang.*(omega_des-state.omega);

if F>params.maxF
    F=params.maxF;
end
% =================== Your code ends here ===================

来，好好说一下，这次和上次有什么不一样呢？-> 没错！我们有好多好多的PD要调了… 示意图看一下哈：

来直接翻到文档第5页，Attitude Control的第10个公式：我懒了我都贴图吧… LATex写公式我累了…

是不是一下就明白了代码中Moment的求值公式咋来的了？ -> 是的。
哦吼，那么细心的伙伴又发现！hi你的 $\phi_{des}$ 那些咋求的呀？看到公式14a,b中：
${\phi _{des}} = \frac{1}{g}({\ddot r_{1,des}}\sin {\psi _T} - {\ddot r_{2,des}}\cos {\psi _T})$ ${\theta_{des}} = \frac{1}{g}({\ddot r_{1,des}}\cos{\psi _T} + {\ddot r_{2,des}}\sin{\psi _T})$
是不是非常舒服了 -> 是的，最后的yaw角是希望能保持原状态，也就是偏航角，转向的那个，不会导致小四旋翼坠机操作的。

那么推力又是怎么写出来的呢？emm，首先看公式11
$({\ddot r_{i,T}} - {\ddot r_{i,des}}) + {k_{d,i}}({\dot r_{i,T}} - {\dot r_i}) + {k_{d,i}}({r_{i,T}} - {r_i}) = 0$ 把 $\ddot r_{i,des}$ 往右边一放，是不是就是我们求rdes_ddot的等式了。
然后F的公式，显而易见… 是力学而得 $F = m a$
$F = m (g + a)$
而a就是我们所求的 $\ddot r_{i,des}$
接下来就轮到调参了… emm 大家感觉吧，这个我就不给啥意见了，（觉得上升速度太慢就加P适当来点D不然又得炸鸡）

轮到… 我看了一下午都不知道他要我干啥的Trajectory Generation了，一句话跟大家解释一下那个部分要干啥：我要从1->2->3请问：我的2->3我不想停，怎么得到让速度继续而不导致坠机的期望状态？
然后… 我gg了，没感觉一点感觉都没有，然后上github找感觉… 然后看到大佬的（写的真复杂）ajtrask的Robotics-3D-Quadcopter-Controller/traj_generator.m
不过其实人家注释的挺详细的，就是一开始我不知道干啥，加上… 限制条件一脸懵逼，看完，觉得emm是这么回事 /笑哭，直接贴进来了哈：

persistent coef_x coef_y coef_z waypoints0 traj_time d0
if nargin > 2
    % setup trajectory segment times
    d = waypoints(:,2:end) - waypoints(:,1:end-1);
    d0 = 2 * sqrt(d(1,:).^2 + d(2,:).^2 + d(3,:).^2);
    traj_time = [0, cumsum(d0)];
    waypoints0 = waypoints;
    
    % solve for coefficients in x,y,z
    coef_x = getCoef(waypoints0(1,1:end)');
    coef_y = getCoef(waypoints0(2,1:end)');
    coef_z = getCoef(waypoints0(3,1:end)');
    
else
    % provide the trajectory point based on the coefficients
    
    if(t > traj_time(end))
        t = traj_time(end) - 0.0001;
    end
    
    t_index = find(traj_time >= t,1)-1; %between 1:n
    
    if (t_index == 0)
        t_index = 1;
    end
    if(t == 0)
        desired_state.pos = waypoints0(:,1);
        desired_state.vel = 0*waypoints0(:,1);
        desired_state.acc = 0*waypoints0(:,1);
    else
        %create scaled time, value between 0 to 1
        scale = (t-traj_time(t_index))/d0(t_index);
        
        index = (t_index-1)*8+1:t_index*8;
        
        %calculate position:
        t0 = polyT(8,0,scale)';
        desired_state.pos = [coef_x(index)'*t0; coef_y(index)'*t0; coef_z(index)'*t0];
        
        %calculate velocity:
        t1 = polyT(8,1,scale)';
        desired_state.vel = [coef_x(index)'*t1; coef_y(index)'*t1; coef_z(index)'*t1].*(1/d0(t_index));
        
        %calculate acceleration:
        t2 = polyT(8,2,scale)';
        desired_state.acc = [coef_x(index)'*t2; coef_y(index)'*t2; coef_z(index)'*t2].*(1/d0(t_index)^2);
    end
    
    % leave desired yaw and yawdot at zero
    desired_state.yaw = 0;
    desired_state.yawdot = 0;
end
end

function [T] = polyT(n, k, t)
% One utility function we are going to build to help us with the above is creating the polynom coefficient-coefficient vector (for lack of better name, these are the actual values you would put into the matrix raws). 
% To understand what this mean here is an example: Lets say I want to get a vector of 8 variables (for a 7th order polynom) for the first derivative when t=1. This utility function should return a vector of: 0 1 2 3 4 5 6 7.
% When we build matrix A we will use this utility function to create those vector for us.
% n is the polynom number of coefficients, k is the requested derivative and t is the actual value of t (this can be anything, not just 0 or 1).
T = zeros(n,1);
D = zeros(n,1);

% Init:
for i=1:n
    D(i) = i-1;
    T(i) = 1;
end

% Derivative:
for j=1:k
    for i=1:n
        T(i) = T(i) * D(i);
        if D(i) > 0
            D(i) = D(i) - 1;
        end
    end
end

% put t value
for i=1:n
    T(i) = T(i) * t^D(i);
end

T = T';

end

function [coef, A, b] = getCoef(waypoints)
% Creates matrix A, b and solves for the coefficient vector coef.

n = size(waypoints,1)-1;

% b matrix is easy, it is just the waypoints repeated in a patter
% (note waypoints is passed one component (x,y,z) at a time, so
% this function would be called three times (once for x, y, and z)
b = zeros(1,8*n);
for i=1:n
    b(1,i) = waypoints(i);
    b(1,i+n) = waypoints(i+1);
end

% A matrix is built up from all the constraints
A=zeros(8*n,8*n);

% Constraint 1 ==> Pi(t=0) = wi for all i=1:n
% Ex: P1(0) = w1, a11 = w1
% Ex: A(1,:)=[1 0 0 0 0 0 0 0 zero(1,8*(n-1))]
% Ex: b(1)=w1
for i=1:n
    A(i,((i-1)*8)+1:i*8) = polyT(8,0,0);
end

% Constraint 2 ==> Pi(t=1) = wi+1 for all i=1:n
% Ex: P1(1) = w2, a11+a12+?+a18 = w2
% Ex: A(n+1,:)=[1 1 1 1 1 1 1 1 zero(1,8*(n-1))]
% Ex: b(n+1)=w2
for i=1:n
    A(i+n,((i-1)*8)+1:i*8) = polyT(8,0,1);
end

% Constraint 3 ==> P1_k(t=0) = 0 for all k=1..3 (derivative)
% Ex: P1_1(t=0)=0, a12 = 0
% Ex: A(2*n+1,:)=[0 1 0 0 0 0 0 0 zero(1,8*(n-1))]
% Ex: b(2*n+1)=0
for k=1:3
    A(2*n+k,1:8) = polyT(8,k,0);
end

% Constraint 4 ==> Pn_k(t=1) = 0 for all k=1..3 (derivative)
% Ex: Pn_1(t=1)=0, a12 + 2a13 + 3a14 +?+ 7a18 = 0
% Ex: A(2*n+3+1,:)=[zero(1,8*(n-1)) 0 1 2 3 4 5 6 7]
% Ex: b(2*n+3+1)=0
for k=1:3
    A(2*n+3+k,(end-7):end) = polyT(8,k,1);
end

% Constraint 5 ==> Pi-1_k(t=1) = Pi_k(t=0) for all i=2..n and k=1..6
% Ex: P1_1(t=1)-P2_1(t=0) = 0, a12 + 2a13 +?+7a18 - a22 = 0
% Ex: A(2*n+6+1,)=[0 1 2 3 4 5 6 7 0 -1 0 0 0 0 0 0 zeros]
% Ex: b(2*n+6+1)=0
for i=2:n
    for k=1:6
        A(2*n+6+(i-2)*6+k, (i-2)*8+1:((i-2)*8+n*n)) = [polyT(8,k,1) -polyT(8,k,0)];
    end
end

% Now solve for the coefficients
coef = A\b';

emm 这个就不是我的代码了，我就给大家解释一通，其实看注释也可以的…（对着文档看，不然又像我这样一脸懵逼…）PS等我领会精髓看看能不能写个简单点的，感觉这个好复杂…

然后submit一下可以看到输出是否符合题目要求：

Evaluating...

Line trajectory:
Cumulative position error: 0.00093411

Helix trajectory:
Cumulative position error: 0.013005

Test trajectory generator:
Successfully passed through 5/5 waypoints in 13.5 sec with a trajectory length of 8.443 meters

此单元课程结业啦！附上几篇论文给大家一起看吧，是视频里的推荐阅读论文，有助于理解视频内容，我一并附在资源里面吧。
在这里插入图片描述

Kin-Zhang

关注

10
点赞
踩
12

收藏

觉得还不错? 一键收藏
打赏
3
评论
1 Robotics: Aerial Robotics 第3+4周课程学习记录及课后习题解答

第1和2周在另一篇帖子下… 分开好写一点此课程在Coursera需要科学上网才能观看，但是b站有人搬运，只是无中英字幕，放一下B站和Coursera的课程链接B站链接 Robotics Specialization #机器人学（宾夕法尼亚大学）Coursera的链接介绍建议将Coursera自身翻译的中文对着b站看，虽然视频播不了，但是能看字幕的txt文件。此文仅为听课记录以及做题...
复制链接

扫一扫