평행주차 알고리즘 matlab

https://kr.mathworks.com/help/mpc/ug/parallel-parking-using-rrt-planner-and-mpc-tracking-controller.html

Parallel Parking Using RRT Planner and MPC Tracking Controller - MATLAB & Simulink - MathWorks 한국

아이디어는 여기서 가지고 왔습니다.

vdims = vehicleDimensions;
egoWheelbase = vdims.Wheelbase;
distToCenter = 0.5*egoWheelbase;

차량 초기갑 입력해줍니다.

egoInitialPose = [7,3.1,0];

[0,0]에 차량 주차하기 위해 초기위치 설정

egoTargetPose = [-distToCenter,0,0];

뒷 바퀴 기준으로 포즈 등록해두기

Tv = 0.1;
helperSLVisualizeParking(egoInitialPose,0);

시뮬레이션은 0.1초 간격!

여기서 각도는 라디안입니다!

xlim = [-10 10];   
ylim = [-2 6];     
yawlim = [-3.1416 3.1416]; 
bounds = [xlim;ylim;yawlim];
stateSpace = stateSpaceReedsShepp(bounds);
stateSpace.MinTurningRadius = 7;

공간을 설정해줍니다.

stateValidator = parkingStateValidator(stateSpace);

충돌 검사기도 만들어줍니다.

planner = plannerRRTStar(stateSpace,stateValidator);
planner.MaxConnectionDistance = 4;
planner.ContinueAfterGoalReached = true;
planner.MaxIterations = 2000;

RRT * planer도 만들어줍니다.

rng(9, 'twister');
[pathObj,solnInfo] = plan(planner,egoInitialPose,egoTargetPose);

목표까지 경로를 계획해줍니다.

f = findobj('Name','Automated Parallel Parking');
ax = gca(f);
hold(ax, 'on');
plot(ax,solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'y.-'); % tree expansion

tree expansion을 그려줍니다.

p = 100;
pathObj.interpolate(p+1);
xRef = pathObj.States;

점을 연결하여 궤적을 생성합니다.

plot(ax,xRef(:,1), xRef(:,2),'b-','LineWidth',2)

경로 작성!

mpcverbosity('off');

 비선형 MPC를 만들기 위해 명령창 메세지를 비활성화 시킵니다.

nlobjTracking = nlmpc(3,3,2);

 컨트롤러 개체를 만듭니다. (state, input, output)

Ts = 0.1;
pTracking = 10;
nlobjTracking.Ts = Ts;
nlobjTracking.PredictionHorizon = pTracking;
nlobjTracking.ControlHorizon = pTracking;

 샘플시간 (TS) 예측범위(predictionHorizon) 및 제어 범위(ControlHorizon)를 지정

nlobjTracking.MV(1).Min = -2;
nlobjTracking.MV(1).Max = 2;
nlobjTracking.MV(2).Min = -pi/6;
nlobjTracking.MV(2).Max = pi/6;

속도(m/s) 및 조향각(rad) 제한조건 대입

nlobjTracking.Weights.OutputVariables = [1,1,3]; 
nlobjTracking.Weights.ManipulatedVariablesRate = [0.1,0.2];

컨트롤러 튜닝 가중치 지정

nlobjTracking.Model.StateFcn = "parkingVehicleStateFcnRRT";
nlobjTracking.Jacobian.StateFcn = "parkingVehicleStateJacobianFcnRRT";

제어기 상태 함수와 쟈코비안 정의

nlobjTracking.Optimization.CustomEqConFcn = "parkingTerminalConFcn";

터미털 제약조건 정의

validateFcns(nlobjTracking,randn(3,1),randn(2,1));

설계 검증

x = egoInitialPose';

시뮬레이션 속도를 높이기 위해 하는 작업 중 초기 차량상태 지정

u = [0;0];

초기 제어 입력 지정

[coredata,onlinedata] = getCodeGenerationData(nlobjTracking,x,u);

코드 생성 데이터를 얻는다.

mexfcn = buildMEX(nlobjTracking,'parkingRRTMex',coredata,onlinedata);

컴트롤러 시뮬레이션하기 위핸 MEX함수 빌드

xTrackHistory = x;
uTrackHistory = u;
mv = u;
Duration = 14;
Tsteps = Duration/Ts;
Xref = [xRef(2:p+1,:);repmat(xRef(end,:),Tsteps-p,1)];

시뮬레이션 하기 전에 데이터 초기화

for ct = 1:Tsteps
    % States
    xk = x;
    % Compute optimal control moves with MEX function
    onlinedata.ref = Xref(ct:min(ct+pTracking-1,Tsteps),:);
    [mv,onlinedata,info] = mexfcn(xk,mv,onlinedata);
    % Implement first optimal control move and update plant states.
    ODEFUN = @(t,xk) parkingVehicleStateFcnRRT(xk,mv);
    [TOUT,YOUT] = ode45(ODEFUN,[0 Ts], xk);
    x = YOUT(end,:)';
    % Save plant states for display.
    xTrackHistory = [xTrackHistory x]; %#ok<*AGROW>
    uTrackHistory = [uTrackHistory mv];
end

시뮬레이션 진행 

plotAndAnimateParkingRRT(p,xRef,xTrackHistory,uTrackHistory);

시각화