一种基于IMU与机器人本体参数融合的姿态测量方法

专利名称：一种基于IMU与机器人本体参数融合的姿态测量方法

专利类型：发明专利

专利申请号：CN202111426697.5

专利申请（专利权）人：北京工业大学
权利人地址：北京市朝阳区平乐园100号

专利发明（设计）人：李睿,崔晓玲,董稷坤,丁宁

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

主权利要求：一种基于IMU与机器人本体参数融合的姿态测量方法
技术领域
[0001] 本发明涉及一种工业机器人末端姿态测量方法，特别是基于IMU与机器人本体参数融合的末端姿态测量方法，属于精密测量领域。
背景技术
[0002] 工业机器人是制造的关键装备，机器人末端姿态的控制精度直接影响了机器人加工、焊接、测量等应用的精度，准确的姿态测量是机器人精确控制的前提。目前，工业机器人姿态的测量可以通过多种方法实现，主要有惯性测量法(InertialMeasurementUnit，IMU)、光学姿态测量法、视觉测量法、机械式测量法等。其中基于IMU的机器人末端姿态测量方法具有实时性好，测量速度快，设备体积小等优点，测量时不受空间大小的限制，方便携带与安装，适用于对测量精度、动态性能、实时性均有较高要求的领域，更适合在工业现场中推广。
[0003] 惯性测量单元由陀螺仪和加速度计组成。在实际应用中，受传感器自身特性的影响，陀螺仪容易发生漂移，误差会随时间的推移而累积；加速度计静态性能较好，但是动态性能差，且通过积分计算会使误差累积更大，这些都导致基于IMU的姿态测量误差增大。因此有必要发明一种基于IMU与机器人本体参数融合的末端姿态高精度测量方法，提高基于IMU的机器人末端姿态测量精度。
发明内容
[0004] 本发明针对基于IMU的机器人末端姿态测量中，由于陀螺仪漂移和积分计算产生的累积误差的问题，提供了一种基于IMU与机器人本体参数融合的姿态测量方法。该方法能够降低由于漂移、积分累积造成的误差，大大提高惯性测量方法的精度，而且无需额外添加视觉，激光跟踪仪等外部测量设备，简单便携，更适用于在复杂生产现场的应用。
[0005] 为了达到上述目的，本发明采用基于IMU与机器人本体参数融合的姿态测量方法，包括以下步骤：
[0006] 步骤1：定义欧拉角旋转顺序，得到旋转矩阵。工业机器人的姿态定义为末端工具坐标系(b系)相对于机器人基坐标系(n系)的转换关系，由分别绕Z、Y、X坐标轴的旋转角度A、B、C决定。不同的旋转顺序对应得到的旋转矩阵不同。定义欧拉角旋转顺序为Z‑Y‑X，依次绕Z轴旋转A角度，绕Y轴旋转B角度，绕X轴旋转C角度。根据指定旋转顺序得到由欧拉角表示的旋转矩阵：
[0007]
[0008] 步骤2：建立D‑H运动学模型，D‑H模型通过使用四个参数来表示串联型工业机器人各连杆之间坐标系的转换关系，分别为：连杆长度ai、连杆扭转角αi、连杆偏移量di、关节角θi。通过D‑H模型可获得工具坐标系和基坐标系的转换关系。其中连杆长度、扭转角和偏移量为系统参数，根据机器人结构获取并进行标定。关节角通过配置机器人通讯接口，实现机器人控制系统和上位机的数据交换，实时读取获得。通过两次绕轴旋转和两次沿轴平移将连杆坐标系Ci转换到连杆坐标系Ci+1。转换矩阵为：
[0009]
[0010] Rot(Z,θi)表示绕Ci坐标系的Z轴旋转θ°，Trans(Z,di)表示沿坐标系的Z轴平移距离dmm，Trans(X,ai)表示沿坐标系的X轴平移距离amm，Rot(X,αi)表示绕坐标系X轴旋转α°[0011] 通过上述矩阵可以将机器人末端工具坐标系与位于机器人底部的基坐标系联系起来。以六轴机器人为例，可得到包含机器人末端姿态信息的矩阵T：
[0012] T＝T1T2T3T4T5T6
[0013] 步骤3：搭建末端惯性测量系统，获取惯性测量数据。将高精度集成惯性传感器(由三轴陀螺仪、三轴加速度计组成)安装到机器人末端的法兰盘中心，采用串口通信方式，利用串口转换模块连接至上位机。测量前，在上位机设置采样频率、波特率、量程、输出量等参数，并对传感器进行初始校准。然后通过串口通信将测量数据(角速度、加速度数据)传输到上位机保存。
[0014] 步骤4：根据步骤3中IMU测得的角速度、加速度信息，利用卡尔曼滤波思想求解得到基于IMU的末端姿态角。
[0015] 1)根据陀螺仪建立状态方程，进行状态更新(先验估计)。根据捷联惯性导航理论，建立四元数微分方程：
[0016]
[0017] 其中，q(t)＝[q0,q1,q2,q3]，表示姿态四元数，Mω(t)为b系相对于n系的角速度矩阵，由IMU中的陀螺仪测量得到：
[0018]
[0019] ωx(t),ωy(t),ωz(t)为t时刻末端惯性坐标系X，Y，Z三个坐标轴方向上的角速度。
[0020] 将四元数微分方程展开矩阵形式为：
[0021]
[0022] 其中，ωx,ωy,ωz为X，Y，Z三个坐标轴方向上陀螺仪测得的角速度。
[0023] 利用陀螺仪数据对姿态进行先验状态估计，采用四阶龙格库塔法求解微分方程，得到先验状态方程为：
[0024]
[0025] 其中， 为先验状态估计， 为前一次更新计算得到的四元数，T为采样时间，I为单位矩阵。
[0026] 则状态转移矩阵为Gk：
[0027]
[0028] 然后计算先验状态协方差：
[0029]
[0030] 其中， 为先验状态协方差，Pk‑1为前一次迭代的后验协方差，Q为系统噪声协方差，来源于陀螺仪。
[0031] 2)根据加速度计数据建立量测方程，进行量测更新(后验校正)。四元数法表示的b系到n系坐标系转换矩阵为：
[0032]
[0033] 结合矩阵 得到加速度计数据在末端b系下投影，即加速度量测方程为：
[0034]
[0035] 对每一个自变量q0,q1,q2,q3求偏导，得到量测方程的雅克比矩阵：
[0036]
[0037] 结合先验状态协方差 雅克比矩阵Hk计算卡尔曼增益Kk：
[0038]
[0039] 其中，R为量测噪声协方差，来源于加速度计。
[0040] 根据加速度计实际测量值来更新后验状态：
[0041]
[0042] 其中， 为后验状态估计，zk为加速度计实际测量值。
[0043] 最后更新后验协方差：
[0044]
[0045] 其中，Pk为后验协方差，I为单位矩阵。
[0046] 3)结合四元数矩阵解得基于IMU的末端姿态角：
[0047]
[0048] 步骤5：利用卡尔曼滤波算法对由安装在机器人末端的IMU测量解算得到的姿态角和由机器人本体参数通过D‑H建模解算得到的末端姿态角进行融合。融合过程中，选取末端姿态角A、B、C作为系统的状态向量：
[0049] x＝[A,B,C]
[0050] 将基于末端外部IMU得到的姿态角作为预测信息，将基于机器人本体得到的末端姿态角作为量测信息。
[0051] 1)状态更新
[0052] 根据IMU解算得到的姿态角进行先验状态估计并计算先验协方差：
[0053]
[0054] 其中， 为k时刻的先验状态估计，Gk为状态转移矩阵，Gk＝I3×3。
[0055]
[0056] 其中， 为先验协方差，Pk‑1为前一次迭代的后验协方差，Q为过程噪声协方差，来源于陀螺仪和加速度计：ωw为陀螺仪噪声，ωa为加速度计噪声。
[0057] 2)量测更新
[0058] 根据机器人本体参数建模解算得到的姿态角进行量测更新。首先计算卡尔曼增益Kk：
[0059]
[0060] 其中，R为量测噪声协方差，来源于机器人本体，Hk＝I3×3，I表示单位矩阵。
[0061] 然后基于机器人本体参数解算结果来更新后验状态：
[0062]
[0063] 其中，zk为基于机器人本体参数解算的末端姿态值。 为先验状态估计姿态角，为基于IMU与机器人本体参数融合后的末端姿态角：
[0064] 最后更新后验协方差：
[0065]
[0066] 其中，I表示单位矩阵。
[0067] 本发明的有益效果为：本发明将基于IMU与机器人本体参数得到的姿态角信息作为状态量进行融合，解算得到末端姿态角：A—横滚角(roll)、B—俯仰角(pitch)、C—偏航角(yaw)。与机器人本体信息融合的思想不仅可以降低由惯性传感器的漂移以及积分计算造成的累积误差，提高姿态精度；而且避免了添加外部大型测量设备，如视觉、光学测量仪器。该方法不受测量空间大小的影响，外部只需体积较小的惯性传感器即可实现高精度姿态测量，成本低，实时性好，简易便携，不仅能在实验室环境下测量，也适用于复杂生产现场的测量应用。
[0068] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面对附图做简单介绍：
附图说明
[0069] 图1为基于IMU与机器人本体参数融合的原理框图。
[0070] 图2为测量系统结构示意图。
[0071] 图3为KUKAKR6R700机器人坐标系旋转示意图。
[0072] 图4为KUKAKR6R700机器人D‑H模型图。
[0073] 图5为单IMU测量和基于IMU与机器人本体参数融合的结果对比图。具体实施方式：

[0074] 下面结合附图和具体实施方式对本发明作进一步详细说明。
[0075] 基于IMU与机器人本体参数融合的末端姿态测量方法原理框图如图1所示。通过外部IMU测量数据和机器人本体参数数据进行融合，得到基于IMU与机器人本体参数融合的姿态角。系统结构示意图如图2所示。
[0076] 步骤1：本实施例以KUKAKR6R700型工业机器人为测量对象。其基坐标系和工具坐标系的转换关系示意图如图3所示。工业机器人的姿态定义为工具坐标系(b系)相对于机器人基坐标系(n系)的转换关系。定义欧拉角旋转顺序为Z‑Y‑X，依次绕Z轴旋转A角度，绕Y轴旋转B角度，绕X轴旋转C角度。得到由欧拉角表示的旋转矩阵：
[0077]
[0078] 步骤2：建立D‑H运动学模型，通过使用连杆长度ai、连杆扭转角αi、连杆偏移量di、关节角θi四个D‑H参数表示串联型工业机器人各连杆之间的坐标系转换关系。通过D‑H模型可以获得机器人基坐标系与末端工具坐标系之间的转换关系。通过两次绕轴旋转和两次沿轴平移将Ci转换到Ci+1连杆坐标系，转换矩阵为：
[0079]
[0080] 其中，Rot(Z,θi)表示绕Ci坐标系的Z轴旋转θ°，Trans(Z,di)表示沿坐标系的Z轴平移距离dmm，Trans(X,ai)表示沿坐标系的X轴平移距离amm，Rot(X,αi)表示绕坐标系X轴旋转α°。
[0081] 四个D‑H参数中，连杆长度、连杆扭转角和连杆偏移量为系统参数，根据机器人结构获取并进行标定。关节角通过实时读取机器人各轴编码器数据获得，需要配置机器人通讯接口。配置过程为：采用基于以太网的通信方式，配置KUKA工业机器人内部集成的通讯接口RSI(RobotSensorInterface)对象ETHERNET的输入输出，以XML的形式与外部系统进行交互。配置传感器系统和接口的连接参数、数据交换方向、发送结构，设置通讯周期为12ms。编程控制机器人运动，实时读取机器人各关节角[θ1，θ2，θ3，θ4，θ5，θ6]并保存。初始各关节角为[0,90,‑90,0,0,0](单位：°)。

[0082] 本发明的实施例对象KUKAKR6R700型号工业机器人，其关节角运动范围如表1，其D‑H参数如表2。机器人D‑H模型示意图如图4所示。
[0083] 表1关节角运动范围
[0084]
[0085] 表2KUKAKR6R700机器人D‑H参数表
[0086]
[0087] 将通过RSI通信读取的关节角[θ1，θ2，θ3，θ4，θ5，θ6]及其它三个连杆参数代入转换矩阵，通过下式即可求得机器人末端矩阵：
[0088]
[0089] 其中，Ti(i＝1～6)为各连杆坐标系之间的转换矩阵，具体为：
[0090]
[0091]
[0092]
[0093]
[0094]
[0095]
[0096] 矩阵T中的前三行三列共九个元素即为末端姿态角信息。通过将矩阵T中的末端姿态元素与矩阵 对比，可求解出基于机器人本体参数的末端姿态角：
[0097]
[0098] 其中，nij为矩阵T中的各元素：
[0099] n11＝cosθ1[(‑cosθ4cosθ5+sinθ5sinθ6)cos(θ2+θ3)‑sinθ5cosθ6sin(θ2+θ3)]‑sinθ1(cosθ4sinθ6+sinθ4cosθ5)
[0100] n21＝sinθ1[(sinθ4sinθ6‑cosθ4cosθ5)cos(θ2+θ3)‑sinθ4sinθ6sin(θ2+θ3)]‑cosθ1(cosθ4sinθ6‑sinθ4cosθ5)
[0101] n31＝sin(θ2+θ3)[sinθ4sinθ6+cosθ4cosθ5cosθ6]+cos(θ2+θ3)sinθ5cosθ6[0102] n12＝‑cosθ1{sinθ5sinθ6sin(θ2+θ3)+cos(θ2+θ3)[sinθ4cosθ6‑cosθ4cosθ5]}+sinθ1(cosθ4sinθ5+cosθ5cosθ6)
[0103] n22＝sinθ1{sinsinθ6sin(θ2+θ3)‑cos(θ2+θ3)[sinθ4cosθ6‑cosθ4cosθ5]}‑cosθ1cos(θ4‑θ5)
[0104] n32＝(sinθ4cosθ6‑cosθ4cosθ5sinθ6)sin(θ2+θ3)‑sinθ5sinθ6cos(θ2+θ3)[0105] n13＝‑cosθ1[sin(θ2+θ3)cosθ5+cos(θ2+θ3)cosθ4sinθ5]+sinθ1sinθ4sinθ5[0106] n23＝sinθ1sin(θ2+θ3‑θ5)‑cosθ1sinθ4sinθ5
[0107] n33＝cos(θ2+θ3)cosθ5‑sin(θ2+θ3)cosθ4sinθ5
[0108] 步骤3：在机器人末端搭建惯性测量系统，获取实时惯性测量数据。本实施例采用JY901型号惯性传感器进行数据采集。该传感器内部集成高精度陀螺仪、加速度计。传感器支持串口和IIC两种数字接口，最高200Hz数据输出速率且0.1‑200Hz可调节。角速度输出稳定性为0.05°/s，加速度输出稳定性为0.01g。将惯性传感器安装到机器人末端法兰盘中心，采用串口通信方式，通过串口转换模块连接至上位机。陀螺仪和加速度计量程设置为2000°/s和16g，采样频率设置为100Hz。

[0109] 采集数据前，进行加速度计水平校准：将传感器静止放置在水平面上读取X，Y，Z三‑3个坐标轴方向的补偿值分别为：‑19，2，65(单位：10 g)，将补偿值写入上位机进行校准。然后编程控制机器人进行运动到达设定的测量点，实时测量并保存角速度、加速度数据用于进行惯性姿态解算。

[0110] 步骤4：根据IMU测得的角速度、加速度数据进行姿态解算。安装时保持IMU坐标系与机器人工具坐标系一致，由于夹具加工误差、安装误差等原因，两坐标系间存在微小误差R：
[0111]
[0112] 其中，δx、δy、δz表示X，Y，Z三个坐标轴方向的误差。
[0113] 采用矢量投影法求解出误差矩阵，通过误差矩阵R将惯性测量数据转换到工具坐标系下。以东北天大地坐标系作为导航坐标系，机器人基坐标系与大地坐标系一致。欧拉角表示姿态时的旋转顺序为Z‑Y‑X。求解姿态即求解两个坐标系之间的转换关系。采用四元数法进行姿态更新求解。求解姿态角过程如下：
[0114] 1)根据陀螺仪建立状态方程，进行状态更新(先验估计)。
[0115] 首先根据捷联惯性导航理论，建立四元数微分方程：
[0116]
[0117] 其中，q(t)为t时刻的姿态四元数，q(t)＝[q0,q1,q2,q3]，Mω(t)为b系相对于n系的角速度矩阵，由IMU中的陀螺仪测量得到：
[0118]
[0119] 其中，ωx(t),ωy(t),ωz(t)为t时刻末端工具坐标系X，Y，Z三个坐标轴方向上的角速度。
[0120] 将四元数微分方程展开成矩阵形式：
[0121]
[0122] 其中，ωx,ωy,ωz为陀螺仪在三个坐标轴方向测得的角速度(单位：°/s)。
[0123] 采用四阶龙格库塔法求解微分方程，得到其近似解，由此建立先验状态方程：
[0124]
[0125] 其中， 为先验状态估计， 为前一次计算得到的四元数值，T为采样周期，I为单位矩阵。状态转移矩阵为Gk：
[0126]
[0127] 最后进行状态更新，并计算先验状态协方差：
[0128]
[0129]
[0130] 其中， 为先验状态协方差，Pk‑1为前一次迭代的后验协方差，Q为系统噪声协方差，来源于陀螺仪。
[0131] 2)根据加速度计的数据进行量测更新，即后验校正。四元数法表示的b系到n系坐标系转换矩阵为：
[0132]
[0133] 当加速度计水平静止时，X，Y，Z三个坐标轴方向上加速度表示为[axayaz]T＝[0T01]。结合四元数矩阵 得到加速度计数据在b系下投影，即加速度量测方程h(qk)为：

[0134]
[0135] 对每一个自变量q0,q1,q2,q3求偏导，得到量测方程的雅克比矩阵Hk：
[0136]
[0137] 然后计算卡尔曼增益Kk：
[0138]
[0139] 其中，R为量测噪声协方差，来源于加速度计。
[0140] 根据加速度计实际测量值来更新后验状态：
[0141]
[0142] 其中，zk为加速度计实际测量值(单位：g)。 为后验状态估计值。
[0143] 最后更新后验协方差：
[0144]
[0145] 其中，Pk为后验协方差，I为单位矩阵。
[0146] 3)对比四元数矩阵 最终即可求得基于IMU的末端姿态角为：
[0147]
[0148] 步骤5：根据步骤二和步骤四得到的两组机器人末端姿态角计算结果，利用扩展卡尔曼滤波算法进行融合。融合过程中，选取三个姿态角作为系统的状态向量，即：
[0149] x＝[A,B,C]
[0150] 利用基于末端外部IMU得到的姿态角作为预测信息进行状态更新，利用基于机器人本体参数计算得到的末端姿态角作为量测信息进行量测更新。
[0151] 1)状态更新
[0152] 根据IMU解算得到的姿态角建立状态方程为：
[0153] x＝[Aimu,Bimu,Cimu]
[0154] 利用IMU状态方程对姿态进行先验状态估计并计算先验状态协方差：
[0155]
[0156]
[0157] 其中，Gk＝I3×3，Gk为状态转移矩阵。 Q为系统噪声协方差，来源于陀螺仪和加速度计，ωw为陀螺仪噪声，ωa为加速度计噪声。

[0158] 2)量测更新
[0159] 首先需要计算卡尔曼增益：
[0160]
[0161] 其中，Hk＝I3×3，R为量测噪声协方差，来源于机器人本体。
[0162] 根据利用机器人本体参数运动学建模解算得到的末端姿态角建立量测方程，进行后验状态更新：
[0163] zk＝[Arsi,Brsi,Crsi]
[0164]
[0165] 其中，zk为基于机器人本体参数解算的末端姿态值。 为先验状态估计姿态角，为基于IMU与机器人本体参数融合后的末端姿态角： 最后更新后验状态协方差：

[0166]
[0167] 其中，I表示单位矩阵。由此可以得到基于IMU与机器人本体参数融合的末端姿态角：Aekf,Bekf,Cekf。
[0168] 利用本发明方法，选取测量点对末端姿态进行测量解算。为了分析本发明方法的有效性和测量精度，本实施例中，编程控制机器人轴1、轴4、轴5分别转动30°，并以单独采用IMU进行测量的数据进行比对，验证基于IMU与机器人本体参数融合的姿态测量方法的精度，三个测量点的姿态解算结果见表3。以测量点1为例进行分析，单独使用IMU测量解算和基于IMU与机器人本体参数融合解算的结果对比如图5所示。其中，蓝色线表示IMU解算结果，红色线表示IMU与本体参数融合解算结果。可以看出，采用本发明方法融合解算的姿态比IMU测量解算的姿态误差明显减小，准确度大大提高，融合后三个姿态角误差比融合前分别减小了68.33％，61.93％，65.27％，误差均小于0.2°。
[0169] 表3机器人末端姿态融合解算结果表
[0170]
[0171] 本实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应在本发明的保护范围之内。

专利地区：北京

专利申请日期：2021-11-27

专利公开日期：2023-12-08

专利公告号：CN114216456B