[发明专利]一种基于IMU与机器人本体参数融合的姿态测量方法

说明书

本发明公开了一种基于IMU与机器人本体参数融合的姿态测量方法，通过配置机器人通讯接口读取本体参数，采用D‑H建模求解得到基于机器人本体参数的末端姿态角。固定在机器人末端的陀螺仪和加速度计组成外部惯性测量单元，将测得的数据利用卡尔曼滤波思想进行姿态解算，得到基于IMU的机器人末端姿态角。以基于外部IMU测量解算得到的末端姿态角作为先验预测量，以基于机器人本体参数解得的末端姿态角作为量测量进行后验更新，采用卡尔曼滤波进行数据融合，得到基于IMU与机器人本体参数融合的姿态角。本发明将外部IMU与机器人本体参数相结合，不仅可以降低由于惯性传感器漂移、积分计算造成的累积误差，提高姿态测量精度，而且无需添加外部大型测量设备。

技术领域

本发明涉及一种工业机器人末端姿态测量方法，特别是基于IMU与机器人本体参数融合的末端姿态测量方法，属于精密测量领域。

背景技术

工业机器人是制造的关键装备，机器人末端姿态的控制精度直接影响了机器人加工、焊接、测量等应用的精度，准确的姿态测量是机器人精确控制的前提。目前，工业机器人姿态的测量可以通过多种方法实现，主要有惯性测量法(Inertial Measurement Unit，IMU)、光学姿态测量法、视觉测量法、机械式测量法等。其中基于IMU的机器人末端姿态测量方法具有实时性好，测量速度快，设备体积小等优点，测量时不受空间大小的限制，方便携带与安装，适用于对测量精度、动态性能、实时性均有较高要求的领域，更适合在工业现场中推广。

惯性测量单元由陀螺仪和加速度计组成。在实际应用中，受传感器自身特性的影响，陀螺仪容易发生漂移，误差会随时间的推移而累积；加速度计静态性能较好，但是动态性能差，且通过积分计算会使误差累积更大，这些都导致基于IMU的姿态测量误差增大。因此有必要发明一种基于IMU与机器人本体参数融合的末端姿态高精度测量方法，提高基于IMU的机器人末端姿态测量精度。

发明内容

本发明针对基于IMU的机器人末端姿态测量中，由于陀螺仪漂移和积分计算产生的累积误差的问题，提供了一种基于IMU与机器人本体参数融合的姿态测量方法。该方法能够降低由于漂移、积分累积造成的误差，大大提高惯性测量方法的精度，而且无需额外添加视觉，激光跟踪仪等外部测量设备，简单便携，更适用于在复杂生产现场的应用。

为了达到上述目的，本发明采用基于IMU与机器人本体参数融合的姿态测量方法，包括以下步骤：

步骤1：定义欧拉角旋转顺序，得到旋转矩阵。工业机器人的姿态定义为末端工具坐标系(b系)相对于机器人基坐标系(n系)的转换关系，由分别绕Z、Y、X坐标轴的旋转角度A、B、C决定。不同的旋转顺序对应得到的旋转矩阵不同。定义欧拉角旋转顺序为Z-Y-X，依次绕Z轴旋转A角度，绕Y轴旋转B角度，绕X轴旋转C角度。根据指定旋转顺序得到由欧拉角表示的旋转矩阵：

步骤2：建立D-H运动学模型，D-H模型通过使用四个参数来表示串联型工业机器人各连杆之间坐标系的转换关系，分别为：连杆长度a_i、连杆扭转角α_i、连杆偏移量d_i、关节角θ_i。通过D-H模型可获得工具坐标系和基坐标系的转换关系。其中连杆长度、扭转角和偏移量为系统参数，根据机器人结构获取并进行标定。关节角通过配置机器人通讯接口，实现机器人控制系统和上位机的数据交换，实时读取获得。通过两次绕轴旋转和两次沿轴平移将连杆坐标系C_i转换到连杆坐标系C_i+1。转换矩阵为：

Rot(Z,θ_i)表示绕C_i坐标系的Z轴旋转θ°，Trans(Z,d_i)表示沿坐标系的Z轴平移距离dmm，Trans(X,a_i)表示沿坐标系的X轴平移距离amm，Rot(X,α_i)表示绕坐标系X轴旋转α°