目录
0 前言
本文基于差速轮式机器人模型做一个运动学应用，即控制机器人两轮的速度差改变其运动轨迹，使机器人完成一个倒车入库的动作。
仿真效果动图如下所示，看完本文相信你也可以做到！(文末有实际机器人运行效果图！)
1 什么是差速轮式机器人？
差速轮式机器人是轮式机器人的一种，如下图所示。其特点是：两轮只有绕轴的旋转运动，而没有其他速度分量，因此这种机器人不能横向运动，想象家里的扫地机，路上开的汽车，他们都是差速结构，因此都不能“横着开”！什么样的机器人可以横着开？这个需要装配麦克纳姆轮，以后有机会再说。
2 差速轮式机器人的运动学方程
差速轮式机器人的运动学方程如下：
p ˙ = [ x ˙ y ˙ θ ˙ ] = [ cos θ 0 sin θ 0 0 1 ] [ v ω ] = S ( q ) u \boldsymbol{\dot{p}}=\left[
x˙y˙θ˙
x˙y˙θ˙
\right] =\left[
cosθsinθ0001
cosθ0sinθ001
\right] \left[
vω
vω
\right] =S\left( \boldsymbol{q} \right) \boldsymbol{u}
p
˙
=
⎣
⎡
x
˙
y
˙
θ
˙
⎦
⎤
=
⎣
⎡
cosθ
sinθ
0
0
0
1
⎦
⎤
[
v
ω
]=S(q)u
本文不推导晦涩的公式，这里写出来只是为了编程。
3 开始编程实现
3.1 机器人轨迹控制
首先，我们要确定两个位置，一个是车位位置ref，一个是车的当前位置p
% 车位 xRef = ref(1); yRef = ref(2); thetaRef = ref(3); % 车 x = p(1); y = p(2); theta = p(3);
接着，我们要计算二者的误差
% 误差 xErr = cos(theta) * (xRef - x) + sin(theta) * (yRef - y); yErr = -sin(theta) * (xRef - x) + cos(theta) * (yRef - y); thetaErr = thetaRef - theta;
然后我们期望把误差降低
% 参数 Kpx = 3 Kpt = 3 % 轮子的线速度和角速度 v = Kpx * sqrt(xErr^2 + yErr^2); w = Kpt * thetaErr;
接触过控制理论的同学一定能看出这就是个P反馈控制器。接着把这个增量反馈出去
dpdt = [v*cos(theta); v*sin(theta); w];
然后把这个函数封装成ode45的被调函数，让Matlab帮我们迭代计算即可。
3.2 画车位和车
车位很简单，就是三条直线
annotation('line', [0.53,0.53], [0.35,0.5], 'Color','k'); annotation('line', [0.63,0.63], [0.35,0.5], 'Color','k'); annotation('line', [0.53,0.63], [0.35,0.35], 'Color','k');
车可以按喜好画成各种样子，本文用箭头表示车的方向。
arrow = quiver(x, y, endPt(1) - x, endPt(2) - y, ... 'MaxHeadSize',5.5,'AutoScaleFactor',1,'AutoScale','off', 'LineWidth', 1.5, 'color', color, ... 'Marker', 'o', 'MarkerSize', 4, 'MarkerFaceColor',color);
效果如下
加点难度，把车位斜过来，也表现的很好！
3.3 制作动图
接着看看如何制作Matlab演示动画，下面是完整代码
figure(1) stableProcess = VideoWriter('video/stableProcess.avi'); open(stableProcess); movie = moviein(t); % 画车位 annotation('line', [0.5,0.5], [0.2,0.3], 'Color','k'); annotation('line', [0.63,0.63], [0.2,0.3], 'Color','k'); annotation('line', [0.5,0.63], [0.2,0.2], 'Color','k'); % 画目标位置 plotPose(refPos); grid on hold on % 画初始位置 handler = plotPose(initPos); 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(stableProcess, movie(:, i)); end hold off close(stableProcess);
4 真车实战
因为限制动图大小为5M内，下面的实例抽调了大部分帧。
🔥 更多精彩专栏：
