轮式移动机器人的模块化动态仿真

摘要

本文提出了一种用于轮式移动机器人(WMRs)三维动态仿真的模块化方法。我们的方法扩展了基于空间向量代数的高效动力学算法，以适应任意铰接式的WMR构型。与一些替代方案相比，我们的方法还支持复杂的非线性轮‐地接触模型。我们采用一种新颖的微分代数方程(DAE)公式来求解接触力，而不是直接添加接触力。为了实现这一点，我们解决了非线性和过约束问题。通过针对两种先进WMRs平台以及车轮‐地面接触模型的仿真，展示了本方法的灵活性和速度。仿真精度通过物理实验得到了验证。

1 引言

本文提出了一种用于轮式移动机器人（WMRs）三维动态仿真的模块化方法。
这里的“动态仿真”是指采用基于二阶物理的运动模型，该模型考虑了惯性和外加力。这是对我们先前发表的一阶速度运动学模型[15]工作的扩展。
“模块化”是指该方法可适用于任意铰接式轮式移动机器人构型，以及表示为具有指定输入函数形式的任意轮‐地接触模型。
我们的同事最近在模型预测规划[10][16]方面取得了进展。为了实现良好的性能，这些规划器需要精确的运动模型，以正确考虑轮胎打滑、倾覆、执行器限制等因素，并且还要求能够以远快于实时的速度进行仿真。我们的方法在这相互矛盾的标准之间实现了理想的平衡。我们将最初为操作臂开发的高效动力学算法进行了扩展，以适用于非固定基座并保证车轮与地面的接触点约束。

模型。这些模型规定了轮地界面上力与滑移之间的非线性关系。纳入这些模型的最简单方法是基于当前的滑移值，每个时间步长计算一次接触力，并直接将其添加到每个车轮[1][19][11]。此时动力学方程仅为常微分方程组；然而，该系统将是“刚性”的，需要采用非常小或自适应时间步长的求解器[1]。
相比之下，我们采用一种新颖的微分代数方程(DAE)形式，通过拉格朗日乘子来施加车轮‐地面接触模型（第3.4节）。这种方法需要解决非线性和过约束问题，但被证明更加稳定和高效。
尽管我们的贡献是理论上的，但它具有直接的实际应用价值。通过使用我们的方法，大量基于实验得出的车轮‐地面接触模型文献可以readily应用于整车仿真，以用于规划、控制和估计。我们在第4节中通过对两个最先进的轮式移动机器人平台及车轮‐地面接触模型的仿真展示了这一点。

2 相关工作

轮地接触建模仍然是一个活跃的研究领域。例如，在机器人学文献中，存在针对松散土壤中刚性轮的基于地面力学的模型[11][5][14]。在汽车工程文献中，存在针对充气轮胎的模型[3]。尽管这些车轮级接触模型的最终目标是改进车辆级仿真，但很少有这些出版物尝试实现这一目标，或许是因为现有的仿真资源不适合。
一些用于车辆仿真的商业资源，如Adams/Car和CarSim，已经存在。这些工具使用非常详细的模型，细化到小型部件和子系统（例如发动机支架）；因此，它们需要了解大量参数，且运行速度可能较慢。更重要的是，这些模型配置仅限于公路车辆，如汽车和卡车。喷气推进实验室（JPL）开发了巡视器分析、 modeling与仿真（ROAMS）软件，用于模拟火星探测漫游车的完整动力学特性，包括轮‐土滑移/下陷相互作用[13]。虽然ROAMS在设计评估方面具有帮助，但其在运动规划中的应用并不可行。
考虑轮胎打滑、倾覆、执行器限制等因素的基于物理的模型可以极大地促进轮式移动机器人运动规划，但由于实现和计算成本较高，这些模型很少被使用。
石上等人提出了一种巡视器规划算法，该算法对完整的动态仿真候选路径进行评估，但指出了计算成本问题；仅评估4条路径（每条路径巡视器执行约需3分钟）就耗时47分钟[12]。伊萨科塔等人在其类RRT规划器中近似模拟轮式移动机器人动力学，以确保路径满足准静态和摩擦锥约束[7]。穆尔和田等人在二维空间中建模轮式移动机器人物理特性，用于反馈控制目的[20][23]。
由于其易于使用，开放动力学引擎（ODE）常被用于轮式移动机器人（WMRs）的仿真[11][17][6][15],，但其在轮‐地接触建模方面选项有限。ODE仅能施加一种基本的接触模型，该模型通过摩擦锥近似库仑力限制，并且滑移速度与力成线性比例关系。
我们的方法支持一种具有ODE易用性的仿真器，同时以前所未有的微分代数方程形式强制实施非线性车轮‐地面接触模型。此外，与ODE不同，我们的方法专门针对轮式移动机器人的效率进行了设计。这一点在我们使用关节空间动力学算法、有限的碰撞引擎以及惯性和偏置力近似中得到了体现，如第3节所述。
在轮式移动机器人应用之外，动态仿真领域也有一些相关研究。一些学者发表了关于具有非完整约束[18][25],的动态系统仿真，其中车轮滑移约束是一类特殊形式。一些研究者为空间应用开发或提出了模块化仿真器，例如SpaceDyn[24]和LRMAS[4]。我们的方法扩展了菲瑟斯通最初为机械臂动力学[8]所开发的空间向量算法。奥琳将这些算法应用于多足车辆[19],，但据我们所知，尚无人将其应用于轮式移动机器人。

3 仿真数学

本节介绍我们仿真方法的数学原理，在此过程中我们使用以下符号约定：
• underline 表示任意维度的列向量 u
• overset harpoon 表示一个三维笛卡尔坐标向量 ⇀u
• overset arrow 表示一个6D普吕克坐标空间向量 →u
• cuba 表示该量 u 是坐标系 a 相对于坐标系 b 的，且用坐标系 c 的坐标表示。uba 意味着 buba。
• Rba表示坐标系a相对于坐标系b的旋转。使用矩阵乘法来变换向量所表达的坐标，如下所示：b⇀u= Rba(a⇀u)。齐次变换和普吕克变换（T，X）用于编码旋转和平移，并采用相同的书写标记法。
•[ ⇀u]×表示由 ⇀u的元素构成的一个斜对称矩阵。该矩阵用于通过矩阵乘法表示叉积： ⇀a× b=[ ⇀a⇀ ⇀]×b
• u(i) 表示向量 u 的第 i 个元素。u(1 : n) 表示由第 1 到第 n 个元素组成的子向量。A(i,j) 表示矩阵 A 第 i 行第 j 列的元素。A(i ∗) 表示第 i 行，A(∗,j) 表示矩阵 A 的第 j 列。

3.1 运动学模型与状态空间

首先，将轮式移动机器人的运动学模型构建为一个坐标系树。根节点是本体坐标系，相对于导航/世界坐标系具有6个自由度（DOF）。通过单自由度旋转或棱柱关节连接附加的转向、悬挂等坐标系。所有分支以车轮坐标系终止，按照惯例，这些车轮坐标系通过绕其y轴的旋转关节连接。每个坐标系的质量属性也被指定。
在每个时间步长，都会为与地形接触的每个车轮附加一个无质量坐标系。该接触坐标系的原点是车轮圆周上侵入地形表面最深的点。这是通过有限碰撞计算得出的


表1 陆地行者坐标系表。由PFM制造公司 制造。

i	坐标系	父级	类型	驱动x	y	z	θx	θy	θz
1	body	nav
2	FL	body	RY	Y	l	w	0	0	0
3	FR	body	RY	Y	l	-w	0	0	0
4	ML	body	RY	Y	0	w	0	0	0
5	MR	body	RY	Y	0	-w	0	0	0
6	BL	body	RY	Y	-l	w	0	0	0
7	BR	body	RY	Y	-l	-w	0	0	0

英寸：l=42，w=32.25，轮半径=16.5，总质量= 3225lbm

表2 岩石7号坐标系表格 [22]

i	坐标系	父级	类型驱动	x	y	z	θx	θy	θz
1	body	nav
2	D1	body	RY	N	k2	k3	k1	0	0
3	S1	D1	RZ	Y	k4	0	0	0	0
4	A1	S1	RY	Y	0	0	-k5	0	0
5	B1	D1	RY	N	-k6x	0	-k6z	0	0
6	A3	B1	RY	Y	k7	0	-k8	0	0
7	A5	B1	RY	Y	-k7	0	-k8	0	0
8	D2	body	RY	N	k2	-k3	k1	0	0
9	S2	D2	RZ	Y	k4	0	0	0	0
10	A2	S2	RY	Y	0	0	-k5	0	0
11	B2	D2	RY	N	-k6x	0	-k6z	0	0
12	A4	B1	RY	Y	k7	0	-k8	0	0
13	A6	B1	RY	Y	-k7	0	-k8	0	0

以厘米为单位：k1=10.5，k2=12.075，k3=20，k4=28.8，k5=12.5，k6x=16·sin(49◦)，k6z=16·cos(49◦)，k7=6.9，k8=2，轮半径=6.5，总质量 = 11 kg

与车轮和表面几何相交的发动机。车轮可以表示为三维圆（离散化为点），表面可以是有界平面、高程网格、三角网格等。接触坐标系的z轴与接触点处的表面法向量对齐。x轴和y轴分别与纵向和侧滑方向对齐。
坐标系信息以有序列表形式存储，使得任意坐标系的索引大于其父级坐标系的索引（i> p(i)，i= 1为本体坐标系）。两个示例轮式移动机器人的坐标系数据见表1和表2。关节类型为绕某一轴(X,Y,Z)的旋转关节(R)或移动关节(P)。
Act. 表示驱动。最后六列表示当关节位移为零时，每个坐标系相对于其父级坐标系的位姿。
我们选择使用广义（或简化）坐标作为我们的方法。状态向量（q）的前几个元素是本体坐标系相对于世界坐标系的位姿（ρ）。姿态（o）可以使用欧拉角或四元数表示。状态向量的后续元素是关节位移（θ），其顺序与坐标系列表中的顺序相同。
q=[ρθ] ρ=[ ⇀t o] (1)

3.2 空间向量代数动力学算法

我们使用Featherstone发表的空间向量代数来表达轮式移动机器人动力学 [8][9]。这与我们之前关于轮式移动机器人运动学的向量代数表述工作相兼容[15]。空间向量是6维的，属于两个向量空间：运动和力。空间速度和加速度（→v，→a）是运动矢量；它们包含3D角分量和线性分量（分别在上方和下方）。类似地，空间→力（f）包含3D力矩和线性力分量。
→v=[ω⇀⇀v] →a=[α⇀⇀a] → f=[⇀⇀fτ] (3)
无约束动力学方程为：
M q¨s+ c(q, q˙s)= τ (4)
˙ qs 等价于状态(˙ q)的一阶时间导数，只是位姿(ρ˙)的时间导数被替换为本体坐标系的空间速度(b→vwb)。同样地， ¨qs 包含空间加速度。 τ是作用在关节处的执行器扭矩/力的向量。
我们使用递归牛顿‐欧拉算法（RNEA）来计算关节空间偏置力c，其中包含科里奥利力和离心力项，如算法1所示。外部车轮接触力可以直接添加到第10行（如同[1][19][11]所示）。然而，我们通过约束条件引入这些力，具体解释见第3.3节。我们不向每个坐标系添加重力，而是直接对基座进行加速度处理（第2行）。nf 是帧的总数，nw 是车轮坐标系的数量。Xi p(i) 是一个 6×6 Plücker变换，用于将motion空间向量从父级坐标系转换到子级坐标系；其转置则用于将force空间向量从子级坐标系转换到父级坐标系。Ii 是第i帧的空间惯性（包含质量、质心和惯性矩）。函数s(i)将第i帧的关节类型映射到一个空间向量索引（RX=1、RY=2、RZ=3、PX=4、PY=5、PZ=6）。

算法1 逆动力学递归算法，关节空间偏差力
1. →v1= ˙ qs(1 : 6)
2. →a1= b→g
3. for i= 1到 nf do
4. 如果 i> 1则
5. →h= 0
6. 如果 i ≤(nf − nw)则 → h(s(i))= q˙s(i+5)
7. →vi=Xi p(i) →vp(i)+ →h
8. →ai=Xi p(i) →ap(i)+ →vi× → h
9. 结束如果
10. →fi=Ii →ai+ →vi×Ii →vi(+ →f ext i)
11. 结束for循环
12. for i= nf到 2，步长为 ‐1 do
13. 如果 i ≤(nf − nw)那么 c(i+ 5)= →fi(s(i))
14. → fp(i) = →fp(i) + (Xi p(i))T →fi
15. 结束for循环
16. c(1 : 6) = →f1

算法2. 复合刚体算法，关节空间惯性
1. 对于 i= 1从 (nf −nw)执行 Iic=Ii
2. for i=(nf −nw) 到 2，步长为 ‐1 do
3. Ipc(i)=Ic p(i)+(Xi p(i)) TIc i(X i p(i))
4. 结束for循环
5. M= 0
6. M(1 : 6,1 : 6) = I1c
7. for i= 2到 (nf −nw) do
8. f→ c= Ic i(∗,s(i))
9. M(i+5,i+5)= →f c(s(i))
10. j=i
11. 当 j> 1 时
12. →f c=(Xi p(i))T →f c
13. j= p(j)
14. 如果 j= 1那么
15. M(1:6,i+5)= →f c
16. M(i+5,1 : 6) =M(i+5,1 : 6)T
17. else
18. M(j+5,i+5)= →f c(s(j))
19. M(i+5,j+5)=M(j+5,i+5)
20. 结束如果
21. 结束循环
22. 结束for循环

3.3 轮地接触约束

每个轮‐地接触坐标系包含三个约束：一个完整表面接触约束，用于限制其 z轴方向的运动；以及两个非完整滑移速度约束，用于限制其x轴和y轴方向的运动。如[25],所述，通过微分将完整约束转化为速度约束。对于单个车轮，约束方程的形式为：
A q˙s = ⇀ vc (5)
算法3 车轮约束矩阵
1. c=接触坐标系索引
2. i= p(c)
3. 当 i> 1时
4. 如果 s(i) ∈{1,2,3} 则
5. A(∗,i+5) =Rwi(∗,s(i)) ×( ⇀twc − ⇀t wi)
6. 否则如果 s(i) ∈{4,5,6} 那么
7. A(∗,i+5) =Rwi(∗,s(i) −3)
8. 结束如果
9. i= p(i)
10. 结束循环
11. A(∗,1 : 3) =[ ⇀t wc − ⇀t 1w]×TR1w
12. A(∗,4 : 6) = R1w
13. A= RcwA
矩阵 A 通过算法 3 计算，该算法沿从接触点到本体坐标系的运动学链逆向进行。在第11行，使用了恒等式ω⇀ ×⇀t= −⇀t × ω⇀=[⇀t]×Tω⇀，因为˙ qs包含本体坐标系的角速度。等式右侧 ⇀vc是 c⇀vwc 的简写：表示接触坐标系相对于地面的速度，并以接触坐标表示。该值不是常数，而是如第3.4节所述通过优化求解得到的。
我们将所有车轮‐地面接触点模型表示为一种通用格式的函数：
⇀ f c= f(⇀vc,Rω,Δz) (6)
地面作用于车轮的力（f c）取决于 ⇀vc、轮半径与角速度的乘积（Rω），以及由于沉陷或压缩导致的接触点与地形表面之间的位移（Δz）。图3展示了两个示例模型的纵向力随滑移率和侧偏角变化的曲线。

所有车轮的约束被堆叠成一个具有3nw行的矩阵方程。此后，令A表示所有车轮约束的堆叠矩阵，vc表示所有车轮约束的堆叠向量。可附加附加约束以考虑机械对关节位移（如横滚/俯仰平均）的限制或强制执行驱动关节的期望速度。

3.4 力平衡优化

本节阐述了本文最重要的理论贡献，即在微分代数方程形式中施加非线性车轮‐地面接触模型。动力学方程(4)被修改为包含如下约束：
[M AT A C][qλ¨s]=[τ −c b] (7)
其中C是一个零矩阵，仅在附加的完整约束对应的对角线上可能具有非零的 “约束力混合”值。这些值的使用方式与开放动力学引擎中引入柔顺性的方式相同。(7)式可重新整理以求解代表约束力的拉格朗日乘子向量(λ)：
λ=[AM−1AT+ C]−1(b−AM−1(τ −c)) (8)
状态速度的约束（ ˙ qs）已被转换为状态加速度的约束（ ¨qs），如下所示：
A q˙s[i+ 1]= vc[i+ 1] (9)
A(q˙s[i]+ q¨sΔt)= vc[i+ 1] (10)
A q¨s=(vc[i+ 1]−A q˙s[i])/Δt (11)
A q¨s=(vc[i+ 1]−vc[i])/Δt= b (12)
[i]和[i+ 1]表示当前和下一个时间步长。vc[i]已在算法1中计算得出，而vc[i+1]必须通过优化计算：
argmin vc[i+1] ‖λ(i|i ∈W)−f c‖ (13)
简而言之，选择接触点速度，使得由动力学方程(8)计算出的约束力与由轮‐地接触模型(6)计算出的约束力相匹配。 fc表示所有车轮的接触模型力的堆叠向量。 W表示车轮约束索引集合（不包括附加约束）。
该优化可使用牛顿法高效地进行。设x表示参数vc[i+ 1]，f(x)为目标函数；则我们对x的估计值按如下方式更新：
xn+1= xn−γ[Hf(xn)] −1 ∇f(xn), 0 ≤γ ≤1 (14)
计算海森矩阵（Hf(xn)）和梯度（∇f(xn)）需要轮‐地接触模型输出 fc相对于其输入的雅可比矩阵。步长 γ的选择需满足强沃尔夫条件，使用如[21]中的算法3.2之类的线搜索算法。
轮式移动机器人在本体坐标系运动上有6个自由度，每个旋转/棱柱关节各有一个自由度。每个接触点处的车轮有三个约束，再加上任何附加约束。许多轮式移动机器人构型是过约束的，导致矩阵A秩亏。为解决此问题，我们选择并施加一组线性无关的约束子集。通过对手持AT进行QR分解可选择出条件良好的子集。
该子集包含所有附加约束，但仅包含部分车轮约束。目标函数(13)中的fc被替换为P fc，其中P将完整的接触力向量投影到子集空间上。
一旦求解出 ¨qs，即可使用辛欧拉积分按如下方式更新状态速度和状态。
q˙s[i+ 1]= q˙s[i]+ q¨sΔt (15)
q˙[i+ 1]← q˙s[i+ 1] (16)
q[i+ 1]= q[i]+ q˙[i+ 1]Δt (17)
注意，(16) 需要将角速度转换为欧拉角或四元数速率。也可采用高阶积分方法，如龙格‐库塔。与“刚性”常微分方程方法不同，我们的微分代数方程方法即使在大时间步长下也保持稳定。

3.5 计算速度提升建议

无论处理器速度多快，更快的仿真器都能通过评估更多的候选轨迹来提升规划性能。可以通过多种方式缩短计算时间，同时不牺牲仿真精度。首先，在优化过程中将接触点速度（vc[i+ 1]）初始化为前一时间步长的值（vc[i]）。轮式移动机器人在执行稳态操作时，这些速度通常变化很小。设定一个成本阈值，当低于该阈值时，无需使用牛顿法进行优化。接下来，为轮‐地接触模型及其雅可比矩阵预计算查找表。这对于[11][14],中基于地面力学的模型尤其有益，因为这些模型需要在轮面沿线上对应力分布进行耗时的积分。
人们可以在精度合理折衷的情况下进一步加快计算速度。首先，在预测时域内，关节空间惯性矩阵 M 的变化对轮式移动机器人运动的影响可能微乎其微。如果是这样，则仅需在第一个时间步长计算 M 和 M−1，并在后续步骤中重复使用这些值。此外，科里奥利力和离心力对内部关节的影响也可能忽略不计。如果是这样，关节空间偏置力 c 可以近似为一个零向量，但以下情况除外：
c(1: 6)= I1c( b→g)+ → v1×I1c →v1 (18)
这将WMR视为一个单刚体（所有关节锁定）。I1c是根节点位于本体坐标系的WMR的合成惯性，与M一样，只需计算一次。

4 结果

在本节中，我们通过三个测试来评估我们的仿真方法。在第一个测试中，我们对陆地行者（LandTamer）车辆（图1）进行了仿真，使用帕塞贾（Pacejka）轮‐地接触模型（图3(a)），使其驶过一个 20◦斜坡，然后以2米/秒的速度向左和向右转弯（图4）。图5显示了该轨迹两次仿真的滑移率和侧偏角：一次使用我们通过约束处理接触力的方法，另一次则直接添加接触力。使用约束消除了抖动现象，并将计算时间从21.40秒减少到2.03秒（在MATLAB中实现，使用2.83 GHz处理器）。两次仿真均受益于我们方法所减少的状态空间以及RNEA/CRBA的使用；这一加速效果将在未来的工作中进行量化。两次仿真均使用约束来控制车轮速度，因为PID扭矩控制器可能使动力学变得非常刚性。
直接添加法需要使用自适应积分器（MATLAB的ode45）。图6(a)显示，为了防止抖动，该积分器所采用的步长相比我们方法的0.04秒步长要小得多。如果直接添加法采用0.04秒步长，则抖动将导致严重的不稳定。图6(b)显示了我们方法中力平衡优化（第3.4节）所需的牛顿法迭代次数；在稳态期期间无需迭代。
我们还在泰勒测试场附近宾夕法尼亚州匹兹堡的一个停车场内，通过物理实验（第二次测试）验证了陆地行者的动态模型。我们驾驶陆地行者以不同弧线行驶（最高达2.5 米/秒和0.5 弧度/秒）。我们调整了动态模型的参数（包括轮胎模型参数和质心），以拟合数据集的一部分，然后在其余部分上进行评估（图7）。表3比较了我们调优后的动态模型与一种在六个车轮上最小化滑移速度的运动学模型的平均位置和绝对偏航误差，即 ||vc||。对于 5‐20米的合理规划时域，我们的预测位置误差为行驶距离的1‐2%。使用我们的动态模型，位置和偏航预测误差比使用运动学模型减少了88‐93%。
在第三次测试中，我们使用基于地面力学的轮‐地接触模型（图3(b)）对Rocky 7火星车（图2）进行仿真。该火星车以0.5 米/秒的速度在不平坦的随机地形上行驶，并进行大转弯，持续10秒钟（图8）。图

表3 陆地行者模型预测误差

	动态模型		运动学模型
预测距离	位置	|偏航|	pos.	|偏航|
5m	0.0669	0.0169	0.5442	0.2006
10m	0.1470	0.0292	1.8076	0.3818
20m	0.3839	0.0515	5.1336	0.6796

位置（米）和绝对偏航角（弧度）误差值是在273米评估路径上等间距的> 50次试验中的平均值。10米和20米试验有重叠。

图9(a)显示了计算时间随时间步长增大而呈指数级减少的情况。计算时间通过除以仿真时间进行归一化处理；小于1的值表示快于实时。第3.5节中提出的每种近似方法均使计算时间减少了约10%。在图9(b)中，误差是指采用无近似和最小(0.005秒)时间步长方法对5米行程进行预测时，预测的末端位姿之间的差异。误差随时间步长线性（而非指数）增加，并且仅略有上升。

5 结论与未来工作

我们提出了一种用于轮式移动机器人仿真的模块化方法。与现有的资源（如开放动力学引擎）相比，我们的方法采用了空间向量代数动力学算法，特别适用于轮式移动机器人。更重要的是，与常微分方程方法不同，我们的方法能够容纳复杂的非线性车轮‐地面接触模型。通过在微分代数方程形式中使用约束来实施这些模型，已被证明比在常微分方程形式中直接添加接触力的常用方法更加稳定和高效（第4节，第一个测试）。
未来的工作中将展示更多关于三维轮式移动机器人运动模型标定的物理实验。我们计划将轮式移动机器人仿真器的MATLAB实现转换为C++；只有这样，才能公平地与替代方案（如ODE、CarSim等）进行计算速度比较。我们还计划将该软件公开可用。这将使现有和未来的车轮‐地面接触模型研究能够直接应用于整车机动性的预测。该仿真器应具备足够快的速度和足够的精度，以支持在许多具有挑战性的应用中实现改进的模型预测规划。