【无人机】HKUST课件学习

# UAV Learning 本篇只是一个目录性质的东西，相关东西自己去看课件  ## Rotation Representation * rotation matrices * Euler angles：任何旋转都可以被描述为三个绕着xyz轴线性旋转 * Exponential coordinates * Quaternions 由于课件上的东西实在是不好整理，于是只挑了些重要的和相关的project记录。[ELEC5660 (gaowenliang.github.io)](https://gaowenliang.github.io/HKUST-ELEC5660-Introduction-to-Aerial-Robots/project/project.html) ## Project1 ### Phase 1 : Control Simulation question：In this project, you are provided with a quadrotor simulator written in Matlab. The simulator implements dynamics model of quadrotor and relies on the numerical solver ode45. You will need to implement controller. [code and data](https://github.com/gaowenliang/ELEC5660_proj1phase1) * 下面是直接运行code（未添加控制）的相关结果： ​  ​  ​  ​ 可以看到的是未添加控制情况下，红线和蓝线偏离较大，不能保持跟踪。 * 运用的知识：    * control.m ```matlab function [F, M] = controller(t, s, s_des) %% s_des = zeros(13,1); % [1:6]: x, y, z, dx, dy, dz % [7:10]: quaternion, qw,qx,qy,qz % [11:13]: oemga: wx, wy, wz %% definition of parameters % model parameters global params m = params.mass; g = params.grav; I = params.I; %PID parameters Kp_position = [4 4 5]; Kd_position = [2.5 2.5 5]; Kp_rpy = [100 100 75]; Kd_rpy = [15 15 10]; %% position control Rotation = quaternion_to_R(s(7:10));%function is in utils Rotation_desired = quaternion_to_R(s_des(7:10)); [phi,theta,psi] = RotToRPY_ZXY(Rotation); [phi_des,theta_des,psi_des] = RotToRPY_ZXY(Rotation_desired); %% newton equation % linearization : u1,0 = mg,therefore ddz_des = 0 ddp_des = [g*(theta_des * cos(psi_des)+phi_des*sin(psi_des)),g*(theta_des * sin(psi_des)-phi_des*cos(psi_des)),0]; w_des = s_des(11:13); w = s(11:13); %% PID control ddp_1c = ddp_des(1) + Kd_position(1)*(s_des(4)-s(4)) + Kp_position(1)*(s_des(1)-s(1)); ddp_2c = ddp_des(2) + Kd_position(2)*(s_des(5)-s(5)) + Kp_position(2)*(s_des(2)-s(2)); ddp_3c = ddp_des(3) + Kd_position(3)*(s_des(6)-s(6)) + Kp_position(3)*(s_des(3)-s(3)); F = m*(g+ddp_3c); phi_c = 1/g*(ddp_1c*sin(psi) - ddp_2c*cos(psi)); theta_c = 1/g*(ddp_1c*cos(psi) + ddp_2c*sin(psi)); %% Attitude control % judge the angle of yaw psi_dif = psi_des-psi; if psi_dif >= pi psi_dif=psi_dif-2*pi; elseif psi_dif <= 2 -pi psi_dif="psi_dif+2*pi;" end dd_rpy_c="[Kp_rpy(1)*(phi_c-phi)+Kd_rpy(1)*(0-w(1));" kp_rpy(2)*(theta_c-theta)+kd_rpy(2)*(0-w(2)); kp_rpy(3)*(psi_dif)+kd_rpy(3)*(0-w(3))]; %notsure abt derivative m="I*dd_rpy_c" + cross(w, i*w); ``` * 使用ziegler-nichols方法调参（我不认为这是一个聪明的做法） [ ]: # "[introduction-to-aerial-robotics proj1phase1 code at master · xyaoab introduction-to-aerial-robotics (github.com)](https: github.com tree code)[elec5660-2021 controller.m main garyandtang elec5660-2021 blob project1 controller.m)" ### phase : trajectory simulation [问题和code and data](https: gaowenliang elec5660_proj1phase2) 运用的知识  这个ppt 上面讲的就比较抽象，我的建议是[机器人路径规划、轨迹优化系列课程_哔哩哔哩_bilibili](https: www.bilibili.com video bv1yt4y1t7eb?p="10)，讲的比较具体形象。[博客](https://blog.csdn.net/q597967420/article/details/76099491)" ------------------- path1="[0.0" 0.0 1.0 ; ... -1.0 2.0 3.0 4.0 5.0 6.0 7.0 8.0 9.0 10.0 ];  代码部分function s_des="trajectory_generator(t," path, h)> 1 定义变量 > > ```matlab > %persistent变量与全局变量有相似之处，它们都创建永久的存储空间，不同在于persistent只对定义它的函数可见。这样可以防止persistent变量被其它函数或在命令行中被改变。 > persistent ts; > persistent ts_delta; > persistent P; > persistent xyz; > ``` > > -------------- > > 2 每一段轨迹的时间分配,总共分配25秒完成轨迹运行，path_seg_length就是两个waypoints之间的欧式距离，cumsum_path是累加路径，cumsum_path(end)就是一共需要走的路程，ts_delta就是两个waypoints的时间间隔。 > > ```matlab > total_time = 25.0; > path_seg_length = sqrt(sum((path(2:end, :) - path(1:end-1,:)).^2,2)); %每一段的长度 > cumsum_path = cumsum(path_seg_length); > ts_proportion = cumsum_path/cumsum_path(end); > ts = [0; ts_proportion]'; > ts = ts*total_time; > ts_delta = ts(2:end) - ts(1:end-1); % interval---->T > ``` > >  > >  > > -------------------- > > 3 m就是轨迹段数，如果有11个waypoints，那么就有10段，这里使用的是Minimum Snap Trajectory Generation，所以N=7，但是有8个自由度（多项式的阶数为7，但是会有8个系数），所以Q_dia就是8*m的方阵。至于Q_dia matrix 的计算，参照上面fig[cost function]。 > > ```matlab > %% cost function > m = length(path)-1; %# segments > %为什么是8*m，是因为minimum snap为7阶，但是有8个自由度，所以一共有8*m，可以发现Q是一个对称矩阵 > Q_dia = zeros(8*m, 8*m); %block diagonal matrix > for k=1:m > for i=4:8 > for j=4:8 > Q_dia((k-1)*8+i,(k-1)*8+j)=i*(i-1)*(i-2)*(i-3)*j*(j-1)*(j-2)*(j-3)/(j+i-7)*(ts_delta(k)^(i+j-7)); > end > end > end > ``` > > 每一组的后4*4改变，而且是一个对称矩阵 > > ------------- > > 4 约束$A_j p_j = d_j$，dim指的是A的维度 > > ```matlab > %% derivative constraint > % 为什么是4*（m-1）？4分别是pos vel acc jerk ，m-1是除了第一个点和最后一个点，中间有m-1个点 > % 2*m是每一段t=0和t=T的时候的连续型约束 > dim = 2*m+4*(m-1)+ 3*2; %continutity[4]+starting[3]+ending[3] > P = zeros(8*m,3); %p_j_i = 8mx3 [p00, p01...;p10...] > A = zeros(dim,8*m); %dimx8m > d= zeros(dim,3); > d(1:2*m,:) = reshape([path(1:end-1, :) path(2:end, :)]',3, [])'; > %dimx3 > %每一段的开始和结束都需要在位置上 > for j=1:m %position > A(2*j-1:2*j,8*j-7:8*j) = [ 1, 0, 0, 0, 0, 0, 0, 0;... > 1, ts_delta(j), ts_delta(j)^2, ts_delta(j)^3, ... > ts_delta(j)^4, ts_delta(j)^5, ts_delta(j)^6, ts_delta(j)^7]; > end > ``` > > 5 连续型约束 > > ```matlab > %% continutity pos,vel,acce,jerk > for j=1:(m-1) > A((2*m+4*j-3):(2*m+4*j),(8*j-7):(8*j+8))=[1, ts_delta(j), ts_delta(j)^2, ts_delta(j)^3, ... > ts_delta(j)^4, ts_delta(j)^5, ts_delta(j)^6, ts_delta(j)^7,-1,0,0,0,0,0,0,0;...%pos > 0, 1, 2*ts_delta(j), 3*ts_delta(j)^2, 4*ts_delta(j)^3, 5*ts_delta(j)^4, ... > 6*ts_delta(j)^5, 7*ts_delta(j)^6,0,-1,0,0,0,0,0,0;...%vel > 0, 0, 2, 6*ts_delta(j), 12*ts_delta(j)^2, 20*ts_delta(j)^3, ... > 30*ts_delta(j)^4, 42*ts_delta(j)^5,0,0,-2,0,0,0,0,0;...%acc > 0, 0, 0, 6, 24*ts_delta(j), 60*ts_delta(j)^2, ... > 120*ts_delta(j)^3, 210*ts_delta(j)^4,0,0,0,-6,0,0,0,0];%jerk > end > ``` > > 6 > > ```matlab > %% starting point vel,acc, jerk > A(6*m-3:6*m-1, 1:8)=[0,1,0,0,0,0,0,0;... > 0,0,2,0,0,0,0,0;... > 0,0,0,6,0,0,0,0]; > A(6*m:6*m+2,8*m-7:8*m) = [0, 1, 2*ts_delta(m), 3*ts_delta(m)^2, 4*ts_delta(m)^3, 5*ts_delta(m)^4, ... > 6*ts_delta(m)^5, 7*ts_delta(m)^6; ... > 0, 0, 2, 6*ts_delta(m), 12*ts_delta(m)^2, 20*ts_delta(m)^3, ... > 30*ts_delta(m)^4, 42*ts_delta(m)^5; ... > 0, 0, 0, 6, 24*ts_delta(m), 60*ts_delta(m)^2, ... > 120*ts_delta(m)^3, 210*ts_delta(m)^4]; > ``` > > 7 QP求解 > > ```matlab > %% QP > f=zeros(8*m,1); > P(:,1) = quadprog(Q_dia,f,[],[],A,d(:,1)); > P(:,2) = quadprog(Q_dia,f,[],[],A,d(:,2)); > P(:,3) = quadprog(Q_dia,f,[],[],A,d(:,3)); > ``` ------------------------ * **结果展示** path1  ----------------- ​ path2  ---------------- 来一颗小心心！o(´^｀)o  ### Phase 3 : Path Finding Simulation [代码和问题](https://github.com/gaowenliang/ELEC5660_proj1phase3) * A*算法  * 关于搜索路径，我写了一篇[总结](https://copy2000.github.io/Dcos/搜索路径.pdf). 代码思路，其实比较简单，就是g(n)+h(n)，h（n）就是使用MAP中的每一个节点到goal 的距离，至于g（n）是accumulated cost，只与之前的有关。 * 代码path_from_Astar.m 初始化MAP ```matlab function Optimal_path = path_from_A_star(map) Optimal_path = []; size_map = size(map,1); MAX_X=10; MAX_Y=10; %Define the 2D grid map array. %Obstacle=-1, Target = 0, Start=1 MAP=2*(ones(MAX_X,MAX_Y)); %Initialize MAP with location of the target xval=floor(map(size_map, 1)); yval=floor(map(size_map, 2)); xTarget=xval; yTarget=yval; MAP(xval,yval)=0; %Initialize MAP with location of the obstacle for i = 2: size_map-1 xval=floor(map(i, 1)); yval=floor(map(i, 2)); MAP(xval,yval)=-1; end %Initialize MAP with location of the start point xval=floor(map(1, 1)); yval=floor(map(1, 2)); xStart=xval; yStart=yval; MAP(xval,yval)=1; ``` start your code here，首先定义一个函数，用于计算欧氏距离。 ```matlab % dim=1是对列求和，dim=2是对行求和 function cost = dist(start_pos, goal_pos,dim) cost = sqrt(sum((double(start_pos) - double(goal_pos)).^2,dim)); end ``` ```matlab %% g(n)和h(n)的初始化 G_score = inf(size(MAP)); G_score(xStart, yStart)=0; herustic=zeros(size(MAP)); %关于ind2sub函数http://blog.sina.com.cn/s/blog_7054a1960102vyby.html %[row,col]就是（1,1）（2,1）（3,1）。。。（1,2）（2,2）.。。 ``` 启发式的路径规划，最重要的是目标导向型，所以herustic（n）就是一个和MAP维度一样的矩阵，只是每个节点存储的是该节点到goal 的距离，这样每一次迭代只需要计算g（n）+h（n）的最小值就可以知道下一步在哪了。 ``` matlab %% h(n)定义 [row,col]=ind2sub(size(G_score), 1:numel(G_score)); matrix_ = [row;col]; dist_temp=dist(matrix_,[xTarget;yTarget],1); herustic(1:end)=dist_temp(1:end); % unvisited=1, visted=inf ``` 两个标记矩阵，unvisited（10\*10），backtrack（10\*10\*2），其中backtrack是存储的是回溯路径，如下图中的backtrack矩阵，第一个var(:,:,1)存储的是回溯的行row，另外一个是column，这样只需要一步一步的回溯，可以得到optimal path。  ```matlab %% 未访问节点矩阵初始化 % 如果未访问unvisited矩阵中的值为1，访问过就是inf unvisited = ones(size(MAP)); backtrack = zeros(size(MAP,1),size(MAP,2),2); % row_delta和col_delta是可以走的八个方向东、西、南、北、东北。。。 row_delta=[-1,-1,-1,0,0,1,1,1]; col_delta=[-1,0,1,-1,1,-1,0,1]; ``` 下面将展示本次代码的核心部分，如何寻找最优路径： ```matlab while(~all(unvisited==inf)) % take lowest g(n) [masked] [min_val,idx]=min(unvisited(:).*(G_score(:)+herustic(:))); [row,col]=ind2sub(size(G_score),idx); % remove and mark as visited unvisited(row,col)=inf; % reach goal if row==xTarget && col==yTarget break; end for i=1:length(row_delta) temp_ind=[row+row_delta(i),col+col_delta(i)]; %no obstacle, unvisted, within the index bound if all(temp_ind>0) && temp_ind(1) <= max_x && temp_ind(2) <="MAX_Y..." map(temp_ind(1),temp_ind(2))~="-1" unvisited(temp_ind(1),temp_ind(2))="=1" temp="G_score(row,col)+dist([row,col],temp_ind,2);" if g_score(temp_ind(1),temp_ind(2)) backtrack(temp_ind(1),temp_ind(2),:)="[row,col];" end % no path found g_score(xtarget,ytarget)="=inf" optimal_path="[];" disp("no found"); else ptr_ind="reshape(backtrack(xTarget,yTarget,:),[1,2]);" while (ptr_ind(1)~="xStart" || ptr_ind(2)~="yStart)" disp(optimal_path) 0.5 0]; ``` * 结果展示  **不是bgm，是听的歌录进去了！！！**