matlab练习程序(PID路径跟踪)
2022/2/3 17:42:48
本文主要是介绍matlab练习程序(PID路径跟踪),对大家解决编程问题具有一定的参考价值,需要的程序猿们随着小编来一起学习吧!
首先要确定反馈量和控制量,这里反馈量用的是车和最近路径点的横向误差,控制量用的是前轮转角。
算法如下:
1. 根据当前定位结果找到路径最邻近点。
2. 计算该点与定位结果横向误差。
3. 以该误差作为反馈测量值通过pid生成控制量即前轮转角。
4. 将前轮转角转化为航向角,带入运动模型计算出下一时刻的位姿。
我这里用一个正弦函数作为已知路径,PID参数是试出来的,如果换个差距比较大的参数应该就不行了。
matlab代码如下:
clear all;close all;clc; kp = 10; ki = 0.01; kd = 30; v = 1; dt = 0.1; L= 2.5; curp=[0 0 0]; x = 0:0.1:50; y = sin(x/5); path = [x' y']; plot(path(:,1),path(:,2),'r.'); hold on; intergral = 0; pre_err =0; for i=1:length(path) d = path(:,1:2) - curp(1:2); dis = d(:,1).^2 + d(:,2).^2; [~,ind] = min(dis); %找路径最近点索引 dx = curp(1) - path(ind,1); dy = curp(2) - path(ind,2); e = (sin(curp(3) - atan2(dy,dx)))*sqrt(dx*dx+dy*dy); %横向偏差作为测量 intergral = intergral + e; u = kp*e + ki*intergral + kd*(e - pre_err); %pid生成控制量,前轮转角 pre_err = e; curp(1) = curp(1) + dt*v*cos(curp(3)); curp(2) = curp(2) + dt*v*sin(curp(3)); curp(3) = curp(3) + dt*v*tan(u)/L; plot(curp(1),curp(2),'g.'); end %axis equal;
结果如下:
绿色为跟踪路径,红色为已知路径。
这篇关于matlab练习程序(PID路径跟踪)的文章就介绍到这儿,希望我们推荐的文章对大家有所帮助,也希望大家多多支持为之网!
- 2024-07-02springboot项目无法注册到nacos-icode9专业技术文章分享
- 2024-06-26结对编程到底难不难?答案在这里
- 2024-06-19《2023版Java工程师》课程升级公告
- 2024-06-15matplotlib作图不显示3D图,怎么办?
- 2024-06-1503-Loki 日志监控
- 2024-06-1504-让LLM理解知识 -Prompt
- 2024-06-05做软件测试需要懂代码吗?
- 2024-06-0514-ShardingSphere的分布式主键实现
- 2024-06-03为什么以及如何要进行架构设计权衡?
- 2024-05-31全网首发第二弹!软考2024年5月《软件设计师》真题+解析+答案!(11-20题)