CN111558941A - 浮动基动力学前馈控制方法、装置和多足机器人 - Google Patents

浮动基动力学前馈控制方法、装置和多足机器人 Download PDF

Info

Publication number
CN111558941A
CN111558941A CN202010671790.1A CN202010671790A CN111558941A CN 111558941 A CN111558941 A CN 111558941A CN 202010671790 A CN202010671790 A CN 202010671790A CN 111558941 A CN111558941 A CN 111558941A
Authority
CN
China
Prior art keywords
base
joint
coordinate system
robot
acceleration
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
CN202010671790.1A
Other languages
English (en)
Other versions
CN111558941B (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 CN202010671790.1A priority Critical patent/CN111558941B/zh
Publication of CN111558941A publication Critical patent/CN111558941A/zh
Application granted granted Critical
Publication of CN111558941B publication Critical patent/CN111558941B/zh
Priority to US17/088,596 priority patent/US11579591B2/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • 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
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
    • G05B19/4155—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by programme execution, i.e. part programme or machine function execution, e.g. selection of a programme
    • 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
    • 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/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
    • 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/1651—Programme controls characterised by the control loop acceleration, rate control
    • 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/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • 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
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
    • G05B19/416—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by control of velocity, acceleration or deceleration
    • G05B19/4166—Controlling feed or in-feed
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/34—Director, elements to supervisory
    • G05B2219/34013—Servocontroller
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40077—Posicast, inverted pendulum, acrobat, balance rod
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40264—Human like, type robot arm
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/50—Machine tool, machine tool null till machine tool work handling
    • G05B2219/50391—Robot

Landscapes

  • Engineering & Computer Science (AREA)
  • Mechanical Engineering (AREA)
  • Robotics (AREA)
  • Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Human Computer Interaction (AREA)
  • Manufacturing & Machinery (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Chemical & Material Sciences (AREA)
  • Combustion & Propulsion (AREA)
  • Transportation (AREA)
  • Manipulator (AREA)

Abstract

本发明实施例公开了一种浮动基动力学前馈控制方法、装置和多足机器人，该方法包括：根据预先规划的机器人运动轨迹基于逆运动学获取每一时刻各关节的运动学参数，将机器人的躯体中心作为浮动的基座；根据各关节的运动学参数通过正运动学求解各关节质心在基座坐标系下的六维加速度，将各关节质心的该六维加速度转换为世界坐标系下的六维加速度；根据各关节质心在世界坐标系下的六维加速度利用逆动力学计算各关节电机所需的力矩并进行前馈控制。相对于现有方案的固定基座的机器人逆动力学算法而言，本发明可简化机器人逆动力学算法，降低计算复杂度且计算量，有效改善在行走过程中因单双脚切换负载突变而造成的转矩突变等问题。

Description

浮动基动力学前馈控制方法、装置和多足机器人
技术领域
本发明涉及机器人控制技术领域，尤其涉及一种浮动基动力学前馈控制方法、装置和多足机器人。
背景技术
对于逆动力学算法的应用，例如，其主要应用场景便是具有固定基座、每个关节依次相连的串联结构机器人。通过给定机器人末端执行器跟随时间的轨迹，基于固定基座利用全动力学方程求解每个关节每时刻所需要的力矩（也称转矩），进而对各关节电机进行位置控制或直接力控制，以实现所需的机器人运动。
然而，对于该固定基逆动力学算法也存在一些不足之处，例如，应用场景有限，该算法无法直接应用到移动的机器人上，如双足或四足等机器人。目前，很多时候为了能应用该固定基座逆运动学算法进行机器人运动控制，通常采用的是对机器人的运动分阶段分状态。比如，双足机器人在行走过程中会分为单足支撑期和双足支撑期。而对于这两种状态，需要采用不同的模型进行逆动力学求解，尤其是双足支撑期，计算复杂度高且计算量太大。此外，在双足机器人进行这两种状态转换时，关节转矩还会出现跳变现象，这给机器人在行走过程中的运动控制带来很大难度等。
发明内容
有鉴于此，本发明目的是为了克服现有技术中的不足，提供一种浮动基动力学前馈控制方法、装置和多足机器人。
本发明的一实施例提供一种浮动基动力学前馈控制方法，包括：
根据预先规划的机器人运动轨迹基于逆运动学获取每一时刻所述机器人各关节的运动学参数，将所述机器人的躯体中心作为浮动的基座；
根据各关节的所述运动学参数通过正运动学算法求解各关节质心在基座坐标系下的六维加速度，将各关节质心在基座坐标系下的六维加速度转换为世界坐标系下的六维加速度；
根据获取的各关节质心在世界坐标系下的六维加速度利用逆动力学算法计算各关节电机所需的力矩，基于计算出的所述力矩通过前馈控制方式对所述机器人对应的关节电机进行控制。
在一些实施例中，所述运动学参数包括关节的角度、角速度和角加速度，所述正运动学的方程如下：
Figure 311932DEST_PATH_IMAGE001
；
其中，
Figure 494651DEST_PATH_IMAGE002
代表第i个关节质心在基座坐标系下的六维加速度；
Figure 743230DEST_PATH_IMAGE003
为第i个关节质心相对 于基座的雅克比矩阵；
Figure 341702DEST_PATH_IMAGE004
为雅克比矩阵的求导矩阵；
Figure 874183DEST_PATH_IMAGE005
表示各关节的角速度向量；
Figure 962225DEST_PATH_IMAGE006
表示 各关节的角加速度向量。
在一些实施例中，所述机器人的躯体上设有惯性测量单元，所述将各关节质心在基座坐标系下的六维加速度转换为世界坐标系下的六维加速度，包括：
根据所述惯性测量单元测量得到的所述浮动的基座的姿态和所述机器人的着地脚通过正运动学算法计算所述浮动的基座在世界坐标系下的三维角速度和六维加速度；
基于所述浮动的基座的所述姿态以及在世界坐标系下的所述三维角速度和六维加速度通过非惯性系到惯性系加速度转化操作符对各关节质心在基座坐标系下的六维加速度进行转换，以得到所述关节质心在世界坐标系下的六维加速度。
在一些实施例中，所述非惯性系到惯性系加速度转化操作符的计算公式如下：
∂word_i=∂base+R base•∂i+w base×R base•v i+β base×R base•r i+w base×w base×R base•r i+w base×R base•v i；
β word_i=β base+w base×R base•w i+R base•β i；
其中，∂word_i和β word_i分别为第i个关节质心在世界坐标系下的线加速度、角加速度；∂base和β base分别为基座在世界坐标系下的线加速度、角加速度；w base为基座在世界坐标系下的三维角速度；R base为基座在世界坐标系下的姿态矩阵；r i为第i个关节质心在基座坐标系下的矢量；v i和w i分别为第i个关节质心在基座坐标系下的线速度、角速度；∂i和β i分别为第i个关节质心在基座坐标系下的线加速度、角加速度。
在一些实施例中，若所述机器人的单脚自由度为n，所述根据获取的各关节质心在世界坐标系下的六维加速度利用逆动力学算法计算各关节电机所需的力矩，包括：
从作为第n连杆的末端杆开始，根据获取的对应关节质心在世界坐标系下的六维加速度和所述末端杆所受的外力和外力矩通过牛顿欧拉公式求解所述末端杆与连接的第n-1连杆之间的作用力及作用力矩，根据所述作用力矩计算所述末端杆所需的力矩；
将所述作用力和所述作用力矩作为所述第n-1连杆所受的外力和外力矩，利用牛顿欧拉公式求解所述第n-1连杆所需的力矩以及所述第n-1连杆与第n-2连杆之间的作用力及作用力矩，如此迭代求解，直到求解得到所有关节所需的力矩。
在一些实施例中，当所述末端杆与外界无接触时，所述末端杆所受的外力为零；当所述末端杆与地面接触时，所述末端杆所受的外力为地面的支撑力。
在一些实施例中，所述末端杆与连接的第n-1连杆之间的作用力
Figure 698100DEST_PATH_IMAGE007
的计算公 式如下：
Figure 100262DEST_PATH_IMAGE008
；
其中，
Figure 237982DEST_PATH_IMAGE009
为该末端杆的质量；
Figure 496925DEST_PATH_IMAGE010
为该末端杆在世界坐标系下的三维线加速度；
Figure 703785DEST_PATH_IMAGE011
为重力加速度；
Figure 909638DEST_PATH_IMAGE012
为该末端杆所受的外力。
在一些实施例中，所述末端杆与连接的第n-1连杆之间的作用力矩的求解过程，包括：
构建所述末端杆在质心处的转矩方程及满足的欧拉公式，以计算所述第n-1连杆对所述末端杆相互作用的作用力矩；
所述末端杆在质心处的合力矩应满足的转矩方程及欧拉公式分别为：
Figure 698603DEST_PATH_IMAGE013
；
Figure 597288DEST_PATH_IMAGE014
；
其中，
Figure 42176DEST_PATH_IMAGE015
为该末端杆在质心处的合力矩；
Figure 58846DEST_PATH_IMAGE016
为外力矩；
Figure 967897DEST_PATH_IMAGE017
为第n-1连杆对末端 杆相互作用的作用力矩；
Figure 771905DEST_PATH_IMAGE018
为第n-1杆接触点到该末端杆质心的矢量；
Figure 704089DEST_PATH_IMAGE019
为地面接触点到 该末端杆质心的矢量；
Figure 251745DEST_PATH_IMAGE020
为该末端杆的转动惯量；
Figure 15301DEST_PATH_IMAGE021
为该末端杆的关节角加速度矢量；
Figure 239478DEST_PATH_IMAGE022
为该末端杆的关节角速度矢量。
在一些实施例中，所述根据所述作用力矩计算所述末端杆所需的力矩包括：
将所述第n-1连杆对所述末端杆相互作用的作用力矩投影至电机旋转轴以得到所述末端杆所需的力矩；
所述末端杆所需的力矩的计算公式为：
Figure 658958DEST_PATH_IMAGE023
；
其中，
Figure 807043DEST_PATH_IMAGE024
为该末端杆所需的力矩；
Figure 628368DEST_PATH_IMAGE025
为电机旋转轴的方向矢量。
本发明的另一实施例提出一种浮动基动力学前馈控制装置，包括：
关节角度获取模块，用于根据预先规划的机器人运动轨迹基于逆运动学获取每一时刻所述机器人各关节的运动学参数，将所述机器人的躯体中心作为浮动的基座；
惯性系修正模块，用于根据各关节的所述运动学参数通过正运动学算法求解各关节质心在基座坐标系下的六维加速度，并将各关节质心在基座坐标系下的六维加速度转换为世界坐标系下的六维加速度；
前馈控制模块，用于根据获取的各关节质心在世界坐标系下的六维加速度利用逆动力学算法计算各关节电机所需的力矩，基于计算的所述力矩通过前馈控制方式对所述机器人对应的关节电机进行控制。
本发明的又一实施例提出一种多足机器人，采用上述的浮动基动力学前馈控制方法对所述多足机器人进行控制。
本发明的再一实施例提出一种计算机可读存储介质，其存储有计算机程序，在所述计算机程序被执行时，实施上所述的浮动基动力学前馈控制方法。
本发明的实施例具有如下优点：
本发明实施例的浮动基动力学前馈控制方法通过将机器人的躯体中心作为机器人的基座，并根据给定的机器人的运动轨迹求解每一时刻各电机所需力矩以进行前馈控制，相对于现有方案的固定基座的机器人逆动力学算法而言，该方法可简化机器人逆动力学算法，降低计算复杂度且计算量，有效地改善在行走过程中因单双脚切换负载突变而造成的转矩突变等问题。
附图说明
为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对本发明保护范围的限定。在各个附图中，类似的构成部分采用类似的编号。
图1示出了基于固定基座的机器人轨迹规划的应用示意图；
图2示出了本发明实施例的浮动基动力学前馈控制方法的第一流程示意图；
图3示出了本发明实施例的浮动基动力学前馈控制方法的第二流程示意图；
图4示出了本发明实施例的浮动基动力学前馈控制方法的第三流程示意图；
图5示出了本发明实施例的浮动基动力学前馈控制方法的串联机器人末端的受力及所受力矩示意图；
图6示出了三自由度机械臂的基于固定基座与浮动基座的原理示意图；
图7示出了三自由度机械臂中的第1号电机分别基于固定基与浮动基的逆动力学进行运动控制的测试结果；
图8示出了三自由度机械臂中的第2号电机分别基于固定基与浮动基的逆动力学进行运动控制的测试示意图；
图9示出了三自由度机械臂中的第3号电机分别基于固定基与浮动基的逆动力学进行运动控制的测试示意图；
图10示出了基于固定基座与浮动基座的双足机器人在单腿支撑的模型示意图；
图11示出了本发明实施例的浮动基动力学前馈控制装置的结构示意图。
主要元件符号说明：
10-浮动基动力学前馈控制装置；110-关节角度获取模块；120-惯性系修正模块；130-前馈控制模块。
具体实施方式
下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。
通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
在下文中，可在本发明的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。
此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本发明的各种实施例中被清楚地限定。
以双足机器人为例，对于传统的双足机器人的轨迹规划，通常采用方法是，使用步态模型，如线性倒立摆、弹簧线性倒立摆等，并根据机器人的行进速度产生该双足机器人质心跟随时间的轨迹，如图1所示的质心轨迹1。接着，根据该双足机器人的单脚支撑期和双脚支撑期的时间分配，分别产生双脚末端的轨迹，如图1所示的足端轨迹2和轨迹3。最后，利用固定基座的机器人逆运动学和逆动力学原理计算出各关节电机所需的位置指令、速度指令或电流（转矩）指令。
其中，在使用固定基逆动力学求解时，该双足机器人在单脚支撑和双脚支撑时，所采用的模型并不相同。例如，在单脚支撑时，模型将以一只着地脚为基座，把该双足机器人当做十几自由度的串联机械臂来进行求解，其计算量太大。而当双脚支撑时，模型将以双脚各自为基座，把每条脚当做一个串联机械臂，从而求解一个闭链系统。然而，在单双脚状态切换时，由于模型进行了切换，导致此时的电机转矩将发生跳变，这给双足机器人行走带来很大的不确定性。
因此，发明人提出了基于浮动基座的逆动力学算法进行前馈控制来解决现有方案中的上述难题。本发明提出的方法通过在固定基逆动力学的基础上运用浮动基逆动力学的方法，使得双足机器人甚至是四足机器人等的运动将不再需要分阶段去建模并使用固定基逆动力学，而是统一框架采用浮动基逆动力学等。下面结合具体实施例对该方法进行说明。
实施例1
请参照图2，本实施例提出一种浮动基动力学前馈控制方法，可应用于对多足机器人进行运动控制，如双足仿人机器人、四足机器人等。通过该方法可以有效地解决现有方案中因采用固定基座逆动力学算法而导致的计算复杂度高、计算量大及在状态切换时引起的关节力矩突变等难题。
如图2所示，下面对该浮动基动力学前馈控制方法进行详细说明。
步骤S10，根据预先规划的机器人运动轨迹基于逆运动学获取每一时刻该机器人各关节的运动学参数，将机器人的躯体中心作为浮动的基座。
对于上述步骤S10，对于预先规划的该机器人的运动轨迹，例如，可以是给定的机器人的质心运动轨迹、足端（也称末端）运动轨迹等，然后利用逆运动学可从该运动轨迹中计算出各时刻该机器人各关节的运动学参数，如关节位置、关节角度、关节角速度、以及关节角加速度等等。示范性地，可通过利用机器人位置环、速度环和加速度环等逆动力学算法计算得到，具体可参考已有的相关文献，在此便不再详述。
进一步地，将机器人的躯体中心作为基座，对应地，将机器人的每只脚将视为一末端，进而在该机器人的躯干的惯性坐标系下独立计算每条脚的逆动力学。可以理解，在运动过程中机器人的躯体中心是运动的，故该基座也称为浮动基座。由于运动状态下基座存在惯性加速度，此时基于该机器人的躯干的坐标系也称为惯性坐标系。
步骤S20，根据各关节的运动学参数基于正运动学算法求解各关节质心在基座坐标系下的六维加速度。
于是，在获取到各关节的关节角度、角速度及角加速度后，进而利用正运动学原理求解出各关节质心在相对于机器人基座的六维加速度，当然也可以求解出各关节质心相对于基座的六维速度等。
对于上述步骤S20，所述的基座坐标系，是指以机器人的基座为参考点建立的坐标系。本实施例将机器人的躯体中心（又称机身、躯干）选取为浮动基座，故上述的基座坐标系下的六维加速度即为各关节质心相对于该躯体中心的六维加速度。
基于正运动学原理的齐次坐标变换矩阵的链式法则根据各关节的关节角、关节角速度及关节角加速度这些运动学参数，可依次计算出为串联结构的各刚体坐标系和末端坐标系的位置、姿态及各刚体的质心位置等。
进一步地，对于各关节质心的速度及加速度，以及末端坐标系的速度及加速度，均满足如下正运动学公式：
Figure 774179DEST_PATH_IMAGE026
；
Figure 930222DEST_PATH_IMAGE001
；
其中，
Figure 616419DEST_PATH_IMAGE027
代表第i个关节质心（或末端坐标系）的六维速度，是6x1的列向量，由空间坐 标系中的三维线速度和三维角速度组成；
Figure 292251DEST_PATH_IMAGE002
代表第i个关节质心（或末端坐标系）在基座坐标系下的六维加速度，是6x1的列向 量，由三维线加速度和三维角加速度组成；
Figure 874542DEST_PATH_IMAGE028
为第i个关节质心（或末端坐标系）相对于基座的雅克比矩阵；对于一个自由度为n的 串联机器人，
Figure 65352DEST_PATH_IMAGE029
为6xn的矩阵；
Figure 758501DEST_PATH_IMAGE004
为第i个关节质心的雅克比求导矩阵，通过对上述的雅克比矩阵求导得到；
Figure 272528DEST_PATH_IMAGE030
代表各关节质心（或末端坐标系）的关节角速度向量，是由n个关节角速度组成的nx1 的列向量；
Figure 25720DEST_PATH_IMAGE006
代表各关节质心（或末端坐标系）的关节角加速度向量，是由n个关节角加速度组 成的nx1的列向量。
通过上述公式可计算对应关节质心在基座坐标系下的六维加速度。例如，对于一 个单腿6自由度的双足机器人，若需求解第2号关节质心在基座坐标系下的六维加速度a 2， 则对应的雅克比矩阵J 2 将为6x6的矩阵，具体可根据上述步骤S10获取的各关节角度基于正 向运动学计算得到；
Figure 703826DEST_PATH_IMAGE005
则为由6个关节角速度组成6x1的列向量；
Figure 935087DEST_PATH_IMAGE006
为由6个关节角加速度 组成6x1的列向量。可以理解，对于该6x6的雅克比矩阵J 2 ，其后4列都为0，前2列中每一列的 上三个元素和下三个元素分别为对应电机对该第2号关节质心的线速度的微分、角速度的 微分。通过对J 2 进行求导，然后利用上述公式即可计算出该第2号关节质心在基座坐标系下 的六维加速度a 2。
由于上述步骤S20所获得的各关节质心的六维加速度是在基座坐标系中的，属于非惯性坐标系，故无法直接使用牛顿-欧拉（Newton-Euler）逆动力学算法进行迭代求解，为此，本实施例提出将对所有关节质心在基座坐标系下的六维加速度进行惯性系修正，将其投影到世界坐标系中。
步骤S30，将各关节质心在基座坐标系下的六维加速度转换为世界坐标系下的六维加速度。
对于上述步骤S30，先求解得到该机器人浮动的基座在世界坐标系下的六维加速度，然后再将各关节质心的六维加速度由基座坐标系转换为世界坐标系。
在一实施例中，对于上述步骤S30，如图3所示，包括以下子步骤：
子步骤S310，根据惯性测量单元测量得到的该浮动的基座的姿态和该机器人的着地脚通过正运动学算法计算该浮动的基座在世界坐标系下的三维角速度和六维加速度。
子步骤S320，基于该浮动的基座的所述姿态以及在世界坐标系下的所述三维角速度和六维加速度通过非惯性系到惯性系加速度转化操作符对各关节质心在基座坐标系下的六维加速度进行转换，以得到该关节质心在世界坐标系下的六维加速度。
对于上述子步骤S310，示范性地，先以机器人的着地脚视为坐标系原点，此时该着地脚可视为一固定基座，然后根据获取的各关节的角度利用正向运动学的齐次坐标变换矩阵的链式法则计算出该机器人的躯干的位置及姿态，以及六维速度、六维加速度等，具体可参考上述的正向运动学公式，在此不再重要描述。同时考虑到当前的着地脚可能落地不稳，如地面不平或落在障碍物上等，还将利用设在该机器人的躯干上的惯性测量单元IMU来测量躯干姿态，从而对该躯干（即浮动基座）进行姿态修正等。
于是，得到的该浮动基座在世界坐标系下的六维加速度后，将进一步执行步骤S320，即由非惯性系到惯性系的加速度转换。通过上述的非惯性系到惯性系加速度转化操作符能够将非惯性系下的关节质心的加速度转化为惯性系下的加速度。
在一实施例中，该操作符的输入量包括：
a. 一关节质心在基座坐标系下的矢量，为3x1的列向量；
b. 基座在世界坐标系下的姿态矩阵，为3x3的矩阵；
b. 基座在世界坐标系下的六维速度，为3x1的三维线速度和3x1的三维角速度构成；
c. 基座在世界坐标系下的六维加速度，为3x1的三维线加速度和3x1的三维角加速度构成；
d. 该关节质心在基座坐标系下的六维速度，为3x1的三维线速度和3x1的三维角速度构成；
e. 该关节质心在基座坐标系下的六维加速度，为3x1的三维线加速度和3x1的三维角加速度构成；
该操作符的输出量包括：
f. 该关节质心在世界坐标系下的三维线加速度，为3x1的列向量；
g. 该关节质心在世界坐标系下的三维角加速度，为3x1的列向量。
其中，对于上述基座在世界坐标系下的姿态矩阵，可通过惯性测量单元IMU测量及计算得到。对于该操作符的作用，可以理解为是在非惯性系中添加惯性力，如平衡惯性力，离心力，科里奥利力等，以得到相应的计算公式来实现转换。需要明白的是，上述的基座坐标系和世界坐标系均为三维坐标系，在该三维坐标系中，x、y和z轴的正轴方向满足右手定则。
在一实施例中，该非惯性系到惯性系加速度转化操作符的计算公式可为：
∂word_i=∂base+R base•∂i+w base×R base•v i+β base×R base•r i+w base×w base×R base•r i+w base×R base•v i；
β word_i=β base+w base×R base•w i+R base•β i；
其中，∂word_i和β word_i分别为第i个关节质心在世界坐标系下的线加速度、角加速度；∂base和β base分别为基座在世界坐标系下的线加速度、角加速度；w base为基座在世界坐标系下的角速度；R base为基座在世界坐标系下的姿态矩阵；r i为第i个关节质心在基座坐标系下的矢量；v i和w i分别为第i个关节质心在基座坐标系下的线速度、角速度；∂i和β i分别为第i个关节质心在基座坐标系下的线加速度、角加速度。应当理解，输入到操作符中的上述各线加速度、角加速度、线速度及角速度在世界坐标系下或是基座坐标系下均为三维矢量。
于是，通过上述转化后将得到各关节质心在世界坐标系下的六维加速度，属于惯性坐标系，故可利用牛顿欧拉（Newton-Euler）逆动力学算法进行多次迭代，依次计算出每一时刻各个关节电机的所需转矩。
步骤S40，根据获取的各关节质心在世界坐标系下的六维加速度利用逆动力学算法计算各关节电机所需的力矩。
本实施例的方法适用于多足机器人，通常地，该机器人存在至少两只脚，由于每只脚都与机器人的躯干连接，而躯干作为机器人基座，各故可以基于浮动基座分别对每只脚进行独立的逆动力学算法求解，且每条脚的求解原理相同。
如图4和图5所示，下面以某只脚的各关节电机所需力矩为例进行求解过程说明。对于上述步骤S40，若该机器人的单脚自由度为n，对于上述步骤S40，如图4所示，包括：
子步骤S410，从作为第n连杆的末端杆开始，根据获取的对应关节质心在世界坐标系下的六维加速度和该末端杆所受的外力和外力矩通过牛顿欧拉公式求解该末端杆与连接的第n-1连杆之间的作用力及作用力矩，根据所述作用力矩计算该末端杆所需的力矩。
子步骤S420，将该作用力和作用力矩作为第n-1连杆所受的外力和外力矩，利用牛顿欧拉公式求解该第n-1连杆所需的力矩以及该第n-1连杆与第n-2连杆之间的作用力作用力矩，如此迭代求解，直到求解得到所有关节所需的力矩。
示范性地，对于末端杆（即第n连杆）而言，如图5所示，它受到外力和连接它直接连 接的第n-1杆对它的相互作用力。在惯性系中，根据刚体受到的合力F满足的牛顿方程：
Figure 585512DEST_PATH_IMAGE031
，其中，m为刚体质量；
Figure 40764DEST_PATH_IMAGE032
为刚体在世界坐标系下的线加速度。于是，对于该末端杆， 所受合力将满足：
Figure 658696DEST_PATH_IMAGE008
；
其中，
Figure 428069DEST_PATH_IMAGE009
为该末端杆的质量；
Figure 198579DEST_PATH_IMAGE010
为该末端杆在世界坐标系下的三维线加速度；
Figure 824732DEST_PATH_IMAGE011
为重力加速度；
Figure 680692DEST_PATH_IMAGE012
为该末端杆所受的外力；
Figure 503024DEST_PATH_IMAGE007
为该末端杆与连接的第n-1连杆之间 的作用力。
对于上述的末端杆在世界坐标系下的三维线加速度
Figure 659199DEST_PATH_IMAGE010
可通过上述步骤S30 计算得到。可以理解，该末端杆所受的外力和外力矩以及末端杆的三维加速度均可根据规 划的轨迹计算得到。其中，当该末端杆与外界无接触时，则该末端杆所受的外力和外力矩均 为零；当末端杆与地面接触时，则该末端杆所受的外力即为地面对它的支撑力。
此外，在受到外力及力矩作用下，该末端杆在质心处的转矩方程满足：
Figure 925095DEST_PATH_IMAGE013
；
其中，
Figure 2772DEST_PATH_IMAGE015
为该末端杆在质心处的合力矩；
Figure 379527DEST_PATH_IMAGE016
为外力矩；
Figure 655788DEST_PATH_IMAGE017
为第n-1连杆对末端杆 相互作用的作用力矩；
Figure 341853DEST_PATH_IMAGE018
为第n-1杆接触点到该末端杆质心的矢量；
Figure 641247DEST_PATH_IMAGE019
为地面接触点到该 末端杆质心的矢量。
同时，该末端杆受到的力矩又满足欧拉方程：
Figure 618430DEST_PATH_IMAGE014
；
其中，
Figure 686881DEST_PATH_IMAGE020
为该末端杆的转动惯量；
Figure 294579DEST_PATH_IMAGE033
为该末端杆的关节角加速度矢量；
Figure 143587DEST_PATH_IMAGE022
为该末端杆 的关节角速度矢量。
联立上述两式，得到：
Figure 845832DEST_PATH_IMAGE034
。
然后，将该第n-1连杆对末端杆相互作用的作用力矩
Figure 96685DEST_PATH_IMAGE017
投影至电机旋转轴以得 到该末端杆所需的力矩。于是，该末端杆所需的力矩的计算公式为：
Figure 609706DEST_PATH_IMAGE023
；
其中，
Figure 149272DEST_PATH_IMAGE024
为该末端杆所需的力矩；
Figure 468258DEST_PATH_IMAGE025
为电机旋转轴的方向矢量。
由上述步骤可计算得到该末端杆所需的力矩，即对应电机在该时刻应施加的力矩。可以理解，对于前面的n-1个串联的连杆，其求解过程类似，利用两连杆之间的作用力与反作用力、以及作用力矩与反作用力矩原理，只需把末端杆受到的外力和外力矩替换成当前连杆与后一连杆的中间作用力和中间作用力矩，然后采用上述相同的步骤，依次计算出这n-1个连杆各自所需的力矩。
步骤S50，基于计算出的所述力矩通过前馈控制方式对所述机器人对应的关节电机进行控制。
对于上述步骤S50，在得到各关节电机应施加的力矩后，可通过前馈控制方式发送到对应的关节电机。例如，对于位置控制的关节电机，可利用力矩与前馈电流之间的对应关系及计算得到的力矩确定对应的前馈电流值，然后将该前馈电流值叠到对应关节的控制信号中，从而实现对该机器人的控制。或者，对于力矩控制的关节电机，则可以根据计算得到的力矩进行直接地力控制等。可以理解，通过该前馈控制方式对该机器人进行加速度补偿，可提高该机器人的运动能力等。
为了更好地说明该浮动基动力学能够应用于多足机器人上，图6为基座固定在地面上的三自由度机械臂模型。对于传统的机械臂控制方案，如图6左边，通常是给定最上端的末端执行器在世界坐标系中的运动轨迹，然后采用固定基逆动力学算法，由末端执行器向前，由上往下依次计算每一时刻各关节电机应施加的转矩。而本实施例的采用浮动基逆动力学算法的控制方案，如图6右边，即在获取到最上端的真实末端执行器的六维加速度后，将其作为“基座”，而将下方的真实基座当作“末端执行器”，由下往上依次计算每一时刻各关节电机应施加的转矩。
图7、图8和图9依次给出了一个三自由度机械臂模型的第1、2和3号电机的转矩测试结果，其中，黑色线条的曲线表示固定基逆动力学的计算结果，点状的曲线表示浮动基逆动力学的计算结果。由图7-图9可知，通过两种方法所计算得到的每个电机的转矩结果是相同的，即说明该浮动基逆动力学能够应用到双足、四足等多足机器人中。
本发明实施例提出的浮动基动力学前馈控制方法具有如下有益效果：
（1）通过将机器人的机身作为机器人基座，并利用浮动基动力学理论来求解机器人运动过程中的逆动力学，通过对固定基座机器人逆动力学算法进行惯性系修正，实现了将传统的基于固定基座机器人逆动力学算法拓展到基于运动中的基座来求解全机器人逆动力学。
（2）通过该方法不需要再将机器人的运动过程划分成单足支撑期或多足支撑期，任何时刻只要获得机器人作为基座的机身的位姿，六维速度，六维加速度和双足末端受力，即可根据预先规划的机器人运动轨迹来求解出每一时刻各关节电机应施加的力矩。
（3）利用该方法中的浮动基逆动力学，可以简化该机器人的逆动力学运算，使计算复杂度大大降低，计算量明显减少。例如，对于一个含两个6自由度单腿的双足仿人机器人，在传统方案中，在单足支撑期时需要求解一个串联12自由度的逆动力学算法，双足支撑期则需要求解并联6自由度的逆动力学算法；而使用浮动基则在任意时刻都只需要将双腿当作两个6自由度的串联机器人来建模，两腿之间的计算互不干扰，如图10所示。
（4）通过浮动基动力学实时求解各关节所需的转矩，根据使用的电机采用前馈控制方式发给对应电机，可以有效改善原有的基于固定基逆动力学算法在行走过程中因单双脚状态切换而造成的转矩突变，可提升各关节电机的跟踪效果以及机器人的行走能力等。
实施例2
请参照图11，基于上述实施例1的方法，本实施例提出一种浮动基动力学前馈控制装置10，该装置包括：
关节角度获取模块110，用于根据预先规划的机器人运动轨迹基于逆运动学获取每一时刻所述机器人各关节的运动学参数，将所述机器人的躯体中心作为浮动的基座；
惯性系修正模块120，用于根据各关节的所述运动学参数通过正运动学算法求解各关节质心在基座坐标系下的六维加速度，并将各关节质心在基座坐标系下的六维加速度转换为世界坐标系下的六维加速度；
前馈控制模块130，用于根据获取的各关节质心在世界坐标系下的六维加速度利用逆动力学算法计算各关节电机所需的力矩，基于计算的所述力矩通过前馈控制方式对所述机器人对应的关节电机进行控制。
可以理解，本实施例的装置对应于上述实施例1的方法，上述实施例1的可选项同样适用于本实施例，故在此不再详述。
本发明的另一实施例还提出一种多足机器人，如可包括但不限于为双足仿人机器人，四足机器人等。其中，该多足机器人能够采用上述实施例1的浮动基动力学前馈控制方法进行控制。
本发明的又一实施例还提供了一种计算机可读存储介质，其存储有计算机程序，在所述计算机程序被实施时，执行上述的浮动基动力学前馈控制方法。
在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
另外，在本发明各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。
所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备（可以是智能手机、个人计算机、服务器、或者网络设备等）执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。

Claims (11)

1.一种浮动基动力学前馈控制方法，其特征在于，包括：
根据预先规划的机器人运动轨迹基于逆运动学获取每一时刻所述机器人各关节的运动学参数，将所述机器人的躯体中心作为浮动的基座；
根据各关节的所述运动学参数通过正运动学算法求解各关节质心在基座坐标系下的六维加速度，将各关节质心在基座坐标系下的六维加速度转换为世界坐标系下的六维加速度；
根据获取的各关节质心在世界坐标系下的六维加速度利用逆动力学算法计算各关节电机所需的力矩，基于计算出的所述力矩通过前馈控制方式对所述机器人对应的关节电机进行控制。
2.根据权利要求1所述的浮动基动力学前馈控制方法，其特征在于，所述运动学参数包括关节的角度、角速度和角加速度，所述正运动学的方程如下：
Figure 530194DEST_PATH_IMAGE001
；
其中，
Figure 712913DEST_PATH_IMAGE002
代表第i个关节质心在基座坐标系下的六维加速度；
Figure 961492DEST_PATH_IMAGE003
为第i个关节质心相对 于基座的雅克比矩阵；
Figure 559963DEST_PATH_IMAGE004
为雅克比矩阵的求导矩阵；
Figure 639915DEST_PATH_IMAGE005
表示各关节的角速度向量；
Figure 914907DEST_PATH_IMAGE006
表 示各关节的角加速度向量。
3.根据权利要求1所述的浮动基动力学前馈控制方法，其特征在于，所述机器人的躯体上设有惯性测量单元，所述将各关节质心在基座坐标系下的六维加速度转换为世界坐标系下的六维加速度，包括：
根据所述惯性测量单元测量得到的所述浮动的基座的姿态和所述机器人的着地脚通过正运动学算法计算所述浮动的基座在世界坐标系下的三维角速度和六维加速度；
基于所述浮动的基座的所述姿态以及在世界坐标系下的所述三维角速度和六维加速度通过非惯性系到惯性系加速度转化操作符对各关节质心在基座坐标系下的六维加速度进行转换，以得到所述关节质心在世界坐标系下的六维加速度。
4.根据权利要求3所述的浮动基动力学前馈控制方法，其特征在于，所述非惯性系到惯性系加速度转化操作符的计算公式如下：
∂word_i=∂base+R base•∂i+w base×R base•v i+β base×R base•r i+w base×w base×R base•r i+w base×R base•v i；
β word_i=β base+w base×R base•w i+R base•β i；
其中，∂word_i和β word_i分别为第i个关节质心在世界坐标系下的线加速度、角加速度；∂base和β base分别为基座在世界坐标系下的线加速度、角加速度；w base为基座在世界坐标系下的三维角速度；R base为基座在世界坐标系下的姿态矩阵；r i为第i个关节质心在基座坐标系下的矢量；v i和w i分别为第i个关节质心在基座坐标系下的线速度、角速度；∂i和β i分别为第i个关节质心在基座坐标系下的线加速度、角加速度。
5.根据权利要求1所述的浮动基动力学前馈控制方法，其特征在于，若所述机器人的一单脚自由度为n，所述根据获取的各关节质心在世界坐标系下的六维加速度利用逆动力学算法计算各关节电机所需的力矩，包括：
从作为第n连杆的末端杆开始，根据获取的对应关节质心在世界坐标系下的六维加速度和所述末端杆所受的外力和外力矩通过牛顿欧拉公式求解所述末端杆与连接的第n-1连杆之间的作用力及作用力矩，根据所述作用力矩计算所述末端杆所需的力矩；
将所述作用力和所述作用力矩作为所述第n-1连杆所受的外力和外力矩，利用牛顿欧拉公式求解所述第n-1连杆所需的力矩以及所述第n-1连杆与第n-2连杆之间的作用力及作用力矩，如此迭代求解，直到求解得到所有关节所需的力矩。
6.根据权利要求5所述的浮动基动力学前馈控制方法，其特征在于，当所述末端杆与外界无接触时，所述末端杆所受的外力为零；当所述末端杆与地面接触时，所述末端杆所受的外力为地面的支撑力。
7.根据权利要求6所述的浮动基动力学前馈控制方法，其特征在于，所述末端杆与连接 的第n-1连杆之间的作用力
Figure 650782DEST_PATH_IMAGE007
的计算公式如下：
Figure 115262DEST_PATH_IMAGE008
；
其中，
Figure 987403DEST_PATH_IMAGE009
为该末端杆的质量；
Figure 449608DEST_PATH_IMAGE010
为该末端杆在世界坐标系下的三维线加速度；
Figure 469517DEST_PATH_IMAGE011
为重力加速度；
Figure 659058DEST_PATH_IMAGE012
为该末端杆所受的外力。
8.根据权利要求7所述的浮动基动力学前馈控制方法，其特征在于，所述末端杆与连接的第n-1连杆之间的作用力矩的求解过程，包括：
构建所述末端杆在质心处的转矩方程及满足的欧拉公式，以计算所述第n-1连杆对所述末端杆相互作用的作用力矩；
所述末端杆在质心处的合力矩满足的转矩方程及欧拉公式分别为：
Figure 651285DEST_PATH_IMAGE013
；
Figure 346709DEST_PATH_IMAGE014
；
其中，
Figure 791596DEST_PATH_IMAGE015
为该末端杆在质心处的合力矩；
Figure 332299DEST_PATH_IMAGE016
为外力矩；
Figure 179033DEST_PATH_IMAGE017
为第n-1连杆对末端杆 相互作用的作用力矩；
Figure 232308DEST_PATH_IMAGE018
为第n-1杆接触点到该末端杆质心的矢量；
Figure 226809DEST_PATH_IMAGE019
为地面接触点到该末 端杆质心的矢量；
Figure 508886DEST_PATH_IMAGE020
为该末端杆的转动惯量；
Figure 272442DEST_PATH_IMAGE021
为该末端杆的关节角加速度矢量；
Figure 512931DEST_PATH_IMAGE022
为该 末端杆的关节角速度矢量。
9.一种浮动基动力学前馈控制装置，其特征在于，包括：
关节角度获取模块，用于根据预先规划的机器人运动轨迹基于逆运动学获取每一时刻所述机器人各关节的运动学参数，将所述机器人的躯体中心作为浮动的基座；
惯性系修正模块，用于根据各关节的所述运动学参数通过正运动学算法求解各关节质心在基座坐标系下的六维加速度，并将各关节质心在基座坐标系下的六维加速度转换为世界坐标系下的六维加速度；
前馈控制模块，用于根据获取的各关节质心在世界坐标系下的六维加速度利用逆动力学算法计算各关节电机所需的力矩，基于计算的所述力矩通过前馈控制方式对所述机器人对应的关节电机进行控制。
10.一种多足机器人，其特征在于，采用权利要求1至8中任一项所述的浮动基动力学前馈控制方法对所述多足机器人进行控制。
11.一种计算机可读存储介质，其特征在于，存储有计算机程序，在所述计算机程序被执行时，实施根据权利要求1至8中任一项所述的浮动基动力学前馈控制方法。
CN202010671790.1A 2020-07-14 2020-07-14 浮动基动力学前馈控制方法、装置和多足机器人 Active CN111558941B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202010671790.1A CN111558941B (zh) 2020-07-14 2020-07-14 浮动基动力学前馈控制方法、装置和多足机器人
US17/088,596 US11579591B2 (en) 2020-07-14 2020-11-04 Feedforward control method for floating base dynamics, computer-readable storage medium and robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010671790.1A CN111558941B (zh) 2020-07-14 2020-07-14 浮动基动力学前馈控制方法、装置和多足机器人

Publications (2)

Publication Number Publication Date
CN111558941A true CN111558941A (zh) 2020-08-21
CN111558941B CN111558941B (zh) 2020-09-29

Family

ID=72068891

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010671790.1A Active CN111558941B (zh) 2020-07-14 2020-07-14 浮动基动力学前馈控制方法、装置和多足机器人

Country Status (2)

Country Link
US (1) US11579591B2 (zh)
CN (1) CN111558941B (zh)

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112536796A (zh) * 2020-11-23 2021-03-23 深圳市优必选科技股份有限公司 机器人控制方法、装置、计算机可读存储介质及机器人
CN112706167A (zh) * 2020-12-29 2021-04-27 合肥联宝信息技术有限公司 用于移动装置的控制方法、设备和计算机可读存储介质
CN113081582A (zh) * 2021-03-18 2021-07-09 上海交通大学 一种机器人辅助站立轨迹生成方法
CN113433338A (zh) * 2021-07-02 2021-09-24 乐聚(深圳)机器人技术有限公司 机器人质心速度计算方法、装置及介质
CN113625735A (zh) * 2021-08-09 2021-11-09 山东大学 基于腿臂复用六足机器人的控制方法和机器人
CN114147714A (zh) * 2021-12-02 2022-03-08 浙江机电职业技术学院 自主机器人机械臂控制参数计算方法和系统
CN114750162A (zh) * 2022-05-17 2022-07-15 北京理工大学 一种仿人机器人高动态运动质心柔顺控制方法
CN115476352A (zh) * 2021-05-31 2022-12-16 腾讯科技（深圳）有限公司 机器人控制方法、机器人控制系统及机器人
WO2023273107A1 (zh) * 2021-06-30 2023-01-05 深圳市优必选科技股份有限公司 机器人行走控制方法、装置、机器人控制设备及存储介质

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114734441B (zh) * 2022-04-15 2023-11-24 北京邮电大学 一种关节部分失效故障空间机械臂运动能力优化方法
CN114888802B (zh) * 2022-05-16 2023-08-08 军事科学院系统工程研究院卫勤保障技术研究所 一种用于并联机器人恒平控制的数据处理方法及装置
CN115107032B (zh) * 2022-07-15 2024-04-05 海南大学 一种基于伪逆且可自适应抗噪的移动机械臂运动规划方法
CN116304512B (zh) * 2023-03-16 2024-01-30 中国兵器装备集团自动化研究所有限公司 一种机器人腿部的逆运动学求解方法及装置
CN117272596A (zh) * 2023-08-24 2023-12-22 中国兵器装备集团自动化研究所有限公司 一种动力学模型建模方法、装置、设备及存储介质
CN116974257B (zh) * 2023-09-25 2023-12-26 浪潮云洲工业互联网有限公司 一种基于逆动力学的鹤管自动充装控制方法、设备及介质

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5252901A (en) * 1990-11-30 1993-10-12 Honda Giken Kogyo Kabushiki Kaisha System for controlling locomotion of legged walking robot
US20040044440A1 (en) * 2000-11-17 2004-03-04 Toru Takenaka Gait pattern generating device for legged mobile robot
CN107263466A (zh) * 2017-05-11 2017-10-20 西北工业大学 空间机器人基于二次规划问题的基座无扰控制方法
CN107962567A (zh) * 2017-11-15 2018-04-27 西安交通大学 浮动柔性机械臂的分布式振动控制系统及其振动控制方法
CN109591023A (zh) * 2018-12-30 2019-04-09 深圳市优必选科技有限公司 串联机器人的迭代计算方法及串联机器人
CN109634100A (zh) * 2018-12-30 2019-04-16 深圳市优必选科技有限公司 仿人机器人行走加速度补偿方法、装置及仿人机器人
CN109987169A (zh) * 2017-12-29 2019-07-09 深圳市优必选科技有限公司 双足机器人的步态控制方法、装置、终端设备及介质

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7986118B2 (en) * 2007-04-23 2011-07-26 Honda Motor Co., Ltd. Open-loop torque control on joint position-controlled robots
KR101633362B1 (ko) * 2010-01-18 2016-06-28 삼성전자 주식회사 인간형 로봇 및 그 보행 제어방법

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5252901A (en) * 1990-11-30 1993-10-12 Honda Giken Kogyo Kabushiki Kaisha System for controlling locomotion of legged walking robot
US20040044440A1 (en) * 2000-11-17 2004-03-04 Toru Takenaka Gait pattern generating device for legged mobile robot
CN107263466A (zh) * 2017-05-11 2017-10-20 西北工业大学 空间机器人基于二次规划问题的基座无扰控制方法
CN107962567A (zh) * 2017-11-15 2018-04-27 西安交通大学 浮动柔性机械臂的分布式振动控制系统及其振动控制方法
CN109987169A (zh) * 2017-12-29 2019-07-09 深圳市优必选科技有限公司 双足机器人的步态控制方法、装置、终端设备及介质
CN109591023A (zh) * 2018-12-30 2019-04-09 深圳市优必选科技有限公司 串联机器人的迭代计算方法及串联机器人
CN109634100A (zh) * 2018-12-30 2019-04-16 深圳市优必选科技有限公司 仿人机器人行走加速度补偿方法、装置及仿人机器人

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112536796B (zh) * 2020-11-23 2024-03-15 深圳市优必选科技股份有限公司 机器人控制方法、装置、计算机可读存储介质及机器人
CN112536796A (zh) * 2020-11-23 2021-03-23 深圳市优必选科技股份有限公司 机器人控制方法、装置、计算机可读存储介质及机器人
CN112706167A (zh) * 2020-12-29 2021-04-27 合肥联宝信息技术有限公司 用于移动装置的控制方法、设备和计算机可读存储介质
CN113081582B (zh) * 2021-03-18 2022-06-28 上海交通大学 一种机器人辅助站立轨迹生成方法
CN113081582A (zh) * 2021-03-18 2021-07-09 上海交通大学 一种机器人辅助站立轨迹生成方法
CN115476352A (zh) * 2021-05-31 2022-12-16 腾讯科技（深圳）有限公司 机器人控制方法、机器人控制系统及机器人
WO2023273107A1 (zh) * 2021-06-30 2023-01-05 深圳市优必选科技股份有限公司 机器人行走控制方法、装置、机器人控制设备及存储介质
CN113433338A (zh) * 2021-07-02 2021-09-24 乐聚(深圳)机器人技术有限公司 机器人质心速度计算方法、装置及介质
CN113625735A (zh) * 2021-08-09 2021-11-09 山东大学 基于腿臂复用六足机器人的控制方法和机器人
CN113625735B (zh) * 2021-08-09 2023-11-10 山东大学 基于腿臂复用六足机器人的控制方法和机器人
CN114147714A (zh) * 2021-12-02 2022-03-08 浙江机电职业技术学院 自主机器人机械臂控制参数计算方法和系统
CN114750162A (zh) * 2022-05-17 2022-07-15 北京理工大学 一种仿人机器人高动态运动质心柔顺控制方法
CN114750162B (zh) * 2022-05-17 2023-10-17 北京理工大学 一种仿人机器人高动态运动质心柔顺控制方法

Also Published As

Publication number Publication date
US11579591B2 (en) 2023-02-14
US20220019196A1 (en) 2022-01-20
CN111558941B (zh) 2020-09-29

Similar Documents

Publication Publication Date Title
CN111558941B (zh) 浮动基动力学前馈控制方法、装置和多足机器人
US9120227B2 (en) Human motion tracking control with strict contact force constraints for floating-base humanoid robots
Buchli et al. Compliant quadruped locomotion over rough terrain
US8781624B2 (en) Systems and methods for tracking and balancing robots for imitating motion capture data
JP3972854B2 (ja) ロボットの運動制御装置
US8442687B2 (en) Control system, control method, and computer program
Nakanishi et al. Comparative experiments on task space control with redundancy resolution
JP2012081568A (ja) ロボットの制御装置及び制御方法、並びにコンピューター・プログラム
CN111506100A (zh) 多足机器人关节控制方法、装置和多足机器人
US20230234222A1 (en) Humanoid robot balance control method, humanoid robot, and storage medium
Samy et al. QP-based adaptive-gains compliance control in humanoid falls
Vigne et al. MOVIE: A velocity-aided IMU attitude estimator for observing and controlling multiple deformations on legged robots
Peng et al. Stability criteria of balanced and steppable unbalanced states for full-body systems with implications in robotic and human gait
Castano et al. A whole body attitude stabilizer for hybrid wheeled-legged quadruped robots
Rezapour et al. Path following control of planar snake robots using virtual holonomic constraints
Hammam et al. Whole-body humanoid control from upper-body task specifications
Stasse et al. Fast foot prints re-planning and motion generation during walking in physical human-humanoid interaction
Kim et al. Dynamic behaviors on the NAO robot with closed-loop whole body operational space control
Abdolmaleki et al. Omnidirectional walking with a compliant inverted pendulum model
JP7221833B2 (ja) 非線形モデル予測制御装置
Huang et al. Approach trajectory planning of space robot for impact minimization
An et al. Zero-moment point based balance control of leg-wheel hybrid structures with inequality constraints of kinodynamic behavior
Kim Motion planning of optimal throw for whole-body humanoid
Nakamura et al. A Study on Kicking Motion Strategy for a Legged Robot
Safeena et al. Adaptive Super Twisting Control of Stewart Platform Based on Super Twisting Observer

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

Inventor after: Wang Yuesong

Inventor after: Ju Xiaozhu

Inventor after: Xiong Youjun

Inventor before: Wang Yuesong

Inventor before: Ju Xiaozhu

Inventor before: Zhao Mingguo

Inventor before: Xiong Youjun