728x90
728x90
nlobjTracking = nlmpc(3,3,2);
Ts = 0.1;
pTracking = 30;
nlobjTracking.Ts = Ts;
nlobjTracking.PredictionHorizon = pTracking;
nlobjTracking.ControlHorizon = pTracking;
nlobjTracking.MV(1).Min = -2;
nlobjTracking.MV(1).Max = 2;
nlobjTracking.MV(2).Min = -pi/8;
nlobjTracking.MV(2).Max = pi/8;
nlobjTracking.Weights.ManipulatedVariablesRate = [0.1,0.2];
nlobjTracking.Weights.OutputVariables = [1,1,3];
nlobjTracking.Model.StateFcn = "parkingVehicleStateFcnRRT";
nlobjTracking.Jacobian.StateFcn = "parkingVehicleStateJacobianFcnRRT";
nlobjTracking.Optimization.CustomEqConFcn = "parkingTerminalConFcn";
validateFcns(nlobjTracking,randn(3,1),randn(2,1));
x0 = egoInitialPose;
ref = xRef;
currentPose = updateVehiclePose(modelStatesSub, tftree);
lastMV = [0, 0]; % 속도와 각속도에 대한 초기 제어 입력
while ~isDestinationReached(currentPose, ref(end,:),0.1) % 목적지에 도달할 때까지 루프
% 현재 상태를 GPS 데이터로 업데이트
currentPose = updateVehiclePose(modelStatesSub, tftree);
x0 = [currentPose(1), currentPose(2), currentPose(3)];
% NMPC를 사용하여 속도 및 각속도 계산
[mv, info] = nlmpcmove(nlobjTracking, x0, lastMV, ref);
% 속도 및 각속도 출력
disp(['속도: ', num2str(mv(1)), ', 각속도: ', num2str(mv(2))]);
[pub, msg] = publish_twist_command(mv(1), mv(2), '/cmd_vel');
send(pub, msg);
% 차량 상태 업데이트 (시뮬레이션 또는 실제 차량)
lastMV = mv;
% 다음 반복을 위한 상태 업데이트
%x0 = info.Xopt(2, :);
vehiclePose = updateVehiclePose(modelStatesSub, tftree);
a = vehiclePose';
if mv(1) >= 0
plot(a(1),a(2),'r.', 'MarkerSize', 20);
else
plot(a(1),a(2),'g.', 'MarkerSize', 20);
end
drawnow; % 그래프를 즉시 업데이트
end
일단 이게 어제까지 작성한 코드입니다..
그러나 ref가 문제인지 자꾸 제자리에서 가만히 있으려고 해서 코드를 코치려고 합니다.
일단 nlobjTracking는 파라미터 변경 없이 코드의 전체적인 부분부터 수정을 보겠습니다.
![](https://blog.kakaocdn.net/dn/bk8Ec5/btsEhXjsxi5/403Rbr4VDKz1cy3zVS3JQ0/img.png)
728x90
'자율주행 자동차 > 판단' 카테고리의 다른 글
ROS - TF 란! (0) | 2024.04.07 |
---|---|
nlmpc를 활용한 주차 알고리즘 (0) | 2024.02.05 |
평행주차 matlab - erp 적용 (0) | 2024.01.31 |
평행주차 알고리즘 matlab (43) | 2024.01.31 |