CN115837669A - 五自由度机械臂笛卡尔空间直线轨迹规划方法及装置 - Google Patents
五自由度机械臂笛卡尔空间直线轨迹规划方法及装置 Download PDFInfo
- Publication number
- CN115837669A CN115837669A CN202211258436.1A CN202211258436A CN115837669A CN 115837669 A CN115837669 A CN 115837669A CN 202211258436 A CN202211258436 A CN 202211258436A CN 115837669 A CN115837669 A CN 115837669A
- Authority
- CN
- China
- Prior art keywords
- data
- mechanical arm
- degree
- speed planning
- freedom mechanical
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 45
- 238000012545 processing Methods 0.000 claims abstract description 55
- 238000004590 computer program Methods 0.000 claims abstract description 24
- 230000001133 acceleration Effects 0.000 claims description 51
- 230000036461 convulsion Effects 0.000 claims description 27
- 238000006073 displacement reaction Methods 0.000 claims description 18
- 239000011159 matrix material Substances 0.000 claims description 18
- 230000009466 transformation Effects 0.000 claims description 15
- 238000010606 normalization Methods 0.000 claims description 11
- 238000005070 sampling Methods 0.000 claims description 11
- 230000006870 function Effects 0.000 description 17
- 230000036544 posture Effects 0.000 description 9
- 238000010586 diagram Methods 0.000 description 7
- 238000004891 communication Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 4
- 229910000831 Steel Inorganic materials 0.000 description 3
- 230000008859 change Effects 0.000 description 3
- 239000010959 steel Substances 0.000 description 3
- 230000009467 reduction Effects 0.000 description 2
- 230000008439 repair process Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- OKTJSMMVPCPJKN-UHFFFAOYSA-N Carbon Chemical compound [C] OKTJSMMVPCPJKN-UHFFFAOYSA-N 0.000 description 1
- 230000006978 adaptation Effects 0.000 description 1
- 230000001413 cellular effect Effects 0.000 description 1
- 230000009194 climbing Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000004880 explosion Methods 0.000 description 1
- 229910021389 graphene Inorganic materials 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Numerical Control (AREA)
Abstract
本申请涉及一种五自由度机械臂笛卡尔空间直线轨迹规划方法、装置、计算机设备、存储介质和计算机程序产品。所述方法包括：获取五自由度机械臂的直线轨迹的初始点位姿数据和末端点位姿数据，以及获取五自由度机械臂采用直线轨迹运行的运动数据；基于初始点位姿数据、末端点位姿数据和运动数据，确定s型速度规划类型，并得到相应的速度规划数据；对速度规划数据进行处理，得到直线轨迹中各插值点位姿数据；基于五自由度机械臂的逆运动学模型，分别处理各插值点位姿数据，相应得到各插值点关节角度数据。采用本方法能够对五自由度机械臂进行高效准确地直线轨迹规划过程。
Description
技术领域
本申请涉及协作机器人技术领域，特别是涉及一种五自由度机械臂笛卡尔空间直线轨迹规划方法及装置。
背景技术
对于桥梁修复等作业，要求具有轻质、大负载、大作业半径、灵敏感知和控制能力的机械臂作为末端工具的载体。协作机器人具有轻质、大负载自重比和灵敏感知等特点，采取协作机械臂的类似特点，设计了一种用于桥梁钢箱梁表面修复作业的五自由度机械臂。
机械臂是操作灵活、精度高、适应性强的复杂系统，已经在工业装配、安全防爆、现场检测等领域得到广泛应用。机械臂运动学模型，包含正解和逆解，并且逆解有多组解。对于不同的任务，需要规划机械臂笛卡尔空间的轨迹，并转化到关节空间输入机械臂进行控制。那么，对于多解情况，如何做适合工作要求的选择、如何规划两点间直线轨迹并对机械臂末端的峰值速度(也称最大速度)、加速度、加加速度做一定的限制，在理论研究和实践上都是备受关注的。
然而，现有对机械臂两点间直线轨迹规划的方法存在规划效率低的问题。
发明内容
基于此，有必要针对上述技术问题，提供一种能够提高规划效率的五自由度机械臂笛卡尔空间直线轨迹规划方法、装置、计算机设备、计算机可读存储介质和计算机程序产品。
第一方面，本申请提供了一种五自由度机械臂笛卡尔空间直线轨迹规划方法。所述方法包括：
获取五自由度机械臂的直线轨迹的初始点位姿数据和末端点位姿数据，以及获取所述五自由度机械臂采用所述直线轨迹运行的运动数据；所述运动数据包括最大速度、最大加速度和加加速度；
基于所述初始点位姿数据、所述末端点位姿数据和所述运动数据，确定s型速度规划类型，并得到相应的速度规划数据；所述s型速度规划类型包括七段式速度规划、六段式速度规划、五段式速度规划和四段式速度规划；
对所述速度规划数据进行处理，得到所述直线轨迹中各插值点位姿数据；
基于所述五自由度机械臂的逆运动学模型，分别处理各所述插值点位姿数据，相应得到各插值点关节角度数据。
在其中一个实施例中，所述基于所述初始点位姿数据、所述末端点位姿数据和所述运动数据，确定s型速度规划类型的步骤包括：
采用齐次变换矩阵模型处理所述初始点位姿数据和所述末端点位姿数据，得到所述直线轨迹的距离数据；
根据所述距离数据、所述最大速度、所述最大加速度和所述加加速度，确定s型速度规划条件；
基于所述s型速度规划条件，选择相应的s型速度规划类型。
在其中一个实施例中，所述对所述速度规划数据进行处理，得到所述直线轨迹中各插值点位姿数据的步骤，包括：
获取所述直线轨迹的插值点采样周期；
基于所述插值点采样周期，根据位移数据和所述距离数据得到各插值点的归一化因子；其中，所述位移数据为基于所述速度规划数据得到；
分别采用各所述归一化因子对所述初始点位姿数据和所述末端点位姿数据进行处理，得到各所述插值点位姿数据。
在其中一个实施例中，还包括：
对所述速度规划数据进行积分处理，得到所述位移数据。
在其中一个实施例中，所述基于所述五自由度机械臂的逆运动学模型，分别处理各所述插值点位姿数据，相应得到各插值点关节角度数据的步骤之前包括：
获取所述五自由度机械臂的D-H模型；
采用齐次变换矩阵模型对所述D-H模型进行处理，得到所述五自由度机械臂的正运动学模型；
对所述正运动学模型进行逆解，得到所述逆运动学模型。
在其中一个实施例中，所述基于所述五自由度机械臂的逆运动学模型，分别处理各所述插值点位姿数据，相应得到各插值点关节角度数据的步骤包括：
获取所述五自由度机械臂中第五关节在所述直线轨迹下的初始点关节角度和末端点关节角度；
根据所述初始点关节角度和所述末端点关节角度，将各所述插值点位姿数据分别代入所述逆运动学模型中进行处理，并在相应的处理结果满足关节角限制条件的情况下，分别得到各插值点对应的所述五自由度机械臂中第二关节的关节角度、所述五自由度机械臂中第三关节的关节角度和所述五自由度机械臂中第四关节的关节角度。
第二方面，本申请还提供了一种五自由度机械臂笛卡尔空间直线轨迹规划装置。所述装置包括：
数据获取模块，用于获取五自由度机械臂的直线轨迹的初始点位姿数据和末端点位姿数据，以及获取所述五自由度机械臂采用所述直线轨迹运行的运动数据；所述运动数据包括最大速度、最大加速度和加加速度；
速度规划模块，用于基于所述初始点位姿数据、所述末端点位姿数据和所述运动数据，确定s型速度规划类型，并得到相应的速度规划数据；所述s型速度规划类型包括七段式速度规划、六段式速度规划、五段式速度规划和四段式速度规划；
位姿确定模块，用于对所述速度规划数据进行处理，得到所述直线轨迹中各插值点位姿数据；
关节角度确定模块，用于基于所述五自由度机械臂的逆运动学模型，分别处理各所述插值点位姿数据，相应得到各插值点关节角度数据。
第三方面，本申请还提供了一种计算机设备。所述计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现以下步骤：
获取五自由度机械臂的直线轨迹的初始点位姿数据和末端点位姿数据，以及获取所述五自由度机械臂采用所述直线轨迹运行的运动数据；所述运动数据包括最大速度、最大加速度和加加速度；
基于所述初始点位姿数据、所述末端点位姿数据和所述运动数据，确定s型速度规划类型，并得到相应的速度规划数据；所述s型速度规划类型包括七段式速度规划、六段式速度规划、五段式速度规划和四段式速度规划；
对所述速度规划数据进行处理，得到所述直线轨迹中各插值点位姿数据；
基于所述五自由度机械臂的逆运动学模型，分别处理各所述插值点位姿数据，相应得到各插值点关节角度数据。
第四方面，本申请还提供了一种计算机可读存储介质。所述计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：
获取五自由度机械臂的直线轨迹的初始点位姿数据和末端点位姿数据，以及获取所述五自由度机械臂采用所述直线轨迹运行的运动数据；所述运动数据包括最大速度、最大加速度和加加速度；
基于所述初始点位姿数据、所述末端点位姿数据和所述运动数据，确定s型速度规划类型，并得到相应的速度规划数据；所述s型速度规划类型包括七段式速度规划、六段式速度规划、五段式速度规划和四段式速度规划；
对所述速度规划数据进行处理，得到所述直线轨迹中各插值点位姿数据；
基于所述五自由度机械臂的逆运动学模型，分别处理各所述插值点位姿数据，相应得到各插值点关节角度数据。
第五方面，本申请还提供了一种计算机程序产品。所述计算机程序产品，包括计算机程序，该计算机程序被处理器执行时实现以下步骤：
获取五自由度机械臂的直线轨迹的初始点位姿数据和末端点位姿数据，以及获取所述五自由度机械臂采用所述直线轨迹运行的运动数据；所述运动数据包括最大速度、最大加速度和加加速度；
基于所述初始点位姿数据、所述末端点位姿数据和所述运动数据，确定s型速度规划类型，并得到相应的速度规划数据；所述s型速度规划类型包括七段式速度规划、六段式速度规划、五段式速度规划和四段式速度规划；
对所述速度规划数据进行处理，得到所述直线轨迹中各插值点位姿数据；
基于所述五自由度机械臂的逆运动学模型，分别处理各所述插值点位姿数据，相应得到各插值点关节角度数据。
上述五自由度机械臂笛卡尔空间直线轨迹规划方法、装置、计算机设备、存储介质和计算机程序产品，通过根据获取的初始点位姿数据、末端点位姿数据和运动数据而确定s型速度规划类型，以得到相应的速度规划数据，进而对速度规划数据进行处理得到直线轨迹中各插值点位姿数据，从而基于五自由度机械臂的逆运动学模型分别处理各插值点位姿数据，相应得到各插值点关节角度数据，本申请采用s型速度规划对五自由度机械臂运行中的峰值速度、最大加速度和加加速度进行限制，保证其各关节角度变化在一定范围内且符合限制，实现了对五自由度机械臂高效准确的直线轨迹规划过程。
附图说明
图1为五自由度机械臂的机构简图；
图2为五自由度机械臂处于折叠状态的第一示意图；
图3为五自由度机械臂处于折叠状态的第二示意图；
图4为一个实施例中五自由度机械臂笛卡尔空间直线轨迹规划方法的流程示意图；
图5为一个实施例中五自由度机械臂D-H模型的示意图；
图6为一个实施例中五自由度机械臂笛卡尔空间直线轨迹规划方法的具体流程示意。
具体实施方式
为了便于理解本申请，下面将参照相关附图对本申请进行更全面的描述。附图中给出了本申请的实施例。但是，本申请可以以许多不同的形式来实现，并不限于本文所描述的实施例。相反地，提供这些实施例的目的是使本申请的公开内容更加透彻全面。
除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本申请。
五自由度机械臂包括一个平移关节和四个转动关节，其中，所属平移关节为第一关节，所属转动关节为第二至第五关节，且第一关节轴线水平，第二关节轴线水平且在第一关节上方，第三至第五关节轴线与第二关节轴线平行。图1示出了五自由度机械臂的机构简图，其中xoy平面为该协作机器人基座所在的平面；如图2所示，五自由度机械臂可处于折叠状态，即第三关节、第四关节和第五关节旋转到特定角度后，可以将臂杆2之后的臂杆全部折叠在臂杆2的长度之内，即臂杆3与臂杆2完全平行且臂杆3在臂杆2的中心线上的投影与臂杆2重叠，臂杆4与臂杆2完全平行且臂杆4在臂杆2的中心线上的投影与臂杆2重叠，臂杆5与臂杆2完全平行且臂杆5在臂杆2的中心线上的投影与臂杆2重叠，此时机械臂处于最小体积状态，便于收纳和运输，以及便于在暂停作业时机械臂处于最小体积状态从而容易躲避障碍物，图3示出了五自由度机械臂处于折叠状态的第二示意图，为图2中处于折叠状态的五自由度机械臂的俯视示意图。
五自由度机械臂具有轻质、大负载、大作业半径、灵敏感知和控制能力，适合搭载在爬壁机器人等对负载大小比较敏感的移动载体上，对桥梁钢箱梁表面的漆面进行修复作业，同时在作业空间内具有冗余自由度，可以很好地通过调整冗余自由度实现对钢箱梁外表面复杂形状的适应和避碰。
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
在一个实施例中，以上述五自由度机械臂在笛卡尔空间两点之间的直线轨迹规划为例，如图4所示，提供了一种五自由度机械臂笛卡尔空间直线轨迹规划方法，本实施例以该方法应用于终端进行举例说明，本实施例中，该方法包括以下步骤：
步骤S410，获取五自由度机械臂的直线轨迹的初始点位姿数据和末端点位姿数据，以及获取五自由度机械臂采用直线轨迹运行的运动数据；运动数据包括最大速度、最大加速度和加加速度；
具体地，初始点位姿数据包括初始点的空间位置和姿态，末端点位姿数据包括末端点的空间位置和姿态；最大速度也是五自由度机械臂采用直线轨迹运行的峰值速度，加加速度又称变加速度、或急动度、或冲动度，是描述加速度变化快慢的物理量。
步骤S420，基于初始点位姿数据、末端点位姿数据和运动数据，确定s型速度规划类型，并得到相应的速度规划数据；s型速度规划类型包括七段式速度规划、六段式速度规划、五段式速度规划和四段式速度规划；
具体地，可以根据初始点位姿数据、末端点位姿数据和运动数据等约束条件将五自由度机械臂的直线轨迹的s型速度曲线分为四种类型，其中七段式速度规划包括加加速阶段、匀加速阶段、减加速阶段、匀速阶段、加减速阶段、匀减速阶段和减减速阶段；六段式速度规划包括加加速阶段、匀加速阶段、减加速阶段、加减速阶段、匀减速阶段和减减速阶段；五段式速度规划包括加加速阶段、减加速阶段、匀速阶段、加减速阶段和减减速阶段；四段式速度规划包括加加速阶段、减加速阶段、加减速阶段和减减速阶段。
在其中一个实施例中，基于初始点位姿数据、末端点位姿数据和运动数据，确定s型速度规划类型的步骤包括：
采用齐次变换矩阵模型处理初始点位姿数据和末端点位姿数据，得到直线轨迹的距离数据；
根据距离数据、最大速度、最大加速度和加加速度，确定s型速度规划条件；
基于s型速度规划条件，选择相应的s型速度规划类型。
具体地，将初始点位姿数据和末端点位姿数据均用齐次变换矩阵表示，在一些示例中，初始点位姿数据对应的齐次变换矩阵如下式(1)所示：
其中，矩阵中左上角三行三列的数据表示初始点的姿态，第四列中前三行的数据表示初始点的空间位置。
末端点位姿数据对应的齐次变换矩阵如下式(2)所示：
其中，同样地，矩阵中左上角三行三列的数据表示末端点的姿态，第四列中前三行的数据表示末端点的空间位置。
根据上述式(1)和式(2)则可得到直线轨迹的距离数据，以s表示直线轨迹之间的距离为例，则s可通过下式(3)计算得到：
以vmax表示最大速度、amax表示最大加速度、J表示加加速度为例，则经过加加速阶段、匀加速阶段和减加速阶段达到最大速度所走的距离s1以及经过加加速和减加速阶段达到最大速度所走的距离s2可通过下式(4)计算得到：
s型速度规划条件包括七段式速度规划条件、六段式速度规划条件、五段式速度规划条件和四段式速度规划条件，进一步地，根据上述距离数据、最大速度、最大加速度和加加速度，可以判断直线轨迹满足哪一种速度规划条件；
当s>2s1时，意味着五自由度机械臂在给定距离内采用直线轨迹运行时既可以达到最大速度，又可以达到最大加速度，因此可以确定直线轨迹满足七段式速度规划条件，相应地，s型速度规划类型选择七段式速度规划，并得到相应的速度规划数据，速度规划数据包括速度函数(也可称速度曲线)，下式(5)给出了七段式速度规划中各阶段的速度函数：
其中，从上之下的速度函数依次表示七段式速度规划中的加加速阶段、匀加速阶段、减加速阶段、匀速阶段、加减速阶段、匀减速阶段和减减速阶段，进一步地，对各阶段的速度函数求导，可得到各阶段相应地加速度函数(或加速度曲线)；式(5)中T1、T2、T3、T4、T5、T6、T7分别为各阶段所用时间，可以通过下式(6)计算得到：
当s>2s2且s<2s1时，意味着五自由度机械臂在给定距离内采用直线轨迹运行时可以达到最大加速度，但不可以达到最大速度，因此可以确定直线轨迹满足六段式速度规划条件，相应地，s型速度规划类型选择六段式速度规划，六段式速度规划中各阶段的速度函数除了没有匀速阶段，其余阶段的速度函数与七段式速度规划中对应的速度函数相同，其中，六段式速度规划中直线轨迹的距离、最大速度、最大加速度和加加速度之间的关系，以及各阶段所用时间如下式(7)所示：
当
且/>
时，意味着五自由度机械臂在给定距离内采用直线轨迹运行时可以达到最大速度，但无法达到最大加速度，因此可以确定直线轨迹满足五段式速度规划条件，相应地，s型速度规划类型选择五段式速度规划，五段式速度规划中各阶段的速度函数除了没有匀加速阶段和匀减速阶段，其余五个阶段的速度函数与七段式速度规划中对应的速度函数相同，其中，五段式速度规划中最大速度、最大加速度和加加速度之间的关系，以及各阶段所用时间如下式(8)所示：/>
当s<2s2时，意味着五自由度机械臂在给定距离内采用直线轨迹运行时既达不到最大加速度，也达不到最大速度，因此可以确定直线轨迹满足四段式速度规划条件，相应地，s型速度规划类型选择四段式速度规划，四段式速度规划中各阶段的速度函数除了没有匀加速阶段、匀速阶段和匀减速阶段，其余阶段的四个速度函数与七段式速度规划中对应的速度函数相同，其中，四段式速度规划中直线轨迹的距离、最大速度、最大加速度和加加速度之间的关系，以及各阶段所用时间如下式(9)所示：
步骤S430，对速度规划数据进行处理，得到直线轨迹中各插值点位姿数据；
具体地，在对五自由度机械臂的直线轨迹进行s型速度规划后，可以在直线轨迹中进行直线插值，并对速度规划数据进行处理，从而得到各插值点位姿数据。
在其中一个实施例中，对速度规划数据进行处理，得到直线轨迹中各插值点位姿数据的步骤，包括：
获取直线轨迹的插值点采样周期；
基于插值点采样周期，根据位移数据和距离数据得到各插值点的归一化因子；其中，位移数据为基于速度规划数据得到；
分别采用各归一化因子对初始点位姿数据和末端点位姿数据进行处理，得到各插值点位姿数据。
具体地，根据插值点采样周期可以得到插值点的个数，在一些示例中，以Ts表示插值点采样周期为例，则插值点的个数可以通过下式(10)计算得到：
进一步地，根据位移数据和距离数据得到各插值点的归一化因子；
在其中一个实施例中，还包括：
对速度规划数据进行积分处理，得到位移数据。
具体来说，以七段式速度规划中各阶段的速度函数为例，分别对式(5)所示的各阶段的速度函数求取积分，从而得到位移数据，即各阶段的位移函数，如下式(11)所示：
其余s型速度规划类型如六段式速度规划、五段式速度规划和四段式速度规划的位移函数也均是通过对相应的速度函数求取积分而得到。
式(11)中，d1、d2、d3、d4、d5、d6依次为加加速阶段、匀加速阶段、减加速阶段、匀速阶段、加减速阶段、匀减速阶段运行的距离，可以通过下式(12)计算得到：
进一步地，各插值点的归一化因子通过下式(13)计算得到：
其中，tk为各插值点的插值时刻。
在一个具体示例中，五自由度机械臂直线轨迹规划中要求末端姿态保持不变，因此各插值点的姿态也保持不变，只对位置进行规划，由此可以根据初始点的位置和末端点的位置得到各插值点的位置，具体可以通过下式(14)计算得到：
其中，(x1，y1，z1)为初始点的位置坐标，(x2，y2，z2)为末端点的位置坐标。
步骤S440，基于五自由度机械臂的逆运动学模型，分别处理各插值点位姿数据，相应得到各插值点关节角度数据。
在其中一个实施例中，基于五自由度机械臂的逆运动学模型，分别处理各插值点位姿数据，相应得到各插值点关节角度数据的步骤之前包括：
获取五自由度机械臂的D-H模型；
采用齐次变换矩阵模型对D-H模型进行处理，得到五自由度机械臂的正运动学模型；
对正运动学模型进行逆解，得到逆运动学模型。
具体地，如图5所示，建立连杆坐标系，将五自由度机械臂连杆依据D-H方法确立Z轴和X轴，其中基坐标系{0}、各关节坐标系{1，2，3，4，5}、末端坐标系{f}，根据五自由度机械臂的结构参数建立该机械臂的D-H(Denavit-Hartenberg)模型；
在一个具体示例中，D-H模型中参数如下表所示：
其中，αi-1表示连杆扭角、ai-1表示连杆长度(即臂杆长度)、di表示连杆偏距、θi表示关节角度，d1、θ2、θ3、θ4、θ5均为变量，图5中标出的a1、a2、a3、a4、af、df均为已知量，θ1为0。
根据上述D-H参数表，以及各插值点位姿数据，则从基坐标系到末端法兰盘坐标系的齐次变换矩阵表示为下式(15)：
其中，矩阵中左上角三行三列的数据表示插值点的姿态，第四列中前三行的数据表示插值点的空间位置。
式(15)中：
nx＝cos(θ2+θ3+θ4+θ5),ny＝sin(θ2+θ3+θ4+θ5),nz＝0,
ox＝-sin(θ2+θ3+θ4+θ5),oy＝cos(θ2+θ3+θ4+θ5),
oz＝0,
ax＝0,
ay＝0,
az＝1,
px＝a1+af+a2 cos(θ2)+a3 cos(θ2+θ3)+a4 cos(θ2+θ3+θ4)；
py＝a2 sin(θ2)+a3 sin(θ2+θ3)+a4 sin(θ2+θ3+θ4)；
Pz＝d1+df。
进一步地，根据上述D-H模型和式(15)，则五自由度机械臂的正运动学模型如下式(16)所示
对式(16)中正运动学模型进行逆解，则得到逆运动学模型，如下式(17)所示：
进一步地，根据各插值点位姿数据，对上述逆运动学模型进行求解，则相应得到各插值点关节角度数据。
在其中一个实施例中，基于五自由度机械臂的逆运动学模型，分别处理各插值点位姿数据，相应得到各插值点关节角度数据的步骤包括：
获取五自由度机械臂中第五关节在直线轨迹下的初始点关节角度和末端点关节角度；
根据初始点关节角度和末端点关节角度，将各插值点位姿数据分别代入逆运动学模型中进行处理，并在相应的处理结果满足关节角限制条件的情况下，分别得到各插值点对应的五自由度机械臂中第二关节的关节角度、五自由度机械臂中第三关节的关节角度和五自由度机械臂中第四关节的关节角度。
具体来说，由于式(17)的方程两边左右元素相等，则可以得到四个等式，分别如下式(18)、(19)、(20)、(21)所示：
d1＝Pz-azdf-nzaf＝Pz-df (18)
其中：
b1＝px-a1-af-a4 cos[arccos(nx)-θ5]；
b2＝py-a4 sin[arccos(nx)-θ5]；
获取五自由度机械臂中第五关节在直线轨迹下的初始点关节角度和末端点关节角度，则第五关节在直线轨迹下各插值点的关节角度可根据初始点关节角度、末端点关节角度和插值点个数计算得到，进一步地，将插值点的位置数据代入式(19)、(20)、(21)中，即可联立求解得到插值点在其位姿下对应的第二关节的关节角度、第三关节的关节角度和第四关节的关节角度，且每个关节角度均有四个解；
进一步地，根据关节角限制条件对所求的四组解进行判断和选择，首先判断关节角度是否超出关节角限制，第一关节为移动关节，限制为[0，385°]；第二至五关节为转动关节，其中第二关节限制为±90°，其他关节限制为±180°；其次判断相邻两个插值点的关节角度的差值，因受限于实际电机转动角度，相邻两个插值点每个关节角度相差不大于1°，最后可以得到一组满足要求的关节角度解。
在一个具体示例中，五自由度机械臂笛卡尔空间直线轨迹规划方法的具体流程示意图如图6所示，首先输入直线轨迹的初始点和末端点的齐次变换矩阵、第五关节分别在两个点位姿下的关节角度，以及五自由度机械臂采用直线轨迹运行的峰值速度、最大加速度和加加速度；进一步可以根据初始点和末端点的齐次变换矩阵中的两点坐标确定直线轨迹的距离s，以及s型速度规划类型选择依据s1和s2；进一步地，根据s型速度规划条件选择采用相应地s型速度规划类型；进一步地，对速度规划得到的速度曲线进行积分得到位移曲线，同时以采样周期Ts为间隔选取插值点，并根据位移计算各插值点的归一化因子；进一步地，可以计算得到各插值点的位置及其齐次变换矩阵，将其代入逆运动学模型中求解，得到四组第二关节的关节角度、第三关节的关节角度和第四关节的关节角度解，进一步根据关节角限制条件对所求的四组解进行判断和选择，得到一组合适的解。
上述五自由度机械臂笛卡尔空间直线轨迹规划方法，不需要进行迭代运算、简洁高效同时具备快速计算运动学逆解的能力。由于该方法采用s型速度规划，保证了机械臂的速度、加速度、加加速度符合要求，同时满足执行器规范，如关节角度变化量和关节角度限制，保证了机械臂运行中的安全，可实现高效准确的直线轨迹规划过程，可推广到其他类似构型的五自由度机械臂轨迹优化上。
应该理解的是，虽然如上所述的各实施例所涉及的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，如上所述的各实施例所涉及的流程图中的至少一部分步骤可以包括多个步骤或者多个阶段，这些步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤中的步骤或者阶段的至少一部分轮流或者交替地执行。
基于同样的发明构思，本申请实施例还提供了一种用于实现上述所涉及的五自由度机械臂笛卡尔空间直线轨迹规划方法的五自由度机械臂笛卡尔空间直线轨迹规划装置。该装置所提供的解决问题的实现方案与上述方法中所记载的实现方案相似，故下面所提供的一个五自由度机械臂笛卡尔空间直线轨迹规划装置实施例中的具体限定可以参见上文中对于五自由度机械臂笛卡尔空间直线轨迹规划方法的限定，在此不再赘述。
在一个实施例中，提供了一种五自由度机械臂笛卡尔空间直线轨迹规划装置，包括：数据获取模块、速度规划模块、位姿确定模块和关节角度确定模块，其中：
数据获取模块，用于获取五自由度机械臂的直线轨迹的初始点位姿数据和末端点位姿数据，以及获取五自由度机械臂采用直线轨迹运行的运动数据；运动数据包括最大速度、最大加速度和加加速度；
速度规划模块，用于基于初始点位姿数据、末端点位姿数据和运动数据，确定s型速度规划类型，并得到相应的速度规划数据；s型速度规划类型包括七段式速度规划、六段式速度规划、五段式速度规划和四段式速度规划；
位姿确定模块，用于对速度规划数据进行处理，得到直线轨迹中各插值点位姿数据；
关节角度确定模块，用于基于五自由度机械臂的逆运动学模型，分别处理各插值点位姿数据，相应得到各插值点关节角度数据。
在其中一个实施例中，速度规划模块还用于：
采用齐次变换矩阵模型处理初始点位姿数据和末端点位姿数据，得到直线轨迹的距离数据；
根据距离数据、最大速度、最大加速度和加加速度，确定s型速度规划条件；
基于s型速度规划条件，选择相应的s型速度规划类型。
在其中一个实施例中，位姿确定模块还用于：
获取直线轨迹的插值点采样周期；
基于插值点采样周期，根据位移数据和距离数据得到各插值点的归一化因子；其中，位移数据为基于速度规划数据得到；
分别采用各归一化因子对初始点位姿数据和末端点位姿数据进行处理，得到各插值点位姿数据。
在其中一个实施例中，位姿确定模块还用于：
对速度规划数据进行积分处理，得到位移数据。
在其中一个实施例中，关节角度确定模块还用于：
获取五自由度机械臂的D-H模型；
采用齐次变换矩阵模型对D-H模型进行处理，得到五自由度机械臂的正运动学模型；
对正运动学模型进行逆解，得到逆运动学模型。
在其中一个实施例中，关节角度确定模块还用于：
获取五自由度机械臂中第五关节在直线轨迹下的初始点关节角度和末端点关节角度；
根据初始点关节角度和末端点关节角度，将各插值点位姿数据分别代入逆运动学模型中进行处理，并在相应的处理结果满足关节角限制条件的情况下，分别得到各插值点对应的五自由度机械臂中第二关节的关节角度、五自由度机械臂中第三关节的关节角度和五自由度机械臂中第四关节的关节角度。
上述五自由度机械臂笛卡尔空间直线轨迹规划装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中，提供了一种计算机设备，该计算机设备可以是终端。该计算机设备包括通过系统总线连接的处理器、存储器、通信接口、显示屏和输入装置。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的通信接口用于与外部的终端进行有线或无线方式的通信，无线方式可通过WIFI、移动蜂窝网络、NFC(近场通信)或其他技术实现。该计算机程序被处理器执行时以实现一种五自由度机械臂笛卡尔空间直线轨迹规划方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。
在一个实施例中，还提供了一种计算机设备，包括存储器和处理器，存储器中存储有计算机程序，该处理器执行计算机程序时实现上述各方法实施例中的步骤。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现上述各方法实施例中的步骤。
在一个实施例中，提供了一种计算机程序产品，包括计算机程序，该计算机程序被处理器执行时实现上述各方法实施例中的步骤。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、数据库或其它介质的任何引用，均可包括非易失性和易失性存储器中的至少一种。非易失性存储器可包括只读存储器(Read-OnlyMemory，ROM)、磁带、软盘、闪存、光存储器、高密度嵌入式非易失性存储器、阻变存储器(ReRAM)、磁变存储器(Magnetoresistive Random Access Memory，MRAM)、铁电存储器(Ferroelectric Random Access Memory，FRAM)、相变存储器(Phase Change Memory，PCM)、石墨烯存储器等。易失性存储器可包括随机存取存储器(Random Access Memory，RAM)或外部高速缓冲存储器等。作为说明而非局限，RAM可以是多种形式，比如静态随机存取存储器(Static Random Access Memory，SRAM)或动态随机存取存储器(Dynamic RandomAccess Memory，DRAM)等。本申请所提供的各实施例中所涉及的数据库可包括关系型数据库和非关系型数据库中至少一种。非关系型数据库可包括基于区块链的分布式数据库等，不限于此。本申请所提供的各实施例中所涉及的处理器可为通用处理器、中央处理器、图形处理器、数字信号处理器、可编程逻辑器、基于量子计算的数据处理逻辑器等，不限于此。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本申请专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请的保护范围应以所附权利要求为准。
Claims (10)
1.一种五自由度机械臂笛卡尔空间直线轨迹规划方法，其特征在于，所述方法包括：
获取五自由度机械臂的直线轨迹的初始点位姿数据和末端点位姿数据，以及获取所述五自由度机械臂采用所述直线轨迹运行的运动数据；所述运动数据包括最大速度、最大加速度和加加速度；
基于所述初始点位姿数据、所述末端点位姿数据和所述运动数据，确定s型速度规划类型，并得到相应的速度规划数据；所述s型速度规划类型包括七段式速度规划、六段式速度规划、五段式速度规划和四段式速度规划；
对所述速度规划数据进行处理，得到所述直线轨迹中各插值点位姿数据；
基于所述五自由度机械臂的逆运动学模型，分别处理各所述插值点位姿数据，相应得到各插值点关节角度数据。
2.根据权利要求1所述的方法，其特征在于，所述基于所述初始点位姿数据、所述末端点位姿数据和所述运动数据，确定s型速度规划类型的步骤包括：
采用齐次变换矩阵模型处理所述初始点位姿数据和所述末端点位姿数据，得到所述直线轨迹的距离数据；
根据所述距离数据、所述最大速度、所述最大加速度和所述加加速度，确定s型速度规划条件；
基于所述s型速度规划条件，选择相应的s型速度规划类型。
3.根据权利要求2所述的方法，其特征在于，所述对所述速度规划数据进行处理，得到所述直线轨迹中各插值点位姿数据的步骤，包括：
获取所述直线轨迹的插值点采样周期；
基于所述插值点采样周期，根据位移数据和所述距离数据得到各插值点的归一化因子；其中，所述位移数据为基于所述速度规划数据得到；
分别采用各所述归一化因子对所述初始点位姿数据和所述末端点位姿数据进行处理，得到各所述插值点位姿数据。
4.根据权利要求3所述的方法，其特征在于，还包括：
对所述速度规划数据进行积分处理，得到所述位移数据。
5.根据权利要求1至4任意一项所述的方法，其特征在于，所述基于所述五自由度机械臂的逆运动学模型，分别处理各所述插值点位姿数据，相应得到各插值点关节角度数据的步骤之前包括：
获取所述五自由度机械臂的D-H模型；
采用齐次变换矩阵模型对所述D-H模型进行处理，得到所述五自由度机械臂的正运动学模型；
对所述正运动学模型进行逆解，得到所述逆运动学模型。
6.根据权利要求5所述的方法，其特征在于，所述基于所述五自由度机械臂的逆运动学模型，分别处理各所述插值点位姿数据，相应得到各插值点关节角度数据的步骤包括：
获取所述五自由度机械臂中第五关节在所述直线轨迹下的初始点关节角度和末端点关节角度；
根据所述初始点关节角度和所述末端点关节角度，将各所述插值点位姿数据分别代入所述逆运动学模型中进行处理，并在相应的处理结果满足关节角限制条件的情况下，分别得到各插值点对应的所述五自由度机械臂中第二关节的关节角度、所述五自由度机械臂中第三关节的关节角度和所述五自由度机械臂中第四关节的关节角度。
7.一种五自由度机械臂笛卡尔空间直线轨迹规划装置，其特征在于，所述装置包括：
数据获取模块，用于获取五自由度机械臂的直线轨迹的初始点位姿数据和末端点位姿数据，以及获取所述五自由度机械臂采用所述直线轨迹运行的运动数据；所述运动数据包括最大速度、最大加速度和加加速度；
速度规划模块，用于基于所述初始点位姿数据、所述末端点位姿数据和所述运动数据，确定s型速度规划类型，并得到相应的速度规划数据；所述s型速度规划类型包括七段式速度规划、六段式速度规划、五段式速度规划和四段式速度规划；
位姿确定模块，用于对所述速度规划数据进行处理，得到所述直线轨迹中各插值点位姿数据；
关节角度确定模块，用于基于所述五自由度机械臂的逆运动学模型，分别处理各所述插值点位姿数据，相应得到各插值点关节角度数据。
8.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至6中任一项所述的方法的步骤。
9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至6中任一项所述的方法的步骤。
10.一种计算机程序产品，包括计算机程序，其特征在于，该计算机程序被处理器执行时实现权利要求1至6中任一项所述的方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211258436.1A CN115837669A (zh) | 2022-10-14 | 2022-10-14 | 五自由度机械臂笛卡尔空间直线轨迹规划方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211258436.1A CN115837669A (zh) | 2022-10-14 | 2022-10-14 | 五自由度机械臂笛卡尔空间直线轨迹规划方法及装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115837669A true CN115837669A (zh) | 2023-03-24 |
Family
ID=85575821
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211258436.1A Pending CN115837669A (zh) | 2022-10-14 | 2022-10-14 | 五自由度机械臂笛卡尔空间直线轨迹规划方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115837669A (zh) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108189038A (zh) * | 2018-01-18 | 2018-06-22 | 广东工业大学 | 一种实用的工业六轴机械臂直线运动轨迹规划方法及系统 |
| US20190160674A1 (en) * | 2017-08-10 | 2019-05-30 | Nanjing Estun Robotics Co., Ltd | Robot joint space point-to-point movement trajectory planning method |
| CN111897216A (zh) * | 2020-07-16 | 2020-11-06 | 华中科技大学 | 一种多运动段速度规划和插补方法 |
| CN111890349A (zh) * | 2020-05-25 | 2020-11-06 | 北京空间飞行器总体设计部 | 一种四自由度机械臂运动规划方法 |
| US20210339390A1 (en) * | 2020-05-04 | 2021-11-04 | X Development Llc | Trajectory planning for path-based applications |
| CN114227655A (zh) * | 2021-12-30 | 2022-03-25 | 深圳市英威腾电气股份有限公司 | 确定规划路径的方法、装置、scara机器人及介质 |
| CN114603558A (zh) * | 2022-03-21 | 2022-06-10 | 合肥哈工图南智控机器人有限公司 | 混合空间过渡轨迹规划方法及装置 |
-
2022
- 2022-10-14 CN CN202211258436.1A patent/CN115837669A/zh active Pending
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20190160674A1 (en) * | 2017-08-10 | 2019-05-30 | Nanjing Estun Robotics Co., Ltd | Robot joint space point-to-point movement trajectory planning method |
| CN108189038A (zh) * | 2018-01-18 | 2018-06-22 | 广东工业大学 | 一种实用的工业六轴机械臂直线运动轨迹规划方法及系统 |
| US20210339390A1 (en) * | 2020-05-04 | 2021-11-04 | X Development Llc | Trajectory planning for path-based applications |
| CN111890349A (zh) * | 2020-05-25 | 2020-11-06 | 北京空间飞行器总体设计部 | 一种四自由度机械臂运动规划方法 |
| CN111897216A (zh) * | 2020-07-16 | 2020-11-06 | 华中科技大学 | 一种多运动段速度规划和插补方法 |
| CN114227655A (zh) * | 2021-12-30 | 2022-03-25 | 深圳市英威腾电气股份有限公司 | 确定规划路径的方法、装置、scara机器人及介质 |
| CN114603558A (zh) * | 2022-03-21 | 2022-06-10 | 合肥哈工图南智控机器人有限公司 | 混合空间过渡轨迹规划方法及装置 |
Non-Patent Citations (1)
| Title |
|---|
| 张志松;: "基于双S形速度曲线的混联码垛机器人轨迹规划", 机电工程, no. 03, pages 166 * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Borrel et al. | A study of multiple manipulator inverse kinematic solutions with applications to trajectory planning and workspace determination | |
| US8560122B2 (en) | Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same | |
| Gong et al. | Analytical inverse kinematics and self-motion application for 7-DOF redundant manipulator | |
| Ceccarelli et al. | Fundamentals of the mechanics of robots | |
| CN113146600B (zh) | 基于运动学迭代学习控制的柔性机器人轨迹规划方法及装置 | |
| CN109866222B (zh) | 一种基于天牛须优化策略的机械臂运动规划方法 | |
| CN110561419B (zh) | 臂型线约束柔性机器人轨迹规划方法及装置 | |
| Scheurer et al. | Path planning method for palletizing tasks using workspace cell decomposition | |
| US20230117928A1 (en) | Nonlinear trajectory optimization for robotic devices | |
| Staicu | Dynamics of Parallel Robots | |
| Wen et al. | Singularities in three-legged platform-type parallel mechanisms | |
| CN116038702B (zh) | 一种七轴机器人逆解方法及七轴机器人 | |
| Zeghloul et al. | SMAR: A robot modeling and simulation system | |
| Abbasnejad et al. | Architecture optimization of 4PUS+ 1PS parallel manipulator | |
| Xi | Obstacle avoidance trajectory planning of redundant robots based on improved Bi-RRT | |
| CN109366486A (zh) | 柔性机器人逆运动学求解方法、系统、设备、存储介质 | |
| CN115837669A (zh) | 五自由度机械臂笛卡尔空间直线轨迹规划方法及装置 | |
| Chaparro-Altamirano et al. | Kinematic and workspace analysis of a parallel robot used in security applications | |
| Liu et al. | A TE-E optimal planning technique based on screw theory for robot trajectory in workspace | |
| Pajak et al. | Planning of a point to point collision-free trajectory for mobile manipulators | |
| CN114888812A (zh) | 一种基于刚度性能优化的移动机械臂站位规划方法 | |
| Lim et al. | Kinematic analysis and design optimization of a cable-driven universal joint module | |
| Yerlikaya et al. | Obtaining high-dimensional configuration space for robotic systems operating in a common environment | |
| Qin et al. | Where to install the manipulator: optimal installation pose planning based on whale algorithm | |
| Gosselin et al. | SIMPA: A graphic simulator for the CAD of parallel manipulators |
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 | ||
| TA01 | Transfer of patent application right | ||
| TA01 | Transfer of patent application right |
Effective date of registration: 20230515 Address after: No. 8 Huanshan Village, Wenquan Town, Haidian District, Beijing, 100089 Applicant after: AECC BEIJING INSTITUTE OF AERONAUTICAL MATERIALS Applicant after: HONG KONG-ZHUHAI-MACAO BRIDGE AUTHORITY Applicant after: SHENYANG INSTITUTE OF AUTOMATION, CHINESE ACADEMY OF SCIENCES Address before: 519000 No. 368, Henglong Road, Nanping Town, Xiangzhou District, Zhuhai City, Guangdong Province Applicant before: HONG KONG-ZHUHAI-MACAO BRIDGE AUTHORITY Applicant before: AECC BEIJING INSTITUTE OF AERONAUTICAL MATERIALS |