Python | C# | MATLAB 库卡机器人微分运动学 | 欧拉-拉格朗日动力学

本文主要是介绍Python | C# | MATLAB 库卡机器人微分运动学 | 欧拉-拉格朗日动力学 | 混合动力控制，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

🎯要点

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

🎯Cpp(Python)和MATLAB差动驱动ROS Raspberry Pi全功能机器人原型。

🍇Python逆动力学算法

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

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

此算法第一遍计算主体速度 $v _i$ 和加速度 $a _i$ 。从运动树的根 $i = 0$ 开始，物体 $i$ 的运动 $v _i, a _i$ 是根据运动 $_{\lambda(i)}, a _{\lambda( i)}$ 其父体 $\lambda(i)$ 的分量，加上它们之间的关节的运动 $\dot{ q }_i, \ddot{ q }_i$ 引起的分量。让我们从主体速度开始：
$_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 }$ 按自由度索引）。因此，运动子空间矩阵的维度为 $\times k$ 。

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

此算法的第二遍计算体积力。从运动树的叶节点开始，物体 $i$ 的广义力 $f _i$ 被添加到迄今为止为其父代 $\lambda(i)$ 计算的力 $_{\lambda(i)}$ :
$_{\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$ 是一个 $\times 1$ 列向量，因此我们以单个数字 $\tau_i= S _i^{\top} f _i$ 结尾：关节伺服系统应提供的驱动扭矩提供跟踪 $\left( q , \dot{ q }, \ddot{ q }, f ^{e x t}\right)$ 。所有其他组件对应于我们的旋转关节的五度约束，并将由关节的力学被动提供。

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

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

请注意，q 是每个关节的广义坐标列表，而不是平面数组，其他参数也是如此。特别是，f_ext 是体力矢量 $_i^{\text {ext }}$ 的列表。使用 Python 类型注释，我们的原型将如下所示：

from typing import Listimport numpy as npdef 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

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