├── Animation ├── PathPlanning │ ├── ParallelParkingTrailer.gif │ ├── ParallelParkingTrailer.jpg │ ├── PerpendicularParkingTrailer.gif │ └── PerpendicularParkingTrailer.jpg └── PathTracking │ ├── CircleTracking.gif │ ├── Demo_Circle.jpg │ ├── Demo_Straight.jpg │ ├── Deviation_Circle.jpg │ ├── Deviation_Straight.jpg │ ├── MPC_Car.jpg │ ├── MPC_Car_DeltaS.jpg │ └── nmpc_trailer_straight_backup.jpg ├── LICENSE ├── PathPlanning ├── ParallelParkingTrailer.avi ├── hybridAStar_CarTrailer.m ├── hybridAStar_CarTrailer_test.m ├── hybridAStar_Trailer.m └── mainRun.m ├── PathTracking ├── MPC │ ├── mpcCar.m │ └── mpcCar_deltaS.m ├── NLMPC │ ├── VehicleTrailerAModelCT.m │ ├── VehicleTrailerAModelDT0.m │ ├── VehicleTrailerModelOutputFcn.m │ └── nlmpcCarTrailerA.m └── pathTracking_CarTrailer.m └── README.md /Animation/PathPlanning/ParallelParkingTrailer.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jingtian123qwe/ADAS_Functions_MATLAB/238506494f264c7b1abf452fb4b376e53c44486d/Animation/PathPlanning/ParallelParkingTrailer.gif -------------------------------------------------------------------------------- /Animation/PathPlanning/ParallelParkingTrailer.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jingtian123qwe/ADAS_Functions_MATLAB/238506494f264c7b1abf452fb4b376e53c44486d/Animation/PathPlanning/ParallelParkingTrailer.jpg -------------------------------------------------------------------------------- /Animation/PathPlanning/PerpendicularParkingTrailer.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jingtian123qwe/ADAS_Functions_MATLAB/238506494f264c7b1abf452fb4b376e53c44486d/Animation/PathPlanning/PerpendicularParkingTrailer.gif -------------------------------------------------------------------------------- /Animation/PathPlanning/PerpendicularParkingTrailer.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jingtian123qwe/ADAS_Functions_MATLAB/238506494f264c7b1abf452fb4b376e53c44486d/Animation/PathPlanning/PerpendicularParkingTrailer.jpg -------------------------------------------------------------------------------- /Animation/PathTracking/CircleTracking.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jingtian123qwe/ADAS_Functions_MATLAB/238506494f264c7b1abf452fb4b376e53c44486d/Animation/PathTracking/CircleTracking.gif -------------------------------------------------------------------------------- /Animation/PathTracking/Demo_Circle.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jingtian123qwe/ADAS_Functions_MATLAB/238506494f264c7b1abf452fb4b376e53c44486d/Animation/PathTracking/Demo_Circle.jpg -------------------------------------------------------------------------------- /Animation/PathTracking/Demo_Straight.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jingtian123qwe/ADAS_Functions_MATLAB/238506494f264c7b1abf452fb4b376e53c44486d/Animation/PathTracking/Demo_Straight.jpg -------------------------------------------------------------------------------- /Animation/PathTracking/Deviation_Circle.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jingtian123qwe/ADAS_Functions_MATLAB/238506494f264c7b1abf452fb4b376e53c44486d/Animation/PathTracking/Deviation_Circle.jpg -------------------------------------------------------------------------------- /Animation/PathTracking/Deviation_Straight.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jingtian123qwe/ADAS_Functions_MATLAB/238506494f264c7b1abf452fb4b376e53c44486d/Animation/PathTracking/Deviation_Straight.jpg -------------------------------------------------------------------------------- /Animation/PathTracking/MPC_Car.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jingtian123qwe/ADAS_Functions_MATLAB/238506494f264c7b1abf452fb4b376e53c44486d/Animation/PathTracking/MPC_Car.jpg -------------------------------------------------------------------------------- /Animation/PathTracking/MPC_Car_DeltaS.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jingtian123qwe/ADAS_Functions_MATLAB/238506494f264c7b1abf452fb4b376e53c44486d/Animation/PathTracking/MPC_Car_DeltaS.jpg -------------------------------------------------------------------------------- /Animation/PathTracking/nmpc_trailer_straight_backup.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jingtian123qwe/ADAS_Functions_MATLAB/238506494f264c7b1abf452fb4b376e53c44486d/Animation/PathTracking/nmpc_trailer_straight_backup.jpg -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 JunyuZhou 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /PathPlanning/ParallelParkingTrailer.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jingtian123qwe/ADAS_Functions_MATLAB/238506494f264c7b1abf452fb4b376e53c44486d/PathPlanning/ParallelParkingTrailer.avi -------------------------------------------------------------------------------- /PathPlanning/hybridAStar_CarTrailer.m: -------------------------------------------------------------------------------- 1 | classdef hybridAStar_CarTrailer < handle 2 | 3 | % Hybrid AStar Algorithm for Car Trailer System 4 | % 5 | % Author: Junyu Zhou 6 | % 7 | % Reference: Practical Search Techniques in Path Planning for Autonomous Driving 8 | % 9 | % Copyright (c) 2020, Junyu Zhou 10 | % All rights reserved. 11 | % License : Modified BSD Software License Agreement 12 | 13 | properties 14 | % parameters of car and trailer 15 | CarLength 16 | CarWidth 17 | CarHitch 18 | WheelBase 19 | WheelTrack 20 | TrailerBase 21 | TrailerTrack 22 | 23 | % map parameters 24 | MapDimX 25 | MapDimY 26 | StartPoint 27 | GoalPoint 28 | Obstacle 29 | 30 | % resolution of A* algorithm 31 | TickXY 32 | TickAngle 33 | 34 | % calculated path 35 | Path 36 | SteeringWheel 37 | 38 | end 39 | 40 | properties (Access = private) 41 | AniFig 42 | end 43 | 44 | methods 45 | 46 | function obj = hybridAStar_CarTrailer(CarLength, CarWidth, WheelBase, ... 47 | WheelTrack, TrailerBase, TrailerTrack, CarHitch) 48 | if nargin == 7 49 | obj.CarLength = CarLength; 50 | obj.CarWidth = CarWidth; 51 | obj.CarHitch = CarHitch; 52 | obj.WheelBase = WheelBase; 53 | obj.WheelTrack = WheelTrack; 54 | obj.TrailerBase = TrailerBase; 55 | obj.TrailerTrack = TrailerTrack; 56 | else 57 | obj.CarLength = 3.5; 58 | obj.CarWidth = 2; 59 | obj.CarHitch = 0.3; 60 | obj.WheelBase = 2.5; 61 | obj.WheelTrack = 1.6; 62 | obj.TrailerBase = 2; 63 | obj.TrailerTrack = 1.5; 64 | end 65 | 66 | obj.StartPoint = [14, 16, pi, pi]; 67 | obj.GoalPoint = [14, 18, pi, pi]; 68 | obj.MapDimX = 20; 69 | obj.MapDimY = 20; 70 | obj.Obstacle = []; 71 | obj.TickXY = 0.5; 72 | obj.TickAngle = 5/180*pi; 73 | obj.Path = []; 74 | obj.SteeringWheel = []; 75 | obj.AniFig = []; 76 | end 77 | 78 | function setStartPoint(obj, x, y, phi_car, phi_trailer) 79 | obj.StartPoint = [x y phi_car phi_trailer]; 80 | end 81 | 82 | function setGoalPoint(obj, x, y) 83 | obj.GoalPoint = [x y phi_car phi_trailer]; 84 | end 85 | 86 | function setMapDim(obj, x, y) 87 | obj.MapDimX = x; 88 | obj.MapDimY = y; 89 | 90 | boundary=[]; 91 | for i1=0: y 92 | boundary=[boundary;[0 i1]]; 93 | end 94 | 95 | for i2=0: x 96 | boundary=[boundary;[i2 0]]; 97 | end 98 | 99 | for i3=0: y 100 | boundary=[boundary;[x i3]]; 101 | end 102 | 103 | for i4=0: x 104 | boundary=[boundary;[i4 y]]; 105 | end 106 | 107 | obj.Obstacle = [obj.Obstacle; boundary]; 108 | end 109 | 110 | function [path, steeringwheel] = hybridAstar(obj) 111 | openv = []; 112 | costv = 0; 113 | closev = zeros(1,12); 114 | 115 | openv = [openv; obj.StartPoint costv + obj.h(obj.StartPoint,obj.GoalPoint)]; 116 | 117 | found = false; 118 | 119 | Numexpand = 0; 120 | 121 | 122 | while ~found 123 | 124 | [ costvalue, id] = min(openv(:,5)); 125 | xv = openv(id, 1); 126 | yv = openv(id, 2); 127 | yawv = openv(id, 3); 128 | yawt = openv(id, 4); 129 | node = [xv, yv, yawv, yawt, costvalue]; 130 | openv(id,:) = []; 131 | 132 | 133 | if obj.isSamePosi(obj.GoalPoint, node(1:4)) 134 | node 135 | found = true; 136 | end 137 | 138 | [mgroup, steeringgroup] = obj.moveNextNode(node); 139 | 140 | for i1 = 1:length(mgroup(:,1)) 141 | 142 | m = mgroup(i1,:); 143 | steering = steeringgroup(i1,:); 144 | idInt = obj.calId(m); 145 | if m(1) > 0 && m(1) <= obj.MapDimX && m(2) > 0 && m(2) <= obj.MapDimY && abs(obj.roundAngle(m(4)-m(3)))<= 45/180*pi 146 | 147 | if ~obj.checkTrailer(m(1), m(2), m(4)) && ~obj.checkCar(m(1), m(2), m(3)) 148 | 149 | id = find(closev(:,11) == idInt); 150 | if ~isempty(id) 151 | if (m(5) < closev(id, 5)) 152 | 153 | openv = [openv; m]; 154 | closev(id, 1:5) = m; 155 | closev(id, 6:10) = node; 156 | closev(id,11) = idInt; 157 | closev(id,12) = steering; 158 | 159 | Numexpand = Numexpand +1 160 | end 161 | else 162 | openv = [openv; m]; 163 | closev = [closev; m, node, idInt, steering]; 164 | 165 | Numexpand = Numexpand +1 166 | obj.stepAnimation(m, steering); 167 | end 168 | end 169 | end 170 | end 171 | end 172 | 173 | [path, steeringwheel] = obj.getPaths(closev, obj.StartPoint, node(1:4)); 174 | 175 | obj.Path = path; 176 | obj.SteeringWheel = steeringwheel; 177 | 178 | end 179 | 180 | 181 | function animationTrailer(obj, VideoName) 182 | 183 | path = obj.Path; 184 | steeringwheel = obj.SteeringWheel; 185 | obstacle = obj.Obstacle; 186 | DimX = obj.MapDimX; 187 | DimY = obj.MapDimY; 188 | 189 | h = figure; 190 | set(gcf,'color','w'); 191 | 192 | hold off; 193 | if length(obstacle)>=1 194 | plot(obstacle(:,1),obstacle(:,2),'ok');hold on; 195 | end 196 | 197 | plot(path(:,1),path(:,2),'-r');hold on; 198 | for i=1:length(path(:,1)) 199 | obj.drawCar(h, path(i,1), path(i,2), path(i,3), steeringwheel(i)) 200 | obj.drawTrailer(h, path(i,1), path(i,2), path(i,3), path(i,4)) 201 | end 202 | axis([0-0.5 DimX+0.5 0-0.5 DimY+0.5]) 203 | xlabel('[m]') 204 | ylabel('[m]') 205 | grid on; 206 | 207 | 208 | if nargin > 1 209 | 210 | writerObj = VideoWriter([VideoName '.avi']); 211 | 212 | writerObj.FrameRate = 24*2; 213 | % set the seconds per image 214 | % open the video writer 215 | open(writerObj); 216 | % write the frames to the video 217 | h1 = figure; 218 | 219 | set(gcf,'position',[200,100,800,750]) 220 | set(gcf,'color','w'); 221 | 222 | for i=1:length(path(:,1)) 223 | if i == 1 224 | hold off; 225 | if length(obstacle)>=1 226 | plot(obstacle(:,1),obstacle(:,2),'ok');hold on; 227 | end 228 | 229 | plot(path(:,1),path(:,2),'-r');hold on; 230 | 231 | obj.drawCar(h1, path(i,1), path(i,2), path(i,3), steeringwheel(i)) 232 | obj.drawTrailer(h1, path(i,1), path(i,2), path(i,3), path(i,4)) 233 | 234 | axis([0-0.5 DimX+0.5 0-0.5 DimY+0.5]) 235 | xlabel('[m]') 236 | ylabel('[m]') 237 | grid on; 238 | frame = getframe(h1); 239 | writeVideo(writerObj, frame); 240 | else 241 | for k=1:24 242 | hold off; 243 | if length(obstacle)>=1 244 | plot(obstacle(:,1),obstacle(:,2),'ok');hold on; 245 | end 246 | 247 | plot(path(:,1),path(:,2),'-r');hold on; 248 | 249 | 250 | xpath = (path(i,1) - path(i-1,1))/24*k + path(i-1,1); 251 | ypath = (path(i,2) - path(i-1,2))/24*k + path(i-1,2); 252 | 253 | if (path(i,3) - path(i-1,3)) >= -pi && (path(i,3) - path(i-1,3)) <= pi 254 | carori = (path(i,3) - path(i-1,3))/24*k + path(i-1,3); 255 | else 256 | if (path(i,3) - path(i-1,3)) < -pi 257 | interval = (path(i,3) - path(i-1,3)) + 2*pi; 258 | carori = path(i,3) - interval + interval/24*k; 259 | else 260 | interval = (path(i,3) - path(i-1,3)) - 2*pi; 261 | carori = path(i,3) - interval + interval/24*k; 262 | end 263 | end 264 | 265 | if (path(i,4) - path(i-1,4)) >= -pi && (path(i,4) - path(i-1,4)) <= pi 266 | trailerori = (path(i,4) - path(i-1,4))/24*k + path(i-1,4); 267 | else 268 | if (path(i,4) - path(i-1,4)) < -pi 269 | interval = (path(i,4) - path(i-1,4)) + 2*pi; 270 | trailerori = path(i,4) - interval + interval/24*k; 271 | else 272 | interval = (path(i,4) - path(i-1,4)) - 2*pi; 273 | trailerori = path(i,4) - interval + interval/24*k; 274 | end 275 | end 276 | 277 | 278 | obj.drawCar(h1, xpath, ypath, carori, steeringwheel(i)) 279 | obj.drawTrailer(h1, xpath, ypath, carori, trailerori) 280 | 281 | axis([0-0.5 DimX+0.5 0-0.5 DimY+0.5]) 282 | xlabel('[m]') 283 | ylabel('[m]') 284 | grid on; 285 | frame = getframe(h1); 286 | writeVideo(writerObj, frame); 287 | end 288 | end 289 | end 290 | 291 | % close the writer object 292 | close(writerObj); 293 | end 294 | end 295 | 296 | function setObstacle(obj, obstacles) 297 | if nargin == 1 298 | ob = []; 299 | for i1 = 0:8 300 | ob = [ob; [i1 17]]; 301 | end 302 | 303 | for i1 = 18:20 304 | ob = [ob; [i1 17]]; 305 | end 306 | 307 | for i1 = 18:20 308 | ob = [ob; [8 i1]]; 309 | end 310 | 311 | for i1 = 18:20 312 | ob = [ob; [18 i1]]; 313 | end 314 | 315 | 316 | for i1 = 0:8 317 | ob = [ob; [i1 7]]; 318 | end 319 | 320 | for i1 = 12:20 321 | ob = [ob; [i1 7]]; 322 | end 323 | 324 | for i1 = 0:7 325 | ob = [ob; [8 i1]]; 326 | end 327 | 328 | for i1 = 0:7 329 | ob = [ob; [12 i1]]; 330 | end 331 | 332 | for i1=0:(obj.MapDimX) 333 | ob=[ob;[i1 12]]; 334 | end 335 | 336 | obj.Obstacle = [obj.Obstacle; ob]; 337 | else 338 | obj.Obstacle = [obj.Obstacle; obstacles]; 339 | end 340 | 341 | end 342 | 343 | end 344 | 345 | methods (Access = private) 346 | 347 | function result = h(obj, a, b) % heuristic function of 2D Euclid distance 348 | result=norm(a(1:2)-b(1:2)); 349 | end 350 | 351 | 352 | function result = isSamePosi(obj, a, b) 353 | result=false; 354 | d=a(1:4)-b; 355 | if abs(d(1))pi 380 | angle=angle-2*pi; 381 | end 382 | while angle<=-pi 383 | angle=angle+2*pi; 384 | end 385 | end 386 | 387 | function [path, steeringwheel] = getPaths(obj, close, start, goal) 388 | id = obj.findPoint(close, goal); 389 | path = [goal]; 390 | steeringwheel = [0]; 391 | path = [close(id, 6:9); path]; 392 | steeringwheel = [close(id,12); steeringwheel]; 393 | prepoint = close(id, 6:9); 394 | %found = false; 395 | while ~obj.isSamePosi(start,prepoint) 396 | for io=1:length(close(:,1)) 397 | 398 | [x1, y1, orientation1, ot1] = obj.roundPos(close(io,1:4)); 399 | [x2, y2, orientation2, ot2] = obj.roundPos(prepoint); 400 | 401 | if obj.isSamePosi([x1, y1, orientation1, ot1],[x2, y2, orientation2, ot2]) 402 | id=io; 403 | prepoint = close(id, 6:9); 404 | break; 405 | end 406 | end 407 | path = [close(id, 6:9); path]; 408 | steeringwheel = [close(id,12); steeringwheel]; 409 | end 410 | end 411 | 412 | function drawTrailer(obj, h, x, y, theta_car, theta_trailer) 413 | 414 | carhitchdis = obj.CarHitch; 415 | x = x - carhitchdis*cos(theta_car); 416 | y = y - carhitchdis*sin(theta_car); 417 | 418 | trailerwidth = obj.TrailerTrack; 419 | trailerlength = obj.TrailerBase; 420 | 421 | rear = linspace(-trailerwidth/2, trailerwidth/2, 50); 422 | rear = [-trailerlength*ones(length(rear),1), rear']; 423 | 424 | rod = linspace(0, -trailerlength, 50); 425 | rod = [ rod', 0*ones(length(rod),1),]; 426 | 427 | rod = obj.changePos(x, y, theta_trailer, rod); 428 | rear = obj.changePos(x, y, theta_trailer, rear); 429 | 430 | 431 | wl = linspace(-trailerlength-0.2, -trailerlength+0.2, 20); 432 | wl = [wl', trailerwidth/2*ones(length(wl),1)]; 433 | 434 | wr = linspace(-trailerlength-0.2, -trailerlength+0.2, 20); 435 | wr = [wr', -trailerwidth/2*ones(length(wr),1)]; 436 | 437 | wl = obj.changePos(x, y, theta_trailer, wl); 438 | wr = obj.changePos(x, y, theta_trailer, wr); 439 | 440 | 441 | figure(h) 442 | hold on 443 | plot(rear(:,1), rear(:,2), 'LineWidth',2, 'color', 'b') 444 | plot(rod(:,1), rod(:,2), 'LineWidth',2, 'color', 'b') 445 | plot(wl(:,1), wl(:,2), 'LineWidth',5, 'color', 'b') 446 | plot(wr(:,1), wr(:,2), 'LineWidth',5, 'color', 'b') 447 | end 448 | 449 | function drawCar(obj, h, x, y, theta, steering) 450 | 451 | carwidth = obj.CarWidth; 452 | carlength = obj.CarLength; 453 | wheelbase = obj.WheelBase; 454 | wheeltrack = obj.WheelTrack; 455 | carhitchdis = obj.CarHitch; 456 | 457 | rear = linspace(-carwidth/2, carwidth/2, 50); 458 | rear = [-carhitchdis*ones(length(rear),1), rear']; 459 | 460 | front = linspace(-carwidth/2, carwidth/2, 50); 461 | front = [(carlength-carhitchdis)*ones(length(front),1), front']; 462 | 463 | left = linspace(-carhitchdis, (carlength-carhitchdis), 50); 464 | left = [ left', carwidth/2*ones(length(left),1),]; 465 | 466 | right = linspace(-carhitchdis, (carlength-carhitchdis), 50); 467 | right = [ right', -carwidth/2*ones(length(right),1),]; 468 | 469 | left = obj.changePos(x, y, theta, left); 470 | right = obj.changePos(x, y, theta, right); 471 | front = obj.changePos(x, y, theta, front); 472 | rear = obj.changePos(x, y, theta, rear); 473 | 474 | 475 | wrl = linspace(-0.2, +0.2, 20); 476 | wrl = [wrl', wheeltrack/2*ones(length(wrl),1)]; 477 | 478 | wrr = linspace(-0.2, +0.2, 20); 479 | wrr = [wrr', -wheeltrack/2*ones(length(wrr),1)]; 480 | 481 | wrl = obj.changePos(x, y, theta, wrl); 482 | wrr = obj.changePos(x, y, theta, wrr); 483 | 484 | 485 | wfl = linspace(-0.2, +0.2, 20); 486 | wfl = [wfl', 0*ones(length(wfl),1)]; 487 | %wfl = [wfl', carwidth/2*ones(length(wfl),1)-0.2]; 488 | 489 | wfr = linspace(-0.2,+0.2, 20); 490 | wfr = [wfr', 0*ones(length(wfr),1)]; 491 | %wfr = [wfr', -carwidth/2*ones(length(wfr),1)+0.2]; 492 | 493 | wfl = obj.changePos(wheelbase, wheeltrack/2, steering, wfl); 494 | wfr = obj.changePos(wheelbase, -wheeltrack/2, steering, wfr); 495 | 496 | wfl = obj.changePos(x, y, theta, wfl); 497 | wfr = obj.changePos(x, y, theta, wfr); 498 | 499 | 500 | figure(h) 501 | hold on 502 | plot(rear(:,1), rear(:,2), 'LineWidth',2, 'color', 'k') 503 | plot(front(:,1), front(:,2), 'LineWidth',2, 'color', 'k') 504 | plot(left(:,1), left(:,2), 'LineWidth',2, 'color', 'k') 505 | plot(right(:,1), right(:,2), 'LineWidth',2, 'color', 'k') 506 | 507 | plot(wfl(:,1), wfl(:,2), 'LineWidth',5, 'color', 'r') 508 | plot(wfr(:,1), wfr(:,2), 'LineWidth',5, 'color', 'r') 509 | plot(wrl(:,1), wrl(:,2), 'LineWidth',5, 'color', 'r') 510 | plot(wrr(:,1), wrr(:,2), 'LineWidth',5, 'color', 'r') 511 | end 512 | 513 | function vectorNew = changePos(obj, x, y, theta, vector) 514 | for i = 1 : length(vector) 515 | vectorNew(i,:) = [x+ vector(i,1)* cos(theta)- vector(i,2)*sin(theta), y + vector(i,2)*cos(theta) + vector(i,1)*sin(theta)]; 516 | end 517 | end 518 | 519 | function id = findPoint(obj, close, point) 520 | id = []; 521 | idx = find(close(:,1) == point(1)); 522 | for k1 = 1:length(idx) 523 | if close(idx(k1),2) == point(2) && close(idx(k1), 3) == point(3) && close(idx(k1), 4) == point(4) 524 | id = idx(k1); 525 | return 526 | end 527 | end 528 | 529 | end 530 | 531 | function status = checkCar_Test(obj, x, y, theta) 532 | 533 | carwidth = obj.CarWidth; 534 | carlength = obj.CarLength; 535 | obstacle = obj.Obstacle; 536 | 537 | rear = linspace(-carwidth/2, carwidth/2, 50); 538 | rear = [-0.3*ones(length(rear),1), rear']; 539 | 540 | front = linspace(-carwidth/2, carwidth/2, 50); 541 | front = [carlength*ones(length(front),1), front']; 542 | 543 | left = linspace(-0.3, carlength, 50); 544 | left = [ left', carwidth/2*ones(length(left),1),]; 545 | 546 | right = linspace(-0.3, carlength, 50); 547 | right = [ right', -carwidth/2*ones(length(right),1),]; 548 | 549 | left = obj.changePos(x, y, theta, left); 550 | right = obj.changePos(x, y, theta, right); 551 | front = obj.changePos(x, y, theta, front); 552 | rear = obj.changePos(x, y, theta, rear); 553 | 554 | 555 | list = [left; right; front; rear]; 556 | 557 | status = false; 558 | for j1 = 1 : length(list) 559 | for j2 = 1 : length(obstacle) 560 | if norm(list(j1, :) - obstacle(j2,:)) < 0.2 561 | status = true; 562 | return; 563 | end 564 | end 565 | 566 | end 567 | end 568 | 569 | function status = checkCar(obj, x, y, theta) 570 | 571 | obstacle = obj.Obstacle; 572 | 573 | carwidth = obj.CarWidth; 574 | carlength = obj.CarLength; 575 | carhitchdis = obj.CarHitch; 576 | 577 | rear = linspace(-carwidth/2, carwidth/2, 50); 578 | rear = [-carhitchdis*ones(length(rear),1), rear']; 579 | 580 | front = linspace(-carwidth/2, carwidth/2, 50); 581 | front = [(carlength-carhitchdis)*ones(length(front),1), front']; 582 | 583 | left = linspace(-carhitchdis, (carlength-carhitchdis), 50); 584 | left = [ left', carwidth/2*ones(length(left),1),]; 585 | 586 | right = linspace(-carhitchdis, (carlength-carhitchdis), 50); 587 | right = [ right', -carwidth/2*ones(length(right),1),]; 588 | 589 | left = obj.changePos(x, y, theta, left); 590 | right = obj.changePos(x, y, theta, right); 591 | front = obj.changePos(x, y, theta, front); 592 | rear = obj.changePos(x, y, theta, rear); 593 | 594 | list = [left; right; front; rear]; 595 | 596 | status = false; 597 | for j1 = 1 : length(list) 598 | if list(j1,1) >= 20-0.2 599 | status = true; 600 | return; 601 | end 602 | 603 | if list(j1,1) <= 0+0.2 604 | status = true; 605 | return; 606 | end 607 | 608 | if list(j1,2) >= 20-0.2 609 | status = true; 610 | return; 611 | end 612 | 613 | if list(j1,2) <= 0+0.2 614 | status = true; 615 | return; 616 | end 617 | if list(j1,2) <= 12+0.2 618 | status = true; 619 | return; 620 | end 621 | 622 | % if list(j1,2) >= 12-0.2 623 | % status = true; 624 | % return; 625 | % end 626 | 627 | if list(j1,2) >= 17 && list(j1, 1) <= 8+0.2 628 | status = true; 629 | return; 630 | end 631 | 632 | if list(j1,2) >= 17 && list(j1, 1) >= 18-0.2 633 | status = true; 634 | return; 635 | end 636 | 637 | if list(j1,2) <= 7 && list(j1, 1) <= 8+0.2 638 | status = true; 639 | return; 640 | end 641 | 642 | if list(j1,2) <= 7 && list(j1, 1) >= 12-0.2 643 | status = true; 644 | return; 645 | end 646 | 647 | end 648 | end 649 | 650 | function status = checkTrailer_Test(obj, x, y, theta_car, theta_trailer) 651 | 652 | obstacle = obj.Obstacle; 653 | 654 | carhitchdis = obj.CarHitch; 655 | x = x - carhitchdis*cos(theta_car); 656 | y = y - carhitchdis*sin(theta_car); 657 | 658 | trailerwidth = obj.TrailerTrack; 659 | trailerlength = obj.TrailerBase; 660 | 661 | rear = linspace(-trailerwidth/2, trailerwidth/2, 50); 662 | rear = [-trailerlength*ones(length(rear),1), rear']; 663 | 664 | rod = linspace(0, -trailerlength, 50); 665 | rod = [ rod', 0*ones(length(rod),1),]; 666 | 667 | rod = obj.changePos(x, y, theta_trailer, rod); 668 | rear = obj.changePos(x, y, theta_trailer, rear); 669 | 670 | 671 | wl = linspace(-trailerlength-0.2, -trailerlength+0.2, 20); 672 | wl = [wl', trailerwidth/2*ones(length(wl),1)]; 673 | 674 | wr = linspace(-trailerlength-0.2, -trailerlength+0.2, 20); 675 | wr = [wr', -trailerwidth/2*ones(length(wr),1)]; 676 | 677 | wl = obj.changePos(x, y, theta_trailer, wl); 678 | wr = obj.changePos(x, y, theta_trailer, wr); 679 | 680 | 681 | list = [rod; rear; wl; wr]; 682 | 683 | status = false; 684 | for j1 = 1 : length(list) 685 | for j2 = 1 : length(obstacle) 686 | if norm(list(j1, :) - obstacle(j2,:)) < 0.2 687 | status = true; 688 | return; 689 | end 690 | end 691 | end 692 | end 693 | 694 | 695 | function status = checkTrailer(obj, x, y, theta) 696 | 697 | trailerwidth = obj.TrailerTrack; 698 | trailerlength = obj.TrailerBase; 699 | obstacle = obj.Obstacle; 700 | 701 | rear = linspace(-trailerwidth/2, trailerwidth/2, 50); 702 | rear = [-trailerlength*ones(length(rear),1), rear']; 703 | 704 | rod = linspace(0, -trailerlength, 50); 705 | rod = [ rod', 0*ones(length(rod),1),]; 706 | 707 | rod = obj.changePos(x, y, theta, rod); 708 | rear = obj.changePos(x, y, theta, rear); 709 | 710 | 711 | list = [rod; rear]; 712 | 713 | status = false; 714 | for j1 = 1 : length(list) 715 | if list(j1,1) >= 20-0.2 716 | status = true; 717 | return; 718 | end 719 | 720 | if list(j1,1) <= 0+0.2 721 | status = true; 722 | return; 723 | end 724 | 725 | if list(j1,2) >= 20-0.2 726 | status = true; 727 | return; 728 | end 729 | 730 | 731 | if list(j1,2) <= 0+0.2 732 | status = true; 733 | return; 734 | end 735 | 736 | if list(j1,2) <= 12+0.5 737 | status = true; 738 | return; 739 | end 740 | 741 | 742 | % if list(j1,2) >= 12 - 0.2 743 | % status = true; 744 | % return; 745 | % end 746 | 747 | 748 | if list(j1,2) >= 17 && list(j1, 1) <= 8+0.2 749 | status = true; 750 | return; 751 | end 752 | 753 | if list(j1,2) >= 17 && list(j1, 1) >= 18-0.2 754 | status = true; 755 | return; 756 | end 757 | 758 | if list(j1,2) <= 7 && list(j1, 1) <= 8+0.2 759 | status = true; 760 | return; 761 | end 762 | 763 | if list(j1,2) <= 7 && list(j1, 1) >= 12-0.2 764 | status = true; 765 | return; 766 | end 767 | end 768 | end 769 | 770 | function stepAnimation(obj, node, steering) 771 | if isempty(obj.AniFig) 772 | obj.AniFig = figure; 773 | set(gcf,'color','w'); 774 | obstacle = obj.Obstacle; 775 | DimX = obj.MapDimX; 776 | DimY = obj.MapDimY; 777 | hold off; 778 | 779 | if length(obstacle)>=1 780 | plot(obstacle(:,1),obstacle(:,2),'ok');hold on; 781 | end 782 | 783 | axis([0-0.5 DimX+0.5 0-0.5 DimY+0.5]) 784 | xlabel('[m]') 785 | ylabel('[m]') 786 | grid on; 787 | 788 | end 789 | h = obj.AniFig; 790 | 791 | obj.drawCar(h, node(1), node(2), node(3), steering); 792 | obj.drawTrailer(h, node(1), node(2), node(3), node(4)); 793 | end 794 | 795 | function [m, steering]=moveNextNode(obj, node) 796 | 797 | 798 | Tick = obj.TickXY; 799 | Wheelbase = obj.WheelBase; 800 | Trailerbase = obj.TrailerBase; 801 | Hitchdis = obj.CarHitch; 802 | 803 | movedis=[Tick*sqrt(2)+0.0001,-Tick*sqrt(2)-0.0001]; 804 | steering=-30:10:30; 805 | 806 | rad=(steering)/180*pi; 807 | 808 | step = 10; 809 | 810 | m = []; 811 | 812 | steering = []; 813 | 814 | nodeStart = node; 815 | 816 | for i=1:length(rad) 817 | 818 | for j = 1:length(movedis) 819 | 820 | node = nodeStart; 821 | 822 | if rad(i) == 0 823 | angle = 0; 824 | costfact = 1; 825 | else 826 | Rc = Wheelbase/tan(rad(i)); 827 | angle = movedis(j)/step/Rc; 828 | costfact = 2; 829 | end 830 | 831 | if movedis(j)> 0 832 | costfact = costfact + 0; 833 | else 834 | costfact = costfact + 3; 835 | end 836 | 837 | for k = 1:step 838 | 839 | if rad(i) == 0 840 | phi_car=node(3); 841 | 842 | delta_phi_trailer = (node(3)-node(4)); 843 | 844 | delta_phi = movedis(j)/step/Trailerbase*sin(delta_phi_trailer); 845 | 846 | next=[movedis(j)/step*cos(phi_car) movedis(j)/step*sin(phi_car) angle delta_phi costfact/step]; 847 | else 848 | Rc = Wheelbase / tan(rad(i)); 849 | 850 | phi_car=node(3); 851 | 852 | delta_phi_trailer = (node(3)-node(4)); 853 | 854 | v_car = movedis(j)/step; 855 | 856 | v1 = v_car*sin(delta_phi_trailer); 857 | 858 | v2 = v_car*Hitchdis/Rc*cos(delta_phi_trailer); 859 | 860 | delta_phi = (v1-v2)/Trailerbase; 861 | 862 | next=[movedis(j)/step*cos(phi_car) movedis(j)/step*sin(phi_car) angle delta_phi costfact/step]; 863 | 864 | end 865 | node = node + next; 866 | 867 | node(3)=obj.roundAngle(node(3)); 868 | node(4)=obj.roundAngle(node(4)); 869 | 870 | end 871 | 872 | node(5) = node(5) + obj.h(node(1:3),obj.GoalPoint)-obj.h(nodeStart(1:3),obj.GoalPoint); 873 | 874 | m = [m; node]; 875 | 876 | steering = [steering;rad(i)]; 877 | 878 | end 879 | end 880 | 881 | end 882 | 883 | function [m, steering]=moveNextNode_MP(obj, node) 884 | 885 | 886 | Tick = obj.TickXY; 887 | Wheelbase = obj.WheelBase; 888 | Trailerbase = obj.TrailerBase; 889 | 890 | movedis=[Tick*sqrt(2)+0.0001,-Tick*sqrt(2)-0.0001]; 891 | steering=-30:10:30; 892 | 893 | rad=(steering)/180*pi; 894 | 895 | step = 10; 896 | 897 | m = []; 898 | 899 | steering = []; 900 | 901 | nodeStart = node; 902 | 903 | for i=1:length(rad) 904 | 905 | for j = 1:length(movedis) 906 | 907 | node = nodeStart; 908 | 909 | if rad(i) == 0 910 | angle = 0; 911 | costfact = 1; 912 | else 913 | Rc = Wheelbase/tan(rad(i)); 914 | angle = movedis(j)/step/Rc; 915 | costfact = 2; 916 | end 917 | 918 | if movedis(j)> 0 919 | costfact = costfact + 0; 920 | else 921 | costfact = costfact + 5; 922 | end 923 | 924 | for k = 1:step 925 | 926 | phi_car=node(3); 927 | 928 | phi_trailer = (node(3)-node(4)); 929 | 930 | delta_phi = movedis(j)/step/Trailerbase*sin(phi_trailer); 931 | 932 | next=[movedis(j)/step*cos(phi_car) movedis(j)/step*sin(phi_car) angle delta_phi costfact/step]; 933 | 934 | node = node + next; 935 | 936 | node(3)=obj.roundAngle(node(3)); 937 | node(4)=obj.roundAngle(node(4)); 938 | 939 | end 940 | 941 | node(5) = node(5) + obj.h(node(1:3),obj.GoalPoint)-obj.h(nodeStart(1:3),obj.GoalPoint); 942 | 943 | m = [m; node]; 944 | 945 | steering = [steering;rad(i)]; 946 | 947 | end 948 | end 949 | 950 | end 951 | end 952 | end -------------------------------------------------------------------------------- /PathPlanning/hybridAStar_CarTrailer_test.m: -------------------------------------------------------------------------------- 1 | classdef hybridAStar_CarTrailer_test < handle 2 | 3 | % Hybrid AStar Algorithm for Car Trailer System 4 | % 5 | % Author: Junyu Zhou 6 | % 7 | % Reference: Practical Search Techniques in Path Planning for Autonomous Driving 8 | % 9 | % Copyright (c) 2020, Junyu Zhou 10 | % All rights reserved. 11 | % License : Modified BSD Software License Agreement 12 | 13 | properties 14 | % parameters of car and trailer 15 | CarLength 16 | CarWidth 17 | CarHitch 18 | WheelBase 19 | WheelTrack 20 | TrailerBase 21 | TrailerTrack 22 | 23 | % map parameters 24 | MapDimX 25 | MapDimY 26 | StartPoint 27 | GoalPoint 28 | Obstacle 29 | 30 | % resolution of A* algorithm 31 | TickXY 32 | TickAngle 33 | 34 | % calculated path 35 | Path 36 | SteeringWheel 37 | 38 | end 39 | 40 | methods 41 | 42 | function obj = hybridAStar_CarTrailer_test(CarLength, CarWidth, WheelBase, ... 43 | WheelTrack, TrailerBase, TrailerTrack, CarHitch) 44 | if nargin == 7 45 | obj.CarLength = CarLength; 46 | obj.CarWidth = CarWidth; 47 | obj.CarHitch = CarHitch; 48 | obj.WheelBase = WheelBase; 49 | obj.WheelTrack = WheelTrack; 50 | obj.TrailerBase = TrailerBase; 51 | obj.TrailerTrack = TrailerTrack; 52 | else 53 | obj.CarLength = 3.5; 54 | obj.CarWidth = 2; 55 | obj.CarHitch = 0.5; 56 | obj.WheelBase = 2.5; 57 | obj.WheelTrack = 1.6; 58 | obj.TrailerBase = 2; 59 | obj.TrailerTrack = 1.5; 60 | end 61 | 62 | obj.StartPoint = [4, 14, pi, pi]; 63 | obj.GoalPoint = [4.5, 13, pi, pi]; 64 | obj.MapDimX = 20; 65 | obj.MapDimY = 20; 66 | obj.Obstacle = []; 67 | obj.TickXY = 0.5; 68 | obj.TickAngle = 5/180*pi; 69 | obj.Path = []; 70 | obj.SteeringWheel = []; 71 | end 72 | 73 | function setStartPoint(obj, x, y, phi_car, phi_trailer) 74 | obj.StartPoint = [x y phi_car phi_trailer]; 75 | end 76 | 77 | function setGoalPoint(obj, x, y) 78 | obj.GoalPoint = [x y phi_car phi_trailer]; 79 | end 80 | 81 | function setMapDim(obj, x, y) 82 | obj.MapDimX = x; 83 | obj.MapDimY = y; 84 | 85 | boundary=[]; 86 | for i1=0: y 87 | boundary=[boundary;[0 i1]]; 88 | end 89 | 90 | for i2=0: x 91 | boundary=[boundary;[i2 0]]; 92 | end 93 | 94 | for i3=0: y 95 | boundary=[boundary;[x i3]]; 96 | end 97 | 98 | for i4=0: x 99 | boundary=[boundary;[i4 y]]; 100 | end 101 | 102 | obj.Obstacle = [obj.Obstacle; boundary]; 103 | end 104 | 105 | function [path, steeringwheel] = hybridAstar(obj) 106 | openv = []; 107 | costv = 0; 108 | closev = zeros(1,12); 109 | 110 | openv = [openv; obj.StartPoint costv + obj.h(obj.StartPoint,obj.GoalPoint)]; 111 | 112 | found = false; 113 | 114 | Numexpand = 0; 115 | 116 | %% 117 | h = figure; 118 | set(gcf,'color','w'); 119 | obstacle = obj.Obstacle; 120 | DimX = obj.MapDimX; 121 | DimY = obj.MapDimY; 122 | hold off; 123 | if length(obstacle)>=1 124 | plot(obstacle(:,1),obstacle(:,2),'ok');hold on; 125 | end 126 | 127 | axis([0-0.5 DimX+0.5 0-0.5 DimY+0.5]) 128 | xlabel('[m]') 129 | ylabel('[m]') 130 | grid on; 131 | %% 132 | 133 | 134 | 135 | while ~found 136 | 137 | [ costvalue, id] = min(openv(:,5)); 138 | xv = openv(id, 1); 139 | yv = openv(id, 2); 140 | yawv = openv(id, 3); 141 | yawt = openv(id, 4); 142 | node = [xv, yv, yawv, yawt, costvalue]; 143 | openv(id,:) = []; 144 | 145 | 146 | if obj.isSamePosi(obj.GoalPoint, node(1:4)) 147 | node 148 | found = true; 149 | end 150 | 151 | [mgroup, steeringgroup] = obj.moveNextNode(node); 152 | 153 | for i1 = 1:length(mgroup(:,1)) 154 | 155 | m = mgroup(i1,:); 156 | steering = steeringgroup(i1,:); 157 | idInt = obj.calId(m); 158 | if m(1) > 0 && m(1) <= obj.MapDimX && m(2) > 0 && m(2) <= obj.MapDimY && abs(obj.roundAngle(m(4)-m(3)))<= 45/180*pi 159 | 160 | if ~obj.checkTrailer(m(1), m(2), m(4)) && ~obj.checkCar(m(1), m(2), m(3)) 161 | 162 | id = find(closev(:,11) == idInt); 163 | if ~isempty(id) 164 | if (m(5) < closev(id, 5)) 165 | 166 | openv = [openv; m]; 167 | closev(id, 1:5) = m; 168 | closev(id, 6:10) = node; 169 | closev(id,11) = idInt; 170 | closev(id,12) = steering; 171 | 172 | Numexpand = Numexpand +1 173 | end 174 | else 175 | openv = [openv; m]; 176 | closev = [closev; m, node, idInt, steering]; 177 | 178 | obj.drawCar(h, m(1), m(2), m(3), steering); 179 | obj.drawTrailer(h, m(1), m(2), m(3), m(4)); 180 | 181 | Numexpand = Numexpand +1 182 | end 183 | end 184 | end 185 | end 186 | end 187 | 188 | [path, steeringwheel] = obj.getPaths(closev, obj.StartPoint, node(1:4)); 189 | 190 | obj.Path = path; 191 | obj.SteeringWheel = steeringwheel; 192 | 193 | end 194 | 195 | 196 | function animationTrailer(obj, VideoName) 197 | 198 | path = obj.Path; 199 | steeringwheel = obj.SteeringWheel; 200 | obstacle = obj.Obstacle; 201 | DimX = obj.MapDimX; 202 | DimY = obj.MapDimY; 203 | 204 | h = figure; 205 | set(gcf,'color','w'); 206 | 207 | hold off; 208 | if length(obstacle)>=1 209 | plot(obstacle(:,1),obstacle(:,2),'ok');hold on; 210 | end 211 | 212 | plot(path(:,1),path(:,2),'-r');hold on; 213 | for i=1:length(path(:,1)) 214 | obj.drawCar(h, path(i,1), path(i,2), path(i,3), steeringwheel(i)) 215 | obj.drawTrailer(h, path(i,1), path(i,2), path(i,3), path(i,4)) 216 | end 217 | axis([0-0.5 DimX+0.5 0-0.5 DimY+0.5]) 218 | xlabel('[m]') 219 | ylabel('[m]') 220 | grid on; 221 | 222 | 223 | if nargin > 1 224 | 225 | writerObj = VideoWriter([VideoName '.avi']); 226 | 227 | writerObj.FrameRate = 24*2; 228 | % set the seconds per image 229 | % open the video writer 230 | open(writerObj); 231 | % write the frames to the video 232 | h1 = figure; 233 | 234 | set(gcf,'position',[200,100,800,750]) 235 | set(gcf,'color','w'); 236 | 237 | for i=1:length(path(:,1)) 238 | if i == 1 239 | hold off; 240 | if length(obstacle)>=1 241 | plot(obstacle(:,1),obstacle(:,2),'ok');hold on; 242 | end 243 | 244 | plot(path(:,1),path(:,2),'-r');hold on; 245 | 246 | obj.drawCar(h1, path(i,1), path(i,2), path(i,3), steeringwheel(i)) 247 | obj.drawTrailer(h1, path(i,1), path(i,2), path(i,3), path(i,4)) 248 | 249 | axis([0-0.5 DimX+0.5 0-0.5 DimY+0.5]) 250 | xlabel('[m]') 251 | ylabel('[m]') 252 | grid on; 253 | frame = getframe(h1); 254 | writeVideo(writerObj, frame); 255 | else 256 | for k=1:24 257 | hold off; 258 | if length(obstacle)>=1 259 | plot(obstacle(:,1),obstacle(:,2),'ok');hold on; 260 | end 261 | 262 | plot(path(:,1),path(:,2),'-r');hold on; 263 | 264 | 265 | xpath = (path(i,1) - path(i-1,1))/24*k + path(i-1,1); 266 | ypath = (path(i,2) - path(i-1,2))/24*k + path(i-1,2); 267 | 268 | if (path(i,3) - path(i-1,3)) >= -pi && (path(i,3) - path(i-1,3)) <= pi 269 | carori = (path(i,3) - path(i-1,3))/24*k + path(i-1,3); 270 | else 271 | if (path(i,3) - path(i-1,3)) < -pi 272 | interval = (path(i,3) - path(i-1,3)) + 2*pi; 273 | carori = path(i,3) - interval + interval/24*k; 274 | else 275 | interval = (path(i,3) - path(i-1,3)) - 2*pi; 276 | carori = path(i,3) - interval + interval/24*k; 277 | end 278 | end 279 | 280 | if (path(i,4) - path(i-1,4)) >= -pi && (path(i,4) - path(i-1,4)) <= pi 281 | trailerori = (path(i,4) - path(i-1,4))/24*k + path(i-1,4); 282 | else 283 | if (path(i,4) - path(i-1,4)) < -pi 284 | interval = (path(i,4) - path(i-1,4)) + 2*pi; 285 | trailerori = path(i,4) - interval + interval/24*k; 286 | else 287 | interval = (path(i,4) - path(i-1,4)) - 2*pi; 288 | trailerori = path(i,4) - interval + interval/24*k; 289 | end 290 | end 291 | 292 | 293 | obj.drawCar(h1, xpath, ypath, carori, steeringwheel(i)) 294 | obj.drawTrailer(h1, xpath, ypath, carori, trailerori) 295 | 296 | axis([0-0.5 DimX+0.5 0-0.5 DimY+0.5]) 297 | xlabel('[m]') 298 | ylabel('[m]') 299 | grid on; 300 | frame = getframe(h1); 301 | writeVideo(writerObj, frame); 302 | end 303 | end 304 | end 305 | 306 | % close the writer object 307 | close(writerObj); 308 | end 309 | end 310 | 311 | function setObstacle(obj, obstacles) 312 | if nargin == 1 313 | ob = []; 314 | for i1 = 0:8 315 | ob = [ob; [i1 17]]; 316 | end 317 | 318 | for i1 = 18:20 319 | ob = [ob; [i1 17]]; 320 | end 321 | 322 | for i1 = 18:20 323 | ob = [ob; [8 i1]]; 324 | end 325 | 326 | for i1 = 18:20 327 | ob = [ob; [18 i1]]; 328 | end 329 | 330 | 331 | for i1 = 0:8 332 | ob = [ob; [i1 7]]; 333 | end 334 | 335 | for i1 = 12:20 336 | ob = [ob; [i1 7]]; 337 | end 338 | 339 | for i1 = 0:7 340 | ob = [ob; [8 i1]]; 341 | end 342 | 343 | for i1 = 0:7 344 | ob = [ob; [12 i1]]; 345 | end 346 | 347 | for i1=0:(obj.MapDimX) 348 | ob=[ob;[i1 12]]; 349 | end 350 | 351 | obj.Obstacle = [obj.Obstacle; ob]; 352 | else 353 | obj.Obstacle = [obj.Obstacle; obstacles]; 354 | end 355 | 356 | end 357 | 358 | end 359 | 360 | methods (Access = private) 361 | 362 | function result = h(obj, a, b) % heuristic function of 2D Euclid distance 363 | result=norm(a(1:2)-b(1:2))*100; 364 | end 365 | 366 | 367 | function result = isSamePosi(obj, a, b) 368 | result=false; 369 | d=a(1:4)-b; 370 | if abs(d(1))pi 395 | angle=angle-2*pi; 396 | end 397 | while angle<=-pi 398 | angle=angle+2*pi; 399 | end 400 | end 401 | 402 | function [path, steeringwheel] = getPaths(obj, close, start, goal) 403 | id = obj.findPoint(close, goal); 404 | path = [goal]; 405 | steeringwheel = [0]; 406 | path = [close(id, 6:9); path]; 407 | steeringwheel = [close(id,12); steeringwheel]; 408 | prepoint = close(id, 6:9); 409 | %found = false; 410 | while ~obj.isSamePosi(start,prepoint) 411 | for io=1:length(close(:,1)) 412 | 413 | [x1, y1, orientation1, ot1] = obj.roundPos(close(io,1:4)); 414 | [x2, y2, orientation2, ot2] = obj.roundPos(prepoint); 415 | 416 | if obj.isSamePosi([x1, y1, orientation1, ot1],[x2, y2, orientation2, ot2]) 417 | id=io; 418 | prepoint = close(id, 6:9); 419 | break; 420 | end 421 | end 422 | path = [close(id, 6:9); path]; 423 | steeringwheel = [close(id,12); steeringwheel]; 424 | end 425 | end 426 | 427 | function drawTrailer(obj, h, x, y, theta_car, theta_trailer) 428 | 429 | carhitchdis = obj.CarHitch; 430 | x = x - carhitchdis*cos(theta_car); 431 | y = y - carhitchdis*sin(theta_car); 432 | 433 | trailerwidth = obj.TrailerTrack; 434 | trailerlength = obj.TrailerBase; 435 | 436 | rear = linspace(-trailerwidth/2, trailerwidth/2, 50); 437 | rear = [-trailerlength*ones(length(rear),1), rear']; 438 | 439 | rod = linspace(0, -trailerlength, 50); 440 | rod = [ rod', 0*ones(length(rod),1),]; 441 | 442 | rod = obj.changePos(x, y, theta_trailer, rod); 443 | rear = obj.changePos(x, y, theta_trailer, rear); 444 | 445 | 446 | wl = linspace(-trailerlength-0.2, -trailerlength+0.2, 20); 447 | wl = [wl', trailerwidth/2*ones(length(wl),1)]; 448 | 449 | wr = linspace(-trailerlength-0.2, -trailerlength+0.2, 20); 450 | wr = [wr', -trailerwidth/2*ones(length(wr),1)]; 451 | 452 | wl = obj.changePos(x, y, theta_trailer, wl); 453 | wr = obj.changePos(x, y, theta_trailer, wr); 454 | 455 | 456 | figure(h) 457 | hold on 458 | plot(rear(:,1), rear(:,2), 'LineWidth',2, 'color', 'b') 459 | plot(rod(:,1), rod(:,2), 'LineWidth',2, 'color', 'b') 460 | plot(wl(:,1), wl(:,2), 'LineWidth',5, 'color', 'b') 461 | plot(wr(:,1), wr(:,2), 'LineWidth',5, 'color', 'b') 462 | end 463 | 464 | function drawCar(obj, h, x, y, theta, steering) 465 | 466 | carwidth = obj.CarWidth; 467 | carlength = obj.CarLength; 468 | wheelbase = obj.WheelBase; 469 | wheeltrack = obj.WheelTrack; 470 | carhitchdis = obj.CarHitch; 471 | 472 | rear = linspace(-carwidth/2, carwidth/2, 50); 473 | rear = [-carhitchdis*ones(length(rear),1), rear']; 474 | 475 | front = linspace(-carwidth/2, carwidth/2, 50); 476 | front = [(carlength-carhitchdis)*ones(length(front),1), front']; 477 | 478 | left = linspace(-carhitchdis, (carlength-carhitchdis), 50); 479 | left = [ left', carwidth/2*ones(length(left),1),]; 480 | 481 | right = linspace(-carhitchdis, (carlength-carhitchdis), 50); 482 | right = [ right', -carwidth/2*ones(length(right),1),]; 483 | 484 | left = obj.changePos(x, y, theta, left); 485 | right = obj.changePos(x, y, theta, right); 486 | front = obj.changePos(x, y, theta, front); 487 | rear = obj.changePos(x, y, theta, rear); 488 | 489 | 490 | wrl = linspace(-0.2, +0.2, 20); 491 | wrl = [wrl', wheeltrack/2*ones(length(wrl),1)]; 492 | 493 | wrr = linspace(-0.2, +0.2, 20); 494 | wrr = [wrr', -wheeltrack/2*ones(length(wrr),1)]; 495 | 496 | wrl = obj.changePos(x, y, theta, wrl); 497 | wrr = obj.changePos(x, y, theta, wrr); 498 | 499 | 500 | wfl = linspace(-0.2, +0.2, 20); 501 | wfl = [wfl', 0*ones(length(wfl),1)]; 502 | %wfl = [wfl', carwidth/2*ones(length(wfl),1)-0.2]; 503 | 504 | wfr = linspace(-0.2,+0.2, 20); 505 | wfr = [wfr', 0*ones(length(wfr),1)]; 506 | %wfr = [wfr', -carwidth/2*ones(length(wfr),1)+0.2]; 507 | 508 | wfl = obj.changePos(wheelbase, wheeltrack/2, steering, wfl); 509 | wfr = obj.changePos(wheelbase, -wheeltrack/2, steering, wfr); 510 | 511 | wfl = obj.changePos(x, y, theta, wfl); 512 | wfr = obj.changePos(x, y, theta, wfr); 513 | 514 | 515 | figure(h) 516 | hold on 517 | plot(rear(:,1), rear(:,2), 'LineWidth',2, 'color', 'k') 518 | plot(front(:,1), front(:,2), 'LineWidth',2, 'color', 'k') 519 | plot(left(:,1), left(:,2), 'LineWidth',2, 'color', 'k') 520 | plot(right(:,1), right(:,2), 'LineWidth',2, 'color', 'k') 521 | 522 | plot(wfl(:,1), wfl(:,2), 'LineWidth',5, 'color', 'r') 523 | plot(wfr(:,1), wfr(:,2), 'LineWidth',5, 'color', 'r') 524 | plot(wrl(:,1), wrl(:,2), 'LineWidth',5, 'color', 'r') 525 | plot(wrr(:,1), wrr(:,2), 'LineWidth',5, 'color', 'r') 526 | end 527 | 528 | function vectorNew = changePos(obj, x, y, theta, vector) 529 | for i = 1 : length(vector) 530 | vectorNew(i,:) = [x+ vector(i,1)* cos(theta)- vector(i,2)*sin(theta), y + vector(i,2)*cos(theta) + vector(i,1)*sin(theta)]; 531 | end 532 | end 533 | 534 | function id = findPoint(obj, close, point) 535 | id = []; 536 | idx = find(close(:,1) == point(1)); 537 | for k1 = 1:length(idx) 538 | if close(idx(k1),2) == point(2) && close(idx(k1), 3) == point(3) && close(idx(k1), 4) == point(4) 539 | id = idx(k1); 540 | return 541 | end 542 | end 543 | 544 | end 545 | 546 | function status = checkCar_Test(obj, x, y, theta) 547 | 548 | carwidth = obj.CarWidth; 549 | carlength = obj.CarLength; 550 | obstacle = obj.Obstacle; 551 | 552 | rear = linspace(-carwidth/2, carwidth/2, 50); 553 | rear = [-0.3*ones(length(rear),1), rear']; 554 | 555 | front = linspace(-carwidth/2, carwidth/2, 50); 556 | front = [carlength*ones(length(front),1), front']; 557 | 558 | left = linspace(-0.3, carlength, 50); 559 | left = [ left', carwidth/2*ones(length(left),1),]; 560 | 561 | right = linspace(-0.3, carlength, 50); 562 | right = [ right', -carwidth/2*ones(length(right),1),]; 563 | 564 | left = obj.changePos(x, y, theta, left); 565 | right = obj.changePos(x, y, theta, right); 566 | front = obj.changePos(x, y, theta, front); 567 | rear = obj.changePos(x, y, theta, rear); 568 | 569 | 570 | list = [left; right; front; rear]; 571 | 572 | status = false; 573 | for j1 = 1 : length(list) 574 | for j2 = 1 : length(obstacle) 575 | if norm(list(j1, :) - obstacle(j2,:)) < 0.2 576 | status = true; 577 | return; 578 | end 579 | end 580 | 581 | end 582 | end 583 | 584 | function status = checkCar(obj, x, y, theta) 585 | 586 | obstacle = obj.Obstacle; 587 | 588 | carwidth = obj.CarWidth; 589 | carlength = obj.CarLength; 590 | carhitchdis = obj.CarHitch; 591 | 592 | rear = linspace(-carwidth/2, carwidth/2, 50); 593 | rear = [-carhitchdis*ones(length(rear),1), rear']; 594 | 595 | front = linspace(-carwidth/2, carwidth/2, 50); 596 | front = [(carlength-carhitchdis)*ones(length(front),1), front']; 597 | 598 | left = linspace(-carhitchdis, (carlength-carhitchdis), 50); 599 | left = [ left', carwidth/2*ones(length(left),1),]; 600 | 601 | right = linspace(-carhitchdis, (carlength-carhitchdis), 50); 602 | right = [ right', -carwidth/2*ones(length(right),1),]; 603 | 604 | left = obj.changePos(x, y, theta, left); 605 | right = obj.changePos(x, y, theta, right); 606 | front = obj.changePos(x, y, theta, front); 607 | rear = obj.changePos(x, y, theta, rear); 608 | 609 | list = [left; right; front; rear]; 610 | 611 | status = false; 612 | for j1 = 1 : length(list) 613 | if list(j1,1) >= 20-0.2 614 | status = true; 615 | return; 616 | end 617 | 618 | if list(j1,1) <= 0+0.2 619 | status = true; 620 | return; 621 | end 622 | 623 | if list(j1,2) >= 20-0.2 624 | status = true; 625 | return; 626 | end 627 | 628 | if list(j1,2) <= 0+0.2 629 | status = true; 630 | return; 631 | end 632 | if list(j1,2) <= 12+0.2 633 | status = true; 634 | return; 635 | end 636 | 637 | % if list(j1,2) >= 12-0.2 638 | % status = true; 639 | % return; 640 | % end 641 | 642 | if list(j1,2) >= 17 && list(j1, 1) <= 8+0.2 643 | status = true; 644 | return; 645 | end 646 | 647 | if list(j1,2) >= 17 && list(j1, 1) >= 18-0.2 648 | status = true; 649 | return; 650 | end 651 | 652 | if list(j1,2) <= 7 && list(j1, 1) <= 8+0.2 653 | status = true; 654 | return; 655 | end 656 | 657 | if list(j1,2) <= 7 && list(j1, 1) >= 12-0.2 658 | status = true; 659 | return; 660 | end 661 | 662 | end 663 | end 664 | 665 | function status = checkTrailer_Test(obj, x, y, theta_car, theta_trailer) 666 | 667 | obstacle = obj.Obstacle; 668 | 669 | carhitchdis = obj.CarHitch; 670 | x = x - carhitchdis*cos(theta_car); 671 | y = y - carhitchdis*sin(theta_car); 672 | 673 | trailerwidth = obj.TrailerTrack; 674 | trailerlength = obj.TrailerBase; 675 | 676 | rear = linspace(-trailerwidth/2, trailerwidth/2, 50); 677 | rear = [-trailerlength*ones(length(rear),1), rear']; 678 | 679 | rod = linspace(0, -trailerlength, 50); 680 | rod = [ rod', 0*ones(length(rod),1),]; 681 | 682 | rod = obj.changePos(x, y, theta_trailer, rod); 683 | rear = obj.changePos(x, y, theta_trailer, rear); 684 | 685 | 686 | wl = linspace(-trailerlength-0.2, -trailerlength+0.2, 20); 687 | wl = [wl', trailerwidth/2*ones(length(wl),1)]; 688 | 689 | wr = linspace(-trailerlength-0.2, -trailerlength+0.2, 20); 690 | wr = [wr', -trailerwidth/2*ones(length(wr),1)]; 691 | 692 | wl = obj.changePos(x, y, theta_trailer, wl); 693 | wr = obj.changePos(x, y, theta_trailer, wr); 694 | 695 | 696 | list = [rod; rear; wl; wr]; 697 | 698 | status = false; 699 | for j1 = 1 : length(list) 700 | for j2 = 1 : length(obstacle) 701 | if norm(list(j1, :) - obstacle(j2,:)) < 0.2 702 | status = true; 703 | return; 704 | end 705 | end 706 | end 707 | end 708 | 709 | 710 | function status = checkTrailer(obj, x, y, theta) 711 | 712 | trailerwidth = obj.TrailerTrack; 713 | trailerlength = obj.TrailerBase; 714 | obstacle = obj.Obstacle; 715 | 716 | rear = linspace(-trailerwidth/2, trailerwidth/2, 50); 717 | rear = [-trailerlength*ones(length(rear),1), rear']; 718 | 719 | rod = linspace(0, -trailerlength, 50); 720 | rod = [ rod', 0*ones(length(rod),1),]; 721 | 722 | rod = obj.changePos(x, y, theta, rod); 723 | rear = obj.changePos(x, y, theta, rear); 724 | 725 | 726 | list = [rod; rear]; 727 | 728 | status = false; 729 | for j1 = 1 : length(list) 730 | if list(j1,1) >= 20-0.2 731 | status = true; 732 | return; 733 | end 734 | 735 | if list(j1,1) <= 0+0.2 736 | status = true; 737 | return; 738 | end 739 | 740 | if list(j1,2) >= 20-0.2 741 | status = true; 742 | return; 743 | end 744 | 745 | 746 | if list(j1,2) <= 0+0.2 747 | status = true; 748 | return; 749 | end 750 | 751 | if list(j1,2) <= 12+0.5 752 | status = true; 753 | return; 754 | end 755 | 756 | 757 | % if list(j1,2) >= 12 - 0.2 758 | % status = true; 759 | % return; 760 | % end 761 | 762 | 763 | if list(j1,2) >= 17 && list(j1, 1) <= 8+0.2 764 | status = true; 765 | return; 766 | end 767 | 768 | if list(j1,2) >= 17 && list(j1, 1) >= 18-0.2 769 | status = true; 770 | return; 771 | end 772 | 773 | if list(j1,2) <= 7 && list(j1, 1) <= 8+0.2 774 | status = true; 775 | return; 776 | end 777 | 778 | if list(j1,2) <= 7 && list(j1, 1) >= 12-0.2 779 | status = true; 780 | return; 781 | end 782 | end 783 | end 784 | 785 | function [m, steering]=moveNextNode(obj, node) 786 | 787 | 788 | Tick = obj.TickXY; 789 | Wheelbase = obj.WheelBase; 790 | Trailerbase = obj.TrailerBase; 791 | Hitchdis = obj.CarHitch; 792 | 793 | movedis=[Tick*sqrt(2)+0.0001,-Tick*sqrt(2)-0.0001]; 794 | steering=-30:10:30; 795 | 796 | rad=(steering)/180*pi; 797 | 798 | step = 10; 799 | 800 | m = []; 801 | 802 | steering = []; 803 | 804 | nodeStart = node; 805 | 806 | for i=1:length(rad) 807 | 808 | for j = 1:length(movedis) 809 | 810 | node = nodeStart; 811 | 812 | if rad(i) == 0 813 | angle = 0; 814 | costfact = 1; 815 | else 816 | Rc = Wheelbase/tan(rad(i)); 817 | angle = movedis(j)/step/Rc; 818 | costfact = 2; 819 | end 820 | 821 | if movedis(j)> 0 822 | costfact = costfact + 0; 823 | else 824 | costfact = costfact + 3; 825 | end 826 | 827 | for k = 1:step 828 | 829 | if rad(i) == 0 830 | phi_car=node(3); 831 | 832 | delta_phi_trailer = (node(3)-node(4)); 833 | 834 | delta_phi = movedis(j)/step/Trailerbase*sin(delta_phi_trailer); 835 | 836 | next=[movedis(j)/step*cos(phi_car) movedis(j)/step*sin(phi_car) angle delta_phi costfact/step]; 837 | else 838 | Rc = Wheelbase / tan(rad(i)); 839 | 840 | phi_car=node(3); 841 | 842 | delta_phi_trailer = (node(3)-node(4)); 843 | 844 | v_car = movedis(j)/step; 845 | 846 | v1 = v_car*sin(delta_phi_trailer); 847 | 848 | v2 = v_car*Hitchdis/Rc*cos(delta_phi_trailer); 849 | 850 | delta_phi = (v1-v2)/Trailerbase; 851 | 852 | next=[movedis(j)/step*cos(phi_car) movedis(j)/step*sin(phi_car) angle delta_phi costfact/step]; 853 | 854 | end 855 | node = node + next; 856 | 857 | node(3)=obj.roundAngle(node(3)); 858 | node(4)=obj.roundAngle(node(4)); 859 | 860 | end 861 | 862 | node(5) = node(5) + obj.h(node(1:3),obj.GoalPoint)-obj.h(nodeStart(1:3),obj.GoalPoint); 863 | 864 | m = [m; node]; 865 | 866 | steering = [steering;rad(i)]; 867 | 868 | end 869 | end 870 | 871 | end 872 | 873 | function [m, steering]=moveNextNode_MP(obj, node) 874 | 875 | 876 | Tick = obj.TickXY; 877 | Wheelbase = obj.WheelBase; 878 | Trailerbase = obj.TrailerBase; 879 | 880 | movedis=[Tick*sqrt(2)+0.0001,-Tick*sqrt(2)-0.0001]; 881 | steering=-30:10:30; 882 | 883 | rad=(steering)/180*pi; 884 | 885 | step = 10; 886 | 887 | m = []; 888 | 889 | steering = []; 890 | 891 | nodeStart = node; 892 | 893 | for i=1:length(rad) 894 | 895 | for j = 1:length(movedis) 896 | 897 | node = nodeStart; 898 | 899 | if rad(i) == 0 900 | angle = 0; 901 | costfact = 1; 902 | else 903 | Rc = Wheelbase/tan(rad(i)); 904 | angle = movedis(j)/step/Rc; 905 | costfact = 2; 906 | end 907 | 908 | if movedis(j)> 0 909 | costfact = costfact + 0; 910 | else 911 | costfact = costfact + 5; 912 | end 913 | 914 | for k = 1:step 915 | 916 | phi_car=node(3); 917 | 918 | phi_trailer = (node(3)-node(4)); 919 | 920 | delta_phi = movedis(j)/step/Trailerbase*sin(phi_trailer); 921 | 922 | next=[movedis(j)/step*cos(phi_car) movedis(j)/step*sin(phi_car) angle delta_phi costfact/step]; 923 | 924 | node = node + next; 925 | 926 | node(3)=obj.roundAngle(node(3)); 927 | node(4)=obj.roundAngle(node(4)); 928 | 929 | end 930 | 931 | node(5) = node(5) + obj.h(node(1:3),obj.GoalPoint)-obj.h(nodeStart(1:3),obj.GoalPoint); 932 | 933 | m = [m; node]; 934 | 935 | steering = [steering;rad(i)]; 936 | 937 | end 938 | end 939 | 940 | end 941 | end 942 | end -------------------------------------------------------------------------------- /PathPlanning/hybridAStar_Trailer.m: -------------------------------------------------------------------------------- 1 | classdef hybridAStar_Trailer < handle 2 | 3 | % Hybrid AStar Algorithm for Car Trailer System 4 | % 5 | % Author: Junyu Zhou 6 | % 7 | % Reference: Practical Search Techniques in Path Planning for Autonomous Driving 8 | % 9 | % Copyright (c) 2020, Junyu Zhou 10 | % All rights reserved. 11 | % License : Modified BSD Software License Agreement 12 | 13 | properties 14 | % parameters of car and trailer 15 | CarLength 16 | CarWidth 17 | WheelBase 18 | WheelTrack 19 | TrailerBase 20 | TrailerTrack 21 | 22 | % map parameters 23 | MapDimX 24 | MapDimY 25 | StartPoint 26 | GoalPoint 27 | Obstacle 28 | 29 | % resolution of A* algorithm 30 | TickXY 31 | TickAngle 32 | 33 | % calculated path 34 | Path 35 | SteeringWheel 36 | 37 | end 38 | 39 | methods 40 | 41 | function obj = hybridAStar_Trailer(CarLength, CarWidth, WheelBase, ... 42 | WheelTrack, TrailerBase, TrailerTrack) 43 | if nargin == 6 44 | obj.CarLength = CarLength; 45 | obj.CarWidth = CarWidth; 46 | obj.WheelBase = WheelBase; 47 | obj.WheelTrack = WheelTrack; 48 | obj.TrailerBase = TrailerBase; 49 | obj.TrailerTrack = TrailerTrack; 50 | else 51 | obj.CarLength = 3; 52 | obj.CarWidth = 2; 53 | obj.WheelBase = 2.5; 54 | obj.WheelTrack = 1.6; 55 | obj.TrailerBase = 2; 56 | obj.TrailerTrack = 1.5; 57 | end 58 | 59 | obj.StartPoint = [5, 9, 0, 0]; 60 | obj.GoalPoint = [10, 3, pi/2, pi/2]; 61 | obj.MapDimX = 20; 62 | obj.MapDimY = 20; 63 | obj.Obstacle = []; 64 | obj.TickXY = 0.5; 65 | obj.TickAngle = 5/180*pi; 66 | obj.Path = []; 67 | obj.SteeringWheel = []; 68 | end 69 | 70 | function setStartPoint(obj, x, y, phi_car, phi_trailer) 71 | obj.StartPoint = [x y phi_car phi_trailer]; 72 | end 73 | 74 | function setGoalPoint(obj, x, y) 75 | obj.GoalPoint = [x y phi_car phi_trailer]; 76 | end 77 | 78 | function setMapDim(obj, x, y) 79 | obj.MapDimX = x; 80 | obj.MapDimY = y; 81 | 82 | boundary=[]; 83 | for i1=0: y 84 | boundary=[boundary;[0 i1]]; 85 | end 86 | 87 | for i2=0: x 88 | boundary=[boundary;[i2 0]]; 89 | end 90 | 91 | for i3=0: y 92 | boundary=[boundary;[x i3]]; 93 | end 94 | 95 | for i4=0: x 96 | boundary=[boundary;[i4 y]]; 97 | end 98 | 99 | obj.Obstacle = [obj.Obstacle; boundary]; 100 | end 101 | 102 | function [path, steeringwheel] = hybridAstar(obj) 103 | openv = []; 104 | costv = 0; 105 | closev = zeros(1,12); 106 | 107 | openv = [openv; obj.StartPoint costv + obj.h(obj.StartPoint,obj.GoalPoint)]; 108 | 109 | found = false; 110 | 111 | Numexpand = 0; 112 | 113 | 114 | while ~found 115 | 116 | [ costvalue, id] = min(openv(:,5)); 117 | xv = openv(id, 1); 118 | yv = openv(id, 2); 119 | yawv = openv(id, 3); 120 | yawt = openv(id, 4); 121 | node = [xv, yv, yawv, yawt, costvalue]; 122 | openv(id,:) = []; 123 | 124 | 125 | if obj.isSamePosi(obj.GoalPoint, node(1:4)) 126 | node 127 | found = true; 128 | end 129 | 130 | [mgroup, steeringgroup] = obj.moveNextNode(node); 131 | 132 | for i1 = 1:length(mgroup(:,1)) 133 | 134 | m = mgroup(i1,:); 135 | steering = steeringgroup(i1,:); 136 | idInt = obj.calId(m); 137 | if m(1) > 0 && m(1) <= obj.MapDimX && m(2) > 0 && m(2) <= obj.MapDimY && abs(m(4)-m(3))<= 45/180*pi 138 | 139 | if ~obj.checkTrailer(m(1), m(2), m(4)) && ~obj.checkCar(m(1), m(2), m(3)) 140 | 141 | id = find(closev(:,11) == idInt); 142 | if ~isempty(id) 143 | if (m(5) < closev(id, 5)) 144 | 145 | openv = [openv; m]; 146 | closev(id, 1:5) = m; 147 | closev(id, 6:10) = node; 148 | closev(id,11) = idInt; 149 | closev(id,12) = steering; 150 | 151 | Numexpand = Numexpand +1 152 | end 153 | else 154 | openv = [openv; m]; 155 | closev = [closev; m, node, idInt, steering]; 156 | 157 | Numexpand = Numexpand +1 158 | end 159 | end 160 | end 161 | end 162 | end 163 | 164 | [path, steeringwheel] = obj.getPaths(closev, obj.StartPoint, node(1:4)); 165 | 166 | obj.Path = path; 167 | obj.SteeringWheel = steeringwheel; 168 | 169 | end 170 | 171 | 172 | function animationTrailer(obj, VideoName) 173 | 174 | path = obj.Path; 175 | steeringwheel = obj.SteeringWheel; 176 | obstacle = obj.Obstacle; 177 | DimX = obj.MapDimX; 178 | DimY = obj.MapDimY; 179 | 180 | h = figure; 181 | set(gcf,'color','w'); 182 | hold off; 183 | if length(obstacle)>=1 184 | plot(obstacle(:,1),obstacle(:,2),'ok');hold on; 185 | end 186 | 187 | plot(path(:,1),path(:,2),'-r');hold on; 188 | for i=1:length(path(:,1)) 189 | obj.drawCar(h, path(i,1), path(i,2), path(i,3), steeringwheel(i)) 190 | obj.drawTrailer(h, path(i,1), path(i,2), path(i,4)) 191 | end 192 | axis([0-0.5 DimX+0.5 0-0.5 DimY+0.5]) 193 | xlabel('[m]') 194 | ylabel('[m]') 195 | grid on; 196 | 197 | 198 | if nargin > 1 199 | 200 | writerObj = VideoWriter([VideoName '.avi']); 201 | 202 | writerObj.FrameRate = 24*2; 203 | % set the seconds per image 204 | % open the video writer 205 | open(writerObj); 206 | % write the frames to the video 207 | h1 = figure; 208 | 209 | set(gcf,'position',[200,100,800,750]) 210 | set(gcf,'color','w'); 211 | for i=1:length(path(:,1)) 212 | if i == 1 213 | hold off; 214 | if length(obstacle)>=1 215 | plot(obstacle(:,1),obstacle(:,2),'ok');hold on; 216 | end 217 | 218 | plot(path(:,1),path(:,2),'-r');hold on; 219 | 220 | obj.drawCar(h1, path(i,1), path(i,2), path(i,3), steeringwheel(i)) 221 | obj.drawTrailer(h1, path(i,1), path(i,2), path(i,4)) 222 | 223 | axis([0-0.5 DimX+0.5 0-0.5 DimY+0.5]) 224 | xlabel('[m]') 225 | ylabel('[m]') 226 | grid on; 227 | frame = getframe(h1); 228 | writeVideo(writerObj, frame); 229 | else 230 | for k=1:24 231 | hold off; 232 | if length(obstacle)>=1 233 | plot(obstacle(:,1),obstacle(:,2),'ok');hold on; 234 | end 235 | 236 | plot(path(:,1),path(:,2),'-r');hold on; 237 | 238 | 239 | xpath = (path(i,1) - path(i-1,1))/24*k + path(i-1,1); 240 | ypath = (path(i,2) - path(i-1,2))/24*k + path(i-1,2); 241 | 242 | if (path(i,3) - path(i-1,3)) >= -pi && (path(i,3) - path(i-1,3)) <= pi 243 | carori = (path(i,3) - path(i-1,3))/24*k + path(i-1,3); 244 | else 245 | if (path(i,3) - path(i-1,3)) < -pi 246 | interval = (path(i,3) - path(i-1,3)) + 2*pi; 247 | carori = path(i,3) - interval + interval/24*k; 248 | else 249 | interval = (path(i,3) - path(i-1,3)) - 2*pi; 250 | carori = path(i,3) - interval + interval/24*k; 251 | end 252 | end 253 | 254 | if (path(i,4) - path(i-1,4)) >= -pi && (path(i,4) - path(i-1,4)) <= pi 255 | trailerori = (path(i,4) - path(i-1,4))/24*k + path(i-1,4); 256 | else 257 | if (path(i,4) - path(i-1,4)) < -pi 258 | interval = (path(i,4) - path(i-1,4)) + 2*pi; 259 | trailerori = path(i,4) - interval + interval/24*k; 260 | else 261 | interval = (path(i,4) - path(i-1,4)) - 2*pi; 262 | trailerori = path(i,4) - interval + interval/24*k; 263 | end 264 | end 265 | 266 | 267 | obj.drawCar(h1, xpath, ypath, carori, steeringwheel(i)) 268 | obj.drawTrailer(h1, xpath, ypath, trailerori) 269 | 270 | axis([0-0.5 DimX+0.5 0-0.5 DimY+0.5]) 271 | xlabel('[m]') 272 | ylabel('[m]') 273 | grid on; 274 | frame = getframe(h1); 275 | writeVideo(writerObj, frame); 276 | end 277 | end 278 | end 279 | 280 | % close the writer object 281 | close(writerObj); 282 | end 283 | end 284 | 285 | function setObstacle(obj, obstacles) 286 | if nargin == 1 287 | ob = []; 288 | for i1 = 0:8 289 | ob = [ob; [i1 17]]; 290 | end 291 | 292 | for i1 = 18:20 293 | ob = [ob; [i1 17]]; 294 | end 295 | 296 | for i1 = 18:20 297 | ob = [ob; [8 i1]]; 298 | end 299 | 300 | for i1 = 18:20 301 | ob = [ob; [18 i1]]; 302 | end 303 | 304 | 305 | for i1 = 0:8 306 | ob = [ob; [i1 7]]; 307 | end 308 | 309 | for i1 = 12:20 310 | ob = [ob; [i1 7]]; 311 | end 312 | 313 | for i1 = 0:7 314 | ob = [ob; [8 i1]]; 315 | end 316 | 317 | for i1 = 0:7 318 | ob = [ob; [12 i1]]; 319 | end 320 | 321 | for i1=0:(obj.MapDimX) 322 | ob=[ob;[i1 12]]; 323 | end 324 | 325 | obj.Obstacle = [obj.Obstacle; ob]; 326 | else 327 | obj.Obstacle = [obj.Obstacle; obstacles]; 328 | end 329 | 330 | end 331 | 332 | end 333 | 334 | methods (Access = private) 335 | 336 | function result = h(obj, a, b) % heuristic function of 2D Euclid distance 337 | result=norm(a(1:2)-b(1:2)); 338 | end 339 | 340 | 341 | function result = isSamePosi(obj, a, b) 342 | result=false; 343 | d=a(1:4)-b; 344 | if abs(d(1))pi 366 | angle=angle-2*pi; 367 | end 368 | while angle<=-pi 369 | angle=angle+2*pi; 370 | end 371 | end 372 | 373 | function [path, steeringwheel] = getPaths(obj, close, start, goal) 374 | id = obj.findPoint(close, goal); 375 | path = [goal]; 376 | steeringwheel = [0]; 377 | path = [close(id, 6:9); path]; 378 | steeringwheel = [close(id,12); steeringwheel]; 379 | prepoint = close(id, 6:9); 380 | %found = false; 381 | while ~obj.isSamePosi(start,prepoint) 382 | for io=1:length(close(:,1)) 383 | 384 | [x1, y1, orientation1, ot1] = obj.roundPos(close(io,1:4)); 385 | [x2, y2, orientation2, ot2] = obj.roundPos(prepoint); 386 | 387 | if obj.isSamePosi([x1, y1, orientation1, ot1],[x2, y2, orientation2, ot2]) 388 | id=io; 389 | prepoint = close(id, 6:9); 390 | break; 391 | end 392 | end 393 | path = [close(id, 6:9); path]; 394 | steeringwheel = [close(id,12); steeringwheel]; 395 | end 396 | end 397 | 398 | function drawTrailer(obj, h, x, y, theta) 399 | 400 | 401 | trailerwidth = obj.TrailerTrack; 402 | trailerlength = obj.TrailerBase; 403 | 404 | rear = linspace(-trailerwidth/2, trailerwidth/2, 50); 405 | rear = [-trailerlength*ones(length(rear),1), rear']; 406 | 407 | rod = linspace(0, -trailerlength, 50); 408 | rod = [ rod', 0*ones(length(rod),1),]; 409 | 410 | rod = obj.changePos(x, y, theta, rod); 411 | rear = obj.changePos(x, y, theta, rear); 412 | 413 | 414 | wl = linspace(-trailerlength-0.2, -trailerlength+0.2, 20); 415 | wl = [wl', trailerwidth/2*ones(length(wl),1)]; 416 | 417 | wr = linspace(-trailerlength-0.2, -trailerlength+0.2, 20); 418 | wr = [wr', -trailerwidth/2*ones(length(wr),1)]; 419 | 420 | wl = obj.changePos(x, y, theta, wl); 421 | wr = obj.changePos(x, y, theta, wr); 422 | 423 | 424 | figure(h) 425 | hold on 426 | plot(rear(:,1), rear(:,2), 'LineWidth',2, 'color', 'b') 427 | plot(rod(:,1), rod(:,2), 'LineWidth',2, 'color', 'b') 428 | plot(wl(:,1), wl(:,2), 'LineWidth',5, 'color', 'b') 429 | plot(wr(:,1), wr(:,2), 'LineWidth',5, 'color', 'b') 430 | end 431 | 432 | function drawCar(obj, h, x, y, theta, steering) 433 | 434 | carwidth = obj.CarWidth; 435 | carlength = obj.CarLength; 436 | wheelbase = obj.WheelBase; 437 | wheeltrack = obj.WheelTrack; 438 | 439 | rear = linspace(-carwidth/2, carwidth/2, 50); 440 | rear = [-0.3*ones(length(rear),1), rear']; 441 | 442 | front = linspace(-carwidth/2, carwidth/2, 50); 443 | front = [carlength*ones(length(front),1), front']; 444 | 445 | left = linspace(-0.3, carlength, 50); 446 | left = [ left', carwidth/2*ones(length(left),1),]; 447 | 448 | right = linspace(-0.3, carlength, 50); 449 | right = [ right', -carwidth/2*ones(length(right),1),]; 450 | 451 | left = obj.changePos(x, y, theta, left); 452 | right = obj.changePos(x, y, theta, right); 453 | front = obj.changePos(x, y, theta, front); 454 | rear = obj.changePos(x, y, theta, rear); 455 | 456 | 457 | wrl = linspace(-0.2, +0.2, 20); 458 | wrl = [wrl', wheeltrack/2*ones(length(wrl),1)]; 459 | 460 | wrr = linspace(-0.2, +0.2, 20); 461 | wrr = [wrr', -wheeltrack/2*ones(length(wrr),1)]; 462 | 463 | wrl = obj.changePos(x, y, theta, wrl); 464 | wrr = obj.changePos(x, y, theta, wrr); 465 | 466 | 467 | wfl = linspace(-0.2, +0.2, 20); 468 | wfl = [wfl', 0*ones(length(wfl),1)]; 469 | %wfl = [wfl', carwidth/2*ones(length(wfl),1)-0.2]; 470 | 471 | wfr = linspace(-0.2,+0.2, 20); 472 | wfr = [wfr', 0*ones(length(wfr),1)]; 473 | %wfr = [wfr', -carwidth/2*ones(length(wfr),1)+0.2]; 474 | 475 | wfl = obj.changePos(wheelbase, wheeltrack/2, steering, wfl); 476 | wfr = obj.changePos(wheelbase, -wheeltrack/2, steering, wfr); 477 | 478 | wfl = obj.changePos(x, y, theta, wfl); 479 | wfr = obj.changePos(x, y, theta, wfr); 480 | 481 | 482 | figure(h) 483 | hold on 484 | plot(rear(:,1), rear(:,2), 'LineWidth',2, 'color', 'k') 485 | plot(front(:,1), front(:,2), 'LineWidth',2, 'color', 'k') 486 | plot(left(:,1), left(:,2), 'LineWidth',2, 'color', 'k') 487 | plot(right(:,1), right(:,2), 'LineWidth',2, 'color', 'k') 488 | 489 | plot(wfl(:,1), wfl(:,2), 'LineWidth',5, 'color', 'r') 490 | plot(wfr(:,1), wfr(:,2), 'LineWidth',5, 'color', 'r') 491 | plot(wrl(:,1), wrl(:,2), 'LineWidth',5, 'color', 'r') 492 | plot(wrr(:,1), wrr(:,2), 'LineWidth',5, 'color', 'r') 493 | end 494 | 495 | function vectorNew = changePos(obj, x, y, theta, vector) 496 | for i = 1 : length(vector) 497 | vectorNew(i,:) = [x+ vector(i,1)* cos(theta)- vector(i,2)*sin(theta), y + vector(i,2)*cos(theta) + vector(i,1)*sin(theta)]; 498 | end 499 | end 500 | 501 | function id = findPoint(obj, close, point) 502 | id = []; 503 | idx = find(close(:,1) == point(1)); 504 | for k1 = 1:length(idx) 505 | if close(idx(k1),2) == point(2) && close(idx(k1), 3) == point(3) && close(idx(k1), 4) == point(4) 506 | id = idx(k1); 507 | return 508 | end 509 | end 510 | 511 | end 512 | 513 | function status = checkCar_Test(obj, x, y, theta) 514 | 515 | carwidth = obj.CarWidth; 516 | carlength = obj.CarLength; 517 | obstacle = obj.Obstacle; 518 | 519 | rear = linspace(-carwidth/2, carwidth/2, 50); 520 | rear = [-0.3*ones(length(rear),1), rear']; 521 | 522 | front = linspace(-carwidth/2, carwidth/2, 50); 523 | front = [carlength*ones(length(front),1), front']; 524 | 525 | left = linspace(-0.3, carlength, 50); 526 | left = [ left', carwidth/2*ones(length(left),1),]; 527 | 528 | right = linspace(-0.3, carlength, 50); 529 | right = [ right', -carwidth/2*ones(length(right),1),]; 530 | 531 | left = obj.changePos(x, y, theta, left); 532 | right = obj.changePos(x, y, theta, right); 533 | front = obj.changePos(x, y, theta, front); 534 | rear = obj.changePos(x, y, theta, rear); 535 | 536 | 537 | list = [left; right; front; rear]; 538 | 539 | status = false; 540 | for j1 = 1 : length(list) 541 | for j2 = 1 : length(obstacle) 542 | if norm(list(j1, :) - obstacle(j2,:)) < 0.2 543 | status = true; 544 | return; 545 | end 546 | end 547 | 548 | end 549 | end 550 | 551 | function status = checkCar(obj, x, y, theta) 552 | 553 | carwidth = obj.CarWidth; 554 | carlength = obj.CarLength; 555 | obstacle = obj.Obstacle; 556 | 557 | rear = linspace(-carwidth/2, carwidth/2, 50); 558 | rear = [-0.3*ones(length(rear),1), rear']; 559 | 560 | front = linspace(-carwidth/2, carwidth/2, 50); 561 | front = [carlength*ones(length(front),1), front']; 562 | 563 | left = linspace(-0.3, carlength, 50); 564 | left = [ left', carwidth/2*ones(length(left),1),]; 565 | 566 | right = linspace(-0.3, carlength, 50); 567 | right = [ right', -carwidth/2*ones(length(right),1),]; 568 | 569 | left = obj.changePos(x, y, theta, left); 570 | right = obj.changePos(x, y, theta, right); 571 | front = obj.changePos(x, y, theta, front); 572 | rear = obj.changePos(x, y, theta, rear); 573 | 574 | 575 | list = [left; right; front; rear]; 576 | 577 | status = false; 578 | for j1 = 1 : length(list) 579 | if list(j1,1) >= 20-0.2 580 | status = true; 581 | return; 582 | end 583 | 584 | if list(j1,1) <= 0+0.2 585 | status = true; 586 | return; 587 | end 588 | 589 | if list(j1,2) >= 20-0.2 590 | status = true; 591 | return; 592 | end 593 | 594 | if list(j1,2) <= 0+0.2 595 | status = true; 596 | return; 597 | end 598 | % if list(j1,2) <= 12+0.2 599 | % status = true; 600 | % return; 601 | % end 602 | 603 | if list(j1,2) >= 12-0.2 604 | status = true; 605 | return; 606 | end 607 | 608 | if list(j1,2) >= 17 && list(j1, 1) <= 8+0.2 609 | status = true; 610 | return; 611 | end 612 | 613 | if list(j1,2) >= 17 && list(j1, 1) >= 18-0.2 614 | status = true; 615 | return; 616 | end 617 | 618 | if list(j1,2) <= 7 && list(j1, 1) <= 8+0.2 619 | status = true; 620 | return; 621 | end 622 | 623 | if list(j1,2) <= 7 && list(j1, 1) >= 12-0.2 624 | status = true; 625 | return; 626 | end 627 | 628 | end 629 | end 630 | 631 | function status = checkTrailer_Test(obj, x, y, theta) 632 | 633 | trailerwidth = obj.TrailerTrack; 634 | trailerlength = obj.TrailerBase; 635 | obstacle = obj.Obstacle; 636 | 637 | rear = linspace(-trailerwidth/2, trailerwidth/2, 50); 638 | rear = [-trailerlength*ones(length(rear),1), rear']; 639 | 640 | rod = linspace(0, -trailerlength, 50); 641 | rod = [ rod', 0*ones(length(rod),1),]; 642 | 643 | rod = obj.changePos(x, y, theta, rod); 644 | rear = obj.changePos(x, y, theta, rear); 645 | 646 | 647 | wl = linspace(-trailerlength-0.2, -trailerlength+0.2, 20); 648 | wl = [wl', trailerwidth/2*ones(length(wl),1)]; 649 | 650 | wr = linspace(-trailerlength-0.2, -trailerlength+0.2, 20); 651 | wr = [wr', -trailerwidth/2*ones(length(wr),1)]; 652 | 653 | wl = obj.changePos(x, y, theta, wl); 654 | wr = obj.changePos(x, y, theta, wr); 655 | 656 | 657 | 658 | list = [rod; rear; wl; wr]; 659 | 660 | status = false; 661 | for j1 = 1 : length(list) 662 | for j2 = 1 : length(obstacle) 663 | if norm(list(j1, :) - obstacle(j2,:)) < 0.2 664 | status = true; 665 | return; 666 | end 667 | end 668 | end 669 | end 670 | 671 | 672 | function status = checkTrailer(obj, x, y, theta) 673 | 674 | trailerwidth = obj.TrailerTrack; 675 | trailerlength = obj.TrailerBase; 676 | obstacle = obj.Obstacle; 677 | 678 | rear = linspace(-trailerwidth/2, trailerwidth/2, 50); 679 | rear = [-trailerlength*ones(length(rear),1), rear']; 680 | 681 | rod = linspace(0, -trailerlength, 50); 682 | rod = [ rod', 0*ones(length(rod),1),]; 683 | 684 | rod = obj.changePos(x, y, theta, rod); 685 | rear = obj.changePos(x, y, theta, rear); 686 | 687 | 688 | list = [rod; rear]; 689 | 690 | status = false; 691 | for j1 = 1 : length(list) 692 | if list(j1,1) >= 20-0.2 693 | status = true; 694 | return; 695 | end 696 | 697 | if list(j1,1) <= 0+0.2 698 | status = true; 699 | return; 700 | end 701 | 702 | if list(j1,2) >= 20-0.2 703 | status = true; 704 | return; 705 | end 706 | 707 | 708 | if list(j1,2) <= 0+0.2 709 | status = true; 710 | return; 711 | end 712 | 713 | % if list(j1,2) <= 12+0.5 714 | % status = true; 715 | % return; 716 | % end 717 | 718 | 719 | if list(j1,2) >= 12 - 0.2 720 | status = true; 721 | return; 722 | end 723 | 724 | 725 | if list(j1,2) >= 17 && list(j1, 1) <= 8+0.2 726 | status = true; 727 | return; 728 | end 729 | 730 | if list(j1,2) >= 17 && list(j1, 1) >= 18-0.2 731 | status = true; 732 | return; 733 | end 734 | 735 | if list(j1,2) <= 7 && list(j1, 1) <= 8+0.2 736 | status = true; 737 | return; 738 | end 739 | 740 | if list(j1,2) <= 7 && list(j1, 1) >= 12-0.2 741 | status = true; 742 | return; 743 | end 744 | end 745 | end 746 | 747 | function [m, steering]=moveNextNode(obj, node) 748 | 749 | 750 | Tick = obj.TickXY; 751 | Wheelbase = obj.WheelBase; 752 | Trailerbase = obj.TrailerBase; 753 | 754 | movedis=[Tick*sqrt(2)+0.0001,-Tick*sqrt(2)-0.0001]; 755 | steering=-30:10:30; 756 | 757 | rad=(steering)/180*pi; 758 | 759 | step = 10; 760 | 761 | m = []; 762 | 763 | steering = []; 764 | 765 | nodeStart = node; 766 | 767 | for i=1:length(rad) 768 | 769 | for j = 1:length(movedis) 770 | 771 | node = nodeStart; 772 | 773 | if rad(i) == 0 774 | angle = 0; 775 | costfact = 1; 776 | else 777 | Rc = Wheelbase/tan(rad(i)); 778 | angle = movedis(j)/step/Rc; 779 | costfact = 2; 780 | end 781 | 782 | if movedis(j)> 0 783 | costfact = costfact + 0; 784 | else 785 | costfact = costfact + 5; 786 | end 787 | 788 | for k = 1:step 789 | 790 | phi_car=node(3); 791 | 792 | phi_trailer = (node(3)-node(4)); 793 | 794 | delta_phi = movedis(j)/step/Trailerbase*sin(phi_trailer); 795 | 796 | next=[movedis(j)/step*cos(phi_car) movedis(j)/step*sin(phi_car) angle delta_phi costfact/step]; 797 | 798 | node = node + next; 799 | 800 | node(3)=obj.roundAngle(node(3)); 801 | node(4)=obj.roundAngle(node(4)); 802 | 803 | end 804 | 805 | node(5) = node(5) + obj.h(node(1:3),obj.GoalPoint)-obj.h(nodeStart(1:3),obj.GoalPoint); 806 | 807 | m = [m; node]; 808 | 809 | steering = [steering;rad(i)]; 810 | 811 | end 812 | end 813 | 814 | end 815 | end 816 | end -------------------------------------------------------------------------------- /PathPlanning/mainRun.m: -------------------------------------------------------------------------------- 1 | CarTrailer = hybridAStar_CarTrailer; 2 | CarTrailer.setMapDim(20, 20); 3 | CarTrailer.setObstacle; 4 | CarTrailer.hybridAstar 5 | CarTrailer.animationTrailer(); -------------------------------------------------------------------------------- /PathTracking/MPC/mpcCar.m: -------------------------------------------------------------------------------- 1 | V = 20; 2 | x0 = [0; -1; 0; V]; 3 | u0 = [0; 0]; 4 | 5 | %% 6 | % Discretize the continuous-time model using the zero-order holder method 7 | % in the |obstacleVehicleModelDT| function. 8 | Ts = 0.02; 9 | [Ad,Bd,Cd,Dd,U,Y,X,DX] = VehicleModelDT(Ts,x0,u0); 10 | dsys = ss(Ad,Bd,Cd,Dd,'Ts',Ts); 11 | dsys.InputName = {'Throttle','Delta'}; 12 | dsys.StateName = {'X','Y','Theta','V'}; 13 | dsys.OutputName = dsys.StateName; 14 | 15 | 16 | %% MPC Design at the Nominal Operating Point 17 | % Design a model predictive controller that can make the ego car maintain 18 | % a desired velocity and stay in the middle of the center lane. 19 | status = mpcverbosity('off'); 20 | mpcobj = mpc(dsys); 21 | 22 | %% 23 | % The prediction horizon is |25| steps, which is equivalent to 0.5 seconds. 24 | mpcobj.PredictionHorizon = 50;%25; 25 | mpcobj.ControlHorizon = 2;%5; 26 | 27 | %% 28 | % To prevent the ego car from accelerating or decelerating too quickly, add 29 | % a hard constraint of 0.2 (m/s^2) on the throttle rate of change. 30 | mpcobj.ManipulatedVariables(1).RateMin = -0.2*Ts; 31 | mpcobj.ManipulatedVariables(1).RateMax = 0.2*Ts; 32 | 33 | %% 34 | % Similarly, add a hard constraint of 6 degrees per second on the steering 35 | % angle rate of change. 36 | mpcobj.ManipulatedVariables(2).RateMin = -pi/30*Ts; 37 | mpcobj.ManipulatedVariables(2).RateMax = pi/30*Ts; 38 | 39 | %% 40 | % Scale the throttle and steering angle by their respective operating 41 | % ranges. 42 | mpcobj.ManipulatedVariables(1).ScaleFactor = 2; 43 | mpcobj.ManipulatedVariables(2).ScaleFactor = 0.2; 44 | 45 | %% 46 | % Since there are only two manipulated variables, to achieve zero 47 | % steady-state offset, you can choose only two outputs for perfect 48 | % tracking. In this example, choose the Y position and velocity by setting 49 | % the weights of the other two outputs (X and theta) to zero. Doing so lets 50 | % the values of these other outputs float. 51 | mpcobj.Weights.OutputVariables = [0.2 30 0 1]; 52 | 53 | %% 54 | % Update the controller with the nominal operating condition. For a 55 | % discrete-time plant: 56 | % 57 | % * |U = u0| 58 | % * |X = x0| 59 | % * |Y = Cd*x0 + Dd*u0| 60 | % * |DX = Ad*X0 + Bd*u0 - x0| 61 | % 62 | mpcobj.Model.Nominal = struct('U',U,'Y',Y,'X',X,'DX',DX); 63 | 64 | % %% Specify Mixed I/O Constraints for Obstacle Avoidance Maneuver 65 | % % There are different strategies to make the ego car avoid an obstacle on 66 | % % the road. For example, a real-time path planner can compute a new path 67 | % % after an obstacle is detected and the controller follows this path. 68 | % % 69 | % % In this example, use a different approach that takes advantage of the 70 | % % ability of MPC to handle constraints explicitly. When an obstacle is 71 | % % detected, it defines an area on the road (in terms of constraints) that 72 | % % the ego car must not enter during the prediction horizon. At the next 73 | % % control interval, the area is redefined based on the new positions of 74 | % % the ego car and obstacle until passing is completed. 75 | % % 76 | % % To define the area to avoid, use the following mixed input/output 77 | % % constraints: 78 | % % 79 | % % E*u + F*y <= G 80 | % % 81 | % % where |u| is the manipulated variable vector and |y| is the output 82 | % % variable vector. You can update the constraint matrices |E|, |F|, and |G| 83 | % % when the controller is running. 84 | % 85 | % %% 86 | % % The first constraint is an upper bound on $y$ ($y \le 6$ on this 87 | % % three-lane road). 88 | % E1 = [0 0]; 89 | % F1 = [0 1 0 0]; 90 | % G1 = laneWidth*lanes/2; 91 | % 92 | % %% 93 | % % The second constraint is a lower bound on $y$ ($y \ge -6$ on this 94 | % % three-lane road). 95 | % E2 = [0 0]; 96 | % F2 = [0 -1 0 0]; 97 | % G2 = laneWidth*lanes/2; 98 | % 99 | % %% 100 | % % The third constraint is for obstacle avoidance. Even though no obstacle 101 | % % is detected at the nominal operating condition, you must add a "fake" 102 | % % constraint here because you cannot change the dimensions of the 103 | % % constraint matrices at run time. For the fake constraint, use a 104 | % % constraint with the same form as the second constraint. 105 | % E3 = [0 0]; 106 | % F3 = [0 -1 0 0]; 107 | % G3 = laneWidth*lanes/2; 108 | % 109 | % %% 110 | % % Specify the mixed input/output constraints in the controller using the 111 | % % |setconstraint| function. 112 | % setconstraint(mpcobj,[E1;E2;E3],[F1;F2;F3],[G1;G2;G3],[1;1;0.1]); 113 | 114 | %% Simulate Controller 115 | % In this example, you use an adaptive MPC controller because it handles 116 | % the nonlinear vehicle dynamics more effectively than a traditional MPC 117 | % controller. A traditional MPC controller uses a constant plant model. 118 | % However, adaptive MPC allows you to provide a new plant model at each 119 | % control interval. Because the new model describes the plant dynamics more 120 | % accurately at the new operating condition, an adaptive MPC controller 121 | % performs better than a traditional MPC controller. 122 | 123 | %% 124 | % Use a constant reference signal. 125 | refSignal = [0 0 0 V]; 126 | 127 | %% 128 | % Initialize plant and controller states. 129 | x = x0; 130 | u = u0; 131 | egoStates = mpcstate(mpcobj); 132 | 133 | %% 134 | T = 0:Ts:10; 135 | 136 | %% 137 | % Log simulation data for plotting. 138 | saveSlope = zeros(length(T),1); 139 | saveIntercept = zeros(length(T),1); 140 | ympc = zeros(length(T),size(Cd,1)); 141 | umpc = zeros(length(T),size(Bd,2)); 142 | 143 | %% 144 | % Run the simulation. 145 | for k = 1:length(T) 146 | % Obtain new plant model and output measurements for interval |k|. 147 | [Ad,Bd,Cd,Dd,U,Y,X,DX] = VehicleModelDT(Ts,x,u); 148 | measurements = Cd * x + Dd * u; 149 | ympc(k,:) = measurements'; 150 | 151 | % % Determine whether the vehicle sees the obstacle, and update the mixed 152 | % % I/O constraints when obstacle is detected. 153 | % detection = obstacleDetect(x,obstacle,laneWidth); 154 | % [E,F,G,saveSlope(k),saveIntercept(k)] = ... 155 | % obstacleComputeCustomConstraint(x,detection,obstacle,laneWidth,lanes); 156 | 157 | % Prepare new plant model and nominal conditions for adaptive MPC. 158 | newPlant = ss(Ad,Bd,Cd,Dd,'Ts',Ts); 159 | newNominal = struct('U',U,'Y',Y,'X',X,'DX',DX); 160 | 161 | [refSignal, PathRef_x, PathRef_y] = refPath(x, Ts, V, (k-1)*Ts); 162 | % Prepare new mixed I/O constraints. 163 | options = mpcmoveopt; 164 | % options.CustomConstraint = struct('E',E,'F',F,'G',G); 165 | 166 | 167 | % Compute optimal moves using the updated plant, nominal conditions, 168 | % and constraints. 169 | [u,Info] = mpcmoveAdaptive(mpcobj,egoStates,newPlant,newNominal,... 170 | measurements,refSignal); 171 | umpc(k,:) = u'; 172 | 173 | % Update the plant state for the next iteration |k+1|. 174 | x = Ad * x + Bd * u; 175 | end 176 | 177 | mpcverbosity(status); 178 | 179 | %% Analyze Results 180 | figure; 181 | subplot(2,1,1) 182 | hold on 183 | plot(PathRef_x, PathRef_y, 'r'); 184 | plot(ympc(:,1),ympc(:,2),'-k'); 185 | xlabel('x [m]'); 186 | ylabel('y [m]'); 187 | grid on 188 | xlim([min(ympc(:,1)) max(ympc(:,1))]) 189 | 190 | subplot(2,1,2) 191 | plot(T, umpc(:,2)*180/pi) 192 | grid on 193 | xlabel('time [s]') 194 | ylabel('Steering angle [°]') 195 | -------------------------------------------------------------------------------- /PathTracking/MPC/mpcCar_deltaS.m: -------------------------------------------------------------------------------- 1 | V = 20; 2 | x0 = [0; -1; 0; V; 0]; 3 | u0 = [0; 0]; 4 | 5 | %% 6 | % Discretize the continuous-time model using the zero-order holder method 7 | % in the |obstacleVehicleModelDT| function. 8 | Ts = 0.02; 9 | [Ad,Bd,Cd,Dd,U,Y,X,DX] = VehicleModelDT_deltaS(Ts,x0,u0); 10 | dsys = ss(Ad,Bd,Cd,Dd,'Ts',Ts); 11 | dsys.InputName = {'Throttle','D_Delta'}; 12 | dsys.StateName = {'X','Y','Theta','V', 'Delta'}; 13 | dsys.OutputName = dsys.StateName; 14 | 15 | %% MPC Design at the Nominal Operating Point 16 | % Design a model predictive controller that can make the ego car maintain 17 | % a desired velocity and stay in the middle of the center lane. 18 | status = mpcverbosity('off'); 19 | mpcobj = mpc(dsys); 20 | 21 | %% 22 | % The prediction horizon is |25| steps, which is equivalent to 0.5 seconds. 23 | mpcobj.PredictionHorizon = 60;%25; 24 | mpcobj.ControlHorizon = 2;%5; 25 | 26 | %% 27 | % To prevent the ego car from accelerating or decelerating too quickly, add 28 | % a hard constraint of 0.2 (m/s^2) on the throttle rate of change. 29 | mpcobj.ManipulatedVariables(1).RateMin = -0.2*Ts; 30 | mpcobj.ManipulatedVariables(1).RateMax = 0.2*Ts; 31 | 32 | %% 33 | % Similarly, add a hard constraint of 6 degrees per second on the steering 34 | % angle rate of change. 35 | mpcobj.ManipulatedVariables(2).RateMin = -pi/30*Ts; 36 | mpcobj.ManipulatedVariables(2).RateMax = pi/30*Ts; 37 | 38 | %% 39 | % Scale the throttle and steering angle by their respective operating 40 | % ranges. 41 | mpcobj.ManipulatedVariables(1).ScaleFactor = 2; 42 | mpcobj.ManipulatedVariables(2).ScaleFactor = 0.2; 43 | 44 | %% 45 | % Since there are only two manipulated variables, to achieve zero 46 | % steady-state offset, you can choose only two outputs for perfect 47 | % tracking. In this example, choose the Y position and velocity by setting 48 | % the weights of the other two outputs (X and theta) to zero. Doing so lets 49 | % the values of these other outputs float. 50 | mpcobj.Weights.OutputVariables = [0.2 30 0.2 1 0]; 51 | 52 | %% 53 | % Update the controller with the nominal operating condition. For a 54 | % discrete-time plant: 55 | % 56 | % * |U = u0| 57 | % * |X = x0| 58 | % * |Y = Cd*x0 + Dd*u0| 59 | % * |DX = Ad*X0 + Bd*u0 - x0| 60 | % 61 | mpcobj.Model.Nominal = struct('U',U,'Y',Y,'X',X,'DX',DX); 62 | 63 | % %% Specify Mixed I/O Constraints for Obstacle Avoidance Maneuver 64 | % % There are different strategies to make the ego car avoid an obstacle on 65 | % % the road. For example, a real-time path planner can compute a new path 66 | % % after an obstacle is detected and the controller follows this path. 67 | % % 68 | % % In this example, use a different approach that takes advantage of the 69 | % % ability of MPC to handle constraints explicitly. When an obstacle is 70 | % % detected, it defines an area on the road (in terms of constraints) that 71 | % % the ego car must not enter during the prediction horizon. At the next 72 | % % control interval, the area is redefined based on the new positions of 73 | % % the ego car and obstacle until passing is completed. 74 | % % 75 | % % To define the area to avoid, use the following mixed input/output 76 | % % constraints: 77 | % % 78 | % % E*u + F*y <= G 79 | % % 80 | % % where |u| is the manipulated variable vector and |y| is the output 81 | % % variable vector. You can update the constraint matrices |E|, |F|, and |G| 82 | % % when the controller is running. 83 | % 84 | % %% 85 | % % The first constraint is an upper bound on $y$ ($y \le 6$ on this 86 | % % three-lane road). 87 | % E1 = [0 0]; 88 | % F1 = [0 1 0 0]; 89 | % G1 = laneWidth*lanes/2; 90 | % 91 | % %% 92 | % % The second constraint is a lower bound on $y$ ($y \ge -6$ on this 93 | % % three-lane road). 94 | % E2 = [0 0]; 95 | % F2 = [0 -1 0 0]; 96 | % G2 = laneWidth*lanes/2; 97 | % 98 | % %% 99 | % % The third constraint is for obstacle avoidance. Even though no obstacle 100 | % % is detected at the nominal operating condition, you must add a "fake" 101 | % % constraint here because you cannot change the dimensions of the 102 | % % constraint matrices at run time. For the fake constraint, use a 103 | % % constraint with the same form as the second constraint. 104 | % E3 = [0 0]; 105 | % F3 = [0 -1 0 0]; 106 | % G3 = laneWidth*lanes/2; 107 | % 108 | % %% 109 | % % Specify the mixed input/output constraints in the controller using the 110 | % % |setconstraint| function. 111 | % setconstraint(mpcobj,[E1;E2;E3],[F1;F2;F3],[G1;G2;G3],[1;1;0.1]); 112 | 113 | %% Simulate Controller 114 | % In this example, you use an adaptive MPC controller because it handles 115 | % the nonlinear vehicle dynamics more effectively than a traditional MPC 116 | % controller. A traditional MPC controller uses a constant plant model. 117 | % However, adaptive MPC allows you to provide a new plant model at each 118 | % control interval. Because the new model describes the plant dynamics more 119 | % accurately at the new operating condition, an adaptive MPC controller 120 | % performs better than a traditional MPC controller. 121 | % 122 | % Also, to enable the controller to avoid the safe zone surrounding the 123 | % obstacle, you update the third mixed constraint at each control interval. 124 | % Basically, the ego car must be above the line formed from the ego car to 125 | % the upper left corner of the safe zone. For more details, open 126 | % |obstacleComputeCustomConstraint|. 127 | 128 | %% 129 | % Use a constant reference signal. 130 | refSignal = [0 0 0 V 0]; 131 | 132 | %% 133 | % Initialize plant and controller states. 134 | x = x0; 135 | u = u0; 136 | egoStates = mpcstate(mpcobj); 137 | 138 | %% 139 | % The simulation time is |4| seconds. 140 | T = 0:Ts:10; 141 | 142 | %% 143 | % Log simulation data for plotting. 144 | saveSlope = zeros(length(T),1); 145 | saveIntercept = zeros(length(T),1); 146 | ympc = zeros(length(T),size(Cd,1)); 147 | umpc = zeros(length(T),size(Bd,2)); 148 | 149 | %% 150 | % Run the simulation. 151 | for k = 1:length(T) 152 | % Obtain new plant model and output measurements for interval |k|. 153 | [Ad,Bd,Cd,Dd,U,Y,X,DX] = VehicleModelDT_deltaS(Ts,x,u); 154 | measurements = Cd * x + Dd * u; 155 | ympc(k,:) = measurements'; 156 | 157 | % % Determine whether the vehicle sees the obstacle, and update the mixed 158 | % % I/O constraints when obstacle is detected. 159 | % detection = obstacleDetect(x,obstacle,laneWidth); 160 | % [E,F,G,saveSlope(k),saveIntercept(k)] = ... 161 | % obstacleComputeCustomConstraint(x,detection,obstacle,laneWidth,lanes); 162 | 163 | % Prepare new plant model and nominal conditions for adaptive MPC. 164 | newPlant = ss(Ad,Bd,Cd,Dd,'Ts',Ts); 165 | newNominal = struct('U',U,'Y',Y,'X',X,'DX',DX); 166 | 167 | [refSignal, PathRef_x, PathRef_y] = refPath(x, Ts, V, (k-1)*Ts); 168 | refSignal = [refSignal zeros(size(refSignal,1),1)]; 169 | % Prepare new mixed I/O constraints. 170 | options = mpcmoveopt; 171 | % options.CustomConstraint = struct('E',E,'F',F,'G',G); 172 | 173 | % Compute optimal moves using the updated plant, nominal conditions, 174 | % and constraints. 175 | [u,Info] = mpcmoveAdaptive(mpcobj,egoStates,newPlant,newNominal,... 176 | measurements,refSignal); 177 | umpc(k,:) = u'; 178 | 179 | % Update the plant state for the next iteration |k+1|. 180 | x = Ad * x + Bd * u; 181 | end 182 | 183 | mpcverbosity(status); 184 | 185 | %% Analyze Results 186 | figure; 187 | subplot(2,1,1) 188 | hold on 189 | plot(PathRef_x, PathRef_y, 'r'); 190 | plot(ympc(:,1),ympc(:,2),'-k'); 191 | xlabel('x [m]'); 192 | ylabel('y [m]'); 193 | grid on 194 | xlim([min(ympc(:,1)) max(ympc(:,1))]) 195 | 196 | subplot(2,1,2) 197 | plot(T, ympc(:,5)*180/pi) 198 | grid on 199 | xlabel('time [s]') 200 | ylabel('Steering angle [°]') -------------------------------------------------------------------------------- /PathTracking/NLMPC/VehicleTrailerAModelCT.m: -------------------------------------------------------------------------------- 1 | function dxdt = VehicleTrailerAModelCT(x, u) 2 | 3 | carLength = 5; 4 | lv = carLength; 5 | lt = 3; 6 | lc = 0; 7 | 8 | theta = x(3); 9 | theta_t = x(4); 10 | V = -3; 11 | delta = u; 12 | 13 | 14 | dxdt = [V*cos(theta-theta_t)*cos(theta_t); V*cos(theta-theta_t)*sin(theta_t);... 15 | tan(delta)/carLength*V;... 16 | 1/lt*(V*sin(theta-theta_t) - V*lc/lv*tan(delta)*cos(theta-theta_t))]; -------------------------------------------------------------------------------- /PathTracking/NLMPC/VehicleTrailerAModelDT0.m: -------------------------------------------------------------------------------- 1 | function xk1 = VehicleTrailerAModelDT0(xk, uk, Ts) 2 | 3 | M = 10; 4 | delta = Ts/M; 5 | 6 | xk1 = xk; 7 | for ct = 1 : M 8 | xk1 = xk1 + delta*VehicleTrailerAModelCT(xk1, uk); 9 | end 10 | -------------------------------------------------------------------------------- /PathTracking/NLMPC/VehicleTrailerModelOutputFcn.m: -------------------------------------------------------------------------------- 1 | function y = VehicleTrailerModelOutputFcn(x, u, Ts) 2 | y = [x(2);x(4)]; -------------------------------------------------------------------------------- /PathTracking/NLMPC/nlmpcCarTrailerA.m: -------------------------------------------------------------------------------- 1 | if ~mpcchecktoolboxinstalled('optim') 2 | disp('Optimization Toolbox is required to run this example.') 3 | return 4 | end 5 | 6 | 7 | %% Create Nonlinear MPC Controller 8 | nx = 4; 9 | ny = 2; 10 | nu = 1; 11 | nlobj = nlmpc(nx, ny, nu); 12 | 13 | %% 14 | Ts = 0.02; 15 | nlobj.Ts = Ts; 16 | 17 | %% 18 | nlobj.PredictionHorizon = 60; 19 | 20 | %% 21 | nlobj.ControlHorizon = 5; 22 | 23 | %% Specify Nonlinear Plant Model 24 | % The major benefit of nonlinear model predictive control is that it uses a 25 | % nonlinear dynamic model to predict plant behavior in the future across a 26 | % wide range of operating conditions. 27 | nlobj.Model.StateFcn = "VehicleTrailerAModelDT0"; 28 | 29 | %% 30 | % To use a discrete-time model, set the |Model.IsContinuousTime| property 31 | % of the controller to |false|. 32 | nlobj.Model.IsContinuousTime = false; 33 | 34 | %% 35 | 36 | nlobj.Model.NumberOfParameters = 1; 37 | 38 | %% 39 | 40 | nlobj.Model.OutputFcn = 'VehicleTrailerModelOutputFcn'; 41 | 42 | %% 43 | nlobj.Jacobian.OutputFcn = @(x,u,Ts) [0 1 0 0;0 0 0 1]; 44 | 45 | 46 | %% Define Cost and Constraints 47 | nlobj.Weights.OutputVariables = [3 3]; 48 | nlobj.Weights.ManipulatedVariablesRate = 0.01; 49 | 50 | %% 51 | nlobj.OV(2).Min = -45/180*pi; 52 | nlobj.OV(2).Max = 45/180*pi; 53 | 54 | %% 55 | nlobj.MV.Min = -30/180*pi; 56 | nlobj.MV.Max = 30/180*pi; 57 | 58 | %% Validate Nonlinear MPC Controller 59 | x0 = [0 ; 0.3; 0; 15/180*pi]; 60 | u0 = 0; 61 | validateFcns(nlobj,x0,u0,[],{Ts}); 62 | 63 | 64 | x = [0;0.3;0;3/180*pi]; 65 | y = [x(1);x(2)]; 66 | 67 | %% 68 | % |mv| is the optimal control move computed at any control interval. 69 | mv = 0; 70 | 71 | %% 72 | % In the first stage of the simulation, the pendulum swings up from a 73 | % downward equilibrium position to an inverted equilibrium position. The 74 | % state references for this stage are all zero. 75 | yref = [0 0]; 76 | 77 | %% 78 | nloptions = nlmpcmoveopt; 79 | nloptions.Parameters = {Ts}; 80 | 81 | %% 82 | % Run the simulation for |20| seconds. 83 | Duration = 5; 84 | hbar = waitbar(0,'Simulation Progress'); 85 | xHistory = x; 86 | uHistory = [0]; 87 | refSignal = yref; 88 | xk = x; 89 | for ct = 1:(Duration/Ts) 90 | % Set references 91 | % Correct previous prediction using current measurement. 92 | xk = x; 93 | 94 | [mv,nloptions,info] = nlmpcmove(nlobj,xk,mv,refSignal,[],nloptions); 95 | x = VehicleTrailerAModelDT0(x,mv,Ts); 96 | %y = VehicleModelOutputFcn(x, mv, Ts); 97 | xHistory = [xHistory x]; 98 | uHistory = [uHistory mv]; 99 | waitbar(ct*Ts/Duration,hbar); 100 | end 101 | close(hbar) 102 | 103 | %% 104 | % Plot the closed-loop response. 105 | figure 106 | subplot(3,2,1) 107 | plot(0:Ts:Duration,xHistory(1,:)) 108 | xlabel('time [s]') 109 | ylabel('x [m]') 110 | title('cart position x') 111 | grid on 112 | subplot(3,2,2) 113 | plot(0:Ts:Duration,xHistory(2,:)) 114 | xlabel('time [s]') 115 | ylabel('y [m]') 116 | title('cart positon y') 117 | grid on 118 | subplot(3,2,3) 119 | plot(0:Ts:Duration,xHistory(4,:)*180/pi) 120 | hold on 121 | plot(0:Ts:Duration, (0:Ts:Duration)*0) 122 | xlabel('time [s]') 123 | ylabel('theta trailer [°]') 124 | title('trailer head angle') 125 | legend('current', 'desired') 126 | grid on 127 | subplot(3,2,4) 128 | plot(xHistory(1,:), xHistory(2,:)) 129 | hold on 130 | plot(xHistory(1,:),xHistory(1,:)*0,'r') 131 | xlabel('x [m]') 132 | ylabel('y [m]') 133 | title('XY postion') 134 | legend('current', 'desired') 135 | grid on 136 | subplot(3,2,5) 137 | plot(0:Ts:Duration,xHistory(3,:)*180/pi) 138 | xlabel('time [s]') 139 | ylabel('theta [°]') 140 | title('vehicle head angle') 141 | grid on 142 | subplot(3,2,6) 143 | plot(0:Ts:Duration,uHistory(:)*180/pi) 144 | xlabel('time [s]') 145 | ylabel('delta [°]') 146 | title('steering angle') 147 | grid on -------------------------------------------------------------------------------- /PathTracking/pathTracking_CarTrailer.m: -------------------------------------------------------------------------------- 1 | classdef pathTracking_CarTrailer < handle 2 | 3 | % Path Tracking for Car Trailer System 4 | % 5 | % Author: Junyu Zhou 6 | % 7 | % Reference: Reversing the General One-Trailer System: Asymptotic 8 | % Curvature Stabilization and Path Tracking 9 | % 10 | % Copyright (c) 2020, Junyu Zhou 11 | % All rights reserved. 12 | % License : Modified BSD Software License Agreement 13 | 14 | properties 15 | % parameters of car and trailer 16 | CarLength 17 | CarWidth 18 | CarHitch 19 | WheelBase 20 | WheelTrack 21 | TrailerBase 22 | TrailerTrack 23 | 24 | % position and rotation of car and trailer 25 | Xcar 26 | Ycar 27 | Phicar 28 | Phitrailer 29 | Steering 30 | Velociy 31 | 32 | % path tracking parameters 33 | K1 34 | K2 35 | K3 36 | 37 | % % map parameters 38 | % MapDimX 39 | % MapDimY 40 | % StartPoint 41 | % GoalPoint 42 | % Obstacle 43 | % 44 | % % resolution of A* algorithm 45 | % TickXY 46 | % TickAngle 47 | % 48 | % % calculated path 49 | % Path 50 | % SteeringWheel 51 | 52 | end 53 | 54 | properties (Access = private) 55 | AniFig 56 | end 57 | 58 | methods 59 | 60 | function obj = pathTracking_CarTrailer(CarLength, CarWidth, WheelBase, ... 61 | WheelTrack, TrailerBase, TrailerTrack, CarHitch, Xcar,... 62 | Ycar, Phicar, Phitrailer, Steering, Velocity) 63 | if nargin == 13 64 | obj.CarLength = CarLength; 65 | obj.CarWidth = CarWidth; 66 | obj.CarHitch = CarHitch; 67 | obj.WheelBase = WheelBase; 68 | obj.WheelTrack = WheelTrack; 69 | obj.TrailerBase = TrailerBase; 70 | obj.TrailerTrack = TrailerTrack; 71 | obj.Xcar = Xcar; 72 | obj.Ycar = Ycar; 73 | obj.Phicar = Phicar; 74 | obj.Phitrailer = Phitrailer; 75 | obj.Steering = Steering; 76 | obj.Velociy = Velocity; 77 | else 78 | obj.CarLength = 3.5; 79 | obj.CarWidth = 2; 80 | obj.CarHitch = 0.3; 81 | obj.WheelBase = 2.5; 82 | obj.WheelTrack = 1.6; 83 | obj.TrailerBase = 2; 84 | obj.TrailerTrack = 1.5; 85 | obj.Xcar = 2.3; 86 | obj.Ycar = 30; 87 | obj.Phicar = 0; 88 | obj.Phitrailer = 0; 89 | obj.Steering = 0; 90 | obj.Velociy = -1; 91 | end 92 | 93 | obj.K1 = 2; 94 | obj.K2 = 20; 95 | obj.K3 = 0; 96 | obj.AniFig = figure; %[]; 97 | 98 | end 99 | 100 | function testCarTrailer(obj) 101 | dt = 0.1; 102 | for i = 1 : 10 103 | moveStep(obj, dt); 104 | end 105 | end 106 | 107 | function testPathTracking_Circle(obj) 108 | 109 | h = obj.AniFig; 110 | figure(h) 111 | hold on 112 | set(gcf,'position',[200,100,800,750]) 113 | set(gcf,'color','w'); 114 | axis([-30 10 15 35]) 115 | xlabel('[m]') 116 | ylabel('[m]') 117 | obj.drawCar(h, obj.Xcar(end), obj.Ycar(end), obj.Phicar(end), obj.Steering(end)); 118 | obj.drawTrailer(h, obj.Xcar(end), obj.Ycar(end), obj.Phicar(end), obj.Phitrailer(end)); 119 | box on 120 | grid on 121 | r_d = 30; 122 | 123 | error = [0 0]; 124 | SW_Ang = [0 0]; 125 | dt = 0.1; 126 | for i = 1 : 300 127 | r_d = 30; 128 | [xt, yt] = obj.calcTrailerPos(); 129 | r = sqrt(xt^2 + yt^2); 130 | d = r - r_d; 131 | error = [error; [error(end,1)+dt, d]]; 132 | if abs(d) > 0.3 133 | d = sign(d)*0.3; 134 | end 135 | r_d = r - d; 136 | Kp_d = 1/ r_d; 137 | Kp1_d = 0; 138 | phi_d = atan(-xt/yt); 139 | 140 | V =obj.calcV(Kp_d, phi_d, d); 141 | 142 | obj.pathTracking_steering(Kp1_d, V) 143 | if mod(i, 50) == 0 144 | obj.moveStep(dt, true); 145 | else 146 | obj.moveStep(dt, false); 147 | end 148 | plot(-r_d*sin(phi_d), r_d*cos(phi_d), 'ro') 149 | SW_Ang = [SW_Ang; [SW_Ang(end,1)+dt, obj.Steering(end)]]; 150 | 151 | end 152 | obj.drawCar(h, obj.Xcar(end), obj.Ycar(end), obj.Phicar(end), obj.Steering(end)); 153 | obj.drawTrailer(h, obj.Xcar(end), obj.Ycar(end), obj.Phicar(end), obj.Phitrailer(end)); 154 | 155 | error(1,:) = []; 156 | figure; 157 | subplot(2,1,1) 158 | plot(error(:,1), error(:,2)) 159 | xlabel('time [s]') 160 | ylabel('deviation [m]') 161 | box on 162 | grid on 163 | axis([0 error(end,1) min(error(:,2)) max(error(:,2))]) 164 | 165 | subplot(2,1,2) 166 | plot(SW_Ang(:,1), SW_Ang(:,2)*180/pi) 167 | xlabel('time [s]') 168 | ylabel('steering angle [°]') 169 | box on 170 | grid on 171 | set(gcf,'position',[200,100,800,300]) 172 | axis([0 SW_Ang(end,1) min(SW_Ang(:,2)*180/pi) max(SW_Ang(:,2)*180/pi)]) 173 | 174 | end 175 | 176 | function testPathTracking_StraightLine(obj) 177 | h = obj.AniFig; 178 | figure(h) 179 | set(gcf,'position',[200,100,800,150]) 180 | set(gcf,'color','w'); 181 | axis([-40 10 28 32]) 182 | xlabel('[m]') 183 | ylabel('[m]') 184 | hold on 185 | obj.drawCar(h, obj.Xcar(end), obj.Ycar(end), obj.Phicar(end), obj.Steering(end)); 186 | obj.drawTrailer(h, obj.Xcar(end), obj.Ycar(end), obj.Phicar(end), obj.Phitrailer(end)); 187 | box on 188 | grid on 189 | 190 | error = [0 0]; 191 | SW_Ang = [0 0]; 192 | dt = 0.1; 193 | for i = 1 : 300 194 | 195 | [xt, yt] = obj.calcTrailerPos(); 196 | d = yt - 30; 197 | error = [error; [error(end,1)+dt, d]]; 198 | if abs(d) > 0.3 199 | d = sign(d)*0.3; 200 | end 201 | Kp_d = 0; 202 | Kp1_d = 0; 203 | phi_d = 0; 204 | 205 | V =obj.calcV(Kp_d, phi_d, d); 206 | 207 | obj.pathTracking_steering(Kp1_d, V) 208 | if mod(i, 50) == 0 209 | obj.moveStep(dt, true); 210 | else 211 | obj.moveStep(dt, false); 212 | end 213 | plot(xt, 30, 'ro') 214 | SW_Ang = [SW_Ang; [SW_Ang(end,1)+dt, obj.Steering]]; 215 | 216 | end 217 | 218 | obj.drawCar(h, obj.Xcar(end), obj.Ycar(end), obj.Phicar(end), obj.Steering(end)); 219 | obj.drawTrailer(h, obj.Xcar(end), obj.Ycar(end), obj.Phicar(end), obj.Phitrailer(end)); 220 | 221 | error(1,:) = []; 222 | figure; 223 | subplot(2,1,1) 224 | plot(error(:,1), error(:,2)) 225 | xlabel('time [s]') 226 | ylabel('deviation [m]') 227 | box on 228 | grid on 229 | axis([0 error(end,1) min(error(:,2)) max(error(:,2))]) 230 | 231 | subplot(2,1,2) 232 | plot(SW_Ang(:,1), SW_Ang(:,2)*180/pi) 233 | xlabel('time [s]') 234 | ylabel('steering angle [°]') 235 | box on 236 | grid on 237 | set(gcf,'position',[200,100,800,300]) 238 | axis([0 SW_Ang(end,1) min(SW_Ang(:,2)*180/pi) max(SW_Ang(:,2)*180/pi)]) 239 | 240 | end 241 | 242 | % function testPathTracking_SloptraightLine(obj) 243 | % h = obj.AniFig; 244 | % hold on 245 | % obj.drawCar(h, obj.Xcar, obj.Ycar, obj.Phicar, obj.Steering); 246 | % obj.drawTrailer(h, obj.Xcar, obj.Ycar, obj.Phicar, obj.Phitrailer); 247 | % box on 248 | % grid on 249 | % 250 | % SlopAngle = 10/180*pi; 251 | % for i = 1 : 300 252 | % [xt, yt] = obj.calcTrailerPos(); 253 | % yt = yt - 30; 254 | % d = -xt*sin(SlopAngle) + yt*cos(SlopAngle); 255 | % d 256 | % x_coor = xt*cos(SlopAngle) + yt*sin(SlopAngle); 257 | % if abs(d) > 0.3 258 | % d = sign(d)*0.3; 259 | % end 260 | % 261 | % plot(x_coor*cos(SlopAngle), x_coor*sin(SlopAngle)+30, 'ro') 262 | % 263 | % Kp_d = 0; 264 | % Kp1_d = 0; 265 | % phi_d = SlopAngle; 266 | % dt = 0.1; 267 | % 268 | % V =obj.calcV(Kp_d, phi_d, d); 269 | % 270 | % obj.pathTracking_steering(Kp1_d, V) 271 | % if mod(i, 50) == 0 272 | % obj.moveStep(dt, true); 273 | % else 274 | % obj.moveStep(dt, false); 275 | % end 276 | % 277 | % end 278 | % 279 | % obj.drawCar(h, obj.Xcar, obj.Ycar, obj.Phicar, obj.Steering); 280 | % obj.drawTrailer(h, obj.Xcar, obj.Ycar, obj.Phicar, obj.Phitrailer); 281 | % end 282 | 283 | function animationCircle(obj) 284 | ang = linspace(0,pi/2/1.5,100); 285 | r_d = 30; 286 | x = -r_d*sin(ang); 287 | y = r_d*cos(ang); 288 | Path = [x' y']; 289 | 290 | animation(obj, Path, 'CircleTracking'); 291 | 292 | end 293 | end 294 | 295 | 296 | 297 | methods (Access = private) 298 | 299 | function animation(obj, Path, VideoName) 300 | 301 | h = figure; 302 | xmin = min(Path(:,1))-3; 303 | xmax = max(Path(:,1))+5; 304 | ymin = min(Path(:,2))-3; 305 | ymax = max(Path(:,2))+3; 306 | 307 | set(gcf,'position',[200,100,800,400]) 308 | set(gcf,'color','w'); 309 | filename = [VideoName, '.gif']; 310 | 311 | for i = 1 : length(obj.Xcar) 312 | 313 | plot(Path(:,1), Path(:,2), 'r'); 314 | hold on 315 | obj.drawCar(h, obj.Xcar(i), obj.Ycar(i), obj.Phicar(i), obj.Steering(i)); 316 | obj.drawTrailer(h, obj.Xcar(i), obj.Ycar(i), obj.Phicar(i), obj.Phitrailer(i)); 317 | drawnow 318 | hold off 319 | axis([xmin, xmax, ymin, ymax]) 320 | xlabel('[m]') 321 | ylabel('[m]') 322 | box on 323 | grid on 324 | frame = getframe(h); 325 | im = frame2im(frame); 326 | [imind,cm] = rgb2ind(im,256); 327 | if i == 1 328 | imwrite(imind,cm,filename,'gif','DelayTime',0, 'Loopcount',inf); 329 | else 330 | imwrite(imind,cm,filename,'gif','DelayTime',0, 'WriteMode','append'); 331 | end 332 | end 333 | 334 | end 335 | 336 | function pathTracking_steering(obj, Kp1_d, V) 337 | lt = obj.TrailerBase; 338 | lv = obj.WheelBase; 339 | lc = obj.CarHitch; 340 | T = 0.2; 341 | v = obj.Velociy; 342 | theta = obj.Steering(end); 343 | phiv = obj.Phicar(end); 344 | phit = obj.Phitrailer(end); 345 | 346 | u = lt*T*v*cos(theta)*cos(theta)/lv^2/lc*... 347 | ((lv*cos(phiv-phit)+lc*sin(phiv-phit)*tan(theta))^3*... 348 | (Kp1_d + V) - (lv^2 + lc^2*tan(theta)*tan(theta))/lt^2*... 349 | (-lt*tan(theta) + lv*sin(phiv-phit) - lc*cos(phiv-phit)*tan(theta)))+... 350 | theta; 351 | 352 | if abs(u) > 30/180*pi 353 | obj.Steering = [obj.Steering, sign(u) * 30/180*pi]; 354 | else 355 | obj.Steering = [obj.Steering, u]; 356 | end 357 | end 358 | 359 | function h1 = hd1(obj, phi_d) 360 | phit = obj.Phitrailer(end); 361 | h1 = phi_d - phit; 362 | end 363 | 364 | function h2 = hd2(obj, Kp_d) 365 | lt = obj.TrailerBase; 366 | lv = obj.WheelBase; 367 | lc = obj.CarHitch; 368 | theta = obj.Steering(end); 369 | phiv = obj.Phicar(end); 370 | phit = obj.Phitrailer(end); 371 | 372 | h2 = -Kp_d + ... 373 | (lv*sin(phiv-phit)-lv*cos(phiv-phit)*tan(theta))/... 374 | lt/(lv*cos(phiv-phit)+lc*sin(phiv-phit)*tan(theta)); 375 | end 376 | 377 | function V =calcV(obj, Kp_d, phi_d, d) 378 | e1 = d; 379 | e2 = obj.hd1(phi_d); 380 | e3 = obj.hd2(Kp_d); 381 | 382 | V = -obj.K1*e1 - obj.K2*e2 -obj.K3*e3; 383 | end 384 | 385 | function [xt, yt] = calcTrailerPos(obj) 386 | x = obj.Xcar(end); 387 | y = obj.Ycar(end); 388 | phicar = obj.Phicar(end); 389 | phitrailer = obj.Phitrailer(end); 390 | 391 | Trailerbase = obj.TrailerBase; 392 | Hitchdis = obj.CarHitch; 393 | 394 | x = x - Hitchdis*cos(phicar); 395 | y = y - Hitchdis*sin(phicar); 396 | 397 | 398 | xt = x - Trailerbase*cos(phitrailer); 399 | yt = y - Trailerbase*sin(phitrailer); 400 | end 401 | 402 | function drawTrailer(obj, h, x, y, theta_car, theta_trailer) 403 | 404 | carhitchdis = obj.CarHitch; 405 | x = x - carhitchdis*cos(theta_car); 406 | y = y - carhitchdis*sin(theta_car); 407 | 408 | trailerwidth = obj.TrailerTrack; 409 | trailerlength = obj.TrailerBase; 410 | 411 | rear = linspace(-trailerwidth/2, trailerwidth/2, 50); 412 | rear = [-trailerlength*ones(length(rear),1), rear']; 413 | 414 | rod = linspace(0, -trailerlength, 50); 415 | rod = [ rod', 0*ones(length(rod),1),]; 416 | 417 | rod = obj.changePos(x, y, theta_trailer, rod); 418 | rear = obj.changePos(x, y, theta_trailer, rear); 419 | 420 | 421 | wl = linspace(-trailerlength-0.2, -trailerlength+0.2, 20); 422 | wl = [wl', trailerwidth/2*ones(length(wl),1)]; 423 | 424 | wr = linspace(-trailerlength-0.2, -trailerlength+0.2, 20); 425 | wr = [wr', -trailerwidth/2*ones(length(wr),1)]; 426 | 427 | wl = obj.changePos(x, y, theta_trailer, wl); 428 | wr = obj.changePos(x, y, theta_trailer, wr); 429 | 430 | 431 | figure(h) 432 | hold on 433 | plot(rear(:,1), rear(:,2), 'LineWidth',2, 'color', 'b') 434 | plot(rod(:,1), rod(:,2), 'LineWidth',2, 'color', 'b') 435 | plot(wl(:,1), wl(:,2), 'LineWidth',5, 'color', 'b') 436 | plot(wr(:,1), wr(:,2), 'LineWidth',5, 'color', 'b') 437 | end 438 | 439 | function drawCar(obj, h, x, y, theta, steering) 440 | 441 | carwidth = obj.CarWidth; 442 | carlength = obj.CarLength; 443 | wheelbase = obj.WheelBase; 444 | wheeltrack = obj.WheelTrack; 445 | carhitchdis = obj.CarHitch; 446 | 447 | rear = linspace(-carwidth/2, carwidth/2, 50); 448 | rear = [-carhitchdis*ones(length(rear),1), rear']; 449 | 450 | front = linspace(-carwidth/2, carwidth/2, 50); 451 | front = [(carlength-carhitchdis)*ones(length(front),1), front']; 452 | 453 | left = linspace(-carhitchdis, (carlength-carhitchdis), 50); 454 | left = [ left', carwidth/2*ones(length(left),1),]; 455 | 456 | right = linspace(-carhitchdis, (carlength-carhitchdis), 50); 457 | right = [ right', -carwidth/2*ones(length(right),1),]; 458 | 459 | left = obj.changePos(x, y, theta, left); 460 | right = obj.changePos(x, y, theta, right); 461 | front = obj.changePos(x, y, theta, front); 462 | rear = obj.changePos(x, y, theta, rear); 463 | 464 | 465 | wrl = linspace(-0.2, +0.2, 20); 466 | wrl = [wrl', wheeltrack/2*ones(length(wrl),1)]; 467 | 468 | wrr = linspace(-0.2, +0.2, 20); 469 | wrr = [wrr', -wheeltrack/2*ones(length(wrr),1)]; 470 | 471 | wrl = obj.changePos(x, y, theta, wrl); 472 | wrr = obj.changePos(x, y, theta, wrr); 473 | 474 | 475 | wfl = linspace(-0.2, +0.2, 20); 476 | wfl = [wfl', 0*ones(length(wfl),1)]; 477 | %wfl = [wfl', carwidth/2*ones(length(wfl),1)-0.2]; 478 | 479 | wfr = linspace(-0.2,+0.2, 20); 480 | wfr = [wfr', 0*ones(length(wfr),1)]; 481 | %wfr = [wfr', -carwidth/2*ones(length(wfr),1)+0.2]; 482 | 483 | wfl = obj.changePos(wheelbase, wheeltrack/2, steering, wfl); 484 | wfr = obj.changePos(wheelbase, -wheeltrack/2, steering, wfr); 485 | 486 | wfl = obj.changePos(x, y, theta, wfl); 487 | wfr = obj.changePos(x, y, theta, wfr); 488 | 489 | 490 | figure(h) 491 | hold on 492 | plot(rear(:,1), rear(:,2), 'LineWidth',2, 'color', 'k') 493 | plot(front(:,1), front(:,2), 'LineWidth',2, 'color', 'k') 494 | plot(left(:,1), left(:,2), 'LineWidth',2, 'color', 'k') 495 | plot(right(:,1), right(:,2), 'LineWidth',2, 'color', 'k') 496 | 497 | plot(wfl(:,1), wfl(:,2), 'LineWidth',5, 'color', 'r') 498 | plot(wfr(:,1), wfr(:,2), 'LineWidth',5, 'color', 'r') 499 | plot(wrl(:,1), wrl(:,2), 'LineWidth',5, 'color', 'r') 500 | plot(wrr(:,1), wrr(:,2), 'LineWidth',5, 'color', 'r') 501 | end 502 | 503 | function stepAnimation(obj, node, steering) 504 | if isempty(obj.AniFig) 505 | obj.AniFig = figure; 506 | set(gcf,'color','w'); 507 | obstacle = obj.Obstacle; 508 | DimX = obj.MapDimX; 509 | DimY = obj.MapDimY; 510 | hold off; 511 | 512 | if length(obstacle)>=1 513 | plot(obstacle(:,1),obstacle(:,2),'ok');hold on; 514 | end 515 | 516 | axis([0-0.5 DimX+0.5 0-0.5 DimY+0.5]) 517 | xlabel('[m]') 518 | ylabel('[m]') 519 | grid on; 520 | 521 | end 522 | h = obj.AniFig; 523 | 524 | obj.drawCar(h, node(1), node(2), node(3), steering); 525 | obj.drawTrailer(h, node(1), node(2), node(3), node(4)); 526 | end 527 | 528 | function moveStep(obj, dt, DrawCar) 529 | x = obj.Xcar(end); 530 | y = obj.Ycar(end); 531 | phicar = obj.Phicar(end); 532 | phitrailer = obj.Phitrailer(end); 533 | 534 | Wheelbase = obj.WheelBase; 535 | Trailerbase = obj.TrailerBase; 536 | Hitchdis = obj.CarHitch; 537 | 538 | SteeringCar = obj.Steering(end); 539 | Velocity = obj.Velociy; 540 | 541 | Movedis = Velocity*dt; 542 | 543 | if SteeringCar == 0 544 | delta_phi_trailer = phicar - phitrailer; 545 | obj.Xcar= [obj.Xcar, x + Movedis * cos(phicar)]; 546 | obj.Ycar = [obj.Ycar, y + Movedis * sin(phicar)]; 547 | obj.Phicar = [obj.Phicar, obj.Phicar(end)]; 548 | obj.Phitrailer = [obj.Phitrailer, obj.Phitrailer(end) + Movedis/Trailerbase*sin(delta_phi_trailer)]; 549 | 550 | % obj.Xcar= x + Movedis * cos(phicar); 551 | % obj.Ycar = y + Movedis * sin(phicar); 552 | % obj.Phicar = obj.Phicar(end); 553 | % obj.Phitrailer = obj.Phitrailer(end) + Movedis/Trailerbase*sin(delta_phi_trailer); 554 | else 555 | 556 | Rc = Wheelbase / tan(SteeringCar); 557 | angle = Movedis/Rc; 558 | delta_phi_trailer = phicar - phitrailer; 559 | v_car = Movedis; 560 | v1 = v_car*sin(delta_phi_trailer); 561 | v2 = v_car*Hitchdis/Rc*cos(delta_phi_trailer); 562 | delta_phi = (v1-v2)/Trailerbase; 563 | 564 | obj.Xcar= [obj.Xcar, x + Movedis * cos(phicar)]; 565 | obj.Ycar = [obj.Ycar, y + Movedis * sin(phicar)]; 566 | obj.Phicar = [obj.Phicar, obj.Phicar(end) + angle]; 567 | obj.Phitrailer = [obj.Phitrailer, obj.Phitrailer(end)+ delta_phi]; 568 | 569 | % obj.Xcar= x + Movedis * cos(phicar); 570 | % obj.Ycar = y + Movedis * sin(phicar); 571 | % obj.Phicar = obj.Phicar(end) + angle; 572 | % obj.Phitrailer = obj.Phitrailer(end) + delta_phi; 573 | end 574 | 575 | [xt, yt] = obj.calcTrailerPos(); 576 | if ~isempty(obj.AniFig) && DrawCar 577 | h = obj.AniFig; 578 | hold on 579 | obj.drawCar(h, obj.Xcar(end), obj.Ycar(end), obj.Phicar(end), obj.Steering(end)); 580 | obj.drawTrailer(h, obj.Xcar(end), obj.Ycar(end), obj.Phicar(end), obj.Phitrailer(end)); 581 | end 582 | if ~isempty(obj.AniFig) && ~DrawCar 583 | h = obj.AniFig; 584 | hold on 585 | figure(h) 586 | plot(xt, yt, 'b*') 587 | end 588 | end 589 | 590 | function vectorNew = changePos(obj, x, y, theta, vector) 591 | for i = 1 : length(vector) 592 | vectorNew(i,:) = [x+ vector(i,1)* cos(theta)- vector(i,2)*sin(theta), y + vector(i,2)*cos(theta) + vector(i,1)*sin(theta)]; 593 | end 594 | end 595 | end 596 | end -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ADAS_Functions_MATLAB 2 | 3 | ## Matlab codes for ADAS functions 4 | 5 | # Hybrid A* Algorithm for Car Trailer System 6 | 7 | Path optimization is not included and will be added further. 8 | 9 | Similar path planning algorithms for a car pulling trailer are discussed by [Habrador](https://blog.habrador.com/2015/11/explaining-hybrid-star-pathfinding.html) and [Atsushi Sakai](https://github.com/AtsushiSakai/HybridAStarTrailer) 10 | 11 | The difference is that hitch point can be set not only at the midpoint of rear axle but also at the rear point of car body, more general for a car pulling a trailer. 12 | 13 | ## Perpendicular parking 14 | ![2](https://github.com/jingtian123qwe/ADAS_Functions_MATLAB/blob/master/Animation/PathPlanning/PerpendicularParkingTrailer.jpg) 15 | 16 | 17 |

18 | animated 19 |

20 | 21 | ## Parallel parking 22 | ![2](https://github.com/jingtian123qwe/ADAS_Functions_MATLAB/blob/master/Animation/PathPlanning/ParallelParkingTrailer.jpg) 23 | 24 | 25 |

26 | animated 27 |

28 | 29 | Ref: 30 | - [Practical Search Techniques in Path Planning for Autonomous Driving](http://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf) 31 | 32 | - [Application of Hybrid A* to an Autonomous Mobile Robot for 33 | Path Planning in Unstructured Outdoor Environments](https://pdfs.semanticscholar.org/6e00/16024b257040db590d2de352556f64f46787.pdf) 34 | 35 | 36 | # Path Tracking for Car Trailer System 37 | 38 | Here the modus operandi of exact input/output linearization technique is used to calculate the steering wheel angle. 39 | 40 | ## Straight Path Tracking 41 | ![2](https://github.com/jingtian123qwe/ADAS_Functions_MATLAB/blob/master/Animation/PathTracking/Demo_Straight.jpg) 42 | 43 | ![2](https://github.com/jingtian123qwe/ADAS_Functions_MATLAB/blob/master/Animation/PathTracking/Deviation_Straight.jpg) 44 | 45 | 46 | ## Circle Path Tracking 47 | ![2](https://github.com/jingtian123qwe/ADAS_Functions_MATLAB/blob/master/Animation/PathTracking/Demo_Circle.jpg) 48 | 49 | ![2](https://github.com/jingtian123qwe/ADAS_Functions_MATLAB/blob/master/Animation/PathTracking/Deviation_Circle.jpg) 50 | 51 | ![2](https://github.com/jingtian123qwe/ADAS_Functions_MATLAB/blob/master/Animation/PathTracking/CircleTracking.gif) 52 | 53 | Ref: 54 | - Reversing the General One-Trailer System: Asymptotic Curvature Stabilization and Path Tracking 55 | 56 | # Path Tracking For Car By Using Model Predictive Control 57 | 58 | Two different state space equations are applied for path tracking. 59 | 60 | Compared to steering angle, the steering angle rate as controlled signal can make system more stable. It can be found in following figures. 61 | 62 | ## Steering angle as controlled signal 63 | ![2](https://github.com/jingtian123qwe/ADAS_Functions_MATLAB/blob/master/Animation/PathTracking/MPC_Car.jpg) 64 | 65 | ## Steering angle rate als controlled signal 66 | ![2](https://github.com/jingtian123qwe/ADAS_Functions_MATLAB/blob/master/Animation/PathTracking/MPC_Car_DeltaS.jpg) 67 | 68 | Ref: 69 | - [Obstacle Avoidance Using Adaptive Model Predictive Control](https://www.mathworks.com/help/mpc/ug/obstacle-avoidance-using-adaptive-model-predictive-control.html) 70 | 71 | # Path Tracking for Car Trailer System By Using nonlinear Model Predictive Control 72 | 73 | Here nonlinear model predictive control is used to calculate the steering wheel angle. 74 | 75 | ## Straight Path Tracking 76 | ![2](https://github.com/jingtian123qwe/ADAS_Functions_MATLAB/blob/master/Animation/PathTracking/nmpc_trailer_straight_backup.jpg) 77 | 78 | Ref: 79 | 80 | - [Nonlinear model predictive controller](https://www.mathworks.com/help/mpc/ref/nlmpc.html) 81 | 82 | 83 | # License 84 | 85 | MIT 86 | 87 | 88 | # Author 89 | - [Junyu Zhou](https://github.com/jingtian123qwe/) --------------------------------------------------------------------------------