🎯要点
🎯正向运动学几何矩阵,Python虚拟机器人模拟动画二连杆平面机械臂 | 🎯 逆向运动学几何矩阵,Python虚拟机器人模拟动画三连杆平面机械臂 | 🎯微分运动学数学形态,Python模拟近似结果 | 🎯欧拉-拉格朗日动力学数学形态,Python模拟机器人操纵器推导的运动方程有效性 | 🎯运动规划算法,Python虚拟机器人和摄像头模拟离线运动规划算法 | 🎯移动导航卡尔曼滤波算法及其它方法,Python虚拟机器人模拟可检测和可磕碰 | 🎯混合动力控制微分数学形态,Python虚拟机器人模拟比例微分积分和逆动态控制 | 🎯阻抗控制,Python模拟二联(三联动)。
🎯 库卡机器人模拟 ,库卡实体机器人对象检测和颜色分割拾取和放置物体 | 🎯 C#远程测试虚拟机器人 | 🎯虚拟机器人从三维文件创建自定义模型。
🎯Cpp(Python)和MATLAB差动驱动ROS Raspberry Pi全功能机器人原型 | 🎯Python | C++ | MATLAB机器人正逆向运动学动力学求解器及算法
🍇Python逆动力学算法
逆动力学是指计算运动中的力。给定配置
q
q
q、广义速度
q
˙
\dot{ q }
q˙ 和广义加速度
q
¨
\ddot{ q }
q¨,相当于找到关节扭矩
τ
\tau
τ 和接触力
f
ext
f ^{\text {ext } }
fext 使得运动约束方程得到满足:
M
(
q
)
q
¨
+
q
˙
⊤
C
(
q
)
q
˙
=
S
⊤
τ
+
τ
g
(
q
)
+
τ
est
+
J
(
q
)
⊤
f
ext
J
(
q
)
q
¨
+
q
˙
⊤
H
(
q
)
q
˙
=
0
\begin{aligned} M ( q ) \ddot{ q }+\dot{ q }^{\top} C ( q ) \dot{ q } & = S ^{\top} \tau + \tau _g( q )+ \tau ^{\text {est }}+ J ( q )^{\top} f ^{\text {ext }} \\ J ( q ) \ddot{ q }+\dot{ q }^{\top} H ( q ) \dot{ q } & = 0 \end{aligned}
M(q)q¨+q˙⊤C(q)q˙J(q)q¨+q˙⊤H(q)q˙=S⊤τ+τg(q)+τest +J(q)⊤fext =0
逆动力学的数学函数如下:
(
τ
,
f
e
x
t
)
=
ID
(
q
,
q
˙
,
q
¨
)
\left(\tau, f ^{e x t}\right)=\operatorname{ID}( q , \dot{ q }, \ddot{ q })
(τ,fext)=ID(q,q˙,q¨)
当我们的线性系统完全确定时,该函数定义明确,例如对于具有六个自由度的手臂,但对于在多个接触下的移动机器人,该函数通常是欠确定的。在这种情况下,我们可以将外力的计算转移到例如接触模型,并仅计算关节扭矩:
τ
=
RNEA
(
q
,
q
˙
,
q
¨
,
f
est
)
\tau =\operatorname{RNEA}\left( q , \dot{ q }, \ddot{ q }, f ^{\text {est }}\right)
τ=RNEA(q,q˙,q¨,fest )
递归牛顿-欧拉算法为我们提供了一种实现此功能的有效方法。该算法分为两步:前向传递,主要是二阶正向运动学,然后是后向传递,计算力和关节扭矩。
此算法第一遍计算主体速度
v
i
v _i
vi 和加速度
a
i
a _i
ai。从运动树的根
i
=
0
i=0
i=0 开始,物体
i
i
i 的运动
v
i
,
a
i
v _i, a _i
vi,ai 是根据运动
v
λ
(
i
)
,
a
λ
(
i
)
v _{\lambda(i)}, a _{\lambda( i)}
vλ(i),aλ(i) 其父体
λ
(
i
)
\lambda(i)
λ(i) 的分量,加上它们之间的关节的运动
q
˙
i
,
q
¨
i
\dot{ q }_i, \ddot{ q }_i
q˙i,q¨i 引起的分量。让我们从主体速度开始:
v
i
=
i
X
λ
(
i
)
v
λ
(
i
)
+
S
i
q
˙
i
v _i={ }^i X _{\lambda(i)} v _{\lambda(i)}+ S _i \dot{ q }_i
vi=iXλ(i)vλ(i)+Siq˙i
在此方程中,
i
X
λ
(
i
)
{ }^i X _{\lambda(i)}
iXλ(i) 是从
λ
(
i
)
\lambda(i)
λ(i) 到
i
i
i 的 Plücker 变换,
S
i
S _i
Si 是关节的运动子空间矩阵。请注意,
q
˙
i
∈
R
k
\dot{ q }_i \in R ^k
q˙i∈Rk 是关节的速度,例如对于浮动底座(又名自由飞行器)关节,
k
=
6
k=6
k=6,对于球形关节,
k
=
2
k=2
k=2,对于旋转关节或棱柱关节,
k
=
1
k=1
k=1。无论如何,
q
˙
i
\dot{ q }_i
q˙i 不是广义速度向量
q
˙
\dot{ q }
q˙ 的
i
th
i^{\text {th }}
ith 分量(这没有意义,因为
i
i
i 是关节的索引,而向量
q
˙
\dot{ q }
q˙ 按自由度索引)。因此,运动子空间矩阵的维度为
6
×
k
6 \times k
6×k。
接下来,让我们假设一个“常见”关节(旋转关节、棱柱关节、螺旋关节、圆柱关节、平面关节、球形关节、自由飞行关节),这样运动子空间矩阵的视在时间导数为零。除非你处理的是不同的关节,否则不要介意这句话。 然后,在前向传递过程中从父关节计算出的主体加速度为:
a
i
=
i
X
λ
(
i
)
a
λ
(
i
)
+
S
i
q
¨
i
+
v
i
×
S
i
q
˙
i
a _i={ }^i X _{\lambda(i)} a _{\lambda(i)}+ S _i \ddot{ q }_i+ v _i \times S _i \dot{ q }_i
ai=iXλ(i)aλ(i)+Siq¨i+vi×Siq˙i
到目前为止,该正向传递是二阶正向运动学。一路上我们要计算的最后一件事是由主体运动
v
i
v _i
vi ,
a
i
a _i
ai产生的主体惯性力:
f
i
=
I
i
a
i
+
v
i
×
∗
I
i
v
i
−
f
i
est
f _i= I _i a _i+ v _i \times{ }^* I _i v _i- f _i^{\text {est }}
fi=Iiai+vi×∗Iivi−fiest
我们将在向后传递期间更新这些力向量。请注意,由于它们是力矢量,因此我们的符号意味着
f
i
ext
f _i^{\text {ext }}
fiext 也是一个物体力矢量。如果外力在惯性系中表示为
0
f
i
ext
{ }^0 f _i^{\text {ext }}
0fiext ,则可以通过以
f
i
=
i
X
0
0
f
i
e
x
t
f _i={ }^i X _0{ }^0 f _i^{e x t}
fi=iX00fiext 映射到主体框架 。
此算法的第二遍计算体积力。从运动树的叶节点开始,物体
i
i
i 的广义力
f
i
f _i
fi 被添加到迄今为止为其父代
λ
(
i
)
\lambda(i)
λ(i) 计算的力
f
λ
(
i
)
f _{\lambda(i)}
fλ(i) :
f
λ
(
i
)
=
f
λ
(
i
)
+
i
X
λ
(
i
)
⊤
f
i
f _{\lambda(i)}= f _{\lambda(i)}+{ }^i X _{\lambda(i)}^{\top} f _i
fλ(i)=fλ(i)+iXλ(i)⊤fi
一旦计算出主体
i
i
i 上的广义力
f
i
f _i
fi,我们就可以通过沿关节轴投影该 6D 主体矢量来获得相应的关节扭矩
τ
i
\tau _i
τi:
τ
i
=
S
i
⊤
f
i
\tau _i= S _i^{\top} f _i
τi=Si⊤fi
对于旋转关节,
S
i
S _i
Si 是一个
6
×
1
6 \times 1
6×1 列向量,因此我们以单个数字
τ
i
=
S
i
⊤
f
i
\tau_i= S _i^{\top} f _i
τi=Si⊤fi 结尾:关节伺服系统应提供的驱动扭矩提供跟踪
(
q
,
q
˙
,
q
¨
,
f
e
x
t
)
\left( q , \dot{ q }, \ddot{ q }, f ^{e x t}\right)
(q,q˙,q¨,fext)。所有其他组件对应于我们的旋转关节的五度约束,并将由关节的力学被动提供。
现在让我们通过在伪 Python 中执行相同的操作来明确更多的事情。我们的(此算法)函数原型是:
def rnea(q, qd, qdd, f_ext):
pass
请注意,q 是每个关节的广义坐标列表,而不是平面数组,其他参数也是如此。特别是,f_ext 是体力矢量 f i ext f _i^{\text {ext }} fiext 的列表。使用 Python 类型注释,我们的原型将如下所示:
from typing import List
import numpy as np
def rnea(
q: List[np.ndarray],
qd: List[np.ndarray],
qdd: List[np.ndarray],
f_ext: List[np.ndarray],
) -> List[np.ndarray]:
pass
这种额外的结构允许更通用的关节,例如球形关节(不常见)或用于移动机器人浮动底座的自由飞行关节(常见)。如果所有关节都是旋转的,那么所有类型都将合并为平面阵列。
让我们用 v 0 = 0 v _0= 0 v0=0 表示运动树根链接的空间速度,用 a 0 a _0 a0 表示其空间加速度。我们将它们分别初始化为零和标准重力加速度:
n = len(qd) - 1 # number of links == number of joints - 1
v = [np.empty((6,)) for i in range(n + 1)]
a = [np.empty((6,)) for i in range(n + 1)]
f = [np.empty((6,)) for i in range(n + 1)]
tau = [np.empty(qd[i].shape) for i in range(n + 1)]
v[0] = np.zeros((6,))
a[0] = -np.array([0.0, 0.0, -9.81])
我们继续前向传递,范围从链接 i = 1 i=1 i=1 到树的最后一个链接 i = n i=n i=n:
for i in range(1, n + 1):
p = lambda_[i] # p for "parent"
X_p_to_i[i], S[i], I[i] = compute_joint(joint_type[i], q[i])
v[i] = X_p_to_i[i] * v[p] + S[i] * qd[i]
a[i] = X_p_to_i[i] * a[p] + S[i] * qdd[i] + spatial_cross(v[i], S[i] * qd[i])
f[i] = I[i] * a[i] + spatial_cross_dual(v[i], I[i] * v[i]) - f_ext[i]
向后传递以相反的顺序遍历相同的范围:
for i in range(n, 0, -1):
p = lambda_[i]
tau[i] = S[i].T * f[i]
f[p] += X_p_to_i[i].T * f[i]
最终,我们得到:
def rnea(q, qd, qdd, f_ext):
n = len(qd)
v = [np.empty((6,)) for i in range(n + 1)]
a = [np.empty((6,)) for i in range(n + 1)]
f = [np.empty((6,)) for i in range(n + 1)]
tau = [np.empty(qd[i].shape) for i in range(n + 1)]
v[0] = np.zeros((6,))
a[0] = -np.array([0.0, 0.0, -9.81])
for i in range(1, n + 1):
p = lambda_[i]
X_p_to_i[i], S[i], I[i] = compute_joint(joint_type[i], q[i])
v[i] = X_p_to_i[i] * v[p] + S[i] * qd[i]
a[i] = X_p_to_i[i] * a[p] + S[i] * qdd[i] + spatial_cross(v[i], S[i] * qd[i])
f[i] = I[i] * a[i] + spatial_cross_dual(v[i], I[i] * v[i]) - f_ext[i]
for i in range(n, 0, -1):
p = lambda_[i]
tau[i] = S[i].T * f[i]
f[p] += X_p_to_i[i].T * f[i]
return tau
长度不同的数组列表通常是刚体动力学库或模拟器中的内部结构。从此类列表到平面数组结构的映射称为关节,并决定如何表示球形和自由飞行关节的方向。