基于Mathematica的机器人仿真环境（移动机器人篇）

目的

　　本文作为“基于 Mathematica 的机器人仿真环境”系列的延续，主要讨论地面移动机器人的仿真，具体包括以下内容：
　　1　定义地面环境
　　2　地面力学模型
　　3　移动机器人的动力学模型
　　4　刚性常微分方程求解
　　算法来源于论文$[1]$提供的 MATLAB 代码（点击此处下载）。本文对其做了一些改进，所使用的版本是Mathematica 11.1，部分自定义的函数参考前面的系列文章。进入正文之前不妨先看几个例子：

1　前言

　　要对机器人在不平地形中的运动进行仿真可不是件简单的事。在笔者最早接触这一问题时完全不知如何下手。机器人显然受到地形的约束，不能乱动，但是哪些状态量是可控的呢，哪些是由地形决定的呢？这个问题困惑了我好久。为了降低难度，我首先想到的是一个简单的情况：把一块平板放在凹凸不平的地形中，平板处于什么状态？如果平板稳定下来，那么它应该处于一种重力势能最小的状态。于是笔者认为这是个最优化问题，约束条件就是平板上任意一点的高度大于地形的高度，优化目标是平板的重心最低。想解这个问题非常麻烦，只能求近似解，将平板离散成很多点，每个点都构成一个约束条件。即便能解，这样的结果也是在稳定的条件下，如果平板是运动的，那么平板还要受到惯性力的作用，这样问题就更麻烦了。
　　经过文献检索，我发现原来可以将机器人与地面的作用进行力学建模，然后按照普通的动力学仿真就能得到机器人的运动。但遗憾的是，所得到的动力学方程刚性很强，仿真步长不能太大，否则就会“崩溃”。这样一来，仿真非常浪费时间，笔者估计计算时间是实际运动时间的5倍。
　　虽然动力学仿真可以使用，但是并不是非常实用，只能验证短时间的运动。幸运的是，最近有人提出了新的求解方法，能使大大提高仿真的效率，本文就来看看这个方法怎么样。

2. 定义地面环境

　　在机器人领域，可以大致将机器人分为两类：移动机器人和机械臂。为什么通常要分开研究呢？因为二者的侧重不太一样，机械臂一般是固定的，与环境的交互不是很频繁，而移动机器人与环境的交互要密切得多，要研究移动机器人就离不开环境因素。因此本文开篇就讨论环境——我们关注地面移动机器人，所以更确切地说是地面环境。机器人在工作中可能会遇到各种各样的地形，要想逼真地模拟出所有的地形很难，所以本文主要考虑能够用数学函数表示的地形。
　　

2.1 地形数据

　　如果你已经有地形数据，可以将其导入 Mathematica 中。地形数据可以是离散的二维或三维坐标，也可以是灰度图。最方便的格式是将数据存储为一个矩阵，里面每个元素的值表示地形上一点的高度值。Mathematica 支持的文本格式有 csv、txt 等。其中 csv 格式最方便，它比 xlsx 文件占用空间更小，你可以用 Excel 软件或者记事本打开 csv 格式的文件。将 csv 格式的数据导入 Mathematica 的代码如下：

terrainData = Import["terrain.csv"];

　　将数据显示出来：

ListPlot3D[terrainData, BoxRatios -> {1, 1, 0.2}, Mesh -> None, Filling -> Bottom, FillingStyle -> Brown, ColorFunction -> "Topographic"]

　

　　如果地形数据存储为灰度图（也叫高程图 Heightmap），如下图左所示，你可以用以下代码导入。导入后的显示效果如下图右所示。

pic = Import["terrain.png"];
terrainData = ImageData[pic];

　

　　如果你没有地形数据，那可以利用初等函数来定义地形，例如

surface[x_, y_] := Sin[x] Cos[y];
surfacePts = Table[surface[x, y], {x, -xlim, xlim, dx}, {y, -ylim, ylim, dy}];
Plot3D[surface[x, y], {x, -Pi, Pi}, {y, -Pi, Pi}, ColorFunction -> "Rainbow", Mesh -> None]

　　可是只借助初等函数生成的地形太“规则”了，真实世界中的地形往往都是不规则的随机形状。下面我们生成随机地形数据，代码如下$^{[2]}$：
　　

xlim = ylim = 15;
dx = dy = 0.5;
cl = 0.7; rmsHeight = 15;
gaussian2D[x_, y_] := Exp[(x^2 + y^2)/(-cl^2/2)];
pts = Table[gaussian2D[x, y], {x, -xlim, xlim, dx}, {y, -ylim, ylim, dy}];
randMatrix = rmsHeight*RandomReal[{0, 1}, Dimensions[pts]];
surfacePts = Re@InverseFourier[Fourier[randMatrix]*Fourier[pts]];
ListPlot3D[surfacePts, PlotRange -> All, ColorFunction -> "Rainbow", Mesh -> None, BoxRatios -> {1, 1, 0.05}]

　　结果如下图所示

2.2　地形数据处理

　　前面我们只是得到了地形数据，但是这些数据只是离散的数值，还不能直接用于后面的仿真，因此要从中抽取出有用的信息。
　　对地面的函数的定义目前只用了z轴的高度坐标，为了后续处理我们要加入x、y两个轴的坐标：

XYgrid = Table[{x, y}, {x, -xlim, xlim, dx}, {y, -ylim, ylim, dy}];
surfaceXYZ = MapThread[List, {XYgrid, surfacePts}, 2];

　　前面的定义得到的只是离散的坐标点，即只在几个网格点处给出了定义。但我们需要任意一点处的坐标，方法就是插值。Interpolation函数能得到连续的函数。

surfaceXYZ = Flatten[surfaceXYZ, 1];
surface = Interpolation[surfaceXYZ];

　　插值后的地面：

Plot3D[surface[x, y], {x, -xlim, xlim}, {y, -ylim, ylim}, ColorFunction -> "Topographic", Mesh -> None, Filling -> Bottom, FillingStyle -> Brown]

　　地形的坡度是一个重要的信息，我们可以通过计算地形的梯度得到。梯度的定义是

$$
\begin{aligned}
\triangledown{f}=\left(\frac{\partial{f}}{\partial{x}},\frac{\partial{f}}{\partial{y}}\right)
\end{aligned}
$$

　　计算梯度使用的函数是computeGradient2D，定义代码如下。computeGradient2D函数使用二阶差分的方法近似计算数值梯度。

computeGradient2D[mat_, dx_, dy_] := 
 Module[{D1, D2, gradX, gradY, grad},
  gradX = gradY = ConstantArray[0.0, Dimensions[mat]];
  (*X方向二阶差分*)
  D1 = Append[mat[[2 ;; -1]], mat[[-1]]];
  D2 = Prepend[mat[[1 ;; -2]], mat[[1]]];
  gradX = (D1 - D2)/(2dx);
  gradX[[1]] = (4 mat[[2]] - 3 mat[[1]] - mat[[3]])/(2dx);
  gradX[[-1]] = -(4 mat[[-2]] - 3 mat[[-1]] - mat[[-3]])/(2dx);
  (*Y方向二阶差分*)
  D1 = Join[mat[[;; , 2 ;; -1]], Transpose[{mat[[;; , -1]]}], 2];
  D2 = Join[Transpose[{mat[[;; , 1]]}], mat[[;; , 1 ;; -2]], 2];
  gradY = (D1 - D2)/(2dy);
  gradY[[;; , 1]] = (4 mat[[;; , 2]] - 3 mat[[;; , 1]] - mat[[;; , 3]])/(2dy);
  gradY[[;; , -1]] = -(4 mat[[;; , -2]] - 3 mat[[;; , -1]] - mat[[;; , -3]])/(2dy);
  grad = MapThread[List, {gradX, gradY}, 2](*XY方向合成梯度*)
  ]

　　计算梯度的过程如下：

grad = computeGradient2D[surfacePts, dx, dy];

向量场：一片玉米地（corn field）里每个地方都有玉米，如果把玉米换成向量就是一个向量地，如果用个高大上的名字，一般翻译为向量场（vector field）。

　　我们将梯度显示出来。每个点的梯度是个二维向量，所有点处的梯度向量构成了一个向量场，显示一个（离散的）向量场可以用ListVectorPlot函数，代码如下，显示效果如下图左所示。我们可以用surface[x_, y_] := Sin[x]*Cos[y]函数来检验computeGradient2D函数。梯度向量指向函数值上升的方向，而且是上升最快的方向。梯度向量应该与等值线垂直，我们可以同时显示等值线和梯度向量场，如下图右侧所示。可以看到，它们的确垂直，这验证了computeGradient2D函数的正确性。这幅图很好地展示了用“梯度下降法”求函数的最小值：我们沿着梯度的反方向总能找到函数的局部极小值。

p1 = ListVectorPlot[grad];
p2 = ListContourPlot[Transpose@surfacePts];
Show[p2, p1];

　　有时我们只使用梯度向量的方向，而不关心它的大小，这时可将梯度向量变成单位向量，也就是归一化，所使用的函数是normalizeVectorField，定义如下：

normalizeVectorField[vectors_] := Map[Normalize, vectors, {ArrayDepth[vectors] - 1}]
gradN = normalizeVectorField[grad];

　　既然我们得到了地面的梯度，那么另一个紧密相关的信息——法向量也能很容易地得到了。曲面的法向量定义为：

$$
\begin{aligned}
\textbf{n}=\left(-\frac{\partial{f}}{\partial{x}},-\frac{\partial{f}}{\partial{y}},1\right)
\end{aligned}
$$
　　将梯度向量简单变换一下就能得到法向量场，如下：

normalVectors = grad /. {{x_, y_} -> {-x, -y, 1}};

　　将法向量场和地形一同显示出来，效果如下图左所示：

surface[x_, y_] := Sin[x] Cos[y];
p0 = Plot3D[surface[x, y], {x, -Pi, Pi}, {y, -Pi, Pi}, ColorFunction -> "Rainbow", Mesh -> None];
surfacePts3D = Table[{x, y, surface[x, y]}, {x, -xlim, xlim, dx}, {y, -ylim, ylim, dy}];
normalVectorsDis = MapThread[List, {surfacePts3D, surfacePts3D + normalVectors}, 2];
drawVectors[vectors_] := Module[{}, {Arrowheads[0.015], Arrow[Tube[#, 0.01]]} & /@ vectors];
p3 = Graphics3D[{Red, drawVectors /@ normalVectorsDis}];
Show[p3, p0]

　　同样，我们希望得到任意一点处的法向量，这就需要一个连续函数。我们能对点插值，也就能对向量插值，代码如下。检验插值的效果如上图右所示。

normalVectorsDis = MapThread[List, {surfacePts3D[[;; , ;; , 1 ;; 2]], normalVectors}, 2];
normalVectorsDis = Flatten[normalVectorsDis, 1];
normalVectorFun = Interpolation[normalVectorsDis];

　　既然Flatten函数展开第一层用的很多，我们就将其定义成一个函数：

flatten := Flatten[#, 1]&;

3. 定义机器人

　　上一节定义了环境，接下来我们定义机器人。就像描述一个人需要很多维度（外貌、体重、年龄等等）一样，要描述一个机器人也需要用很多参数，例如它的外形、质量、状态变量等等。

3.1 拓扑结构

　　机械臂很多人都熟悉，它的拓扑结构非常简单，就是由一个个首尾相接的连杆组成的“链条”，被被称为“链形”结构。但是对于移动机器人，其结构就复杂一些了。移动机器人由多个链条组成，含有分支，这被称为“树形”结构。链形结构中，连杆的“父连杆”和“子连杆”都是唯一的；但是在树形结构中，一个连杆的“子连杆”可能不是唯一的。
　　

　　我们有时要指代某个连杆，应该怎么做呢？一个简单的方法是，为每个连杆分配一个唯一的编号，这样我们就能用编号操作对应的连杆了。当然，这个编号不是随便分配的，有些小小的“门道”。在树形结构中，通常将最大最重的那个连杆当做“树根”（本文称为“主体”：mainbody），其编号规定为 $1$，每个“树枝”按照离树根的距离从小到大分配编号值。我们要定义连杆间的连接关系，列表parentIdx中存储着每个连杆的父连杆的编号，每个连杆在列表中的位置同时也是它的编号值（体会到这种编号的巧妙之处了吧）。例如第一个连杆是主体，它的没有父连杆，也可以认为它的父连杆是大地（编号为$0$），第二个连杆的父连杆是主体（主体的编号为$1$）。wheelIdx列表存储的是车轮的编号，我们将车轮也视为一个特殊的连杆。

parentIdx = {0, 1, 1, 3, 2, 2, 4, 4}; (*各连杆的父连杆*)
wheelIdx = {5, 6, 7, 8};

nf = 8; (*坐标系的数量*)
nv = nf - 1 + 6; (*状态向量的维数，主体的姿态6个，其余是关节变量*)
nw = 4; (*车轮数量*)
na = 5; (*驱动关节的数量*)
njc = 0; (*关节约束*)

3.2 几何参数

　　我们以轮式移动机器人为例进行讲解，所以车轮的几何参数必不可少。首先，定义车轮的半径，单位是米：

radius = 0.325; (*车轮半径*)

　　当然，车辆的轮胎不一定完全相等，可以分别定义。本文为了简单，假设所有车轮的尺寸一样，即

radii = ConstantArray[radius, nw];

　　机器人包含一定活动部件，它们之间的运动关系由关节来描述，定义关节的类型。常用的关节只有两种：转动关节（revolute）和移动关节（prismatic）。简单起见，我们只考虑一个关节只有一个自由度，所以根据关节的运动方向来定义。转动轴沿着$x$、$y$、$z$轴，分别用$1~3$的数字表示，移动轴沿着$x$、$y$、$z$轴，分别用$4~6$的数字表示。数字$0$表示该连杆不使用关节。根据关节类型可以定义关节旋量$\xi s$，由函数defineJoint完成。

dofType = {0, 6, 4, 6, 5, 5, 5, 5};(*关节类型*)
jointType[dofType_] := If[1 <= dofType <= 3, "prismatic", "revolute"];
defineJoint = IdentityMatrix[6][[#]] &;
\[Xi]s = defineJoint /@ dofType;

　　我们要提供初始状态下，各连杆的相对位姿。

{bl, bb, bh} = {1.91, 1.64, 0.119};
gParents0 = {PToH[{0, 0, 0}], PToH[{bl/2, 0, 0}], PToH[{-bl/2, 0, 0}], PToH[{0, 0, 0}], PToH[{0, bb/2, -bh}], PToH[{0, -bb/2, -bh}], PToH[{0, bb/2, -bh}], PToH[{0, -bb/2, -bh}]};

3.3 惯性参数

　　动力学仿真离不开惯性参数，一个连杆的惯性可以用三个量完全描述，它们是：
　　质量 $m$ ，它是个标量，$m\in\mathbb{R}$；
　　质心的位置 $p$ ，它是个三维向量，$p\in\mathbb{R^{3}}$；
　　转动惯量 $\mathcal{I}$，它是个$3\times3$矩阵，$\mathcal{I}\in\mathbb{R^{3\times3}}$；
　　在动力学计算时，这三个量被合成为一个量，被称为广义惯性张量，用符号 $\mathcal{M}$ 表示。函数toSpatialInertia的功能就是组装广义惯性张量。
　　我们要为每个连杆设置这三个惯性参数，它们存储在inertiaParams列表中。假设主体的质量是mb，轮子的质量是mw，其它连杆的质量可以类似定义。

m = 1980; (*机器人的总质量，单位是千克*)
{mb, mw} = {0.9*m, 0.1*m/nw};
inertiaParams = {{mb, {0, 0, 0.5/2}, Id[3]}, {0, {0, 0, 0}, 0 Id[3]}, {0, {0, 0, 0}, 0 Id[3]}, {0, {0, 0, 0}, 0 Id[3]}, {mw, {0, 0, 0}, Id[3]}, {mw, {0, 0, 0}, Id[3]}, {mw, {0, 0, 0}, Id[3]}, {mw, {0, 0, 0}, Id[3]}};
\[ScriptCapitalM]s = toSpatialInertia @@@ inertiaParams;

toSpatialInertia[m_, p_, \[ScriptCapitalI]_] := Module[{sp},
  sp = skew[p];
  ArrayFlatten[{{m*IdentityMatrix[3], m*Transpose[sp]}, {m*sp, \[ScriptCapitalI] - m*sp.sp}}]]

　　函数toSpatialInertia是怎么来的呢？为了加深印象，我们可以推导一下。

$$
g_{bc}=\left[
\begin{matrix}
R & p\
0 & 1\
\end{matrix}
\right]\tag{1}
$$

$$
\mathcal{M}=\left[
\begin{matrix}
m & 0\
0 & \mathcal{I}\
\end{matrix}
\right]\tag{2}
$$

$$
T=\frac{1}{2}V^{T}\mathcal{M}V\tag{3}
$$
　　从代码封装和安全的角度考虑，全局变量越少越好。Matlab 支持面向对象的编程方式，文$[1]$提供的代码就利用了面向对象的技术，通过传递实例来避免使用全局变量。但在 Mathematica 中使用面向对象不是很方便，如果都采用局部变量会使函数的输入参数过多，写起来太繁琐，所以本文只好使用全局变量。其缺点是，一次只能仿真一个机器人。如果想同时仿真几个机器人，那么最好不用全局变量，否则会乱。
　　全局变量可以分为以下几种：
　　①　首先，一些不随仿真改变的量可以定义成全局变量，例如重力加速度 gravityAcc、时间步长dt；
　　②　环境一般也是固定不变的，因此也可以定义为全局变量，例如地形函数surface[x, y]；
　　③　机器人的参数较多，只好也定义成全局变量；

4. 地面力学模型

　　地面移动机器人能够移动是因为与地面有力的作用。这个力是什么样的呢？这就是地面力学（Terramechanics）研究的内容。力学是最古老的学科之一，而地面力学却是一个非常年轻的分支。具体来说，地面力学研究车轮或者履带与各种地面的相互作用。对这个学科作出较大贡献的学者有 Bekker 和 Wong，二人的著作$^{[3,4]}$被广泛引用。

4.1 最简单的模型——库伦摩擦模型

4.2 离散化

　　将车轮简化为一个圆柱，并为每个车轮分配一个坐标系。车轮坐标系的原点 $O_w$ 位于圆柱的几何中心，其 $y$ 轴与圆柱的底面垂直，同时也与车轮的转动轴共线，而 $z$ 轴的指向与水平地面垂直向上，$x$ 轴通过右手法则确定，其指向车辆的前进方向。

　

　　车轮上的接触点 $c$ 的坐标是$(x_c,y_c,z_c)$，那么地面曲面函数在 $(x_c,y_c)$ 处的高度就是 $f(x_c,y_c)$，对应 $s$ 点，所以 $s$ 点与 $c$ 点的高度差就是：
$$
\begin{aligned}
h=f(x_c,y_c)-z_c
\end{aligned}
$$　　$(x_c,y_c)$ 处的法向量是 $\textbf{n}$，定义陷入深度$\Delta z$为$^{[5]}$
$$
\begin{aligned}
\Delta z=(0,0,h)\centerdot\textbf{n}
\end{aligned}
$$

　 　　

4.3 神奇公式

5　移动机器人的动力学模型

5.1 状态表达

　　大家都知道，一个刚体在三维空间中有6个自由度——三个移动自由度、三个转动自由度。移动构成一个欧式空间，所以可以用一个三维向量来表示；但是转动不再是欧式空间，如果也用三维向量来表示会有一些问题。以最常用的欧拉角为例，用三个欧拉角描述转动导致空间存在“奇异点”——转动空间中有些点对应不止一组欧拉角。举个形象的例子：我们的地球表面如果用经纬度来描述就存在“奇异点”，北极的纬度是90度，但是经度不能确定，经度可以是任意值，造成这个现象的原因是地球球面与平面在拓扑上是不等价的。为了避免“奇异点”，就要付出额外的代价，例如增加一个坐标，四元数就是一种最常用的四坐标表示。但是四元数的表示法不方便动力学方程的推导，而且不是很直观。所以本文使用一个三维旋转矩阵来表示一个转动姿态，也就是$3\times3=9$ 个坐标，而用一个 $4\times4$ 齐次变换矩阵来表示一个刚体的位姿。

　　其中，是主体的位姿矩阵，$q$是关节值。

StateToHT[S_] := 
 Module[{k, qi, idofType, r, p, R, gParents, gILs, axis},
  gParents = gParents0;
  gILs = ConstantArray[0, {nf, 4, 4}];(*nf个4X4齐次变换矩阵*)
  gILs[[1]] = gParents[[1]] = S[[1]];
  Do[qi = S[[i]];
   idofType = dofType[[i]];
   If[WhichType[idofType] == "revolute",
    axis = IdentityMatrix[3][[idofType - 3]];
    R = RotationMatrix[qi, axis];
    gParents[[i, 1 ;; 3, 1 ;; 3]] = HToR[gParents0[[i]]].R;
    ,
    r = gParents0[[i]][[idofType, 1 ;; 3]];
    p = HToP[gParents0[[i]]];
    gParents[[i, 1 ;; 3, 4]] = p + Transpose[r]*qi]
   , {i, 2, nf}];
  Do[k = parentIdx[[i]]; 
   gILs[[i]] = gILs[[k]].gParents[[i]], {i, 2, nf}];
  {gParents, gILs}]

catenate[g_, q_] := Join[{g}, q]

StateIntegration[S_, V_, dt_] := Module[{gIb, q, Vb, dq},
  {gIb, q} = {First[S], Rest[S]};
  Vb = V[[1 ;; 6]];
  dq = Drop[V, 6];
  gIb = gIb.MatrixExp[hat[Vb*dt]];
  q = q + dq*dt;
  catenate[gIb, q]
  ]

　　5.2 惯性矩阵与惯性力

subtreeInertias[Xp_, \[ScriptCapitalM]s_] := 
 Module[{\[ScriptCapitalM]ssubtree, j},
  \[ScriptCapitalM]ssubtree = \[ScriptCapitalM]s;
  Do[j = parentIdx[[i]];
   \[ScriptCapitalM]ssubtree[[j]] = \[ScriptCapitalM]ssubtree[[j]] + Transpose[Xp[[i]]].\[ScriptCapitalM]ssubtree[[i]].Xp[[i]];
   , {i, nf, 2, -1}];
  \[ScriptCapitalM]ssubtree]

jointSpaceInertia[Xp_, \[ScriptCapitalM]c_] := 
 Module[{M, j, vj, vi, F},
  M = ConstantArray[0, {nv, nv}];
  M[[1 ;; 6, 1 ;; 6]] = \[ScriptCapitalM]c[[1]];
  Do[vi = i + 5;
   F = \[ScriptCapitalM]c[[i]].\[Xi]s[[i]];
   M[[vi, vi]] = \[Xi]s[[i]].F;
   j = i;
   While[j > 1,
    F = Transpose[Xp[[j]]].F;
    j = parentIdx[[j]];
    If[j == 1,
     vj = Range[6];
     M[[vj, vi]] = F;
     ,
     vj = j + 5;
     M[[vj, vi]] = F.\[Xi]s[[j]];
     ];
    M[[vi, vj]] = M[[vj, vi]];
    ]
   , {i, 2, nf}];
  M = (M + Transpose[M])/2;
  M
  ]

jointSpaceBiasForce[\[ScriptCapitalM]s_, Xp_, V_] := 
 Module[{v, c, pi, avp, fvp, vJ, agrav},
  c = ConstantArray[0, nv];
  v = avp = fvp = ConstantArray[0, {nf, 6}];
  agrav = Xp[[1]].{0, 0, -gravityAcc, 0, 0, 0};
  v[[1]] = V[[1 ;; 6]];
  avp[[1]] = -agrav;
  Do[
   If[i > 1,
    pi = parentIdx[[i]];
    vJ = \[Xi]s[[i]]*V[[i + 5]];
    v[[i]] = Xp[[i]].v[[pi]] + vJ;
    avp[[i]] = Xp[[i]].avp[[pi]] + ad[v[[i]]].vJ];
   fvp[[i]] = \[ScriptCapitalM]s[[i]].avp[[i]] - Transpose[ad[v[[i]]]].\[ScriptCapitalM]s[[i]].v[[i]];
   , {i, nf}];
  Do[
   c[[i + 5]] = fvp[[i]].\[Xi]s[[i]];
   pi = parentIdx[[i]];
   fvp[[pi]] = fvp[[pi]] + Transpose[Xp[[i]]].fvp[[i]];
   , {i, nf, 2, -1}];
  c[[1 ;; 6]] = fvp[[1]];
  c
  ]
MandC[gParents_, \[ScriptCapitalM]s_, V_] := 
 Module[{Xp, \[ScriptCapitalM]c, M, c},
  Xp = Ad /@ (Inverse /@ gParents);
  \[ScriptCapitalM]c = subtreeInertias[Xp, \[ScriptCapitalM]s];
  M = jointSpaceInertia[Xp, \[ScriptCapitalM]c];
  c = jointSpaceBiasForce[\[ScriptCapitalM]s, Xp, V];
  {M, c}]

5.3 运动学

wheelJacobians[gILs_, gIContact_] := 
 Module[{axis, r, i = 0, fi, vi, j, Apt, A, cis, gfi, ncc},
  ncc = 3 Length[inContact];
  A = ConstantArray[0, {ncc, nv}];
  Do[Apt = ConstantArray[0, {3, nv}];
   fi = wheelIdx[[wno]];
   While[fi > 1,
    vi = fi + 5;(*index in V*)
    gfi = gILs[[fi]];
    If[jointType[dofType[[fi]]] == "revolute",
     axis = gfi[[1 ;; 3, dofType[[fi]] - 3]];
     r = HToP[gIContact[[wno]]] - HToP[gfi];
     Apt[[;; , vi]] = Cross[axis, r];
     ,
     axis = gfi[[1 ;; 3, dofType[[fi]]]];
     Apt[[;; , vi]] = axis];
    fi = parentIdx[[fi]];
    ];
   r = HToP[gIContact[[wno]]] - HToP[gILs[[1]]];
   Apt[[;; , 1 ;; 3]] = HToR[gILs[[1]]];
   Apt[[;; , 4 ;; 6]] = 
    Transpose[skew[r]].HToR[gILs[[1]]];(*注意：此处容易出错*)
   Apt = Transpose[HToR[gIContact[[wno]]]].Apt;(*注意：此处容易出错*)
   j = 3 i + Range[3];
   A[[j, ;;]] = Apt;
   i++, {wno, inContact}];
  A]

constraintJacobians[gILs_, gIContact_, visAct_] := 
 Module[{ncc, nc, Acontact, A, Aact, cisContact, cisAct, cisJoint, cis, ncon},
  ncc = 3 Length[inContact];(*接触约束的数量，每个车轮对应三个约束方程，因此乘3*)
  nc = ncc + na + njc;(*总约束的数量*)
  Aact = IdentityMatrix[nv];
  Aact = Aact[[visAct]];
  Acontact = wheelJacobians[gILs, gIContact];
  A = ConstantArray[0, {nc, nv}];(*A.dV=b*)
  cisContact = {1, ncc};
  cisAct = {}(*{ncc+1,ncc+na}*);
  cisJoint = {};
  cis = {cisContact, cisAct, cisJoint};
  If[ncc > 0, A[[1 ;; ncc]] = Acontact];
  If[na > 0, A[[ncc + Range[na]]] = Aact];
  ncon = {ncc, na, njc};
  {A, cis, ncon}]

6. 刚性常微分方程求解

6.1 什么是刚性常微分方程？

　　这样得到的微分方程有一个特点——刚性很强。就像弹簧一样，刚性越强意味着。对于数值仿真来说，刚性强意味着
　　

6.2 刚性常微分方程的数值解法

　　计算海森矩阵 $H$ 非常浪费时间，所以作者使用了近似的值
$$
H=J^TJ\tag{4}
$$
　　近似值省略了海森矩阵中的二阶项。在函数比较平滑（近似线性）时，二阶导数很小，因此这样的近似是合理的[6,7]。

引用文献

[1]　High-Fidelity Yet Fast Dynamic Models of Wheeled Mobile Robots
[2]　Rough Surface Generation.
[3]　Theory of Land Locomotion, M.G. Bekker.
[4]　Theory of Ground Vehicles, Jo Y. Wong.
[5]　Dynamic Model Formulation and Calibration for Wheeled Mobile Robots, Neal A. Seegmiller, PhD Thesis, p18.
[6]　Why is the approximation of Hessian=$J^TJ$ reasonable?.
[7]　Gauss–Newton algorithm.