技术领域
[0001] 本
发明属于机器人标定技术领域，更具体地，涉及一种用于机器人参数辨识的空间雅克比矩阵构造方法。
背景技术
[0002]
工业机器人的运动
精度对其在生产中的应用可靠性起着至关重要的作用。机器人各
连杆的几何参数误差是造成机器人
定位误差的最主要环节，主要源自制造和安装过程中连杆实际几何参数与理论参数值之间的偏差，一般被视为系统误差。
[0003] 对机器人的各连杆几何参数误差进行辨识是提高机器人运动精度的重要手段，参数辨识的主要工作是构建机器人参数误差向机器人末端误差的变换关系，具体分为两步，首先构建机器人参数误差向关节
坐标系误差的变换关系Gi，然后构建
机器人关节坐标系误差向末端误差的变换关系Ji(即空间雅克比矩阵或物体雅克比矩阵)。得到了机器人参数误差向末端误差的变换关系后通过最小二乘法等方法可求解机器人的参数误差值。现有的构造机器人空间雅克比矩阵的方法中，一般首先构造机器人的物体雅克比矩阵，而后利用二者之间的变换关系构造出空间雅克比矩阵，参数辨识
算法的时间消耗比较长，难以适应机器人标定的技术需求。
发明内容
[0004] 针对
现有技术的以上
缺陷或改进需求，本发明提供一种用于机器人参数辨识的空间雅克比矩阵构造方法，基于微分变换原理和虚拟坐标系法的机器人空间雅克比矩阵构造方法，较传统方法而言具有更低的时间消耗，能够快速构造机器人的空间雅克比矩阵，为实现机器人的参数辨识提供了有利条件。
[0005] 为了实现上述目的，本发明提供一种用于机器人参数辨识的空间雅克比矩阵构造方法，包括如下步骤：
[0006] S100:分析机器人D-H模型中杆长、扭
角、偏距和关节角对机器人连杆及关节之间关系的影响，利用平移和旋转算子，计算机器人相邻关节的齐次变换矩阵，构建机器人
运动学模型；
[0007] S200:基于
机器人运动学模型，分析机器人关节微分运动特性，建立微分运动情况下坐标系间的齐次变换矩阵；
[0008] S300:假设某关节坐标系发生了微分运动,在此
基础上建立与该关节对应的虚拟坐标系，并构建二者之间的变换矩阵，进而计算机器人末端相对于基坐标系的实际
位姿，与原理论位姿进行比较，获得该关节坐标系的运动量误差导致的机器人末端相对于基坐标系的位姿误差，构造机器人的空间雅克比矩阵。
[0009] 进一步地，步骤S200中，所述齐次变换矩阵的建立包括如下具体步骤：
[0010] S201：微分运动中平移和转动的幅度较小，所以在微分变换对机器人的角度运算进行近似处理，当关节角很小时sinθ≈θ,cosθ≈1；
[0011] S202：机器人关节i-1到关节i做微分旋转运动δ＝[δx δy δz]T和平移运动d＝[dx dy dz]T，所述微分旋转运动δ＝[δx δy δz]T的变换矩阵为：
[0012]
[0013] 其中，k＝[kx ky kz]T,δx＝kxδθ,δy＝kyδθ,δz＝kzδθ；
[0014] 所述平移运动d＝[dx dy dz]T对应的变换矩阵为：
[0015]
[0016] 进一步地，通过微分旋转运动和平移运动，构建机器人关节坐标系之间微分运动的总变换矩阵为:
[0017]
[0018] 进一步地，步骤S200中还包括：
[0019] S203：引入反对称矩阵[δ]：
[0020]
[0021] 进一步地，S300包括如下具体步骤：
[0022] S301：分析机器人关节坐标系的误差形式，并引入虚拟坐标系以示意关节坐标系的位姿误差；
[0023] S302：利用微分变化法的基本原理，求取所述虚拟坐标系与原关节坐标系之间的齐次变换关系；
[0024] S303：求取引入了虚拟坐标系后机器人末端坐标系相对于基坐标系的位姿关系，并与原位姿关系进行比较，得出机器人末端坐标系的位姿偏差；
[0025] S304：根据机器人末端坐标系的位姿偏差，构造出机器人的空间雅克比矩阵。
[0026] 进一步地，S304中所述机器人的空间雅克比矩阵J(q)为：
[0027]
[0028] 式中：Jli和Jai分别表示关节i的单位关节运动量引起的
末端执行器的平移量和转动量。
[0029] 进一步地，S301中，机器人末端关节n相对于基坐标系的变换矩阵记为
[0030]
[0031] 式中： 分别表示坐标系n、i、n相对于坐标系0、0、i的变换矩阵；分别表示 中的旋转矩阵；0Pi0、iPn0、0Pn0分别表示 中的
位置向量。
[0032] 进一步地，S302中，假设坐标系i发生微分运动后到达了虚拟坐标系i'处，则坐标系n运动至n'处，此时坐标系n'相对于基坐标系的变换矩阵为
[0033]
[0034] 式中： 分别表示坐标系i、i′、n′相对于坐标系o、i、i′的变换矩阵；0 i i′
分别表示 中的旋转矩阵；Pi0、Pi′0、Pn′0分别表示 中的位置向量。
[0035] 进一步地，S303中，所述机器人末端坐标系的位姿偏差为：
[0036] 0Dni＝[0dniT 0δniT]T
[0037] 其中，0dni为位置偏差，其等于最终状态下坐标系n的原点在基坐标系下的位置0Pno'减去初始状态下坐标系n的原点在基坐标系下的位置0Pno；0δni为
姿态偏差，表示 的旋转矩阵。
[0038] 进一步地，S100中，机器人运动学模型为机器人连杆坐标系i相对于连杆坐标系i-1的变换矩阵：
[0039]
[0040] 其中：ai-1为连杆i-1的长度；αi-1为连杆i-1的扭角；di为连杆i相对连杆i-1的偏距；θi为连杆i相对连杆i-1绕轴线i的旋转角度；Rot(x,αi-1)表示绕坐标系的x轴旋转角度αi-1所对应的齐次变换矩阵，具体为：
[0041]
[0042] Trans(x,ai-1)表示绕坐标系的x轴平移距离ai-1所对应的齐次变换矩阵，具体为：
[0043]
[0044] Rot(z,θi)表示绕坐标系的z轴旋转角度θi所对应的齐次变换矩阵，具体为[0045]
[0046] Trans(z,di)表示绕坐标系的z轴平移距离di所对应的齐次变换矩阵，具体为：
[0047]
[0048] 总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：
[0049] 1.本发明的空间雅克比矩阵构造方法，基于微分变换原理和虚拟坐标系法的机器人空间雅克比矩阵构造方法，较传统方法而言具有更低的时间消耗，能够快速构造机器人的空间雅克比矩阵，为实现机器人的参数辨识提供了有利条件。
[0050] 2.本发明的空间雅克比矩阵构造方法，利用微分变换的基本原理，分析机器人关节微分运动的特性，建立微分运动情况下坐标系间的齐次变换矩阵，将误差的产生理解为坐标系微分运动的结果，方便地建立了坐标系误差的数学描述方法，为求解末端位姿误差打下了基础。
[0051] 3.本发明的空间雅克比矩阵构造方法，建立与关节对应的虚拟坐标系，并推导二者之间的变换矩阵，进而计算机器人末端相对于基坐标系的实际位姿，与原理论位姿进行比较，构造出机器人的空间雅克比矩阵。
附图说明
[0052] 图1为本发明
实施例用于机器人参数辨识的空间雅克比矩阵构造方法的
流程图；
[0053] 图2是本发明实施例中机器人连杆的描述示意图；
[0054] 图3是本发明实施例中连杆之间的连接方式描述示意图；
[0055] 图4是本发明实施例中机器人连杆坐标系描述示意图；
[0056] 图5是本发明实施例中基于微分变换法求解齐次变换矩阵的流程图；
[0057] 图6是本发明实施例中微分变换过程示意图；
[0058] 图7是本发明实施例中关节坐标系微分运动及虚拟坐标系示意图；
[0059] 图8是本发明实施例中基于虚拟坐标系法构造空间雅克比矩阵的流程图。
具体实施方式
[0060] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
[0061] 如图1所示，本发明实施例提供一种用于机器人参数辨识的空间雅克比矩阵构造方法，包括如下步骤：
[0062] S100：基于D-H方法建立机器人的运动学模型。分析D-H模型中杆长a、扭角α、偏距d和关节角θ对机器人连杆及关节之间关系的影响；利用平移和旋转算子，计算机器人相邻关节的齐次变换矩阵，从而得出机器人的运动学模型；
[0063] S200：利用微分变换的基本原理，推导微分运动情况下，坐标系之间的齐次变换矩阵。分析机器人关节微分运动的特性，建立微分运动情况下坐标系间的齐次变换矩阵；
[0064] S300：利用虚拟坐标系法推导机器人空间雅克比矩阵公式。假设某关节坐标系发生了微分运动，在此基础上建立与该关节对应的虚拟坐标系，并推导二者之间的变换矩阵，进而计算机器人末端相对于基坐标系的实际位姿，与原理论位姿进行比较，最终构造出机器人的空间雅克比矩阵。
[0065] 具体而言，包括如下步骤：
[0066] (1)基于D-H方法建立机器人的运动学模型。
[0067] 如图2所示，连杆i-1是由关节轴线i-1和关节轴线i的公法线长度ai-1以及两轴线之间的夹角αi-1所确定的。ai-1称为连杆i-1的长度，其方向规定为由关节i-1指向关节i；αi-1称为连杆i-1的扭角，其方向规定为从轴线i-1绕公法线转至轴线i的平行线。
[0068] 如图3所示，相邻连杆之间有一条关节轴线，相应地，每一条关节轴线有两条公法线与其垂直，这两条公法线(连杆)之间的距离称为连杆的偏距，记为di，代表连杆i相对连杆i-1的偏距；这两条公法线(连杆)之间的夹角称为关节角，记为θi，表示连杆i相对连杆i-1绕轴线i的旋转角度。
[0069] 如图4所示，为了确定机器人各连杆之间的相对运动和位姿关系，在每一个连杆上固接一个坐标系。与
基座(连杆0)固接的称为基坐标系，与连杆i固接的称为坐标系i。
[0070] 对于中间坐标系i，规定其zi轴与关节轴线i共线，指向任意；其xi轴与ai共线，由关节i指向i+1，当ai＝0时，取xi＝±zi+1×zi；其yi轴按照右手法则确定；坐标系的原点oi取在xi和zi的交点处，当zi+1、zi相交时，oi取在交点处，当zi+1、zi平行时，oi取在使di+1＝0的地方。
[0071] 对于首端和末端连杆坐标系，一般规定坐标系0的z轴沿关节轴线1的方向，当关节变量1为零时，坐标系0、1重合。末端连杆坐标系的规定与坐标系0类似，对于旋转关节n，选取xn使得当θn＝0时，xn、xn-1重合，坐标系n原点的选择使dn＝0；对于
移动关节n，规定坐标系n使θn＝0，且当dn＝0时，xn、xn-1重合。
[0072] 基于上述机器人连杆坐标系的建立方法可知，连杆坐标系i可以看作是连杆坐标系i-1通过以下变换而来的：
[0073] (a)绕xi-1轴转动αi-1角；(b)沿xi-1轴移动ai-1；(c)绕zi轴转θi角；(d)沿zi轴移动di。
[0074] 根据机器人运动学原理可知，机器人连杆坐标系i相对于连杆坐标系i-1的变换矩阵可由下式求得：
[0075]
[0076] 其中：ai-1为连杆i-1的长度；αi-1为连杆i-1的扭角；di为连杆i相对连杆i-1的偏距；θi为连杆i相对连杆i-1绕轴线i的旋转角度；Rot(x,αi-1)表示绕坐标系的x轴旋转角度αi-1所对应的齐次变换矩阵，具体为：
[0077]
[0078] Trans(x,ai-1)表示绕坐标系的x轴平移距离ai-1所对应的齐次变换矩阵，具体为：
[0079]
[0080] Rot(z,θi)表示绕坐标系的z轴旋转角度θi所对应的齐次变换矩阵，具体为[0081]
[0082] Trans(z,di)表示绕坐标系的z轴平移距离di所对应的齐次变换矩阵，具体为：
[0083]
[0084] 具体为：
[0085]
[0086] 采用此建模方法建立机器人的运动学模型。
[0087] (2)微分变换的基本原理
[0088] 微分变换法的基本原理如图5所示，由于微分运动中，平移和转动的幅度较小，所以在微分变换对角度运算进行近似处理，即sinθ≈θ,cosθ≈1，经过此近似处理后，可进一步推导出微分运动前后坐标系间的齐次变换矩阵，观察该齐次变换矩阵的构成可知，其中的旋转矩阵为微分转动量对应的反对称矩阵，故引入反对称矩阵以简化该齐次变换矩阵的表示。下面结合图示具体阐述微分变换法的基本原理。
[0089] 如图6所示，假设坐标系B最初与坐标系A重合，之后B相对于A依次发生了微分旋转运动δ＝[δx δy δz]T和平移运动d＝[dx dy dz]T到达B'。根据机器人运动学原理，绕x轴、y轴和z轴旋转θ角的旋转变换矩阵依次为：
[0090]
[0091]
[0092]
[0093] 当θ很小时，可以近似得到如下公式：
[0094] sinθ＝θ,cosθ＝1 (10)
[0095] 则对于B相对于A发生的微分旋转运动δ＝[δx δy δz]T，可得绕各单轴的微分转动变换为：
[0096]
[0097]
[0098]
[0099] 微分旋转运动的总变换可以看做是以上三种变换的复合作用，如公式(11)所示。其中，等价
转轴k＝[kx ky kz]T，等价微分转角δθ与δx，δy，δz之间的关系为：
[0100] δx＝kxδθ，δy＝kyδθ，δz＝kzδθ (14)
[0101]
[0102] 微分转动变换矩阵相乘符合交换律，即算子Rot(x,δx)、Rot(y,δy)、Rot(z,δz)的顺序可以任意调换。
[0103] 为了便于后续计算，将微分转动变换矩阵扩充为4阶的齐次变换矩阵如下：
[0104]
[0105] 当B相对于A发生了微分旋转运动后，又相对于A发生了平移运动d＝[dx dy dz]T，平移运动对应的变换矩阵为：
[0106]
[0107] 因为B始终相对于A(参考系)做微分运动，所以计算总的变换矩阵时采用左乘规则，则微分运动[dT δT]T对应的总变换矩阵为：
[0108]
[0109] 实际计算中，我们往往将δ＝[δx δy δz]T的反对称矩阵记为[δ]，可表示为：
[0110]
[0111] 则 可表示为：
[0112]
[0113] 式中：δ＝[δx δy δz]T，d＝[dx dy dz]T，E为3阶单位矩阵。
[0114] (3)利用虚拟坐标系法推导机器人空间雅克比矩阵公式
[0115] 本发明实施例基于虚拟坐标系法构造机器人的空间雅克比矩阵，如图8所示，首先分析机器人关节坐标系的误差形式(位置误差和姿态误差)，将该误差的产生理解为机器人关节坐标系微分运动的结果，并引入虚拟坐标系以示意关节坐标系的位姿误差。之后利用微分变化法的基本原理，求解虚拟坐标系与原关节坐标系(无误差的、理论的坐标系)之间的齐次变换关系，进而推导引入了虚拟坐标系后机器人末端坐标系相对于基坐标系的位姿关系，并与原位姿关系(无误差的、理论的位姿关系)进行比较，得出机器人末端坐标系的位姿偏差，观察该位姿偏差的构成，可构造出机器人的空间雅克比矩阵。下面结合配图具体阐述基于虚拟坐标系法构造机器人空间雅克比矩阵的基本原理。
[0116] 机器人运动学中规定，机器人的速度雅可比矩阵J(q)表示从关节速度矢量 到操作速度矢量 的线性映射。由于速度可以看作是单位时间内的微分运动，因此，速度雅可比矩阵可以看成是关节空间的微分运动dq向操作空间的微分运动D之间的转换矩阵。机器人的物体雅克比矩阵是指关节运动量误差向末端相对于其理论位姿的运动量误差(相对于末端工具坐标系表示)的转换矩阵；空间雅克比矩阵则是指关节运动量误差向末端相对于基坐标系的运动量误差(相对于基坐标系表示)的转换矩阵。机器人的物体雅克比矩阵与空间雅克比矩阵可相互转化。即
[0117] D＝J(q)dq (21)
[0119]
[0120] 式中：Jli和Jai分别表示关节i的单位关节运动量引起的末端执行器的平移量和转动量。末端微分运动量D＝[dT δT]T在末端工具坐标系中表示时，对应的雅可比矩阵记为TJ(q)，称为物体雅可比矩阵；D＝[dT δT]T在基坐标系中表示时，对应的雅可比矩阵记为J(q)，称为空间雅可比矩阵。
[0121] 此处假设关节坐标系i对应的D-H参数ai-1，αi-1，di及θi产生了微小变化量Δai-1，Δαi-1，Δdi及Δθi，从而导致坐标系i相对于其理论位姿发生了微分转动量δi和微分平移量T T Tdi，即产生了微分运动Di＝[di δi ] ，该微分运动会导致机器人末端坐标系n发生微分运动，使得坐标系n相对于基坐标系(坐标系0)的位姿发生变化。
[0122] 如图7所示，初始状态下，将机器人末端关节n相对于基坐标系的变换矩阵记为根据机器人运动学原理，有下述关系：
[0123]
[0124] 式中： 分别表示坐标系n、i、n相对于坐标系o、o、i的变换矩阵；分别表示 中的旋转矩阵；0Pi0、iPn0、0Pn0分别表示 中的位置向量。
[0125] 假设坐标系i发生微分运动后到达了坐标系i'(虚拟坐标系)处，相应地，坐标系n运动至n'处。此时将坐标系n'相对于基坐标系的变换矩阵记为 则：
[0126]
[0127] 因为在此过程中，坐标系n相对于坐标系i的位姿始终保持不变，故
[0128]
[0129] 将式(25)代入式(24)得：
[0130]
[0131] 根据前述内容可知，坐标系i'相对于坐标系i的变换矩阵 可由微分变换求得，已知微分运动量为Di＝[diT δiT]T，则：
[0132]
[0133] 将式(27)代入(25)可得：
[0134]
[0135] 特别地，式中： 及0Pio与式(23)中的相应元素一致。
[0136] 将 记作
[0137]
[0138] 由式(28)与(29)对应相等可得：
[0139]
[0140]
[0141] 至此，我们只需将式(28)中的 与式(23)中的 比较，即可得到关节坐标系i的微分运动Di＝[diT δiT]T导致末端坐标系n相对于基坐标系产生的位姿偏差0Dni＝[0dniT 0δniT]T，其中0dni为位置偏差，0δni为姿态偏差。
[0142] 对于位置偏差0dni的计算，我们只需利用最终状态下坐标系n的原点在基坐标系下的位置0Pno'减去初始状态下坐标系n的原点在基坐标系下的位置0Pno即可，由式(31)可得：
[0143]
[0144] 对于姿态偏差0δni的计算，分析公式(30)可知， 由 左乘 得到，这说明末端坐标系n在初始姿态的基础上又相对于基坐标系发生了微分转动，再结合公式(20)可知，该微分转动量0δni为：
[0145]
[0146] 将公式(32)、(33)
整理成矩阵形式可得：
[0147]
[0148] 与公式(22)对比可知：
[0149]
[0150] 据此可利用公式(22)求得机器人的空间雅克比矩阵。值得说明的是，0Dni是在机器人基坐标系下描述的，所以根据公式(22)求出的是机器人的空间雅克比矩阵。
[0151] 本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内所作的任何
修改、等同替换和改进等，均应包含在本发明的保护范围之内。
