CN116304512A - 一种机器人腿部的逆运动学求解方法及装置 - Google Patents

一种机器人腿部的逆运动学求解方法及装置 Download PDF

Info

Publication number
CN116304512A
CN116304512A CN202310255424.1A CN202310255424A CN116304512A CN 116304512 A CN116304512 A CN 116304512A CN 202310255424 A CN202310255424 A CN 202310255424A CN 116304512 A CN116304512 A CN 116304512A
Authority
CN
China
Prior art keywords
joint
angle
leg
rotation angle
coordinate system
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202310255424.1A
Other languages
English (en)
Other versions
CN116304512B (zh
Inventor
连文康
宋夙冕
田蒋仁
陈鹏
袁海辉
樊强
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China South Industries Group Automation Research Institute
Original Assignee
China South Industries Group Automation Research Institute
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by China South Industries Group Automation Research Institute filed Critical China South Industries Group Automation Research Institute
Priority to CN202310255424.1A priority Critical patent/CN116304512B/zh
Publication of CN116304512A publication Critical patent/CN116304512A/zh
Application granted granted Critical
Publication of CN116304512B publication Critical patent/CN116304512B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G06F17/13—Differential equations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F7/00—Methods or arrangements for processing data by operating upon the order or content of the data handled
    • G06F7/76—Arrangements for rearranging, permuting or selecting data according to predetermined rules, independently of the content of the data
    • G06F7/78—Arrangements for rearranging, permuting or selecting data according to predetermined rules, independently of the content of the data for changing the order of data flow, e.g. matrix transposition or LIFO buffers; Overflow or underflow handling therefor

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • Computing Systems (AREA)
  • Operations Research (AREA)
  • Automation & Control Theory (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了一种机器人腿部的逆运动学求解方法及装置，该方法主要针对双足机器人中含有耦合驱动平行四边形并联关节的腿部逆运动学求解，由于存在耦合驱动，且髋关节电机轴三轴不交于一点，导致逆运动学解析解不存在。该方法将踝关节处的位置与姿态进行解耦，对于位置，根据电机初始位置作为迭代初值，采用数值解法进行逆运动学求解；对于姿态，采用几何解法直接获得踝关节角度关系，从而将位置与姿态解耦，进行快速求解。

Description

一种机器人腿部的逆运动学求解方法及装置
技术领域
本发明涉及机器人技术领域，特别是涉及一种含有并联结构的机器人腿部的逆运动学求解方法及装置。
背景技术
近年来，双足机器人的发展受到了国内外的空前关注，并出台了一系列的双足机器人发展计划，目前许多国家都已经设计出双足机器人的原型机，如美国波士顿动力公司的Atlas、Agility robotics公司的Cassie、日本本田公司的ASIMO、AIST的HRP系列、韩国KAIST的HUBO系列、欧洲开源机器人iCub、我国北京理工大学的汇童系列、哈尔滨工业大学的WLR-II与GoRoBoT、中国科学技术大学的XT、浙江大学的悟空以及清华大学的THU-Strider等。总体来看，虽然双足机器人技术已经取得很大突破，但其总体性能与人类要求仍然存在一定的差距。
机器人的本体性能决定机器人自身的极限性能。双足机器人驱动方式分为电机驱动型以及液压驱动型。考虑到液压驱动源所占体积较大，应用在双足机器人平台上较为困难，现阶段绝大多数双足机器人仍然采用电机驱动的方式。由于电机的驱动力矩有限，其机械结构对其本体的运动性能影响较大。
电机驱动型双足机器人到目前为止有三种经典构型。原位布局型、串联型、并联型。其中原位布局型结构特点是电机严格按照关节自由度进行布置，由于踝部存在两个自由度，这种布置方式往往导致其踝关节处较为臃肿，腿部惯量较大，影响机器人本身的运动性能。
基于减小机器人腿部惯量的考虑，考虑尽量将电机向腿部上部移动，将质量尽量向躯干集中，这样不仅可以减少腿部惯量，降低对电机的驱动力要求，还可以将下肢质量尽量集中于上部，便于简化模型的应用，并且可以在平行四边形并联关节处使用板簧，减小落地时的冲击，从而进一步提高双足机器人行进速度。但是采用平行四边形并联关节有着不存在解析解的问题，必须通过迭代的方法获得运动学逆解。
因此，如何提供一种适用于含有耦合驱动平行四边形并联关节的机器人使用的腿部逆运动学快速求解方法，是迫切需要本领域技术人员解决的技术问题。
发明内容
鉴于上述问题，本发明提供一种克服上述问题或者至少部分地解决上述问题的一种机器人腿部的逆运动学求解方法及装置。
本发明提供了如下方案：
一种机器人腿部的逆运动学求解方法，包括：
根据腿部平行四边形并联关节处的几何关系，求出耦合关节之间的角度约束关系；
以躯干坐标系作为基座标系，根据所述角度约束关系以及各个关节的DH关系求出踝部关节在耦合下的位姿表达；
根据所述位姿表达，求出踝关节在所述基座标系下的雅可比矩阵；
根据所述踝部关节的目标位置，以各个腿部关节的当前位置作为迭代的初始值，使用牛顿拉夫逊算法，计算获得各个腿部关节角度值；
根据所述几何关系以及机器人行走时的脚板姿态约束，计算出所述踝部关节的俯仰旋转角度；
根据所述踝部关节的俯仰旋转角度计算获得各个腿部关节的旋转角度；将所述踝部关节的俯仰旋转角度以及各个腿部关节的旋转角度作为目标值发送到电机端。
优选地：将所述踝部关节的俯仰旋转角度以及各个关节的旋转角度作为下次迭代的初始值。
优选地：所述腿部平行四边形并联关节包括平行四连杆传动结构；所述平行四连杆传动结构包括由M点、N点、A点、B点构成的平行四边形MNAB；∠MNA转动通过平行四边形MNAB带动AF杆转动；所述M点的坐标系相对所述基座标系转动的角度为θ3，所述N点的坐标系相对所述基座标系转动的角度为θ4，所述A点的坐标系相对所述基座标系转动的角度为θ5，所述角度约束关系包括：
θ5＝π-∠BAF-θ4
其中，所述M点设置有控制髋关节的俯仰角的电机，所述N点设置有控制膝关节的俯仰角的电机，所述A点以及所述B点位于膝关节位置处且与F点处于同一固定零件上；所述F点为机器人竖直站立时脚踝的位置。
优选地：所述角度约束关系包括：
θ5＝0.21-θ4。
优选地：所述根据所述角度约束关系以及各个关节的DH关系求出踝部关节在耦合下的位姿表达，包括：
由DH法建立坐标系之间的齐次变换矩阵，则脚踝F点相对于质心的位姿坐标为：
Figure BDA0004129423850000031
式中：T为两坐标系之间的齐次变换矩阵；
给定各个部分DH坐标以及目标转角，根据给出的DH参数计算获得：
Figure BDA0004129423850000032
优选地：将坐标系N与坐标系A之间的角度约束关系带入获得F点位置；
引入角度约束后计算角度雅可比矩阵JacR：
Figure BDA0004129423850000033
对角度求偏导可以获得速度雅可比矩阵JacV：
Figure BDA0004129423850000034
计算雅可比矩阵：
Figure BDA0004129423850000041
上次读取到的关节角度为：
qtar＝[0.08 0.17 0.03 0.28 0.03 -0.03]T
设置迭代终止条件为||dθ||＜10-4，执行上述迭代步骤获得：
qslove＝[0.1 0.2 -0.0006 0.3013 0.0004 0]T。
优选地：根据编码器反馈得到的电机数据，获得所述踝部关节当前位姿Tcur；根据所述踝部关节目标位置计算微分运动矩阵Δ：
Figure BDA0004129423850000042
Figure BDA0004129423850000043
D＝[dx dy dz δx δy δz]T
建立牛顿-拉夫逊迭代方程：
dθ＝Jac+D
式中，Jac+为雅可比矩阵得伪逆，D为末端的位置与姿态误差向量，dθ为末端误差映射到关节上所产生每个腿部关节对应的误差角度；dx、dy、dz代表xyz三个方向的位置误差，δx,δy，δz是角度误差向量。
优选地：计算||dθ||，设定满足精度的数eps，当||dθ||≤eps时，退出循环并输出当前位姿；当循环次数大于Nmax时，循环结束并输出失败信息；
其中，Nmax为满足预定循环次数要求的自定义充分大的整数。
优选地：根据几何关系以及机器人行走时的脚板姿态水平的约束，计算出踝关节pitch角度为：
θ6＝θ3+0.21。
一种机器人腿部的逆运动学求解装置，包括：
角度约束关系求解单元，用于根据腿部平行四边形并联关节处的几何关系，求出耦合关节之间的角度约束关系；
位姿表达求解单元，用于以躯干坐标系作为基座标系，根据所述角度约束关系以及各个关节的DH关系求出踝部关节在耦合下的位姿表达；
雅可比矩阵求解单元，用于根据所述位姿表达，求出踝关节在所述基座标系下的雅可比矩阵；
关节角度值计算单元，用于根据所述踝部关节的目标位置，以各个腿部关节的当前位置作为迭代的初始值，使用牛顿拉夫逊算法，计算获得各个腿部关节角度值；
脚踝的旋转角度计算单元，用于根据所述几何关系以及机器人行走时的脚板姿态约束，计算出所述踝部关节的俯仰旋转角度；
目标值发送单元，用于根据所述踝部关节的俯仰旋转角度计算获得各个腿部关节的旋转角度；将所述踝部关节的俯仰旋转角度以及各个腿部关节的旋转角度作为目标值发送到电机端。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本申请实施例提供的一种机器人腿部的逆运动学求解方法及装置，该方法主要针对双足机器人中含有耦合驱动平行四边形并联关节的腿部逆运动学求解，由于存在耦合驱动，且髋关节电机轴三轴不交于一点，导致逆运动学解析解不存在。该方法将踝关节处的位置与姿态进行解耦，对于位置，根据电机初始位置作为迭代初值，采用数值解法进行逆运动学求解；对于姿态，采用几何解法直接获得踝关节角度关系，从而将位置与姿态解耦，进行快速求解。
当然，实施本发明的任一产品并不一定需要同时达到以上所述的所有优点。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来说，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的一种机器人腿部的逆运动学求解方法的流程图；
图2是本发明实施例提供的机器人的三维结构图；
图3是本发明实施例提供的三维结构中的坐标系定义图；
图4是本发明实施例提供的DH坐标定义图；
图5是本发明实施例提供的机器人单腿平面图；
图6是本发明实施例提供的机器人单腿连杆图；
图7是本发明实施例提供的一种机器人腿部的逆运动学求解装置的示意图；
图8是本发明实施例提供的一种机器人腿部的逆运动学求解设备的示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员所获得的所有其他实施例，都属于本发明保护的范围。
参见图1，为本发明实施例提供的一种机器人腿部的逆运动学求解方法，如图1所示，该方法可以包括：
S101:根据腿部平行四边形并联关节处的几何关系，求出耦合关节之间的角度约束关系；
S102:以躯干坐标系作为基座标系，根据所述角度约束关系以及各个关节的DH关系求出踝部关节在耦合下的位姿表达；
S103:根据所述位姿表达，求出踝关节在所述基座标系下的雅可比矩阵；
S104:根据所述踝部关节的目标位置，以各个腿部关节的当前位置作为迭代的初始值，使用牛顿拉夫逊算法，计算获得各个腿部关节角度值；
S105：根据所述几何关系以及机器人行走时的脚板姿态约束，计算出所述踝部关节的俯仰旋转角度；
S106:根据所述踝部关节的俯仰旋转角度计算获得各个腿部关节的旋转角度；将所述踝部关节的俯仰旋转角度以及各个腿部关节的旋转角度作为目标值发送到电机端。
本申请实施例提供的机器人腿部的逆运动学求解方法，首先根据踝部关键的目标位置可以获取踝部关节的俯仰旋转角度，由于踝部关节处于机器人腿部的末端，各个关节具有联动机构，因此可以根据踝部关节的俯仰旋转角度推导出各个腿部关节所需的旋转角度。将所述踝部关节的俯仰旋转角度以及各个腿部关节的旋转角度作为目标值发送到电机端，各个电机即可执行相应的动作，使得机器人朝向目标位置运动。
为了方便下次迭代，本申请实施例还可以提供将所述踝部关节的俯仰旋转角度以及各个关节的旋转角度作为下次迭代的初始值。
进一步的，所述腿部平行四边形并联关节包括平行四连杆传动结构。
所述平行四连杆传动结构包括由M点、N点、A点、B点构成的平行四边形MNAB；∠MNA转动通过平行四边形MNAB带动AF杆转动；所述M点的坐标系相对所述基座标系转动的角度为θ3，所述N点的坐标系相对所述基座标系转动的角度为θ4，所述A点的坐标系相对所述基座标系转动的角度为θ5，所述角度约束关系包括：
θ5＝π-∠BAF-θ4
其中，所述M点设置有控制髋关节的俯仰角的电机，所述N点设置有控制膝关节的俯仰角的电机，所述A点以及所述B点位于膝关节位置处且与所述F点处于同一固定零件上；所述F点为机器人竖直站立时脚踝的位置。
进一步的，所述角度约束关系包括：
θ5＝0.21-θ4。
所述根据所述角度约束关系以及各个关节的DH关系求出踝部关节在耦合下的位姿表达，包括：
由D-H法建立坐标系之间的齐次变换矩阵，则脚踝F点相对于质心的位姿坐标为：
Figure BDA0004129423850000071
式中：T均为两坐标系之间的齐次变换矩阵；其中例如
Figure BDA0004129423850000081
表示坐标系G相对于坐标系O的齐次变换矩阵，其他含义相同。
给定各个部分DH坐标以及目标转角，根据给出的DH参数计算获得：
Figure BDA0004129423850000082
将坐标系N与坐标系A之间的角度约束关系带入获得F点位置；
引入角度约束后计算角度雅可比矩阵JacR：
Figure BDA0004129423850000083
对角度求偏导可以获得速度雅可比矩阵JacV：
Figure BDA0004129423850000084
计算雅可比矩阵：
Figure BDA0004129423850000085
上次读取到的关节角度为
qtar＝[0.08 0.17 0.03 0.28 0.03 -0.03]T
设置迭代终止条件为||dθ＜10-4，执行上述迭代步骤获得：
qslove＝[0.1 0.2 -0.0006 0.3013 0.0004 0]T
根据编码器反馈得到的电机数据，获得所述踝部关节当前位姿Tcur；
根据所述踝部关节目标位置计算微分运动矩阵Δ：
Figure BDA0004129423850000091
Figure BDA0004129423850000092
D＝[dx dy dz δx δy δz]T
建立牛顿-拉夫逊迭代方程：
dθ＝Jac+D
其中，Jac+为雅可比矩阵得伪逆，D为末端的位置与姿态误差向量，dθ为末端误差映射到关节上所产生每个腿部关节对应的误差角度；dx、dy、dz代表xyz三个方向的位置误差，δx,δy，δz是角度误差向量。
计算||dθ||，设定满足精度的数eps，当||dθ||≤eps时，退出循环并输出当前位姿；当循环次数大于Nmax时，循环结束并输出失败信息；
其中，Nmax为满足预定循环次数要求的自定义充分大的整数；
根据几何关系以及机器人行走时的脚板姿态水平的约束，计算出踝关节pitch角度为：
θ6＝θ3+0.21。
下面结合幅图对本申请实施例提供的方法进行详细介绍。
步骤一：参见图2，根据图5中单侧腿部的几何关系，求出两个耦合关节之间的约束关系；
参照图6所示的腿部平面连杆机构，由于MNAB为平行四连杆传动，所以当∠MNA转动时会通过平行四边形MNAB带动AF杆转动，在图6中表示出图4中坐标系转动的角度分别为θ3、θ4、θ5，其中θ3、θ4、θ5分别为各自对应坐标系绕y轴旋转所得角度，存在正负号，根据平面几何关系可以获得以下关系：
θ5＝π-∠BAF-θ4
获得θ4与θ5之间的几何关系
θ5＝0.21-θ4
步骤二：以躯干坐标系作为基座标系，根据耦合关节的角度约束以及各个关节的DH关系，求出踝部关节在耦合下的位姿表达；
步骤二具体通过以下步骤来实现的：
如图3所示为机器人三维结构，其坐标系建立如图4所示，虚线为转轴，从G点到F点坐标系沿坐标轴的旋转角度依次为θ1、θ2、θ3、θ4、θ5、θ6、θ7，由D-H法建立坐标系之间的齐次变换矩阵，则脚踝F点相对于质心的位姿坐标为：
Figure BDA0004129423850000101
给定各个部分DH坐标以及目标转角如表1
表1
关节名称 x y Z x-roll z-yaw y-pitch
G_l 115.15 140 0 0.1
E_l -115.15 0 0 0.2
M_l 0 0 -129 0
N_l 0 0 -123 0.3
A_l 0 0 -474 耦合
F_lpitch -27.15 0 -435.04 0
F_lroll 0 0 0 0
G_r 115.15 -140 0 0.1
E_r -115.15 0 0 0.2
M_r 0 0 -129 0
N_r 0 0 -123 0.3
A_r 0 0 -474 耦合
F_rpitch -27.15 0 -435.04 0
F_rroll 0 0 0 0
根据给出的DH参数计算
Figure BDA0004129423850000111
步骤三：根据踝关节位姿在基座标系下的表达式，求出踝关节在躯干基座标系下的雅可比矩阵；
步骤三具体通过以下步骤来实现的：
(3.1)考虑平行四连杆机构引入的角度约束，计算角度雅可比矩阵：
Figure BDA0004129423850000112
Figure BDA0004129423850000113
Figure BDA0004129423850000114
Figure BDA0004129423850000115
Figure BDA0004129423850000116
Figure BDA0004129423850000117
Figure BDA0004129423850000118
(3.2)可以获得角度雅可比如下：
Figure BDA0004129423850000119
(3.3)考虑角度约束关系可以获得如下关系：
Figure BDA00041294238500001110
(3.4)计算速度雅可比矩阵：
将坐标系N与坐标系A之间的角度约束关系带入获得F点位置，对角度求偏导可以获得速度雅可比矩阵：
Figure BDA0004129423850000121
(3.5)计算雅可比矩阵：
Figure BDA0004129423850000122
上次读取到的关节角度为：
qtar＝[0.08 0.17 0.03 0.28 0.03 -0.03]T
设置迭代终止条件为||dθ||＜10-4，执行上述迭代步骤获得：
qslove＝[0.1 0.2 -0.0006 0.3013 0.0004 0]T
步骤四：根据机器人踝部的目标位置，以当前的位置作为迭代的初始值，使用牛顿--拉夫逊算法，计算各个关节角度值；
步骤四具体通过以下步骤来实现的：
(4.1)根据编码器反馈得到的电机数据，获得脚踝当前位姿Tcur；
(4.2)根据脚踝目标位置计算微分运动矩阵Δ：
Figure BDA0004129423850000123
Figure BDA0004129423850000124
D＝[dx dy dz δx δy δz]T
(4.3)建立牛顿-拉夫逊迭代方程：
dθ＝Jac+D
其中，Jac+为雅可比矩阵得伪逆，D为末端的位置与姿态误差向量，dθ为末端误差映射到关节上所产生每个腿部关节对应的误差角度；
(4.4)计算||dθ||，设定一个满足精度的充分小的数eps，当||dθ|≤eps时，退出循环并输出当前位姿；当循环次数大Nmax时，循环结束并输出失败信息；其中，Nmax为满足预定循环次数要求的自定义充分大的整数；
根据几何关系以及机器人行走时的脚板姿态水平的约束，可以计算出踝关节pitch角度为：
θ6＝θ3+0.21
步骤五：根据几何关系以及机器人行走时的脚板姿态约束，计算出脚踝的角度；
步骤六：将计算出的各个关节角度作为目标值发送到电机端，并将关节迭代计算出的关节角度值作为下次迭代的初始值。循环执行以上三、四、五步骤，从而实现机器人的闭环控制。
总之，本申请提供的机器人腿部的逆运动学求解方法，主要针对双足机器人中含有耦合驱动平行四边形并联关节的腿部逆运动学求解，由于存在耦合驱动，且髋关节电机轴三轴不交于一点，导致逆运动学解析解不存在。该方法将踝关节处的位置与姿态进行解耦，对于位置，根据电机初始位置作为迭代初值，采用数值解法进行逆运动学求解；对于姿态，采用几何解法直接获得踝关节角度关系，从而将位置与姿态解耦，进行快速求解。
参见图7，本申请实施例还可以提供一种机器人腿部的逆运动学求解装置，如图7所示，该装置可以包括：
角度约束关系求解单元701，用于根据腿部平行四边形并联关节处的几何关系，求出耦合关节之间的角度约束关系；
位姿表达求解单元702，用于以躯干坐标系作为基座标系，根据所述角度约束关系以及各个关节的DH关系求出踝部关节在耦合下的位姿表达；
雅可比矩阵求解单元703，用于根据所述位姿表达，求出踝关节在所述基座标系下的雅可比矩阵；
关节角度值计算单元704，用于根据所述踝部关节的目标位置，以各个腿部关节的当前位置作为迭代的初始值，使用牛顿拉夫逊算法，计算获得各个腿部关节角度值；
脚踝的旋转角度计算单元705，用于根据所述几何关系以及机器人行走时的脚板姿态约束，计算出所述踝部关节的俯仰旋转角度；
目标值发送单元706，用于根据所述踝部关节的俯仰旋转角度计算获得各个腿部关节的旋转角度；将所述踝部关节的俯仰旋转角度以及各个腿部关节的旋转角度作为目标值发送到电机端。
本申请实施例还可以提供一种机器人腿部的逆运动学求解设备，所述设备包括处理器以及存储器：
所述存储器用于存储程序代码，并将所述程序代码传输给所述处理器；
所述处理器用于根据所述程序代码中的指令执行上述的机器人腿部的逆运动学求解方法的步骤。
如图8所示，本申请实施例提供的一种机器人腿部的逆运动学求解设备，该设备可以包括：处理器10、存储器11、通信接口12和通信总线13。处理器10、存储器11、通信接口12均通过通信总线13完成相互间的通信。
在本申请实施例中，处理器10可以为中央处理器(Central Processing Unit，CPU)、特定应用集成电路、数字信号处理器、现场可编程门阵列或者其他可编程逻辑器件等。
处理器10可以调用存储器11中存储的程序，具体的，处理器10可以执行机器人腿部的逆运动学求解方法的实施例中的操作。
存储器11中用于存放一个或者一个以上程序，程序可以包括程序代码，程序代码包括计算机操作指令，在本申请实施例中，存储器11中至少存储有用于实现以下功能的程序：
根据腿部平行四边形并联关节处的几何关系，求出耦合关节之间的角度约束关系；
以躯干坐标系作为基座标系，根据所述角度约束关系以及各个关节的DH关系求出踝部关节在耦合下的位姿表达；
根据所述位姿表达，求出踝关节在所述基座标系下的雅可比矩阵；
根据所述踝部关节的目标位置，以各个腿部关节的当前位置作为迭代的初始值，使用牛顿拉夫逊算法，计算获得各个腿部关节角度值；
根据所述几何关系以及机器人行走时的脚板姿态约束，计算出所述踝部关节的俯仰旋转角度；
根据所述踝部关节的俯仰旋转角度计算获得各个腿部关节的旋转角度；将所述踝部关节的俯仰旋转角度以及各个腿部关节的旋转角度作为目标值发送到电机端。
在一种可能的实现方式中，存储器11可包括存储程序区和存储数据区，其中，存储程序区可存储操作系统，以及至少一个功能(比如文件创建功能、数据读写功能)所需的应用程序等；存储数据区可存储使用过程中所创建的数据，如初始化数据等。
此外，存储器11可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件或其他易失性固态存储器件。
通信接口12可以为通信模块的接口，用于与其他设备或者系统连接。
当然，需要说明的是，图8所示的结构并不构成对本申请实施例中机器人腿部的逆运动学求解设备的限定，在实际应用中机器人腿部的逆运动学求解设备可以包括比图8所示的更多或更少的部件，或者组合某些部件。
本申请实施例还可以提供一种计算机可读存储介质，所述计算机可读存储介质用于存储程序代码，所述程序代码用于执行上述的机器人腿部的逆运动学求解方法的步骤。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
通过以上的实施方式的描述可知，本领域的技术人员可以清楚地了解到本申请可借助软件加上必需的通用硬件平台的方式来实现。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例或者实施例的某些部分所述的方法。
本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于系统或系统实施例而言，由于其基本相似于方法实施例，所以描述得比较简单，相关之处参见方法实施例的部分说明即可。以上所描述的系统及系统实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
以上所述仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等，均包含在本发明的保护范围内。

Claims (10)

1.一种机器人腿部的逆运动学求解方法，其特征在于，包括：
根据腿部平行四边形并联关节处的几何关系，求出耦合关节之间的角度约束关系；
以躯干坐标系作为基座标系，根据所述角度约束关系以及各个关节的DH关系求出踝部关节在耦合下的位姿表达；
根据所述位姿表达，求出踝关节在所述基座标系下的雅可比矩阵；
根据所述踝部关节的目标位置，以各个腿部关节的当前位置作为迭代的初始值，使用牛顿拉夫逊算法，计算获得各个腿部关节角度值；
根据所述几何关系以及机器人行走时的脚板姿态约束，计算出所述踝部关节的俯仰旋转角度；
根据所述踝部关节的俯仰旋转角度计算获得各个腿部关节的旋转角度；将所述踝部关节的俯仰旋转角度以及各个腿部关节的旋转角度作为目标值发送到电机端。
2.根据权利要求1所述的机器人腿部的逆运动学求解方法，其特征在于，将所述踝部关节的俯仰旋转角度以及各个关节的旋转角度作为下次迭代的初始值。
3.根据权利要求1所述的机器人腿部的逆运动学求解方法，其特征在于，所述腿部平行四边形并联关节包括平行四连杆传动结构；所述平行四连杆传动结构包括由M点、N点、A点、B点构成的平行四边形MNAB；∠MNA转动通过平行四边形MNAB带动AF杆转动；所述M点的坐标系相对所述基座标系转动的角度为θ3，所述N点的坐标系相对所述基座标系转动的角度为θ4，所述A点的坐标系相对所述基座标系转动的角度为θ5，所述角度约束关系包括：
θ5＝π-∠BAF-θ4
其中，所述M点设置有控制髋关节的俯仰角的电机，所述N点设置有控制膝关节的俯仰角的电机，所述A点以及所述B点位于膝关节位置处且与F点处于同一固定零件上；所述F点为机器人竖直站立时脚踝的位置。
4.根据权利要求3所述的机器人腿部的逆运动学求解方法，其特征在于，所述角度约束关系包括：
θ5＝0.21-θ4。
5.根据权利要求4所述的机器人腿部的逆运动学求解方法，其特征在于，所述根据所述角度约束关系以及各个关节的DH关系求出踝部关节在耦合下的位姿表达，包括：
由DH法建立坐标系之间的齐次变换矩阵，则脚踝F点相对于质心的位姿坐标为：
Figure FDA0004129423830000021
式中：T为两坐标系之间的齐次变换矩阵；
给定各个部分DH坐标以及目标转角，根据给出的DH参数计算获得：
Figure FDA0004129423830000022
6.根据权利要求5所述的机器人腿部的逆运动学求解方法，其特征在于，将坐标系N与坐标系A之间的角度约束关系带入获得F点位置；
引入角度约束后计算角度雅可比矩阵JacR：
Figure FDA0004129423830000023
对角度求偏导可以获得速度雅可比矩阵JacV：
Figure FDA0004129423830000024
计算雅可比矩阵：
Figure FDA0004129423830000031
上次读取到的关节角度为：
qtar＝[0.08 0.17 0.03 0.28 0.03 -0.03]T
设置迭代终止条件为||dθ||＜10-4，执行上述迭代步骤获得：
qslove＝[0.1 0.2 -0.0006 0.3013 0.0004 0]T。
7.根据权利要求6所述的机器人腿部的逆运动学求解方法，其特征在于，根据编码器反馈得到的电机数据，获得所述踝部关节当前位姿Tcur；根据所述踝部关节目标位置计算微分运动矩阵Δ：
Figure FDA0004129423830000032
Figure FDA0004129423830000033
D＝[dx dy dz δx δy δz]T
建立牛顿-拉夫逊迭代方程：
dθ＝Jac+D
式中，Jac+为雅可比矩阵得伪逆，D为末端的位置与姿态误差向量，dθ为末端误差映射到关节上所产生每个腿部关节对应的误差角度；dx、dy、dz代表xyz三个方向的位置误差，δx,δy，δz是角度误差向量。
8.根据权利要求7所述的机器人腿部的逆运动学求解方法，其特征在于，计算||dθ||，设定满足精度的数eps，当||dθ||≤eps时，退出循环并输出当前位姿；当循环次数大于Nmax时，循环结束并输出失败信息；
其中，Nmax为满足预定循环次数要求的自定义充分大的整数。
9.根据权利要求8所述的机器人腿部的逆运动学求解方法，其特征在于，根据几何关系以及机器人行走时的脚板姿态水平的约束，计算出踝关节pitch角度为：
θ6＝θ3+0.21。
10.一种机器人腿部的逆运动学求解装置，其特征在于，包括：
角度约束关系求解单元，用于根据腿部平行四边形并联关节处的几何关系，求出耦合关节之间的角度约束关系；
位姿表达求解单元，用于以躯干坐标系作为基座标系，根据所述角度约束关系以及各个关节的DH关系求出踝部关节在耦合下的位姿表达；
雅可比矩阵求解单元，用于根据所述位姿表达，求出踝关节在所述基座标系下的雅可比矩阵；
关节角度值计算单元，用于根据所述踝部关节的目标位置，以各个腿部关节的当前位置作为迭代的初始值，使用牛顿拉夫逊算法，计算获得各个腿部关节角度值；
脚踝的旋转角度计算单元，用于根据所述几何关系以及机器人行走时的脚板姿态约束，计算出所述踝部关节的俯仰旋转角度；
目标值发送单元，用于根据所述踝部关节的俯仰旋转角度计算获得各个腿部关节的旋转角度；将所述踝部关节的俯仰旋转角度以及各个腿部关节的旋转角度作为目标值发送到电机端。
CN202310255424.1A 2023-03-16 2023-03-16 一种机器人腿部的逆运动学求解方法及装置 Active CN116304512B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310255424.1A CN116304512B (zh) 2023-03-16 2023-03-16 一种机器人腿部的逆运动学求解方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310255424.1A CN116304512B (zh) 2023-03-16 2023-03-16 一种机器人腿部的逆运动学求解方法及装置

Publications (2)

Publication Number Publication Date
CN116304512A true CN116304512A (zh) 2023-06-23
CN116304512B CN116304512B (zh) 2024-01-30

Family

ID=86837517

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310255424.1A Active CN116304512B (zh) 2023-03-16 2023-03-16 一种机器人腿部的逆运动学求解方法及装置

Country Status (1)

Country Link
CN (1) CN116304512B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116859978A (zh) * 2023-08-08 2023-10-10 之江实验室 一种双足机器人踝关节控制的逆运动学求解方法及装置

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190204848A1 (en) * 2017-12-29 2019-07-04 Ubtech Robotics Corp Gait control method, device, and terminal device for biped robot
US20200139541A1 (en) * 2017-05-09 2020-05-07 Institute Of Computing Technology Inverse kinematics solution system for use with robots
CN111914416A (zh) * 2020-07-24 2020-11-10 之江实验室 一种高能效轻量化结构双足机器人的逆运动学求解方法
US20220019196A1 (en) * 2020-07-14 2022-01-20 Ubtech Robotics Corp Ltd Feedforward control method for floating base dynamics, computer-readable storage medium and robot

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20200139541A1 (en) * 2017-05-09 2020-05-07 Institute Of Computing Technology Inverse kinematics solution system for use with robots
US20190204848A1 (en) * 2017-12-29 2019-07-04 Ubtech Robotics Corp Gait control method, device, and terminal device for biped robot
US20220019196A1 (en) * 2020-07-14 2022-01-20 Ubtech Robotics Corp Ltd Feedforward control method for floating base dynamics, computer-readable storage medium and robot
CN111914416A (zh) * 2020-07-24 2020-11-10 之江实验室 一种高能效轻量化结构双足机器人的逆运动学求解方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
房立金;高瑞;: "一般6R机器人逆运动学算法的改进", 机械科学与技术, no. 09 *
王宪;杨国梁;张方生;丁锋;: "基于牛顿―拉夫逊迭代法的6自由度机器人逆解算法", 传感器与微系统, no. 10 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116859978A (zh) * 2023-08-08 2023-10-10 之江实验室 一种双足机器人踝关节控制的逆运动学求解方法及装置
CN116859978B (zh) * 2023-08-08 2024-05-03 之江实验室 一种双足机器人踝关节控制的逆运动学求解方法及装置

Also Published As

Publication number Publication date
CN116304512B (zh) 2024-01-30

Similar Documents

Publication Publication Date Title
US8140189B2 (en) Apparatus and method for computing operational-space physical quantity
Kofinas et al. Complete analytical forward and inverse kinematics for the NAO humanoid robot
CN109397244B (zh) 一种一体化双7自由度机械臂全向移动机器人系统与控制方法
US8364314B2 (en) Method and apparatus for automatic control of a humanoid robot
US8818555B2 (en) Path planning apparatus of robot and method and computer-readable medium thereof
US9043030B2 (en) Manipulator and path generation method thereof
CN116304512B (zh) 一种机器人腿部的逆运动学求解方法及装置
JPH07281722A (ja) 工業用ロボットを制御する方法
CN106842959A (zh) 一种Nao机器人步态规划遗传算法
Huang et al. A new method of inverse kinematics solution for industrial 7DOF robot
Xu et al. Models for three new screw-based IK sub-problems using geometric descriptions and their applications
CN114055467B (zh) 基于五自由度机器人的空间位姿在线仿真系统
CN115994288B (zh) 一种双足机器人逆运动学求解方法及装置
CN113263498B (zh) 机械臂移动控制方法、装置、终端设备及存储介质
Zhu et al. Inverse kinematics solution of a new circumferential drilling machine for aircraft assembly
Lin et al. Intuitive kinematic control of a robot arm via human motion
Hajdu et al. Kinematical simulation methods for Stewart Platform in medical equipments
Selig et al. Constrained robot dynamics II: Parallel machines
Hu et al. Trajectory planning method of 6-DOF modular manipulator based on polynomial interpolation
Wang et al. Robust humanoid robot vehicle ingress with a finite state machine integrated with deep reinforcement learning
CN108107765A (zh) 一种异构主从系统的运动映射方法及系统
Yang et al. An Analytic Solution Study for a 7-DOF Redundant Robot Manipulator
CN117733872B (zh) 一种基于方向性能的串联机器人逆运动学控制方法
US11787045B2 (en) Robot with an inverse kinematics (IK)-based controller for retargeting input motions
Zhang et al. Kinematics decoupling analysis of a hyper-redundant manipulator driven by cables

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant