# MATLAB和Python发那科ABB库卡史陶比尔工业机器人模拟示教框架

{% hint style="success" %}
MATLAB | Python | 工业机器人 | 可视化参数 | 关节位置 | 速度加速度 | 关节扭矩 | 示教 | 机器人模型 | 运动学 | 动力学
{% endhint %}

{% embed url="https://viadean.notion.site/MATLAB-Python-ABB-404d8c2bab39411f8a60751bce758a2a" %}

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

### 🎯要点

🎯模拟工业机器人 | 🎯可视化机器人DH 参数，机器人三维视图 | 🎯绘制观察运动时关节坐标位置、速度和加速度 | 🎯绘制每个关节处的扭矩和力 | 🎯图形界面示教机器人 | 🎯工业机器人模型​ | 🎯计算工业机器人正向和反向运动学、正向和反向动力学

#### 📜机器人用例

📜[人形机器人训练模型：Python人形机踊跃跨栏举重投篮高维数动作算法模型](python-ren-xing-ji-yong-yue-kua-lan-ju-zhong-tou-lan-gao-wei-shu-dong-zuo-suan-fa-mo-xing)

📜[机器人集群：​C++和Python蚂蚁搬食和蚊虫趋光性和浮标机群行为算法神经网络](../qian-ru-shi-embedded/c++-he-python-ma-yi-ban-shi-he-wen-chong-qu-guang-xing-he-fu-biao-ji-qun-xing-wei-suan-fa-shen-jing)

### 🍪语言内容分比

{% @mermaid/diagram content="pie title 语言分比
"MATLAB":90
"Python":60
"C++":50" %}

{% @mermaid/diagram content="pie title 内容分比
"工业机器人":90
"关节坐标位置、速度和加速度":40
"关节扭矩和力":46
"图形示教":30
"机器人运动学、动力学":40" %}

### 🍇Python惯性单元标定

首先我们来描述一下惯性测量单元的模型。对于陀螺仪和加速度计，其形式为

$$
\bar{x}=T x+b+n
$$

* $$x$$ - 在参考体坐标系中解析的真实运动矢量（角速率或比力）
* $$\tilde{x}$$- 测量向量
* $$T$$- 接近恒等式的变换矩阵：对角线元素定义比例因子误差，非对角线元素定义轴未对齐
* $$b$$ - 偏差向量，在这种情况下假设是恒定的，从某种意义上说，它是变化偏差中最重要的恒定部分
* $$n$$ - 噪音和未考虑的影响

在这里，我们考虑 MEMS 类型的惯性测量单元，它们往往具有显着的噪声、随机和温度偏置不稳定性。对于精确的惯性测量单元，原则上方法是相同的，但在处理偏差方面更加细致。陀螺仪和加速度计的校准方法不同。

在此过程中不会精确估计偏置矢量，因为通常必须在整个温度范围内测量它，并且无论如何它都不会很稳定。但粗略估计并消除以获得变换矩阵的正确估计

对 3 个轴中的每一个重复校准过程：

* 将惯性测量单元保持静止 30-60 秒，并估计当前陀螺仪偏差（加上地球角速率）作为其平均读数
* 将惯性测量单元绕轴旋转 360 度（也可以是其他角度），角速度可达到显著水平（20-50 度）

偏差测量是加速度计校准过程所固有的。此外，通常加速度计对温度变化的偏置敏感性较低，并且单个偏置值可能是合适的。校准是通过将每个 IMU 轴垂直向上和向下放置并计算平均读数的一半和和一半差来完成的，以获得变换矩阵元素和偏差。具体的计算可以在代码中看到。

首先创建一个 T 对象。我们假设该表稍微不水平，并且其轴之间存在小的非正交性（在弧秒级别）。航向角是任意的。

```python
 import numpy as np
 import matplotlib.pyplot as plt
 from scipy.integrate import trapezoid
 ​
 plt.rcParams['figure.figsize'] = (10, 6)
 plt.rcParams['axes.grid'] = True
 plt.rcParams['font.size'] = 12
```

```python
 table_lla = [55, 58, 0]
 table_rph = [-2/3600, 1/3600, 137]
 axes_nonorthogonality = 3/3600
 ​
 table = pyx.sim.Tutable(table_lla, table_rph, axes_nonorthogonality)
```

现在执行旋转命令，将用于校准的段被专门标记（使用 label=...）。

```python
 table.rest(30, label='gyro_bias_x')
 table.rotate('outer', 360, label='gyro_rot_x')
 ​
 table.rest(30, label='gyro_bias_z')
 table.rotate('inner', 360, label='gyro_rot_z')
 table.rest(5)
 ​
 table.rotate('inner', -90)
 table.rest(30, label='gyro_bias_y')
 table.rotate('outer', 360, label='gyro_rot_y')
 table.rest(5)
 ​
 table.rest(10, label='accel_z_down')
 table.rotate('outer', 180)
 table.rest(10, label='accel_z_up')
 ​
 table.rotate('outer', 90)
 table.rest(10, label='accel_x_down')
 table.rotate('outer', 180)
 table.rest(10, label='accel_x_up')
 ​
 table.rotate('inner', 90)
 table.rest(10, label='accel_y_down')
 table.rotate('inner', 180)
 table.rest(10, label='accel_y_up')
```

生成理想的惯性测量单元数据。

```python
 trajectory, imu_ideal, labels = table.generate_imu(0.01)
```

现在让我们定义测量单元误差参数（生成随机给定的统计模型）。

```python
 gyro_model = pyx.iner_sens.EstimationModel(
     bias_sd=400 * pyx.transform.DH_TO_RS, 
     noise=1 * pyx.transform.DRH_TO_RRS,
     bias_walk=20 * pyx.transform.DH_TO_RS / 60,
     scale_misal_sd=0.01
 )
 accel_model = pyx.iner_sens.EstimationModel(
     bias_sd=0.2,
     noise=0.1 / 60,
     bias_walk=0.003 / 60,
     scale_misal_sd=0.01
 )
 rng = np.random.RandomState(0)
 gyro_parameters = pyx.iner_sens.Parameters.from_EstimationModel(gyro_model, rng)
 accel_parameters = pyx.inert_sens.Parameters.from_EstimationModel(accel_model, rng)
```

并应用参数来获取最终的惯性测量单元读数：

```python
 imu = pyx.inert_sens.apply_imu_parameters(imu_ideal, 'rate', 
                                                  gyro_parameters, accel_parameters)
```

让我们绘制惯性测量单元读数来演示校准过程是如何组织的。

```python
 plt.plot(imu[pyx.GYRO_COLS], label=['gyro_x', 'gyro_y', 'gyro_z'])
 plt.xlabel("System time, s")
 plt.title("Gyro readings in rad/s")
 plt.legend()
```

现在数据已准备好估计惯性测量单元校准参数。
