CN112605996B - 一种面向冗余机械臂的无模型碰撞避免控制方法 - Google Patents

Abstract

本发明涉及一种面向冗余机械臂的无模型碰撞避免控制方法，包括：S1：建立冗余机械臂的逆向运动学方程；并建立碰撞避免的约束不等式；S2：将冗余机械臂的逆向运动学问题表示为微分方程形式；S3：建立估计雅克比矩阵的微分方程；估计冗余机械臂末端的雅克比矩阵的估计值和冗余机械臂临界点处的雅克比矩阵估计值；S4：确定冗余机械臂的每个关节的角度随时间变化的函数；将关节角度‑时间信息输入该函数，即可让机械臂按照躲避障碍物的轨迹运动。本发明所述方法适应于各种类型冗余机械臂的准确的碰撞避免控制。

Description

一种面向冗余机械臂的无模型碰撞避免控制方法

技术领域

本发明涉及冗余机械臂碰撞避免控制方法领域，更具体地，涉及一种面向冗余机械臂的无模型碰撞避免控制方法。

背景技术

在现代制造业中，冗余机械臂已经得到了广泛的运用。而冗余机械臂的碰撞避免控制则是一种非常常见的应用场景。在冗余机械臂的任务轨迹中，经常会需要躲避各种各样的环境障碍物，如进行外科手术、组装零件等。即使无环境障碍物，碰撞避免控制也可用于躲避冗余机械臂自身或基座、躲避奇异构形或改善当前构形等。目前对于冗余机械臂的碰撞避免控制算法已经有许多研究，主要方向有两种，一是控制使得冗余机械臂与障碍物之间的距离最大化，二是将无碰撞要求视为一个等式约束，即为易受碰撞的冗余机械臂连杆产生逃离障碍物的速度，但是这些碰撞避免控制算法都需要基于冗余机械臂模型进行控制。

目前，有文献中提出的基于线性变分不等式的原始对偶神经网络二次规划冗余机械臂碰撞避免控制方法。该方法提出了以下机械臂的控制方案：

约束条件为：

其中，

是待实时求解的决策变量，也即冗余机械臂的关节的角速度，n是冗余机械臂的自由度。系数矩阵W是单位矩阵。系数向量q＝4(θ-θ(0))，其中θ(0)是每个关节的初始角度，θ是t时刻的关节角度。J_e是冗余机械臂末端的雅可比矩阵。

是冗余机械臂的末端执行器在任务空间的期望轨迹的时间导数，也即末端执行器的速度。ξ^-和ξ⁺分别表示冗余机械臂的关节的物理极限。sgn函数为符号函数，其输出值为1或-1。J_c是冗余机械臂临界点处的雅可比矩阵。d₂为警戒距离，当机械臂与障碍物的距离小于该值时开始避障计算。d₁为极限距离，我们一般认为当机械臂与障碍物的距离小于该值时发生碰撞。

可使用许多方法求解该线性变分不等式问题，上述文献使用了原对偶神经网络二次规划来将该线性变分不等式转化为微分方程形式，然后使用通用微分方程求解器来对其进行求解。

但是，现有技术要求知晓约束条件中冗余机械臂末端的雅可比矩阵J_e和临界点处的雅可比矩阵J_c的解析式，以求得每个时刻的雅可比矩阵J_e和J_c的精确值，才能确定每个时刻的约束条件，从而得到二次规划在每个时刻的解。该方法的解的精度和正确性会受到雅可比矩阵表达式的直接影响。但是即使是同一批次、型号的机械臂，雅可比矩阵表达式也可能由于机械臂的组装差异而各自不同，在我们使用冗余机械臂的正向运动学模型推导出的雅可比矩阵表达式可能与实际的雅可比矩阵表达式有偏差，致在应用该算法时产生误差，影响解的精确度。其次，冗余机械臂的型号繁多，对于一种机械臂，就需要重新计算一次其雅可比矩阵表达式，而有些机械臂的正向运动学模型难以测算，导致该方法的可移植性不强。最后，冗余机械臂的临界点在整个碰撞避免运动过程中会不断变化，这使得其雅可比矩阵表达式也需要不断重新计算，这就给控制带来了很多的不便。

2018年11月9日公开的中国专利CN108772835A公开了一种障碍物与物理极限躲避方法，包括如下步骤：依据目标系统的物理模型，列写其运动学方程表达式；求解目标系统末端执行器的雅克比矩阵；依据目标系统与障碍物之间的关系，定义其障碍物躲避约束；将障碍物躲避约束进行优化，设计基于矢量的障碍物躲避方法；依据目标系统的物理模型，设定其物理极限约束表达式；将基于矢量的障碍物躲避方法与物理极限约束表达式合并，并结合末端执行器的雅克比矩阵，设计目标系统的基于矢量的障碍物与物理极限躲避方法；进行变换得到与其等价的线性微分不等式；进行变换得到与其等价的线性投影方程；将线性投影方程代入原对偶神经网络求解器进行解算，即可完成目标系统对障碍物与物理极限的躲避。CN108772835A是在已知物理模型和运动学情况下实现物理极限的躲避。只能适应特定运动学方程下的冗余机械臂，不能适应多种类型冗余机械臂。

发明内容

本发明为克服上述现有技术所述的机械臂碰撞避免控制方法不能适应多种类型冗余机械臂的缺陷，提供一种面向冗余机械臂的无模型碰撞避免控制方法。

所述方法包括以下步骤：

S1：建立冗余机械臂的逆向运动学方程；并建立碰撞避免的约束不等式；

S2：将冗余机械臂的逆向运动学问题表示为微分方程形式；得到冗余机械臂的逆向运动学微分方程；

S3：建立估计雅克比矩阵的微分方程；估计冗余机械臂末端的雅克比矩阵的估计值和冗余机械臂临界点处的雅克比矩阵估计值；

S4：联立冗余机械臂的逆向运动学微分方程、冗余机械臂末端的雅克比矩阵的估计值、冗余机械臂临界点处的雅克比矩阵估计值，得到在碰撞避免运动中冗余机械臂在任意时刻各关节的期望位置，从而控制冗余机械臂完成无模型碰撞避免任务。

优选地，S1中所述冗余机械臂的逆向运动学方程具体为：

其中，

是待实时求解的决策变量，也即冗余机械臂的关节的角速度。n是冗余机械臂的自由度。系数矩阵W是单位矩阵。系数向量q＝4(θ-θ(0))，其中θ(0)是每个关节的初始角度，θ是t时刻的关节角度。W是n*n维的单位矩阵

优选地，S1中所述碰撞避免的约束不等式为：

其中，J_e是冗余机械臂末端的雅可比矩阵。

是冗余机械臂的末端执行器在任务空间的期望轨迹的时间导数，也即末端执行器的速度。

矩阵A和向量b分别由下式推导得出：

sgn函数为符号函数，其输出值为1或-1。J_c是冗余机械臂临界点处的雅可比矩阵。d₂为警戒距离，当机械臂与障碍物的距离小于该值时开始避障计算。d₁为极限距离，我们一般认为当机械臂与障碍物的距离小于该值时发生碰撞。x_o、y_o、z_o分别为障碍物点O在x轴、y轴、z轴的坐标；x_c、y_c、z_c分别为机械臂临界点C在x轴、y轴、z轴的坐标；

ξ^-和ξ⁺都是n维向量，分别表示冗余机械臂的关节的物理最小极限和最大极限。

优选地，假设冗余机械臂的关节具有速度极限和角度极限：

θ^-≤θ≤θ⁺

则ξ^-的具体定义如下：

β是调节参数，用于调节关节极限转换之后的可行域的大小。θ^-、θ⁺为关节角度的最小、最大极限，

为关节角速度的最小、最大极限。

优选地，ξ⁺的具体定义如下：

优选地，S2具体为使用原对偶神经网络方法将冗余机械臂的逆向运动学问题表示为微分方程形式。

优选地，S2所述冗余机械臂的逆向运动学微分方程为：

其中，y是待求解的变量，是一个有着m+n+3个元素的向量。实际上，y是上述二次规划问题中

的对偶变量，其前n个元素即为

γ是原对偶神经网络的收敛率参数，I是一个行数和列数都为m+2n的单位矩阵。系数向量q＝4(θ-θ(0))；P(·)为投影函数；m是冗余机械臂的末端的任务空间的维度；

矩阵Q和向量p的定义如下：

优选地，投影函数P(·)的参数是一个向量，这个投影函数对输入的向量v_i×1的每一个元素做上下限的约束，第i个元素的上下限的具体值分别由

和

指定。

优选地，冗余机械臂末端的雅克比矩阵的估计值和冗余机械臂临界点处的雅克比矩阵估计值采用零化神经动力学方法来估计。

优选地，S3具体为：

设冗余机械臂的运动方程为如下的向量等式：

f(θ(t))＝r_a(t)

r_a(t)表示冗余机械臂的某点在t时刻的实际位置，θ(t)是冗余机械臂的每个关节在t时刻的角度，f函数是冗余机械臂的正向运动学函数，将冗余机械臂的关节状态映射到冗余机械臂的某点位置。对以上等式两端同时对时间t求导，有：

为了保持简便，使用J来代替J(θ)。使用

来替代

定义一个误差函数∈来表示计算J时的误差为：

对其应用零化神经动力学方法，令：

其中μ为用于调节收敛率的正参数，

为∈关于时间t的导数。将

代入

中，则可以得到以下等式：

对

进行变形，得到：

其中，

表示J对时间的导数，

表示冗余机械臂的末端的实际加速度，

表示冗余机械臂各关节的加速度，

表示冗余机械臂的末端的实际速度，

表示冗余机械臂各关节的速度，

依据m和n的值表示

的逆或伪逆。

可用于估算冗余机械臂上某点雅可比矩阵，通过对该微分方程进行求解，即可求得每个时刻t的雅可比矩阵的估计值。

对于式中的

可以通过传感器或视觉仪器求得，雅可比矩阵估计方程

是无模型限制的，无论是J_e还是J_c都可以用该雅可比矩阵估计方程进行估计。故：

其中r_a1、r_a2分别表示在末端执行器和临界点处的实际轨迹，y为

的对偶变量。

与现有技术相比，本发明技术方案的有益效果是：

本发明通过引入雅可比矩阵估计方法，使得无需预先知晓冗余机械臂的正向运动学模型也可解决冗余机械臂的逆向运动学问题，从而完成适应于各种类型冗余机械臂的准确的碰撞避免控制。

附图说明

图1为实施例1所述面向冗余机械臂的无模型碰撞避免控制方法计算流程示意图。

图2为冗余机械臂在球形障碍物时的运动轨迹仿真图(碰撞半径已加上机械臂厚度)。

图3为冗余机械臂在球形障碍物碰撞避免控制时的运动轨迹仿真图(碰撞半径已加上机械臂厚度)。

图4为冗余机械臂末端在球形障碍物碰撞避免控制时的误差情况。

图5为机械臂在球形障碍物碰撞避免控制时的初始状态。

图6为冗余机械臂在球形障碍物无控制时的运动轨迹图。

图7为机械臂在窗形障碍物碰撞避免控制时的初始状态。

图8为冗余机械臂在窗形障碍物无控制时的运动轨迹图。

图9为冗余机械臂在窗形障碍物时的运动轨迹仿真图。

图10为冗余机械臂在窗形障碍物碰撞避免控制时的运动轨迹仿真图。

图11为冗余机械臂末端在窗形障碍物碰撞避免控制时的误差情况。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；

为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；

对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

在阐述具体实施例之前首先对与本实施例相关的缩略语和关键术语定义进行说明：

冗余机械臂：冗余机械臂是指末端执行器在执行给定任务时有超出所需自由度的机械臂。

机器人正向运动学：机器人正向运动学主要研究在给定了关节机械臂各关节的位姿的情况下，机械臂的末端执行器会如何运动的问题。

机器人逆向运动学：机器人逆向运动学主要研究在给定了关节机械臂的末端执行器的期望运动轨迹的情况下，各关节要如何运动才能让末端执行器的实际运动轨迹和期望运动轨迹相一致的问题。

二次规划：二次规划指一种优化问题，通常具有以下形式：

subject to some Conditions

其中，x(t)是一个向量，其每个元素都是时间t的函数。要在一系列约束条件下使得y(t)最小情况下的x(t)。

机械臂的运动学模型：机械臂的运动学模型一般指正向运动学模型，含有机械臂各连杆之间的相对位置等信息。

原对偶神经网络：原对偶神经网络是一种结构简单的循环神经网络，可用于求解二次规划问题。可以将二次规划问题表示为原对偶神经网络的动态系统方程，即微分方程，该微分方程的解即为二次规划问题的解。

碰撞避免控制方法：碰撞避免控制方法是指在关节机械臂附近存在障碍物时，既保证机械臂的末端执行器按照期望运动轨迹，又保证机械臂与障碍物之间保持一定的安全距离，避免碰撞的发生。

机械臂的临界点：机械臂的临界点指机械臂上离障碍物最近的一处，临界点可在运动过程中不断变化。

下面结合附图和实施例对本发明的技术方案做进一步的说明。

实施例1：

本发明提供了一种面向冗余机械臂的无模型碰撞避免控制方法，使冗余机械臂的末端执行器能够按照预设的路径运动，并且使冗余机械臂在运动过程中可以躲避空间中存在的障碍物，避免碰撞的发生，良好地完成任务。

本实施例所提出的冗余机械臂碰撞避免控制方法可简要归纳如下：

(一)建立求解逆向运动学的约束等式。首先将逆向运动学问题表示为二次规划问题，然后将冗余机械臂各关节的速度极限、角度极限和末端执行器的期望轨迹表示为二次规划问题的约束。

(二)建立碰撞避免的约束不等式。使用从传感器或视觉仪器得到的障碍物位置，障碍物和冗余机械臂之间距离等条件计算得出冗余机械臂的临界点，并表示为二次规划问题的不等式约束。

(三)建立估计末端执行器和临界点处雅可比矩阵的微分方程，需要分别获取冗余机械臂的末端执行器和临界点的速度、加速度以及冗余机械臂各关节的角速度、角加速度。在计算步长足够小的情况下，临界点变化的幅度并不大，因此我们可以将上一步估算得出的临界点雅可比矩阵近似用于计算。

(四)将步骤(一)、(二)中建立二次规划约束转换成微分方程，该微分方程需要使用末端执行器和临界点雅可比矩阵作为参数，而步骤(三)中的微分方程可以估计出两个雅可比矩阵的值。因此，可以将(一)、(二)和(三)中的微分方程联合求解。该微分方程组的解就是冗余机械臂的每个关节的角度随时间变化的函数。将关节角度-时间信息输入冗余机械臂，即可让机械臂按照躲避障碍物的轨迹运动。

本实施例所述方法的详细步骤如下：

首先使用二次规划方法来求解冗余机械臂的逆向运动学问题：

约束条件为：

其中，

是待实时求解的决策变量，也即冗余机械臂的关节的角速度。m是冗余机械臂的末端的任务空间的维度，比如在一个轨迹跟踪任务中，只规定了冗余机械臂末端的在3维空间中的期望位置，那么m＝3，而n是冗余机械臂的自由度。系数矩阵W是n*n维的单位矩阵。系数向量q＝4(θ-θ(0))，其中θ(0)是每个关节的初始角度，θ是t时刻的关节角度。I_e是冗余机械臂末端的雅可比矩阵。

是冗余机械臂的末端执行器在任务空间的期望轨迹的时间导数，也即末端执行器的速度，W是n*n维的单位矩阵

矩阵A和向量b分别由下式推导得出：

sgn函数为符号函数，其输出值为1或-1。J_c是冗余机械臂临界点处的雅可比矩阵。d₂为警戒距离，当机械臂与障碍物的距离小于该值时开始避障计算。d₁为极限距离，我们一般认为当机械臂与障碍物的距离小于该值时发生碰撞，x_o、y_o、z_o分别为障碍物点O在x轴、y轴、z轴的坐标；x_c、y_c、z_c分别为机械臂临界点C在x轴、y轴、z轴的坐标。

ξ^-和ξ⁺都是n维向量，分别表示冗余机械臂的关节的物理极限。假设冗余机械臂的关节具有速度极限和角度极限：

θ^-≤θ≤θ⁺

ξ^-和ξ⁺的具体定义如下：

β是一个参数，用于调节关节极限转换之后的可行域的大小，θ^-、θ⁺为关节角度的最小、最大极限，

为关节角速度的最小、最大极限。在本次仿真中β的值设为0.25。使用原对偶神经网络来把上述二次规划问题变为微分方程形式：

其中，y是待求解的变量，是一个有着m+n+3个元素的向量。实际上，y是上述二次规划问题中

的对偶变量，其前n个元素即为

因此，求解这条微分方程就等于求解上述的二次规划问题，从而能解决冗余机械臂的碰撞避免控制问题，m是冗余机械臂的末端的任务空间的维度；。

在此微分方程中，γ是原对偶神经网络的收敛率参数，此值越大，冗余机械臂进行碰撞避免后的轨迹精度就越高。在本次仿真中，γ取的值为10000。I是一个行数和列数都为m+2n的单位矩阵。系数向量q＝4(θ-θ(0))。投影函数P(·)、矩阵Q和向量p的定义如下：

投影函数P(·)的参数是一个向量，这个投影函数对输入的向量v_i×1的每一个元素做上下限的约束，第i个元素的上下限的具体值分别由

和

指定。

在上述流程中，雅可比矩阵J_e和J_c本来是需要根据冗余机械臂的正向运动学模型来推算的，但是为了让本发明提出的方法不需要使用冗余机械臂的正向运动学的先验模型，我们使用了零化神经动力学方法(ZNN)来估计两个雅可比矩阵的估计值，并在计算中代替J_e和J_c。下面介绍这一方法。

冗余机械臂的运动方程可写为如下的向量等式：

f(θ(t))＝r_a(t) (1)

r_a(t)表示冗余机械臂的某点在t时刻的实际位置，θ(t)是冗余机械臂的每个关节在t时刻的角度，f函数是冗余机械臂的正向运动学函数，将冗余机械臂的关节状态映射到冗余机械臂的某点位置。对以上等式两端同时对时间t求导，有：

为了保持简便，在之后的内容里，我们使用I来代替I(θ)。我们定义一个误差函数∈来表示计算J时的误差为：

对其应用零化神经动力学方法，令：

其中μ为用于调节收敛率的正参数，

为∈关于时间t的导数。将(4)带入(5)中，则可以得到以下等式：

对(5)进行变形，我们就可以得到：

其中，

表示J对时间的导数，

表示冗余机械臂的末端的实际加速度，

表示冗余机械臂各关节的加速度，

表示冗余机械臂的末端的实际速度，

表示冗余机械臂各关节的速度，

依据m和n的值表示

的逆或伪逆。(6)式可用于估算冗余机械臂上某点雅可比矩阵，通过对该微分方程进行求解，即可求得每个时刻t的雅可比矩阵的估计值。对于式中的

我们可以通过传感器或视觉仪器求得，因此我们所得到的雅可比矩阵估计方程是无模型限制的。并且无论是J_e还是J_c，我们都可以用此方法进行估计。

最后，将三条微分方程联合求解：

其中r_a1、r_a2分别表示在末端执行器和临界点处的实际轨迹，y也即

的对偶变量。我们即可求得在碰撞避免运动中冗余机械臂在t时刻各关节的期望位置，从而控制冗余机械臂完成无模型碰撞避免任务。

本实施例所使用的是一个仅利用逃逸速度方向进行冗余机械臂避障的不等式约束，这种基于不定性的判据被证明是一种变幅值逃逸率方法。但是，目前使用这一方向的控制方法仍是基于冗余机械臂模型的。而无模型的碰撞避免控制算法不依赖于机械手的显式动力学模型和运动学模型，从而避免了在实际中对机器人进行精确建模，实现和调试都较为方便。

本实施例提出的碰撞避免控制方法属于基于反馈控制的无模型碰撞避免控制算法，可以控制冗余机械臂末端以特定轨迹进行运动，在躲避各种障碍物的同时顺利完成各种任务。

下面结合图形，以在KinovaJaco2机械臂上进行冗余机械臂碰撞避免控制仿真实验作为例子，对本发明提出的方法进行进一步说明。

1.球形障碍物碰撞避免控制仿真。

(1)设置冗余机械臂的初始姿态为：

[-1.659；4.474；1.665；4.195；1.740；2.309]

每个值从左到右依次表示冗余机械臂从基座到末端的每个关节的角度，单位为rad。在Matlab中调用KinovaJaco2机械臂提供的API，将关节角度发送到机械臂的控制器，机械臂将运动到该位置。此时机械臂的状态如图5所示：

障碍物球心为[-0.07，-0.48，0.16]，警戒距离d₂为0.09 m，极限距离d₁为0.065m。

定义冗余机械臂末端在三维空间中的期望轨迹：

x＝x₀+r·cos(2π(sin²(πt/2T)))-r

y＝y₀+r·cos(π/6)·sin(2π(sin²(πt/2T)))

z＝z₀+r·sin(π/6)·sin(2π(sin²(πt/2T)))

其中，r＝0.04m，t表示当前时刻，T表示碰撞避免任务的总时长，T＝10。x₀、y₀和z₀分别表示冗余机械臂末端的初始位置。该轨迹如图6所示，实际轨迹较好地贴合了目的轨迹。

(2)在定义了冗余机械臂初始状态和期望轨迹后，按照图1所示的算法流程，建立三条微分方程：

(2.1)在第一条方程中，y是待求解的变量，它是一个m+n+3维的向量。其中m表示冗余机械臂的末端任务空间的维度，在本次仿真中，我们只规定了三维空间中的位置，因此m＝3。而n是冗余机械臂的自由度。γ是原对偶神经网络的收敛率参数，此值越大，冗余机械臂进行轨迹跟踪的精度就越高。在实验中，γ取的值为10000。I是一个行数和列数都为m+n的单位矩阵。投影函数P(·)、矩阵Q和向量p的定义如下：

q＝4(θ-θ(0))

是冗余机械臂末端执行器在时刻t时，在任务空间中的期望位置对时间的导数，即在任务空间中的期望速度。投影函数P(·)的参数是一个向量，这个投影函数对输入的向量v_i×1的每一个元素做上下限的约束，第i个元素的上下限的具体值分别由

和

指定。

和

由以下方法求得。假设冗余机械臂的关节具有速度极限和角度极限：

θ^-≤θ≤θ⁺

则这两个约束可以通过关节极限转换统一化为一条双端约束：

ξ^-和ξ⁺的具体定义如下：

β是一个参数，用于调节关节极限转换之后的可行域的大小，在本次仿真中设为0.25。可以通过调节上述参数实现对冗余机械臂关节极限的限制，但这部分并不是本次仿真的重点，故不做赘述。

(2.2)后两条方程分别用来估计冗余机械臂末端执行器雅可比矩阵J_e和临界点雅可比矩阵J_c。

和

分别表示J_e和J_c对时间的导数，

和

分别表示冗余机械臂的末端执行器和临界点的实际速度，

和

分别表示冗余机械臂的末端执行器和临界点的实际加速度，

表示冗余机械臂各关节的加速度，

表示冗余机械臂各关节的速度，

表示

的逆或伪逆，μ是零化神经网络的收敛率参数，在实验中，μ取的值为5000。

和

都可从冗余机械臂的传感器或视觉仪器中得到具体数值。

(3)使用Matlab中的ode45方法求解推导出的微分方程组，最终可得到随时间变化的向量函数y的数值解。该函数的输入为时刻t，输出为一个m+n+3维的向量y。向量y的前n项就是冗余机械臂的n个关节在t时刻所应处的角度，根据该信息便可控制冗余机械臂完成轨迹跟踪任务。

(4)接下来我们对得到的仿真结果进行分析，对比图2和图3，我们可以看出在碰撞避免算法的作用下，冗余机械臂做出了明显的避障反应，最后的轨迹显示并未触碰到最内部的球体，即碰撞区域。而根据图4，我们可以看到整个碰撞避免过程的均方根误差为0.533mm，是一个可以接受的误差，图中RMSE表示均方根误差。

2.窗形障碍物碰撞避免控制仿真：

(1)设置冗余机械臂的初始姿态为：

[-1.638；3.934；1.209；4.026；2.000；1.913]

我们稍微调整了冗余机械臂的初始姿态，以便与观察，此时机械臂的状态如图7所示：

图7中，窗形障碍物四角分别为[-0.05，-0.45，0.4]、[-0.05，-0.285，0.4]、[0.1，-0.285，0.4]、[0.1，-0.45，0.4]，警戒距离d₂为0.07m，极限距离d₁为0.05m。

定义冗余机械臂末端在三维空间中的期望轨迹：

x＝x₀+r·sin(π/6)·sin(2π(sin²(πt/2T)))

y＝y₀+r·cos(π/6)·sin(2π(sin²(πt/2T)))

z＝z₀-r·cos(2π(sin²(πt/2T)))+r

其中，r＝0.04m，t表示当前时刻，T表示轨迹跟踪任务的总时长，T＝10。x₀、y₀和z₀分别表示冗余机械臂末端的初始位置。Γ的取值为21000，μ的取值为50000。该轨迹如图8所示，可以看到末端轨迹还是较好地贴合了目的轨迹。

图9和图10分别为碰撞避免前后冗余机械臂在窗形障碍物时的仿真图。图中外框表示窗形障碍物，内框表示考虑机械臂厚度后的安全极限。

而根据图11我们看到冗余机械臂在窗形障碍物碰撞避免控制的轨迹误差情况。

由实验结果可知，本实施例在不使用冗余机械臂的先验的正向运动学模型的情况下，能够控制冗余机械臂完成碰撞避免控制任务，同时还具有关节极限躲避的功能。

附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制；

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (10)

1.一种面向冗余机械臂的无模型碰撞避免控制方法，其特征在于，所述方法包括以下步骤：

S1：对于冗余机械臂的无模型碰撞的二次规划问题，建立冗余机械臂的逆向运动学方程，并建立碰撞避免的约束不等式；

S2：将冗余机械臂的逆向运动学方程表示为微分方程形式；得到冗余机械臂的逆向运动学微分方程；

S3：建立估计雅克比矩阵的微分方程；估计冗余机械臂末端的雅克比矩阵的估计值和冗余机械臂临界点处的雅克比矩阵估计值；

S4：联立冗余机械臂的逆向运动学微分方程、冗余机械臂末端的雅克比矩阵的估计值、冗余机械臂临界点处的雅克比矩阵估计值，得到冗余机械臂的每个关节的角度随时间变化的函数；将关节角度-时间信息输入该函数，即可让机械臂按照躲避障碍物的轨迹运动。

2.根据权利要求1所述面向冗余机械臂的无模型碰撞避免控制方法，其特征在于，S1中所述冗余机械臂的逆向运动学方程具体为：

其中，

是待实时求解的决策变量，也即冗余机械臂的关节的角速度；n是冗余机械臂的自由度；系数矩阵W是n*n维的单位矩阵；系数向量q＝4(θ-θ(0))，其中θ(0)是每个关节的初始角度，θ是t时刻的关节角度。

3.根据权利要求2所述面向冗余机械臂的无模型碰撞避免控制方法，其特征在于，S1中所述碰撞避免的约束不等式为：

其中，J_e是冗余机械臂末端的雅可比矩阵；

是冗余机械臂的末端执行器在任务空间的期望轨迹的时间导数，也即末端执行器的速度；

矩阵A和向量b分别由下式推导得出：

sgn函数为符号函数，其输出值为1或-1；J_c是冗余机械臂临界点处的雅可比矩阵；d₂为警戒距离，当机械臂与障碍物的距离小于该值时开始避障计算；d₁为极限距离，我们一般认为当机械臂与障碍物的距离小于该值时发生碰撞；x_o、y_o、z_o分别为障碍物点O在x轴、y轴、z轴的坐标；x_c、y_c、z_c分别为机械臂临界点C在x轴、y轴、z轴的坐标；

ξ^-和ξ⁺都是n维向量，分别表示冗余机械臂的关节的物理最小极限和最大极限。

4.根据权利要求3所述面向冗余机械臂的无模型碰撞避免控制方法，其特征在于，假设冗余机械臂的关节具有速度极限和角度极限：

θ^-≤θ≤θ⁺

则ξ^-的具体定义如下：

β是调节参数，用于调节关节极限转换之后的可行域的大小；θ^-、θ⁺为关节角度的最小、最大极限，

为关节角速度的最小、最大极限。

5.根据权利要求4所述面向冗余机械臂的无模型碰撞避免控制方法，其特征在于，

ξ⁺的具体定义如下：

6.根据权利要求4所述面向冗余机械臂的无模型碰撞避免控制方法，其特征在于，S2具体为使用原对偶神经网络方法将冗余机械臂的逆向运动学方程表示为微分方程形式。

7.根据权利要求4所述面向冗余机械臂的无模型碰撞避免控制方法，其特征在于，S2所述冗余机械臂的逆向运动学微分方程为：

其中，y是待求解的变量，是一个有着m+n+3个元素的向量；y是上述二次规划问题中

的对偶变量，其前n个元素即为

γ是原对偶神经网络的收敛率参数，I是一个行数和列数都为m+2n的单位矩阵；系数向量q＝4(θ-θ(0))；P(·)为投影函数，m是冗余机械臂的末端的任务空间的维度；

矩阵Q和向量p的定义如下：

8.根据权利要求7所述面向冗余机械臂的无模型碰撞避免控制方法，其特征在于，投影函数P(·)的参数是一个向量，这个投影函数对输入的向量v_i×1的每一个元素做上下限的约束，第i个元素的上下限的具体值分别由

和

指定；

9.根据权利要求5所述面向冗余机械臂的无模型碰撞避免控制方法，其特征在于，冗余机械臂末端的雅克比矩阵的估计值和冗余机械臂临界点处的雅克比矩阵估计值采用零化神经动力学方法来估计。

10.根据权利要求7所述面向冗余机械臂的无模型碰撞避免控制方法，其特征在于，S3具体为：

设冗余机械臂的运动方程为如下的向量等式：

f(θ(t))＝r_a(t)

r_a(t)表示冗余机械臂的某点在t时刻的实际位置，θ(t)是冗余机械臂的每个关节在t时刻的角度，f函数是冗余机械臂的正向运动学函数，将冗余机械臂的关节状态映射到冗余机械臂的某点位置；对以上等式两端同时对时间t求导，有：

为了保持简便，使用J来代替J(θ)；使用

来替代

定义一个误差函数∈来表示计算J时的误差为：

对其应用零化神经动力学方法，令：

其中μ为用于调节收敛率的正参数，

为∈关于时间t的导数；将

代入

中，则可以得到以下等式：

对

进行变形，得到：

其中，

表示J对时间的导数，

表示冗余机械臂的末端的实际加速度，

表示冗余机械臂各关节的加速度，

表示冗余机械臂的末端的实际速度，

表示冗余机械臂各关节的速度，

依据m和n的值表示

的逆或伪逆，m是冗余机械臂的末端的任务空间的维度，n是冗余机械臂的自由度；

可用于估算冗余机械臂上某点雅可比矩阵，通过对该微分方程进行求解，即可求得每个时刻t的雅可比矩阵的估计值；

对于式中的

可以通过传感器或视觉仪器求得，雅可比矩阵估计方程

是无模型限制的，无论是J_e还是J_c都可以用该雅可比矩阵估计方程进行估计，故：

其中r_a1、r_a2分别表示在末端执行器和临界点处的实际轨迹，y为

的对偶变量。

Images