CN114995784A - 一种基于dmspso-cil算法的七自由度机械臂解析解优化方法 - Google Patents

一种基于dmspso-cil算法的七自由度机械臂解析解优化方法 Download PDF

Info

Publication number
CN114995784A
CN114995784A CN202210797130.7A CN202210797130A CN114995784A CN 114995784 A CN114995784 A CN 114995784A CN 202210797130 A CN202210797130 A CN 202210797130A CN 114995784 A CN114995784 A CN 114995784A
Authority
CN
China
Prior art keywords
subgroup
particles
cil
mechanical arm
algorithm
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
Application number
CN202210797130.7A
Other languages
English (en)
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.)
Nanchang University
Original Assignee
Nanchang University
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 Nanchang University filed Critical Nanchang University
Priority to CN202210797130.7A priority Critical patent/CN114995784A/zh
Publication of CN114995784A publication Critical patent/CN114995784A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F7/00—Methods or arrangements for processing data by operating upon the order or content of the data handled
    • G06F7/38—Methods or arrangements for performing computations using exclusively denominational number representation, e.g. using binary, ternary, decimal representation
    • G06F7/48—Methods or arrangements for performing computations using exclusively denominational number representation, e.g. using binary, ternary, decimal representation using non-contact-making devices, e.g. tube, solid state device; using unspecified devices
    • G06F7/544—Methods or arrangements for performing computations using exclusively denominational number representation, e.g. using binary, ternary, decimal representation using non-contact-making devices, e.g. tube, solid state device; using unspecified devices for evaluating functions by calculation
    • G06F7/548—Trigonometric functions; Co-ordinate transformations
    • 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/0258—Two-dimensional joints
    • B25J17/0266—Two-dimensional joints comprising more than two actuating or connecting rods
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J18/00—Arms
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • 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
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Physics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computing Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • Feedback Control In General (AREA)

Abstract

本发明提供了一种基于DMSPSO‑CIL算法的七自由度机械臂解析解优化方法，包括以下步骤：步骤1：获取当前机械臂关节角信息θcur，机械臂下一时刻(姿态)位姿信息PT,RT；步骤2：将当前位姿信息输入逆运动学解算器(ASIK‑RMLO)得到冗余圆冗余角
Figure DDA0003736120930000011
步骤3：将
Figure DDA0003736120930000012
作为DMSPSO‑CIL算法中粒子的位置，进行更新迭代；判断是否满足终止条件，满足则跳到步骤4，否则重复执行步骤3；步骤4：给出最优解
Figure DDA0003736120930000013
及其对应的机械臂关节角值θnest。本发明求解了七自由机械臂逆运动学问题，基于“关节角变化量最小”这一系统需求，建立了相应的目标函数，提出的DMSPSO‑CIL算法对该实际问题进行求解优化。

Description

一种基于DMSPSO-CIL算法的七自由度机械臂解析解优化方法
技术领域
本发明涉及人机交互技术领域，具体为一种基于DMSPSO-CIL算法的七自由度机械臂解析解优化方法。
背景技术
医疗机器人是指用于医院、诊所、康复中心等医疗场景下进行医疗操作或辅助医疗的机器人。医疗机器人能够辅助医生，扩展医生的能力，具有医用性、临床适应性以及良好交互性三大特性。近年来，众多研究机构和商业公司致力于医疗机器人系统的研究和实践，对遥操式机器人在医疗行业中的应用进行了深入研究。在实际机器人作业场景中，6自由度机器人可以在三维空间中，进行绕轴的自由移动或者旋转，可以满足大部分场景的操作需求，然而在复杂非结构化应用场景中，七自由度冗余机器人多出一个额外的自由度可以允许机械臂躲避特定障碍，便于末端执行器到达指定位置，具有更全面的灵活性。例如，在进行超声诊断时，为了全方位的识别病灶，超声探头需要有灵活的多角度旋转特性。然而，灵活性的提高必将会带来机械臂结构的复杂化，从而导致逆运动学解的多样性。在三维空间中，对于给定的末端执行器位姿，自由度的冗余将会产生无穷解。我们将所有无穷解的集合称作封闭解。在机械臂实际应用中，封闭解对于解决关节受限、主动避障、奇异配置、机械臂动力学优化等问题起到了关键的作用。
求解机械臂逆运动学解是一项困难且颇具挑战性的工作。利用雅可比矩阵的数值迭代法求机械臂运动学逆解是最为常见的方法。然而，由于该方法需要先计算逆雅克比矩阵，这是一个非常复杂的过程。特别对于冗余臂而言，雅克比矩阵非方阵，这无疑会增加求解难度。基于此，研究者们还探索了其他不依赖于雅克比矩阵的数值迭代方法，然而这些方法仍然不能避免繁杂的迭代带来的求解速度慢，收敛精度低等问题。因此，寻找另一种求解方法(即封闭形式的解析解)便成为解决机械臂逆运动学问题的研究热点。
发明内容
本发明的目的是针对七自由度机械臂求逆解复杂的问题、传统粒子群存在的收敛速度慢，搜索精度低的问题。
提出一种新型的粒子群优化算法DMSPSO-CIL来进行改进的收敛速度慢，搜索精度低的问题。它主要包括两个策略：第一，动态多子群策略(DMS)。在DMS中，种群数量随着算法迭代过程的进行按照既定的规则逐渐减小直至为一，以此达到平衡种群的全局和局部搜索能力，增强搜索多样性的目的；第二，全面互动学习策略(CIL)。该策略通过采用四种交互学习模式：子群内学习(INTRA-SL)，子群间学习(INTER-SL)，最佳学习(OL)和快速收敛(FC)，允许每个子群中的粒子相互学习，从其他子群中的粒子学习或从整个子群中的最优子群学习。从而进一步增强了种群的局部搜索和全局搜索能力，同时加快收敛速度。
针对七自由度机械臂求逆解复杂的问题，与传统的利用数值迭代法对六自由度机械臂求逆解方法不同，我们采用基于ASIK-RMLO的几何解析法分析并求解了Barrett WAM七自由度机器人逆运动学问题。其次，对于冗余机器人存在无穷多逆解的情况，我们基于“关节角变化量最小”这一系统需求，建立了相应的目标函数，采用提出的DMSPSO-CIL算法对该实际问题进行求解优化。
为实现上述目的，本发明提供如下技术方案：一种基于DMSPSO-CIL算法的七自由度机械臂解析解优化方法，包括以下步骤：
步骤s1：获取当前机械臂关节角信息θcur，机械臂下一时刻位姿信息PT,RT；
步骤s2：将当前位姿信息输入逆运动学解算器得到冗余圆和冗余角
Figure BDA0003736120910000011
步骤s3：将
Figure BDA00037361209100000218
作为新型的粒子群优化算法DMSPSO-CIL算法中粒子的位置，进行更新迭代；DMSPSO-CIL包括两个策略：动态多子群策略DMS和全面互动学习策略CIL；在DMS中，种群数量随着算法迭代过程的进行按照既定的规则逐渐减小直至为一；CIL通过采用四种交互学习模式：子群内学习INTRA-SL，子群间学习INTER-SL，最佳学习OL和快速收敛FC，允许每个子群中的粒子相互学习，从其他子群中的粒子学习或从整个子群中的最优子群学习；
步骤s4：给出最优解
Figure BDA00037361209100000219
及其对应的机械臂关节角值θnest；
将最优解定义为解集中与上一个解关节角之差最小者，具体的数学模型阐述如下：
Figure BDA0003736120910000021
其中，
Figure BDA0003736120910000022
是冗余圆冗余角参数，θnest,θcur分别为当前时刻机械臂关节角值和下一时刻的关节角可行解，PT和RT为机械臂下一时刻的手部期望位姿，
Figure BDA00037361209100000220
即是带连杆偏移的冗余机械臂逆运动学封闭解方法的公式表示。
进一步的，DMSPSO-CIL算法：
s301:在DMSPSO-CIL中，首先随机初始化所有N个粒子的速度和位置，并评估其相应的适应度值，同时gbest也被初始化，N个粒子被分成n个子群，每个子群对应的gbestj也会被初始化，将最大迭代次数设置为tmax，分为m个阶段，每个阶段包括
Figure BDA0003736120910000023
次迭代，m定义为m＝log2n+1；
s302:阶段l，l∈[1,m]，包括t次迭代，阶段l第t次迭代，
Figure BDA0003736120910000024
整个群体包括
Figure BDA0003736120910000025
个子群，对于子群j，
Figure BDA0003736120910000026
有
Figure BDA0003736120910000027
个粒子；
s303：对于子群j的粒子i，
Figure BDA0003736120910000028
其速度更新根据INTRA-SL，INTER-SL和OL模式，如果
Figure BDA0003736120910000029
且p<r都成立，则每个粒子都根据INTRA-SL进行速度更新，如果满足
Figure BDA00037361209100000210
且p≥r，则每个粒子速度更新执行INTEA-SL，如果
Figure BDA00037361209100000211
则每个粒子根据FC模式进行速度更新，然后，更新每个粒子的位置，pbesti以及子群j中的gbestj；
s304：如果子群j中的gbestj在连续ρ次迭代中没有变化，且整个群中子群的数量大于1，即
Figure BDA00037361209100000212
则为gbestj执行OL模式；
s305：对于阶段l的第t次迭代，
Figure BDA00037361209100000213
将更新整个群体中的gbest；
s306：判断是否满足终止条件，满足则跳到步骤s4，否则循环执行步骤s302～s305。
进一步的，DMS机制的过程如下：
s3011:整个种群包含N个粒子{X1,X2,…,XN}；将N个粒子分为n个子群{S1,S2,…,Sn}，每个子群由N/n个粒子组成；对于j∈[1,n]，Sj表示为Sj＝X(j-1)N/n+1,X(j-1)N/n+2,…,XN/n+N/n}；整个迭代过程包括tmax次迭代；将tmax次迭代分为m个阶段，其中m＝log2n+1；每个阶段包括tmax/m次迭代；第j个子群的最佳解记作gbestj；更新所有粒子和gbestj；
s3012：在第l阶段l∈[1,m]，N个粒子被分成
Figure BDA00037361209100000214
个子群，对于
Figure BDA00037361209100000215
第j个子群包含
Figure BDA00037361209100000216
个粒子，定义为
Figure BDA00037361209100000217
第j个子群的最佳解可以写成gbestj，更新所有粒子和gbestj；
s3013：如果子群数量变为1，则算法终止；否则，跳转执行s3012。
进一步的，全面互动学习策略如下:
s3021:在INTRA-SL模式下，子群中的粒子可以通过学习其个人最佳位置和子群中的最佳位置来更新其速度，因此在这个阶段算法更加专注于局部搜索，它的速度更新公式如下：
Vi＝ωVi+c1R1*(pbesti-Xi)+c2R2*(gbestj-Xi)，r≤p
其中Vi和pbesti分别表示第j个子群中第i个粒子的速度和个人最佳位置；gbestj表示在第j个子群中的最佳位置；
s3022:在INTER-SL模式下，子群中的粒子可以通过学习其个人的最佳位置和ebest更新其速度，因此算法此时更加关注全局搜索，它的速度更新公式如下：
Vi＝ωVi+c1R1*(pbesti-Xi)+c2R2*(ebestj-Xi)，r＞p
其中，ebestj表示从所有子群中筛选的粒子个人最佳位置的平均值，详述如下：
Figure BDA0003736120910000031
其中，
Figure BDA0003736120910000032
表示从第j个子群
Figure BDA0003736120910000033
中随机选择的粒子个人最佳位置，
Figure BDA0003736120910000034
表示整个种群的子群总数；Xj表示从第j个子群中随机选择的粒子的下标索引；
s3023:在OL模式下，若某子群的群体最佳位置在迭代过程中没有更新，称之为粒子停滞，并将停滞次数记作ρ，如果某个子群的ρ超过设定值，则从所有子群中随机选择两个不同子群的最佳位置对停滞子群的最佳位置进行更新：
Figure BDA0003736120910000035
其中gbestj是第j个子群的最佳粒子，R是一个区间[0,1]的随机数，
Figure BDA0003736120910000036
和
Figure BDA0003736120910000037
分别是从两个不同的子群Sa和Sb中选择的最佳粒子；Sa和Sb的选取也是随机的，如果子群中的最优粒子在连续的ρ次迭代内没有得到改善，则可以认为子群陷入局部最优，通过从其他子群中学习最优粒子，OL模式可以帮助子群避免过早收敛并跳出局部最优；
S3024：在FC模式下，整个种群中的每个粒子都向gbest学习，以加快其收敛速度并提高求解精度，如下所示：
Vi＝ωVi+c2R2*(gbest-Xi)
更具体地说，如果子群的数量大于1，即
Figure BDA0003736120910000038
则CIL方案将执行INTRA-SL，INTER-SL和OL模式；否则
Figure BDA0003736120910000039
CIL方案将仅执行FC模式。
与现有技术相比，本发明的有益效果是：
本发明在优化七自由度机械臂逆运动学解析解的应用上，提出的DMSPSO-CIL算法的实时性、精确性和稳定性都有优秀的表现，尤其是实时性上，与传统的遍历搜索(SWSA)方法相比，DMSPSO-CIL解算速度提高了近1倍；同时在精确性与稳定性上，在所有比较算法中，提出的DMSPSO-CIL算法也有最佳的表现。
附图说明
图1为DMS策略示意图。
图2为全面互动学习策略(CIL)。
图3为DMSPSO-CIL算法的实现步骤。
图4为基于DMSPSO-CIL算法的ASIK-RMLO原理框图。
图5为基于DMSPSO-CIL算法的解析解优化方法。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施，对本发明进行进一步的详细说明。此处所描述的具体实施例仅用于解释本发明技术方案，并不限于本发明。
本发明提供一种技术方案：一种基于DMSPSO-CIL算法的七自由度机械臂解析解优化方法，其流程如图5所示，包括以下步骤：
步骤s1：获取当前机械臂关节角信息θcur，机械臂下一时刻(姿态)位姿信息PT,RT；
给定机械臂末端执行器(手掌或工具)的位姿信息：位置矢量PT＝[0.25，-0.7，0.1]，旋转矩阵TRZ＝[0，0，π/2]。
步骤s2：将当前位姿信息输入逆运动学解算器得到冗余圆和冗余角
Figure BDA0003736120910000049
(如图4)；
S201：ASIK-RMLO：冗余臂冗余圆
假定机械臂的末端(手掌/工具)期望位姿已给定，位置矢量PT＝[0.25，-0.7，0.1]和旋转矩阵RT＝[TRx，TRy，TRz]，可得机械臂腕部位置PW：
PW＝PT-TRZ×L3
其中L3为机械臂手掌长度(或工具长度)。从而得到腕部到肩部距离：
Figure BDA0003736120910000041
其中“·”为矢量点积。
在腕部位姿确定的情况下，通过上述几何结构，可以求得冗余圆的圆心及半径。已知机械臂的上臂长为L1，前臂长为L2，肩关节与腕关节之间的距离为d。根据三角形余弦定理可以得到上臂L1、前臂L2和水平线之间的夹角α1，α2：
Figure BDA0003736120910000042
Figure BDA0003736120910000043
进一步求得dc和RC：
dc＝L1×cos(α1)
Rc＝L1×sin(α1)
由于连杆偏移的存在，肩关节、腕关节与肘关节之间的距离将发生变化，记作NL1、NL2，根据三角形勾股定理不难得出：
Figure BDA0003736120910000044
Figure BDA0003736120910000045
故NL1、NL2与水平线l的夹角为：
Figure BDA0003736120910000046
Figure BDA0003736120910000047
生成的冗余圆圆心C到肩关节S的距离dc，圆的半径Rc(即圆心C到肘关节的距离)可由下式计算得到：
dc＝NL1×cos(α1)
Rc＝NL1×sin(α1)
因此，我们得到冗余圆，记作Circle：
Figure BDA0003736120910000048
中间隔取值为1°。按照腕部归一化旋转矩阵RWZ旋转冗余圆，最终冗余圆为：
CR＝＝Circle×RWz
S202：ASIK-RMLO：计算关节角：
已知机械臂末端执行器的位置矢量PT＝[0.25，-0.7，0.1]，姿态描述矩阵RT＝[TRx，TRy，TRz]，求解到达该状态时各个关节角度，七个自由度对应七个关节角，记作θ＝[θ1，θ2，θ3，θ4，θ5，θ6，θ7]。其中，前四个关节角可以定位手腕的位置，后三个关节角给定末端执行器的姿态。
s2021:计算手腕位置
已经求得了冗余圆的圆心及其半径，关节角θ1,θ2分别为上臂相对于肩关节(基础位置)的方位角和仰角，可以通过先求得节点E1的位置PE1进行求解：
ECR＝CR/||CR||2
ENcw＝CR×Pw/||CR×PW||2
Figure BDA0003736120910000051
因此可得到方位角(θ1)和仰角(θ2)：
Figure BDA0003736120910000052
Figure BDA0003736120910000053
关节角θ2是上臂的扭转自由度，可以通过归一化向量计算得到：
NE1Z＝(PE1×EZ)×PE1
Figure BDA0003736120910000054
关节角θ4对应前臂的基础高度，也称为肘角。由于连杆偏移的灵活性，理论上讲，如果将冗余圆描述称为肘外姿态，则还有一种与之对应的肘内姿态，这也意味着可以生成第二个冗余圆，但由于其不符合人体骨骼结构特性(人类手臂无法到达该姿态)，故不做考虑。因此得到肘外姿态时θ4：
Figure BDA0003736120910000055
s2022:计算末端执行器(手部)姿态
关节角θ5和关节角θ6分别对应末端执行器的方位角和仰角。已经求前四个关节角[θ1,θ2,θ3,θ4]，因此可得到连杆相对于基坐标系的齐次变换矩阵
Figure BDA0003736120910000056
以及旋转矩阵
Figure BDA0003736120910000057
如下：
Figure BDA0003736120910000058
然后，我们以腕部位置PW为基坐标系构建末端执行器(手掌/工具)坐标系：
Figure BDA0003736120910000059
式中，
Figure BDA00037361209100000510
为在以腕部为原点的基坐标系中末端执行器的位置，进一步可得到：
Figure BDA00037361209100000511
Figure BDA00037361209100000512
关节角θ7对应手部的旋转角度。前面我们已经求得前六个关角[θ1,θ2,θ3,θ4,θ5,θ6]，故可计算出腕部旋转矩阵
Figure BDA00037361209100000513
手部旋转关节角便可由下式得出：
θ7＝arccos(WRx·TRy)
步骤s3：将
Figure BDA00037361209100000514
作为DMSPSO-CIL算法中粒子的位置，进行更新迭代(如图3)。
S301：如图1所示，在DMSPSO-CIL中，首先随机初始化所有N个粒子的速度和位置，并评估其相应的适应度值，同时gbest也被初始化。N个粒子被分成n个子群。每个子群对应的gbestj也会被初始化。将最大迭代次数设置为tmax，分为m个阶段，每个阶段包括
Figure BDA0003736120910000061
次迭代，m定义为m＝log2n+1。
S302：阶段l，l∈[1,m]，包括t次迭代。阶段l第t次迭代，
Figure BDA0003736120910000062
整个群体包括
Figure BDA0003736120910000063
个子群，对于子群j，
Figure BDA0003736120910000064
有
Figure BDA0003736120910000065
个粒子。
s303：如图2所示，对于子群j的粒子i，
Figure BDA0003736120910000066
其速度更新根据INTRA-SL，INTER-SL和OL模式。具体地，如果
Figure BDA0003736120910000067
且p<r都成立，则每个粒子都根据INTRA-SL进行速度更新，如果满足
Figure BDA0003736120910000068
且p≥r，则每个粒子速度更新执行INTEA-SL；如果
Figure BDA0003736120910000069
则每个粒子根据FC模式进行速度更新。然后，更新每个粒子的位置，pbesti以及子群j中的gbestj。
s304：如果子群j中的gbestj在连续ρ次迭代中没有变化，且整个群中子群的数量大于1(即
Figure BDA00037361209100000610
)，则为gbestj执行OL模式。
s305：对于阶段l的第t次迭代，
Figure BDA00037361209100000611
将更新整个群体中的gbest。
s306：判断是否满足终止条件(算法迭代次数达到最大或所得解满足精度要求)，满足则跳到步骤s4，否则重复执行步骤s302～s305；
若给定机械臂末端执行器(手掌或工具)的位姿信息：位置矢量PT＝[0.25,-0.7,0.1]，旋转矩阵TRZ＝[0,0,π/2]。将位姿信息送入逆运动学求解器，得到同一位姿下多组冗余圆参数
Figure BDA00037361209100000617
对应的不同逆解。将部分冗余圆参数
Figure BDA00037361209100000619
对应逆解列出，WAM机械臂的关节角有范围限制，因此所有得出的解都符合该限制条件。
步骤s4：给出最优解
Figure BDA00037361209100000616
及其对应的机械臂关节角值θnest。
最优解定义如下：
Figure BDA00037361209100000612
其中，
Figure BDA00037361209100000613
是冗余圆冗余角参数，θnest,θcur分别为当前时刻(姿态)机械臂关节角值和下一时刻(姿态)的关节角可行解，PT和RT为机械臂下一时刻(姿态)的手部(工具)期望位姿，
Figure BDA00037361209100000614
即是我们分析的一种带连杆偏移的冗余机械臂逆运动学封闭解方法的公式表示。对于冗余圆上每一个冗余角
Figure BDA00037361209100000615
都存在一组逆解θnest，于是求最优解的过程就转化为：在特定条件下(终止条件)找到使目标方程最小值的
Figure BDA00037361209100000618
并返回其相应逆解θnest的过程。
以上所述仅表达了本发明的优选实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形、改进及替代，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (4)

1.一种基于DMSPSO-CIL算法的七自由度机械臂解析解优化方法，其特征在于：包括以下步骤：
步骤s1：获取当前机械臂关节角信息θcur，机械臂下一时刻位姿信息PT,RT；
步骤s2：将当前位姿信息输入逆运动学解算器得到冗余圆和冗余角
Figure FDA00037361209000000115
步骤s3：将
Figure FDA00037361209000000116
作为新型的粒子群优化算法DMSPSO-CIL算法中粒子的位置，进行更新迭代；DMSPSO-CIL包括两个策略：动态多子群策略DMS和全面互动学习策略CIL；在DMS中，种群数量随着算法迭代过程的进行按照既定的规则逐渐减小直至为一；CIL通过采用四种交互学习模式：子群内学习INTRA-SL，子群间学习INTER-SL，最佳学习OL和快速收敛FC，允许每个子群中的粒子相互学习，从其他子群中的粒子学习或从整个子群中的最优子群学习；
步骤s4：给出最优解
Figure FDA00037361209000000117
及其对应的机械臂关节角值θnest；
将最优解定义为解集中与上一个解关节角之差最小者，具体的数学模型阐述如下：
Figure FDA0003736120900000011
其中，
Figure FDA0003736120900000012
是冗余圆冗余角参数，θnest,θcur分别为当前时刻机械臂关节角值和下一时刻的关节角可行解，PT和RT为机械臂下一时刻的手部期望位姿，
Figure FDA0003736120900000013
即是带连杆偏移的冗余机械臂逆运动学封闭解方法的公式表示。
2.根据权利要求1所述的一种基于DMSPSO-CIL算法的七自由度机械臂解析解优化方法，其特征在于：
DMSPSO-CIL算法：
s301:在DMSPSO-CIL中，首先随机初始化所有N个粒子的速度和位置，并评估其相应的适应度值，同时gbest也被初始化，N个粒子被分成n个子群，每个子群对应的gbestj也会被初始化，将最大迭代次数设置为tmax，分为m个阶段，每个阶段包括
Figure FDA0003736120900000014
次迭代，m定义为m＝log2n+1；
s302:阶段l，l∈[1,m]，包括t次迭代，阶段l第t次迭代，
Figure FDA0003736120900000015
整个群体包括
Figure FDA0003736120900000016
个子群，对于子群j，
Figure FDA0003736120900000017
有
Figure FDA0003736120900000018
个粒子；
s303：对于子群j的粒子i，
Figure FDA0003736120900000019
其速度更新根据INTRA-SL，INTER-SL和OL模式，如果
Figure FDA00037361209000000110
且p＜r都成立，则每个粒子都根据INTRA-SL进行速度更新，如果满足
Figure FDA00037361209000000111
且p≥r，则每个粒子速度更新执行INTEA-SL，如果
Figure FDA00037361209000000112
则每个粒子根据FC模式进行速度更新，然后，更新每个粒子的位置，pbesti以及子群j中的gbestj；
s304：如果子群j中的gbestj在连续ρ次迭代中没有变化，且整个群中子群的数量大于1，即
Figure FDA00037361209000000113
则为gbestj执行OL模式；
s305：对于阶段l的第t次迭代，
Figure FDA00037361209000000114
将更新整个群体中的gbest；
s306：判断是否满足终止条件，满足则跳到步骤s4，否则循环执行步骤s302～s305。
3.根据权利要求1所述的一种基于DMSPSO-CIL算法的七自由度机械臂解析解优化方法，其特征在于：
DMS机制的过程如下：
s3011:整个种群包含N个粒子{X1,X2,…,XN}；将N个粒子分为n个子群{S1,S2,…,Sn}，每个子群由N/n个粒子组成；对于j∈[1,n]，Sj表示为Sj＝{X(j-1)N/n+1,X(j-1)N/n+2,…,XN/n+N/n}；整个迭代过程包括tmax次迭代；将tmax次迭代分为m个阶段，其中m＝log2n+1；每个阶段包括tmax/m次迭代；第j个子群的最佳解记作gbestj；更新所有粒子和gbestj；
s3012：在第l阶段l∈[1,m]，N个粒子被分成
Figure FDA0003736120900000021
个子群，对于
Figure FDA0003736120900000022
第j个子群包含
Figure FDA0003736120900000023
个粒子，定义为
Figure FDA0003736120900000024
第j个子群的最佳解可以写成gbestj，更新所有粒子和gbestj；
s3013：如果子群数量变为1，则算法终止；否则，跳转执行s3012。
4.根据权利要求1所述的一种基于DMSPSO-CIL算法的七自由度机械臂解析解优化方法，其特征在于：
全面互动学习策略如下:
s3021:在INTRA-SL模式下，子群中的粒子可以通过学习其个人最佳位置和子群中的最佳位置来更新其速度，因此在这个阶段算法更加专注于局部搜索，它的速度更新公式如下：
Vi＝ωVi+c1R1*(pbesti-Xi)+c2R2*(gbestj-Xi)，r≤p
其中Vi和pbesti分别表示第j个子群中第i个粒子的速度和个人最佳位置；gbestj表示在第j个子群中的最佳位置；
s3022:在INTER-SL模式下，子群中的粒子可以通过学习其个人的最佳位置和ebest更新其速度，因此算法此时更加关注全局搜索，它的速度更新公式如下：
Vi＝ωVi+c1R1*(pbesti-Xi)+c2R2*(ebestj-Xi),r＞p
其中，ebestj表示从所有子群中筛选的粒子个人最佳位置的平均值，详述如下：
Figure FDA0003736120900000025
其中，
Figure FDA0003736120900000026
表示从第j个子群中随机选择的粒子个人最佳位置，
Figure FDA0003736120900000027
Figure FDA0003736120900000028
表示整个种群的子群总数；Xj表示从第j个子群中随机选择的粒子的下标索引；
s3023:在OL模式下，若某子群的群体最佳位置在迭代过程中没有更新，称之为粒子停滞，并将停滞次数记作ρ，如果某个子群的ρ超过设定值，则从所有子群中随机选择两个不同子群的最佳位置对停滞子群的最佳位置进行更新：
Figure FDA0003736120900000029
其中gbestj是第j个子群的最佳粒子，R是一个区间[0,1]的随机数，
Figure FDA00037361209000000210
和
Figure FDA00037361209000000211
分别是从两个不同的子群Sa和Sb中选择的最佳粒子；Sa和Sb的选取也是随机的，如果子群中的最优粒子在连续的ρ次迭代内没有得到改善，则可以认为子群陷入局部最优，通过从其他子群中学习最优粒子，OL模式可以帮助子群避免过早收敛并跳出局部最优；
S3024：在FC模式下，整个种群中的每个粒子都向gbest学习，以加快其收敛速度并提高求解精度，如下所示：
Vi＝ωVi+c2R2*(gbest-Xi)
如果子群的数量大于1，即
Figure FDA00037361209000000212
则CIL方案将执行INTRA-SL，INTER-SL和OL模式；否则
Figure FDA00037361209000000213
CIL方案将仅执行FC模式。
CN202210797130.7A 2022-07-08 2022-07-08 一种基于dmspso-cil算法的七自由度机械臂解析解优化方法 Pending CN114995784A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210797130.7A CN114995784A (zh) 2022-07-08 2022-07-08 一种基于dmspso-cil算法的七自由度机械臂解析解优化方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210797130.7A CN114995784A (zh) 2022-07-08 2022-07-08 一种基于dmspso-cil算法的七自由度机械臂解析解优化方法

Publications (1)

Publication Number Publication Date
CN114995784A true CN114995784A (zh) 2022-09-02

Family

ID=83020166

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210797130.7A Pending CN114995784A (zh) 2022-07-08 2022-07-08 一种基于dmspso-cil算法的七自由度机械臂解析解优化方法

Country Status (1)

Country Link
CN (1) CN114995784A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115741672A (zh) * 2022-10-21 2023-03-07 杭州邦杰星医疗科技有限公司 一种基于刚体变换的dh推导方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115741672A (zh) * 2022-10-21 2023-03-07 杭州邦杰星医疗科技有限公司 一种基于刚体变换的dh推导方法
CN115741672B (zh) * 2022-10-21 2024-04-19 杭州邦杰星医疗科技有限公司 一种基于刚体变换的dh推导方法

Similar Documents

Publication Publication Date Title
US11845186B2 (en) Inverse kinematics solving method for redundant robot and redundant robot and computer readable storage medium using the same
Duka Neural network based inverse kinematics solution for trajectory tracking of a robotic arm
Gong et al. Analytical inverse kinematics and self-motion application for 7-DOF redundant manipulator
Zhang et al. Time optimal trajectory planing based on improved sparrow search algorithm
US8560122B2 (en) Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same
Li et al. An inverse kinematics method for robots after geometric parameters compensation
Peng et al. Dynamic modeling and trajectory tracking control method of segmented linkage cable-driven hyper-redundant robot
WO2023024317A1 (zh) 一种机器人避障方法、装置和机器人
CN110682286A (zh) 一种协作机器人实时避障方法
CN109344477B (zh) 一种6自由度机械臂逆运动学求解方法
CN113146600A (zh) 基于运动学迭代学习控制的柔性机器人轨迹规划方法及装置
CN114995784A (zh) 一种基于dmspso-cil算法的七自由度机械臂解析解优化方法
CN113878571B (zh) 一种七自由度协作机器人构型优化综合方法
Chen et al. Static modeling and analysis of soft manipulator considering environment contact based on segmented constant curvature method
Abbasnejad et al. Architecture optimization of 4PUS+ 1PS parallel manipulator
Xing et al. Unknown geometrical constraints estimation and trajectory planning for robotic door-opening task with visual teleoperation assists
Kong et al. Dexterity analysis and motion optimization of in-situ torsionally-steerable flexible surgical robots
CN114589701A (zh) 一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法
Valsamos et al. Introduction of the high performance area measure for the evaluation of metamorphic manipulator anatomies
Wu et al. Global motion planning and redundancy resolution for large objects manipulation by dual redundant robots with closed kinematics
Cheng et al. Analysis on the trajectory planning and simulation of six degrees of freedom manipulator
Qian et al. Path planning approach for redundant manipulator based on Jacobian pseudoinverse-RRT algorithm
Fang et al. RRT-based motion planning with sampling in redundancy space for robots with anthropomorphic arms
Zhang et al. Path planning of a 5-dof robotic arm based on BiRRT-APF algorithm considering obstacle avoidance
Jiang et al. Modeling and experimental analysis of wire-driven continuum surgical robot

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