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

{% hint style="success" %}
Python | 算法 | 比例 | 微分 | 积分 | 控制 | C# | C++ | MATLAB | 虚拟机器人 | 二连杆 | 机械臂 | 逆向运动学 | 正向运动学 | 几何 | 矩阵 | 三连杆 | 数学 | 近似 | 动力学 | 欧拉-拉格朗日 | 运动方程 | 有效性 | 运动规划 | 摄像头 | 卡尔曼滤波 | 混合动力控制 | 阻抗控制 | 库卡 | 物理
{% endhint %}

{% embed url="https://viadean.notion.site/Python-C-MATLAB-fcca2d085e1c4f94acf0b17c54b77258" %}

{% content-ref url="../zhi-dian-mi-jin-brief" %}
[zhi-dian-mi-jin-brief](../zhi-dian-mi-jin-brief)
{% endcontent-ref %}

### 🎯要点

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

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

🎯[Cpp(Python)和MATLAB差动驱动ROS Raspberry Pi全功能机器人原型](../qian-ru-shi-embedded/cpppython-he-matlab-cha-dong-qu-dong-ros-raspberry-pi-quan-gong-neng-ji-qi-ren-yuan-xing) | :dart:[Python | C++ | MATLAB机器人正逆向运动学动力学求解器及算法](../suan-fa-mo-xing-algorithm/python-c++-matlab-ji-qi-ren-zheng-ni-xiang-yun-dong-xue-dong-li-xue-qiu-jie-qi-ji-suan-fa)

### 🍇Python逆动力学算法

逆动力学是指计算运动中的力。给定配置 $$q$$、广义速度 $$\dot{ q }$$ 和广义加速度 $$\ddot{ q }$$，相当于找到关节扭矩 $$\tau$$和接触力 $$f ^{\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$$ 是根据运动 $$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$$，对于旋转关节或棱柱关节，$$=1$$。无论如何，$$\dot{ q }_i$$不是广义速度向量 $$\dot{ q }$$ 的 $$i^{\text {th }}$$ 分量（这没有意义，因为$$i$$是关节的索引，而向量 $$\dot{ q }$$ 按自由度索引）。因此，运动子空间矩阵的维度为 $$\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{ }^* 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^{e x t}​$$ 映射到主体框架 。

此算法的第二遍计算体积力。从运动树的叶节点开始，物体 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 ^{e x t}\right)$$。所有其他组件对应于我们的旋转关节的五度约束，并将由关节的力学被动提供。

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

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

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

```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$$ 表示其空间加速度。我们将它们分别初始化为零和标准重力加速度：

```python
 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：

```python
 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]
```

向后传递以相反的顺序遍历相同的范围：

```python
 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]
```

最终，我们得到：

```python
 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
```

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