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

news2024/10/6 18:29:29

🎯要点

📜机器人用例

📜人形机器人训练模型：Python人形机踊跃跨栏举重投篮高维数动作算法模型

📜机器人集群：C++和Python蚂蚁搬食和蚊虫趋光性和浮标机群行为算法神经网络

🍪语言内容分比

在这里插入图片描述

🍇Python惯性单元标定

首先我们来描述一下惯性测量单元的模型。对于陀螺仪和加速度计，其形式为
$\bar{x}=T x+b+n$

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

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

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

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

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

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

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

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

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=…）。

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')

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

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

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

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)

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

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

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

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()

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