🎯要点

🎯正向运动学几何矩阵，Python虚拟机器人模拟动画二连杆平面机械臂 | 🎯 逆向运动学几何矩阵，Python虚拟机器人模拟动画三连杆平面机械臂 | 🎯微分运动学数学形态，Python模拟近似结果 | 🎯欧拉-拉格朗日动力学数学形态，Python模拟机器人操纵器推导的运动方程有效性 | 🎯运动规划算法，Python虚拟机器人和摄像头模拟离线运动规划算法 | 🎯移动导航卡尔曼滤波算法及其它方法，Python虚拟机器人模拟可检测和可磕碰 | 🎯混合动力控制微分数学形态，Python虚拟机器人模拟比例微分积分和逆动态控制 | 🎯阻抗控制，Python模拟二联（三联动）。

🎯 库卡机器人模拟 ，库卡实体机器人对象检测和颜色分割拾取和放置物体 | 🎯 C#远程测试虚拟机器人 | 🎯虚拟机器人从三维文件创建自定义模型。

🎯Cpp(Python)和MATLAB差动驱动ROS Raspberry Pi全功能机器人原型 | 🎯Python | C++ | MATLAB机器人正逆向运动学动力学求解器及算法

🍇Python逆动力学算法

逆动力学是指计算运动中的力。给定配置 $q$、广义速度 $\dot{q}$ 和广义加速度 $\ddot{q}$，相当于找到关节扭矩 $\tau$ 和接触力 $f^{\text{ext }}$ 使得运动约束方程得到满足：
$$\begin{array}{rl}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{array}$$

逆动力学的数学函数如下：
$$\left(\tau ,f^{ext}\right)=\mathrm{ID}(q,\dot{q},\ddot{q})$$
当我们的线性系统完全确定时，该函数定义明确，例如对于具有六个自由度的手臂，但对于在多个接触下的移动机器人，该函数通常是欠确定的。在这种情况下，我们可以将外力的计算转移到例如接触模型，并仅计算关节扭矩：
$$\tau =\mathrm{RNEA}\left(q,\dot{q},\ddot{q},f^{\text{est }}\right)$$
递归牛顿-欧拉算法为我们提供了一种实现此功能的有效方法。该算法分为两步：前向传递，主要是二阶正向运动学，然后是后向传递，计算力和关节扭矩。

此算法第一遍计算主体速度 $v_i$ 和加速度 $a_i$。从运动树的根 $i=0$ 开始，物体 $i$ 的运动 $v_i,a_i$ 是根据运动 $v_{\lambda (i)},a_{\lambda (i)}$ 其父体 $\lambda (i)$ 的分量，加上它们之间的关节的运动 ${\dot{q}}_i,{\ddot{q}}_i$​ 引起的分量。让我们从主体速度开始：
$$v_i={}^i{X}_{\lambda (i)}{v}_{\lambda (i)}+S_i{\dot{q}}_i$$
在此方程中，${}^i{X}_{\lambda (i)}$ 是从 $\lambda (i)$ 到 $i$ 的 Plücker 变换，$S_i$ 是关节的运动子空间矩阵。请注意，${\dot{q}}_i\in R^k$ 是关节的速度，例如对于浮动底座（又名自由飞行器）关节，$k=6$，对于球形关节，$k=2$，对于旋转关节或棱柱关节，$k=1$。无论如何，${\dot{q}}_i$ 不是广义速度向量 $\dot{q}$ 的 $i^{\text{th }}$ 分量（这没有意义，因为 $i$ 是关节的索引，而向量 $\dot{q}$ 按自由度索引）。因此，运动子空间矩阵的维度为 $6\times k$。

接下来，让我们假设一个“常见”关节（旋转关节、棱柱关节、螺旋关节、圆柱关节、平面关节、球形关节、自由飞行关节），这样运动子空间矩阵的视在时间导数为零。除非你处理的是不同的关节，否则不要介意这句话。 然后，在前向传递过程中从父关节计算出的主体加速度为：
$$a_i={}^i{X}_{\lambda (i)}{a}_{\lambda (i)}+S_i{\ddot{q}}_i+v_i\times S_i{\dot{q}}_i$$
到目前为止，该正向传递是二阶正向运动学。一路上我们要计算的最后一件事是由主体运动 $v_i$ ，$a_i$​产生的主体惯性力：
$$f_i=I_i{a}_i+v_i\times {}^{\ast }{I}_i{v}_i-f_i^{\text{est }}$$
我们将在向后传递期间更新这些力向量。请注意，由于它们是力矢量，因此我们的符号意味着 $f_i^{\text{ext }}$ 也是一个物体力矢量。如果外力在惯性系中表示为 ${}^0{f}_i^{\text{ext }}$，则可以通过以 $f_i={}^i{X}_0{}^0{f}_i^{ext}$​ 映射到主体框架 。

此算法的第二遍计算体积力。从运动树的叶节点开始，物体 $i$ 的广义力 $f_i$ 被添加到迄今为止为其父代 $\lambda (i)$ 计算的力 $f_{\lambda (i)}$​ :
$$f_{\lambda (i)}=f_{\lambda (i)}+{}^i{X}_{\lambda (i)}^{\top }{f}_i$$
一旦计算出主体 $i$ 上的广义力 $f_i$，我们就可以通过沿关节轴投影该 6D 主体矢量来获得相应的关节扭矩 ${\tau }_i$​：
$${\tau }_i=S_i^{\top }{f}_i$$
对于旋转关节，$S_i$ 是一个 $6\times 1$ 列向量，因此我们以单个数字 ${\tau }_i=S_i^{\top }{f}_i$ 结尾：关节伺服系统应提供的驱动扭矩提供跟踪$\left(q,\dot{q},\ddot{q},f^{ext}\right)$。所有其他组件对应于我们的旋转关节的五度约束，并将由关节的力学被动提供。

现在让我们通过在伪 Python 中执行相同的操作来明确更多的事情。我们的（此算法）函数原型是：

def rnea(q, qd, qdd, f_ext):
    pass

请注意，q 是每个关节的广义坐标列表，而不是平面数组，其他参数也是如此。特别是，f_ext 是体力矢量 $f_i^{\text{ext }}$ 的列表。使用 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$ 表示运动树根链接的空间速度，用 $a_0$ 表示其空间加速度。我们将它们分别初始化为零和标准重力加速度：

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=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

长度不同的数组列表通常是刚体动力学库或模拟器中的内部结构。从此类列表到平面数组结构的映射称为关节，并决定如何表示球形和自由飞行关节的方向。

👉参阅：亚图跨际