基于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 轴的高度坐标,为了后续处理我们要加入 xy 两个轴的坐标:

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]

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

f=(fx,fy)

  计算梯度使用的函数是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];


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

n=(fx,fy,1)

  将梯度向量简单变换一下就能得到法向量场,如下:

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 所有连杆坐标系的数量(不包括接触坐标系)
  nv 速度向量的维数
  nw 车轮的数量
  na 有驱动器的关节数量
  njc 存在约束的关节的数量
nf = 8; (*坐标系的数量*)
nv = nf - 1 + 6; (*状态向量的维数,主体的姿态6个,其余是关节变量*)
nw = 4; (*车轮数量*)
na = 5; (*驱动关节的数量*)
njc = 0; (*关节约束*)

3.2 几何参数

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

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

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

radii = ConstantArray[radius, nw];

  机器人包含一定活动部件,它们之间的运动关系由关节来描述,定义关节的类型。常用的关节只有两种:转动关节(revolute)和移动关节(prismatic)。简单起见,我们只考虑一个关节只有一个自由度,所以根据关节的运动方向来定义。转动轴沿着xyz轴,分别用1~3的数字表示,移动轴沿着xyz轴,分别用4~6的数字表示。数字0表示该连杆不使用关节。根据关节类型可以定义关节旋量ξ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 ,它是个标量,mR
  质心的位置 p ,它是个三维向量,pR3
  转动惯量 I,它是个3×3矩阵,IR3×3
  在动力学计算时,这三个量被合成为一个量,被称为广义惯性张量,用符号 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是怎么来的呢?为了加深印象,我们可以推导一下。



gbc=[R0p1](1)


M=[m00I](2)


T=12VTMV(3)

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

4. 地面力学模型

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

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

  

4.2 离散化

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

 

  车轮上的接触点 c 的坐标是(xc,yc,zc),那么地面曲面函数在 (xc,yc) 处的高度就是 f(xc,yc),对应 s 点,所以 s 点与 c 点的高度差就是:

h=f(xc,yc)zc
  (xc,yc) 处的法向量是 n,定义陷入深度Δz[5]
Δz=(0,0,h)n

    

4.3 神奇公式

  

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

5.1 状态表达

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

s=(gIb,q)

  其中,gIb 是主体的位姿矩阵,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=JTJ(2)

  近似值省略了海森矩阵中的二阶项。在函数比较平滑(近似线性)时,二阶导数很小,因此这样的近似是合理的[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=JTJ reasonable?.
[7] Gauss–Newton algorithm.

阅读更多
换一批

没有更多推荐了,返回首页