一种高能效轻量化结构双足机器人的逆运动学求解方法转让专利
申请号 : CN202010722914.4
文献号 : CN111914416B
文献日 : 2021-03-30
发明人 : 袁海辉 , 施佳晨 , 顾建军 , 朱世强
申请人 : 之江实验室
摘要 :
本发明公开了一种高能效轻量化结构双足机器人的逆运动学求解方法，该方法采用旋量法建立机器人坐标系，并通过构造可表征姿态信息的矢量，构建出了可用于逆运动学求解的表达式方程，进而采用牛顿拉夫逊算法对机器人的逆运动学进行数值求解；该方法还进一步考虑了驱动连杆的存在，并采用解析方法计算出驱动电机实际需要转动的角度。本发明可用于机器人髋关节电机轴不相交的情形下进行逆运动学求解。
权利要求 :
1.一种高能效轻量化结构双足机器人的逆运动学求解方法，其特征在于，包括以下步骤：
步骤一，采用旋量法建立机器人坐标系，并构造表征姿态信息的矢量，进而构建出用于逆运动学求解的表达式方程，根据期望的末端位置采用牛顿拉夫逊算法对机器人的逆运动学进行数值求解获得机器人各关节角度；
步骤二，基于步骤一得到的关节角度，采用几何方法计算出各关节驱动电机需要转动的角度；
所述步骤一具体为：
首先，利用旋量法建立机器人坐标系并获得机器人位姿的表达式；在初始状态下，各关节角度为0，各关节的局部坐标系与世界坐标系平行；机器人第i个关节角度qi相对第i‑1个关节角度qi‑1的齐次变换矩阵 为：其中，ai为关节角度qi的单位转轴向量，表示由ai生成的斜对称矩阵，bi表示关节角度qi‑1局部坐标系下qi局部坐标系原点的位置矢量， 的表达如下：其中，E为单位矩阵；基于齐次转换矩阵 机器人关节角度qi的位姿矩阵为Ti：其中，Ri为关节角度qi的姿态矩阵，pi为关节角度qi的绝对位置矢量，R0和p0分别为机器人起始杆件的位姿矩阵和绝对位置矢量；基于位姿矩阵Ti，令第j个关节角度qj为机器人的末端关节角度，则机器人末端的位姿矩阵Tj为：然后，基于机器人末端的位姿矩阵Tj，构建机器人逆运动学求解的表达式方程：f(q)＝fref
T
其中， q＝[q1,…,qj] 为机器人关节角度组成的向量，pref为末端位置矢量的期望值，向量vor构造如下：T
vor＝[M(1,1) M(2,2) M(3,3)]其中，矩阵 为机器人末端期望姿态矩阵Rref的转置；
接着，利用牛顿拉夫逊算法对方程f(q)＝fref进行求解，方程求解的迭代方程如下：‑1
Xk＝Xk‑1‑J (f(Xk‑1)‑fref)‑1
其中，Xk为向量q在第k次的迭代值； J 为J的逆，X0为关节角度向量q的初始估计值；当向量f(Xk)‑fref的欧几里得范数小于阈值λ时，迭代终止，最终得到关节角度向量q＝Xk。
2.如权利要求1所述高能效轻量化结构双足机器人的逆运动学求解方法，其特征在于，‑7
阈值λ＝10 。
3.如权利要求1所述高能效轻量化结构双足机器人的逆运动学求解方法，其特征在于，所述步骤二中，机器人踝关节驱动电机的输出曲柄、连杆以及起始杆件构成空间四边形，计算采用踝关节驱动电机需要转动的角度具体为：首先，找到该空间四边形中可基于局部坐标系表示的三个顶点，即连杆与起始杆件的连接点、电机输出曲柄与电机的连接点以及机器人起始杆件上的一个固定点，所述固定点位于电机输出曲柄的运动平面内，并在局部坐标系下对三个顶点进行表达；然后，将电机输出曲柄末端的位置表示为电机转动角度的函数，进而表示出连杆在局部坐标系下的位置向量；最后，根据连杆的长度，计算出踝关节驱动电机实际需要转动的角度。
说明书 :
一种高能效轻量化结构双足机器人的逆运动学求解方法
技术领域
[0001] 本发明属于机器人技术领域，尤其涉及一种具有高能效轻量化腿足结构双足机器人的逆运动学求解方法。
背景技术
[0002] 双足机器人是具有多个自由度的高维非线性系统，对其进行步态规划与控制是一项非常具有挑战性的工作。如何实现快速的步态规划一直是国内外研究人员重点关注的问
题，它关系到双足机器人的实时性与鲁棒性能。为实现高实时性的步态规划，国内外研究人
员目前一般先基于简化模型规划出机器人的质心(或躯干)以及落脚点轨迹，然后再利用逆
运动学求解出机器人腿足各驱动关节的轨迹。
题，它关系到双足机器人的实时性与鲁棒性能。为实现高实时性的步态规划，国内外研究人
员目前一般先基于简化模型规划出机器人的质心(或躯干)以及落脚点轨迹，然后再利用逆
运动学求解出机器人腿足各驱动关节的轨迹。
[0003] 目前，国内外研究人员在进行双足机器人结构设计时常使其髋关节的三个电机轴交于一点，包括硕士论文《仿人机器人结构设计与分析》所述的浙江大学的ZJUKong，以及最
近的期刊论文《Contact Force/Torque Control Based on Viscoelastic Model for
Stable Bipedal Walking on Indefinite Uneven Terrain》所述的北京理工大学的BHR‑
6P。当双足机器人髋关节的电机轴交于一点时，其逆运动学可采用解析方法进行几何求解，
如最近的硕士论文《GTX‑Ⅲ型双足机器人样机研制与行走实验研究》以及专利《基于闭环控
制的仿人机器人全向行走方法》(专利号CN201310060399.8)所述。随着双足机器人技术的
发展，国内外研究人员从追求走稳向追求走好发展。为实现高效步行，提高续航能力，国内
外研究人员开始考虑高能效轻量化腿足结构布局。在此类布局设计中，一般以减轻机器人
腿足结构的转动惯量作为主要优化目标，机器人驱动电机会往躯干方向布置以尽量减轻腿
足结构的质量，并采用平行连杆进行驱动。在此情形下，机器人髋关节的电机轴不一定交于
一点，从而难以采用几何解析方法进行逆运动学求解。此外，由于加入了连杆，如何从机器
人的关节角度计算出驱动电机实际需要转动的角度，也需要进一步考虑。
近的期刊论文《Contact Force/Torque Control Based on Viscoelastic Model for
Stable Bipedal Walking on Indefinite Uneven Terrain》所述的北京理工大学的BHR‑
6P。当双足机器人髋关节的电机轴交于一点时，其逆运动学可采用解析方法进行几何求解，
如最近的硕士论文《GTX‑Ⅲ型双足机器人样机研制与行走实验研究》以及专利《基于闭环控
制的仿人机器人全向行走方法》(专利号CN201310060399.8)所述。随着双足机器人技术的
发展，国内外研究人员从追求走稳向追求走好发展。为实现高效步行，提高续航能力，国内
外研究人员开始考虑高能效轻量化腿足结构布局。在此类布局设计中，一般以减轻机器人
腿足结构的转动惯量作为主要优化目标，机器人驱动电机会往躯干方向布置以尽量减轻腿
足结构的质量，并采用平行连杆进行驱动。在此情形下，机器人髋关节的电机轴不一定交于
一点，从而难以采用几何解析方法进行逆运动学求解。此外，由于加入了连杆，如何从机器
人的关节角度计算出驱动电机实际需要转动的角度，也需要进一步考虑。
发明内容
[0004] 本发明的目的在于针对现有技术的不足，提供一种高能效轻量化结构双足机器人的逆运动学求解方法。
[0005] 本发明的目的是通过以下技术方案来实现的：一种高能效轻量化结构双足机器人的逆运动学求解方法，包括以下步骤：
[0006] 步骤一，采用旋量法建立机器人坐标系，并构造表征姿态信息的矢量，进而构建出用于逆运动学求解的表达式方程，根据期望的末端位置采用牛顿拉夫逊算法对机器人的逆
运动学进行数值求解获得机器人各关节角度；
运动学进行数值求解获得机器人各关节角度；
[0007] 步骤二，基于步骤一得到的关节角度，采用几何方法计算出各关节驱动电机需要转动的角度。
[0008] 进一步地，所述步骤一具体为：
[0009] 首先，利用旋量法建立机器人坐标系并获得机器人位姿的表达式；在初始状态下，各关节角度为0，各关节的局部坐标系与世界坐标系平行；机器人第i个关节角度qi相对第
i‑1个关节角度qi‑1的齐次变换矩阵 为：
i‑1个关节角度qi‑1的齐次变换矩阵 为：
[0010]
[0011] 其中，ai为关节角度qi的单位转轴向量， 表示由ai生成的斜对称矩阵，bi表示关节角度qi‑1局部坐标系下qi局部坐标系原点的位置矢量， 的表达如下：
[0012]
[0013] 其中，E为单位矩阵；基于齐次转换矩阵 机器人关节角度qi的位姿矩阵为Ti：
[0014]
[0015] 其中，Ri为关节角度qi的姿态矩阵，pi为关节角度qi的绝对位置矢量，R0和p0分别为机器人起始杆件的位姿矩阵和绝对位置矢量；基于位姿矩阵Ti，令第j个关节角度qj为机器
人的末端关节角度，则机器人末端的位姿矩阵Tj为：
人的末端关节角度，则机器人末端的位姿矩阵Tj为：
[0016]
[0017] 然后，基于机器人末端的位姿矩阵Tj，构建机器人逆运动学求解的表达式方程：
[0018] f(q)＝frefT
[0019] 其中， q＝[q1,…,qj]为机器人关节角度组成的向量，pref为末端位置矢量的期望值，向量vor构造如下：
[0020] vor＝[M(1,1) M(2,2) M(3,3)]T
[0021] 其中，矩阵 为机器人末端期望姿态矩阵Rref的转置；
[0022] 接着，利用牛顿拉夫逊算法对方程f(q)＝fref进行求解，方程求解的迭代方程如下：
[0023] Xk＝Xk‑1‑J‑1(f(Xk‑1)‑fref)‑1
[0024] 其中，Xk为向量q在第k次的迭代值； J 为J的逆，X0为关节角度向量q的初始估计值；当向量f(Xk)‑fref的欧几里得范数小于阈值λ时，迭代终止，最终得到关节角
度向量q＝Xk。
度向量q＝Xk。
[0025] 进一步地，所述阈值λ＝10‑7。
[0026] 进一步地，所述步骤二中，机器人踝关节驱动电机的输出曲柄、连杆以及起始杆件构成空间四边形，计算采用踝关节驱动电机需要转动的角度具体为：首先，找到该空间四边
形中可基于局部坐标系表示的三个顶点，即连杆与脚掌的连接点、电机输出曲柄与电机的
连接点以及机器人起始杆件上的一个固定点；该固定点位于电机输出曲柄的运动平面内，
以便基于该固定点表达电机的转动角度；并在局部坐标系下对三个顶点进行表达；然后，将
电机输出曲柄末端的位置表示为电机转动角度的函数，进而表示出连杆在局部坐标系下的
位置向量；最后，根据连杆的长度为定值，计算出踝关节驱动电机实际需要转动的角度。
形中可基于局部坐标系表示的三个顶点，即连杆与脚掌的连接点、电机输出曲柄与电机的
连接点以及机器人起始杆件上的一个固定点；该固定点位于电机输出曲柄的运动平面内，
以便基于该固定点表达电机的转动角度；并在局部坐标系下对三个顶点进行表达；然后，将
电机输出曲柄末端的位置表示为电机转动角度的函数，进而表示出连杆在局部坐标系下的
位置向量；最后，根据连杆的长度为定值，计算出踝关节驱动电机实际需要转动的角度。
[0027] 本发明的有益效果在于：本发明可用于机器人髋关节电机轴不相交的情形，通过构造可表征姿态信息的矢量，构建出了可用于逆运动学求解的表达式方程，并进一步采用
牛顿拉夫逊算法对机器人的逆运动学进行数值求解；还进一步考虑了驱动连杆的存在，并
采用几何方法计算出驱动电机实际需要转动的角度。
牛顿拉夫逊算法对机器人的逆运动学进行数值求解；还进一步考虑了驱动连杆的存在，并
采用几何方法计算出驱动电机实际需要转动的角度。
附图说明
[0028] 图1为具有高能效轻量化腿足结构的双足机器人示意图；其中，a为机器人结构示意图，b为机器人左腿关节角度与尺寸示意图。
具体实施方式
[0029] 以下结合附图和实施例对本发明作进一步说明。
[0030] 本发明所提供的一种高能效轻量化结构双足机器人的逆运动学求解方法，包含如下步骤：
[0031] 步骤一，采用旋量法建立机器人坐标系，并构造出可表征姿态信息的矢量，进而构建出可用于逆运动学求解的表达式方程，并进一步采用牛顿拉夫逊算法对机器人的逆运动
学进行数值求解，以获得机器人各关节角度：
学进行数值求解，以获得机器人各关节角度：
[0032] 首先，利用旋量法建立具有高能效轻量化腿足结构双足机器人的坐标系，并获得机器人位姿的表达式。机器人各关节局部坐标系如图1所示，机器人起始杆件(即脚掌)绝对
T
位置矢量为p0，其姿态矩阵为R0；q1为踝关节俯仰角度，q1对应的转动轴矢量a1＝[1,0,0] 、
T T
相对位置矢量b1＝[0,0,0] ；q2为踝关节滚动角度，q2对应的转动轴矢量a2＝[0,1,0] 、相对
T T
位置矢量b2＝[0,0,0] ；q3为膝关节俯仰角度，q3对应的转动轴矢量a3＝[1,0,0] 、相对位置
T T
矢量b3＝[0,0,l1] ；q4为髋关节俯仰角度，q4对应的转动轴矢量a4＝[1,0,0] 、相对位置矢
T T
量b4＝[0,0,l2] ；q5为髋关节旋转角度，q5对应的转动轴矢量a5＝[0,0,1] 、相对位置矢量b5
T T
＝[0,0,0] ；q6为髋关节滚动角度，q6对应的转动轴矢量a6＝[0,1,0] 、相对位置矢量b6＝
T
[0,0,l3] ；髋关节绝对位置矢量为p6，其姿态矩阵为R6；其中，l1为小腿的长度，l2为大腿的
长度，l3为髋关节到躯干的距离；本实施例中l1＝0.275m，l2＝0.275m，l3＝0.05m。
T
位置矢量为p0，其姿态矩阵为R0；q1为踝关节俯仰角度，q1对应的转动轴矢量a1＝[1,0,0] 、
T T
相对位置矢量b1＝[0,0,0] ；q2为踝关节滚动角度，q2对应的转动轴矢量a2＝[0,1,0] 、相对
T T
位置矢量b2＝[0,0,0] ；q3为膝关节俯仰角度，q3对应的转动轴矢量a3＝[1,0,0] 、相对位置
T T
矢量b3＝[0,0,l1] ；q4为髋关节俯仰角度，q4对应的转动轴矢量a4＝[1,0,0] 、相对位置矢
T T
量b4＝[0,0,l2] ；q5为髋关节旋转角度，q5对应的转动轴矢量a5＝[0,0,1] 、相对位置矢量b5
T T
＝[0,0,0] ；q6为髋关节滚动角度，q6对应的转动轴矢量a6＝[0,1,0] 、相对位置矢量b6＝
T
[0,0,l3] ；髋关节绝对位置矢量为p6，其姿态矩阵为R6；其中，l1为小腿的长度，l2为大腿的
长度，l3为髋关节到躯干的距离；本实施例中l1＝0.275m，l2＝0.275m，l3＝0.05m。
[0033] 在初始状态下，各关节角度为0，此时选取各关节的局部坐标系与世界坐标系W平行。在此局部坐标系下，机器人关节角度qi(i＝1～6)相对关节角度qi‑1的齐次变换矩阵
为：
为：
[0034]
[0035] 其中，ai为关节角度qi的单位转轴向量， 表示由ai生成的斜对称矩阵，bi表示关节角度qi‑1局部坐标系下qi局部坐标系原点的位置矢量， 表达如下：
[0036]
[0037] 其中，E为单位矩阵。基于齐次转换矩阵 机器人关节角度qi的位姿矩阵Ti为：
[0038]
[0039] 其中，Ri(i＝0,6)为关节角度qi的姿态矩阵，pi(i＝0,6)为关节qi的绝对位置矢量，R0和p0分别为机器人起始杆件的位姿矩阵和绝对位置矢量。基于上述位姿矩阵表达式，
设q6为机器人的末端关节，则机器人末端的位姿矩阵可表示为：
设q6为机器人的末端关节，则机器人末端的位姿矩阵可表示为：
[0040]
[0041] 然后，基于机器人末端的位姿矩阵，构建机器人逆运动学求解的表达式方程：
[0042] f(q)＝fref
[0043] 其中， q＝[q1,q2,q3,q4,q5,q6]T为机器人关节角度所组成的向量，pref为末端位置矢量的期望值；向量vor构造如下：
[0044] vor＝[M(1,1) M(2,2) M(3,3)]T
[0045] 其中，矩阵 为机器人末端期望姿态矩阵Rref的转置。构造vor的原理在于，当末端姿态矩阵Rj＝Rref时， 为单位矩阵E，从而取M的对角线元素
T
构造目标向 量vo r。本实 施例取pr ef＝[ 0 ‑0 .1234 0 .5 841] ，
T
构造目标向 量vo r。本实 施例取pr ef＝[ 0 ‑0 .1234 0 .5 841] ，
[0046] 接着，利用牛顿拉夫逊算法对方程f(q)＝fref进行求解，方程求解的迭代方程如下所示：
[0047] Xk＝Xk‑1‑J‑1(f(Xk‑1)‑fref)
[0048] 其中，Xk为向量q在第k次的迭代值； J‑1为J的逆；X0为关节角度向量q‑7
的初始估计值。当向量f(Xk)‑fref的欧几里得范数小于某一足够小的阈值λ＝10 时，迭代终
T
止，最终获得q＝[0.1 0 0.2 0 0.5235 0.6283]。
的初始估计值。当向量f(Xk)‑fref的欧几里得范数小于某一足够小的阈值λ＝10 时，迭代终
T
止，最终获得q＝[0.1 0 0.2 0 0.5235 0.6283]。
[0049] 步骤二，基于步骤一所求解的关节角度q，采用几何方法计算出驱动电机实际需要转动的角度。本实施例以踝关节右侧驱动电机的转动值qm为例进行计算，其余关节的驱动
电机可类似处理：
电机可类似处理：
[0050] 首先，如图1b所示，基于驱动电机曲柄pa1‑pa2与连杆pa2‑pa3构建空间四边形pa1pa2pa3pa4，其中pa4为机器人小腿杆件上选择的一个任意已知点，满足pa1pa4与pa1pa2间的夹
角即为电机实际需要的转动角度；然后，在局部坐标系下，对三个顶点pa1、pa3、pa4进行表达，
并将pa2的位置表示为qm的函数，再进一步在局部坐标系下表示向量pa3pa2；最后，根据连杆
pa2‑pa3长度为一定值，即可求解出电机的实际转动角度qm＝‑0.1。
角即为电机实际需要的转动角度；然后，在局部坐标系下，对三个顶点pa1、pa3、pa4进行表达，
并将pa2的位置表示为qm的函数，再进一步在局部坐标系下表示向量pa3pa2；最后，根据连杆
pa2‑pa3长度为一定值，即可求解出电机的实际转动角度qm＝‑0.1。
[0051] 以上实施例仅用于说明本发明的设计思想和特点，其目的在于使本领域内的技术人员能够了解本发明的内容并据以实施，本发明的保护范围不限于上述实施例。所以，凡依
据本发明所揭示的原理、设计思路所作的等同变化或修饰，均在本发明的保护范围之内。
据本发明所揭示的原理、设计思路所作的等同变化或修饰，均在本发明的保护范围之内。