基于PID控制器和卡尔曼滤波器的机器人控制（Matlab代码实现）

天天程序猿

于 2024-07-22 11:01:47 发布

阅读量823

点赞数 14

文章标签：机器人 matlab 算法

本文链接：https://blog.csdn.net/2301_77414277/article/details/140604349

版权

👨‍🎓个人主页：研学社的博客

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

📚2 运行结果

2.1 Estimated vs Real Position

2.2 Estimated vs Real Velocity

2.3 Acceleration - Controller Output

🎉3 参考文献

🌈4 Matlab代码实现

💥1 概述

目标是使用线性卡尔曼滤波器估计速度，并使用PID控制器控制机器人。

考虑一个在无摩擦、笔直的车道上的机器人。最初，机器人静止在位置 0。我们每 \Delta t 秒测量一次机器人的位置，但这些测量并不精确;我们希望保持机器人位置和速度的模型。

我们在这里展示如何推导创建卡尔曼滤波器的模型。由于 F、H、R 和 Q 是恒定的，因此它们的时间指数被删除。

机器人的位置和速度由线性状态空间描述：位置 ;：速度

我们假设 a（k）是未知的，并且呈正态分布，均值为 0，标准差为 \sigma_a。根据牛顿运动定律，我们得出结论：x（k） = F x（k-1） + G a（k）

我们假设 a（k）$ 项$G没有控制输入，其中 $$F = [1 \quad Δt;0 \quad 1]$$ $$G = [Δt^2/2;Δt]$$

然后，使用PID控制器生成加速度（$a$），以使用估计的加速度（$\tilde{v}$）来控制机器人的速度（v）。

📚2 运行结果

2.1 Estimated vs Real Position

2.2 Estimated vs Real Velocity

2.3 Acceleration - Controller Output

部分代码：

%% Plot Data
maxfig
subplot(2,1,1)
plot(time,x(1,:),'-',time,x_hat(1,:),'--',time, y,'--','Linewidth',2)
xlabel('Time (s)','fontweight','bold')
ylabel('Distance (m)','fontweight','bold')
grid on
legend('True Position','Estimated Position','Measurement','Location','northwest')
title('Estimated vs Real Position', 'FontSize', 14)
axes('position',[.68 .64 .2 .2])
box on
your_index = 58<time & time<65;
plot(time(your_index),x(1,your_index),time(your_index),x_hat(1,your_index),time(your_index),y(your_index),'Linewidth',2)
axis tight

subplot(2,1,2)
plot(time,x(1,:)-x_hat(1,:),time,x_hat(1,:)-y,'Linewidth',1.5)
legend('True Position','Measurement','Location','northwest')
xlabel('Time (s)','fontweight','bold')
ylabel('Distance (m)','fontweight','bold')
grid on
title('Error', 'FontSize', 14)
saveas(gcf,'Position.png')

maxfig
subplot(2,1,1)
plot(time,Velocity,time,x(2,:),time,x_hat(2,:),'Linewidth',2)
xlabel('Time (s)','fontweight','bold')
ylabel('Velocity (m/s)','fontweight','bold')
legend('Velocity Profile','True Velocity','Estimated Velocity','Location','northeast')
grid on
title('Estimated vs Real Velocity ', 'FontSize', 14)
subplot(2,1,2)
plot(time,Velocity-x(2,:),'Linewidth',1.5)
xlabel('Time (s)','fontweight','bold')
ylabel('Velocity (m/s)','fontweight','bold')
grid on
title('Error', 'FontSize', 14)
saveas(gcf,'Velocity.png')

maxfig
plot(time,a,'Linewidth',2)
xlabel('Time (s)','fontweight','bold')
ylabel('Acceleration (m/s^2)','fontweight','bold')
grid on
title('Acceleration - Controller Output', 'FontSize', 14)
saveas(gcf,'Acceleration.png')