CN109591023A - 串联机器人的迭代计算方法及串联机器人 - Google Patents

串联机器人的迭代计算方法及串联机器人 Download PDF

Info

Publication number
CN109591023A
CN109591023A CN201811648964.1A CN201811648964A CN109591023A CN 109591023 A CN109591023 A CN 109591023A CN 201811648964 A CN201811648964 A CN 201811648964A CN 109591023 A CN109591023 A CN 109591023A
Authority
CN
China
Prior art keywords
joint
serial manipulator
active force
mass center
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
CN201811648964.1A
Other languages
English (en)
Other versions
CN109591023B (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.)
Beijing Youbixuan Intelligent Robot Co ltd
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 CN201811648964.1A priority Critical patent/CN109591023B/zh
Publication of CN109591023A publication Critical patent/CN109591023A/zh
Priority to US16/669,554 priority patent/US11007639B2/en
Application granted granted Critical
Publication of CN109591023B publication Critical patent/CN109591023B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/06—Programme-controlled manipulators characterised by multi-articulated arms
    • 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
    • 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/085—Force or torque sensors
    • 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
    • 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
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J17/00—Joints
    • B25J17/02—Wrist joints
    • B25J17/0283—Three-dimensional joints
    • 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/02—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type
    • B25J9/04—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type by rotating at least one arm, excluding the head movement itself, e.g. cylindrical coordinate type or polar coordinate type
    • B25J9/046—Revolute coordinate type
    • 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/06—Programme-controlled manipulators characterised by multi-articulated arms
    • B25J9/065—Snake robots
    • 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/39—Robotics, robotics to robotics hand
    • G05B2219/39261—Calculate driving torque from dynamic model, computed torque method variant

Abstract

本发明公开了一种串联机器人的迭代计算方法及串联机器人，该方法包括：针对末端关节，根据其前一关节对它的作用力及所受外力进行分析，根据分析结果计算前一关节对它的作用力；在个别的于末端关节之前的每一关节，均根据前一关节对它的作用力及其对后一关节的作用力进行分析，根据分析结果计算前一关节对它的作用力；在得到前一关节对它的作用力后，获取每一关节的关节角速度和关节角加速度，每一关节均根据前一关节对它的作用力、关节角速度及关节角加速度计算该关节对应的力矩；将关节对应的力矩投影到关节内电机，得到当前时刻应施加的力矩。本发明算法结构简单，代码量少，提高末端的跟踪精度，较少跟踪误差。

Description

串联机器人的迭代计算方法及串联机器人
技术领域
本发明涉及机器人技术领域，具体而言，涉及一种串联机器人的迭代计算方法及串联机器人。
背景技术
串联机器人因为具有高非线性和高耦合性，其动力学问题受到广泛的关注，一个n自由度的串联机器人的全动力学方程式是：
其中，为关节角速度，为关节角加速度，τ为各关节的电机转矩，JT为串联机器人末端关节质心的雅克比矩阵的转置矩阵，F为串联机器人末端关节所受外力，M为串联机器人质量惯量矩阵，C为串联机器人速度项矩阵，G为串联机器人重力项矩阵。
上述的全动力学方程，求解各关节的电机转矩需要首先获得M，C，G等矩阵，然而由于串联机器人的高非线性和高耦合性，计算M，C，G等矩阵的过程非常复杂，算法运行效率不高，运算时间比较长，在串联机器人的自由度较多以后，很难将它应用于1毫秒周期的硬实时控制系统。
发明内容
鉴于上述问题，本发明实施例的目的在于提供一种串联机器人的迭代计算方法及串联机器人，以解决现有技术的不足。
根据本发明的一个实施方式，提供一种串联机器人的迭代计算方法，所述串联机器人包括依次串联的多个关节，每一关节均包括电机和所述电机带动的连杆，该方法包括：
针对末端关节，根据其前一关节对它的作用力及其所受外力进行受力分析，根据分析结果计算该末端关节受到的前一关节对它的作用力；
针对所述末端关节之前的每一关节，均根据其前一关节对它的作用力及其对后一关节的作用力进行受力分析，根据分析结果计算该关节受到的前一关节对它的作用力；
在得到前一关节对它的作用力后，获取每一关节的关节角速度和关节角加速度，每一关节均根据所述前一关节对它的作用力、该关节的关节角速度及关节角加速度计算该关节对应的力矩；
将该关节对应的力矩投影到该关节对应电机，得到该电机在当前时刻应施加的力矩，以使所述电机根据该应施加的力矩控制该关节进行运动。
在上述的串联机器人的迭代计算方法中，通过下式计算末端关节对应的力矩：
其中，Nn-1,n为第n个关节即末端关节对应的力矩，In为末端关节质心坐标系的转动惯量，为第n个关节的关节角速度，为第n个关节的关节角加速度，N外为外界对所述串联机器人末端关节的外力矩，Fn-1,n为第i-1个关节对第i个关节的作用力，r1为Fn-1,n到第i个关节质心的矢量，F外为外界对串联机器人末端关节的外力，r2为F外到第i个关节质心的矢量。
在上述的串联机器人的迭代计算方法中，通过下式计算末端关节之前每一关节对应的力矩：
其中，Nn-2,n-1为第n-1个关节对应的力矩，In-1为第n-1个关节即末端关节质心坐标系的转动惯量，为第n-1个关节的关节角速度，为第n-1个关节的关节角加速度，N外为外界对所述串联机器人末端关节的外力矩，Fn-2,n-1为第i-2个关节对第i-1个关节的作用力，r3为Fn-2,n-1到第i-1个关节质心的矢量，Fn-1,n为第i-1个关节对第i个关节的作用力，r4为Fn-1,n到第i-1个关节质心的矢量。
在上述的串联机器人的迭代计算方法中，还包括：
针对串联机器人的每个关节构建一结构体数组，所述结构体数组包括关节属性信息及根据所述关节属性信息计算得到的关节运动信息，其中，所述关节运动信息包括关节角速度和关节角加速度。
在上述的串联机器人的迭代计算方法中，所述关节属性信息包括关节中电机旋转轴的空间矢量及关节中电机转角；
所述方法还包括：
根据所述关节属性信息计算关节角速度；其具体包括：
根据所述关节中电机旋转轴的空间矢量及所述关节中电机转角计算该关节的在世界坐标系的姿态；
根据该关节的在世界坐标系的姿态计算关节角；
通过下式将所述关节角对时间进行微分得到关节角速度：
其中，为关节角速度，θ为关节角，为末端关节质心的雅克比矩阵的逆矩阵，V为末端关节的速度。
在上述的串联机器人的迭代计算方法中，所述方法还包括：
通过下式计算关节角加速度：
其中，为关节角加速度，A为末端关节质心加速度，为对所述末端关节质心的雅克比矩阵求导后的矩阵。
在上述的串联机器人的迭代计算方法中，通过以下步骤计算末端关节质心加速度：
根据该末端关节的关节角速度及末端关节坐标系原点的雅克比矩阵得到末端关节坐标系原点在世界坐标系的速度；
根据所述末端关节坐标系原点在世界坐标系的速度得到该末端关节质心在世界坐标系的速度；
对所述末端关节质心在世界坐标系的速度对时间进行求导，得到该末端关节质心加速度。
在上述的串联机器人的迭代计算方法中，通过下式将所述关节对应的力矩投影到该关节内电机旋转轴：
τn＝Zn·Nn-1,n
其中，τn为第n个关节内电机在当前时刻应施加的力矩，Zn为第n个关节内电机旋转轴矢量。
在上述的串联机器人的迭代计算方法中，每隔预设时间对各关节的关节属性信息进行初始化，并根据初始化的关节属性信息计算所述关节运动信息。
在上述的串联机器人的迭代计算方法中，所述关节质心的雅克比矩阵通过下式获得：
其中，Jj为第i个关节质心的雅克比矩阵，J1i为第i个关节质心对第j个关节质心的线速度微分，J1i＝Zi×Di-j，Zi为第i个关节内电机的旋转轴矢量，Di-1为第i个电机到第j个关节质心的位置矢量，J2i为第i个关节质心对第j个关节质心的角速度微分，J2i＝Zi，i＝j。
在上述的串联机器人的迭代计算方法中，所述关节坐标系原点的雅克比矩阵通过下式获得：
其中，Gj为第i个关节坐标系原点的雅克比矩阵，G1i为第i个关节坐标系原点对第j个关节坐标系原点的线速度微分，G1i＝Zi×D1i-j，Zi为第i个电机的旋转轴矢量，D1i-j为第i个电机到第j个关节关节坐标系原点的位置矢量，G2i为第i个关节坐标系原点对第j个关节坐标系原点的角速度微分，G2i＝Zi，i＝j。
根据本发明的另一个实施方式，提供一种串联机器人的迭代计算装置，所述串联机器人包括依次串联的多个关节，每一关节均包括电机和所述电机带动的连杆，该装置包括：
第一分析计算模块，用于针对末端关节，根据其前一关节对它的作用力及其所受外力进行受力分析，根据分析结果计算该末端关节受到的前一关节对它的作用力；
第二分析计算模块，用于针对所述末端关节之前的每一关节，均根据其前一关节对它的作用力及其对后一关节的作用力进行受力分析，根据分析结果计算该关节受到的前一关节对它的作用力；
力矩计算模块，用于在得到前一关节对它的作用力后，获取每一关节的关节角速度和关节角加速度，每一关节均根据所述前一关节对它的作用力、该关节的关节角速度及关节角加速度计算该关节对应的力矩；
投影模块，用于将该关节对应的力矩投影到该关节内电机，得到该关节内电机在当前时刻应施加的力矩，以使所述关节内电机根据该应施加的力矩控制该关节进行运动。
根据本发明的再一个实施方式，提供一种串联机器人，所述串联机器人包括存储器以及处理器，所述存储器用于存储计算机程序，所述处理器运行所述计算机程序以使所述串联机器人执行上述的串联机器人的迭代计算方法或者上述的串联机器人的迭代计算装置中的各个模块的功能。
根据本发明的又一个实施方式，提供一种计算机可读存储介质，其存储有所述串联机器人中所用的所述计算机程序。
本公开的实施例提供的技术方案可以包括如下有益效果：
本发明中一种串联机器人的迭代计算方法及串联机器人，通过逆向动力学方式，从末端关节开始，依次向前求解个各个关节所需的力矩和它们之间的相互作用力，避免计算上述的M、C及G等矩阵，算法结构简单，代码量少，算法复杂度低，可实现在1毫秒硬实时控制，提高末端关节的跟踪精度，减少跟踪误差，加强串联机器人的快速行走能力。
为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对本发明保护范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1示出了本发明实施例提供的一种串联机器人的迭代计算方法的流程示意图。
图2示出了本发明实施例提供的一种串联机器人各关节的结构示意图。
图3示出了本发明实施例提供的一种关节角的几何示意图。
图4a-4b示出了本发明实施例提供的一种关节受力分析的示意图。
图5示出了本发明实施例提供的一种串联机器人的迭代计算装置的结构示意图。
主要元件符号说明：
200-串联机器人的迭代计算装置；210-第一分析计算模块；220-第二分析计算模块；230-力矩计算模块；240-投影模块。
具体实施方式
下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例1
图1示出了本发明实施例提供的一种串联机器人的迭代计算方法的流程示意图。
该串联机器人的迭代计算方法应用于串联机器人，所述串联机器人包括依次串联的多个关节，每一关节均包括电机和所述电机带动的连杆。
如图2所示，所述串联机器人可包括依次串联的多个关节1～n，第1个关节和第2二个关节串联连接，第2个关节和第3个关节串联连接，第3个关节和第4个关节串联连接，……，第n-1个关节和第n个关节串联连接，其中，该串联机器人的最后一个关节即第n个关节可视为末端关节，该末端关节的另一端(即末端)可以串联连接机械爪等。每个关节均包括电机P及该电机带动的连杆Q。
该串联机器人的迭代计算方法包括以下步骤：
在步骤S110中，针对末端关节，根据其前一关节对它的作用力及其所受外力进行受力分析，根据分析结果计算该末端关节受到的前一关节对它的作用力。
进一步地，在步骤S110之前还包括：
针对串联机器人的每个关节构建一结构体数组，所述结构体数组包括关节属性信息及根据所述关节属性信息计算得到的关节运动信息，其中，所述关节运动信息包括关节角速度、关节角加速度。
具体地，为串联机器人的每一关节构建一结构体数组，将所述关节对应的关节属性信息及根据该属性信息计算得到的关节运动信息均存储在该关节对应的结构体数组内。串联机器人的全部关节根据串联顺序可形成链式数据结构，便于循环执行所述的串联机器人的迭代计算方法。通过一种简单的数据结构，记录该关节的全部信息，在串联机器人的迭代计算方法执行过程中，以循环迭代的方式，通过少量的代码直接调用每一关节对应结构体数组中的所有信息，高效的执行串联机器人的迭代计算方法。
进一步地，所述关节属性信息包括关节内电机旋转轴的空间矢量及关节内电机转角。
根据所述关节属性信息计算关节角速度包括：
根据所述关节中电机旋转轴的空间矢量及所述关节中电机转角计算该关节的在世界坐标系的姿态；根据该关节的在世界坐标系的姿态计算关节角；通过下式将所述关节角对时间进行微分得到关节角速度：
其中，为关节角速度，θ为关节角，为末端关节质心的雅克比矩阵的逆矩阵，V为末端关节的速度。
具体地，已知各关节转矩求解各关节角加速度的过程叫做正向运动学；已知各关节角加速度，求解各关节转矩的过程叫做逆向动力学。
将所述关节中电机旋转轴的空间矢量及所述关节中电机转角通过正向运动学方程式计算该关节在世界坐标系的姿态。
所述关节属性信息还包括该关节坐标系与下一坐标系的矢量。
将该关节坐标系与下一坐标系的矢量、关节中电机旋转轴的空间矢量及所述关节中电机转角通过正向运动学方程式计算该关节在世界坐标系的位置。
将所述关节在世界坐标系的姿态及所述关节在世界坐标系的位置通过逆向动力学方程式计算该关节的关节角。
例如，如图3所示，以具有三个依次串联的关节的串联机器人为例进行说明计算第2个关节的关节角的过程：
令r＝norm(d1,3)
其中，θ为第2个关节的关节角，L3为第1个关节到第2个关节的距离，L4为第2个关节到第3个关节的距离，d1,3为第3个关节到第1个关节之间的位置矢量，可通过第3个关节在世界坐标系的姿态、第3个关节在世界坐标系的位置及第3个关节坐标在世界坐标系的位置等获得。
进一步地，所述关节质心的雅克比矩阵通过下式获得：
其中，Jj为第i个关节质心的雅克比矩阵，J1i为第i个关节质心对第j个关节质心的线速度微分，J1i＝Zi×Di-j，Zi为第i个关节内电机的旋转轴矢量，Di-j为第i个电机到第j个关节质心的位置矢量，J2i为第i个关节质心对第j个关节质心的角速度微分，J2i＝Zi，i＝j。
值得注意的是，从第一个关节处计算关节质心的雅克比矩阵，后续关节均根据其自身及其前面所有关节的关节质心雅克比矩阵计算自身的关节质心雅克比矩阵。
例如，计算第4个关节的关节质心雅克比矩阵：
其中，Z1为第1个关节内电机的旋转轴矢量，Z2为第2个关节内电机的旋转轴矢量，Z3为第3个关节内电机的旋转轴矢量，Z4为第4个关节内电机的旋转轴矢量，D1-4为第1个关节的电机到第4个关节质心的位置矢量，D2-4为第2个关节的电机到第4个关节质心的位置矢量，D3-4为第3个关节的电机到第4个关节质心的位置矢量，D4-4为第4个关节的电机到第4个关节质心的位置矢量。
进一步地，通过下式计算关节角加速度：
其中，为关节角加速度，A为末端关节质心加速度，为对所述末端关节质心的雅克比矩阵求导后的矩阵。
其中，对雅克布矩阵的求导，就是对雅克布矩阵中每一个分量求导。
例如，对J13＝Z3×D3-4进行求导后可得：
进一步地，通过以下步骤计算串联机器人中每一关节质心加速度：
根据该关节关节角速度及关节坐标系原点的雅克比矩阵得到关节坐标系原点在世界坐标系的速度；根据所述关节坐标系原点在世界坐标系的速度得到该关节质心在世界坐标系的速度；对所述关节质心在世界坐标系的速度对时间进行求导，得到该关节质心加速度。
具体地，通过下式计算关节坐标系原点在世界坐标系的速度：
其中，vi为关节坐标系原点在世界坐标系的速度，Jlinko为关节坐标系原点的雅克比矩阵。
进一步地，所述关节坐标系原点的雅克比矩阵通过下式获得：
其中，Gj为第i个关节坐标系原点的雅克比矩阵，G1i为第i个关节坐标系原点对第j个关节坐标系原点的线速度微分，G1i＝Zi×D1i-j，Zi为第i个电机的旋转轴矢量，D1i-j为第i个电机到第j个关节关节坐标系原点的位置矢量，G2i为第i个关节坐标系原点对第j个关节坐标系原点的角速度微分，G2i＝Zi，i＝j。
值得注意的是，从第一个关节处计算关节坐标系原点的雅克比矩阵，后续关节均根据其自身及其前面所有关节的关节坐标系原点的雅克比矩阵计算自身的关节坐标系原点的雅克比矩阵。
例如，计算第4个关节的关节坐标系原点的雅克比矩阵：
其中，Z1为第1个关节内电机的旋转轴矢量，Z2为第2个关节内电机的旋转轴矢量，Z3为第3个关节内电机的旋转轴矢量，Z4为第4个关节内电机的旋转轴矢量，D11-4为第1个关节的电机到第4个关节坐标系原点的位置矢量，D12-4为第2个关节的电机到第4个关节坐标系原点的位置矢量，D13-4为第3个关节的电机到第4个关节坐标系原点的位置矢量，D14-4为第4个关节的电机到第4个关节坐标系原点的位置矢量。
通过下式计算该关节质心在世界坐标系的速度：
具体地，以6维关节质心在世界坐标系的速度为例进行说明，关节质心在世界坐标系下的3维角速度等于关节坐标系原点在世界坐标系下的3维角速度；关节质心在世界坐标系下的3维线速度等于关节坐标系原点在世界坐标系下的3线角速度点乘关节质心与关节坐标系原点之间的距离。
其中，vc为关节质心在世界坐标系的6维速度，S1为关节质心在世界坐标系的3维角速度，S2为关节质心在世界坐标系的3维线速度，Dc-o为关节质心与关节坐标系原点之间的距离。
通过下式得到该关节质心加速度：
其中，a为关节质心加速度，Jlink为该关节质心雅克比矩阵。
进一步地，可通过上述的计算关节质心加速度的方式计算末端关节质心加速度：
根据该末端关节的关节角速度及末端关节坐标系原点的雅克比矩阵得到末端关节坐标系原点在世界坐标系的速度；根据所述末端关节坐标系原点在世界坐标系的速度得到该末端关节质心在世界坐标系的速度；对所述末端关节质心在世界坐标系的速度对时间进行求导，得到该末端关节质心加速度。
进一步地，每隔预设时间对各关节的关节属性信息进行初始化，并根据初始化的关节属性信息计算所述关节运动信息。
具体地，可以每隔每个指令周期均执行一次初始化操作，减少维护成本。
具体地，串联机器人在设定好各坐标系后，每个关节的结构体数组中，各关节坐标系之间的位置关系、旋转轴矢量、各关节的质量、各关节质心位置及各关节相对于质心的转动惯量等数据可以按照一定的顺序制作成文件，所有关节的结构体数组均为相同的格式、数据和文件顺序，按照统一的流程快速读取文件，并为读取文件中的数据赋初始值，快速而规范地完成该串联机器人的初始化过程。便于循环执行复杂动力学算法。
不同的串联机器人，初始化的文件的长度和相关参数可不同，其执行的串联机器人的迭代计算方法均相同，将串联机器人的迭代计算方法中所需的数据部分内容(即存储在结构体数组中数据)和方法本身分割开来，提高算法执行的速度，减少算法之间耦合性。
如图2所示，第n个关节即为末端关节，该第n个关节可根据其前一关节即第n-1个关节对它的相互作用力、该第n个关节在与外界物体进行接触时受到的外界物体对它的外力及该第n个关节受到的重力，对该第n个关节所受的力进行分析，根据已规划的该末端关节质心加速度，及受力分析结果，得到该第n-1个关节对第n个关节的作用力。
具体地，如图4a所示，对于第n个关节即末端关节来说，它受外力F外、第n-1个关节对它的作用力Fn-1,n和重力G：
Fn-1,n+F外+mg＝ma
其中，a为末端关节质心规划的加速度，在没有和外界进行接触时，F外为零，m末端关节的质量，g为末端关节所受重力。
那么，Fn-1,n＝ma-mg-F外。
需要指出的是，由于两个物体之间的力是相互的，第n-1个关节对第n个关节的作用力Fn-1,n，同样也是第n个关节对第n-1个关节的作用力。
在步骤S120中，针对所述末端关节之前的每一关节，均根据其前一关节对它的作用力及其对后一关节的作用力进行受力分析，根据分析结果计算该关节受到的前一关节对它的作用力。
如图2所示，针对第n个关节之前的每一关节，该关节可根据其前一关节对它的相互作用力、该关节对其后一个关节的相互作用力及该关节受到的重力，对该关节所受的力进行分析，并从该关节对应的结构体数组中获取该关节质心加速度，根据该关节质心加速度及受力分析结果，得到该关节对其前一关节的作用力。
如图4b所示，以第n-1个关节为例进行说明。第n-1个关节受到3个力，分别为：第n-2个关节对它的作用力Fn-2,n-1、第n个关节对它的作用力Fn-1,n及重力G。
Fn-2,n-1+Fn-1,n+mn-1g＝mn-1an-1
其中，an-1为第n-1个关节质心的加速度，mn-1为第n-1个关节的质量，g为第n-1个关节所受重力。
那么，Fn-2,n-1＝mn-1an-1-Fn-1,n-mn-1g。
需要指出的是，由于两个物体之间的力是相互的，第n-2个关节对第n-1个关节的作用力Fn-2,n-1，同样也是第n-1个关节对第n-2个关节的作用力。
对于第n个关节之前的每一关节来说，其均受前一关节对它的作用力、其对后一关节的作用力和重力的影响，所以，可通过前一关节对它的作用力、其对后一关节的作用力计算该关节受到的前一关节对它的作用力，并通过迭代的思想计算所有关节受到的前一全解对它的作用力。
在步骤S130中，在得到前一关节对它的作用力后，获取每一关节的关节角速度和关节角加速度，每一关节均根据前一关节对它的作用力、该关节的关节角速度及关节角加速度计算该关节对应的力矩。
具体地，每一关节均从其对应的结构体数组获取已存储的关节角速度和关节角加速度。
进一步地，通过下式计算末端关节对应电机的力矩：
其中，Nn-1,n为第n个关节即末端关节对应的力矩，In为末端关节质心坐标系的转动惯量，为第n个关节的关节角速度，为第n个关节的关节角加速度，N外为外界对所述串联机器人末端关节的外力矩，Fn-1,n为第i-1个关节对第i个关节的作用力，r1为Fn-1,n到第i个关节质心的矢量，F外为外界对串联机器人末端关节的外力，r2为F外到第i个关节质心的矢量。
进一步地，通过下式计算末端关节之前每一关节对应电机的力矩：
其中，Nn-2,n-1为第n-1个关节对应的力矩，In-1为第n-1个关节即末端关节质心坐标系的转动惯量，为第n-1个关节的关节角速度，为第n-1个关节的关节角加速度，N外为外界对所述串联机器人末端关节的外力矩，Fn-2,n-1为第i-2个关节对第i-1个关节的作用力，r3为Fn-2,n-1到第i-1个关节质心的矢量，Fn-1,n为第i-1个关节对第i个关节的作用力，r4为Fn-1,n到第i-1个关节质心的矢量。
在步骤S140中，将该关节对应的力矩投影到该关节内电机，得到该关节内电机在当前时刻应施加的力矩，以使关节内电机根据该应施加的力矩控制该关节进行运动。
进一步地，由于计算得到的关节对应的力矩为矢量，需要将该矢量投影到该关节内电机的旋转轴，得到关节内电机在当前时刻应施加的力矩的标量：
τn＝Zn·Nn-1,n
其中，τn为第n个关节内电机在当前时刻应施加的力矩，Zn为第n个关节内电机的旋转轴矢量。
在得到关节内电机在当前时刻应施加的力矩后，使关节内电机根据该应施加的力矩控制该关节内电机所带动的连杆进行运动。
实施例2
图5示出了本发明实施例提供的一种串联机器人的迭代计算装置的结构示意图。该串联机器人的迭代计算装置200对应于实施例1中的串联机器人的迭代计算方法，实施例1中的串联机器人的迭代计算方法同样也适用于该串联机器人的迭代计算装置200，在此不再赘述。
所述串联机器人的迭代计算装置200包括第一分析计算模块210、第二分析计算模块220、力矩计算模块230及投影模块240。
第一分析计算模块210，用于针对末端关节，根据其前一关节对它的作用力及其所受外力进行受力分析，根据分析结果计算该末端关节受到的前一关节对它的作用力。
第二分析计算模块220，用于针对所述末端关节之前的每一关节，均根据其前一关节对它的作用力及其对后一关节的作用力进行受力分析，根据分析结果计算该关节受到的前一关节对它的作用力。
力矩计算模块230，用于在得到前一关节对它的作用力后，获取每一关节的关节角速度和关节角加速度，每一关节均根据所述前一关节对它的作用力、该关节的关节角速度及关节角加速度计算该关节对应的力矩。
投影模块240，用于将该关节对应的力矩投影到该关节内电机，得到该关节内电机在当前时刻应施加的力矩，以使所述关节内电机根据该应施加的力矩控制该关节进行运动。
本发明另一实施例还提供了一种串联机器人，所述串联机器人包括存储器以及处理器，所述存储器用于存储计算机程序，所述处理器运行所述计算机程序以使所述串联机器人执行上述的串联机器人的迭代计算方法或上述的串联机器人的迭代计算装置中各模块的功能。
存储器可包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序等；存储数据区可存储根据计算机设备的使用所创建的数据等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
本实施例还提供了一种计算机存储介质，用于储存上述的串联机器人中所使用的串联机器人的迭代计算方法。
本发明中一种串联机器人的迭代计算方法及串联机器人，为串联机器人中每一关节均建立一结构体数组，用来存储关节属性信息及根据所述关节属性信息计算得到的关节运动信息，在串联机器人的迭代计算方法执行过程中，以循环迭代的方式，通过少量的代码直接调用每一关节对应结构体数组中的所有信息，高效的执行文中的串联机器人的迭代计算方法；通过逆向动力学方程式及迭代方式计算关节运动信息，避免计算上述的M、C、G矩阵，减少算法计算量及复杂度；通过逆向动力学方式，从末端关节开始，通过迭代的思想依次向前求解个关节关节所需的力矩和它们之间的相互作用力，算法结构简单，代码量少，算法复杂度低，可实现在1毫秒硬实时控制，提高末端关节的跟踪精度，减少跟踪误差，加强串联机器人的快速行走能力。
在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
另外，在本发明各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。
所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种串联机器人的迭代计算方法，其特征在于，所述串联机器人包括依次串联的多个关节，每一关节均包括电机和所述电机带动的连杆，该方法包括：
针对末端关节，根据其前一关节对它的作用力及其所受外力进行受力分析，根据分析结果计算该末端关节受到的前一关节对它的作用力；
针对所述末端关节之前的每一关节，均根据其前一关节对它的作用力及其对后一关节的作用力进行受力分析，根据分析结果计算该关节受到的前一关节对它的作用力；
在得到前一关节对它的作用力后，获取每一关节的关节角速度和关节角加速度，每一关节均根据所述前一关节对它的作用力、该关节的关节角速度及关节角加速度计算该关节对应的力矩；
将该关节对应的力矩投影到该关节对应电机，得到该电机在当前时刻应施加的力矩，以使所述电机根据该应施加的力矩控制该关节进行运动。
2.根据权利要求1所述的串联机器人的迭代计算方法，其特征在于，通过下式计算末端关节对应的力矩：
其中，Nn-1,n为第n个关节即末端关节对应的力矩，In为末端关节质心坐标系的转动惯量，为第n个关节的关节角速度，为第n个关节的关节角加速度，N外为外界对所述串联机器人末端关节的外力矩，Fn-1,n为第i-1个关节对第i个关节的作用力，r1为Fn-1,n到第i个关节质心的矢量，F外为外界对串联机器人末端关节的外力，r2为F外到第i个关节质心的矢量。
3.根据权利要求2所述的串联机器人的迭代计算方法，其特征在于，通过下式计算末端关节之前每一关节对应的力矩：
其中，Nn-2,n-1为第n-1个关节对应的力矩，In-1为第n-1个关节即末端关节质心坐标系的转动惯量，为第n-1个关节的关节角速度，为第n-1个关节的关节角加速度，N外为外界对所述串联机器人末端关节的外力矩，Fn-2,n-1为第i-2个关节对第i-1个关节的作用力，r3为Fn-2,n-1到第i-1个关节质心的矢量，Fn-1,n为第i-1个关节对第i个关节的作用力，r4为Fn-1,n到第i-1个关节质心的矢量。
4.根据权利要求1所述的串联机器人的迭代计算方法，其特征在于，还包括：
针对串联机器人的每个关节构建一结构体数组，所述结构体数组包括关节属性信息及根据所述关节属性信息计算得到的关节运动信息，其中，所述关节运动信息包括关节角速度和关节角加速度。
5.根据权利要求4所述的串联机器人的迭代计算方法，其特征在于，所述关节属性信息包括关节中电机旋转轴的空间矢量及关节中电机转角；所述方法还包括：
根据所述关节属性信息计算关节角速度；其具体包括：
根据所述关节中电机旋转轴的空间矢量及所述关节中电机转角计算该关节在世界坐标系的姿态；
根据该关节在世界坐标系的姿态计算关节角；
通过下式将所述关节角对时间进行微分得到关节角速度：
其中，为关节角速度，θ为关节角，为末端关节质心的雅克比矩阵的逆矩阵，V为末端关节的速度。
6.根据权利要求5所述的串联机器人的迭代计算方法，其特征在于，所述方法还包括：
通过下式计算关节角加速度：
其中，为关节角加速度，A为末端关节质心加速度，为对所述末端关节质心的雅克比矩阵求导后的矩阵。
7.根据权利要求6所述的串联机器人的迭代计算方法，其特征在于，
通过以下步骤计算末端关节质心加速度：
根据该末端关节的关节角速度及末端关节坐标系原点的雅克比矩阵得到末端关节坐标系原点在世界坐标系的速度；
根据所述末端关节坐标系原点在世界坐标系的速度得到该末端关节质心在世界坐标系的速度；
对所述末端关节质心在世界坐标系的速度对时间进行求导，得到该末端关节质心加速度。
8.根据权利要求4所述的串联机器人的迭代计算方法，其特征在于，每隔预设时间对各关节的关节属性信息进行初始化，并根据初始化的关节属性信息计算所述关节运动信息。
9.一种串联机器人，其特征在于，包括存储器以及处理器，所述存储器用于存储计算机程序，所述处理器运行所述计算机程序以使所述串联机器人执行权利要求1至8任一项所述的串联机器人的迭代计算方法。
10.一种计算机存储介质，其特征在于，其储存有权利要求9所述串联机器人中所用的所述计算机程序。
CN201811648964.1A 2018-12-30 2018-12-30 串联机器人的迭代计算方法及串联机器人 Active CN109591023B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201811648964.1A CN109591023B (zh) 2018-12-30 2018-12-30 串联机器人的迭代计算方法及串联机器人
US16/669,554 US11007639B2 (en) 2018-12-30 2019-10-31 Joint control method for serial robot and serial robot using the same

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811648964.1A CN109591023B (zh) 2018-12-30 2018-12-30 串联机器人的迭代计算方法及串联机器人

Publications (2)

Publication Number Publication Date
CN109591023A true CN109591023A (zh) 2019-04-09
CN109591023B CN109591023B (zh) 2020-09-08

Family

ID=65965637

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811648964.1A Active CN109591023B (zh) 2018-12-30 2018-12-30 串联机器人的迭代计算方法及串联机器人

Country Status (2)

Country Link
US (1) US11007639B2 (zh)
CN (1) CN109591023B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111558941A (zh) * 2020-07-14 2020-08-21 深圳市优必选科技股份有限公司 浮动基动力学前馈控制方法、装置和多足机器人
CN113263503A (zh) * 2021-07-19 2021-08-17 上海捷勃特机器人有限公司 一种机器人系统的控制单元、机器人系统及机器人系统的控制方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104750116A (zh) * 2014-12-31 2015-07-01 中国航天科技集团公司第五研究院第五一三研究所 一种双臂机器人协调操作的路径规划方法
CN205521390U (zh) * 2016-01-21 2016-08-31 南京航空航天大学 一种双臂机器人
CN106514636A (zh) * 2016-12-16 2017-03-22 宁波帝洲自动化科技有限公司 一种机器人末端位姿分析方法
CN107662209A (zh) * 2017-09-21 2018-02-06 上海新时达机器人有限公司 一种控制方法及机器人
CN107953324A (zh) * 2017-12-29 2018-04-24 华南理工大学 基于旋量理论和凯恩方法的蛇形机器人动力学分析方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5377310A (en) * 1992-04-03 1994-12-27 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration Controlling under-actuated robot arms using a high speed dynamics
JP3247832B2 (ja) * 1995-11-20 2002-01-21 富士通株式会社 キネマティクス演算装置
US7684896B2 (en) * 2001-06-29 2010-03-23 Honda Motor Co., Ltd. System and method of estimating joint loads using an approach of closed form dynamics
CN103901898B (zh) 2014-03-28 2016-08-24 哈尔滨工程大学 一种多自由度机器人的逆运动学通用求解方法
WO2018219952A1 (en) * 2017-05-29 2018-12-06 Franka Emika Gmbh Collision handling by a robot

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104750116A (zh) * 2014-12-31 2015-07-01 中国航天科技集团公司第五研究院第五一三研究所 一种双臂机器人协调操作的路径规划方法
CN205521390U (zh) * 2016-01-21 2016-08-31 南京航空航天大学 一种双臂机器人
CN106514636A (zh) * 2016-12-16 2017-03-22 宁波帝洲自动化科技有限公司 一种机器人末端位姿分析方法
CN107662209A (zh) * 2017-09-21 2018-02-06 上海新时达机器人有限公司 一种控制方法及机器人
CN107953324A (zh) * 2017-12-29 2018-04-24 华南理工大学 基于旋量理论和凯恩方法的蛇形机器人动力学分析方法

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111558941A (zh) * 2020-07-14 2020-08-21 深圳市优必选科技股份有限公司 浮动基动力学前馈控制方法、装置和多足机器人
CN111558941B (zh) * 2020-07-14 2020-09-29 深圳市优必选科技股份有限公司 浮动基动力学前馈控制方法、装置和多足机器人
CN113263503A (zh) * 2021-07-19 2021-08-17 上海捷勃特机器人有限公司 一种机器人系统的控制单元、机器人系统及机器人系统的控制方法
CN113263503B (zh) * 2021-07-19 2021-11-09 上海捷勃特机器人有限公司 一种机器人系统的控制单元、机器人系统及机器人系统的控制方法

Also Published As

Publication number Publication date
US11007639B2 (en) 2021-05-18
US20200206942A1 (en) 2020-07-02
CN109591023B (zh) 2020-09-08

Similar Documents

Publication Publication Date Title
Safeea et al. Kuka sunrise toolbox: Interfacing collaborative robots with matlab
Gualtieri et al. Learning 6-dof grasping and pick-place using attention focus
US8924021B2 (en) Control of robots from human motion descriptors
Popović et al. A strategy for grasping unknown objects based on co-planarity and colour information
Kazemi et al. Randomized kinodynamic planning for robust visual servoing
Adorno et al. Dq robotics: A library for robot modeling and control
US8467904B2 (en) Reconstruction, retargetting, tracking, and estimation of pose of articulated systems
US8140189B2 (en) Apparatus and method for computing operational-space physical quantity
US8560122B2 (en) Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same
Saut et al. Efficient models for grasp planning with a multi-fingered hand
Featherstone A beginner's guide to 6-d vectors (part 1)
US20070146371A1 (en) Reconstruction, Retargetting, Tracking, And Estimation Of Motion For Articulated Systems
Pokorny et al. Grasping objects with holes: A topological approach
CN104842371B (zh) 一种基于非最小化的最优化算法的机器人手眼标定方法
Bajracharya et al. A mobile manipulation system for one-shot teaching of complex tasks in homes
CN109591023A (zh) 串联机器人的迭代计算方法及串联机器人
Xia et al. Gibson env v2: Embodied simulation environments for interactive navigation
Mohammad et al. Tackling the correspondence problem: closed-form solution for gesture imitation by a humanoid’s upper body
Löw et al. Geometric algebra for optimal control with applications in manipulation tasks
Agarwal et al. Dexterous functional grasping
US11883962B2 (en) Object manipulation with collision avoidance using complementarity constraints
Wu et al. Depth estimation of objects with known geometric model for IBVS using an eye-in-hand camera
Escalera et al. Symbolic Geometric Modelling of Tree‐Structure Robotic Mechanisms Using Lie Groups and Graph Theory
De Coninck et al. Learning to Grasp Arbitrary Household Objects from a Single Demonstration
Luo et al. AE-Reorient: Active Exploration Based Reorientation for Robotic Pick-and-Place

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
CP01 Change in the name or title of a patent holder
CP01 Change in the name or title of a patent holder

Address after: Nanshan District Xueyuan Road in Shenzhen city of Guangdong province 518000 No. 1001 Nanshan Chi Park C1 building 16, 22 floor

Patentee after: Shenzhen Youbixuan Technology Co.,Ltd.

Address before: Nanshan District Xueyuan Road in Shenzhen city of Guangdong province 518000 No. 1001 Nanshan Chi Park C1 building 16, 22 floor

Patentee before: Shenzhen Youbixuan Technology Co.,Ltd.

TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20231206

Address after: Room 601, 6th Floor, Building 13, No. 3 Jinghai Fifth Road, Beijing Economic and Technological Development Zone (Tongzhou), Daxing District, Beijing, 100176

Patentee after: Beijing Youbixuan Intelligent Robot Co.,Ltd.

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.