├── ACC ├── CBF.png ├── acc.m └── acc_main.m ├── Robot2D ├── CM_dynamics.m ├── sensing.m ├── CM_draw.m ├── CM_main.m ├── map.m └── CM_exe.m └── README.md /ACC/CBF.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Weixy21/HOCBF/HEAD/ACC/CBF.png -------------------------------------------------------------------------------- /Robot2D/CM_dynamics.m: -------------------------------------------------------------------------------- 1 | function dx = CM_dynamics(t,x) 2 | global u 3 | 4 | dx = zeros(4,1); 5 | dx(1) = x(4)*cos(x(3)); 6 | dx(2) = x(4)*sin(x(3)); 7 | dx(3) = u(1); 8 | dx(4) = u(2); -------------------------------------------------------------------------------- /Robot2D/sensing.m: -------------------------------------------------------------------------------- 1 | function rt = sensing(sense, obs) 2 | len = length(sense.x); 3 | rt = 0; 4 | for i = 1:1:len 5 | if(sqrt((sense.x(i) - obs(1))^2 + (sense.y(i) - obs(2))^2) <= 6) 6 | rt = 1; 7 | break; 8 | end 9 | end 10 | end -------------------------------------------------------------------------------- /ACC/acc.m: -------------------------------------------------------------------------------- 1 | function dx = acc(t,x) 2 | global u; 3 | global v0; 4 | m = 1650; 5 | f0 = 0.1; 6 | f1 = 5; 7 | f2 = 0.25; 8 | dx = zeros(3,1); 9 | dx(1) = x(2); 10 | dx(2) = (1/m)*u(1); %Fr = 0 11 | %dx(2) = (1/m)*(u(1) - f0 - f1*x(2) - f2*x(2).^2); 12 | dx(3) = v0 - x(2); -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # High Order Control Barrier Functions (HOCBFs) 2 | 3 | Lyapunov methods for safety-critical control theory (in Matlab) 4 | 5 | ![pipeline](ACC/CBF.png) 6 | 7 | 8 | There are two simple demos using HOCBFs: Adaptive Cruise Control and Robot Control. 9 | 10 | 11 | 12 | If you find this helpful, please cite our work: 13 | ``` 14 | @article{xiao2021high, 15 | title={High-order control barrier functions}, 16 | author={Xiao, Wei and Belta, Calin}, 17 | journal={IEEE Transactions on Automatic Control}, 18 | volume={67}, 19 | number={7}, 20 | pages={3655--3662}, 21 | year={2021}, 22 | publisher={IEEE} 23 | } 24 | ``` 25 | ``` 26 | @inproceedings{xiao2019control, 27 | title={Control barrier functions for systems with high relative degree}, 28 | author={Xiao, Wei and Belta, Calin}, 29 | booktitle={2019 IEEE 58th conference on decision and control (CDC)}, 30 | pages={474--479}, 31 | year={2019}, 32 | organization={IEEE} 33 | } 34 | ``` -------------------------------------------------------------------------------- /Robot2D/CM_draw.m: -------------------------------------------------------------------------------- 1 | function [j, sense] = CM_draw(mode, pl, pt, ps, ptheta, pos, heading, px_history, py_history, i, j, size) 2 | global va vb theta_d 3 | j = j + 1; 4 | if(j > size) 5 | j = 1; 6 | end 7 | if i <= size 8 | p_history = [px_history(1:i); py_history(1:i)]; 9 | else 10 | if j < 2 11 | p_history = [px_history; py_history]; 12 | else 13 | p_history = [[px_history(j:end),px_history(1:j-1)];[py_history(j:end),py_history(1:j-1)]]; 14 | end 15 | end 16 | if (strcmp(mode,'3D')) 17 | pz = 0; 18 | px = pos(1); 19 | py = pos(2); 20 | lll = length(p_history(1,:)); 21 | ppp = zeros(1,lll); 22 | set(pl,'XData',px,'YData',py,'ZData',pz) 23 | set(pt,'XData',p_history(1,:),'YData',p_history(2,:),'ZData',ppp) 24 | if(i >= 100 && i < 199) 25 | va = -75/99*i + 15 + 100*75/99; 26 | view(va, vb); 27 | end 28 | if(i >= 299 && i < 399) 29 | va =0.9*i -60 - 0.9*299; 30 | view(va, vb); 31 | end 32 | if(i >= 749 && i < 799) 33 | va =1.2*i + 30 - 1.2*749; 34 | view(va, vb); 35 | end 36 | if(i >= 899 && i < 959) 37 | vb =0.8*i + 40 - 0.8*899; 38 | view(va, vb); 39 | end 40 | drawnow 41 | else 42 | px = pos(1); 43 | py = pos(2); 44 | set(pl,'XData',px,'YData',py) 45 | set(pt,'XData',p_history(1,:),'YData',p_history(2,:)) 46 | r=7; theta=heading - pi/3:pi/20:heading + pi/3; 47 | len = length(theta); 48 | noi = rand(1,len)-0.5; 49 | x=r*cos(theta) + noi; y=r*sin(theta) + noi; 50 | set(ps,'XData',[px, px + x, px],'YData',[py, py + y, py]) 51 | sense.x = px + x; sense.y = py + y; 52 | dd = 2; 53 | xd = cos(theta_d)*dd; yd = sin(theta_d)*dd; 54 | xarrow = px + 0.6*xd; yarrow = py + 0.6*yd; 55 | theta1 = theta_d + pi/2; theta2 = theta_d - pi/2; 56 | xa1 = xarrow + 0.4*cos(theta1); ya1 = yarrow + 0.4*sin(theta1); 57 | xa2 = xarrow + 0.4*cos(theta2); ya2 = yarrow + 0.4*sin(theta2); 58 | set(ptheta,'XData',[px, px + xd, xa1, px + xd, xa2],'YData',[py, py + yd, ya1, py + yd, ya2]) 59 | drawnow 60 | end 61 | end -------------------------------------------------------------------------------- /ACC/acc_main.m: -------------------------------------------------------------------------------- 1 | clc; 2 | x0 = [0 20 100]; % ego vehicle state, x, v, distance with respect to the preceding vehicle 3 | g = 9.81; 4 | m = 1650; 5 | f0 = 0.1; 6 | f1 = 5; 7 | f2 = 0.25; 8 | vd = 30; %24 9 | eps = 10; 10 | gamma = 1; 11 | ca = 0.4; 12 | cd = 0.4; 13 | psc = 1; 14 | result3 = zeros(300,5); 15 | data1 = zeros(3000,8); 16 | c = 10; 17 | set(0,'DefaultTextInterpreter','latex') 18 | warning('off'); 19 | global u; 20 | global v0; 21 | v0 = 13.89; % the speed of the preceding vehicle (constant) 22 | 23 | 24 | mode = 1; % control model: 0 - without minimum braking distance, 1-with minimum braking distance. 25 | 26 | k = 1; 27 | for i = 1:300 28 | i 29 | Fr = f0 + f1*x0(2) + f2*x0(2)^2; Fr = 0; 30 | phi0 = -2*(x0(2) - vd)*Fr/m + eps*(x0(2) - vd)^2; 31 | phi1 = 2*(x0(2) - vd)/m; 32 | 33 | if mode == 0 % CBF without minimum braking distance 34 | LfB =v0 - x0(2) +1.8*Fr/m + x0(3) - 1.8*x0(2); % 1st degree CBF 35 | LgB = 1.8/m; 36 | else %CBF with minimum braking distance 37 | LfB = v0 - x0(2) - ((v0 - x0(2))/(cd*g) - 1.8)*Fr/m + x0(3) - 1.8*x0(2) - 0.5*(v0 - x0(2))^2/(cd*g); 38 | LgB = -((v0 - x0(2))/(cd*g) - 1.8)/m; 39 | end 40 | 41 | 42 | A = [phi1 -1;LgB 0;1 0]; 43 | b = [-phi0;LfB;ca*m*g]; 44 | H = [2/(m^2) 0; 0 2*psc]; 45 | F = [-2*Fr/(m^2); 0]; 46 | options = optimoptions('quadprog',... 47 | 'Algorithm','interior-point-convex','Display','off'); 48 | [u,fval,exitflag,output,lambda] = ... 49 | quadprog(H,F,A,b,[],[],[],[],[],options); 50 | t=[0 0.1]; 51 | data1(i,7:8) = [u(1)/m, u(2)]; 52 | [tt,xx]=ode45('acc',t,x0); 53 | x0 = [xx(end, 1) xx(end, 2) xx(end, 3)]; 54 | result1(i,:) = [0.1*i xx(end,2) u(1) x0(3)-1.8*x0(2)]; 55 | end 56 | 57 | figure(1) 58 | subplot(3, 1, 1) 59 | plot(result1(:, 1),result1(:, 2),'b-',[0,30],[vd,vd], 'k--',[0,30],[13.89,13.89], 'k--') 60 | xlabel('$t/s$','fontsize',15) 61 | ylabel('speed', 'fontsize',15) 62 | grid on 63 | subplot(3, 1, 2) 64 | plot(result1(:, 1),result1(:, 3)/(m*g), 'b-',[0,30],[ca,ca], 'k--',[0,30],[-cd,-cd], 'k--') 65 | axis([0 30 -0.75 0.75]); 66 | xlabel('$t/s$','fontsize',15) 67 | ylabel('control', 'fontsize',15) 68 | grid on 69 | subplot(3, 1, 3) 70 | plot(result1(:, 1),result1(:, 4), 'b-',[0,30],[0,0], 'k--') 71 | axis([0 30 -5 75]); 72 | grid on 73 | xlabel('$t/s$','fontsize',15) 74 | ylabel('safety', 'fontsize',15) 75 | 76 | 77 | -------------------------------------------------------------------------------- /Robot2D/CM_main.m: -------------------------------------------------------------------------------- 1 | clc; clear 2 | set(0,'DefaultTextInterpreter', 'latex') 3 | mode = '2D'; global va vb first1 first2 firstB firstC firstD %pa pb pc pd pA pB pC pD 4 | global u once b_c set1 set2 5 | first1 = 1; first2 = 1;firstB = 1; firstC = 1; firstD = 1; va = 15; vb = 40; 6 | sense.x = 0; sense.y = 0; set1 = 0; set2 = 0; 7 | 8 | once = 1; b_c = 0; 9 | te = 2500; 10 | ax = 38; ay = 40; bx = 39; by = 35; cx = 30; cy = 15; dx = 20; dy = 28; %target location 10 20 11 | Ax = 32; Ay = 25; Bx = 28; By = 35; Cx = 30; Cy = 40;%Obstacle location 12 | x1 = [12 0 atan2(35,bx - 12) 1]; % 13 | 14 | [rt, ps, ptheta] = map(mode, ax, ay, bx, by, cx, cy, dx, dy, Ax, Ay, Bx, By, Cx, Cy); 15 | pl = rt(1); pt = rt(2); 16 | 17 | size = 2500; j = 1; %700 18 | px_history = zeros(1,size); py_history = zeros(1,size); 19 | % pause(15); 20 | % start = 1 21 | % pause(2); 22 | u_history = zeros(te,5); 23 | 24 | 25 | coe = [2.5, 0, 0, 0.28, 0]; %0, 0, 0.3, 0, 0.2 %or 2.5, 0.1, 0, 0.28, 0 26 | coe = [0.7426 1.9148 1.9745 0.7024]; 27 | coe = [0.7535 1.0046 0.6664 1.0267]; 28 | for i = 1:te 29 | if(i < 870) 30 | x1 = CM_exe(x1,[bx,by],[Ax,Ay],[Bx,By],[Cx,Cy],sense,coe); 31 | else 32 | if(i < 1200) 33 | x1 = CM_exe(x1,[cx,cy],[Ax,Ay],[Bx,By],[Cx,Cy],sense,coe); 34 | else 35 | if(i < 1865) %1880 36 | x1 = CM_exe(x1,[ax,ay],[Ax,Ay],[Bx,By],[Cx,Cy],sense,coe); 37 | else 38 | x1 = CM_exe(x1,[dx,dy],[Ax,Ay],[Bx,By],[Cx,Cy],sense,coe); 39 | end 40 | end 41 | end 42 | u_history(i,:) = [0.1*i, u(1), u(2) set1 set2]; 43 | pos = [x1(1) x1(2)]; 44 | px_history(j) = pos(1); py_history(j) = pos(2); 45 | 46 | [j, sense] = CM_draw(mode, pl, pt,ps, ptheta, pos, x1(3), px_history, py_history, i, j, size); 47 | if(b_c ~= 0) 48 | b_c 49 | b_c = 0; 50 | end 51 | end 52 | figure(2) 53 | plot(u_history(:,1),u_history(:,2)) 54 | figure(3) 55 | plot(u_history(:,1),u_history(:,3)) 56 | figure(4) 57 | plot(u_history(:,1),u_history(:,4)) 58 | figure(5) 59 | plot(u_history(:,1),u_history(:,5)) 60 | 61 | 62 | % for i = 1:te 63 | % if(i < 700) 64 | % x1 = CM_exe(x1,[bx,by],[Ax,Ay]); 65 | % else 66 | % if(i < 1200) 67 | % x1 = CM_exe(x1,[dx,dy],[Ax,Ay]); 68 | % else 69 | % if(i < 1600) 70 | % x1 = CM_exe(x1,[ax,ay],[Ax,Ay]); 71 | % else 72 | % x1 = CM_exe(x1,[cx,cy],[Ax,Ay]); 73 | % end 74 | % end 75 | % end 76 | % u_history(i,:) = [0.1*i, u(1), u(2)]; 77 | % pos = [x1(1) x1(2)]; 78 | % px_history(j) = pos(1); py_history(j) = pos(2); 79 | % 80 | % j = CM_draw(mode, pl, pt, pos, px_history, py_history, i, j, size); 81 | % end 82 | 83 | 84 | -------------------------------------------------------------------------------- /Robot2D/map.m: -------------------------------------------------------------------------------- 1 | function [rt, ps, ptheta] = map(mode, ax, ay, bx, by, cx, cy, dx, dy, Ax, Ay, Bx, By, Cx, Cy) 2 | global pa pb pc pd pA pB pC pD pobsA pobsB pobsC 3 | if(strcmp(mode,'3D')) 4 | figure(1) 5 | hold on; axis equal 6 | warning('off','Matlab:hg:EraseModeIgnored'); 7 | R = 6; h = 6; m = 400; 8 | [x,y,z] = cylinder(R,m,'b'); 9 | x = x + Ax; y = y + Ay; z = h*z; 10 | mesh(x,y,z) 11 | r=7; theta=0:pi/100:2*pi; 12 | x=r*cos(theta); y=r*sin(theta); 13 | plot3(x+Ax,y+Ay,zeros(1,length(x)),'--') 14 | x=R*cos(theta); y=R*sin(theta); 15 | fill3(x+Ax,y+Ay,ones(1,length(x))*h,'b') 16 | [x,y,z] = cylinder(R,m,'b'); 17 | 18 | pa = plot3(ax,ay,0,'k+','LineWidth',5); 19 | pA = text(ax,ay+2,0,'A','FontSize',20); 20 | pa.MarkerEdgeColor = 'green'; pa.MarkerFaceColor= 'green'; 21 | pA.Color = 'green'; 22 | pb = plot3(bx,by,0,'k+','LineWidth',5); 23 | pB = text(bx,by+2,0,'B','FontSize',20); 24 | pc = plot3(cx,cy,0,'k+','LineWidth',5); 25 | pC = text(cx,cy+2,0,'C','FontSize',20); 26 | pd = plot3(dx,dy,0,'k+','LineWidth',5); 27 | pD = text(dx,dy+2,0,'D','FontSize',20); 28 | 29 | t=0; y1 = 0; 30 | pl = plot3(t,y1,3,'go',... %vehicle trunk 31 | 'EraseMode','background','LineWidth',10); 32 | pl.MarkerEdgeColor = 'red'; 33 | pt = plot3(t,y1,3,'r-',... %vehicle trajectory 34 | 'EraseMode','background','LineWidth',1); 35 | axis([0 50 0 50]); 36 | set(gcf,'unit','centimeters','position',[8 3 20 15]); 37 | txt_legend = text(0,0,0,''); 38 | grid on 39 | view(15, 40); 40 | else 41 | figure(1) 42 | hold on;axis equal 43 | 44 | r=6; theta=0:pi/100:2*pi; 45 | x=r*cos(theta); y=r*sin(theta); 46 | rho=r*sin(theta); 47 | plot(x+Ax,y+Ay,'k-'); 48 | fill(x+Ax,y+Ay,'c') 49 | r=7; theta=0:pi/100:2*pi; 50 | x=r*cos(theta); y=r*sin(theta); 51 | rho=r*sin(theta); 52 | pobsA = text(Ax-6,Ay,'Detected','FontSize',20); 53 | pobsA.Color = 'cyan'; 54 | plot(x+Ax,y+Ay,'b--') 55 | 56 | r=5; theta=0:pi/100:2*pi; 57 | x=r*cos(theta); y=r*sin(theta); 58 | rho=r*sin(theta); 59 | plot(x+Bx,y+By,'r-'); 60 | fill(x+Bx,y+By,'c') 61 | r=6; theta=0:pi/100:2*pi; 62 | x=r*cos(theta); y=r*sin(theta); 63 | rho=r*sin(theta); 64 | pobsB = text(Bx-6,By,'Detected','FontSize',20); 65 | pobsB.Color = 'cyan'; 66 | plot(x+Bx,y+By,'b--') 67 | 68 | r=6; theta=0:pi/100:2*pi; 69 | x=r*cos(theta); y=r*sin(theta); 70 | rho=r*sin(theta); 71 | plot(x+Cx,y+Cy,'r-'); 72 | fill(x+Cx,y+Cy,'c') 73 | r=7; theta=0:pi/100:2*pi; 74 | x=r*cos(theta); y=r*sin(theta); 75 | rho=r*sin(theta); 76 | pobsC = text(Cx-6,Cy,'Detected','FontSize',20); 77 | pobsC.Color = 'cyan'; 78 | plot(x+Cx,y+Cy,'b--') 79 | 80 | pa = plot(ax,ay,'k+','LineWidth',5); 81 | pA = text(ax,ay+2,'A','FontSize',20); 82 | pa.MarkerEdgeColor = 'green'; pa.MarkerFaceColor= 'green'; 83 | pA.Color = 'green'; 84 | pb = plot(bx,by,'k+','LineWidth',5); 85 | pB = text(bx,by+2,'B','FontSize',20); 86 | pb.MarkerEdgeColor = 'green'; pb.MarkerFaceColor= 'green'; 87 | pB.Color = 'green'; 88 | pc = plot(cx,cy,'k+','LineWidth',5); 89 | pC = text(cx,cy+2,'C','FontSize',20); 90 | pc.MarkerEdgeColor = 'green'; pc.MarkerFaceColor= 'green'; 91 | pC.Color = 'green'; 92 | pd = plot(dx,dy,'k+','LineWidth',5); 93 | pD = text(dx,dy+2,'D','FontSize',20); 94 | pd.MarkerEdgeColor = 'green'; pd.MarkerFaceColor= 'green'; 95 | pD.Color = 'green'; 96 | warning('off','Matlab:hg:EraseModeIgnored'); 97 | t=0; y1 = 0; 98 | pl = plot(t,y1,'go',... %vehicle trunk 99 | 'EraseMode','background','LineWidth',10); 100 | pl.MarkerEdgeColor = 'red'; 101 | pt = plot(t,y1,'r-',... %vehicle trajectory 102 | 'EraseMode','background','LineWidth',1); 103 | ps = plot(t,y1,'r:',... %vehicle sense 104 | 'EraseMode','background','LineWidth',2); 105 | ptheta = plot(t,y1,'k',... %vehicle sense 106 | 'EraseMode','background','LineWidth',2); 107 | 108 | axis([0 50 0 50]); 109 | set(gcf,'unit','centimeters','position',[8 3 20 15]); 110 | txt_legend = text(0,0,''); 111 | end 112 | rt = [pl,pt]; 113 | end -------------------------------------------------------------------------------- /Robot2D/CM_exe.m: -------------------------------------------------------------------------------- 1 | function [rt, dist_dst] = CM_exe(state, dst, unsafe,unsafeB,unsafeC, sense, coe) 2 | global u once b_c set1 set2 theta_d pobsA pobsB pobsC 3 | x = state(1); y = state(2); theta = state(3); speed = state(4); 4 | px = dst(1); py = dst(2); 5 | radius = 7; 6 | p1 = coe(1);q1 = coe(2);p2 = coe(3);q2 = coe(4); 7 | u_max = 0.2; u_min = -0.2; 8 | a_max = 0.5; a_min = -0.5; 9 | v_max = 2; v_min = 0; 10 | dist_dst = sqrt((x - px)^2 + (y - py)^2); 11 | eps = 10; 12 | psc = 1; 13 | if(theta < -pi) 14 | theta = pi; 15 | state(3) = pi; 16 | end 17 | if(theta > pi) 18 | theta = -pi; 19 | state(3) = -pi; 20 | end 21 | theta_d = atan2(py - y, px - x); 22 | % if(theta < 0 && theta > -pi && theta_d > 0) 23 | % theta_d = -1.5*pi; 24 | % end 25 | if(theta < 0 && theta > -pi && theta_d >= pi + theta && theta_d <= pi) 26 | theta_d = -1.5*pi; 27 | end 28 | if(theta > 0 && theta < pi && theta_d <= -pi + theta && theta_d >= -pi) 29 | theta_d = 1.5*pi; 30 | end 31 | 32 | V = (theta - theta_d)^2; 33 | LfV = 0; 34 | LgV = 2*(theta - theta_d); 35 | b_V = -LfV - eps*V; 36 | 37 | nx = unsafe(1); ny = unsafe(2); %%%%%%%%%%%%%%Obs A 38 | dist = sqrt((x - nx)^2 + (y - ny)^2); 39 | if(sensing(sense,[nx,ny])) 40 | pobsA.Color = 'red'; 41 | b = dist - radius; 42 | b_dot = ((x - nx)*speed*cos(theta) + (y - ny)*speed*sin(theta))/dist; 43 | LgLfb = ((y - ny)*speed*cos(theta) - (x - nx)*speed*sin(theta))/dist; 44 | LgLfb2 = ((x - nx)*cos(theta) + (y - ny)*sin(theta))/dist; 45 | Lf2b = (speed^2*dist^2 - ((x - nx)*speed*cos(theta) + (y - ny)*speed*sin(theta))^2)/dist^3; 46 | A_safe = -LgLfb; 47 | A_safeu2 = -LgLfb2; 48 | A_safe = [A_safe A_safeu2 0 0]; 49 | psi_1 = b_dot + p1*b^q1; 50 | b_safe = Lf2b + p1*q1*b^(q1 - 1)*b_dot + p2*psi_1^q2; 51 | set1 = b; set2 = psi_1; 52 | else 53 | pobsA.Color = 'cyan'; 54 | A_safe = []; 55 | b_safe = []; 56 | end 57 | 58 | 59 | nx = unsafeB(1); ny = unsafeB(2); %%%%%%%%%%%%%%Obs B 60 | dist = sqrt((x - nx)^2 + (y - ny)^2); 61 | if(sensing(sense,[nx,ny])) 62 | pobsB.Color = 'red'; 63 | radius = 6; 64 | b = dist - radius; 65 | b_dot = ((x - nx)*speed*cos(theta) + (y - ny)*speed*sin(theta))/dist; 66 | LgLfb = ((y - ny)*speed*cos(theta) - (x - nx)*speed*sin(theta))/dist; 67 | LgLfb2 = ((x - nx)*cos(theta) + (y - ny)*sin(theta))/dist; 68 | Lf2b = (speed^2*dist^2 - ((x - nx)*speed*cos(theta) + (y - ny)*speed*sin(theta))^2)/dist^3; 69 | A_safeB = -LgLfb; 70 | A_safeu2B = -LgLfb2; 71 | A_safeB = [A_safeB A_safeu2B 0 0]; 72 | psi_1 = b_dot + p1*b^q1; 73 | b_safeB = Lf2b + p1*q1*b^(q1 - 1)*b_dot + p2*psi_1^q2; 74 | else 75 | pobsB.Color = 'cyan'; 76 | A_safeB = []; 77 | b_safeB = []; 78 | end 79 | 80 | nx = unsafeC(1); ny = unsafeC(2); %%%%%%%%%%%%%%Obs C 81 | dist = sqrt((x - nx)^2 + (y - ny)^2); 82 | if(sensing(sense,[nx,ny])) 83 | radius = 7; 84 | pobsC.Color = 'red'; 85 | b = dist - radius; 86 | b_dot = ((x - nx)*speed*cos(theta) + (y - ny)*speed*sin(theta))/dist; 87 | LgLfb = ((y - ny)*speed*cos(theta) - (x - nx)*speed*sin(theta))/dist; 88 | LgLfb2 = ((x - nx)*cos(theta) + (y - ny)*sin(theta))/dist; 89 | Lf2b = (speed^2*dist^2 - ((x - nx)*speed*cos(theta) + (y - ny)*speed*sin(theta))^2)/dist^3; 90 | A_safeC = -LgLfb; 91 | A_safeu2C = -LgLfb2; 92 | A_safeC = [A_safeC A_safeu2C 0 0]; 93 | psi_1 = b_dot + p1*b^q1; 94 | b_safeC = Lf2b + p1*q1*b^(q1 - 1)*b_dot + p2*psi_1^q2; 95 | else 96 | pobsC.Color = 'cyan'; 97 | A_safeC = []; 98 | b_safeC = []; 99 | end 100 | 101 | vd = dist_dst*v_max/10; 102 | if(dist_dst < 1) 103 | vd = 0; 104 | end 105 | V_speed = (speed - vd)^2; 106 | LfV_speed = 0; 107 | LgV_speed = 2*(speed - vd); 108 | 109 | b_vmax = v_max - speed; 110 | Lgb_vmax = -1; 111 | A_vmax = -Lgb_vmax; 112 | b_vmin = speed - v_min; 113 | Lgb_vmin = 1; 114 | A_vmin = -Lgb_vmin; 115 | %angle_control, acc, relax for theta, relax for speed %0 -1 0 0; 116 | % -a_min; 117 | A = [LgV 0 -1 0;A_safe;A_safeB;A_safeC; 1 0 0 0; -1 0 0 0; 0 1 0 0; 0 A_vmax 0 0; 0 A_vmin 0 0;0 LgV_speed 0 -1]; 118 | b = [b_V;b_safe; b_safeB;b_safeC; u_max; -u_min; a_max; b_vmax; b_vmin;-LfV_speed - eps*V_speed]; 119 | H = [2 0 0 0;0 2 0 0;0 0 2*psc 0;0 0 0 2*psc]; 120 | F = [0; 0; 0; 0]; 121 | options = optimoptions('quadprog',... 122 | 'Algorithm','interior-point-convex','Display','off'); 123 | [u,fval,~,~,~] = ... 124 | quadprog(H,F,A,b,[],[],[],[],[],options); 125 | t=[0 0.1]; 126 | if(u(3) > 0.1 && once == 1) 127 | b_c = dist - radius; 128 | once = 0; 129 | end 130 | [~,xx]=ode45('CM_dynamics',t,state); 131 | rt = [xx(end, 1), xx(end, 2), xx(end, 3), xx(end, 4)]; 132 | end --------------------------------------------------------------------------------