双轮差速机器人运动控制模型推导与CoppeliaSim仿真实现

  博客第一部分 运动学分析建模是参考此博客 两轮差速移动机器人运动分析、建模和控制进行整理扩展，第二部分为根据个人实际应用进行理解整理，如有错误之处，敬请指正。

1. 运动学分析建模

  运动特性为两轮差速驱动，其底部后方两个同构驱动轮的转动为其提供动力，前方的随动轮起支撑作用并不推动其运动，如图两轮差速驱动示意图所示。

  定义其左右驱动轮的中心分别为 $W_l$和$W_r$，且车体坐标系中这两点在惯性坐标系下移动的线速度为$v_l$和$v_r$ ，理想情况下即为左右轮转动时做圆周运动的线速度。该值可以通过电机驱动接口输出的角转速${\varphi }_l$ 、${\varphi }_r$和驱动轮半径$r$求得，即：
$$v_l=r\cdot {\varphi }_l$$
  令两驱动轮中心连线的中点为机器的基点$C$，$C$ 点在大地坐标系$XOY$ 下坐标为 $(x,y)$，机器的瞬时线速度为$v_c$ ，瞬时角速度 ${\omega }_c$ ，姿态角$\theta$ 即为$v_c$
与X 轴夹角。此时，机器位姿信息可用矢量 $P=[x,y,\theta {]}^T$ 表示。机器人瞬时线速度为$v_c$可以表示为：
$$v_c=\frac{v_r+v_l}{2}$$
  令左右轮间距为$l$，且机器瞬时旋转中心（ICR）为$O_c$，转动半径即为$C$到$O_c$的距离$R$。机器在做同轴（轴为左右轮到ICR连线）圆周（圆心为ICR）运动时，左右轮及基点所处位置在该圆周运动中的角速度相同 ${\omega }_l={\omega }_r={\omega }_c$，到旋转中心的半径不同，有 $l=\frac{v_r}{{\omega }_r}-\frac{v_l}{{\omega }_l}$。则机器的瞬时角速度${\omega }_c$可以表示为：
$${\omega }_c=\frac{v_r-v_l}{l}$$
  联立两式，利用$v_r$和$v_l$求出机器转动半径：
$$R=\frac{v_c}{{\omega }_c}=\frac{l}{2}\cdot \frac{v_r+v_l}{v_r-v_l}$$

1.1 三种运动状态分析

  差速驱动方式，即$V_1$和$V_2$间存在的速度差关系决定了其具备不同的三种运动状态，如图：

• 当$v_l>v_r$时，机器做圆弧运动；（准确来说，应该是$|v_l|\ne |v_r|$）
• 当$v_l=v_r$时，机器做直线运动；
• 当$v_l=-v_r$时，机器以左右轮中心点做原地旋转。

1.2 运动学模型

  通过上述两节的运动分析，在驱动轮与地面接触运动为纯滚动无滑动情况下，机器的运动学模型可以表示为：
$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right]=\left[\begin{array}{c}{v}_x\\ v_y\\ \omega \end{array}\right]=\left[\begin{array}{c}v\cdot cos\theta \\ v\cdot sin\theta \\ \omega \end{array}\right]=\left[\begin{array}{c}\frac{v_r+v_l}{2}\cdot cos\theta \\ \frac{v_r+v_l}{2}\cdot sin\theta \\ \frac{v_r-v_l}{L}\end{array}\right]=\left[\begin{array}{cc}\frac{cos\theta }{2}& \frac{cos\theta }{2}\\ \frac{sin\theta }{2}& \frac{sin\theta }{2}\\ \frac{1}{L}& -\frac{1}{L}\end{array}\right]=\left[\begin{array}{cc}cos\theta & 0\\ sin\theta & 0\\ 0& 1\end{array}\right]\left[\begin{array}{cc}\frac{1}{2}& \frac{1}{2}\\ \frac{1}{L}& -\frac{1}{L}\end{array}\right]\left[\begin{array}{c}{v}_r\\ v_l\end{array}\right]$$

2. 双轮差速机器人控制推导

  双轮差速移动机器人直观的控制量是上述建模中所述的左右轮转速，为了更一般的描述机器人的运动以及与ROS（Robot Operation System，机器人操作系统）的对接，控制量选机器人线速度$v_c$与角速度${\omega }_c$，左右轮转速可由模型反求取。

方法一

  根据机器人的线速度 $v_c$ 和角速度 ${\omega }_c$，以及公式 $v=\omega \ast r$ 可计算出机器人的实时转弯半径 $r_c$（即$O_{c}C$）：

$$r_c=\frac{v_c}{w_c}$$

  已知机器人左右轮 $W_1{W}_2$ 距离 $L$，$W_1$ $W_2$ 与 $C$ 同轴，所以左右轮以$O_c$为旋转中心的旋转角速度 ${\omega }_l={\omega }_r={\omega }_c$，可计算两轮线速度：（注意：以各自的轮子中心为旋转中心的旋转速度在此称为转速）
$$\begin{gathered} v_l={\omega }_l\ast r_l={\omega }_c\ast (r_c-\frac{L}{2})=v_c-\frac{L}{2}\ast {\omega }_c \\ {v}_r={\omega }_r\ast r_r={\omega }_c\ast (r_c+\frac{L}{2})=v_c+\frac{L}{2}\ast {\omega }_c \end{gathered}$$
  已知轮子半径是 $r$，计算控制量左右轮转速：
$$\begin{gathered} {\varphi }_l=\frac{v_l}{r}=\frac{1}{r}\ast (v_c-\frac{L}{2}\ast {\omega }_c) \\ {\varphi }_r=\frac{v_r}{r}=\frac{1}{r}\ast (v_c+\frac{L}{2}\ast {\omega }_c) \end{gathered}$$
方法二

  根据第一部分中推导的两个公式直接联立

$$\begin{gathered} v_c=\frac{v_l+v_r}{2} \\ {\omega }_c=\frac{v_r-v_l}{L} \end{gathered}$$

  计算左右轮速度以及转速
$$\begin{gathered} v_l=v_c-\frac{L}{2}\ast {\omega }_c \\ {v}_r=v_c+\frac{L}{2}\ast {\omega }_c \\ {\varphi }_l=\frac{v_l}{r}=\frac{1}{r}\ast (v_c-\frac{L}{2}\ast {\omega }_c) \\ {\varphi }_r=\frac{v_r}{r}=\frac{1}{r}\ast (v_c+\frac{L}{2}\ast {\omega }_c) \end{gathered}$$

  两种方法都是一个结果的，至于计算左右轮速度时是 $v_c-\frac{L}{2}\ast {\omega }_c$ 还 是 $v_c+\frac{L}{2}\ast {\omega }_c$ ，这与机器人坐标系的定义以及角速度矢量的定义有关，逆时针为正，顺时针为负。

3. CoppeliaSim仿真实现

  此部分搭建了基于ROS的CoppeliaSim仿真环境，环境搭建可参考我的博客，在环境中实现了ROS端发布 geometry_msgs/Twist 类型的话题 /cmd_vel，CoppeliaSim端订阅此话题，控制电机转动从而控制机器人的移动。Twist.msg中包括机器人控制量线速度linear.x和角速度angular.z。

#Twist.msg
# This expresses velocity in free space broken into its linear and angular parts.          
Vector3  linear
Vector3  angular

---

# Vector3.msg
# This represents a vector in free space. 
# It is only meant to represent a direction. Therefore, it does not
# make sense to apply a translation to it (e.g., when applying a 
# generic rigid transformation to a Vector3, tf2 will only apply the
# rotation). If you want your data to be translatable too, use the                           
# geometry_msgs/Point message instead.

float64 x
float64 y
float64 z

场景下载链接：controlledViaRos.ttt 提取码: c96d

function sysCall_init()
    leftMotor=sim.getObjectHandle("rosInterfaceControlledBubbleRobLeftMotor") -- Handle of the left motor
    rightMotor=sim.getObjectHandle("rosInterfaceControlledBubbleRobRightMotor") -- Handle of the right motor
    -- Launch the ROS client application:
    if simROS then
        sim.addLog(sim.verbosity_scriptinfos,"ROS interface was found.")
        cmdVelSub  = simROS.subscribe('/cmd_vel','geometry_msgs/Twist','cmd_vel_callback')
    else
        sim.addLog(sim.verbosity_scripterrors,"ROS interface was not found. Cannot run.")
    end
end

function cmd_vel_callback(msg)
    -- This is the sub_velocity callback function
    local L = 0.2  -- wheel_tread
    local r = 0.04  -- wheel_radius
    phi_l = (1.0/r)*(msg.linear.x - L/2.0*msg.angular.z)
    phi_r = (1.0/r)*(msg.linear.x + L/2.0*msg.angular.z)
    sim.setJointTargetVelocity(leftMotor, phi_l)
    sim.setJointTargetVelocity(rightMotor,phi_r)
end

function sysCall_cleanup()
    if simROS then
        simROS.shutdownSubscriber(cmdVelSub)
    end
end

发布话题

rostopic pub /cmd_vel geometry_msgs/Twist "linear:
  x: 0.2
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 1.0" -r 15

效果图：

部分参考 两轮差速移动机器人运动分析、建模和控制