这一章讲移动机器人相关的内容。许久没有更新了，有人私聊我是不是烂尾了。写的这几篇屁文我本来是拿来当自己的笔记本的，不以为有人会认真看。没想到居然还有人追，真是受宠若惊。在这里深表感谢！一直没更新其实是因为年后的工作实在是太忙了，连休息的时间都没有，更何谈写这些博客。另外我还看到之前写的一些内容甚至我自己都没有印象了，看来还得抽时间再回顾一遍。

话不多说，开始吧：

开篇放了一个这样的公式：意为讲固定系s下的角速度、沿xy方向的线速度变换到body系下并用旋量表示。在2维平面中，角速度其实无所谓哪个系，因为尽管是body系，瞬间来说也是固定的，其旋转的角速度瞬间而言是相同的。而线速度，可以看出从s系到b系，是乘了一个二维的旋转矩阵Rbs，而从b系是乘了一个Rsb。

这要注意一下：标准的Rsb，第一列是cos，sin。回顾一下以前的知识：对绕轴旋转得到的旋转矩阵而言，对x，z轴旋转，除了把对应位置填成1，行和列其他位置填成0，剩下的第一列就是填cos，sin。写出了第一列，另一列就自然把sin取反即可。y则相反，头一列是cos，-sin。

轮式机器人类型

主要分为非完整（nonholonomic）类型和全向（omnidirectional）类型。

非完整类型主要受到Pfaffian速度约束，大意就是速度约束无法通过积分来转换成一个等效的位形约束，参见：【现代机器人学】学习笔记一：位形空间_grubler公式_zkk9527的博客-CSDN博客

而全向类型的又分为全向轮和麦克纳姆轮，书上有明确的图，就是一圈轮胎上还挂着横向的轮，全向轮的话横向的轮的旋转轴在车轮平面上，而麦克纳姆轮的旋转轴和轮的平面有一个夹角。

实际上，要实现全向移动，机器人底盘上至少需要有三个轮子，绕起来是一个圆，通过控制三个轮子的转速，才能实现全向功能。（为什么呢？因为我们移动机器人实际上是想生成三维底盘速度，即角速度和xy方向的线速度。我们既然不是非完整约束的轮子，三个自由度的输出，那我们就得需要三个自由度的输入，即三个轮子的转速）而我们图上的轮子：

它这个横向的轮子只是确保能横着转，这样不会对主动轮造成横向摩擦，并不是说上位机可以控制主动轮上面的一个一个小的滚轮主动运动，这点要注意。

和机械臂不同，轮式机器人的运动学，主要考虑车轮速度如何映射为机器人本体速度，而动力学则考虑车轮的扭矩如何映射为机器人的加速度。这章只考虑运动学的内容。实际上按我的工程经验而言，好像对移动机器人而言一般并不是很在意动力学，更重要的是避开障碍物。也许这些内容做自动驾驶的朋友们更关注，但目前我还没有接触过。

全向轮式机器人

建模

这本书很好，好就好在讲内容以前总是从宏观角度先说明，自己打算讲什么东西。让读者带着问题去继续读，总比稀里糊涂读下去好一些。

这里列出了两个关键问题：

1.车轮怎么转，才能使得底盘达到期望的速度？

2.根据车轮转速的极限，怎么得到底盘速度的极限？

好那让我们带着问题继续读下去。

单个全向轮是怎么运动的

在说三个全向轮子组成的机器人的运动之前，我们需要先考虑单个轮子转起来会怎么样。如左一图所示，这个麦克纳姆轮上的被动轮，和主动轮的轴之间存在一个夹角γ，被动轮在转起来的时候，可能会发生一个“滑动”，这是被允许的。假设滑动的方向沿着左图的方向，因此我们可以得到如下的公式：

相信上面的公式大家都可以看明白，就是一个简单的几何关系。但是这个问题中呢，我们是想分清主次。我们事实上是想控制Vx,Vy的，但是我们这里只能控制主动速度Vdrive，因此需要解方程：

滑动速度Vslide是被动轮在转的，我们管不了，我们只能通过控制轮子的驱动角速度，来控制主动速度Vdrive：

因此我们想控制一个轮子的速度vx，vy的话，只要通过上式，控制主动轮的驱动角速度就可以了。

多个全向轮是如何带动底盘运动的

我们假设 $u_i$ 是第i个轮子的驱动角速度， $\dot{q}$ 是期望的底盘速度，那么这里向做的就是计算出各个 $u_i$ ，从而实现 $\dot{q}$ 。

书里二话不说，摆了一个公式。实际上这里摆快了，容易吓到读者。

现在我们不要急，从右到左一个一个看：

这一项是干嘛呢？别忘记我们开头的内容，是把s系下的线速度，通过Rbs，转换到body系下得到一个旋量速度。

这个公式难在第二个变换：

这是个什么玩意呢？看的让人一头雾水。

我们知道，这个东西得到一个3*1的向量，即代表底盘在body系下的旋量。

那左边再乘以这样一个2*3的矩阵，会得到一个2*1的向量，目的是得到车轮在body系（也就是底盘坐标系）中xy方向的线速度。

那这里我不得不放一个图，否则说不清楚：

假如蓝色的v是底盘瞬时的角速度引起的垂直于半径方向的线速度，我们要做的就是把它给拆分，得到xy分量上的速度，然后和原本xy方向的速度叠加在一起，得到

那么这时候乘以的含义就很清楚了。就是得到了车轮在body系下的线速度，去除了角速度的影响。

然后我们把车轮在body系下的速度，变换到车轮坐标系下，我们根据左图，可以看出有一个β的夹角。我们知道如果从A系到B系，如果根据右手坐标系，从A转到B的夹角为θ，那么旋转矩阵RAB为：

$\begin{pmatrix} cos\theta & -sin\theta\\ sin\theta & cos\theta \end{pmatrix}$

在上图中，从body系到车轮系的夹角为β，而我们已经有了body系下的线速度，因此，Rbody_steer 为

$\begin{pmatrix} cos\beta & -sin\beta\\ sin\beta & cos\beta \end{pmatrix}$

而Rsteer_body为：

$\begin{pmatrix} cos\beta & sin\beta\\ -sin\beta & cos\beta \end{pmatrix}$

即中的第三个变换。

这时得到了在车轮系下的线速度，我们要变成控制的驱动角速度，这是套用之前在 单个全向轮是怎么运动的 这部分的公式：，我们采用向量的形式来写，就是：

注意，这个里面它是一个1*2的向量，通过乘以右边的2*1的向量，得到一个1*1的角速度。可不能犯傻理解为这里是 $\frac{1}{r_i} * \frac{tan \gamma}{r_i}$ 啊。

这时候难点攻克下来了，我们这里化简，把H矩阵算出来，这样编码的时候就是一劳永逸，直接就可以从底盘期望速度得到各个车轮的速度了：

这个过程我就不想推了，毕竟不是在研究数学。

然后u就表示为：

这里的u就是各个车轮的转速了，而不只是之前的ui代表第i个车轮的转速。

如果我们想让H中没有角度变量，那就是把

中的前三项合在一起，后两项合在一起：

这个的作用是，对车辆的构型可以起到指导作用，即选择车轮在底盘坐标系的位置，滑动方向，就可以合理选择，使得H(0)的秩为3。

那么到现在来说，实际上就完成了开篇的内容中的第一个重要问题：如何通过底盘的期望速度，计算得到各个车轮的转速。

那么根据上面得到的方法，再配合实际的机器人构型：

我们带入公式就可以得到实际的运动学模型：

那么上面的推导实际上就是授人以渔。

对于图示的四个麦克纳姆轮的全向小车，如果想沿着+x方向运动，所有车轮需要相同速度向前行驶。如果想沿着+y移动，1和3后驱动，2和4前驱动。如果想旋转，就是同侧相同驱动，另一侧反向。这个想想就可以理解。

第二个重要问题：根据车轮转速的极限，怎么得到底盘速度的极限？

我们知道，车轮速度是受到约束的：

那么我们就可以得到两个约束平面，只要在它之间的Vb就可以满足。

多个轮子，就是多个约束平面，构成的多面体，内部空间实际上就意味着在凸多面体内部的运动旋量都是可行的。

运动规划和反馈控制

对全向轮小车而言，好处就在于任何方向都可以直接移动。因此运动规划无需多讲，和前面的内容完全一致，参见：【现代机器人学】学习笔记九：运动规划_虚拟势场法_zkk9527的博客-CSDN博客

而反馈控制，也可以回顾一下：

通过这种方式来跟踪轨迹，其中q为传感器对实际位形的估计。我们在底层使用上面推导得到的公式，就可以把期望速度转化为车轮驱动速度进行控制了。

非完整约束轮式移动机器人

建模

独轮车

独轮车的运动学方程其实蛮好理解，总之就是和其他非完整约束的机器人一样，可以写成的形式。u是控制变量，而qdot则是各个速度。值得注意的是，其中提到了向量场，即把Gq的列单拿出来，第i列的值意味着ui为1的时候，q对应的速度。其实这看看公式就可以理解。书中配了一个向量场的图，这个图其实只是一个示例，并不是说一定代表独轮车，它的意思只是：机器人处于特定位形处的速度是和位形相关的，用向量来做可视化。

差速驱动机器人

这个恐怕是所有做机器人的同学入门的机器人，控制左右两个轮子的转速，然后实现前进或者转向。这个运动学模型也比较固定：

运动学方程通过几何关系就可以推导得到。记住结论或者现用现查就行。想不通的网上搜搜，我这里就不细说了。

车型机器人

这个比较好玩，我就没有操作过车型机器人（自己开车除外）。但是一般自动驾驶，园区配送的车辆，许多是车型的。他们采用阿克曼转向系统，并且不管怎么转，汽车底盘的旋转中心位于穿过后轮的线和前轮垂直平分线的交叉处。

这里我们确定一个参考点，即图里的(x,y）点。我们从世界坐标系下的原点，通过车型机器人的v和w，来推算它在世界系下的瞬时的各个速度，包括xy方向的线速度，以及角度改变的速度,以及本身的小车的转速，这俩角速度对于差速小车是一样的，但是对于车型机器人可不太一样。

运动学方程长上面这样。这时我得写出它为什么是这样了，不然读者会骂街了，上一个都没写，这个也不写的话，岂不是这个笔记变成抄书了？

我们假设小车此时的速度是v，这个v是一个标量速度，正如上式右侧的控制变量v。

那么在世界系下x方向的速度vx=v*cosΦ，同理vy = v*sinΦ，这都没有什么疑问。

那么这时候Φ的变化量，我们观察参考点(x,y)处的角度变化量：

对了，这里或许有朋友对右上角那个图想不通，觉得长短边搞错了。觉得似乎是这样：

为啥不是这样呢？好好想想。

很简单啊！因为小车本身是靠轮子带动的，仔细看上面的图，轮子的方向是沿黑色箭头的速度，所以黑色方向一定是斜边，其他方向都是从黑色方向分出来的，因此实际上是：

而不是

话说画图真麻烦。

到最后推得了一个A(q)*qdot = 0的形式，构成非完整约束：

即不存在g(q)对q的导数为A，使得A(q)*qdot = 0，即A是不可积分的，这个具体可以参考一下中文版21页的内容，回顾一下积分过程，理解它为什么是非完整约束。

非完整移动机器人的规范简化模型

把各种小车的控制极限可视化出来:

可以看出，车型机器人和差分轮的区别是，车型机器人有转弯半径，即当v为0的时候，w也不得不为0。

值得注意的是，对于移动机器人，它也有类似雅可比的概念。

为什么要搞一个P点？这其实也很简单，因为无法保证定位传感器装在底盘中心xy处，所以有一个固定的偏置，作为刚体连接到车上。

我们发现，当xr=0的时候，矩阵第一行为0，即雅可比奇异。这里的雅可比意味着P点在世界坐标系下的运动雅可比，这意味着，当传感器装在底盘的两轮之间（差速轮是两轮之间，车型小车是后轮之间），或者干脆说两轮之间的所有点，都只能在车辆瞬间的前进方向上移动。（但是观察其他点，车有旋转的时候，它的瞬时移动方向并不见得是车的前进方向。

可控性

可控性定义

这一章的内容呢，比较数学，有许多定义和定理。我在这里简单的总结一下：

1.这一节主要讨论非完整机器人，因为全向轮小车想怎么走就怎么走。要么就跟随轨迹，要么就无所谓轨迹，只想通过某种方式到达终点，用一个类似弹簧的K矩阵即可实现。

2.对于控制系统qdot=G(q)u，如果G的秩小于其维度，那么系统无法通过“连续时不变反馈控制律”稳定到q=0。这个听起来很深奥，其实举个例子就明白了，对于差速小车，它的G不是满秩的，那么它也不能横着走，所以q就不能直接以一个K矩阵，通过线性控制让它到达任意点。这原因也很简单，因为非完整小车不能直接横着走。这也就是文中所说的，“对于非完整机器人，没有线性控制器可以使得整个底盘位形稳定到q=0”。不过要注意，这里只是说“没有线性控制器”而已。

3.STLA，STLC，可控的区别：

可控：通俗说，就是"能去"，有一条轨迹可以在有限时间让机器人从q驱动到qgoal。

STLA是 短时局部可及 ，STLC是 短时局部可控。定义看书中的内容，简单的说，就是对于STLA而言，它是“能去”，对于STLC而言，它是“限制邻域也能去”。

这两个容易让人犯糊涂，似乎觉得好像全向小车就是STLC，非完整小车就是STLA。这可不对啊。

STLA,STLC是和“邻域”相关的，也即是说至少有一个邻域。

举个例子，就算你的机器人是非完整机器人，给定一个比机器人大一点点的框，让机器人到达其中的任意区域，难吗？车前进方向的肯定不难，我们主要考虑侧向方向和后退方向。

横着的区域，即文中所说的“平行趴车”，（本文译者发明了这个词，原文为 parking into it），即平行一点一点挪进去。

对于差分小车或者车型小车，虽然说不能直接横着走，但是也可以往前一丢丢，然后转向，后退，再往前一丢丢，再转向，再后退.... 也许需要很长时间，只要有一个邻域，那就能完成这个任务。当然邻域越小，完成这个任务的时间越长，但终究它是能停进去的。

对于不带倒挡的小车，那就不见得可以，如果邻域很小，对于后退方向，以及侧向方向，就不见得在邻域内可以完成。

因此，两种车在自由空间都是可控的，但是不带倒挡的小车，顶多算是STLA。而差分轮和正常小车都是STLC。

可控性检验

1.假如先沿着向量场gi走一段，再沿着向量场gj走一段，或先沿gj走一段，再沿gi走一段，他们最后的位形是相同的，那么两个向量场是“可交换的”。

2.如果向量场不可交换，那么是使用向量场的顺序会影响到最终位形，因此gi和gj之间进行的切换，可以在原先向量场的集合中不存在的方向上产生运动。实际上本文提到的趴车，就是利用向量场计算李括号，通过求解李括号可以解锁原先不能直接有的运动方向。

3.计算非交换性：

注意，其中ε足够小，它代表沿向量场的移动时间。根据上面的公式，发现李括号造成的运动是二阶的，而原始在向量场方向的运动是一阶的，所以沿李括号的运动相对于原始向量场的运动变得缓慢。

4.如上一点所述，定义李括号：

5.定理13.7的总结：按列摆放向量场的运动以及其李括号的运动向量（假设有m列），如果它的维度和本身位形空间的维度相同，并且它的向量的线性组合是m的实数空间，那么它就是局部短时可及的（STLA）。如果向量的正线性组合是m的实数空间，那么它就是局部短时可控的（STLC）。

注意，这里用到了上一章的定义，span为线性组合，pos为正线性组合，具体我们可以参见中文版的294页。

这个定理的用处是可以证明机器人的可控性。可以看下面的13.8-13.10的例题。

运动规划

就运动规划而言，独轮车和差速小车，由于其没有转弯半径，因此从A位姿移动到B位姿，最短路径就是先旋转，再平移到目标位置，最后旋转到目标姿态。即文中所述“这个问题并不有趣”。

但是对车型机器人而言，因为有转弯半径，因此使得这个问题变得更加复杂，但同时也让问题“有趣”起来。

作者在书中列出了各种可能的轨迹类型。另外它特意从图13.17，说明了并非一定是通过旋转-平移-旋转就有更短的行进时间。另外，对于存在障碍物的情形，通过图13.18说明，可以先假设没有障碍物，选取定理13.12中的某一些最短路径，然后当发生碰撞以后，再将路径分为两半，分别进行规划。

反馈控制

机器人移动过程中的控制，一般指的是三种：

1.稳定位形，要求机器人从当前位形到达一个期望的位形并稳定下来。

2.轨迹跟踪，给一个轨迹与时间的函数，要求时间趋于无穷大的时候，可以使误差为0。

3.路径跟踪：只考虑跟踪几何路径，不考虑时间因素。

这章主要考虑轨迹的跟踪。

我们能否只跟踪机器地盘上的一个参考点的运动呢？

如图13.19所示，这样无法保证姿态的正确。

我们可以看到，如果浅色车要跟踪深色车的运动，对于右侧的图示，跟踪的结果是位置正确了，但是姿态相反。所以我们跟踪的方法要连姿态一起跟踪，即“控制律中明确考虑底盘的角度误差”。

由于我们只能控制底盘的前进线速度与角速度，作者在这里不加推理和证明的直接给出了一个可能的控制率：

即“非线性前馈+反馈控制率”

前馈即vd和wd，是我们希望的指令。

而反馈，主要取决于姿态误差Φe以及位置误差xe和ye。我们观察上面的控制律，如果姿态误差和位置误差都是0，那么vd和wd后面减去的那项就是0。即纯粹的前馈控制。

在v的控制器中，可以看到由于存在角度误差，角度误差会减小指令xe，从而赶上期望xe。其余也同理，根据书上的分析即可。当然，当角度误差增大，从90度两边接近90度的时候，分母会趋近于0，控制就崩溃了。这里假设角度误差一直小于90度。

这个控制律只是一种形式，即书中给出了一种可能的方法。实际上也无需太过迷信这种方法，还有其他很多方法可以实现轨迹的跟踪。

里程计测距

这块即前面内容的反过程。在运动学建模部分，即书中332页前后的内容，我们想知道的是控制过程，即如何根据期望的底盘运动，得到轮子的转速。

而测距这块是感知过程，是控制的反过程。是如何根据轮子的转速，得到底盘的运动位姿。

所以只要求出前面运动学建模部分的u=HVb中的H的伪逆（文中表示为F），得到Vb和u的关系，就可以实时根据增量进行积分，进行里程计的测距了。

当然这个过程由于没有观测信息，是纯粹的开环过程，误差会越来越大。一般来说需要结合视觉或激光传感器进行建图定位，从而修正不正确的估计。这块详见我的SLAM学习笔记：https://blog.csdn.net/zkk9527/category_8809309.html?spm=1001.2014.3001.5482

移动操作

这一块是个好玩的内容，也是小车+机械臂的控制水平的分水岭。

简单的控制是分开的，开小车，停稳底盘，然后动机械臂，然后开走。

但是如果能一起使用底盘和机械臂的运动组合，就是很高级了。

文中的方法呢，是构造了一个组合雅可比：

u是底盘速度，θdot是机械臂的关节速度，前面的Jarm是机械臂的雅可比，这个本书一直在讲这个内容。而现在想做的，就是把底盘的移动雅可比也加进来，从而直接通过底盘和机械臂关节，来控制末端运动。

Jbase的求解，书中是这样做的：

这里我有必要解释一下为什么是这样的：

因为旋量表示为[w,v]，其实是[rpyxyz]，rpy为翻滚角，俯仰角，和航向角。平面运动一般可以控制yaw，x，y三个维度，因此F如上式所示，在正中间。

这样底盘的运动可以表示到末端坐标系中：

这样得到了底盘雅可比：

就可以组合成整体的雅可比，

进行连底盘带机械臂整体的运动了。当然这种方式依赖于控制器的实现，不仅对理论，实际上对工程实现的要求也很高。

具体控制上，可以采取运动任务空间的控制方法：

这个具体可以参见中文版的266页，在这里伴随矩阵Ad下标的X-1中的X指的是Xdr，d为期望的位置，r为实际的位置。感兴趣的同学可以回顾：

【现代机器人学】学习笔记十：机器人控制_机器人前馈控制_zkk9527的博客-CSDN博客

然后实际执行的时候，我们可以根据前面得到的组合雅可比的伪逆，从末端速度转换为关节速度，进行控制：

这个内容，实际上很有用。其实目前我工作上就急需利用这种方式实现一些目前比较棘手的问题。但是可惜目前业务需求很多，导致每天都很忙，时间一直不够。

后面应该会补充更多实践内容进来。

截止目前，这本书的所有的章节，我就都介绍一遍了。

但是这绝对不是完结，因为我发现两点：第一，有些内容时间长了，就忘记了。第二，我大多数章节并没有提到工程编码实现相关的内容。

因此我在接下来的时间会抽时间进行回顾，并且继续进行学习笔记的编写：包括：1.勘误。这本书在阅读过程中发现许多印刷、翻译、推导错误，我会整理一章内容，帮助大家。2.工程实现。我会阅读书中自带的python代码，以及结合实际我在日常工作中的C++代码，进行一些整理和更新。3.当然如果有余力，我也会把课后习题的更新一些内容上来。希望大家可以继续关注我的专栏：

https://blog.csdn.net/zkk9527/category_12118173.html?spm=1001.2014.3001.5482

才疏学浅，有错误的地方，感谢各位大佬不吝指正！

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/羊村懒王/article/detail/415863