硬核，机器人只需5秒就跟踪上了爱心曲线
0 前言
硬核，这年头机器人都开始自学“倒车入库”了这篇文章本质上属于机器人的镇定问题。本文仍基于差速轮式机器人模型完成一个轨迹跟踪应用，使机器人在尽可能短的时间内跟踪上爱心轨迹。
仿真效果动图如下所示，看完本文相信你也可以做到！(文末有==实际机器人运行效果图==！)
1 什么是轨迹跟踪？
==轨迹跟踪==以稳定性和快速性为核心，基于时间微分约束(如速度、加速度)，规划路径点间的运动状态 ，轨迹规划赋予路径规划时间信息。简单来说：就是控制机器人用最短的时间跟上期望的轨迹。
和上一期的==镇定==问题做个对比，镇定问题是设计一个控制律在有限时间内使误差衰减到0，当前位姿稳定在参考位姿上。
这两次博文讨论的问题表面上轻松简单，但背后的应用极为广泛，常见于智能车竞赛的==循线==、==平衡==等赛题，学术界方面也有相当的论文研究这两个问题，感兴趣的同学可以继续深入了解，我把相关的参考论文放在文章最后。
2 开始编程实现
2.1 选择轨迹
R = 5.0;
T = 40.0;
w = 2 * pi / T;
ref = @(t) [R*(2*sin(w*t)-sin(2*w*t)), R*(2*cos(w*t)-cos(2*w*t))];
打印出来看看
2.2 编写轨迹跟踪算法
function dpdt = wmrTrackOde45Func(t, p, ref)
获取机器人当前状态
% robot state
x = p(1);
y = p(2);
theta = p(3);
获取参考位姿信息
% reference path
pRef = ref(t);
xRef = pRef(1);
yRef = pRef(2);
vRef = 1;
wRef = 0;
获取目标向量
% predict reference path
targetVec = [xRef - x;
yRef - y];
if targetVec(2) > 0
thetaRef = acos(targetVec(1) / sqrt(targetVec(1)^2 + targetVec(2)^2));
else
thetaRef = - acos(targetVec(1) / sqrt(targetVec(1)^2 + targetVec(2)^2));
end
控制器参数设置
% controller parameters
Kx = 15;
Ky = 25;
Kt = 10;
控制
% controller
ex = cos(theta) * (xRef - x) + sin(theta) * (yRef - y);
ey = -sin(theta) * (xRef - x) + cos(theta) * (yRef - y);
u = [ vRef * cos(thetaRef - theta) + Kx * ex;
wRef + vRef*(Ky * ey + Kt * sin(thetaRef - theta))];
v = u(1);
w = u(2);
dpdt = [v*cos(theta); v*sin(theta); w];
end
然后把这个函数封装成ode45的被调函数，让Matlab帮我们迭代计算即可。
2.3 测试跟踪性能
其中绿色是机器人初始位置，蓝色是机器人实际轨迹。
再来看看误差曲线，大概在5s的时候各参数全部跟踪上预期曲线，这个效果可以通过调节控制器参数来观察变化，加深对控制作用的理解，上文代码中的控制器是我个人整定的，大家可以按需调整。
对其他轨迹的效果怎么样呢？看看下面圆轨迹的跟踪效果，即使初始位置很偏也没有任何问题。
2.4 制作动图
figure(1)
trackProcess = VideoWriter('video/trackProcess.avi');
open(trackProcess);
movie = moviein(t);
% 画参考轨迹
plot(refPos(:,1),refPos(:,2), "--b", 'LineWidth', 1);
grid on
hold on
% 画初始位置
handler = plotPose(initPos, 0.8, 0.3, 'r', size);
for i=1:length(t)
delete(handler);
handler = plotPose(actualPos(i,:));
plot(actualPos(i,1), actualPos(i,2), 'Marker', '.', 'color', [0,0.5,0]);
movie(:, i) = getframe;
writeVideo(trackProcess, movie(:, i));
end
hold off
close(trackProcess);
3 真车实战
由于限制5M，只放出部分动图，后续补充完整视频。
4 参考文献
希望做相关研究的同学可以进一步阅读下面的文献
[1]杨士超. 非完整轮式移动机器人的反馈镇定和轨迹跟踪控制研究[D]. 长安大学.
[2]Kanayama Y , Kimura Y , Miyazaki F , et al. A stable tracking control method for an autonomous mobile robot[C]// IEEE International Conference on Robotics & Automation. IEEE, 1991.
🔥 更多精彩专栏：
🏠 欢迎加入社区和更多志同道合的朋友交流：AI 技术社
- 点赞
- 收藏
- 关注作者
评论（0）