自动驾驶-预瞄-Pure pursuit纯跟踪算法-MATLAB实现

有空把引入、逻辑、原理介绍给写了，目前先给大家看看代码。
将来写大概会分成这么几块：

• 汽车运动学自行车模型
  
• 跟踪算法主流模型及特点
  
• 纯跟踪算法原理推导
  
• 代码介绍
  

代码原创，来之不易，请勿不注明转载。
喜欢点个赞吧！网上许多代码都跑不起来hh

1. clc;
2. clear;
4. % ------------form road------------
5. cx = 0:0.1:50;
6. cx = cx';
7. for i = 1:length(cx)
8.     cy(i) = sin(cx(i)/5)*cx(i)/2;
9. end
11. %--------form initial state---------
12. L = 2.9  ;% [m] wheel base of vehicle
13. i = 1;
14. target_speed = 2.5;
15. T = 80;
16. x = 0; y = -3; yaw = 0; v = 0;
17. time = 0;
19. %--------how to update--------------
20. lastIndex = length(cx)-1;
21. dt = 0.1  ;% [s]
22. Lfc = 1.0;  % look-ahead distance
23. k = 0.1;  % look forward gain
24. Kp = 1.0 ; % speed propotional gain
25. Lf = k * v + Lfc;
26. %--------show initial state---------
28. target_ind= calc_target_index(x,y,cx,cy,Lf)
32. %--------Update now keep on track-----
33. while T > time && target_ind < lastIndex-10
34.     target_ind= calc_target_index(x,y,cx,cy,Lf)
35.     ai = PIDcontrol(target_speed, v,Kp);
36.     di = pure_pursuit_control(x,y,yaw,v,cx,cy,target_ind,k,Lfc,L,Lf);
37.    
38.     [x,y,yaw,v] = update(x,y,yaw,v, ai, di,dt,L)
39.    
40.     time = time + dt;
41.     pause(0.1)
42.     subplot(1,2,1)
43.     plot(cx,cy,'b',x,y,'r-*')
44.     title(['time=' num2str(time), 'v=' num2str(v),'ind=' num2str(target_ind)])
45.     drawnow
46.     hold on
47.     box off
48. % should also show errors
49. end
51. %-----growing speed-----------------
52. function [a] = PIDcontrol(target_v, current_v, Kp)
53.     a = Kp * (target_v - current_v);
54. end
55. %-----pute pursuit-------------------
56. function [delta] = pure_pursuit_control(x,y,yaw,v,cx,cy,ind,k,Lfc,L,Lf)
57.     tx = cx(ind);
58.     ty = cy(ind);
59.    
60.     alpha = atan((ty-y)/(tx-x))-yaw;
61. %--------should also show lateral error---------
62.     latError =abs((ty-y)*cos(yaw) - (tx-x)*sin(yaw))     
63.     subplot(1,2,2)
64.     plot(x,latError,'b-*')
65.     title(['latError=' num2str(latError)])
66.     drawnow
67.     hold on
68.     box off
69. %------------------------------------------------
70.     Lf = k * v + Lfc;
71.     delta = atan(2*L * sin(alpha)/Lf)  ;
72. end
73. %-----update state-------------------
74. function [x, y, yaw, v] = update(x, y, yaw, v, a, delta,dt,L)
75.     x = x + v * cos(yaw) * dt;
76.     y = y + v * sin(yaw) * dt;
77.     yaw = yaw + v / L * tan(delta) * dt;
78.     v = v + a * dt;
79. end
80. %-----find the nearest point---------
81. function [ind] = calc_target_index(x,y, cx,cy,Lf)
82.     N =  length(cx);
83.     %N =  length(cx)-11;
84.     Distance = zeros(N,1);
85.     for i = 1:N
86.         Distance(i) =  sqrt((cx(i)-x)^2 + (cy(i)-y)^2);
87.     end
88.     [~, location]= min(Distance);
89.     ind = location;
90.     ind = ind + 10;
92. end

复制代码

结果图是实时的，包含一个路径跟踪展示和横向误差图，大概如下（图形并不精美，展示原理所用）：

凑字数凑字数使得可以投稿！不用理会
凑字数凑字数使得可以投稿！不用理会
凑字数凑字数使得可以投稿！不用理会

免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！