两轮差速运动分析、建模和控制

1 运动学分析建模
- 1.1 三种运动状态分析
- 1.2 函数模型
- 1.3 仿真验证
- - 1.3.1 直线验证
  - 1.3.2 曲线验证
  - 1.3.3 旋转验证
2 运动控制
- 2.1 点到点
- - 2.1.1 任意姿态到达目标点
  - 2.1.1 固定姿态到达目标点
- 2.2 路径跟踪
- - 2.2.1 Pure pursuit 纯跟踪

文中每个模型、算法和仿真的程序都将在仿真结果前给出对应的下载链接。

1 运动学分析建模

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

定义其左右驱动轮的中心分别为WlW_lWl和WrW_rWr，且车体坐标系中这两点在惯性坐标系下移动的线速度为vlv_lvl和vrv_rvr，理想情况下即为左右轮转动时做圆周运动的线速度。该值可以通过电机驱动接口输出的角转速ϕl\phi_lϕl，ϕr\phi_rϕr和驱动轮半径rrr求得，即：
vl=r⋅ϕlv_l = r\cdot {\phi_l} vl=r⋅ϕl
令两驱动轮中心连线的中点为机器的基点CCC，CCC点在大地坐标系XOYXOYXOY下坐标为(x,y)(x, y)(x,y)，机器的瞬时线速度为vcv_cvc，瞬时角速度ωc\omega_cωc，姿态角θθθ即为vcv_cvc与XXX轴夹角。此时，机器位姿信息可用矢量P=[x,y,θ]TP = [x, y, θ]^TP=[x,y,θ]T表示。机器人瞬时线速度为vcv_cvc可以表示为：
vc=vr+vl2v_c = \frac{v_r+v_l}{2} vc=2vr+vl
令左右轮间距为lll，且机器瞬时旋转中心（ICR）为OcO_cOc，转动半径即为CCC到OcO_cOc的距离RRR。机器在做同轴（轴为左右轮到ICR连线）圆周（圆心为ICR）运动时，左右轮及基点所处位置在该圆周运动中的角速度相同ωl=ωr=ωc\omega_l=\omega_r=\omega_cωl=ωr=ωc，到旋转中心的半径不同，有l=vrωr−vlωll=\frac{v_r}{\omega_r}-\frac{v_l}{\omega_l}l=ωrvr−ωlvl。则机器的瞬时角速度ωc\omega_cωc可以表示为：
ωc=vr−vll\omega_c = \frac{v_r-v_l}{l} ωc=lvr−vl
联立两式，利用vrv_rvr和vlv_lvl求出机器转动半径：
R=vcωc=l2vr+vlvr−vlR = \frac{v_c}{ω_c}=\frac{l}{2}\frac{v_r+v_l}{v_r-v_l} R=ωcvc=2lvr−vlvr+vl

1.1 三种运动状态分析

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

当vl>vrv_l>v_rvl>vr时，机器做圆弧运动；
当vl=vrv_l=v_rvl=vr时，机器做直线运动；
当vl=−vrv_l=-v_rvl=−vr时，机器以左右轮中心点做原地旋转。

1.2 函数模型

通过上述两节的运动分析，在驱动轮与地面接触运动为纯滚动无滑动情况下，机器的运动学模型可以表示为：
[x˙y˙θ˙]=[cosθ0sinθ001][12121l−1l][vrvl]\left[\begin{matrix} \dot x \\ \dot y \\ \dot θ \\ \end{matrix}\right]= \left[\begin{matrix} cosθ & 0 \\ sinθ & 0 \\ 0 & 1 \\ \end{matrix}\right]\left[\begin{matrix} \frac{1}{2} & \frac{1}{2} \\ \frac{1}{l} & -\frac{1}{l} \\ \end{matrix}\right]\left[\begin{matrix} v_r\\ v_l\\ \end{matrix}\right] ⎣⎡x˙y˙θ˙⎦⎤=⎣⎡cosθsinθ0001⎦⎤[21l121−l1][vrvl]

1.3 仿真验证

MATLAB仿真程序免积分下载链接。

1.3.1 直线验证

位移变化：

姿态角变化曲线：

1.3.2 曲线验证

位移变化：
姿态角变化曲线：
（机器人坐标系服从右手定则，其顺时旋转针姿态角减少，逆时针旋转姿态角增加）

1.3.3 旋转验证

位移变化：
姿态角变化曲线：
（机器人坐标系服从右手定则，其顺时旋转针姿态角减少，逆时针旋转姿态角增加）

2 运动控制

运动控制的被控对象是所分析的两轮差速移动机器人；直观的控制量是上述建模中所述的左右轮转速，为了更一般的描述车体的运动，控制量选车体线速度vvv与角速度ω\omegaω，左右轮转速可由模型反求取。

2.1 点到点

控制机器人由当前点移动到指定目标点，其核心是令机器人在控制器作用下（以一定合适的方式）持续地朝向目标点运动。其控制系统框图为：

2.1.1 任意姿态到达目标点

设机器人实时位姿为[xt,yt,θt][x_t, y_t,\theta_t][xt,yt,θt]，目标位置为[xG,yG,θG][x_G, y_G,\theta_G][xG,yG,θG]，易求出机器与目标点间实时的距离差derr=(xG−xt)2+(yG−yt)2d_{err}=\sqrt{(x_G-x_t)^2+(y_G-y_t)^2}derr=(xG−xt)2+(yG−yt)2与角度差θerr=tan⁡(yG−ytxG−xt)−θt\theta_{err} = \tan(\frac{y_G-y_t}{x_G-x_t})-\theta_tθerr=tan(xG−xtyG−yt)−θt（即图中δ=φ−θ\delta = \varphi-\thetaδ=φ−θ），如下图所示。
一种简单通用的控制器，PID控制器可以构成运动控制器。其设计方法是：简单地，运动控制器由两个并联的PID控制器组成：

一个PID控制器，输入为距离差derrd_{err}derr输出为线速度vvv，即距离决定速度。距离远速度大，距离近速度小。【多说一句，这里要注意的是速度的连续性，（如起步时距离远，线速度由0跳变为较大值，且距离无穷远速度无穷大）所以需要将PID输出先限幅再平滑(即限幅∥vpidout∥≤vmax\left \| v_{pidout} \right \|\leq v_{max}∥vpidout∥≤vmax，平滑∥vt−1−vpidout∥≤σ\left \| v_{t-1}-v_{pidout} \right \|\leq\sigma∥vt−1−vpidout∥≤σ)，最后输出vt=vpidoutv_t=v_{pidout}vt=vpidout】
另一个PID控制器，输入为距离差θerr\theta_{err}θerr输出为角速度ω\omegaω，即偏角误差决定转速。正偏左转，负偏右转；偏多转快，偏少转慢。【多说一句，这里要注意的是角度表示方式带来的过界问题，即当求出的θerr\theta_{err}θerr大于或小于π\piπ（如φ=34π\varphi=\frac{3}{4}\piφ=43π，θ=−34π\theta=-\frac{3}{4}\piθ=−43π）时，要将其归一化到(−π,π](-\pi,\pi](−π,π]。即 θerr\theta_{err}θerr大于π\piπ减2π2\pi2π；小于等于−π-\pi−π，加2π2\pi2π 】

使用上述公式求出线速度vcv_cvc、角速度ωc\omega_cωc后，再解算为左右轮转速交给机器执行。控制器会控制机器人持续朝向目标移动，当其距离目标小于一定值，即移动到以目标为圆心，τ\tauτ为半径（目标半径）的圆内derr≤τd_{err}\leq \tauderr≤τ时，判定机器人到达目标位置，完成运动控制过程。
该控制系统简单易实现，这里是仿真的下载链接，控制效果如图所示。

2.1.1 固定姿态到达目标点

上述点到点的运动控制只要求到达目标点，并没有其他约束条件，虽然在实际应用中往往会有许多约束，但是其确是一切运动控制的基石。
在某些具体应用中，如泊车parking，对接docking等往往需要机器人以某个固定的姿态到达某一具体位置。

2.2 路径跟踪

路径跟踪（巡迹/沿线）是指机器人在已知地图上跟随已规划好路径进行运动的过程。路径跟踪的算法有很多，较为常用的如Pure pursuit，Stanley(针对Ackerman模型)，MPC等等。考虑到通用性，这里主要介绍Pure pursuit控制算法。

2.2.1 Pure pursuit 纯跟踪

路径是由什么组成的？很显然，路径是由无数个离散的目标点组成的。通过遍历这些目标点，是不是就可以完成路径的跟随？那么，这些目标点的遍历过程是不是可以通过上面介绍的点到点运动控制来完成？路径上离散的目标点间距那么小，又怎么实现控制？pure pursuit纯跟踪算法应运而生。
Pure pursuit是最基础的路径跟踪算法了，它的核心思想是：通过航向控制，不断地将机器人从当前位置移动到机器人前面的某个前瞻点。pure pursuit算法将根据机器人的当前位置计算出此时路径上对应的前瞻点，并控制其朝向前瞻点移动，不断循环上述过程（机器动，前瞻动）直到到达路径的最后一个点。通俗一点的说，就是机器人在不断地追逐它前面的一个点。
对于pure pursuit纯跟踪算法来说，前瞻点的选取至关重要！前瞻点是根据机器人实时位置通过前瞻距离向前搜素而来。机器人实时位置和前瞻点如图所示，注意实际路径与规划路径不匹配。

前瞻距离决定了跟踪路径的方式，前瞻距离较小，机器将更快速的靠向规划路径，从而实际路径将更贴合规划路径，但是也会伴随着实际路径的震荡；反之，较大的前瞻距离，可以减少沿路径的振荡，但也会导致拐角附近的曲率较大。同时，不同的线速度和角速度也会影响机器对规划路径的响应，在控制过程中也应该考虑到这一点。
纯追踪算法有几个局限性需要注意：

纯追踪算法不能完全遵循规划路径。必须调整参数以优化性能，使得机器随时间收敛到路径上。
纯追踪算法不能使机器在某一点上保持稳定。可以用目标位置的距离阈值来将其停在目标附近。

算法的整体和细节效果如下，红色虚线为规划轨迹，蓝色实线为实际路径。位置不会一直贴合路径，但是随时间一定会收敛到路径上。

将前瞻距离加大，贴合路径的速度降低收敛变慢：

两轮差速移动机器人运动分析、建模和控制相关推荐

两轮差速移动机器人从A点移动到B点的C++语言代码
(⊙﹏⊙)如下同样是AI撰写. 您可以使用两轮差速移动机器人的控制代码来控制它从A点移动到B点.可以使用各种语言,如C,C++,Python等来编写控制代码. 从A点移动到B点的C代码应该是:move ...
四轮驱动移动机器人(SSMR)与两轮差速驱动机器人、car-like robot的对比分析
关注同名微信公众号"混沌无形",有趣好文! 原文链接:https://mp.weixin.qq.com/s/Fzrpn5_3TB6apqG2Ds1v5Q(包含原文PDF百度云下载链 ...
自动驾驶—两轮差速机器人基于PID控制下轨迹跟踪
机器人轨迹跟踪一般有两种方式,一是基于PID,二是基于MPC模型预测控制方法.本文主要介绍了PID控制,这里我只采用了航向角的控制,对速度没有控制,因为,我认为,小车只要能到达目标点即可,复杂的我后续 ...
matlab差速器建模仿真,matlab 两轮差速模型运动模型公式和运动轨迹模拟
一.两轮差速模型运动模型公式,轨迹模拟 % 参考博客 % https://blog.csdn.net/bufengzj/article/details/103353238?utm_medium=di ...
两轮差速驱动机器人轮间距校准方法
关注同名微信公众号"混沌无形",有趣好文! 原文链接: 差速驱动机器人轮间距校准(包含原文PDF百度云下载链接) 如图 3.2所示,两轮差速驱动机器人是采用自旋(旋转)的方式来校准 ...
一种开环控制的两轮差速驱动小车的设计
内容摘要:为了让电磁屏蔽效能自动测试装置能够在屏蔽室中按照已给定的路径运动,设计了一种开环控制的两轮差速驱动小车,对其运动学进行了分析,并采用双圆弧拟合曲线方法使小车实现按照给定曲线行走.通过大量运动 ...
差分轮式机器人模型matlab,两轮差速机器人运动学分析和控制研究
龙源期刊网 http://www.qikan.com.cn 两轮差速机器人运动学分析和控制研究作者:王维新来源:<现代电子技术>2012年第10期摘要:对两轮差速机器人的运动控制进 ...
两轮差速运动模型推导
由于目前的小车EAI底盘是两轮的(其底部后方两个同构驱动轮的转动为其提供动力,前方的随动轮起支撑作用并不推动其运动),所以记录一下其航迹推算公式两篇博客值得参考: 1．两轮差速运动模型推导过程,ht ...
两轮差速机器人舵机转轴示意图_灵活、快捷的机房巡检机器人少不了伺服电动轮...
背景目前,数据中心机房采用7x24小时专人值守的方式,工作人员需按时巡查机房基础设施,并对基础运行数据进行检查核对分析.这种方式耗时费力,生产效率低,巡检标准化程度也相对较低. 对 ...

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