CN113263496A - 一种六自由度机械臂优化路径的方法和计算机设备 - Google Patents

一种六自由度机械臂优化路径的方法和计算机设备 Download PDF

Info

Publication number
CN113263496A
CN113263496A CN202110353992.6A CN202110353992A CN113263496A CN 113263496 A CN113263496 A CN 113263496A CN 202110353992 A CN202110353992 A CN 202110353992A CN 113263496 A CN113263496 A CN 113263496A
Authority
CN
China
Prior art keywords
path
mechanical arm
singular
calculating
vicinity
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
CN202110353992.6A
Other languages
English (en)
Other versions
CN113263496B (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 Institute of Radio Measurement
Original Assignee
Beijing Institute of Radio Measurement
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 Beijing Institute of Radio Measurement filed Critical Beijing Institute of Radio Measurement
Priority to CN202110353992.6A priority Critical patent/CN113263496B/zh
Publication of CN113263496A publication Critical patent/CN113263496A/zh
Application granted granted Critical
Publication of CN113263496B publication Critical patent/CN113263496B/zh
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
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1661—Programme controls characterised by programming, planning systems for manipulators characterised by task planning, object-oriented languages
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/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

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Automation & Control Theory (AREA)
  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了一种六自由度机械臂优化路径的方法和计算机设备，所述方法包括：判断机械臂状态，若处于静止态，则使用判别方法识别空间路径是否经过机械臂奇异位形附近；若处于运动态，则使用跟踪算法规划轨迹使机械臂平稳跟踪经过奇异位形附近的空间路径；本文提出的判别算法运行时间小于1秒，可以在机械臂执行实际动作之前判断是否经过奇异点，不影响控制指令的响应速度。本发明所述的技术方案可以避免角速度突变，路径跟踪的动态误差小于1毫米。

Description

一种六自由度机械臂优化路径的方法和计算机设备
技术领域
本发明涉及工业机器人领域。具体的，涉及一种六自由度机械臂优化路径的方法和计算机设备。
背景技术
六自由度串联机械臂是现代工业中被广泛应用的机械臂。六自由度机械臂存在奇异位形，是机器人工作时的严重隐患。在奇异点处，为了使末端执行器获得在笛卡尔空间某些方向上的速度，根据机器人逆运动学求解出的关节角速度趋于无穷大，这表示末端执行器在至少一个方向上失去了运动能力。
机器人的奇异位形可以分为两类：1、由于结构限制，各关节角只能在一定的工作范围内运动，机器人在工作空间边界处于奇异位形；2、工作空间内部的奇异位形，通常是由两个或多个关节轴线重合造成的。边界奇异很容易被识别出来，工作空间内部的奇异位形是重点研究对象。当机械臂按照关节角运动时，不存在内部奇异的问题，内部奇异点出现在机械臂按照笛卡尔空间路径运动的情况下。
典型的内部奇异位形包括：1、4轴和6轴的轴线共线，5轴和4轴、6轴的轴线相交；2、6轴位于1轴的轴线上；3、2轴、3轴的轴线和4轴、5轴、6轴的轴线交点在同一条直线上。
在实际场景中，机械臂不仅在奇异点处完全丧失了一个或多个自由度，在奇异点附近，其运动学特性也会变差，因此应尽量避免机械臂出现在奇异点附近。计算出六个关节处于奇异位姿时的转角可以为示教和轨迹规划提供可靠的依据，这对设计机械臂和使用机械臂都有重要的意义。
发明内容
为解决上述问题至少之一，本发明的一个目的在于提供一种六自由度机械臂优化路径的方法，包括：
判断机械臂状态，若处于静止态，则使用判别方法识别空间路径是否经过机械臂奇异位形附近；若处于运动态，则使用跟踪算法规划轨迹使机械臂平稳跟踪经过奇异位形附近的空间路径；
所述判别算法包括：根据路径的起点空间坐标、终点空间坐标、相邻两点的时间间隔和运动时长计算路径函数；计算当前位置的条件数和条件数对时间的导数判断所述机械臂输出路径是否经过奇异位形附近；
所述跟踪算法包括：若所述机械臂处于运动态，根据其运动路径得到起点空间坐标p0、终点空间坐标pn和运动时长tf；计算当前位置的条件数和其条件数对时间的导数，判断所述运动态机械臂输出路径是否经过奇异位附近并根据判断结果进行路径优化。
在一种可能的实现方式中，所述根据路径的起点空间坐标、终点空间坐标、相邻两点的时间间隔和运动时长计算路径函数具体包括：
设定起点和终点处的空间速度和加速度为0，根据已知路径得到起点空间坐标p0、终点空间坐标pn、相邻两个点的时间间隔Δt和运动时长tf并利用多项式插值得到路径函数：
Figure BDA0003002994150000021
其中，pi表示路径上的第i个点，i为自然数且小于等于n，所述多项式的系数a0＝p0，a1＝a2＝0，a3＝10(pn-p0)/tf 3，a4＝15(pn-p0)/tf 4，a5＝6(pn-p0)/tf 5。
在一种可能的实现方式中，所述计算位置点Pi的条件数和其条件数对时间的导数，其中Pi表示路径上第i个点，0<i<(n+1)，i为自然数；判断所述机械臂输出路径是否经过奇异位形附近具体包括：
S0011：计算机械臂位于pi时对应的关节角度qi的雅可比矩阵J(qi)和所述雅可比矩阵J(qi)的条件数κi及其对时间的导数
Figure BDA0003002994150000022
κi＝σmax(J(qi))/σmin(J(qi)) (2)
其中，J(qi)表示位于pi时对应的关节角度qi的雅可比矩阵，获取机械臂的参数和关节角度求得，σmax(J(qi))和σmin(J(qi))分别表示J(qi)的最大奇异值和最小奇异值，可以通过对J(qi)进行奇异值分解得到；
S0012：若κi＞ε或者
Figure BDA0003002994150000023
则所述机械臂的输出路径经过奇异位形附近，算法结束；若κi≤ε并且
Figure BDA0003002994150000024
则所述机械臂的输出路径不经过奇异位形附近，令奇异标志s＝0；
S0013：根据公式(3)计算下一位置的关节角度：
qi+1＝qi+J-1(qi)(pi+1-pi) (3)
判断i+1是否等于n：若i+1等于n，则算法结束；若i+1小于n，则跳转至步骤S0011进行下一位置的判断。
所述计算当前位置的条件数和条件数对时间的导数包括：
S0020：根据运动路径得到起点空间坐标p0、终点空间坐标pn和运动时长tf；
S0030：从起始点开始，根据位置点Pi的关节角度，计算当前位形的雅可比矩阵J(qi)，其中Pi表示路径上第i个点，0<i<(n+1)；并根据公式(2)计算条件数κi及其对时间的导数
Figure BDA0003002994150000031
其中，插值间隔Δt＝0.005秒。
在一种可能的实现方式中，所述判断所述运动态机械臂输出路径是否经过奇异位附近并根据判断结果进行路径优化包括：
S0041：若κi＞ε或者
Figure BDA0003002994150000032
则所述机械臂的输出路径经过奇异位形附近，令奇异标志s＝1，计算阻尼系数λ：
令λ0＝λ0’＝10-5，设定阈值ε＝106、ε’＝104，根据公式(4)计算阻尼系数λ：
Figure BDA0003002994150000033
其中，参数ε、ε’、λ0和λ0’为设置好的阈值；
根据公式(5)计算空间速度
Figure BDA0003002994150000034
Figure BDA0003002994150000035
vx、vy和vz分别表示机械臂末端沿基坐标系x轴、y轴和z轴平移的速度，ωx、ωy和ωz分别表示所述机械臂末端绕x轴、y轴和z轴旋转的角速度：
Figure BDA0003002994150000036
其中a1＝a2＝0，a3＝10(pn-p0)/tf 3，a4＝15(pn-p0)/tf 4，a5＝6(pn-p0)/tf 5
根据公式(6)计算六轴角速度
Figure BDA0003002994150000037
Figure BDA0003002994150000038
Figure BDA0003002994150000039
其中ω1 ω2 ω3 ω4 ω5 ω6分别表示6个关节的旋转角速度，λ为阻尼系数，I为单位矩阵；跳转至步骤S0060。
所述判断所述运动态机械臂输出路径是否经过奇异位附近并根据判断结果进行路径优化还包括：
S0042：若κi≤ε并且
Figure BDA00030029941500000310
判断奇异标志s的值，若所述奇异标志s的值为1，则以机械臂当前实际位置
Figure BDA00030029941500000311
为起点，重新规划路径：
Figure BDA00030029941500000312
所述多项式的系数
Figure BDA0003002994150000041
Figure BDA0003002994150000042
计算空间速度
Figure BDA0003002994150000043
Figure BDA0003002994150000044
根据公式(9)计算六轴角速度
Figure BDA0003002994150000045
Figure BDA0003002994150000046
将奇异标志s的值改为0并跳转至步骤S0060。
S0043：若κi≤ε并且
Figure BDA0003002994150000047
判断奇异标志s的值，若所述奇异标志s的值为0，则根据公式(9)计算六轴角速度
Figure BDA0003002994150000048
跳转至步骤S0060。
所述步骤S0060包括：
根据公式(10)计算下一个位置处的关节角度：
Figure BDA0003002994150000049
设qi+1为所述机械臂的目标关节角度使其移动至qi+1，若所述下一位置关节角度qi+1为终点，算法结束，否则回到跟踪算法的步骤S0030继续执行。
本申请的第二方面提供一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现本申请第一方面提供的方法。
本申请的第三方面提供一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现本申请第一方面提供的方法。
本发明的有益效果如下：
本发明的技术方案根据雅克比矩阵的条件数自动判断空间路径是否经过机械臂的奇异位形附近，并且可以平稳地跟踪奇异路径。本文提出的判别算法运行时间小于1秒，可以在机械臂执行实际动作之前判断是否经过奇异点，不影响控制指令的响应速度。本发明所述的技术方案可以避免角速度突变，路径跟踪的动态误差小于1毫米，精准度较高。
附图说明
图1示出了本发明的一个实施例提供的一种六自由度机械臂跟踪奇异路径的方法的步骤图；
图2示出了本发明的一个实施例提供的一种六自由度机械臂跟踪奇异路径的方法的判别算法的步骤图；
图3示出了本发明的一个实施例提供的一种六自由度机械臂跟踪奇异路径的方法的跟踪算法的步骤图；
图4示出计算机***的结构示意图。
具体实施方式
为了更清楚地说明本发明，下面结合优选实施例和附图对本发明做进一步的说明。附图中相似的部件以相同的附图标记进行表示。本领域技术人员应当理解，下面所具体描述的内容是说明性的而非限制性的，不应以此限制本发明的保护范围。
本发明的一个实施例提出了一种六自由度机械臂优化路径的方法，如图1所示，
在本实施例中，在进行六自由度机械臂的路径规划时，通过判别算法，在机械臂运动前识别空间路径是否经过机械臂奇异位形附近，若经过奇异位形附近，其运动学特性会变差；
通过判别算法，在机械臂运动前识别空间路径是否经过机械臂奇异位形附近包括：根据路径的起点空间坐标、终点空间坐标、相邻两点的时间间隔和运动时长计算路径函数；计算当前位置(位置点Pi)的条件数和条件数对时间的导数判断所述机械臂输出路径是否经过奇异位形附近；
如图2所示，具体包括：
设定起点和终点处的空间速度和加速度为0，根据已知路径得到起点空间坐标p0、终点空间坐标pn、相邻两个点的时间间隔Δt和运动时长tf，计算五次多项式系数：a0＝p0，a1＝a2＝0，a3＝10(pn-p0)/tf 3，a4＝15(pn-p0)/tf 4，a5＝6(pn-p0)/tf 5；
利用五次多项式插值得到路径函数：
Figure BDA0003002994150000051
其中，pi表示路径上的第i个点，i为自然数且小于等于n。
计算位置点Pi(当前位置)的条件数和其条件数对时间的导数，其中Pi表示路径上第i个点，0<i<(n+1)；判断所述机械臂输出路径是否经过奇异位形附近具体包括：
S0011：计算机械臂位于pi时对应的关节角度qi的雅可比矩阵J(qi)和所述雅可比矩阵J(qi)的条件数κi及其对时间的导数
Figure BDA0003002994150000061
κi＝σmax(J(qi))/σmin(J(qi)) (2)
其中，J(qi)表示位于pi时对应的关节角度qi的雅可比矩阵，获取机械臂的参数和关节角度求得，σmax(J(qi))和σmin(J(qi))分别表示J(qi)的最大奇异值和最小奇异值，可以通过对J(qi)进行奇异值分解得到；设定阈值ε＝106、ε’＝104；
S0012：若κi＞ε或者
Figure BDA0003002994150000062
则所述机械臂的输出路径经过奇异位形附近，算法结束；若κi≤ε并且
Figure BDA0003002994150000063
则所述机械臂的输出路径不经过奇异位形附近，令奇异标志s＝0；
S0013：根据公式(3)计算下一位置(位置点P(i+1))的关节角度：
qi+1＝qi+J-1(qi)(pi+1-pi) (3)
判断i+1是否等于n：若i+1等于n，则算法结束；若i+1小于n，则跳转至步骤S0011进行下一位置的判断。
在本实施例中，在进行六自由度机械臂的路径规划时，通过跟踪算法，在机械臂运动时规划轨迹使机械臂平稳跟踪经过奇异位形附近的空间路径；
所述跟踪算法包括：若所述机械臂处于运动态，根据其运动路径得到起点空间坐标p0、终点空间坐标pn和运动时长tf；计算当前位置的条件数和条件数对时间的导数，判断所述运动态机械臂输出路径是否经过奇异位附近并根据判断结果进行路径优化；
如图3所示，具体包括：
S0020：插值间隔Δt＝0.005秒，初始化奇异标志S＝0；根据运动路径得到起点空间坐标p0、终点空间坐标pn和运动时长tf；
所述计算当前位置的条件数和条件数对时间的导数包括：
S0030：从起始点开始，根据位置点Pi的关节角度，计算当前位形的雅可比矩阵J(qi)，其中Pi表示路径上第i个点，0<i<(n+1)；并根据公式(2)计算条件数κi及其对时间的导数
Figure BDA0003002994150000064
所述判断所述运动态机械臂输出路径是否经过奇异位附近并根据判断结果进行路径优化包括：
S0041：若κi＞ε或者
Figure BDA0003002994150000071
则所述机械臂的输出路径经过奇异位形附近，令奇异标志s＝1，计算阻尼系数λ：
令λ0＝λ0’＝10-5，根据公式(4)计算阻尼系数λ：
Figure BDA0003002994150000072
其中，参数ε、ε’、λ0和λ0’为设置好的阈值；
根据公式(5)计算空间速度
Figure BDA0003002994150000073
Figure BDA0003002994150000074
vx、vy和vz分别表示机械臂末端沿基坐标系x轴、y轴和z轴平移的速度，ωx、ωy和ωz分别表示所述机械臂末端绕x轴、y轴和z轴旋转的角速度：
Figure BDA0003002994150000075
其中a1＝a2＝0，a3＝10(pn-p0)/tf 3，a4＝15(pn-p0)/tf 4，a5＝6(pn-p0)/tf 5
根据公式(6)计算六轴角速度
Figure BDA0003002994150000076
Figure BDA0003002994150000077
Figure BDA0003002994150000078
其中ω1 ω2 ω3 ω4 ω5 ω6分别表示6个关节的旋转角速度，λ为阻尼系数，I为单位矩阵；跳转至步骤S0060。
所述判断所述运动态机械臂输出路径是否经过奇异位附近并根据判断结果进行路径优化还包括：
S0042：若κi≤ε并且
Figure BDA0003002994150000079
判断奇异标志s的值，若所述奇异标志s的值为1，则以机械臂当前实际位置
Figure BDA00030029941500000710
为起点，重新规划路径：
Figure BDA00030029941500000711
所述多项式的系数
Figure BDA00030029941500000712
Figure BDA00030029941500000713
计算空间速度
Figure BDA00030029941500000714
Figure BDA0003002994150000081
根据公式(9)计算六轴角速度
Figure BDA0003002994150000082
Figure BDA0003002994150000083
将奇异标志s的值改为0并跳转至步骤S0060。
还包括：
S0043：若κi≤ε并且
Figure BDA0003002994150000084
判断奇异标志s的值，若所述奇异标志s的值为0，则根据公式(9)计算六轴角速度
Figure BDA0003002994150000085
跳转至步骤S0060。
所述步骤S0060包括：
根据公式(10)计算下一个位置处的关节角度：
Figure BDA0003002994150000086
设qi+1为所述机械臂的目标关节角度使其移动至qi+1，若所述下一位置关节角度qi+1为终点，算法结束，否则回到跟踪算法的步骤S0030继续执行。
本发明的第二个实施例提供了一种计算机可读存储介质，其上存储有计算机指令，该程序被处理器执行时实现上述实施例以所提供的方法。在实际应用中，所述计算机可读存储介质可以采用一个或多个计算机可读的介质的任意组合。计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质。
计算机可读存储介质例如可以是但不限于电、磁、光、电磁、红外线、或半导体的***、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本实施例中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行***、装置或者器件使用或者与其结合使用。
计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行***、装置或者器件使用或者与其结合使用的程序。
本发明的第三个实施例提供了一种适于用来实现上述实施例提供的滚动排产方法的计算机***，如图4所示，包括中央处理模块(CPU)，其可以根据存储在只读存储器(ROM)中的程序或者从存储部分加载到随机访问存储器(RAM)中的程序而执行各种适当的动作和处理。在RAM中，还存储有计算机***操作所需的各种程序和数据。CPU、ROM以及RAM通过总线被此相连。输入/输入(I/O)接口也连接至总线。
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定，对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。

Claims (10)

1.一种六自由度机械臂优化路径的方法，其特征在于，包括：
判断机械臂状态，若处于静止态，则使用判别方法识别空间路径是否经过机械臂奇异位形附近；若处于运动态，则使用跟踪算法规划轨迹使机械臂平稳跟踪经过奇异位形附近的空间路径；
所述判别算法包括：根据路径的起点空间坐标、终点空间坐标、相邻两点的时间间隔和运动时长计算路径函数；计算当前位置的条件数和其条件数对时间的导数，判断所述机械臂输出路径是否经过奇异位形附近；
所述跟踪算法包括：若所述机械臂处于运动态，根据其运动路径得到起点空间坐标p0、终点空间坐标pn和运动时长tf；计算当前位置的条件数和其条件数对时间的导数，判断所述运动态机械臂输出路径是否经过奇异位附近并根据判断结果进行路径优化。
2.根据权利要求1所述的方法，其特征在于，
所述根据路径的起点空间坐标、终点空间坐标、相邻两点的时间间隔和运动时长计算路径函数具体包括：
设定起点和终点处的空间速度和加速度为0，根据已知路径得到起点空间坐标p0、终点空间坐标pn、相邻两个点的时间间隔Δt和运动时长tf并利用多项式插值得到路径函数：
Figure FDA0003002994140000011
其中，pi表示路径上的第i个点，i为自然数且小于等于n，所述多项式的系数a0＝p0，a1＝a2＝0，a3＝10(pn-p0)/tf 3，a4＝15(pn-p0)/tf 4，a5＝6(pn-p0)/tf 5。
3.根据权利要求1所述的方法，其特征在于，
所述计算当前位置的条件数和其条件数对时间的导数，判断所述机械臂输出路径是否经过奇异位形附近具体包括：
S0011：计算机械臂位于当前位置(位置点Pi)对应的关节角度qi的雅可比矩阵J(qi)和所述雅可比矩阵J(qi)的条件数κi及其对时间的导数
Figure FDA0003002994140000012
其中Pi表示路径上第i个点，0<i<(n+1)，i为自然数；
κi＝σmax(J(qi))/σmin(J(qi)) (2)
其中，J(qi)表示位于pi时对应的关节角度qi的雅可比矩阵，获取机械臂的参数和关节角度求得，σmax(J(qi))和σmin(J(qi))分别表示J(qi)的最大奇异值和最小奇异值，可以通过对J(qi)进行奇异值分解得到；
S0012：设定阈值ε＝106、ε’＝104，若κi＞ε或者
Figure FDA0003002994140000021
则所述机械臂的输出路径经过奇异位形附近，算法结束；若κi≤ε并且
Figure FDA0003002994140000022
则所述机械臂的输出路径不经过奇异位形附近，令奇异标志s＝0；
S0013：根据公式(3)计算下一位置的关节角度：
qi+1＝qi+J-1(qi)(pi+1-pi) (3)
判断i+1是否等于n：若i+1等于n，则算法结束；若i+1小于n，则跳转至步骤S0011进行下一位置的判断。
4.根据权利要求1所述的方法，其特征在于，
所述计算当前位置的条件数和条件数对时间的导数包括：
S0020：根据运动路径得到起点空间坐标p0、终点空间坐标pn和运动时长tf；
S0030：从起始点开始，根据位置点Pi的关节角度，计算当前位形的雅可比矩阵J(qi)，其中Pi表示路径上第i个点，0<i<(n+1)；并根据公式(2)计算条件数κi及其对时间的导数
Figure FDA0003002994140000023
其中，插值间隔Δt＝0.005秒。
5.根据权利要求1所述的方法，其特征在于，
所述判断所述运动态机械臂输出路径是否经过奇异位附近并根据判断结果进行路径优化包括：
S0041：若κi＞ε或者
Figure FDA0003002994140000029
则所述机械臂的输出路径经过奇异位形附近，令奇异标志s＝1，计算阻尼系数λ：
令λ0＝λ0’＝10-5，根据公式(4)计算阻尼系数λ：
Figure FDA0003002994140000024
其中，参数ε、ε’、λ0和λ0’为设置好的阈值；
根据公式(5)计算空间速度
Figure FDA0003002994140000025
Figure FDA0003002994140000026
vx、vy和vz分别表示机械臂末端沿基坐标系x轴、y轴和z轴平移的速度，ωx、ωy和ωz分别表示所述机械臂末端绕x轴、y轴和z轴旋转的角速度：
Figure FDA0003002994140000027
其中a1＝a2＝0，a3＝10(pn-p0)/tf 3，a4＝15(pn-p0)/tf 4，a5＝6(pn-p0)/tf 5
根据公式(6)计算六轴角速度
Figure FDA0003002994140000028
Figure FDA0003002994140000031
Figure FDA0003002994140000032
其中ω1 ω2 ω3 ω4 ω5 ω6分别表示6个关节的旋转角速度，λ为阻尼系数，I为单位矩阵；跳转至步骤S0060。
6.根据权利要求5所述的方法，其特征在于，
所述判断所述运动态机械臂输出路径是否经过奇异位附近并根据判断结果进行路径优化还包括：
S0042：若κi≤ε并且
Figure FDA0003002994140000033
判断奇异标志s的值，若所述奇异标志s的值为1，则以机械臂当前实际位置
Figure FDA0003002994140000034
为起点，重新规划路径：
Figure FDA0003002994140000035
所述多项式的系数
Figure FDA0003002994140000036
Figure FDA0003002994140000037
计算空间速度
Figure FDA0003002994140000038
Figure FDA0003002994140000039
根据公式(9)计算六轴角速度
Figure FDA00030029941400000310
Figure FDA00030029941400000311
将奇异标志s的值改为0并跳转至步骤S0060。
7.根据权利要求5所述的方法，其特征在于，
还包括：
S0043：若κi≤ε并且
Figure FDA00030029941400000312
判断奇异标志s的值，若所述奇异标志s的值为0，则根据公式(9)计算六轴角速度
Figure FDA00030029941400000313
跳转至步骤S0060。
8.根据权利要求5、6或7所述的方法，其特征在于，
所述步骤S0060包括：
根据公式(10)计算下一个位置处的关节角度：
Figure FDA0003002994140000041
设qi+1为所述机械臂的目标关节角度使其移动至qi+1，若所述下一位置关节角度qi+1为终点，算法结束，否则回到跟踪算法的步骤S0030继续执行。
9.一种计算机可读存储介质，其上存储有计算机指令，其特征在于，
该指令被处理器执行时实现权利要求1-8任意一项权利要求所述方法的步骤。
10.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-8中任一项所述的方法。
CN202110353992.6A 2021-04-01 2021-04-01 一种六自由度机械臂优化路径的方法和计算机设备 Active CN113263496B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110353992.6A CN113263496B (zh) 2021-04-01 2021-04-01 一种六自由度机械臂优化路径的方法和计算机设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110353992.6A CN113263496B (zh) 2021-04-01 2021-04-01 一种六自由度机械臂优化路径的方法和计算机设备

Publications (2)

Publication Number Publication Date
CN113263496A true CN113263496A (zh) 2021-08-17
CN113263496B CN113263496B (zh) 2022-09-23

Family

ID=77228413

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110353992.6A Active CN113263496B (zh) 2021-04-01 2021-04-01 一种六自由度机械臂优化路径的方法和计算机设备

Country Status (1)

Country Link
CN (1) CN113263496B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114378830A (zh) * 2022-02-18 2022-04-22 深圳市大族机器人有限公司 一种机器人腕关节奇异规避方法及***
CN114800523A (zh) * 2022-05-26 2022-07-29 江西省智能产业技术创新研究院 机械臂轨迹修正方法、***、计算机及可读存储介质

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO1995023054A1 (en) * 1994-02-25 1995-08-31 Uk Robotics Limited Manipulator controller
CN101505925A (zh) * 2006-08-31 2009-08-12 波音公司 接近机床运动学奇点并补偿
CN102528554A (zh) * 2010-12-09 2012-07-04 中国科学院沈阳计算技术研究所有限公司 一种五轴加工奇异区域的轨迹优化方法
US8532825B1 (en) * 2010-08-25 2013-09-10 The Boeing Company Software compensation for kinematically singular machines
CN103909522A (zh) * 2014-03-19 2014-07-09 华南理工大学 一种六自由度工业机器人通过奇异域的方法
CN106003057A (zh) * 2016-05-18 2016-10-12 北京空间飞行器总体设计部 一种冗余自由度机械臂构型奇异快速判决方法
CN107234617A (zh) * 2017-07-10 2017-10-10 北京邮电大学 一种避障任务无关人工势场引导的避障路径规划方法
CN108638067A (zh) * 2018-05-17 2018-10-12 北京邮电大学 一种空间机械臂运动性能严重退化预防策略
CN109310473A (zh) * 2016-09-19 2019-02-05 直观外科手术操作公司 用于可控臂的基部定位***以及相关方法
CN110802600A (zh) * 2019-11-28 2020-02-18 合肥工业大学 一种六自由度关节型机器人的奇异性处理方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO1995023054A1 (en) * 1994-02-25 1995-08-31 Uk Robotics Limited Manipulator controller
CN101505925A (zh) * 2006-08-31 2009-08-12 波音公司 接近机床运动学奇点并补偿
US8532825B1 (en) * 2010-08-25 2013-09-10 The Boeing Company Software compensation for kinematically singular machines
CN102528554A (zh) * 2010-12-09 2012-07-04 中国科学院沈阳计算技术研究所有限公司 一种五轴加工奇异区域的轨迹优化方法
CN103909522A (zh) * 2014-03-19 2014-07-09 华南理工大学 一种六自由度工业机器人通过奇异域的方法
CN106003057A (zh) * 2016-05-18 2016-10-12 北京空间飞行器总体设计部 一种冗余自由度机械臂构型奇异快速判决方法
CN109310473A (zh) * 2016-09-19 2019-02-05 直观外科手术操作公司 用于可控臂的基部定位***以及相关方法
CN107234617A (zh) * 2017-07-10 2017-10-10 北京邮电大学 一种避障任务无关人工势场引导的避障路径规划方法
CN108638067A (zh) * 2018-05-17 2018-10-12 北京邮电大学 一种空间机械臂运动性能严重退化预防策略
CN110802600A (zh) * 2019-11-28 2020-02-18 合肥工业大学 一种六自由度关节型机器人的奇异性处理方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
谢碧云等: "机器人运动灵活性问题的研究", 《高技术通讯》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114378830A (zh) * 2022-02-18 2022-04-22 深圳市大族机器人有限公司 一种机器人腕关节奇异规避方法及***
CN114378830B (zh) * 2022-02-18 2024-02-20 深圳市大族机器人有限公司 一种机器人腕关节奇异规避方法及***
CN114800523A (zh) * 2022-05-26 2022-07-29 江西省智能产业技术创新研究院 机械臂轨迹修正方法、***、计算机及可读存储介质
CN114800523B (zh) * 2022-05-26 2023-12-01 江西省智能产业技术创新研究院 机械臂轨迹修正方法、***、计算机及可读存储介质

Also Published As

Publication number Publication date
CN113263496B (zh) 2022-09-23

Similar Documents

Publication Publication Date Title
Almusawi et al. A new artificial neural network approach in solving inverse kinematics of robotic arm (denso vp6242)
Lu et al. Collision-free and smooth joint motion planning for six-axis industrial robots by redundancy optimization
Palmer et al. Real-time method for tip following navigation of continuum snake arm robots
JP5754454B2 (ja) ロボットピッキングシステム及び被加工物の製造方法
CN111278613B (zh) 校准装置、校准方法和控制装置
Baeten et al. Hybrid vision/force control at corners in planar robotic-contour following
US8560122B2 (en) Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same
CN113263496B (zh) 一种六自由度机械臂优化路径的方法和计算机设备
JP5025598B2 (ja) 干渉チェック制御装置および干渉チェック制御方法
JP2019177436A (ja) ロボット制御装置、ロボットの関節の角度を求める方法、プログラム
CN112025772A (zh) 一种基于视觉测量的机械臂自主标定方法
Ahmad et al. Shape recovery from robot contour-tracking with force feedback
JPH0457688A (ja) 慣性系におけるロボットの制御装置
JP2020171989A (ja) ロボット教示システム
CN112476435B (zh) 重力加速度方向的标定方法、标定装置及存储介质
US20240005060A1 (en) Simulation device, storage medium having simulation program and method
CN114378809A (zh) 软体机器人操纵器的无奇点运动学参数化
JP2014065098A (ja) ロボット装置及び多関節ロボットの軌道シミュレーション装置並びに軌道生成方法
Bellakehal et al. Force/position control of parallel robots using exteroceptive pose measurements
Xue et al. The posture optimization method based on deformation index in robotic milling process
He et al. A strategy for large workpiece assembly based on hybrid impedance control
JP2013111684A (ja) ロボットアームの制御装置及び制御方法、ロボット、ロボットアームの制御プログラム、並びに、ロボットアームの制御用集積電子回路
Angel et al. Parametric identification of a delta type parallel robot
CN112135718B (zh) 机器人的控制
Jin et al. A Synthetic Algorithm for Tracking a Moving Object in a Multiple‐Dynamic Obstacles Environment Based on Kinematically Planar Redundant 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
GR01 Patent grant
GR01 Patent grant