CN114002990A - 并联双足机器人关节实时控制方法和装置 - Google Patents

并联双足机器人关节实时控制方法和装置 Download PDF

Info

Publication number
CN114002990A
CN114002990A CN202111649171.3A CN202111649171A CN114002990A CN 114002990 A CN114002990 A CN 114002990A CN 202111649171 A CN202111649171 A CN 202111649171A CN 114002990 A CN114002990 A CN 114002990A
Authority
CN
China
Prior art keywords
angle
parallel
point
motor
real
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
CN202111649171.3A
Other languages
English (en)
Other versions
CN114002990B (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.)
Zhejiang Lab
Original Assignee
Zhejiang Lab
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 Zhejiang Lab filed Critical Zhejiang Lab
Priority to CN202111649171.3A priority Critical patent/CN114002990B/zh
Publication of CN114002990A publication Critical patent/CN114002990A/zh
Application granted granted Critical
Publication of CN114002990B publication Critical patent/CN114002990B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/04—Programme control other than numerical control, i.e. in sequence controllers or logic controllers
    • G05B19/042—Programme control other than numerical control, i.e. in sequence controllers or logic controllers using digital processors
    • G05B19/0428—Safety, monitoring
    • 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/20—Pc systems
    • G05B2219/24—Pc safety
    • G05B2219/24024—Safety, surveillance

Abstract

本发明公开了一种并联双足机器人关节实时控制方法和装置，本发明基于双足机器人步态规划的连续性以及双足机器人关节闭环控制的实时性，利用牛顿‑拉夫逊数值算法进行关节角度的求解；在求解过程中，将实际控制时上次周期的并联关节角度控制的指令值作为其关节角度求解的初值，从而缩小迭代范围，减小迭代次数，实现在保持双足机器人需求的频率下对并联关节的实时求解。

Description

并联双足机器人关节实时控制方法和装置
技术领域
本发明属于机器人技术领域，尤其涉及一种并联双足机器人关节实时控制方法和装置。
背景技术
机器人的本体性能决定机器人自身的极限性能。双足机器人驱动方式分为电机驱动型以及液压驱动型。考虑到液压驱动源所占体积较大，应用在双足机器人平台上较为困难，现阶段绝大多数双足机器人仍然采用电机驱动的方式。由于电机的驱动力矩有限，其机械结构对其本体的运动性能影响较大。电机驱动型双足机器人到目前为止有三种经典构型。原位布局型、串联型、并联型。其中原位布局型主要应用于小仿，其结构特点就是电机严格按照关节自由度进行布置，由于踝部存在两个自由度，这种布置方式往往导致其踝关节处较为臃肿，腿部惯量较大，影响机器人本身的运动性能。基于减小机器人腿部惯量的考虑，考虑将踝关节以及膝关节pitch驱动器上移，采用四连杆、滚轴丝杠、带传动等方法进行传动，控制踝关节、膝关节的pitch方向的转动。这种方法在一定程度上减小了机器人腿部的转动惯量，但是踝关节roll方向转动的电机仍然造成了较大的惯量。并联型关节应运而生，并联关节型足式机器人将膝关节pitch，踝关节roll、pitch处电机上移，在增加关节刚度的同时大大减少了腿部的转动惯量，提高了机器人的运动能力，但是由于并联关节的固有属性，导致踝关节处roll、pitch角度需要通过并联关节的解算来实现，尤其是并联关节角度实时解算困难，对于机器人控制的实时性有较大影响。
发明内容
本发明的目的在于针对现有技术的不足，提出了一种并联双足机器人关节实时控制方法和装置。
本发明的目的是通过以下技术方案来实现的：一种并联双足机器人关节实时控制方法，包括以下步骤：
步骤一：根据双足机器人并联关节的几何关系，列出并联关节角度与电机角度的运动方程；
步骤二：根据并联关节角度与电机角度的运动方程，获得电机角度与并联关节角度的数值联系；
步骤三：读取初始并联关节角度，由电机角度与并联关节角度的数值联系计算得到初始并联关节角度相对应的初始电机角度，将并联关节角度与电机角度的运动方程转化为牛顿-拉夫逊迭代的标准形式，获得牛顿-拉夫迭代的计算式；
步骤四：以初始并联关节角度作为牛顿-拉夫逊迭代的计算式的初值进行求解；当迭代计算过程达到结束条件，则迭代计算结束，将求得的迭代求解值作为并联双足机器人相对于初始并联关节角度的下一并联关节角度；当迭代计算过程中出现异常处理机制，则终止迭代计算过程，将初始并联关节角度作为并联双足机器人的下一并联关节角度；
步骤五：以步骤四得到的下一并联关节角度作为牛顿-拉夫逊迭代的计算式的初值，重复步骤四，从而实现机器人的闭环控制。
进一步地，所述步骤一具体为：
所述双足机器人并联关节包括联合带动所述双足机器人的足部的横滚角和俯仰角转动的第一运动链M1N1P1O和第二运动链M2N2P2O，所述第一运动链M1N1P1O包括第一水平杆和第一竖直杆，所述第二运动链M2N2P2O包括第二水平杆和第二竖直杆；其中点N1为第一水平杆和第一竖直杆的铰接点，点N2为第二水平杆和第二竖直杆的铰接点，点P1为第一竖直杆与脚板的铰接点，点P2为第二竖直杆与脚板的铰接点，O点为脚板与十字铰接轴的中心点，以O点为圆心建立X、Y、Z坐标系，并给出并联关节角度与电机角度的运动方程：
Figure 73312DEST_PATH_IMAGE001
，
Figure 892364DEST_PATH_IMAGE002
；
其中
Figure 654783DEST_PATH_IMAGE003
和
Figure 633104DEST_PATH_IMAGE004
为电机角度，即电机带动第一水平杆和第二水平杆的转动角度，当第一水平杆和第二水平杆水平时
Figure 557197DEST_PATH_IMAGE003
和
Figure 112813DEST_PATH_IMAGE004
的角度分别为0，当第一水平杆和第二水平杆向上转动时
Figure 475661DEST_PATH_IMAGE003
和
Figure 511750DEST_PATH_IMAGE004
的角度分别为正值，反之则为负值；
Figure 544428DEST_PATH_IMAGE005
和
Figure 665968DEST_PATH_IMAGE006
是并联关节角度，分别为所述双足机器人的足部的俯仰角和横滚角，当脚板水平时
Figure 832507DEST_PATH_IMAGE005
和
Figure 378895DEST_PATH_IMAGE006
都为90°，当脚板绕x轴正方向转动时
Figure 910370DEST_PATH_IMAGE005
的角度变小，反之则变大，当脚板绕y轴正方向转动时
Figure 315944DEST_PATH_IMAGE006
的角度变小，反之则变大；
Figure 223857DEST_PATH_IMAGE007
，
Figure 906642DEST_PATH_IMAGE008
，
Figure 405757DEST_PATH_IMAGE009
，
Figure 236309DEST_PATH_IMAGE010
，
Figure 869285DEST_PATH_IMAGE011
，
Figure 468893DEST_PATH_IMAGE012
，P点为线P1P2的中点，R点为点M1在Z轴上的投影，Q点为点M2在Z轴上的投影，OM1M2组成的平面始终在XOZ平面内，点Z’为过通过P点且平行于Z轴的辅助线正半轴的任意一点。
进一步地，所述步骤二具体为：根据并联关节角度与电机角度的运动方程获得电机角度与并联关节角度的数值联系，由并联关节角度计算电机角度，得到电机角度
Figure 404488DEST_PATH_IMAGE003
和
Figure 722337DEST_PATH_IMAGE004
的解析式：
Figure 378578DEST_PATH_IMAGE013
，
其中，A，B，D，E，F，G，H，I的中间变量如下：
Figure 160589DEST_PATH_IMAGE014
。
进一步地，所述牛顿-拉夫逊迭代的标准形式为：
Figure 204768DEST_PATH_IMAGE015
；
其中
Figure 196864DEST_PATH_IMAGE016
，
Figure 984691DEST_PATH_IMAGE017
。
进一步地，所述牛顿-拉夫逊迭代的计算式为：
Figure 621209DEST_PATH_IMAGE018
，
其中
Figure 508394DEST_PATH_IMAGE019
和
Figure 535255DEST_PATH_IMAGE020
为牛顿-拉夫逊迭代的计算式的初值，
Figure 189091DEST_PATH_IMAGE021
和
Figure 617798DEST_PATH_IMAGE022
为迭代求解值。
进一步地，所述结束条件为：
Figure 190731DEST_PATH_IMAGE023
。
进一步地，所述异常处理机制包括迭代计算时间超过设定的最大值或迭代计算过程中出现奇异值。
本发明还提供一种并联双足机器人关节实时控制装置，包括一个或多个处理器，用于实现上述并联双足机器人关节实时控制方法。
本发明还提供一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，用于实现上述并联双足机器人关节实时控制方法。
本发明的有益效果为：在不增加硬件成本的前提下，提出了将实际控制时上次周期的并联关节角度控制的指令值作为其关节角度求解的初值的数值解法，快速迭代获得数值解，从而实现了在控制周期内对并联关节的实时解算。基于本发明可以实现较为稳定可靠的关节数据解算。
附图说明
图1为本发明提供的一种并联双足机器人关节实时控制方法的流程示意图；
图2为一种并联双足机器人关节的抽象坐标图；
图3为一种并联双足机器人关节的正视投影图；
图4为一种并联双足机器人关节的侧视投影图；
图5为一种并联双足机器人关节的俯视投影图；
图6为本发明一种并联双足机器人关节实时控制装置的结构图；
其中，1-第一电机，2-第二电机，3-第一竖直杆，4-第二竖直杆，5-十字铰接轴，6-脚板，7-第一水平杆，8-第二水平杆。
具体实施方式
以下结合图1-图6对本发明作进一步说明。
一种并联双足机器人关节实时控制方法，包括以下步骤：
步骤一：根据双足机器人并联关节的几何关系，列出并联关节角度与电机角度的运动方程；
所述步骤一具体为：
所述双足机器人并联关节包括联合带动所述双足机器人的足部的横滚角和俯仰角转动的第一运动链M1N1P1O和第二运动链M2N2P2O，所述第一运动链M1N1P1O包括第一水平杆和第一竖直杆，所述第二运动链M2N2P2O包括第二水平杆和第二竖直杆；其中点N1为第一水平杆和第一竖直杆的铰接点，点N2为第二水平杆和第二竖直杆的铰接点，点P1为第一竖直杆与脚板的铰接点，点P2为第二竖直杆与脚板的铰接点，O点为脚板与十字铰接轴的中心点，以O点为圆心建立X、Y、Z坐标系，并给出并联关节角度与电机角度的运动方程：
Figure 970468DEST_PATH_IMAGE001
，
Figure 896836DEST_PATH_IMAGE002
；
其中
Figure 117732DEST_PATH_IMAGE003
和
Figure 940195DEST_PATH_IMAGE004
为电机角度，即电机带动第一水平杆和第二水平杆的转动角度，当第一水平杆和第二水平杆水平时
Figure 3966DEST_PATH_IMAGE003
和
Figure 937287DEST_PATH_IMAGE004
的角度分别为0，当第一水平杆和第二水平杆向上转动时
Figure 996379DEST_PATH_IMAGE003
和
Figure 52059DEST_PATH_IMAGE004
的角度分别为正值，反之则为负值；
Figure 540809DEST_PATH_IMAGE005
和
Figure 949925DEST_PATH_IMAGE006
是并联关节角度，分别为所述双足机器人的足部的俯仰角和横滚角，当脚板水平时
Figure 207731DEST_PATH_IMAGE005
和
Figure 168734DEST_PATH_IMAGE006
都为90°，当脚板绕x轴正方向转动时
Figure 66152DEST_PATH_IMAGE005
的角度变小，反之则变大，当脚板绕y轴正方向转动时
Figure 341275DEST_PATH_IMAGE006
的角度变小，反之则变大；
Figure 781484DEST_PATH_IMAGE007
，
Figure 851071DEST_PATH_IMAGE008
，
Figure 252096DEST_PATH_IMAGE009
，
Figure 65332DEST_PATH_IMAGE010
，
Figure 94468DEST_PATH_IMAGE011
，
Figure 521907DEST_PATH_IMAGE012
，P点为线P1P2的中点，R点为点M1在Z轴上的投影，Q点为点M2在Z轴上的投影，OM1M2组成的平面始终在XOZ平面内，点Z’为过通过P点且平行于Z轴的辅助线正半轴的任意一点。
并联关节角度与电机角度的运动方程的求解过程为：
如图2-图5所示，本实施例考虑的是具有两个自由度的并联踝部关节结构，踝部关节电机上移，采用连杆驱动的方式对踝关节进行驱动，导致电机角度需要经过转换才能换算成脚板绕x，y轴的转动。
以脚板6平行于地面、第一竖直杆3和第二竖直杆4垂直于脚板6为初始状态，此时点P位于脚板6上，第一水平杆7和第二水平杆8平行于脚板6；以O点十字轴中心点为原点，建立如图3、图4中所示笛卡尔坐标系；以脚板6为XY平面，以垂直于脚板6的方向为Z轴；由图3、图4的坐标位置抽象出来得到图2。
如图2所示，M1N1为固定在第一电机1上的第一水平杆7，M2N2为固定在第二电机2上的第二水平杆8，N1P1为第一竖直杆3，N2P2为第二竖直杆4；第一水平杆7和第二水平杆8分别在第一电机1和第二电机2的驱动下转动，由第一水平杆7带动第一竖直杆3转动，由第二水平杆8带动第二竖直杆4转动，从而带动脚板6绕十字铰接轴5转动，其中点N1为第一水平杆7和第一竖直杆3的铰接点，点N2为第二水平杆8和第二竖直杆4的铰接点，点P1为第一竖直杆3与脚板6的铰接点，点P2为第二竖直杆4与脚板6的铰接点；O点为脚板6与十字铰接轴5的中心点，也是脚板6旋转轴x轴和y轴的交点，P点为P1P2的中点；R点为点M1在Z轴上的投影，Q点为点M2在Z轴上的投影，随着并联关节角度的变化，OR、OQ长度不变，OM1M2组成的平面始终在XOZ平面内，两条封闭运动链分别为第一运动链M1N1P1O和第二运动链M2N2P2O，定义点Z’为过通过P点且平行于Z轴的辅助线正半轴的任意一点。
并定义点P、P1、P2的坐标为：
P（0， y， z） ， P1（x1， y1， z1） ， P2（x2， y2， z2）；
其中
Figure 472545DEST_PATH_IMAGE024
，
Figure 151788DEST_PATH_IMAGE025
；
Figure 238693DEST_PATH_IMAGE026
，
Figure 587766DEST_PATH_IMAGE027
，
Figure 822438DEST_PATH_IMAGE028
；
Figure 977476DEST_PATH_IMAGE029
，
Figure 371417DEST_PATH_IMAGE030
，
Figure 953708DEST_PATH_IMAGE031
；
点P、P1、P2的坐标为：
P
Figure 675676DEST_PATH_IMAGE032
，
P1
Figure 306509DEST_PATH_IMAGE033
，
P2
Figure 368006DEST_PATH_IMAGE034
；
当第一水平杆7和第二水平杆8处于水平时，即第一水平杆7和第二水平杆8与脚板6平行，点N1和N2处于此时的位置时设定其为N10、N20，当第一电机1带动第一水平杆7转动以及第二电机2带动第二水平杆8转动时，第一水平杆7和第二水平杆8与水平方向形成电机角度
Figure 449094DEST_PATH_IMAGE035
：
Figure 596042DEST_PATH_IMAGE036
，
Figure 25973DEST_PATH_IMAGE037
；
点N1、N2在移动过程中的坐标表示为：
N1
Figure 4293DEST_PATH_IMAGE038
，N2
Figure 193966DEST_PATH_IMAGE039
；
通过第一运动链M1N1P1O和第二运动链M2N2P2O获得并联关节角度与电机角度的运动方程，所述方程如下：
对于第一运动链M1N1P1O：
Figure 765893DEST_PATH_IMAGE040
；
对于第二运动链M2N2P2O：
Figure 66424DEST_PATH_IMAGE041
。
步骤二：根据并联关节角度与电机角度的运动方程，获得电机角度与并联关节角度的数值联系；
所述步骤二具体为：
根据并联关节角度与电机角度的运动方程获得电机角度与并联关节角度的数值联系，由并联关节角度计算电机角度，得到电机角度
Figure 899251DEST_PATH_IMAGE003
和
Figure 181196DEST_PATH_IMAGE004
的解析式：
Figure 302736DEST_PATH_IMAGE013
，
其中，A，B，D，E，F，G，H，I的中间变量如下：
Figure 203696DEST_PATH_IMAGE014
。
步骤三：读取初始并联关节角度，由电机角度与并联关节角度的数值联系计算得到初始并联关节角度相对应的初始电机角度，将并联关节角度与电机角度的运动方程转化为牛顿-拉夫逊迭代的标准形式，获得牛顿-拉夫迭代的计算式；
所述步骤三具体为：
读取初始并联关节角度
Figure 94292DEST_PATH_IMAGE042
和
Figure 563450DEST_PATH_IMAGE043
，由电机角度与并联关节角度的数值联系计算得到初始并联关节角度相对应的初始电机角度
Figure 703445DEST_PATH_IMAGE044
和
Figure 611358DEST_PATH_IMAGE045
，所述初始电机角度
Figure 543410DEST_PATH_IMAGE044
和
Figure 245787DEST_PATH_IMAGE045
的计算过程如下：
假设读取的初始并联关节角度的数值为：
Figure 138657DEST_PATH_IMAGE046
和
Figure 319103DEST_PATH_IMAGE047
；并假设a、b、c、d的值为：
Figure 856394DEST_PATH_IMAGE048
，
Figure 791989DEST_PATH_IMAGE049
，
Figure 109838DEST_PATH_IMAGE050
，
Figure 280925DEST_PATH_IMAGE051
；
将
Figure 620DEST_PATH_IMAGE042
、
Figure 841537DEST_PATH_IMAGE043
、a、b、c和d的数值代入电机角度的解析式，可得：
Figure 584365DEST_PATH_IMAGE052
，
Figure 372192DEST_PATH_IMAGE053
。
利用牛顿-拉夫逊方法作为数值求解的方法，将并联关节角度与电机角度的运动方程转化为牛顿-拉夫逊迭代的标准形式，所述牛顿-拉夫逊迭代的标准形式为：
Figure 8710DEST_PATH_IMAGE015
；
其中
Figure 223791DEST_PATH_IMAGE016
，
Figure 437603DEST_PATH_IMAGE017
；
并根据牛顿-拉夫逊迭代的标准形式得到牛顿-拉夫逊迭代的计算式，所述牛顿-拉夫逊迭代的计算式为：
Figure 29121DEST_PATH_IMAGE018
，
其中
Figure 254566DEST_PATH_IMAGE019
和
Figure 578231DEST_PATH_IMAGE020
为牛顿-拉夫逊迭代的计算式的初值，
Figure 357969DEST_PATH_IMAGE021
和
Figure 549916DEST_PATH_IMAGE022
为迭代求解值。
步骤四：以初始并联关节角度作为牛顿-拉夫逊迭代的计算式的初值进行求解；当迭代计算过程达到结束条件，则迭代计算结束，将求得的迭代求解值作为并联双足机器人相对于初始并联关节角度的下一并联关节角度；当迭代计算过程中出现异常处理机制，则终止迭代计算过程，将初始并联关节角度作为并联双足机器人的下一并联关节角度；
步骤四具体为：以初始并联关节角度
Figure 833129DEST_PATH_IMAGE042
、
Figure 842543DEST_PATH_IMAGE043
作为牛顿-拉夫逊迭代的计算式的初值
Figure 843997DEST_PATH_IMAGE054
进行求解，求得迭代求解值
Figure 574055DEST_PATH_IMAGE055
；
Figure 649459DEST_PATH_IMAGE056
；
当迭代计算过程达到结束条件，则迭代计算结束，将求得的迭代求解值作为并联双足机器人相对于初始并联关节角度的下一并联关节角度；
所述结束条件为
Figure 642822DEST_PATH_IMAGE023
；
当迭代计算过程中出现异常处理机制，则终止迭代计算过程，将初始并联关节角度作为并联双足机器人的下一并联关节角度；
所述异常处理机制包括迭代计算时间超过设定的最大值或迭代计算过程中出现奇异值；
迭代计算过程中是否出现奇异值采用计算雅可比矩阵行列式的方法进行判断；
所述雅可比矩阵行列式为：
Figure 928310DEST_PATH_IMAGE057
；
当
Figure 665322DEST_PATH_IMAGE058
，迭代计算过程中存在奇异值，则终止迭代计算过程，将初始并联关节角度作为并联双足机器人的下一并联关节角度；
步骤五：以步骤四得到的下一并联关节角度作为牛顿-拉夫逊迭代的计算式的初值，重复步骤四，从而实现机器人的闭环控制。
与前述并联双足机器人关节实时控制方法的实施例相对应，本发明还提供了并联双足机器人关节实时控制装置的实施例。
参见图6，本发明实施例提供的一种并联双足机器人关节实时控制装置，包括一个或多个处理器，用于实现上述实施例中的并联双足机器人关节实时控制方法。
本发明并联双足机器人关节实时控制装置的实施例可以应用在任意具备数据处理能力的设备上，该任意具备数据处理能力的设备可以为诸如计算机等设备或装置。装置实施例可以通过软件实现，也可以通过硬件或者软硬件结合的方式实现。以软件实现为例，作为一个逻辑意义上的装置，是通过其所在任意具备数据处理能力的设备的处理器将非易失性存储器中对应的计算机程序指令读取到内存中运行形成的。从硬件层面而言，如图6所示，为本发明并联双足机器人关节实时控制装置所在任意具备数据处理能力的设备的一种硬件结构图，除了图6所示的处理器、内存、网络接口、以及非易失性存储器之外，实施例中装置所在的任意具备数据处理能力的设备通常根据该任意具备数据处理能力的设备的实际功能，还可以包括其他硬件，对此不再赘述。
上述装置中各个单元的功能和作用的实现过程具体详见上述方法中对应步骤的实现过程，在此不再赘述。
对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本发明方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
本发明实施例还提供一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现上述实施例中的并联双足机器人关节实时控制方法。
所述计算机可读存储介质可以是前述任一实施例所述的任意具备数据处理能力的设备的内部存储单元，例如硬盘或内存。所述计算机可读存储介质也可以是任意具备数据处理能力的设备的外部存储设备，例如所述设备上配备的插接式硬盘、智能存储卡（Smart Media Card，SMC）、SD卡、闪存卡（Flash Card）等。进一步的，所述计算机可读存储介质还可以既包括任意具备数据处理能力的设备的内部存储单元也包括外部存储设备。所述计算机可读存储介质用于存储所述计算机程序以及所述任意具备数据处理能力的设备所需的其他程序和数据，还可以用于暂时地存储已经输出或者将要输出的数据。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。

Claims (9)

1.一种并联双足机器人关节实时控制方法，其特征在于，包括以下步骤：
步骤一：根据双足机器人并联关节的几何关系，列出并联关节角度与电机角度的运动方程；
步骤二：根据并联关节角度与电机角度的运动方程，获得电机角度与并联关节角度的数值联系；
步骤三：读取初始并联关节角度，由电机角度与并联关节角度的数值联系计算得到初始并联关节角度相对应的初始电机角度，将并联关节角度与电机角度的运动方程转化为牛顿-拉夫逊迭代的标准形式，获得牛顿-拉夫迭代的计算式；
步骤四：以初始并联关节角度作为牛顿-拉夫逊迭代的计算式的初值进行求解；当迭代计算过程达到结束条件，则迭代计算结束，将求得的迭代求解值作为并联双足机器人相对于初始并联关节角度的下一并联关节角度；当迭代计算过程中出现异常处理机制，则终止迭代计算过程，将初始并联关节角度作为并联双足机器人的下一并联关节角度；
步骤五：以步骤四得到的下一并联关节角度作为牛顿-拉夫逊迭代的计算式的初值，重复步骤四，从而实现机器人的闭环控制。
2.根据权利要求1所述的一种并联双足机器人关节实时控制方法，其特征在于，所述步骤一具体为：
所述双足机器人并联关节包括联合带动所述双足机器人的足部的横滚角和俯仰角转动的第一运动链M1N1P1O和第二运动链M2N2P2O，所述第一运动链M1N1P1O包括第一水平杆和第一竖直杆，所述第二运动链M2N2P2O包括第二水平杆和第二竖直杆；其中点N1为第一水平杆和第一竖直杆的铰接点，点N2为第二水平杆和第二竖直杆的铰接点，点P1为第一竖直杆与脚板的铰接点，点P2为第二竖直杆与脚板的铰接点，O点为脚板与十字铰接轴的中心点，以O点为圆心建立X、Y、Z坐标系，并给出并联关节角度与电机角度的运动方程：
Figure 674191DEST_PATH_IMAGE001
，
Figure 325752DEST_PATH_IMAGE002
；
其中
Figure 902227DEST_PATH_IMAGE003
和
Figure 31857DEST_PATH_IMAGE004
为电机角度，即电机带动第一水平杆和第二水平杆的转动角度，当第一水平杆和第二水平杆水平时
Figure 518333DEST_PATH_IMAGE003
和
Figure 75216DEST_PATH_IMAGE004
的角度分别为0，当第一水平杆和第二水平杆向上转动时
Figure 873408DEST_PATH_IMAGE003
和
Figure 993680DEST_PATH_IMAGE004
的角度分别为正值，反之则为负值；
Figure 396979DEST_PATH_IMAGE005
和
Figure 187081DEST_PATH_IMAGE006
是并联关节角度，分别为所述双足机器人的足部的俯仰角和横滚角，当脚板水平时
Figure 675831DEST_PATH_IMAGE005
和
Figure 819367DEST_PATH_IMAGE006
都为90°，当脚板绕x轴正方向转动时
Figure 139490DEST_PATH_IMAGE005
的角度变小，反之则变大，当脚板绕y轴正方向转动时
Figure 303755DEST_PATH_IMAGE006
的角度变小，反之则变大；
Figure 466752DEST_PATH_IMAGE007
，
Figure 273034DEST_PATH_IMAGE008
，
Figure 650926DEST_PATH_IMAGE009
，
Figure 658196DEST_PATH_IMAGE010
，
Figure 121538DEST_PATH_IMAGE011
，
Figure 997091DEST_PATH_IMAGE012
，P点为线P1P2的中点，R点为点M1在Z轴上的投影，Q点为点M2在Z轴上的投影，OM1M2组成的平面始终在XOZ平面内，点Z’为过通过P点且平行于Z轴的辅助线正半轴的任意一点。
3.根据权利要求2所述的一种并联双足机器人关节实时控制方法，其特征在于，所述步骤二具体为：根据并联关节角度与电机角度的运动方程获得电机角度与并联关节角度的数值联系，由并联关节角度计算电机角度，得到电机角度
Figure 229489DEST_PATH_IMAGE003
和
Figure 656928DEST_PATH_IMAGE004
的解析式：
Figure 404304DEST_PATH_IMAGE013
，
其中，A，B，D，E，F，G，H，I的中间变量如下：
Figure 21230DEST_PATH_IMAGE014
。
4.根据权利要求2所述的一种并联双足机器人关节实时控制方法，其特征在于，所述牛顿-拉夫逊迭代的标准形式为：
Figure 45818DEST_PATH_IMAGE015
；
其中
Figure 722787DEST_PATH_IMAGE016
，
Figure 957459DEST_PATH_IMAGE017
。
5.根据权利要求4所述的一种并联双足机器人关节实时控制方法，其特征在于，所述牛顿-拉夫逊迭代的计算式为：
Figure 112497DEST_PATH_IMAGE018
，
其中
Figure 506438DEST_PATH_IMAGE019
和
Figure 151046DEST_PATH_IMAGE020
为牛顿-拉夫逊迭代的计算式的初值，
Figure 810698DEST_PATH_IMAGE021
和
Figure 707109DEST_PATH_IMAGE022
为迭代求解值。
6.根据权利要求5所述的一种并联双足机器人关节实时控制方法，其特征在于，所述结束条件为：
Figure 503027DEST_PATH_IMAGE023
。
7.根据权利要求5所述的一种并联双足机器人关节实时控制方法，其特征在于，所述异常处理机制包括迭代计算时间超过设定的最大值或迭代计算过程中出现奇异值。
8.一种并联双足机器人关节实时控制装置，其特征在于，包括一个或多个处理器，用于实现权利要求1-7中任一项所述的并联双足机器人关节实时控制方法。
9.一种计算机可读存储介质，其上存储有程序，其特征在于，该程序被处理器执行时，用于实现权利要求1-7中任一项所述的并联双足机器人关节实时控制方法。
CN202111649171.3A 2021-12-30 2021-12-30 并联双足机器人关节实时控制方法和装置 Active CN114002990B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111649171.3A CN114002990B (zh) 2021-12-30 2021-12-30 并联双足机器人关节实时控制方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111649171.3A CN114002990B (zh) 2021-12-30 2021-12-30 并联双足机器人关节实时控制方法和装置

Publications (2)

Publication Number Publication Date
CN114002990A true CN114002990A (zh) 2022-02-01
CN114002990B CN114002990B (zh) 2022-04-08

Family

ID=79932294

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111649171.3A Active CN114002990B (zh) 2021-12-30 2021-12-30 并联双足机器人关节实时控制方法和装置

Country Status (1)

Country Link
CN (1) CN114002990B (zh)

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA2888603A1 (en) * 2012-10-19 2014-04-24 Inos Automationssoftware Gmbh Method for in-line calibration of an industrial robot, calibration system for performing such a method and industrial robot comprising such a calibration system
CN104933232A (zh) * 2015-06-03 2015-09-23 西安交通大学 一类杆端浮动型六自由度并联机器人带角度补偿的运动学求解方法
CN107688293A (zh) * 2017-07-28 2018-02-13 重庆金山医疗器械有限公司 一种微创外科手术机器人的建模方法
CN109202889A (zh) * 2017-06-30 2019-01-15 沈阳新松机器人自动化股份有限公司 一种柔性多关节机器人电流力控制系统及方法
CN109895101A (zh) * 2019-04-09 2019-06-18 大连理工大学 一种关节型机械臂逆运动学数值唯一解求取方法
CN109987169A (zh) * 2017-12-29 2019-07-09 深圳市优必选科技有限公司 双足机器人的步态控制方法、装置、终端设备及介质
CN111914416A (zh) * 2020-07-24 2020-11-10 之江实验室 一种高能效轻量化结构双足机器人的逆运动学求解方法
CN112882485A (zh) * 2021-01-25 2021-06-01 河南大学 一种双足机器人逆运动学的几何计算方法
CN113835429A (zh) * 2021-09-08 2021-12-24 南方科技大学 一种仿生双足机器人的控制方法及仿生双足机器人

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA2888603A1 (en) * 2012-10-19 2014-04-24 Inos Automationssoftware Gmbh Method for in-line calibration of an industrial robot, calibration system for performing such a method and industrial robot comprising such a calibration system
CN104933232A (zh) * 2015-06-03 2015-09-23 西安交通大学 一类杆端浮动型六自由度并联机器人带角度补偿的运动学求解方法
CN109202889A (zh) * 2017-06-30 2019-01-15 沈阳新松机器人自动化股份有限公司 一种柔性多关节机器人电流力控制系统及方法
CN107688293A (zh) * 2017-07-28 2018-02-13 重庆金山医疗器械有限公司 一种微创外科手术机器人的建模方法
CN109987169A (zh) * 2017-12-29 2019-07-09 深圳市优必选科技有限公司 双足机器人的步态控制方法、装置、终端设备及介质
CN109895101A (zh) * 2019-04-09 2019-06-18 大连理工大学 一种关节型机械臂逆运动学数值唯一解求取方法
CN111914416A (zh) * 2020-07-24 2020-11-10 之江实验室 一种高能效轻量化结构双足机器人的逆运动学求解方法
CN112882485A (zh) * 2021-01-25 2021-06-01 河南大学 一种双足机器人逆运动学的几何计算方法
CN113835429A (zh) * 2021-09-08 2021-12-24 南方科技大学 一种仿生双足机器人的控制方法及仿生双足机器人

Also Published As

Publication number Publication date
CN114002990B (zh) 2022-04-08

Similar Documents

Publication Publication Date Title
US11579591B2 (en) Feedforward control method for floating base dynamics, computer-readable storage medium and robot
US9334002B2 (en) Balance control apparatus of robot and control method thereof
US9120227B2 (en) Human motion tracking control with strict contact force constraints for floating-base humanoid robots
US8874263B2 (en) Walking robot and control method thereof
US8271137B2 (en) Robot and method of controlling the same
CN110262510B (zh) 一种降低行走能耗的仿人机器人质心轨迹规划方法
CN1873572A (zh) 操纵装置轨迹的控制
Wang et al. REDMAX: Efficient & flexible approach for articulated dynamics
CN109760063B (zh) 并联机器人的控制方法、装置、设备及存储介质
CN113043279B (zh) 手术机器人的控制方法、控制器、系统、电子设备与介质
Date et al. Control of 3D snake-like locomotive mechanism based on continuum modeling
CN114002990B (zh) 并联双足机器人关节实时控制方法和装置
Liu et al. An articulated closed kinematic chain planar robotic leg for high-speed locomotion
CN115994288B (zh) 一种双足机器人逆运动学求解方法及装置
CN116304512B (zh) 一种机器人腿部的逆运动学求解方法及装置
CN112199833A (zh) 关节动力学模型优化方法、系统、终端设备及存储介质
Song Modeling and control of three-degree-of-freedom medical assistant robot
US20230182307A1 (en) Jumping motion control method for biped robot, biped robot and computer-readable storage medium
Szomiński et al. Development of a cyber-physical system for mobile robot control using erlang
CN113650010B (zh) 一种乒乓球机器人的运动控制方法、系统及存储介质
Masuda et al. Sim-to-real transfer of compliant bipedal locomotion on torque sensor-less gear-driven humanoid
Shi et al. Reference-free learning bipedal motor skills via assistive force curricula
BAYRAKTAROĞLU et al. Design and development of the ITU biped robot
CN112606035A (zh) 一种三自由度机器人颈部装置及工作方法
US20220193899A1 (en) Pose control method, robot and computer-readable storage medium

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