CN106625666A - 冗余机械臂的控制方法及装置 - Google Patents

Abstract

本发明实施例公开了一种冗余机械臂的控制方法及装置。该方法包括：获取冗余机械臂所在的当前点信息和目标点信息；根据所述当前点信息和所述目标点信息确定所述冗余机械臂从当前点移动到目标点的运动轨迹对应的轨迹函数；以冗余度空间向量为自变量建立所述轨迹函数对应的方程；根据目标接近法求解所述轨迹函数对应的方程，得到所述运动轨迹对应的所述冗余机械臂中各关节的位置和速度。本发明实施例通过采用上述技术方案，可以在得到满足设计目标的全局最优解的前提下，减少冗余机械臂多目标优化问题的搜索空间，避免多目标求解过程中维度爆炸问题的发生，简化冗余机械臂控制过程中所需的计算量，提高冗余机械臂的反应速度。

Description

冗余机械臂的控制方法及装置

技术领域

本发明涉及机器人技术领域，尤其涉及一种冗余机械臂的控制方法及装置。

背景技术

近年来，随着人工智能技术和机械控制技术的提高以及机器人所执行任务复杂程度的提高，冗余机械臂(关节数大于6个的机械臂)越来越多的被应用到了机器人中来完成各种较为复杂任务。

冗余机械臂执行笛卡尔空间特定任务时在关节空间有无穷组解，在具体应用时，一般可以根据关节速度、关节力矩或障碍物距离等优化指标选择一组最优解来确定冗余机械臂的移动过程。在机器人执行较复杂的任务时，通常需要同时对冗余机械臂的多个目标进行优化，目前，常用来进行多目标优化的方法可归类为加权函数法和帕累托前沿法。但是，加权函数法虽然可以利用加权系数对优化目标进行加权从而将多目标优化问题转化为单目标优化问题，但是，此类方法难以判断加权系数变化对优化结果造成的影响，每次只能获得一个解，需不断改变加权系数的值，计算步骤繁琐且存在一定的局限性；帕累托前沿类方法的计算复杂程度随机械臂自由度的增加而急剧增加，当冗余机械臂关节数较多时，该类方法也存在计算复杂、步骤繁琐的问题。

发明内容

有鉴于此，本发明实施例提供一种冗余机械臂的控制方法及装置，以解决现有技术中多目标求解方法计算复杂、步骤繁琐的技术问题。

第一方面，本发明实施例提供了一种冗余机械臂的控制方法，包括：

获取冗余机械臂所在的当前点信息和目标点信息；

根据所述当前点信息和所述目标点信息确定所述冗余机械臂从当前点移动到目标点的运动轨迹对应的轨迹函数；

以冗余度空间向量为自变量建立所述轨迹函数对应的方程；

根据目标接近法求解所述轨迹函数对应的方程，得到所述运动轨迹对应的所述冗余机械臂中各关节的位置和速度。

第二方面，本发明实施例还提供了一种冗余机械臂的控制装置，包括：

信息获取单元，用于获取冗余机械臂所在的当前点信息和目标点信息；

轨迹单元，与所述信息获取单元相连，用于根据所述当前点信息和所述目标点信息确定所述冗余机械臂从当前点移动到目标点的运动轨迹对应的轨迹函数；

冗余度处理单元，与所述轨迹单元相连，用于以冗余度空间向量为自变量建立所述轨迹函数对应的方程；根据目标接近法求解所述轨迹函数对应的方程，得到所述运动轨迹对应的所述冗余机械臂中各关节的位置和速度。

本发明实施例提供的控制冗余机械臂的技术方案，获取冗余机械臂的当前点信息和目标点信息，根据所获取的当前点信息和目标点信息确定冗余机械臂从当前点移动到目标点的运动轨迹对应的轨迹函数，以冗余度空间向量为自变量建立该轨迹函数对应的方程，通过目标接近法求解该方程以得到运动轨迹对应的冗余机械臂中各关节的位置和速度。本发明实施例通过采用上述技术方案，可以在得到满足设计目标的全局最优解的前提下，减少冗余机械臂多目标优化问题的搜索空间，避免多目标求解过程中维度爆炸问题的发生，简化冗余机械臂控制过程中所需的计算量，提高冗余机械臂的反应速度。

附图说明

通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明实施例一提供的一种冗余机械臂的控制方法的流程示意图；

图2为本发明实施例二提供的一种冗余机械臂的控制方法的流程示意图；

图3为本发明实施例二提供的一种九自由度冗余机械臂的坐标系示意图

图4为本发明实施例三提供的一种冗余机械臂的控制装置的结构框图。

具体实施方式

下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部内容。

实施例一

本发明实施例一提供一种冗余机械臂的控制方法。该方法可由冗余机械臂的控制装置执行，其中，该装置可通过硬件和/或软件实现，一般可集成在用于控制机器人的控制模块中。图1是本发明实施例一提供的冗余机械臂的控制方法的流程示意图，如图1所示，该方法包括：

S110、获取冗余机械臂所在的当前点信息和目标点信息。

本实施例中，冗余机械臂的当前点信息和目标点信息可以是冗余机械臂末端的当前点信息和目标点信息，也可以是冗余机械臂各关节的当前点信息和目标点信息，此处不作限制。考虑到当前点信息和目标点信息的实用性，可选的，冗余机械臂所在的当前点信息可以包括机械臂末端的当前点信息和冗余机械臂各关节的当前点信息，目标点信息可以包括机械臂末端的目标点信息。其中，当前点信息(目标点信息)可以包括机械臂末端或机械臂各关节在当前(目标)位置的位置坐标、角度坐标、线速度、角速度、线加速度和/或角加速度等信息等。

示例性的，在获取冗余机械臂的当前点信息时，可以直接通过传感器获取冗余机械臂的当前点信息或者通过采用上一时刻的计算结果作为冗余机械臂的当前点信息；在获取冗余机械臂的目标点信息时，可以根据冗余机械臂末端的当前点信息和冗余机械臂末端的运动轨迹信息计算得到冗余机械臂末端需要移动到的目标点信息。以当前点信息为角度坐标、角速度和角加速度为例，冗余机械臂某一关节在当前位置处的角度信息和角速度信息可以通过安装在该关节位置的编码器获得，其角加速度信息可以通过差分运算获得；或者，也可以直接调用上一时刻计算得到的机械臂各关节的目标点信息作为机械臂各关节在当前时刻的当前点信息。

S120、根据所述当前点信息和所述目标点信息确定所述冗余机械臂从当前点移动到目标点的运动轨迹对应的轨迹函数。

本实施例中，冗余机械臂从当前点移动到目标点的运动轨迹对应的轨迹函数可以包含在冗余机械臂末端从当前点移动到目标点时冗余机械臂各关节运动轨迹对应的轨迹函数，冗余机械臂各关节的轨迹函数可以是一个或多个，即，冗余机械臂各关节运动轨迹对应的轨迹函数可以以一个总的轨迹函数进行整体描述，也可以以每个关节的运动轨迹对应一个或几个轨迹函数的形式进行描述。在此，需要指出的是，冗余机械臂各关节的运动轨迹对应的轨迹函数可以包含等式和/或不等式，例如，冗余机械臂某一关节的轨迹函数可以包含该关节的位置坐标、速度和/或加速度与其它变量(如机械臂末端的位置坐标、速度和/或加速度)的对应关系，还可以包含冗余机械臂正常移动时该关节位置坐标、移动速度和/或移动角速度的取值范围等，此处不作限制。

具体的，在确定冗余机械臂从当前点移动到目标点的运动轨迹对应的轨迹函数时，可以首先根据冗余机械臂末端的当前点信息和目标点信息确定冗余机械臂末端的轨迹函数，然后根据冗余机械臂末端与各关节的连接关系确定各关节运动轨迹与冗余机械臂末端运动轨迹之间的关系，进而根据冗余机械臂末端的轨迹函数确定冗余机械臂各关机的轨迹函数。

S130、以冗余度空间向量为自变量建立所述轨迹函数对应的方程。

具体的，可以首先确定冗余度向量与各关节位置坐标、速度或加速度等变量的对应关系，然后根据该对应关系采用冗余度空间向量替换各关节对应的轨迹函数中的位置坐标、速度或加速度等自变量，进而建立冗余机械臂各关节以冗余度空间向量为自变量的轨迹函数对应的方程。其中，冗余度空间向量可以是冗余度向量空间的任一向量，冗余度向量空间可以是已有的向量空间，也可以是自定义的空间，此处不作限制。

S140、根据目标接近法求解所述轨迹函数对应的方程，得到所述运动轨迹对应的所述冗余机械臂中各关节的位置和速度。

本实施例中，在得到冗余机械臂各关节的位置和速度后，可以根据需要进一步求得冗余机械臂各关节的加速度。示例性的，当冗余机械臂某一关节的轨迹函数对应的方程存在有限组解时，可以求得方程的所有解，然后根据实际需要或根据设定的选取规则选取方程的一组解得到该关节在下一时刻的目标位置并同时确定该关节从当前位置移动到目标位置时的移动速度和加速度。当冗余机械臂某一关节的轨迹函数对应的方程存在无穷组解时，可以任意求得该方程的一组解并通过该组解得到该关节在下一时刻的目标位置以及从当前位置移动到目标位置的移动速度和加速度；也可以首先设定方程解的约束条件，然后求符合该约束条件的方程的解，并以该组解指导该关节从当前时刻到下一时刻的移动过程，此处不作限制。为保证冗余机械臂各关节都可以高效地移动，减少各关节的输出力矩和运动、规避奇异与避免关节运动的极限，优选的，可以根据符合约束条件的方程的解得到运动轨迹对应的冗余机械臂中各关节的位置、速度和加速度。

本发明实施例一提供的冗余机械臂的控制方法，获取冗余机械臂的当前点信息和目标点信息，根据所获取的当前点信息和目标点信息确定冗余机械臂从当前点移动到目标点的运动轨迹对应的轨迹函数，以冗余度空间向量为自变量建立该轨迹函数对应的方程，通过目标接近法求解该方程以得到运动轨迹对应的冗余机械臂中各关节的位置和速度。本实施例通过采用上述技术方案，可以在得到满足设计目标的全局最优解的前提下，减少冗余机械臂多目标优化问题的搜索空间，避免多目标求解过程中维度爆炸问题的发生，简化冗余机械臂控制过程中所需的计算量，提高冗余机械臂的反应速度。

实施例二

图2为本发明实施例二提供的一种冗余机械臂的控制方法的流程示意图。本实施例在上述实施例的基础上进行优化，进一步地，所述获取冗余机械臂所在的当前点信息和目标点信息包括：获取所述冗余机械臂末端在所述目标点的速度；根据所述冗余机械臂末端在目标当前点的速度将所述冗余机械臂的关节角、关节角速度和关节角加速度映射到冗余度空间。

进一步地，所述根据所述当前点信息和所述目标点信息确定所述冗余机械臂从当前点移动到目标点的运动轨迹对应的轨迹函数包括：根据逆运动学方程，确定所述冗余机械臂从当前点移动到目标点的运动轨迹对应的优化目标函数和约束条件。

进一步地，所述根据目标接近法求解所述轨迹函数对应的方程，得到所述运动轨迹对应的所述冗余机械臂中各关节的位置和速度包括：在冗余度空间中，根据优化目标函数、与优化目标对应的最小目标值、对应的约束条件和预设的加权系数，通过辅助向量和单目标优化算法求解所述轨迹函数对应的方程，得到所述运动轨迹对应的所述冗余机械臂中各关节的位置和速度。

相应的，如图2所示，本实施例提供的冗余机械臂的控制方法包括：

S210、获取所述冗余机械臂末端在所述目标点的速度。

优选的，所述获取所述冗余机械臂末端在所述当前点的速度包括：根据所述冗余机械臂末端在所述当前点的位姿和所述冗余机械臂末端在所述运动轨迹中的下一采样点的位姿确定所述冗余机械臂末端在所述下一采样点的速度。其中，冗余机械臂末端的位姿指的是冗余机械臂的位置和姿态，位置可以用位置坐标来表示，姿态可以用角度来表示。示例性的，可以根据安装在冗余机械臂末端的编码器获取冗余机械臂在当前点的角速度和/或角加速度等信息，或者直接根据冗余机械臂在上一时刻的计算结果得到冗余机械臂末端在当前点的角速度和/或角加速度等信息，然后根据冗余机械臂在当前点的角度和下一采样的角度得到冗余机械臂在下一采样点的角速度，进而得到冗余机械臂在下一采样点的速度。

S220、根据所述冗余机械臂末端在所述目标点的速度将所述冗余机械臂的关节角、关节角速度和关节角加速度映射到冗余度空间。

示例性的，冗余机械臂末端速度与冗余机械臂的关节角速度的关系(即冗余机械臂各关节以角速度为自变量的轨迹函数)可以表示为：

其中，为机械臂末端的速度，v_i(i＝x,y,z)表示线速度，ω_i(i＝θ,Ψ,Φ)表示角速度；为机械臂的关节角速度，N为冗余机械臂的自由度，N＝6+r，r＞1；J∈R^6×N为雅克比矩阵。

假设α是雅可比矩阵J中的任意r(r＝n-6＞1)列，使得剩下的6列构成非奇异矩阵J^*，则式(1)可以改写为：

从而，任何满足式(2)的关节角速度可以进一步改写为：

其中，是满足式(3)的一个特解，是式(3)的齐次解，且

令

其中，i＝1,2,…,r，α_j是矩阵α的第j列；为列矩阵，且其前r(r＝N-6)行为0；为列矩阵，且其第i行为1，前r行中除第i行外的其他行为0。定义冗余度空间向量k＝(k₁,…,k_r)^T为冗余度向量空间的任一向量，则有：

假设角速度以S(k)进行表示，角度θ以P(k)进行表示，角加速度以A(k)进行表示，则冗余机械臂关节角速度可表示为冗余度空间向量k的函数：

冗余机械臂关节角度θ关于冗余度空间向量k的表达式为：

冗余机械臂关节角加速度关于冗余度空间向量k的表达式为：

其中，θ₀为上一时刻的关节角度，为上一时刻的关节角速度，Δt为采样步长。

S230、根据逆运动学方程，确定所述冗余机械臂从当前点移动到目标点的运动轨迹对应的优化目标函数和约束条件。

本实施例中，冗余机械臂从当前点移动到目标点的运动轨迹对应的优化目标函数和约束条件可以根据需要灵活进行设定，例如，可以对冗余机械臂移动的范围进行限制以躲避障碍物，或者，对冗余机械臂的弯曲角度进行限制以避免各关节达到其运动极限等。考虑到各约束条件的实用性，可选的，所述优化目标函数包括所述冗余机械臂的关节运动幅度最小或者所述冗余机械臂受对应的齿轮间隙影响最小；所述约束条件包括：将所述冗余机械臂的关节速度约束在预设的速度范围内或者将所述冗余机械臂的关节角度约束在预设的角度范围内。示例性的，冗余机械臂关节运动幅度最小的优化目标函数可以为：冗余机械臂受对应的齿轮间隙影响最小的优化目标函数可以为：将冗余机械臂的关节速度约束在预设的速度范围内的约束条件可以为：将冗余机械臂的关节角度约束在预设的角度范围内的约束条件可以为：θ_min≤θ≤θ_max。

S240、以冗余度空间向量为自变量建立所述轨迹函数对应的方程。

本实施例中，可以基于冗余度空间建立冗余机械臂轨迹的优化目标函数和约束方程，由于冗余机械臂的优化目标函数和约束方程都可以表示为以关节角度、关节角速度和/或关节角加速度为自变量的表达式，因此，将上式(7)、(8)、(9)代入表达式中即可得到以冗余度空间向量k为自变量的优化目标函数和约束方程。例如，关于关节运动幅度最小的优化目标函数可以进一步表示为冗余度空间向量k的方程：

关于机械臂受对应的齿轮间隙影响最小的优化目标函数可以进一步表示为冗余度空间向量k的方程：

关于关节角速度限制的约束条件可以进一步表示为以冗余度空间向量k为自变量的方程：

关于关节角度限制的约束条件可以进一步表示为以冗余度空间向量k为自变量的方程：

S250、在冗余度空间中，根据优化目标函数、与优化目标对应的最小目标值、对应的约束条件和预设的加权系数，通过辅助向量和单目标优化算法求解所述轨迹函数对应的方程，得到所述运动轨迹对应的所述冗余机械臂中各关节的位置和速度。

示例性的，求解轨迹函数对应的方程的单目标优化算法可以根据需要灵活确定，此处不作限制。优选的，所述单目标优化算法包括：牛顿-欧拉算法、Nelder-Mead单纯形算法、内点算法、基因算法和/或Pattern Search算法。

帕累托前沿是多目标优化问题的解集，其定义如下：在多目标可行域Ω上，存在一点x^*∈Ω，若不存在点x∈Ω，使得f(x)≤f(x^*)成立，且至少存在一点x′∈Ω使严格不等式f(x′)＞f(x^*)成立，则x^*为一个帕累托最优解。以双目标优化问题为例，其轨迹函数对应的方程如下：

上式中，F(x)＝{f₁(x) f₂(x)}为优化目标函数，为与优化目标函数对应的最小目标值，A为矩阵约束条件，b为向量约束条件，ω为加权系数向量，γ为辅助向量。其中，加权系数向量ω可由用户或开发商根据需要进行设置，辅助向量γ用于将原来的多目标优化为题转换为单目标优化问题。

本实施例中，可试用内点法求解式(12)得到方程的最优解，进而通过式(7)、式(8)和式(9)得到冗余机械臂各关节与该最优解对应的角度、角速度和角角速度，从而得到冗余机械臂各关节在下一时刻的位置和速度。

以九自由度的冗余机械臂为例(假设该九自由度冗余机械臂的D-H坐标如图3所示，该九自由度冗余机械臂连杆D-H参数表如表1所示)，求解冗余机械臂各关节位置和速度的过程可以为：

表1

θi	di(mm)	αi	ai(mm)	范围
					0	d1	90°	0	1550≤d1≤8276
θ2	0	0	a2＝2200	-90≤θ2≤90
					θ3	0	-90°	0	-90≤θ3≤90
θ4	d4＝2800	90°	a4＝-375	-180≤θ4≤180
					θ5	0	-90°	a5＝-a4＝375	-180≤θ5≤180
θ6	d4＝2800	90°	0	-180≤θ6≤180
					θ7	0	-90°	0	-90≤θ7≤90
θ8	d8＝1650	90°	0	-90≤θ8≤90
					θ9	0	0	ah＝350	-90≤θ9≤90

首先，把冗余机械臂末端的速度转换为以连杆3的坐标系为参考：

然后，定义腕部坐标系。考虑到计算的简便性，可以根据冗余机械臂的结构特点定义腕部坐标系，其中，腕部坐标系的具体坐标与方向可以根据需要灵活设置，此处不作限定。示例性的，可以定义腕部坐标系为与冗余机械臂末端坐标系平行、原点与连杆8坐标系原点重合的坐标系，此时，当腕部和冗余机械臂末端的速度以连杆3坐标系为参考时，二者的角速度和线速度存在如下关系：

³ω_w＝³ω_h (15)

³v_w＝³v_h-³R_w(^wω_h×^wP_h)＝³v_h-³ω_h׳P_h＝³v_h-³ω_h×a_h ³x_h (16)

其中，³ω_w是以连杆3坐标系为参考的腕部坐标系中的角速度；³ω_h是以连杆3坐标系为参考的冗余机械臂末端坐标系中的角速度；³v_w是以连杆3坐标系为参考的腕部坐标系中的线速度；³v_h是以连杆3坐标系为参考的冗余机械臂末端坐标系中的线速度；³x_h是以连杆3坐标系为参考的冗余机械臂末端坐标系中Z方向的单位矢量的值：

上式中，c_i表示cos(θ_i)，s_i表示sin(θ_i)，c_ij表示cos(θ_i+θ_j)，s_ij表示sin(θ_i+θ_j)。

定义³J_w为联系关节角速度与腕部速度的雅克比矩阵，则有：

用矢量积法可求出该雅克比矩阵如下：

³J_w＝[J₁ J₂ J₃ J₄ J₅ J₆ J₇ J₈ J₉] (19)

其中，

本实施例中，可在式(19)中取任意三列作为α，J^*为除组成α的三列之外的其余列。示例性的，取J₅、J₆和J₇作为α，其余列(J₁、J₂、J₃、J₄、J₈和J₉)组成J^*，通过式(5)可以得到：

其中，

J₂₁＝a₂s₃-d₆c₅-d₄-a₅s₅-d₇c₅c₇+d₇s₅c₆s₇，

J₂₃＝a₂c₃+a₄c₄+a₅c₄c₅-d₆c₄s₅-d₇c₄s₅c₇+d₇s₄s₆s₇-d₇c₄c₅c₆s₇，

J₃₁＝-d₄-d₇(c₅c₇-s₅c₆s₇)-d₆c₅-a₅s₅，

J₃₃＝a₄c₄+d₇(s₇(s₄s₆-c₄c₅c₆)-c₄s₅c₇)+a₅c₄c₅-d₆c₄s₃，

J₄₁＝d₇(s₇(c₄s₆+s₄c₅c₆)+s₄s₅c₇)-a₄s₄-a₅s₄c₅+d₆s₄s₅，

J₄₂＝d₇(s₇(s₄s₆-c₄c₅c₆)-c₄s₅c₇)+a₄c₄+a₅c₄c₅-d₆c₄s₅，

J₈₄＝s₇(s₄s₆-c₄c₅c₆)-c₄s₅c₇，

J₈₅＝-s₇(c₄s₆+s₄c₅c₆)-s₄s₅c₇，

J₈₆＝c₅c₇-s₅c₆s₇，

J₉₄＝c₈(s₄c₆+c₄c₅s₆)-s₈(c₇(s₄s₆-c₄c₅c₆))+c₄s₅s₇，

J₉₅＝s₈(c₇(s₄s₆+s₄c₅c₆))-s₄s₅s₇-c₈(c₄c₆-s₄c₅s₆)，

J₉₆＝s₈(c₅s₇-s₅c₆c₇)+c₈s₅s₆。

令和由式(20)可得：

其中，为矩阵的第i个元素。

由式(6)可得：

其中，α＝(J₅ J₆ J₇)。

因此，将式(21)至(26)中的(j＝1,2,3,4,8和9)用(i＝5,6和7，j＝1,2,3,4,8和9)代替，将式(21)至(26)中的(k＝1,2,3,4,5和6)用-J_ik(i＝5,6和7，k＝1,2,3,4,5和6)代替即可得到和的值。以为例，将式(21)至(26)中的(j＝1,2,3,4,8和9)用(j＝1,2,3,4,8和9)代替，可以得到：

将式(21)至(26)中的(k＝1,2,3,4,5和6)用-J_5k(k＝1,2,3,4,5和6)代替，可得到：

由式(19)可知因此可得：

其中，为矩阵的第i个元素。同理可以得出：

其中，

将关节角速度表示为冗余度向量(k₅,k₆,k₇)的函数：

将式(28)代入式(8)中，可以得到关节角度θ以冗余度向量(k₅,k₆,k₇)为自变量的函数：

其中，θ₀是当前关节角度，Δt是采样步长。

将式(28)代入式(10-1)、将式(29)代入式(10-2)可以得到关节运动幅度最小以及机械臂受对应的齿轮间隙影响最小的方程分别为：

其中，H_i(θ(k₅,k₆,k₇))是连杆变换矩阵⁰T_i(θ(k₅,k₆,k₇))中第二行第四列对应的元素：

⁰T_i(θ(k₅,k₆,k₇))＝⁰T₁(d₁)·¹T₂(θ₂)L ^i-1T_i(θ_i),(i＝1,L,9) (32)

其中，^i-1T_i(θ_i)是Denavit-Hartenberg矩阵：

其中，d_i、α_i和a_i是Denavit-Hartenberg参数，其具体数值如表1所示。

将式(28)代入式(11-1)、将式(29)代入式(11-2)可以得到关节角速度限制以及关节角度限制的约束条件为：

由式(33)可得：

由式(34)可以进一步得到式(12)中的矩阵A和向量b如下：

从而，多目标优化问题可以转化为：

由式(30)、(31)和(35)可以求得冗余度向量(k₅,k₆,k₇)的最优值，将冗余度向量(k₅,k₆,k₇)的最优值代入式(28)和式(29)即可得到冗余机械臂各关节在下一采样点的角速度和角度，从而得到冗余机械臂各关节的位置和速度。

本发明实施例二提供的冗余机械臂的控制方法，获取冗余机械臂末端在目标点的速度，根据冗余机械臂末端在目标点的速度将冗余机械臂的关节角、关节角速度和关节角加速度映射到冗余度空间，根据逆运动学方程确定冗余机械臂从当前点移动到目标点的运动轨迹对应的优化目标函数和约束条件，以冗余度空间向量为自变量建立运动轨迹对应的方程，通过辅助向量和单目标优化算法求解轨迹函数对应的方程，得到运动轨迹对应的冗余机械臂中各关节的位置和速度。本实施例通过采用上述技术方案，建立多目标约束优化问题的数学模块，可以为求解多目标优化问题提供理论基础，在得到满足设计目标的全局最优解的前提下，减少冗余机械臂多目标优化问题的搜索空间，避免多目标求解过程中维度爆炸问题的发生，简化冗余机械臂控制过程中所需的计算量，提高冗余机械臂的反应速度。

实施例三

本发明实施例三提供一种冗余机械臂的控制装置。该装置可由硬件和/或软件实现，一般集成在用于控制机器人的控制模块中，可通过执行冗余机械臂的控制方法实现对冗余机械臂的控制。图4所示为本实施例提供的冗余机械臂的控制装置的结构框图，如图4所示，该装置包括：

信息获取单元410，用于获取冗余机械臂所在的当前点信息和目标点信息；

轨迹单元420，与所述信息获取单元相连，用于根据所述当前点信息和所述目标点信息确定所述冗余机械臂从当前点移动到目标点的运动轨迹对应的轨迹函数；

冗余度处理单元430，与所述轨迹单元相连，用于以冗余度空间向量为自变量建立所述轨迹函数对应的方程；根据目标接近法求解所述轨迹函数对应的方程，得到所述运动轨迹对应的所述冗余机械臂中各关节的位置和速度。

本发明实施例三提供的冗余机械臂的控制装置，通过信息获取单元获取冗余机械臂的当前点信息和目标点信息，通过轨迹单元根据所获取的当前点信息和目标点信息确定冗余机械臂从当前点移动到目标点的运动轨迹对应的轨迹函数，通过冗余度处理单元以冗余度空间向量为自变量建立该轨迹函数对应的方程，通过目标接近法求解该方程以得到运动轨迹对应的冗余机械臂中各关节的位置和速度。本发明实施例通过采用上述技术方案，可以在得到满足设计目标的全局最优解的前提下，减少冗余机械臂多目标优化问题的搜索空间，避免多目标求解过程中维度爆炸问题的发生，简化冗余机械臂控制过程中所需的计算量，提高冗余机械臂的反应速度。

进一步地，所述信息获取单元410具体用于：获取所述冗余机械臂末端在所述目标点的速度；根据所述冗余机械臂末端在所述目标点的速度将所述冗余机械臂的关节角、关节角速度和关节角加速度映射到冗余度空间。

进一步地，所述获取所述冗余机械臂末端在所述当前点的速度包括：根据所述冗余机械臂末端在所述当前点的位姿和所述冗余机械臂末端在所述运动轨迹中的下一采样点的位姿确定所述冗余机械臂末端在所述下一采样点的速度。

进一步地，所述轨迹单元420具体用于根据逆运动学方程，确定所述冗余机械臂从当前点移动到目标点的运动轨迹对应的优化目标函数和约束条件。

进一步地，所述优化目标函数包括所述冗余机械臂的关节运动幅度最小或者所述冗余机械臂受对应的齿轮间隙影响最小；所述约束条件包括：将所述冗余机械臂的关节速度约束在预设的速度范围内或者将所述冗余机械臂的关节角度约束在预设的角度范围内。

进一步地，所述根据目标接近法求解所述轨迹函数对应的方程，得到所述运动轨迹对应的所述冗余机械臂中各关节的位置和速度包括：在冗余度空间中，根据优化目标函数、与优化目标对应的最小目标值、对应的约束条件和预设的加权系数，通过辅助向量和单目标优化算法求解所述轨迹函数对应的方程，得到所述运动轨迹对应的所述冗余机械臂中各关节的位置和速度。

进一步地，所述单目标优化算法包括：牛顿-欧拉算法、Nelder-Mead单纯形算法、内点算法、基因算法和/或Pattern Search算法。

本实施例提供的冗余机械臂的控制装置可执行本发明任意实施例提供的冗余机械臂的控制方法，具备执行冗余机械臂的控制方法相应的功能模块和有益效果。未在本实施例中详尽描述的技术细节，可参见本发明任意实施例所提供的冗余机械臂的控制方法。

注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。

Claims (10)

1.一种冗余机械臂的控制方法，其特征在于，包括：

获取冗余机械臂所在的当前点信息和目标点信息；

根据所述当前点信息和所述目标点信息确定所述冗余机械臂从当前点移动到目标点的运动轨迹对应的轨迹函数；

以冗余度空间向量为自变量建立所述轨迹函数对应的方程；

根据目标接近法求解所述轨迹函数对应的方程，得到所述运动轨迹对应的所述冗余机械臂中各关节的位置和速度。

2.根据权利要求1所述的冗余机械臂的控制方法，其特征在于，所述获取冗余机械臂所在的当前点信息和目标点信息包括：

获取所述冗余机械臂末端在所述目标点的速度；

根据所述冗余机械臂末端在所述目标点的速度将所述冗余机械臂的关节角、关节角速度和关节角加速度映射到冗余度空间。

3.根据权利要求2所述的冗余机械臂的控制方法，其特征在于，所述获取所述冗余机械臂末端在所述当前点的速度包括：

根据所述冗余机械臂末端在所述当前点的位姿和所述冗余机械臂末端在所述运动轨迹中的下一采样点的位姿确定所述冗余机械臂末端在所述下一采样点的速度。

4.根据权利要求1或2所述的冗余机械臂的控制方法，其特征在于，所述根据所述当前点信息和所述目标点信息确定所述冗余机械臂从当前点移动到目标点的运动轨迹对应的轨迹函数包括：

根据逆运动学方程，确定所述冗余机械臂从当前点移动到目标点的运动轨迹对应的优化目标函数和约束条件。

5.根据权利要求4所述的冗余机械臂的控制方法，其特征在于，所述优化目标函数包括所述冗余机械臂的关节运动幅度最小或者所述冗余机械臂受对应的齿轮间隙影响最小；所述约束条件包括：将所述冗余机械臂的关节速度约束在预设的速度范围内或者将所述冗余机械臂的关节角度约束在预设的角度范围内。

6.根据权利要求5所述的冗余机械臂的控制方法，其特征在于，所述根据目标接近法求解所述轨迹函数对应的方程，得到所述运动轨迹对应的所述冗余机械臂中各关节的位置和速度包括：

在冗余度空间中，根据优化目标函数、与优化目标对应的最小目标值、对应的约束条件和预设的加权系数，通过辅助向量和单目标优化算法求解所述轨迹函数对应的方程，得到所述运动轨迹对应的所述冗余机械臂中各关节的位置和速度。

7.根据权利要求6所述的冗余机械臂的控制方法，其特征在于，所述单目标优化算法包括：牛顿-欧拉算法、Nelder-Mead单纯形算法、内点算法、基因算法和/或Pattern Search算法。

8.一种冗余机械臂的控制装置，其特征在于，包括：

信息获取单元，用于获取冗余机械臂所在的当前点信息和目标点信息；

轨迹单元，与所述信息获取单元相连，用于根据所述当前点信息和所述目标点信息确定所述冗余机械臂从当前点移动到目标点的运动轨迹对应的轨迹函数；

冗余度处理单元，与所述轨迹单元相连，用于以冗余度空间向量为自变量建立所述轨迹函数对应的方程；根据目标接近法求解所述轨迹函数对应的方程，得到所述运动轨迹对应的所述冗余机械臂中各关节的位置和速度。

9.根据权利要求8所述的冗余机械臂的控制装置，其特征在于，所述信息获取单元具体用于：获取所述冗余机械臂末端在所述当前点的速度；根据所述冗余机械臂末端在所述当前点的速度将所述冗余机械臂的关节角、关节角速度和关节角加速度映射到冗余度空间。

10.根据权利要求8或9所述的冗余机械臂的控制装置，其特征在于，所述轨迹单元具体用于根据逆运动学方程，确定所述冗余机械臂从当前点移动到目标点的运动轨迹对应的优化目标函数和约束条件。