CN109703645B - 机器人质心位置调整方法、装置、机器人和数据存储介质 - Google Patents

机器人质心位置调整方法、装置、机器人和数据存储介质 Download PDF

Info

Publication number
CN109703645B
CN109703645B CN201811630615.7A CN201811630615A CN109703645B CN 109703645 B CN109703645 B CN 109703645B CN 201811630615 A CN201811630615 A CN 201811630615A CN 109703645 B CN109703645 B CN 109703645B
Authority
CN
China
Prior art keywords
value
centroid position
waist
initial value
robot
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.)
Active
Application number
CN201811630615.7A
Other languages
English (en)
Other versions
CN109703645A (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.)
Ubtech Robotics Corp
Original Assignee
Ubtech Robotics Corp
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 Ubtech Robotics Corp filed Critical Ubtech Robotics Corp
Priority to CN201811630615.7A priority Critical patent/CN109703645B/zh
Publication of CN109703645A publication Critical patent/CN109703645A/zh
Priority to US16/587,069 priority patent/US11353887B2/en
Priority to JP2019230711A priority patent/JP6961669B2/ja
Priority to KR1020190172878A priority patent/KR102268286B1/ko
Application granted granted Critical
Publication of CN109703645B publication Critical patent/CN109703645B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
    • G05D1/0891—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for land vehicles
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62D—MOTOR VEHICLES; TRAILERS
    • B62D57/00—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track
    • B62D57/02—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members
    • B62D57/032—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members with alternately or sequentially lifted supporting base and legs; with alternately or sequentially lifted feet or skid
    • 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/1615—Programme controls characterised by special kind of manipulator, e.g. planar, scara, gantry, cantilever, space, closed chain, passive/active joints and tendon driven manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • B25J13/088—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices with position, velocity or acceleration sensors
    • B25J13/089—Determining the position of the robot with reference to its environment
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/0008—Balancing devices
    • 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
    • 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/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
    • 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/1615—Programme controls characterised by special kind of manipulator, e.g. planar, scara, gantry, cantilever, space, closed chain, passive/active joints and tendon driven manipulators
    • B25J9/162—Mobile manipulator, movable base with manipulator arm mounted on it
    • 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/1628—Programme controls characterised by the control loop
    • B25J9/1653—Programme controls characterised by the control loop parameters identification, estimation, stiffness, accuracy, error analysis
    • 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/1679—Programme controls characterised by the tasks executed

Abstract

本发明涉及机器人质心位置调整方法、装置、机器人和数据存储介质。该方法包括根据三质量块模型和质心位置规划值获取腰位姿初始值、双脚位姿初始值和质心位置初始值；根据质心位置初始值和质心位置规划值，利用腰位姿变化和质心位置变化对应的线性正相关性，得到腰速度调整值；根据腰位姿初始值、腰速度调整值和双脚位姿初始值计算得到质心位置当前值；比较质心位置当前值同质心位置规划值是否相等，若是，则获取质心位置当前值作为下一质心位置初始值，并返回至根据质心位置初始值和质心位置规划值的步骤执行，直至质心位置当前值同质心位置规划值相等；若否，则根据质心位置当前值生成目标控制系统命令。上述方法提高了机器人平衡能力。

Description

机器人质心位置调整方法、装置、机器人和数据存储介质
技术领域
本发明涉及机器人控制领域，具体而言，涉及一种机器人质心位置调整方法、装置、机器人和数据存储介质。
背景技术
在常规的双足机器人控制领域，计算机器人行走时实际质心位置时，通常采用三质量块模型对机器人结构进行简化，即将机器人简化成只有腰、左腿以及右腿三个刚体的模型，由于该过程中使用了简化模型和近似，导致按照三质量块模型计算得到的质心实际位置，与按照预设参数规划出的机器人质心位置存在较大偏差。例如，对于一个身高1.5米、体重50公斤左右的机器人，这个质心位置误差大约在3-5厘米左右。由于该误差，机器人行走时其平衡控制和快速行进的能力往往受到影响。
发明内容
鉴于上述问题，本发明提供了一种机器人质心位置调整方法、装置、机器人和数据存储介质，能够大幅度降低上述质心误差值，使得机器人行走时其平衡控制能力能够大幅增强。
一种机器人质心位置调整方法，包括：
根据三质量块模型和质心位置规划值获取腰位姿初始值、双脚位姿初始值以及质心位置初始值；
根据质心位置初始值和质心位置规划值，利用腰位姿变化和质心位置变化对应的线性正相关性，得到腰速度调整值；
根据腰位姿初始值、腰速度调整值、左脚位姿初始值和右脚位姿初始值，计算得到质心位置当前值；
比较质心位置当前值同质心位置规划值是否相等，若否，则获取质心位置当前值作为下一质心位置初始值，并返回至根据质心位置初始值和质心位置规划值的步骤继续执行，直至质心位置当前值同质心位置规划值相等；若是，则根据上述质心位置当前值生成目标控制系统命令。
在一个实施例中，根据三质量块模型和质心位置规划值获取对应的腰位姿初始值、双脚位姿初始值以及质心位置初始值的步骤之前还包括：
根据线性倒立摆模型和机器人预设参数，生成质心位置规划值。
在一个实施例中，根据三质量块模型和质心位置规划值获取对应的腰位姿初始值、双脚位姿初始值以及质心位置初始值的步骤包括：
根据三质量块模型和质心位置规划值，获取腰位姿初始值和双脚位姿初始值；
根据腰位姿初始值和所述双脚位姿初始值，计算得到质心位置初始值。
在一个实施例中，根据腰位姿初始值、腰速度调整值和双脚位姿初始值，计算得到质心位置当前值的步骤包括：
根据所述腰位姿初始值、所述腰速度调整值和所述双脚位姿初始值，采用腰位姿逆运动学算法，计算得到每个关节的关节角度；
根据所述腰位姿初始值、所述腰速度调整值和所述每个关节的关节角度，采用质心位置逆运动学算法，计算得到质心位置当前值。
此外，还提供一种机器人质心位置调整装置，包括：
初始值获取单元，用于根据三质量块模型和质心位置规划值获取腰位姿初始值、双脚位姿初始值以及质心位置初始值；
腰速度调整值获取单元，用于根据质心位置初始值和质心位置规划值，利用腰位姿变化和质心位置变化对应的线性正相关性，得到腰速度调整值；
质心位置当前值获取单元，用于根据腰位姿初始值、腰速度调整值和所述双脚位姿初始值，计算得到质心位置当前值；
判断单元，用于比较质心位置当前值同质心位置规划值是否相等，若否，则进入循环执行单元；
循环执行单元，用于获取质心位置当前值作为下一质心位置初始值，并返回至腰速度调整值获取单元执行，直至质心位置当前值同质心位置规划值相等；若是，则进入命令生成单元；
命令生成单元，用于根据所述质心位置当前值生成目标控制系统命令。
在一个实施例中，上述装置还包括：
规划单元，用于根据线性倒立摆模型和机器人预设参数，生成质心位置规划值。
在一个实施例中，初始值获取单元包括：
第一子单元，用于根据三质量块模型和质心位置规划值，获取腰位姿初始值和双脚位姿初始值；
第二子单元，用于根据腰位姿初始值、左脚位姿初始值以及右脚位姿初始值，计算得到质心位置初始值。
在一个实施例中，质心位置当前值获取单元包括：
关节角度获取子单元，用于根据所述腰位姿初始值、所述腰速度调整值和所述双脚位姿初始值，采用腰位姿逆运动学算法，计算得到每个关节的关节角度；
质心位置当前值获取子单元，用于根据根据所述腰位姿初始值、所述腰速度调整值和所述每个关节的关节角度，采用质心位置逆运动学算法，计算得到质心位置当前值。
此外，还提供了一种机器人，包括存储器以及处理器，存储器用于存储计算机程序，处理器运行计算机程序以使机器人执行任上述机器人质心位置调整方法。
此外，还提供了一种数据存储介质，存储有上述机器人所使用的计算机程序。
上述机器人质心位置调整方法、装置、机器人和数据存储介质，首先通过利用根据三质量块模型和质心位置规划值获取机器人相关初始参数，包括腰位姿初始值、双脚位姿初始值和质心位置初始值，然后根据质心位置初始值和质心位置规划值，利用腰位姿变化和质心位置变化对应的线性正相关性，得到腰速度调整值，进一步的根据腰位姿初始值、腰速度调整值和双脚位姿初始值计算得到质心位置当前值，最后比较质心位置当前值同质心位置规划值是否相等，若否，则获取质心位置当前值作为下一质心位置初始值，并返回至根据质心位置初始值和质心位置规划值的步骤执行，直至质心位置当前值同质心位置规划值相等；若是，则根据质心位置当前值生成目标控制系统命令，巧妙的利用了机器人在目标控制系统命令周期内双脚位姿固定不变的情况下，腰位姿变化和质心位置变化对应的线性正相关性，采用腰位姿变化量来对质心位置进行反馈调整，通过多次进行迭代循环计算，大幅降低了机器人质心规划位置同按照三质量块模型计算出来的机器人质心实际位置之间的误差，有效的减少了机器人行走时的扰动，提高了机器人行走稳定性，增强了平衡控制能力。
附图说明
图1为一个实施例中一种机器人质心位置调整方法的流程示意图；
图2为一个实施例中腰位姿变化和质心位置变化的关系示意图；
图3为一个实施例中机器人质心位置调整效果示意图；
图4为另一个实施例中一种机器人质心位置调整方法的流程示意图；
图5为一个实施例中获取腰位姿初始值、双脚位姿初始值以及质心位置初始值方法的流程示意图；
图6为一个实施例中机器人结构和坐标系示意图；
图7为一个实施例中初始小腿前摆角的示意图；
图8为一个实施例中计算质心位置当前值的流程示意图；
图9为一个实施例中一种机器人质心位置调整装置的结构框图；
图10为一个实施例中一种机器人质心位置调整装置的结构框图；
图11为一个实施例中初始值获取单元的结构框图；
图12为一个实施例中质心位置当前值获取单元的结构框图。
具体实施方式
在下文中，将更全面地描述本公开的各种实施例。本公开可具有各种实施例，并且可在其中做出调整和改变。然而，应理解：不存在将本公开的各种实施例限于在此公开的特定实施例的意图，而是应将本公开理解为涵盖落入本公开的各种实施例的精神和范围内的所有调整、等同物和/或可选方案。
在下文中，可在本公开的各种实施例中使用的术语“包括”或“可包括”指示所公开的功能、操作或元件的存在，并且不限制一个或更多个功能、操作或元件的增加。此外，如在本公开的各种实施例中所使用，术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。
在本公开的各种实施例中，表述A或/和B中的至少一个”包括同时列出的文字的任何组合或所有组合。例如，表述“A或B”或“A或/和B中的至少一个”可包括A、可包括B或可包括A和B二者。
在本公开的各种实施例中使用的表述(诸如“第一”、“第二”等)可修饰在各种实施例中的各种组成元件，不过可不限制相应组成元件。例如，以上表述并不限制所述元件的顺序和/或重要性。以上表述仅用于将一个元件与其它元件区别开的目的。例如，第一用户装置和第二用户装置指示不同用户装置，尽管二者都是用户装置。例如，在不脱离本公开的各种实施例的范围的情况下，第一元件可被称为第二元件，同样地，第二元件也可被称为第一元件。
应注意到：如果描述将一个组成元件“连接”到另一组成元件，则可将第一组成元件直接连接到第二组成元件，并且可在第一组成元件和第二组成元件之间“连接”第三组成元件。相反地，当将一个组成元件“直接连接”到另一组成元件时，可理解为在第一组成元件和第二组成元件之间不存在第三组成元件。
在本公开的各种实施例中使用的术语“用户”可指示使用电子装置的人或使用电子装置的装置(例如，人工智能电子装置)。
在本公开的各种实施例中使用的术语仅用于描述特定实施例的目的并且并非意在限制本公开的各种实施例。如在此所使用，单数形式意在也包括复数形式，除非上下文清楚地另有指示。除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本公开的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本公开的各种实施例中被清楚地限定。
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明，需要注意的是以下计算过程中的坐标系通常采用世界坐标系。
在一个实施例中，如图1所示，提供了一种机器人质心位置调整方法，包括以下步骤：
步骤S110，根据三质量块模型和质心位置规划值获取腰位姿初始值、双脚位姿初始值以及质心位置初始值。
其中，质心位置规划值一般可以根据具体情况对机器人进行预先设置得到，针对普通的双足仿人机器人，三质量块模型通常是指将机器人简化成只有腰、左腿和右腿三个刚体的模型，这样的简化过程，采用质心逆运动学计算方法，可以粗略地依据质心位置规划值计算得到腰位姿初始值、双脚位姿初始值以及质心位置初始值，由于该过程计算过程精度不高，故后续还需要调整计算。
其中，上述双脚位姿初始值是机器人在目标控制系统命令周期内对应的初始值，也就是指机器人在单个控制系统命令周期内对应的初始值，例如，机器人控制系统命令周期通常以1ms为单位，在这1ms内，机器人双脚位姿初始值(包括左脚位姿初始值和右脚位姿初始值)始终保持不变。
其中，上述质心规划值、腰位姿初始值、双脚位姿初始值以及质心位置初始值均采用世界坐标系进行表示。
步骤S120，根据质心位置初始值和质心位置规划值，利用腰位姿变化和质心位置变化对应的线性正相关性，得到腰速度调整值。
其中，腰位姿变化和质心位置变化两个变量的变化方向相同，当机器人双脚位姿固定时，移动机器人腰的位置，机器人的质心会朝相同方向移动，两者的变化满足正相关性；此外，腰位姿和质心位置满足类线性关系(可以近似为线性关系处理)，进而腰位姿和质心位置可满足线性叠加原理，因此腰位姿变化和质心位置变化满足对应的线性正相关性。
在一个实施例中，如图2所示，机器人双脚固定，向右移动机器人的腰，机器人质心也会相应地向右移动，且这种关系接近线性(虽不是标准意义上的线性关系，但是线性相关系数较高，数学上可以作为线性处理)，比如腰向右移动6厘米，质心的位移大概向右移动3厘米，质心的位置变化和腰位姿变化存在线性相关性，在数学上存在可以作为线性处理的特性，因而针对上述腰位姿变化和质心位置变化，可以将上述正相关性和可以作为线性处理的数学特性结合起来，通过质心逆运动学算法构造基于牛顿-拉夫逊循环迭代运算来计算调整机器人对应的质心实际位置。
在一个实施例中，如下述公式所示，Vwaist表示腰速度调整值，COMdeisred表示质心位置规划值，COMactual表示质心位置初始值，(COMdeisred-COMactual)表示质心位置变化量，Kp表示线性调整系数。
Vwaist＝Kp(COMdeisred-COMactual)
其中，上述公式中，Kp的上限是由机器人质量分布决定的。例如质心在腰和脚的正中间，那么Kp上限一般不超过2，这里Kp的取值可根据实际情况进行调整，以满足质心位置变化和实际腰位姿变化实际对应关系为准；Vwaist这里仅仅表示计算过程中虚拟的腰速度调整值。
步骤S130，根据腰位姿初始值、腰速度调整值和双脚位姿初始值，计算得到质心位置当前值。
其中，根据腰位姿初始值和腰速度调整值通过数学积分可以计算得到腰位姿当前值，进一步的，通过采用逆运动学算法，根据腰位姿当前值和双脚位姿初始值可以计算出质心位置当前值。
步骤S140，比较质心位置当前值同质心位置规划值是否相等，若否，则进入步骤S150，若是，则进入步骤S160。
通过上述计算得到的质心位置当前值，是经过计算调整得到的质心位置当前值，需要同质心位置规划值进行进一步比较，如果不相等，则还需要继续进行迭代计算调整，进入步骤S150，如果相等，则进入步骤S160。
步骤S150，获取质心位置当前值作为下一质心位置初始值，并返回至步骤120执行，直至质心位置当前值同质心位置规划值相等。
其中，由于在当前控制系统命令周期内，质心位置规划值、腰位姿初始值和双脚位姿初始值均没有改变，此时返回步骤S120继续执行，则将上一计算过程中得到的质心位置当前值作为下一计算过程中的质心位置初始值进行迭代计算，最后得到该过程对应的质心位置当前值，然后继续同质心位置规划值进行比较，判断是否相等，如果相等，则进入步骤S160，如果不相等，则依然返回进入步骤S120，重复上述计算过程，直至得到的质心位置当前值同上述质心位置规划值相等，然后进入步骤S160。
步骤S160，根据质心位置当前值生成目标控制系统命令。
在当前控制系统命令周期内，上述计算过程可以进行多次，当计算得到的质心位置当前值同当前控制系统命令周期内对应的质心位置规划值相等时，则机器人根据质心位置当前值生成目标控制系统命令，根据目标控制系统命令获取对应的控制参数，从而能够对各个关节的角度进行调整，实现机器人质心实际位置的调整。
图3为一个实施例中应用上述机器人质心位置调整方法的机器人质心位置调整效果示意图，机器人为双足机器人，采用上述机器人质心位置调整方法(与图3中新算法相对应)，有效将计算得到的机器人质心位置当前值同质心位置规划值之间的质心位置误差从厘米降低到了毫米级，有效的减少了机器人行走时的扰动，提高了机器人行走稳定性，增强了平衡控制能力，且上述调整方法实时性好，以一个12自由度仿人机器人为例，可以将质心误差降低到1毫米以下，只需要0.3毫秒的计算力，远小于机器人对应的控制系统命令周期，完全不会影响机器人的实时控制系统。
上述机器人质心位置调整方法，首先通过利用根据三质量块模型和质心位置规划值获取机器人相关初始参数，包括腰位姿初始值、双脚位姿初始值和质心位置初始值，然后根据质心位置初始值和质心位置规划值，利用腰位姿变化和质心位置变化对应的线性正相关性，得到腰速度调整值，进一步的根据腰位姿初始值、腰速度调整值和双脚位姿初始值计算得到质心位置当前值，最后比较质心位置当前值同质心位置规划值是否相等，若否，则获取质心位置当前值作为下一质心位置初始值，并返回至根据质心位置初始值和质心位置规划值的步骤执行，直至质心位置当前值同质心位置规划值相等；若是，则根据质心位置当前值生成目标控制系统命令。
上述机器人质心位置调整方法巧妙的利用了机器人在当前控制系统命令周期内双脚位姿固定不变的情况下，腰位姿变化和质心位置变化对应的线性正相关性，采用腰位姿变化量来对质心位置进行反馈调整，通过多次进行迭代循环计算，大幅降低了机器人质心规划位置同按照三质量块模型计算出来的机器人质心实际位置之间的误差，有效的减少了机器人行走时的扰动，提高了机器人行走稳定性，增强了平衡控制能力。
在一个实施例中，如图4所示，上述机器人质心位置调整方法还包括：
步骤S170，根据线性倒立摆模型和机器人预设参数，生成质心位置规划值。
其中，利用线性倒立摆模型，结合机器人规划速度和规划步长，能够生成关于质心位置的时间轨迹，进一步可以获得当前控制系统命令周期内对应的质心位置规划值。
在一个实施例中，如图5所示，步骤S110包括：
步骤S112，根据三质量块模型和质心位置规划值，获取腰位姿初始值和双脚位姿初始值。
在双足机器人控制领域，计算机器人行走时质心实际位置时，通常采用三质量块模型对机器人结构进行简化，即将机器人简化成只有腰、左腿以及右腿三个刚体的模型。在当前控制系统命令周期内，通过上述模型，这样可以粗略的将质心位置的时间轨迹转化为腰位姿和双脚位姿的时间轨迹，进一步的可以将当前控制系统命令周期内对应的质心位置规划值转化为腰位姿初始值和双脚位姿初始值。
步骤S114，根据腰位姿初始值和双脚位姿初始值，计算得到质心位置初始值。
其中，根据腰位姿初始值和双脚位姿初始值均采用世界坐标系，通过逆运动学算法计算得到质心位置初始值。
在一个实施例中，如图6所示，其中,机器人为双足机器人，机器人每条腿有六个电机，每条腿中有三个电机的转轴相较于机器人髋部中心。例如在左腿中，左腿和髋部交汇处设置有三个电机，膝盖位置设置有一个电机，另外还有两个电机转轴相较于脚踝，左腿总共六个电机，这里为方便起见，不妨假设1号电机负责大腿旋转，2号电机大腿侧摆，3号电机大腿前摆，4号电机小腿前摆，5号电机脚踝前摆，6号电机脚踝侧摆。
图6中，所有坐标系都是Z轴向上，X轴向前，Y轴向左。为更容易理解，左腿和右腿整体上具有对应的相同标注，其中(X0，Y0，Z0)是腰坐标系，(图中为直观显示只标注了Z0)，(X1，Y1，Z1)代表髋旋转坐标系，(X2，Y2，Z2)代表髋侧摆坐标系，(X3，Y3，Z3)代表髋前摆坐标系，(X4，Y4，Z4)代表小腿前摆坐标系，(X5，Y5，Z5)代表脚踝前摆坐标系，(X6，Y6，Z6)代表脚踝外摆坐标系，foot是脚底坐标系(简称脚坐标系)，Z1、X2、Y3、Y4、Y5和X6分别同1-6号电机各自的转动轴相平行。Rfoot和Lfoot分别代表左脚坐标系和右脚坐标系，与上述其他坐标系相同的零位朝向。
上述根据腰位姿初始值和双脚位姿初始值，采用逆运动学算法计算得到质心位置初始值的过程中，包括腰位姿逆运动学算法和质心位置逆运动学算法两个大步骤，其中，关键是先求得脚踝到臀关节的矢量并在脚踝坐标系中的表示。
在一个实施例中，以右脚悬空为例，采用腰位姿逆运动学进行计算右腿的六个初始关节角θ1、θ2、θ3、θ4、θ5和θ6，根据右脚位姿初始值和腰位姿初始值，可以求得出右脚相对于腰的位姿，这里用wTf表示，计算右腿的六个初始关节角θ1、θ2、θ3、θ4、θ5和θ6，分别表示初始大腿旋转角、初始大腿侧摆角、初始大腿前摆角、初始小腿前摆角、初始脚踝前摆角和初始脚踝侧摆角。
右脚在腰坐标系的初始位姿用wTf表示，6Tf表示右脚位姿初值在右脚踝外摆坐标系(X6，Y6，Z6)中的齐次变换矩阵，利用齐次变换矩阵的链式法则有：
Figure BDA0001928912390000121
Figure BDA0001928912390000131
Figure BDA0001928912390000132
脚踝到髋关节矢量在脚踝坐标系的表示：
Figure BDA0001928912390000133
然后利用余弦定理计算出θ4，如图7所示，θ4是初始小腿前摆角(膝盖弯曲角)，因为膝盖只能向后弯，它的范围是[-π，0]：
r＝norm(d6,0)
Figure BDA0001928912390000134
将髋关节投影在脚踝坐标系的y-z平面，可以计算脚踝的侧摆θ6：
θ6＝atan2(-ry,rz)
利用θ4,θ6的信息，将髋关节投影到脚踝坐标系的x-z平面，可以计算脚踝的前摆θ5:
Figure BDA0001928912390000135
Figure BDA0001928912390000136
θ5＝-γ-β
髋关节处三个旋转轴彼此正交，分别对应θ1,θ2,θ3,它们三个产生的姿态可以用θ4,θ5,θ6和wRf反推出：
0R3·3R4·4R5·5R6·＝wRf
0R3＝wRf·(3R4·4R5·5R6)T
θ1＝atan2(0R3(2,1),0R3(2,2))
θ2＝atan2(-0R3(2,3),0R3(2,1)·sinθ1+0R3(2,2)·cosθ1)θ3＝atan2(0R3(1,3),0R3(3,3))
进一步的，采用质心位置算法计算质心位置初始值，根据上述右腿的六个初始关节角θ1、θ2、θ3、θ4、θ5和θ6，结合腰位姿初始值,通过正向运动学质心位置算法可以得到质心位置初始值，具体过程如下:
Figure BDA0001928912390000141
质心位置初始值rcom等于各刚体质心位置初始值ri对它们各自质量mi的加权平均，每个刚体的质心位置初始值ri，可以通过正向运动学算法得到。
其中，每个刚体的坐标系原点在世界坐标系中的位置用rOi表示，rOi可以通过齐次变换矩阵的链式法则连乘得到，每个刚体的齐次变换矩阵可以通过述右腿的六个初始关节角(θ1、θ2、θ3、θ4、θ5和θ6)和腰位姿初始值计算得到，comi是每个刚体质心位置初始值到该刚体自身坐标系原点的矢量，每个刚体在世界坐标系中的姿态用旋转矩阵Ri表示，icomi表示每个刚体质心位置初始值在自身刚体坐标系中的矢量(通常是一个定值，在机械设计时通过Solidworks等软件得到)。
在一个实施例中，如图8所示，步骤S130包括：
步骤S132，根据所述腰位姿初始值、所述腰速度调整值和所述双脚位姿初始值，采用腰位姿逆运动学算法，计算得到每个关节的关节角度。
根据腰位姿初始值和腰速度调整值，可以计算得到腰位姿当前值，进一步的采用腰位置逆运动学算法，根据腰位姿当前值和双脚位姿初始值计算得到每个关节的关节角度，该计算方法同步骤S110中计算每个关节的初始关节角的计算方法基本相同。
步骤S134，根据所述腰位姿初始值、所述腰速度调整值和所述每个关节的关节角度，采用质心位置逆运动学算法，计算得到质心位置当前值。
该步骤中根据腰位姿初始值和腰速度调整值，可以计算得到腰位姿当前值，进一步的采用质心位置逆运动学算法，计算得到质心位置当前值，该计算过程同上述步骤S110中计算质心位置初始值的过程基本相同。
此外，还提供一种机器人质心位置调整装置，如图9所示，包括：
初始值获取单元210，用于根据三质量块模型和质心位置规划值获取腰位姿初始值、双脚位姿初始值以及质心位置初始值；
腰速度调整值获取单元220，用于根据质心位置初始值和质心位置规划值，利用腰位姿变化和质心位置变化对应的线性正相关性，得到腰速度调整值；
质心位置当前值获取单元230，用于根据腰位姿初始值、腰速度调整值、左脚位姿初始值和右脚位姿初始值，计算得到质心位置当前值；
判断单元240，用于比较质心位置当前值同质心位置规划值是否相等，若否，则进入循环执行单元；
循环执行单元250，用于获取质心位置当前值作为下一质心位置初始值，并返回至腰速度调整值获取单元执行，直至质心位置当前值同质心位置规划值相等；若是，则进入命令生成单元。
命令生成单元260，用于根据所述质心位置当前值生成目标控制系统命令。
在一个实施例中，如图10所示，上述装置还包括：
规划单元270，用于根据线性倒立摆模型和机器人预设参数，生成质心位置规划值。
在一个实施例中，如图11所示，初始值获取单元210包括：
第一子单元212，用于根据三质量块模型和质心位置规划值，获取腰位姿初始值和双脚位姿初始值；
第二子单元214，用于根据腰位姿初始值、左脚位姿初始值以及右脚位姿初始值，计算得到质心位置初始值。
在一个实施例中，如图12所示，质心位置当前值获取单元230包括：
关节角度获取子单元232，用于根据所述腰位姿初始值、所述腰速度调整值和所述双脚位姿初始值，采用腰位姿逆运动学算法，计算得到每个关节的关节角度；
质心位置当前值获取子单元234，用于根据根据所述腰位姿初始值、所述腰速度调整值和所述每个关节的关节角度，采用质心位置逆运动学算法，计算得到质心位置当前值。
此外，还提供了一种机器人，包括存储器以及处理器，存储器用于存储计算机程序，处理器运行计算机程序以使机器人执行任上述机器人质心位置调整方法。
此外，还提供了一种数据存储介质，存储有上述机器人所使用的计算机程序。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种机器人质心位置调整方法，其特征在于，所述方法包括：
根据三质量块模型和质心位置规划值获取腰位姿初始值、双脚位姿初始值以及质心位置初始值；
根据所述质心位置初始值和所述质心位置规划值，利用腰位姿变化和质心位置变化对应的线性正相关性，得到腰速度调整值；
根据所述腰位姿初始值、所述腰速度调整值和所述双脚位姿初始值，计算得到质心位置当前值；
比较所述质心位置当前值同所述质心位置规划值是否相等，若否，则获取所述质心位置当前值作为下一质心位置初始值，并返回至所述根据所述质心位置初始值和所述质心位置规划值，利用腰位姿变化和质心位置变化对应的线性正相关性，得到腰速度调整值的步骤执行，直至所述质心位置当前值同所述质心位置规划值相等；若是，则根据所述质心位置当前值生成目标控制系统命令。
2.根据权利要求1所述的方法，其特征在于，所述根据三质量块模型和质心位置规划值获取对应的腰位姿初始值、双脚位姿初始值以及质心位置初始值的步骤之前还包括：
根据线性倒立摆模型和机器人预设参数，生成质心位置规划值。
3.根据权利要求1所述的方法，其特征在于，所述根据三质量块模型和质心位置规划值获取对应的腰位姿初始值、双脚位姿初始值以及质心位置初始值的步骤包括：
根据三质量块模型和质心位置规划值，获取腰位姿初始值和双脚位姿初始值；
根据所述腰位姿初始值以及所述双脚位姿初始值，计算得到质心位置初始值。
4.根据权利要求1所述的方法，其特征在于，所述根据所述腰位姿初始值、所述腰速度调整值和所述双脚位姿初始值，计算得到质心位置当前值的步骤包括：
根据所述腰位姿初始值、所述腰速度调整值和所述双脚位姿初始值，采用腰位姿逆运动学算法，计算得到每个关节的关节角度；
根据所述腰位姿初始值、所述腰速度调整值和每个关节的关节角度，采用质心位置逆运动学算法，计算得到质心位置当前值。
5.一种机器人质心位置调整装置，其特征在于，所述装置包括：
初始值获取单元，用于根据三质量块模型和质心位置规划值获取腰位姿初始值、双脚位姿初始值以及质心位置初始值；
腰速度调整值获取单元，用于根据所述质心位置初始值和所述质心位置规划值，利用腰位姿变化和质心位置变化对应的线性正相关性，得到腰速度调整值；
质心位置当前值获取单元，用于根据所述腰位姿初始值、所述腰速度调整值和所述双脚位姿初始值，计算得到质心位置当前值；
判断单元，用于比较所述质心位置当前值同所述质心位置规划值是否相等，若否，则进入循环执行单元；若是，则进入命令生成单元；
循环执行单元，用于获取所述质心位置当前值作为下一质心位置初始值，并返回至所述腰速度调整值获取单元执行，直至所述质心位置当前值同所述质心位置规划值相等；
命令生成单元，用于根据所述质心位置当前值生成目标控制系统命令。
6.根据权利要求5所述的装置，其特征在于，所述装置还包括：
规划单元，用于根据线性倒立摆模型和机器人预设参数，生成质心位置规划值。
7.根据权利要求5所述的装置，其特征在于，所述初始值获取单元包括：
第一子单元，用于根据三质量块模型和质心位置规划值，获取腰位姿初始值和双脚位姿初始值；
第二子单元，用于根据所述腰位姿初始值以及所述双脚位姿初始值，计算得到质心位置初始值。
8.根据权利要求5所述的装置，其特征在于，所述质心位置当前值获取单元包括：
关节角度获取子单元，用于根据所述腰位姿初始值、所述腰速度调整值和所述双脚位姿初始值，采用腰位姿逆运动学算法，计算得到每个关节的关节角度；
质心位置当前值获取子单元，用于根据所述腰位姿初始值、所述腰速度调整值和每个关节的关节角度，采用质心位置逆运动学算法，计算得到质心位置当前值。
9.一种机器人，包括存储器以及处理器，所述存储器用于存储计算机程序，所述处理器运行所述计算机程序以使所述机器人执行根据权利要求1至4中任一项所述的机器人质心位置调整方法。
10.一种数据存储介质，其特征在于，其存储有权利要求9所述机器人所使用的所述计算机程序。
CN201811630615.7A 2018-12-29 2018-12-29 机器人质心位置调整方法、装置、机器人和数据存储介质 Active CN109703645B (zh)

Priority Applications (4)

Application Number Priority Date Filing Date Title
CN201811630615.7A CN109703645B (zh) 2018-12-29 2018-12-29 机器人质心位置调整方法、装置、机器人和数据存储介质
US16/587,069 US11353887B2 (en) 2018-12-29 2019-09-30 Robot centroid position adjustment method and apparatus and robot using the same
JP2019230711A JP6961669B2 (ja) 2018-12-29 2019-12-20 ロボット重心位置調整方法、装置、ロボット及びデータ記憶媒体
KR1020190172878A KR102268286B1 (ko) 2018-12-29 2019-12-23 로봇 중심 위치 조정방법, 장치, 로봇 및 데이터 기억매체

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811630615.7A CN109703645B (zh) 2018-12-29 2018-12-29 机器人质心位置调整方法、装置、机器人和数据存储介质

Publications (2)

Publication Number Publication Date
CN109703645A CN109703645A (zh) 2019-05-03
CN109703645B true CN109703645B (zh) 2020-05-22

Family

ID=66259327

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811630615.7A Active CN109703645B (zh) 2018-12-29 2018-12-29 机器人质心位置调整方法、装置、机器人和数据存储介质

Country Status (4)

Country Link
US (1) US11353887B2 (zh)
JP (1) JP6961669B2 (zh)
KR (1) KR102268286B1 (zh)
CN (1) CN109703645B (zh)

Families Citing this family (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110244791B (zh) * 2019-07-11 2020-05-15 北京理工大学 一种双足机器人足部力和力矩跟随控制方法
WO2021208917A1 (zh) * 2020-04-14 2021-10-21 腾讯科技（深圳）有限公司 质心位置确定方法、装置、足式机器人及存储介质
CN111736615B (zh) * 2020-08-04 2020-12-15 深圳市优必选科技股份有限公司 一种步态规划方法、装置、计算机可读存储介质及机器人
CN111730605B (zh) * 2020-08-04 2020-12-15 深圳市优必选科技股份有限公司 一种机器人姿态控制方法、装置、可读存储介质及机器人
CN112114589B (zh) * 2020-09-07 2024-02-09 腾讯科技（深圳）有限公司 足式机器人运动控制方法、装置、设备及介质
CN112182787B (zh) * 2020-11-03 2022-08-05 中国运载火箭技术研究院 确定运载火箭的总体偏差量的方法、系统、终端及介质
WO2022126433A1 (zh) * 2020-12-16 2022-06-23 深圳市优必选科技股份有限公司 仿人机器人的类人步态控制方法、装置、设备及存储介质
CN112720483B (zh) * 2020-12-23 2022-04-19 深圳市优必选科技股份有限公司 合质心状态获取方法、装置、仿人机器人和可读存储介质
CN112731952B (zh) * 2020-12-24 2022-03-01 深圳市优必选科技股份有限公司 一种机器人质心规划方法、装置、可读存储介质及机器人
CN112947312B (zh) * 2021-01-28 2021-09-14 仲恺农业工程学院 一种农业机器人运动控制方法
CN112947069B (zh) * 2021-01-28 2022-10-28 内蒙古大学 一种移动两轮机器人的控制方法
CN113146638B (zh) * 2021-04-30 2022-09-16 深圳市优必选科技股份有限公司 质心位姿估计方法、装置、计算机可读存储介质及机器人
CN113246123B (zh) * 2021-04-30 2022-10-18 深圳市优必选科技股份有限公司 机器人控制方法、装置、计算机可读存储介质及机器人
CN113290559B (zh) * 2021-05-26 2022-11-11 深圳市优必选科技股份有限公司 机器人平衡控制方法、装置、机器人控制设备及存储介质
CN113283082B (zh) * 2021-05-26 2023-12-15 深圳市优必选科技股份有限公司 质心轨迹生成方法、装置、计算机可读存储介质及机器人
CN113359792A (zh) * 2021-05-27 2021-09-07 深圳市优必选科技股份有限公司 一种双足机器人控制方法、装置、双足机器人及存储介质
CN114161401B (zh) * 2021-11-17 2023-12-15 深圳市优必选科技股份有限公司 质心高度规划方法、装置、计算机可读存储介质及机器人
CN114083540B (zh) * 2021-12-01 2024-01-02 大连海事大学 一种基于类阻尼现象的运动模型构建方法
CN114750162B (zh) * 2022-05-17 2023-10-17 北京理工大学 一种仿人机器人高动态运动质心柔顺控制方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7119510B2 (en) * 2003-04-17 2006-10-10 Honda Motor Co., Ltd. Method of assuming acting point of floor reaction force to biped walking mobile body and method of assuming joint moment of biped walking mobile body
KR20130078886A (ko) * 2012-01-02 2013-07-10 현대자동차주식회사 보행로봇의 균형제어방법
CN104252173A (zh) * 2013-06-27 2014-12-31 广州中国科学院先进技术研究所 双足步行机器人的步行控制方法
CN106547206A (zh) * 2016-06-29 2017-03-29 中南大学 一种基于脚力估计算法的多足机器人动态稳定性定量判定方法
CN109032142A (zh) * 2018-08-14 2018-12-18 浙江大学 一种含腰部结构的双足机器人设计以及反馈控制方法

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3660332B2 (ja) * 2002-08-29 2005-06-15 独立行政法人科学技術振興機構 脚式移動装置の歩行・走行制御方法および脚式移動装置の歩行・走行制御装置
WO2005098733A2 (en) * 2004-03-31 2005-10-20 Honda Motor Co., Ltd. Systems and methods for controlling a legged robot based on rate of change of angular momentum
JP5251253B2 (ja) * 2008-05-20 2013-07-31 株式会社安川電機 脚式歩行ロボットの安定化制御装置
US8942848B2 (en) * 2011-07-06 2015-01-27 Florida Institute for Human and Machine Cognition Humanoid robot that can dynamically walk with limited available footholds in the presence of disturbances
JP5661023B2 (ja) * 2011-12-02 2015-01-28 本田技研工業株式会社 脚式移動ロボットの歩容生成装置及びロボットの動作目標生成装置
CN103217903B (zh) 2013-04-07 2016-01-20 南京邮电大学 基于双平衡控制机制的仿人足球机器人全向踢球方法
JP6450279B2 (ja) * 2015-08-05 2019-01-09 本田技研工業株式会社 移動ロボットの目標ｚｍｐ軌道の生成装置
KR20180004397A (ko) * 2017-12-28 2018-01-11 삼성전자주식회사 보행 로봇 및 그 자세 제어 방법

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7119510B2 (en) * 2003-04-17 2006-10-10 Honda Motor Co., Ltd. Method of assuming acting point of floor reaction force to biped walking mobile body and method of assuming joint moment of biped walking mobile body
KR20130078886A (ko) * 2012-01-02 2013-07-10 현대자동차주식회사 보행로봇의 균형제어방법
CN104252173A (zh) * 2013-06-27 2014-12-31 广州中国科学院先进技术研究所 双足步行机器人的步行控制方法
CN106547206A (zh) * 2016-06-29 2017-03-29 中南大学 一种基于脚力估计算法的多足机器人动态稳定性定量判定方法
CN109032142A (zh) * 2018-08-14 2018-12-18 浙江大学 一种含腰部结构的双足机器人设计以及反馈控制方法

Also Published As

Publication number Publication date
KR20200083259A (ko) 2020-07-08
CN109703645A (zh) 2019-05-03
US11353887B2 (en) 2022-06-07
JP2020108915A (ja) 2020-07-16
KR102268286B1 (ko) 2021-06-22
JP6961669B2 (ja) 2021-11-05
US20200209890A1 (en) 2020-07-02

Similar Documents

Publication Publication Date Title
CN109703645B (zh) 机器人质心位置调整方法、装置、机器人和数据存储介质
US11022983B2 (en) Gait control method, device, and terminal device for biped robot
US7076337B2 (en) Ambulation control apparatus and ambulation control method of robot
US11045945B2 (en) Method for controlling walking of robot and robot
US9073209B2 (en) Walking robot and control method thereof
US6898485B2 (en) Device and method for controlling operation of legged robot, and robot device
Chen et al. Bioinspired control of walking with toe-off, heel-strike, and disturbance rejection for a biped robot
Tlalolini et al. Comparison of different gaits with rotation of the feet for a planar biped
US20200393490A1 (en) Information processing device, information processing method, and storage medium
CN112643679A (zh) 机器人运动控制方法、装置、控制器及存储介质
Goddard et al. Control of the heel-off to toe-off motion of a dynamic biped gait
Buss et al. Towards an autonomous, humanoid, and dynamically walking robot: Modeling, optimal trajectory planning, hardware architecture, and experiments
Fevre et al. Design and experimental validation of a velocity decomposition-based controller for underactuated planar bipeds
CN113618740B (zh) 双足机器人舞蹈平衡控制方法、装置和双足机器人
JP2002210681A (ja) 脚式移動ロボットのための動作制御装置及び動作制御方法、並びにロボット装置
Nguyen et al. Proposing a Graphic Simulator for an Upper Limb Exoskeleton Robot
Siqueira et al. Nonlinear H∞ control applied to biped robots
Shala et al. Modelling of the Biped Robot with 10 DoF
Sun et al. Hierarchical Trajectory Optimization for Humanoid Robot Jumping Motion
Tlalolini et al. Optimal reference motions with rotation of the feet for a biped
Shala et al. Modelling of the biped robot using Maple Software
Pan et al. Towards an Integrated System for Estimating Multi-joint Movement from Diverse Sensor Data
Kim et al. Kookmin University Dream of Soccer Team Description for Humanoid Kidsize League of RoboCup 2013
Naksuk Whole-body human-to-humanoid motion transfer with balance manipulability ellipsoid
Syrseloudis Efficient algorithms for the study and design of parallel robotic mechanisms with physiotherapy applications

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
CB03 Change of inventor or designer information

Inventor after: Xiong Youjun

Inventor after: Wang Yuesong

Inventor before: Xiong Youjun

Inventor before: Wang Yuesong

Inventor before: Zhao Mingguo

CB03 Change of inventor or designer information
CP03 Change of name, title or address

Address after: Room 2201, Building C1, Nanshan Zhiyuan, No. 1001, Xueyuan Avenue, Changyuan Community, Taoyuan Street, Nanshan District, Shenzhen, Guangdong 518000

Patentee after: Shenzhen Youbixuan Technology Co.,Ltd.

Country or region after: China

Address before: 518000 16th and 22nd Floors, C1 Building, Nanshan Zhiyuan, 1001 Xueyuan Avenue, Nanshan District, Shenzhen City, Guangdong Province

Patentee before: Shenzhen Youbixuan Technology Co.,Ltd.

Country or region before: China

CP03 Change of name, title or address