硬核，机器人只需5秒就跟踪上了爱心曲线

Posted 2022-01-29 FrigidWinter

---

🔥 作者：FrigidWinter

🔥 简介：主攻机器人与人工智能领域的理论研究和工程应用，业余丰富各种技术栈。主要涉足：【机器人(ROS)】【机器学习】【深度学习】【计算机视觉】

🔥 专栏：

• 《机器人原理与技术》
• 《计算机视觉教程》
• 《机器学习》
• 《嵌入式系统》
• …

---

目录

• 0 前言
• 1 什么是轨迹跟踪？
• 2 开始编程实现
• • 2.1 选择轨迹
  • 2.2 编写轨迹跟踪算法
  • 2.3 测试跟踪性能
  • 2.4 制作动图
• 3 真车实战
• 4 参考文献

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.