trucksim纯跟踪控制仿真

实现方法：

原算法链接：https://blog.csdn.net/weixin_43796045/article/details/128723833

• 将算法移植到MatlabFunction中，需注意角度弧度转换
• 频率设定，太低不太行
  
• Trucksim输入接口设定
  
• Trucksim输出接口设定
  

---

实现效果：

下面展示部分代码 内联代码片。

// An highlighted block
k = 0.1;  % look forward gain
Lfc = 1;  % look-ahead distance
Kp = 1.0 ; % speed propotional gain
dt = 0.1  ;% [s]
L = 2.9  ;% [m] wheel base of vehicle
cx = 0:0.1:50;
cx = cx';
for i = 1:length(cx)
cy(i) = sin(cx(i)/5)*cx(i)/2;                                               %曲线函数
% cy(i)=(3.75/(3.^5))*(6*i.^5-15*3*i.^4+10*3^2*i.^3);
% cy(i) = i;
end

i = 1;
target_speed = 3;
T = 80;
lastIndex = length(cx);                                                     %lastIndex=501
x = 0; y = -3; yaw = 0; v = 2;                                               %初始状态
time = 0;
Lf = k * v + Lfc;                                                           %相当于预瞄距离
figure
 
while T > time   
    [target_ind,~]= calc_target_index(x,y,cx,cy,Lf)                         %找到预瞄点
    ai = PIDcontrol(target_speed, v,Kp);                                    %pid控速度
    [~,After_Lf]= calc_target_index(x,y,cx,cy,Lf)  
    di = pure_pursuit_control(x,y,yaw,v,cx,cy,target_ind,k,Lfc,L,After_Lf);
    
    [x,y,yaw,v] = update(x,y,yaw,v, ai, di,dt,L)
    time = time + dt;
    pause(0.05)
    plot(cx,cy,'b',x,y,'r-*')
    drawnow
    hold on
————————————————
版权声明：本文为CSDN博主「weixin_43796045」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/weixin_43796045/article/details/128723833