硬核，这年头机器人都开始自学“倒车入库”了

🔥 作者：FrigidWinter

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

🔥 专栏：

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

目录

0 前言

本文基于差速轮式机器人模型做一个运动学应用，即控制机器人两轮的速度差改变其运动轨迹，使机器人完成一个倒车入库的动作。

仿真效果动图如下所示，看完本文相信你也可以做到！(文末有实际机器人运行效果图！)

1 什么是差速轮式机器人？

差速轮式机器人是轮式机器人的一种，如下图所示。其特点是：两轮只有绕轴的旋转运动，而没有其他速度分量，因此这种机器人不能横向运动，想象家里的扫地机，路上开的汽车，他们都是差速结构，因此都不能“横着开”！什么样的机器人可以横着开？这个需要装配麦克纳姆轮，以后有机会再说。

2 差速轮式机器人的运动学方程

差速轮式机器人的运动学方程如下：

       p
      
      
       ˙
      
     
     
      =
     
     
      
       [
      
      
       
        
         
          
           
            x
           
           
            ˙
           
          
         
        
       
       
        
         
          
           
            y
           
           
            ˙
           
          
         
        
       
       
        
         
          
           
            θ
           
           
            ˙
           
          
         
        
       
      
      
       ]
      
     
     
      =
     
     
      
       [
      
      
       
        
         
          
           
            cos
           
           
            ⁡
           
           
            θ
           
          
         
        
        
         
          
           0
          
         
        
       
       
        
         
          
           
            sin
           
           
            ⁡
           
           
            θ
           
          
         
        
        
         
          
           0
          
         
        
       
       
        
         
          
           0
          
         
        
        
         
          
           1
          
         
        
       
      
      
       ]
      
     
     
      
       [
      
      
       
        
         
          
           v
          
         
        
       
       
        
         
          
           ω
          
         
        
       
      
      
       ]
      
     
     
      =
     
     
      S
     
     
      
       (
      
      
       q
      
      
       )
      
     
     
      u
     
    
    
     \boldsymbol{\dot{p}}=\left[ \begin{array}{c} \dot{x}\\ \dot{y}\\ \dot{\theta}\\\end{array} \right] =\left[ \begin{matrix} \cos \theta& 0\\ \sin \theta& 0\\ 0& 1\\\end{matrix} \right] \left[ \begin{array}{c} v\\ \omega\\\end{array} \right] =S\left( \boldsymbol{q} \right) \boldsymbol{u}
    
   
  p˙​=⎣⎡​x˙y˙​θ˙​⎦⎤​=⎣⎡​cosθsinθ0​001​⎦⎤​[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内，下面的实例抽调了大部分帧。