目录
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.
🔥 更多精彩专栏：
《机器人原理与技术》
《计算机视觉教程》
《机器学习》
《嵌入式系统》
《数值优化方法》
…
