Allowing change the level of continuity

minimum_snap_trajectory_generation/README.md

@@ -0,0 +1,19 @@
+# minimum snap trajectory planning in MATLAB
+This repository contains sample code in MATLAB for minimum snap trajectory planning described in http://blog.csdn.net/q597967420/article/details/73647190.
+This README provides a brief overview of our trajectory generation utilities with some examples.
+
+## Required
+1. MATLAB(R2013a is tested)
+
+## How to run
+There are 4 demo codes in 2D space, you can directly run these and see results.
+
+ - demo1: minimum snap trajectory planning with waypoints **strong constrants**(equality constraints).
+ - demo2: minimum snap trajectory planning with **corridor constraints**(iequality constraints).
+ - demo3: minimum snap trajectory planning with waypoints strong constrants by **close form** solution.
+ - demo4: minimum snap trajectory planning with **guiding path**
+
+## Licence
+The source code is released under GPLv3 license.
+
+Any problem, please contact [email protected]

minimum_snap_trajectory_generation/arrangeT.m

@@ -0,0 +1,6 @@
+function ts = arrangeT(waypts,T)
+    x = waypts(:,2:end) - waypts(:,1:end-1);
+    dist = sum(x.^2,1).^0.5;
+    k = T/sum(dist);
+    ts = [0 cumsum(dist*k)];
+end

minimum_snap_trajectory_generation/calc_tvec.m

@@ -0,0 +1,7 @@
+% r=0:pos  1:vel  2:acc 3:jerk
+function tvec = calc_tvec(t,n_order,r)
+    tvec = zeros(1,n_order+1);
+    for i=r+1:n_order+1
+        tvec(i) = prod(i-r:i-1)*t^(i-r-1);
+    end
+end

minimum_snap_trajectory_generation/computeQ.m

@@ -0,0 +1,19 @@
+% n:polynormial order
+% r:derivertive order, 1:minimum vel 2:minimum acc 3:minimum jerk 4:minimum snap
+% t1:start timestamp for polynormial
+% t2:end timestap for polynormial
+function Q = computeQ(n,r,t1,t2)
+T = zeros((n-r)*2+1,1);
+for i = 1:(n-r)*2+1
+    T(i) = t2^i-t1^i;
+end
+Q = zeros(n);
+for i = r+1:n+1
+    for j = i:n+1
+        k1 = i-r-1;
+        k2 = j-r-1;
+        k = k1+k2+1;
+        Q(i,j) = prod(k1+1:k1+r)*prod(k2+1:k2+r)/k*T(k);
+        Q(j,i) = Q(i,j);
+    end
+end

minimum_snap_trajectory_generation/demo1_minimum_snap_simple.m

@@ -0,0 +1,110 @@
+function demo1_minimum_snap_simple()
+    clear,clc;
+
+    %% condition
+    waypts = [0,0;
+              1,2;
+              2,-1;
+              4,8;
+              5,2]';
+    v0 = [0,0];
+    a0 = [0,0];
+    v1 = [0,0];
+    a1 = [0,0];
+    T = 5;
+    ts = arrangeT(waypts,T);
+    n_order = 5;
+
+    %% trajectory plan
+    polys_x = minimum_snap_single_axis_simple(waypts(1,:),ts,n_order,v0(1),a0(1),v1(1),a1(1));
+    polys_y = minimum_snap_single_axis_simple(waypts(2,:),ts,n_order,v0(2),a0(2),v1(2),a1(2));
+
+    %% result show
+    figure(1)
+    plot(waypts(1,:),waypts(2,:),'*r');hold on;
+    plot(waypts(1,:),waypts(2,:),'b--');
+    title('minimum snap trajectory');
+    color = ['grc'];
+    for i=1:size(polys_x,2)
+        tt = ts(i):0.01:ts(i+1);
+        xx = polys_vals(polys_x,ts,tt,0);
+        yy = polys_vals(polys_y,ts,tt,0);
+        plot(xx,yy,color(mod(i,3)+1));
+    end
+
+    figure(2)
+    vxx = polys_vals(polys_x,ts,tt,1);
+    axx = polys_vals(polys_x,ts,tt,2);
+    jxx = polys_vals(polys_x,ts,tt,3);
+    vyy = polys_vals(polys_y,ts,tt,1);
+    ayy = polys_vals(polys_y,ts,tt,2);
+    jyy = polys_vals(polys_y,ts,tt,3);
+
+    subplot(421),plot(tt,xx);title('x position');
+    subplot(422),plot(tt,yy);title('y position');
+    subplot(423),plot(tt,vxx);title('x velocity');
+    subplot(424),plot(tt,vyy);title('y velocity');
+    subplot(425),plot(tt,axx);title('x acceleration');
+    subplot(426),plot(tt,ayy);title('y acceleration');
+    subplot(427),plot(tt,jxx);title('x jerk');
+    subplot(428),plot(tt,jyy);title('y jerk');
+end
+
+
+function polys = minimum_snap_single_axis_simple(waypts,ts,n_order,v0,a0,ve,ae)
+p0 = waypts(1);
+pe = waypts(end);
+
+n_poly = length(waypts)-1;
+n_coef = n_order+1;
+
+% compute Q
+Q_all = [];
+for i=1:n_poly
+    Q_all = blkdiag(Q_all,computeQ(n_order,3,ts(i),ts(i+1)));
+end
+b_all = zeros(size(Q_all,1),1);
+
+Aeq = zeros(4*n_poly+2,n_coef*n_poly);
+beq = zeros(4*n_poly+2,1);
+
+% start/terminal pva constraints  (6 equations)
+Aeq(1:3,1:n_coef) = [calc_tvec(ts(1),n_order,0);
+                     calc_tvec(ts(1),n_order,1);
+                     calc_tvec(ts(1),n_order,2)];
+Aeq(4:6,n_coef*(n_poly-1)+1:n_coef*n_poly) = ...
+                    [calc_tvec(ts(end),n_order,0);
+                     calc_tvec(ts(end),n_order,1);
+                     calc_tvec(ts(end),n_order,2)];
+beq(1:6,1) = [p0,v0,a0,pe,ve,ae]';
+
+% mid p constraints    (n_ploy-1 equations)
+neq = 6;
+for i=1:n_poly-1
+    neq=neq+1;
+    Aeq(neq,n_coef*i+1:n_coef*(i+1)) = calc_tvec(ts(i+1),n_order,0);
+    beq(neq) = waypts(i+1);
+end
+
+% continuous constraints  ((n_poly-1)*3 equations)
+for i=1:n_poly-1
+    tvec_p = calc_tvec(ts(i+1),n_order,0);
+    tvec_v = calc_tvec(ts(i+1),n_order,1);
+    tvec_a = calc_tvec(ts(i+1),n_order,2);
+    neq=neq+1;
+    Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_p,-tvec_p];
+    neq=neq+1;
+    Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_v,-tvec_v];
+    neq=neq+1;
+    Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_a,-tvec_a];
+end
+
+Aieq = [];
+bieq = [];
+
+p = quadprog(Q_all,b_all,Aieq,bieq,Aeq,beq);
+
+polys = reshape(p,n_coef,n_poly);
+
+end
+

minimum_snap_trajectory_generation/demo2_minimum_snap_corridor.m

@@ -0,0 +1,125 @@
+function demo2_minimum_snap_corridor()
+clear,clc;
+
+%% condition
+waypts = [0,0;
+          1,2;
+          2,0;
+          4,5;
+          5,2]';
+v0 = [0,0];
+a0 = [0,0];
+v1 = [0,0];
+a1 = [0,0];
+T = 5;
+n_order = 5;
+
+%% sample mid points
+r = 0.2;  %% corridor r
+step = r;
+new_waypts = waypts(:,1);
+for i=2:size(waypts,2)
+    x1 = waypts(1,i-1);
+    y1 = waypts(2,i-1);
+    x2 = waypts(1,i);
+    y2 = waypts(2,i);
+    n = ceil(hypot(x1-x2,y1-y2)/step)+1;
+    sample_pts = [linspace(x1,x2,n);linspace(y1,y2,n)];
+    new_waypts = [new_waypts sample_pts(:,2:end)];
+end
+ts = arrangeT(new_waypts,T);
+
+%% trajectory plan
+polys_x = minimum_snap_single_axis_corridor(new_waypts(1,:),ts,n_order,v0(1),a0(1),v1(1),a1(1),r);
+polys_y = minimum_snap_single_axis_corridor(new_waypts(2,:),ts,n_order,v0(2),a0(2),v1(2),a1(2),r);
+
+%% result show
+figure(1)
+% plot(waypts(1,:),waypts(2,:),'c','LineWidth',30);hold on;
+plot(new_waypts(1,:),new_waypts(2,:),'.g');hold on;
+plot(waypts(1,:),waypts(2,:),'*r');hold on;
+% plot(waypts(1,:),waypts(2,:),'--b');
+for i=1:size(new_waypts,2)
+    plot_rect(new_waypts(:,i),r);
+end
+
+title('minimum snap trajectory with corridor');
+tt = 0:0.01:T;
+xx = polys_vals(polys_x,ts,tt,0);
+yy = polys_vals(polys_y,ts,tt,0);
+plot(xx,yy,'r');
+
+end
+
+function plot_rect(center,r)
+p1 = center+[-r;-r];
+p2 = center+[-r;r];
+p3 = center+[r;r];
+p4 = center+[r;-r];
+plot_line(p1,p2);
+plot_line(p2,p3);
+plot_line(p3,p4);
+plot_line(p4,p1);
+end
+
+function plot_line(p1,p2)
+a = [p1(:),p2(:)];    
+plot(a(1,:),a(2,:),'b');
+end
+
+function polys = minimum_snap_single_axis_corridor(waypts,ts,n_order,v0,a0,ve,ae,corridor_r)
+p0 = waypts(1);
+pe = waypts(end);
+
+n_poly = length(waypts)-1;
+n_coef = n_order+1;
+
+% compute Q
+Q_all = [];
+for i=1:n_poly
+    Q_all = blkdiag(Q_all,computeQ(n_order,3,ts(i),ts(i+1)));
+end
+b_all = zeros(size(Q_all,1),1);
+
+Aeq = zeros(3*n_poly+3,n_coef*n_poly);
+beq = zeros(3*n_poly+3,1);
+
+% start/terminal pva constraints  (6 equations)
+Aeq(1:3,1:n_coef) = [calc_tvec(ts(1),n_order,0);
+                     calc_tvec(ts(1),n_order,1);
+                     calc_tvec(ts(1),n_order,2)];
+Aeq(4:6,n_coef*(n_poly-1)+1:n_coef*n_poly) = ...
+                    [calc_tvec(ts(end),n_order,0);
+                     calc_tvec(ts(end),n_order,1);
+                     calc_tvec(ts(end),n_order,2)];
+beq(1:6,1) = [p0,v0,a0,pe,ve,ae]';
+neq = 6;
+
+% continuous constraints  ((n_poly-1)*3 equations)
+for i=1:n_poly-1
+    tvec_p = calc_tvec(ts(i+1),n_order,0);
+    tvec_v = calc_tvec(ts(i+1),n_order,1);
+    tvec_a = calc_tvec(ts(i+1),n_order,2);
+    neq=neq+1;
+    Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_p,-tvec_p];
+    neq=neq+1;
+    Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_v,-tvec_v];
+    neq=neq+1;
+    Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_a,-tvec_a];
+end
+
+% corridor constraints (n_ploy-1 iequations)
+Aieq = zeros(2*(n_poly-1),n_coef*n_poly);
+bieq = zeros(2*(n_poly-1),1);
+for i=1:n_poly-1
+    tvec_p = calc_tvec(ts(i+1),n_order,0);
+    Aieq(2*i-1:2*i,n_coef*i+1:n_coef*(i+1)) = [tvec_p;-tvec_p];
+    bieq(2*i-1:2*i) = [waypts(i+1)+corridor_r corridor_r-waypts(i+1)];
+end
+
+p = quadprog(Q_all,b_all,Aieq,bieq,Aeq,beq);
+
+polys = reshape(p,n_coef,n_poly);
+
+end
+