CN115213905B - 冗余度机械臂位姿控制的方法、系统及机器人 - Google Patents

冗余度机械臂位姿控制的方法、系统及机器人 Download PDF

Info

Publication number
CN115213905B
CN115213905B CN202210919315.0A CN202210919315A CN115213905B CN 115213905 B CN115213905 B CN 115213905B CN 202210919315 A CN202210919315 A CN 202210919315A CN 115213905 B CN115213905 B CN 115213905B
Authority
CN
China
Prior art keywords
pose
joint
redundant manipulator
redundant
function
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.)
Active
Application number
CN202210919315.0A
Other languages
English (en)
Other versions
CN115213905A (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.)
Sun Yat Sen University
Original Assignee
Sun Yat Sen 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 Sun Yat Sen University filed Critical Sun Yat Sen University
Priority to CN202210919315.0A priority Critical patent/CN115213905B/zh
Publication of CN115213905A publication Critical patent/CN115213905A/zh
Application granted granted Critical
Publication of CN115213905B publication Critical patent/CN115213905B/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/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1643—Programme controls characterised by the control loop redundant control
    • 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/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
    • 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/1612—Programme controls characterised by the hand, wrist, grip control
    • 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/1628—Programme controls characterised by the control loop
    • B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
    • 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

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • Orthopedic Medicine & Surgery (AREA)
  • General Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Health & Medical Sciences (AREA)
  • Evolutionary Computation (AREA)
  • Fuzzy Systems (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了一种冗余度机械臂位姿控制方法、系统及机器人，方法包括如下步骤：步骤一：规划冗余机械臂执行器末端位姿；步骤二：提出一种位姿控制的约束优化方案；步骤三：将步骤二的约束优化方案转化为二次规划QP问题；步骤四：基于maxQ函数，设计新的零化神经网络ZNN模型作为QP问题求解器；步骤五：驱动机械臂运动到达指定位姿。本发明针对冗余度机械臂提出一种新的位姿控制的约束优化方案和一种新的ZNN求解器。所设计的ZNN求解器是一种全新的神经网络算法，其可处理冗余度机械臂的位姿控制问题，同时实现关节运动的优化控制。本发明相比其他方法而言，无需训练和迭代计算，求解精度高，还可通过选择合适的激活函数，保障本方法良好的抗噪性能。

Description

冗余度机械臂位姿控制的方法、系统及机器人
技术领域
本发明涉及位姿控制的技术领域，更具体的，涉及一种冗余度机械臂位姿控制方法、系统及机器人。
背景技术
冗余度机械臂具有关节自由度数目大于任务空间所需自由度的特性，即：完成某一特定任务时，机械臂具有冗余的自由度。冗余度机械臂的逆运动学问题是指已知机械臂末端位姿，确定机械臂的关节构型的问题。冗余度机械臂的位姿控制应用范围广泛，包括医疗领域中手术机器人的遥操作，工业领域中使用机械臂进行抓取、焊接和喷漆，航天领域中通过扩展机器人手臂进行样品的采集等。由于具备冗余的自由度，一方面，冗余度机械臂的逆运动学问题具有无穷多解，给冗余度机械臂的位姿控制带来了挑战；另一方面，冗余的自由度可保证机械臂末端在完成所要求任务的同时，还能根据不同的任务要求和工作环境选择不同的优化指标。因此，冗余度机械臂的位姿控制问题通常可以进一步表征为求解优化问题。
实际上，机器人的每个关节都拥有自身极限，在任务执行过程中如果超出机器人的关节极限，将可能损坏机器人。因此，一般为了处理冗余机器人的关节约束，可将其逆运动学问题表示成约束优化问题，如二次规划QP问题。求解约束优化问题的求解器有动态神经网络(Dynamic Neural Network,DNN)求解器、基于梯度的神经网络(Gradient-basedNeural Network,GNN)求解器、零化神经网络(Zeroing Neural Network,ZNN)求解器和数值算法等。ZNN求解器广泛应用时变优化问题的求解，具有求解精度高、无需训练和迭代运算等优点。然而，目前尚没有适用于位姿控制的ZNN求解器和优化控制方案。
发明内容
本发明为克服上述所述的冗余度机械臂的位姿控制的方法的缺陷，提供一种冗余度机械臂位姿控制方法、系统及机器人，本发明可以处理冗余机械臂的位姿控制问题，同时利用冗余的自由度实现关节速度层的优化控制。
为了达到上述优点，本发明采用以下技术方案：
本发明一方面提供了一种冗余度机械臂位姿控制方法，包括下述步骤：
步骤一：规划冗余度机械臂执行器末端位姿，即冗余度机械臂执行器末端预期到达的位置Pd以及姿态qd；
步骤二：根据具体的冗余度机械臂建立其相应的位姿控制方案；通过使用单位四元数定义姿态误差e，并考虑方案冗余度机械臂的关节速度和执行器末端的线速度v之间的等式约束/>以及关节极限θ-≤θ≤θ+和关节速度极限/>的双端约束，由此建立其相应的位姿控制方案；
步骤三：将上述位姿控制方案转化为QP问题；通过将用于考虑关节位置极限和关节速度极限的双端约束，转化成一个不等式约束，以在速度层实现对关节位置和关节速度的极限处理；最终得到的QP方案中，性能指标为xTWx/2+hTx，等式约束为Cx＝d以及不等式约束为Ex≤f；
步骤四：基于maxQ函数，设计新的零化神经网络ZNN模型作为QP问题求解器；首先基于maxQ函数L＝min3{0,Ex-f}对不等式约束条件进行处理；然后，通过为QP问题定义相应的拉格朗日函数，进行求导得到KKT条件；最后，构建误差函数e(t):＝g(t,y)并使用演变法则和激活函数Φ(g(t,y))推导出一种全新的ZNN求解器；
步骤五：驱动机械臂关节到达指定位姿；首先，通过对上述步骤四所得到的冗余度机械臂各个关节速度将其对时间进行积分求解得冗余度机械臂各个关节角度θ；最后，将冗余度机械臂各个关节速度/>或关节角度θ发送给下位机，驱动冗余度机械臂运动到达指定位姿。
作为优选的技术方案，所述冗余度机械臂由末端执行器和7个驱动关节θ1…θ7组成，所述冗余度机械臂速度层运动学方程为其中v∈R3，ω∈R3分别表示末端执行器的线速度和角速度，/>表示关节速度，J表示雅可比矩阵。
作为优选的技术方案，所述冗余度机械臂加速度层运动学方程为其中a∈R3，/>分别表示末端执行器的线加速度和角加速度，/>表示关节速度，/>表示关节加速度，J表示雅可比矩阵，/>是雅可比矩阵对时间的一阶导数。
作为优选的技术方案，所述冗余度机械臂末端执行器的姿态误差为其中qse和qve分别代表四元数qe的实部和虚部，表示用四元数所表征的机械臂末端执行器的期望姿态，/>表示用四元数所表征的机械臂末端执行器的实际姿态，/>代表q的共轭四元数，/>表示四元数乘法。
作为优选的技术方案，所述位姿控制方案的姿态误差取四元数qe的虚部qve，也即姿态误差e＝qve＝Bq，其中
作为优选的技术方案，所述冗余度机械臂的速度层运动学方程中，设计优化性能指标并考虑各个关节角度及其速度的上下限约束，建立冗余度机械臂的位姿控制方案；
经过等价整理，数学上表征为具有一般形式的二次规划问题，其中性能指标为xTWx/2+hTx，等式约束为Cx＝d以及不等式约束为Ex≤f；进一步基于maxQ函数和KKT条件，将二次规划问题等价转化成非线性方程组g(t,x)＝0；所述maxQ函数是处处可微的。
作为优选的技术方案，通过采用maxQ函数L＝min3{0,Ex-f}并构建误差函数e(t):＝g(t,y)，并使用演变法则和激活函数Φ(g(t,y))推导出一种全新的ZNN求解器/>其中有：
其中D1＝diag{μ},D2＝diag{max{0,Ex-f}}均为对角矩阵。
本发明另一方面提供了一种冗余度机械臂位姿控制系统，包括位姿规划模块、方案构建模块、转化模块、求解模块和驱动模块；
所述位姿规划模块，用于规划冗余度机械臂执行器末端的位姿，即冗余度机械臂执行器末端预期到达的位置Pd以及姿态qd；
所述方案构建模块，用于根据具体的冗余度机械臂建立其相应的位姿控制方案；通过使用单位四元数定义姿态误差e，并考虑方案冗余度机械臂的关节速度和执行器末端的线速度v之间的等式约束/>以及关节极限θ-≤θ≤θ+和关节速度极限/>的双端约束，从而达到对末端执行器的位姿控制；
所述转化模块，用于将上述位姿控制方案转化为QP问题；通过将用于考虑关节位置极限和关节速度极限的双端约束，转化成一个不等式约束，以在速度层实现对关节位置和关节速度的极限处理；最终得到的QP方案中，性能指标为xTWx/2+hTx，等式约束为Cx＝d以及不等式约束为Ex≤f；
所述求解模块，通过引入maxQ函数，设计新的ZNN模型求解QP；首先基于maxQ函数L＝min3{0,Ex-f}对不等式约束条件进行处理；然后，通过为QP问题定义相应的拉格朗日函数，进行求导得到KKT条件；最后，构建误差函数e(t):＝g(t,y)并使用演变法则和激活函数Φ(g(t,y))推导出一种全新的ZNN求解器；
所述驱动模块，用于驱动冗余机械臂关节到达指定位姿；首先，通过对求解模块所得到的冗余度机械臂各个关节速度将其对时间进行积分求解得冗余度机械臂各个关节角度θ；最后，将冗余度机械臂各个关节速度/>或关节角度θ发送给下位机，驱动冗余度机械臂运动到达指定位姿。
本发明又一方面提供了一种计算机可读存储介质，存储有程序，所述程序被处理器执行时，实现所述的冗余度机械臂位姿控制方法。
本发明再一方面提供了一种机器人，所述机器人包括：
至少一个处理器；以及，
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的计算机程序指令，所述计算机程序指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行所述的冗余度机械臂位姿控制方法。
本发明与现有技术相比，具有如下优点和有益效果：首先，本发明通过引入单位四元数和演化法则设计新的位姿控制性能指标，同时通过将用于考虑关节位置极限和关节速度极限的双端约束，转化成一个不等式约束，以在速度层实现对关节位置和关节速度的极限处理，进而构建新的位姿控制方案；其次，本发明通过引入maxQ函数进行不等式约束处理和位姿控制方案的等价转化，结合演化法则以及激活函数得到了一种全新的ZNN求解器。相比于DNN、GNN等其他求解器，ZNN求解器在每个时间步仅需一次迭代，操作方便、无需训练，能够运用在实时性强的一些应用中。另外，所引入的maxQ具有处处可微的性质；最后，本发明还可以通过选择合适的激活函数，使得本方法具有良好的抗噪性能。本发明提供了一种操作方便、无需训练和迭代计算的冗余度机械臂的位姿控制方法。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例冗余度机械臂位姿控制方法的流程图。
图2为本发明实施例的仿真Franka机械臂的模型图。
图3为本发明实施例的仿真Franka机械臂末端执行器的位姿误差图。
图4为本发明实施例的仿真Franka机械臂的关节角度变化图。
图5为本发明实施例的仿真Franka机械臂关节速度变化图。
图6为本发明实施例的仿真Franka机械臂末端执行器的目标轨迹与实际轨迹图。
图7为本发明实施例冗余度机械臂位姿控制方法的结构示意图。
图8为本发明实施例机器人的结构示意图。
具体实施方式
为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请中的实施例及附图，对本发明的技术方案进行清楚、完整地描述，应当理解，附图仅用于示例性说明，不能理解为对本专利的限制。显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
在本申请中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本申请所描述的实施例可以与其它实施例相结合。
实施例
如图1所示，本实施例为一种冗余度机械臂位姿控制方法，该方法包括以下步骤：
步骤一：规划冗余机械臂执行器末端位姿，即冗余机械臂执行器末端预期到达的位置Pd以及姿态qd。为了用更少的参数表示冗余机械臂末端执行器的姿态，本方法将表示预期姿态的旋转矩阵Rd转化为单位四元数qd。
步骤二：根据具体的冗余度机械臂建立其相应的位姿控制方案。
首先，使用单位四元数定义姿态误差e＝qve，其中qve为四元数误差的虚部qe，具体表达式如下。
其中qd代表期望四元数，q代表实际值，q*代表q的共轭，qe代表四元数误差，代表四元数乘法。
其次，使用演变法则使误差e能够指数收敛，进而得到/>通过将关节速度/>与四元数速度/>联系在一起。考虑到末端执行器轨迹和物理特性限制，将该求解问题转化为求最小化问题/>A＝BJq。其中γ作为控制所述方法的正实数，Jq为四元数q的雅可比矩阵。
最后，考虑方案冗余度机械臂的关节速度和执行器末端的线速度v之间的等式约束/>其中Jp为冗余度机械臂的位置雅可比矩阵，以及关节极限θ-≤θ≤θ+和关节速度极限/>的双端约束。
最终得到的运动控制方案中，性能指标为等式约束为/>以及双端约束为θ-≤θ≤θ+，/>
步骤三：将上述位姿控制方案转化为QP问题。
首先，令C＝Jp,d＝v,W＝ATA,h＝γATBq，将性能指标/>转化为标准形式：xTWx/2+hTx，令/>f＝[x+,-x-]T和E＝[I,-I]T巧妙地将关节速度的双端约束等价转换成不等式约束Ex≤f，以在速度层实现对关节位置和关节速度的极限处理。其中g1(θ)和g2(θ)的表达式如下：
θ1＝σ1θ-，θ2＝(1-σ1)θ-，θ3＝σ2θ+，θ4＝(1-σ2)θ+，
其中，σ1和σ2为均为(0,1)之间的常量。最终得到的二次规划问题方案中，性能指标为xTWx/2+hTx，等式约束为Cx＝d以及不等式约束为Ex≤f。
步骤四：基于maxQ函数，设计新的零化神经网络ZNN模型作为QP问题求解器。
首先，基于maxQ函数L＝min3{0,Ex-f}对不等式约束条件Ex≤f进行处理。
然后，通过为QP问题定义相应的拉格朗日函数，进行求导得到KKT条件。其中定常方程式为Wx+h+CTλ+3ET(max2{0,Ex-f}ομ)＝0，约束条件为Cx-d＝0，互补松弛性条件为min3{0,Ex-f}。其中λ和μ分别为等式约束和不等式约束的拉格朗日乘子。
最后，构建误差函数e(t):＝g(t,y)并使用演变法则和激活函数Φ(g(t,y))，推导出一种全新的ZNN求解器/>以此求得上述QP问题的最优解x，即获得冗余度机械臂各个关节速度。具体表达式如下。
其中，y＝[xT,λT,μT]T，α为调节ZNN求解器收敛速度的正实数，D1＝diag{μ}，D2＝diag{max{0,Ex-f}}均为对角矩阵。
步骤五：驱动机械臂关节到达指定位姿。
首先，通过对步骤四上述所得到的冗余度机械臂各个关节角速度将其对时间进行积分求解得冗余度机械臂各个关节角度θ；
最后，将冗余度机械臂各个关节速度或关节角度θ发送给下位机，驱动冗余度机械臂运动到达指定位姿。
下面结合附图对本发明做进一步说明。
如图2所示，该冗余度机械臂的模型是由关节1、关节2、关节3、关节4、关节5、关节6、关节7和末端执行器组成的，该冗余度机械臂速度层运动学关系式为其中v∈R3,ω∈R3，分别表示末端执行器的线速度和角速度，/>表示关节速度，J表示雅可比矩阵。
如图3所示，其中实线ex表示仿真Franka机械臂末端执行器在x方向上的误差，黑虚线ey表示仿真Franka机械臂末端执行器在y方向上的误差，黑点线ez表示仿真Franka机械臂末端执行器在z方向上的误差，黑点划线1-qs表示仿真Franka机械臂末端执行器在姿态实部上的误差，灰虚线qv1，灰点线qv2，灰点划线qv3表示仿真Franka机械臂末端执行器在姿态虚部上的误差。其中在任务执行期间，位置和姿态上的误差分量皆小于等于10-4米，具有亚毫米级高位姿精度。
如图4所示，其中，θ1,θ2,θ3,θ4,θ5,θ6,θ7分别表示仿真Franka机械臂的第一关节1、第二关节2、第三关节3、第四关节4、第五关节5、第六关节6以及第七关节7角度。在任务执行期间，各个关节角度不断变化，对应机械臂出现各种不同的运动。
如图5所示，其中，分别表示仿真Franka机械臂的第一关节1、第二关节2、第三关节3、第四关节4、第五关节5、第六关节6以及第七关节7速度。从图中可以看到，在任务执行的过程中，各个关节速度可以保证在一定的范围内变化，从而可以说明关节速度层约束的有效性。
如图6所示，其中直线为虚线为仿真Franka机械臂末端执行器的位姿控制实施例的实际轨迹，右边为机械臂的初始状态，左边为机械臂的最终状态。从图中可以看出目标位姿和机械臂末端最终状态的位姿重合，说明利用该方法可以实现对机械臂末端执的运动实现位姿控制。
如图7所示，在本申请的另一个实施例中，提供了一种冗余度机械臂位姿控制系统100，包括位姿规划模块101、方案构建模块102、转化模块103、求解模块104和驱动模块105；
所述位姿规划模块101，用于规划冗余度机械臂执行器末端的位姿，即冗余度机械臂执行器末端预期到达的位置Pd以及姿态qd；
所述方案构建模块102，用于根据具体的冗余度机械臂建立其相应的位姿控制方案；通过使用单位四元数定义姿态误差e，并考虑方案冗余度机械臂的关节速度和执行器末端的线速度v之间的等式约束/>以及关节极限θ-≤θ≤θ+和关节速度极限的双端约束，由此建立其相应的位姿控制方案；
所述转化模块103，用于将上述位姿控制方案转化为QP问题；通过将用于考虑关节位置极限和关节速度极限的双端约束，转化成一个不等式约束，以在速度层实现对关节位置和关节速度的极限处理；最终得到的QP方案中，性能指标为xTWx/2+hTx，等式约束为Cx＝d以及不等式约束为Ex≤f；
所述求解模块104，用于基于maxQ函数，设计新的ZNN模型求解QP；首先基于maxQ函数L＝min3{0,Ex-f}对不等式约束条件进行处理；然后，通过为QP问题定义相应的拉格朗日函数，进行求导得到KKT条件；最后，构建误差函数e(t):＝g(t,y)并使用演变法则和激活函数Φ(g(t,y))推导出一种全新的ZNN求解器；
所述驱动模块105，用于控制冗余机械臂关节运动到达指定位姿；首先，通过对求解模块所得到的冗余度机械臂各个关节速度将其对时间进行积分求解得冗余度机械臂各个关节角度θ；最后，将冗余度机械臂各个关节速度/>或关节角度θ发送给下位机，驱动冗余度机械臂运动。
此外，上述实施例的冗余度机械臂位姿控制系统的实施方式中，各程序模块的逻辑划分仅是举例说明，实际应用中可以根据需要，例如出于相应硬件的配置要求或者软件的实现的便利考虑，将上述功能分配由不同的程序模块完成，即将所述冗余度机械臂位姿控制系统的内部结构划分成不同的程序模块，以完成以上描述的全部或者部分功能。
如图8所示，在一个实施例中，提供了一种机器人200，所述机器人200可以包括第一处理器201、第一存储器202和总线，还可以包括存储在所述第一存储器202中并可在所述第一处理器201上运行的计算机程序，如冗余度机械臂位姿控制程序203。
其中，所述第一存储器202至少包括一种类型的可读存储介质，所述可读存储介质包括闪存、移动硬盘、多媒体卡、卡型存储器(例如：SD或DX存储器等)、磁性存储器、磁盘、光盘等。所述第一存储器202在一些实施例中可以是机器人200的内部存储单元，例如该机器人200的移动硬盘。所述第一存储器202在另一些实施例中也可以是机器人200的外部存储设备，例如机器人200上配备的插接式移动硬盘、智能存储卡(Smart Media Card，SMC)、安全数字(SecureDigital，SD)卡、闪存卡(Flash Card)等。进一步地，所述第一存储器202还可以既包括机器人200的内部存储单元也包括外部存储设备。所述第一存储器202不仅可以用于存储安装于机器人200的应用软件及各类数据，例如冗余度机械臂位姿控制程序203的代码等，还可以用于暂时地存储已经输出或者将要输出的数据。
所述第一处理器201在一些实施例中可以由集成电路组成，例如可以由单个封装的集成电路所组成，也可以是由多个相同功能或不同功能封装的集成电路所组成，包括一个或者多个中央处理器(Central Processing unit，CPU)、微处理器、数字处理芯片、图形处理器及各种控制芯片的组合等。所述第一处理器201是所述电子设备的控制核心(Control Unit)，利用各种接口和线路连接整个电子设备的各个部件，通过运行或执行存储在所述第一存储器202内的程序或者模块，以及调用存储在所述第一存储器202内的数据，以执行机器人200的各种功能和处理数据。
图8仅示出了具有部件的机器人，本领域技术人员可以理解的是，图8示出的结构并不构成对所述机器人200的限定，可以包括比图示更少或者更多的部件，或者组合某些部件，或者不同的部件布置。
所述机器人200中的所述第一存储器202存储的冗余度机械臂位姿控制程序203是多个指令的组合，在所述第一处理器201中运行时，可以实现：
步骤一：规划冗余度机械臂执行器末端位姿，即冗余度机械臂执行器末端预期到达的位置Pd以及姿态qd；
步骤二：根据具体的冗余度机械臂建立其相应的位姿控制方案；通过使用单位四元数定义姿态误差e，并考虑方案冗余度机械臂的关节速度和执行器末端的线速度v之间的等式约束/>以及关节极限θ-≤θ≤θ+和关节速度极限/>的双端约束，由此建立其相应的位姿控制方案；
步骤三：将上述位姿控制方案转化为QP问题；通过将用于考虑关节位置极限和关节速度极限的双端约束，转化成一个不等式约束，以在速度层实现对关节位置和关节速度的极限处理；最终得到的QP方案中，性能指标为xTWx/2+hTx，等式约束为Cx＝d以及不等式约束为Ex≤f；
步骤四：基于maxQ函数，设计新的零化神经网络ZNN模型作为QP问题求解器；首先基于maxQ函数L＝min3{0,Ex-f}对不等式约束条件进行处理；然后，通过为QP问题定义相应的拉格朗日函数，进行求导得到KKT条件；最后，构建误差函数e(t):＝g(t,y)并使用演变法则和激活函数Φ(g(t,y))推导出一种全新的ZNN求解器；
步骤五：驱动机械臂关节到达指定位姿；首先，通过对上述步骤四所得到的冗余度机械臂各个关节速度将其对时间进行积分求解得冗余度机械臂各个关节角度θ；最后，将冗余度机械臂各个关节速度/>或关节角度θ发送给下位机，驱动冗余度机械臂运动到达指定位姿。
进一步地，所述机器人200集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个非易失性计算机可读取存储介质中。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一非易失性计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (10)

1.一种冗余度机械臂位姿控制方法，其特征在于，包括下述步骤：
步骤一：规划冗余度机械臂执行器末端位姿，即冗余度机械臂执行器末端预期到达的位置Pd以及姿态qd；
步骤二：根据具体的冗余度机械臂建立其相应的位姿控制方案；通过使用单位四元数定义姿态误差e，并考虑冗余度机械臂的关节速度和执行器末端的线速度v之间的等式约束/>其中Jp为冗余度机械臂的位置雅可比矩阵，以及关节极限θ-≤θ≤θ+和关节速度极限/>的双端约束，由此建立其相应的位姿控制方案；
步骤三：将上述位姿控制方案转化为QP问题；通过将用于考虑关节位置极限和关节速度极限的双端约束，转化成一个不等式约束，以在速度层实现对关节位置和关节速度的极限处理；最终得到的QP方案中，性能指标为xTWx/2+hTx，等式约束为Cx＝d以及不等式约束为Ex≤f；
步骤四：基于maxQ函数，设计新的零化神经网络ZNN模型作为QP问题求解器；首先基于maxQ函数L＝min3{0,Ex-f}对不等式约束条件进行处理；然后，通过为QP问题定义相应的拉格朗日函数，进行求导得到KKT条件；最后，构建误差函数e(t):＝g(t,y)并使用演变法则和激活函数Φ(g(t,y))推导出一种全新的ZNN求解器，其中α为调节ZNN求解器收敛速度的正实数；
步骤五：驱动机械臂关节到达指定位姿；首先，通过对上述步骤四所得到的冗余度机械臂各个关节速度将其对时间进行积分求解得冗余度机械臂各个关节角度θ；最后，将冗余度机械臂各个关节速度/>或关节角度θ发送给下位机，驱动冗余度机械臂运动到达指定位姿。
2.根据权利要求1所述冗余度机械臂位姿控制方法，其特征在于，所述冗余度机械臂由末端执行器和7个驱动关节θ1…θ7组成，所述冗余度机械臂速度层运动学方程为其中v∈R3，ω∈R3分别表示末端执行器的线速度和角速度，/>表示关节速度，J表示雅可比矩阵。
3.根据权利要求1所述冗余度机械臂位姿控制方法，其特征在于，所述冗余度机械臂加速度层运动学方程为其中a∈R3，/>分别表示末端执行器的线加速度和角加速度，/>表示关节速度，/>表示关节加速度，J表示雅可比矩阵，/>是雅可比矩阵对时间的一阶导数。
4.根据权利要求1所述冗余度机械臂位姿控制方法，其特征在于，所述冗余度机械臂末端执行器的姿态误差为其中qse和qve分别代表四元数qe的实部和虚部，/>表示用四元数所表征的机械臂末端执行器的期望姿态，/>表示用四元数所表征的机械臂末端执行器的实际姿态，/>代表q的共轭四元数，/>表示四元数乘法。
5.根据权利要求4所述冗余度机械臂位姿控制方法，其特征在于，所述位姿控制方案的姿态误差取四元数qe的虚部qve，也即姿态误差e＝qve＝Bq，其中
6.根据权利要求1所述冗余度机械臂位姿控制方法，其特征在于，所述冗余度机械臂的速度层运动学方程中，设计优化性能指标并考虑各个关节角度及其速度的上下限约束，建立冗余度机械臂的位姿控制方案；
经过等价整理，数学上表征为具有一般形式的二次规划问题，其中性能指标为xTWx/2+hTx，等式约束为Cx＝d以及不等式约束为Ex≤f；进一步基于maxQ函数和KKT条件，将二次规划问题等价转化成非线性方程组g(t,x)＝0；所述maxQ函数是处处可微的。
7.根据权利要求6所述冗余度机械臂位姿控制方法，其特征在于，通过引入maxQ函数L＝min3{0,Ex-f}并构建误差函数e(t):＝g(t,y)，并使用演变法则和激活函数Φ(g(t,y))推导出一种全新的ZNN求解器/>其中有：
其中D1＝diag{μ},D2＝diag{max{0,Ex-f}}均为对角矩阵，μ表示不等式约束Ex≤f对应的拉格朗日系数。
8.一种冗余度机械臂位姿控制系统，其特征在于，包括位姿规划模块、方案构建模块、转化模块、求解模块和驱动模块；
所述位姿规划模块，用于规划冗余度机械臂执行器末端的位姿，即冗余度机械臂执行器末端预期到达的位置Pd以及姿态qd；
所述方案构建模块，用于根据具体的冗余度机械臂建立其相应的位姿控制方案；通过使用单位四元数定义姿态误差e，并考虑方案冗余度机械臂的关节速度和执行器末端的线速度v之间的等式约束/>以及关节极限θ-≤θ≤θ+和关节速度极限/>的双端约束，由此建立其相应的位姿控制方案；
所述转化模块，用于将上述位姿控制方案转化为QP问题；通过将用于考虑关节位置极限和关节速度极限的双端约束，转化成一个不等式约束，以在速度层实现对关节位置和关节速度的极限处理；最终得到的QP方案中，性能指标为xTWx/2+hTx，等式约束为Cx＝d以及不等式约束为Ex≤f；
所述求解模块，利用maxQ函数，设计新的ZNN模型求解QP；首先基于maxQ函数L＝min3{0,Ex-f}对不等式约束条件进行处理；然后，通过为QP问题定义相应的拉格朗日函数，进行求导得到KKT条件；最后，构建误差函数e(t):＝g(t,y)并使用演变法则和激活函数Φ(g(t,y))推导出一种全新的ZNN求解器；
所述驱动模块，用于驱动冗余机械臂关节到达指定位姿；首先，通过对求解模块所得到的冗余度机械臂各个关节速度将其对时间进行积分求解得冗余度机械臂各个关节角度θ；最后，将冗余度机械臂各个关节速度/>或关节角度θ发送给下位机，驱动冗余度机械臂运动到达指定位姿。
9.一种计算机可读存储介质，存储有程序，其特征在于，所述程序被处理器执行时，实现权利要求1-7任一项所述的冗余度机械臂位姿控制方法。
10.一种机器人，其特征在于，所述机器人包括：
至少一个处理器；以及，
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的计算机程序指令，所述计算机程序指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如权利要求1-7中任意一项所述的冗余度机械臂位姿控制方法。
CN202210919315.0A 2022-08-02 2022-08-02 冗余度机械臂位姿控制的方法、系统及机器人 Active CN115213905B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210919315.0A CN115213905B (zh) 2022-08-02 2022-08-02 冗余度机械臂位姿控制的方法、系统及机器人

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210919315.0A CN115213905B (zh) 2022-08-02 2022-08-02 冗余度机械臂位姿控制的方法、系统及机器人

Publications (2)

Publication Number Publication Date
CN115213905A CN115213905A (zh) 2022-10-21
CN115213905B true CN115213905B (zh) 2023-08-25

Family

ID=83615056

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210919315.0A Active CN115213905B (zh) 2022-08-02 2022-08-02 冗余度机械臂位姿控制的方法、系统及机器人

Country Status (1)

Country Link
CN (1) CN115213905B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116276986B (zh) * 2023-02-28 2024-03-01 中山大学 一种柔性驱动机器人的复合学习自适应控制方法
CN116901063B (zh) * 2023-07-13 2024-01-26 长春通视光电技术股份有限公司 一种基于二次规划的冗余空间机械臂轨迹规划方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104908040A (zh) * 2015-06-23 2015-09-16 广东顺德中山大学卡内基梅隆大学国际联合研究院 一种冗余度机械臂加速度层的容错规划方法
CN107972030A (zh) * 2017-11-10 2018-05-01 浙江科技学院 一种冗余机械臂重复运动中的初始位置修正方法
CN108326844A (zh) * 2017-01-20 2018-07-27 香港理工大学深圳研究院 冗余度机械臂的可操作度优化的运动规划方法和装置
CN110076770A (zh) * 2019-03-28 2019-08-02 陕西理工大学 一种用于冗余机械臂的自运动方法
CN110434854A (zh) * 2019-08-20 2019-11-12 兰州大学 一种基于数据驱动的冗余度机械臂视觉伺服控制方法与装置
CN110561441A (zh) * 2019-10-23 2019-12-13 中山大学 一种冗余度机械臂位姿控制的单94lvi迭代算法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111538949B (zh) * 2020-07-10 2020-10-16 深圳市优必选科技股份有限公司 冗余机器人逆运动学求解方法、装置和冗余机器人

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104908040A (zh) * 2015-06-23 2015-09-16 广东顺德中山大学卡内基梅隆大学国际联合研究院 一种冗余度机械臂加速度层的容错规划方法
CN108326844A (zh) * 2017-01-20 2018-07-27 香港理工大学深圳研究院 冗余度机械臂的可操作度优化的运动规划方法和装置
CN107972030A (zh) * 2017-11-10 2018-05-01 浙江科技学院 一种冗余机械臂重复运动中的初始位置修正方法
CN110076770A (zh) * 2019-03-28 2019-08-02 陕西理工大学 一种用于冗余机械臂的自运动方法
CN110434854A (zh) * 2019-08-20 2019-11-12 兰州大学 一种基于数据驱动的冗余度机械臂视觉伺服控制方法与装置
CN110561441A (zh) * 2019-10-23 2019-12-13 中山大学 一种冗余度机械臂位姿控制的单94lvi迭代算法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
Junqiao Qiu.Two Numerical Algorithms and Numerical Experiments for Efficiently Solving Inequality-and-Bound Constrained QP.《The 2014 2nd International Conference on Systems and Informatics》.2015,第39-44页. *

Also Published As

Publication number Publication date
CN115213905A (zh) 2022-10-21

Similar Documents

Publication Publication Date Title
CN115213905B (zh) 冗余度机械臂位姿控制的方法、系统及机器人
Kalani et al. Improved general solution for the dynamic modeling of Gough–Stewart platform based on principle of virtual work
Monsarrat et al. Workspace analysis and optimal design of a 3-leg 6-DOF parallel platform mechanism
CN108908347B (zh) 一种面向冗余移动机械臂容错型重复运动规划方法
Wang et al. A multi-objective approach for the trajectory planning of a 7-DOF serial-parallel hybrid humanoid arm
CN115157262B (zh) 关节受限冗余机械臂的障碍物躲避与优化控制方法、系统及机器人
Huang et al. A geometric algebra algorithm for the closed-form forward displacement analysis of 3-PPS parallel mechanisms
Lai et al. A learning-based inverse kinematics solver for a multi-segment continuum robot in robot-independent mapping
CN109366486B (zh) 柔性机器人逆运动学求解方法、系统、设备、存储介质
Jiang et al. An integrated inverse kinematic approach for the 7-DOF humanoid arm with offset wrist
CN114851168B (zh) 关节受限冗余并联机械臂的运动规划和控制方法、系统及机器人
Wang et al. Kinematical Research of Free‐Floating Space‐Robot System at Position Level Based on Screw Theory
Song et al. Efficient formulation approach for the forward kinematics of the 3-6 Stewart-Gough platform
CN114952860A (zh) 基于离散时间神经动力学的移动机器人重复运动控制方法及系统
Garg et al. Path tracing and object avoidance algorithm for robotic manipulators incorporating constrained filters
Ma et al. The kinematic analysis and trajectory planning study of high-speed SCARA robot handling operation
Boiadjiev et al. Dynamics of a Planar Redundant Robot Based on Energy Conservation Law and Graph Theory
Liu et al. Kinematic calibration of a Gough-Stewart based high-precision pointing mechanism
Xiao et al. Forward and inverse kinematics analysis of SMA spring-driven flexible manipulator
Lukić et al. School of Electrical Engineering, University of Belgrade, 11120 Belgrade, Serbia {branko, knezevic, kostaj}@ etf. rs
Abdullah et al. Algebraic models based on trigonometric and Cramer's rules for computing inverse kinematics of robotic arm
Dong et al. Global Redundancy Optimization of Manipulability for 7-DOFs Anthropomorphic Manipulator Using Joint Monotonicity
CN117681196A (zh) 一种实现冗余机械臂路径跟踪与障碍物躲避的方法、系统及机器人
Sekiguchi et al. A Stable Algorithm for Unsolvable Inverse Kinematics of a Class of Six-DoF Manipulators
Abdullah et al. Algebraic-Trigonometric Nonlinear Analytical Inverse Kinematic Modeling and Simulation for Robotic Manipulator Arm Motion Control

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