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

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

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

🏈指点迷津 | Brief

🎯要点

🎯正向运动学几何矩阵，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{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 中执行相同的操作来明确更多的事情。我们的（此算法）函数原型是：

 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

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