一种面向冗余机械臂的无模型关节容错控制方法及装置

1.本发明涉及机械臂控制技术领域，尤其是一种面向冗余机械臂的无模型关节容错控制方法及装置。


背景技术：

2.在现代制造业中，冗余机械臂已经得到了广泛的运用。而冗余机械臂的关节容错控制则是一种非常常见的应用场景。在冗余机械臂的实际应用场景中，经常会出现某个甚至是多个关节失效的情况。此时如果仍让冗余机械臂按之前的控制方案运行，将无法顺利完成原定的轨迹追踪任务。但由于各种情况，对冗余机械臂关节的维修通常是昂贵且不必要的，在一些特殊情况下甚至是不可能的。冗余机械臂由于其冗余性，能够实现在某些关节失效的情况下，利用冗余关节补偿损坏关节的自由度，从而完成预期的轨迹追踪任务。这即为冗余机械臂的关节容错控制。目前对于冗余机械臂的关节容错控制算法已经有许多研究，主要方向为保持失效的关节速度为零，但是这些关节容错控制算法都需要基于冗余机械臂模型已知的情况下进行控制。
3.目前，相关技术中提出的基于线性变分不等式的原始对偶神经网络二次规划冗余机械臂关节容错控制方法。该方法提出了以下机械臂的控制方案：
[0004][0005][0006][0007]
其中，是待实时求解的决策变量，也即冗余机械臂的关节的角速度，n是冗余机械臂的自由度。j是冗余机械臂末端的雅可比矩阵。是冗余机械臂的末端执行器在任务空间的期望轨迹的时间导数，也即末端执行器的速度。ξ-和ξ
+
分别表示冗余机械臂的关节的物理极限。
[0008]
可使用许多方法求解该线性变分不等式问题，上述相关技术使用了原对偶神经网络二次规划来将该线性变分不等式转化为微分方程形式，然后使用通用微分方程求解器来对其进行求解。
[0009]
但是，现有技术要求知晓约束条件中冗余机械臂末端的雅可比矩阵j的解析式，以求得每个时刻的雅可比矩阵j的精确值，才能确定每个时刻的约束条件，从而得到微分方程在每个时刻的解。该方法的解的精度和正确性会受到雅可比矩阵表达式的直接影响。但是即使是同一批次、型号的机械臂，雅可比矩阵表达式也可能由于机械臂的组装差异而各自不同，使用冗余机械臂的正向运动学模型推导出的雅可比矩阵表达式可能与实际的雅可比矩阵表达式有偏差，致在应用该算法时产生误差，影响解的精确度。另外，冗余机械臂的型号繁多，对于一种机械臂，就需要重新计算一次其雅可比矩阵表达式，而有些机械臂的正向运动学模型难以测算，导致该方法的可移植性不强。


技术实现要素：

[0010]
有鉴于此，本发明实施例提供一种精确度高且适应性强的，面向冗余机械臂的无模型关节容错控制方法及装置。
[0011]
本发明的一方面提供了一种面向冗余机械臂的无模型关节容错控制方法，包括：
[0012]
建立冗余机械臂的逆向运动学方程，并建立关节容错和轨迹追踪的约束不等式；
[0013]
根据所述逆向运动学方程和所述约束不等式，得到冗余机械臂的逆向运动学微分方程；
[0014]
使用零化神经网络建立估计雅可比矩阵的微分方程，采用离散化零化神经动力学方法估计冗余机械臂末端的雅可比矩阵的估计值；
[0015]
联立冗余机械臂的逆向运动学微分方程和冗余机械臂末端的雅可比矩阵的估计值，并使用离散方程对所述联立的结果进行离散处理，得到在碰撞避免运动中冗余机械臂在任意时刻各关节的期望位置；
[0016]
根据所述期望位置控制冗余机械臂完成无模型关节容错任务。
[0017]
可选地，所述冗余机械臂的逆向运动学方程的表达式为：
[0018][0019]
其中，是冗余机械臂的关节的角速度，是待实时求解的决策变量；n是冗余机械臂的自由度；j是冗余机械臂末端的雅可比矩阵；是冗余机械臂的末端执行器在任务空间的期望轨迹的时间导数，是末端执行器的速度。
[0020]
可选地，所述关节容错的约束不等式的表达式为：
[0021][0022]
其中，矩阵λ是一个mf*n维的矩阵；mf为冗余机械臂中损坏关节的个数；ξ-和ξ
+
都是n维向量，分别表示冗余机械臂的关节的物理最小极限和最大极限。
[0023]
可选地，所述冗余机械臂的关节的物理最小极限的计算表达式为：
[0024][0025]
其中，β是调节参数，用于调节关节极限转换之后的可行域的大小；θ代表冗余机械臂的关节的角度，θ-≤θ≤θ
+
；代表冗余机械臂的关节的角速度，θ-为关节的角度的最小极限，θ
+
为关节的角度的最大极限，为关节的角速度的最小极限，为关节的角速度的最大极限。
[0026]
可选地，所述冗余机械臂的关节的物理最大极限的计算表达式为：
[0027][0028]
可选地，所述根据所述逆向运动学方程和所述约束不等式，得到冗余机械臂的逆向运动学微分方程，具体为：
[0029]
根据所述逆向运动学方程和所述约束不等式，使用零化神经网络方法将冗余机械臂的逆向运动学问题表示为微分方程形式，得到冗余机械臂的逆向运动学微分方程；
[0030]
所述逆向运动学微分方程的表达式为：
[0031][0032]
其中，为冗余机械臂关节加速度；和分别为a(t)，b(t)，c(t)以及d(t)的时间导数；函数r(
·
)表示向量中的每个元素都经过函数r(
·
)处理，即r(x)＝(r(x1),
…
,r(xk))
[0033]
a(t)，b(t)，c(t)以及d(t)的定义表达式为：
[0034][0035]
其中，i为单位矩阵；
[0036]
函数r(
·
)定义表达式为：
[0037][0038]
其中，a代表任意实数。
[0039]
可选地，所述离散处理的离散公式为：
[0040][0041]
其中，代表x在t＝kφ时的瞬时导数；xk代表x在t＝kφ时的瞬时值；φ代表仿真时间间隔；o()代表时间复杂度。
[0042]
本发明实施例的另一方面还提供了一种面向冗余机械臂的无模型关节容错控制装置，包括：
[0043]
第一模块，用于建立冗余机械臂的逆向运动学方程，并建立关节容错和轨迹追踪的约束不等式；
[0044]
第二模块，用于根据所述逆向运动学方程和所述约束不等式，得到冗余机械臂的逆向运动学微分方程；
[0045]
第三模块，用于使用零化神经网络建立估计雅可比矩阵的微分方程，采用离散化零化神经动力学方法估计冗余机械臂末端的雅可比矩阵的估计值；
[0046]
第四模块，用于联立冗余机械臂的逆向运动学微分方程和冗余机械臂末端的雅可比矩阵的估计值，并使用离散方程对所述联立的结果进行离散处理，得到在碰撞避免运动中冗余机械臂在任意时刻各关节的期望位置；
[0047]
第五模块，用于根据所述期望位置控制冗余机械臂完成无模型关节容错任务。
[0048]
本发明实施例的另一方面还提供了一种电子设备，包括处理器以及存储器；
[0049]
所述存储器用于存储程序；
[0050]
所述处理器执行所述程序实现如前面所述的方法。
[0051]
本发明实施例的另一方面还提供了一种计算机可读存储介质，所述存储介质存储有程序，所述程序被处理器执行实现如前面所述的方法。
[0052]
本发明实施例还公开了一种计算机程序产品或计算机程序，该计算机程序产品或计算机程序包括计算机指令，该计算机指令存储在计算机可读存储介质中。计算机设备的
处理器可以从计算机可读存储介质读取该计算机指令，处理器执行该计算机指令，使得该计算机设备执行前面的方法。
[0053]
本发明的实施例建立冗余机械臂的逆向运动学方程，并建立关节容错和轨迹追踪的约束不等式；根据所述逆向运动学方程和所述约束不等式，得到冗余机械臂的逆向运动学微分方程；使用零化神经网络建立估计雅可比矩阵的微分方程，采用离散化零化神经动力学方法估计冗余机械臂末端的雅可比矩阵的估计值；联立冗余机械臂的逆向运动学微分方程和冗余机械臂末端的雅可比矩阵的估计值，并使用离散方程对所述联立的结果进行离散处理，得到在碰撞避免运动中冗余机械臂在任意时刻各关节的期望位置；根据所述期望位置控制冗余机械臂完成无模型关节容错任务。本发明的精确度高且适应性强。
附图说明
[0054]
为了更清楚地说明本技术实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0055]
图1为本发明实施例的计算流程示意图；
[0056]
图2为冗余机械臂在所有关节均正常工作时的运动轨迹仿真图；
[0057]
图3为冗余机械臂在单关节失效且使用本发明的关节容错方法控制时的运动轨迹仿真图；
[0058]
图4为冗余机械臂在单关节失效且使用本发明的关节容错控制方法时的运动轨迹图；
[0059]
图5为冗余机械臂在单关节失效且使用本发明的关节容错控制方法时的误差情况；
[0060]
图6为冗余机械臂在单关节失效且使用本发明的关节容错控制方法时的关节速度情况；
[0061]
图7为冗余机械臂在双关节失效且使用本发明的关节容错控制方法时的运动轨迹仿真图；
[0062]
图8为冗余机械臂在双关节失效且使用本发明的关节容错控制方法时的运动轨迹图；
[0063]
图9为冗余机械臂在双关节失效且使用本发明的关节容错控制方法时的误差情况；
[0064]
图10为冗余机械臂在双关节失效且使用本发明的关节容错控制方法时的关节速度情况。
具体实施方式
[0065]
为了使本技术的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本技术进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本技术，并不用于限定本技术。
[0066]
针对现有技术存在的问题，本发明一方面提供了一种面向冗余机械臂的无模型关
节容错控制方法，包括：
[0067]
建立冗余机械臂的逆向运动学方程，并建立关节容错和轨迹追踪的约束不等式；
[0068]
根据所述逆向运动学方程和所述约束不等式，得到冗余机械臂的逆向运动学微分方程；
[0069]
使用零化神经网络建立估计雅可比矩阵的微分方程，采用离散化零化神经动力学方法估计冗余机械臂末端的雅可比矩阵的估计值；
[0070]
联立冗余机械臂的逆向运动学微分方程和冗余机械臂末端的雅可比矩阵的估计值，并使用离散方程对所述联立的结果进行离散处理，得到在碰撞避免运动中冗余机械臂在任意时刻各关节的期望位置；
[0071]
根据所述期望位置控制冗余机械臂完成无模型关节容错任务。
[0072]
可选地，所述冗余机械臂的逆向运动学方程的表达式为：
[0073][0074]
其中，是冗余机械臂的关节的角速度，是待实时求解的决策变量；n是冗余机械臂的自由度；j是冗余机械臂末端的雅可比矩阵；是冗余机械臂的末端执行器在任务空间的期望轨迹的时间导数，是末端执行器的速度。
[0075]
可选地，所述关节容错的约束不等式的表达式为：
[0076][0077]
其中，矩阵λ是一个mf*n维的矩阵；mf为冗余机械臂中损坏关节的个数；ξ-和ξ
+
都是n维向量，分别表示冗余机械臂的关节的物理最小极限和最大极限。
[0078]
可选地，所述冗余机械臂的关节的物理最小极限的计算表达式为：
[0079][0080]
其中，β是调节参数，用于调节关节极限转换之后的可行域的大小；θ代表冗余机械臂的关节的角度，θ-≤θ≤θ
+
；代表冗余机械臂的关节的角速度，θ-为关节的角度的最小极限，θ
+
为关节的角度的最大极限，为关节的角速度的最小极限，为关节的角速度的最大极限。
[0081]
可选地，所述冗余机械臂的关节的物理最大极限的计算表达式为：
[0082][0083]
可选地，所述根据所述逆向运动学方程和所述约束不等式，得到冗余机械臂的逆向运动学微分方程，具体为：
[0084]
根据所述逆向运动学方程和所述约束不等式，使用零化神经网络方法将冗余机械臂的逆向运动学问题表示为微分方程形式，得到冗余机械臂的逆向运动学微分方程；
[0085]
所述逆向运动学微分方程的表达式为：
[0086][0087]
其中，为冗余机械臂关节加速度；和分别为a(t)，b(t)，c
(t)以及d(t)的时间导数；函数r(
·
)表示向量中的每个元素都经过函数r(
·
)处理，即r(x)＝(r(x1),
…
,r(xk))
[0088]
a(t)，b(t)，c(t)以及d(t)的定义表达式为：
[0089][0090]
其中，i为单位矩阵；
[0091]
函数r(
·
)定义表达式为：
[0092][0093]
其中，a代表任意实数。
[0094]
可选地，所述离散处理的离散公式为：
[0095][0096]
其中，代表x在t＝kφ时的瞬时导数；xk代表x在t＝kφ时的瞬时值；φ代表仿真时间间隔；o()代表时间复杂度。
[0097]
本发明实施例的另一方面还提供了一种面向冗余机械臂的无模型关节容错控制装置，包括：
[0098]
第一模块，用于建立冗余机械臂的逆向运动学方程，并建立关节容错和轨迹追踪的约束不等式；
[0099]
第二模块，用于根据所述逆向运动学方程和所述约束不等式，得到冗余机械臂的逆向运动学微分方程；
[0100]
第三模块，用于使用零化神经网络建立估计雅可比矩阵的微分方程，采用离散化零化神经动力学方法估计冗余机械臂末端的雅可比矩阵的估计值；
[0101]
第四模块，用于联立冗余机械臂的逆向运动学微分方程和冗余机械臂末端的雅可比矩阵的估计值，并使用离散方程对所述联立的结果进行离散处理，得到在碰撞避免运动中冗余机械臂在任意时刻各关节的期望位置；
[0102]
第五模块，用于根据所述期望位置控制冗余机械臂完成无模型关节容错任务。
[0103]
本发明实施例的另一方面还提供了一种电子设备，包括处理器以及存储器；
[0104]
所述存储器用于存储程序；
[0105]
所述处理器执行所述程序实现如前面所述的方法。
[0106]
本发明实施例的另一方面还提供了一种计算机可读存储介质，所述存储介质存储有程序，所述程序被处理器执行实现如前面所述的方法。
[0107]
本发明实施例还公开了一种计算机程序产品或计算机程序，该计算机程序产品或计算机程序包括计算机指令，该计算机指令存储在计算机可读存储介质中。计算机设备的处理器可以从计算机可读存储介质读取该计算机指令，处理器执行该计算机指令，使得该计算机设备执行前面的方法。
[0108]
下面结合说明书附图，对本发明的具体实现原理进行详细说明：
[0109]
在阐述具体实施例之前首先对与本实施例相关的缩略语和关键术语定义进行说明：
[0110]
冗余机械臂：冗余机械臂是指末端执行器在执行给定任务时有超出所需自由度的机械臂。
[0111]
机器人正向运动学：机器人正向运动学主要研究在给定了关节机械臂各关节的位姿的情况下，机械臂的末端执行器会如何运动的问题。
[0112]
机器人逆向运动学：机器人逆向运动学主要研究在给定了关节机械臂的末端执行器的期望运动轨迹的情况下，各关节要如何运动才能让末端执行器的实际运动轨迹和期望运动轨迹相一致的问题。
[0113]
机械臂的运动学模型：机械臂的运动学模型一般指正向运动学模型，含有机械臂各连杆之间的相对位置等信息。
[0114]
零化神经网络：零化神经网络是一种结构简单的神经网络，可用于求解冗余机械臂逆运动学问题和冗余机械臂雅可比矩阵估算问题。可以将冗余机械臂逆运动学问题和冗余机械臂雅可比矩阵估算问题表示为零化神经网络的动态系统方程，即微分方程，该微分方程的解即为上述问题的解。
[0115]
关节容错控制方法：关节容错控制方法是指在关节机械臂中的某个甚至某几个关节发生故障时，冗余机械臂通过利用其余正常关节的冗余度，仍能完成预期的轨迹追踪任务。
[0116]
本发明提供了一种面向冗余机械臂的无模型关节容错控制方法，使冗余机械臂的末端执行器在某个甚至某些关节失效的情况下仍能够按照预设的路径运动，良好地完成任务。
[0117]
本实施例所提出的冗余机械臂关节容错控制方法可简要归纳如下：
[0118]
(一)建立求解逆向运动学的约束不等式。将冗余机械臂关节容错、各关节的速度极限、角度极限和末端执行器的期望轨迹表示为不等式约束。
[0119]
(二)建立估计末端执行器处雅可比矩阵的微分方程，需要分别获取冗余机械臂的末端执行器的速度、加速度。基于此实现方法的无模型特性。
[0120]
(三)将步骤(一)中建立二次规划约束转换成微分方程，该微分方程需要使用末端执行器雅可比矩阵作为参数，而步骤(二)中的微分方程可以估计出雅可比矩阵的值。因此，可以将(一)和(二)中的微分方程联合求解。并使用离散方程对微分方程组进行离散化处理。该微分方程组的解就是冗余机械臂的每个关节的角度随时间变化的函数。将关节角度-时间信息输入冗余机械臂，即可让机械臂按照预期的轨迹运动。
[0121]
本实施例所述方法的详细步骤如下：
[0122]
首先求解冗余机械臂的逆向运动学问题约束条件为：
[0123][0124][0125][0126]
其中，是待实时求解的决策变量，也即冗余机械臂的关节的角速度。m是冗余机械臂的末端的任务空间的维度，比如在一个轨迹跟踪任务中，只规定了冗余机械臂末端的在3维空间中的期望位置，那么m＝3，而n是冗余机械臂的自由度。θ是t时刻的关节角度。j是冗余机械臂末端的雅可比矩阵。是冗余机械臂的末端执行器在任务空间的期望轨
迹的时间导数，也即末端执行器的速度。
[0127]
矩阵λ是一个mf*n维的矩阵；mf为冗余机械臂中损坏关节的个数。对于一个冗余机械臂，假定其有mf个关节发生了故障(例如，第i个关节、
…
和第j个关节)，则这些关节所对应的关节速度应强制设置为零。因此，冗余机械臂必须满足下列等式约束：
[0128][0129]
上述等式约束可被改写为以下矩阵形式：
[0130][0131]
矩阵λ为：
[0132][0133]
ξ-和ξ
+
都是n维向量，分别表示冗余机械臂的关节的物理极限。假设冗余机械臂的关节具有速度极限和角度极限：
[0134]
θ-≤θ≤θ
+
[0135][0136]
ξ-和ξ
+
的具体定义如下：
[0137][0138][0139]
β是一个参数，用于调节关节极限转换之后的可行域的大小，θ-、θ
+
为关节角度的最小、最大极限，为关节角速度的最小、最大极限。在本次仿真中β的值设为0.25。
[0140]
使用零化神经网络(znn)来把冗余机械臂的逆运动学问题变为微分方程形式：
[0141][0142]
其中，为冗余机械臂关节加速度；和分别为a(t)，b(t)，c(t)以及d(t)的时间导数。a(t)，b(t)，c(t)以及d(t)的定义如下：
[0143][0144][0145]
其中，i为单位矩阵。函数r(
·
)表示向量中的每个元素都经过函数r(
·
)处理，即r(x)＝(r(x1),
…
,r(xk))。其中，函数r(
·
)定义如下：
[0146]
[0147]
在上述流程中，雅可比矩阵j本来是需要根据冗余机械臂的正向运动学模型来推算的，但是为了让本发明提出的方法不需要使用冗余机械臂的正向运动学的先验模型，本实施例使用了零化神经动力学方法(znn)来估计两个雅可比矩阵的估计值，并在计算中代替j。下面介绍这一方法。
[0148]
冗余机械臂的运动方程可写为如下的向量等式：
[0149]
f(θ(t))＝r(t)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0150]
r(t)表示冗余机械臂的某点在t时刻的实际位置，θ(t)是冗余机械臂的每个关节在t时刻的角度，f函数是冗余机械臂的正向运动学函数，将冗余机械臂的关节状态映射到冗余机械臂的某点位置。对以上等式两端同时对时间t求导，有：
[0151][0152]
为了保持简便，在之后的内容里，本实施例使用j来代替j(θ)。本实施例定义一个误差函数∈来表示计算j时的误差为：
[0153][0154]
对其应用零化神经动力学方法，令：
[0155][0156]
其中μ为用于调节收敛率的正参数，为∈关于时间t的导数。将(4)带入(5)中，则可以得到以下等式：
[0157][0158]
对(5)进行变形，本实施例就可以得到：
[0159][0160]
其中，表示j对时间的导数，表示冗余机械臂的末端的实际加速度，表示冗余机械臂各关节的加速度，表示冗余机械臂的末端的实际速度，表示冗余机械臂各关节的速度，依据m和n的值表示的逆或伪逆。(6)式可用于估算冗余机械臂上某点雅可比矩阵，通过对该微分方程进行求解，即可求得每个时刻t的雅可比矩阵的估计值。对于式中的本实施例可以通过传感器或视觉仪器求得，因此本实施例所得到的雅可比矩阵估计方程是无模型限制的。
[0161]
然后，将两条微分方程联合求解：
[0162][0163][0164]
本实施例即可求得在碰撞避免运动中冗余机械臂在t时刻各关节的期望位置，从而控制冗余机械臂完成无模型碰撞避免任务。
[0165]
最后，为了提高算法的精度和运行速度，本实施例还使用离散方程对上述微分方
程进行离散处理。离散方程为：
[0166][0167]
将微分方程经过离散方程离散处理后，可以得到以下方程组：
[0168][0169][0170][0171][0172][0173]
将通过以上方程组实时得到的关节角度θ输入至冗余机械臂中，就能实现对冗余机械臂的关节容错控制。
[0174]
本实施例所使用的是一个设置关节速度为零进行冗余机械臂关节容错的不等式约束。目前使用这一方向的控制方法仍是基于冗余机械臂模型的。而无模型的碰撞避免控制算法不依赖于机械手的显式动力学模型和运动学模型，从而避免了在实际中对机器人进行精确建模，实现和调试都较为方便。
[0175]
本实施例提出的关节容错控制方法属于基于反馈控制的无模型关节容错控制算法，可以控制冗余机械臂末端以特定轨迹进行运动，在某个甚至多个关节失效的情况下顺利完成各种任务。
[0176]
下面结合图形，以在kinova jaco2机械臂上进行冗余机械臂关节容错控制仿真实验作为例子，对本发明提出的方法进行进一步说明。
[0177]
1.单关节失效关节容错控制仿真。
[0178]
(1)设置冗余机械臂的初始姿态为：
[0179]
[1.596；2.341；3.045；-2.498；-1.131；0.770]
[0180]
每个值从左到右依次表示冗余机械臂从基座到末端的每个关节的角度，单位为rad。在matlab中调用kinova jaco2机械臂提供的api，将关节角度发送到机械臂的控制器，机械臂将运动到该位置。
[0181]
定义冗余机械臂末端在三维空间中的期望轨迹：
[0182]
x＝x0+3r cos(2π(sin2(πt/2t)))+r cos(6π(sin2(πt/2t)))-4r
[0183]
y＝y0+r sin(2π(sin2(πt/2t)))+r sin(6π(sin2(πt/2t)))
[0184]
z＝z
0-r
·
sin(2π(sin2(πt/2t)))-sin(4π(sin2(πt/2t)))
[0185]
其中，r＝0.03m，t表示当前时刻，t表示碰撞避免任务的总时长，t＝20s。x0、y0和z0分别表示冗余机械臂末端的初始位置。该轨迹如图2所示，实际轨迹较好地贴合了目的轨迹。冗余机械臂的第三关节失效。
[0186]
(2)在定义了冗余机械臂初始状态和期望轨迹后，按照图1所示的算法流程，建立两条微分方程：
[0187][0188][0189]
(2.1)在第一条方程中，是待求解的变量，它是一个n维的向量。其中n是冗余机械臂的自由度。λ是零化神经网络的收敛率参数，此值越大，冗余机械臂进行轨迹跟踪的精度就越高。在实验中，λ取的值为10。其中，为冗余机械臂关节加速度；和分别为a(t)，b(t)，c(t)以及d(t)的时间导数。a(t)，b(t)，c(t)以及d(t)的定义如下：
[0190][0191][0192]
其中，i为单位矩阵。函数r(
·
)表示向量中的每个元素都经过函数r(
·
)处理，即r(x)＝(r(x1),
…
,r(xk))。其中，函数r(
·
)定义如下：
[0193][0194]
假设冗余机械臂的关节具有速度极限和角度极限：
[0195]
θ-≤θ≤θ
+
[0196][0197]
则这两个约束可以通过关节极限转换统一化为一条双端约束：
[0198][0199]
ξ-和ξ
+
的具体定义如下：
[0200][0201][0202]
β是一个参数，用于调节关节极限转换之后的可行域的大小，在本次仿真中设为0.25。可以通过调节上述参数实现对冗余机械臂关节极限的限制，但这部分并不是本次仿真的重点，故不做赘述。
[0203]
(2.2)为了提高算法的运行速率和精度，接下来使用以下离散公式对微分方程组进行离散化处理：
[0204][0205]
将微分方程经过离散方程离散处理后，可以得到以下方程组：
[0206][0207][0208][0209][0210][0211]
第二和第五两条方程分别用来估计冗余机械臂末端执行器雅可比矩阵j。和表示对j时间的导数，表示冗余机械臂的末端执行器的实际速度，表示冗余机械臂的末端执行器的实际加速度，表示冗余机械臂各关节的加速度，表示冗余机械臂各关节的速度，表示的逆或伪逆，μ是零化神经网络的收敛率参数，在实验中，μ取的值为10。和都可从冗余机械臂的传感器或视觉仪器中得到具体数值。
[0212]
(3)使用求解上述推导出的方程组，最终可得到随时间变化的向量函数θ的数值解。该函数的输入为时刻t，输出为一个n维的向量θ。向量θ表示冗余机械臂的关节角度，根据该信息便可控制冗余机械臂完成轨迹跟踪任务。
[0213]
(4)接下来本实施例对得到的仿真结果进行分析，对比图2和图3，本实施例可以看出在关节容错控制算法的作用下，冗余机械臂在单关节失效时的情况仍顺利完成了轨迹追踪任务(失效关节为红色连杆和青色连杆之间的关节，已被红圈圈出)。根据图4和图5，本实施例可以看到整个轨迹追踪任务末端执行器与目标轨迹贴合的很好，均方根误差约为0.0013mm，是一个可以忽略的误差，图中rmse表示均方根误差。图6则显示了整个轨迹追踪任务中冗余机械臂各关节的运动角速度，可以看到失效的第三关节速度一直为0。
[0214]
2.双关节失效关节容错控制仿真：
[0215]
(1)本次仿真中，冗余机械臂的第三和第五关节失效。其他设置与上节仿真相同。
[0216]
对比图2和图7本实施例可以看出在关节容错控制算法的作用下，冗余机械臂在双关节失效时的情况仍顺利完成了轨迹追踪任务(失效关节分别为红色连杆和青色连杆之间的关节与紫色连杆和蓝色连杆之间的关节，均已被红圈圈出)。根据图8和图9，本实施例可以看到整个轨迹追踪任务末端执行器与目标轨迹贴合的很好，均方根误差约为8.3
×
10-5
mm，仍是一个可以忽略的误差，图中rmse表示均方根误差。图10则显示了整个轨迹追踪任务中冗余机械臂各关节的运动角速度，可以看到失效的第三关节和第五关节速度一直为0。
[0217]
由实验结果可知，本实施例在不使用冗余机械臂的先验的正向运动学模型的情况下，能够控制冗余机械臂完成关节容错控制任务，同时还具有轨迹追踪的功能。
[0218]
在一些可选择的实施例中，在方框图中提到的功能/操作可以不按照操作示图提到的顺序发生。例如，取决于所涉及的功能/操作，连续示出的两个方框实际上可以被大体上同时地执行或所述方框有时能以相反顺序被执行。此外，在本发明的流程图中所呈现和
描述的实施例以示例的方式被提供，目的在于提供对技术更全面的理解。所公开的方法不限于本文所呈现的操作和逻辑流程。可选择的实施例是可预期的，其中各种操作的顺序被改变以及其中被描述为较大操作的一部分的子操作被独立地执行。
[0219]
此外，虽然在功能性模块的背景下描述了本发明，但应当理解的是，除非另有相反说明，所述的功能和/或特征中的一个或多个可以被集成在单个物理装置和/或软件模块中，或者一个或多个功能和/或特征可以在单独的物理装置或软件模块中被实现。还可以理解的是，有关每个模块的实际实现的详细讨论对于理解本发明是不必要的。更确切地说，考虑到在本文中公开的装置中各种功能模块的属性、功能和内部关系的情况下，在工程师的常规技术内将会了解该模块的实际实现。因此，本领域技术人员运用普通技术就能够在无需过度试验的情况下实现在权利要求书中所阐明的本发明。还可以理解的是，所公开的特定概念仅仅是说明性的，并不意在限制本发明的范围，本发明的范围由所附权利要求书及其等同方案的全部范围来决定。
[0220]
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read-on ly memory)、随机存取存储器(ram，random access memory)、磁碟或者光盘等各种可以存储程序代码的介质。
[0221]
在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。
[0222]
计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(ram)，只读存储器(rom)，可擦除可编辑只读存储器(eprom或闪速存储器)，光纤装置，以及便携式光盘只读存储器(cdrom)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。
[0223]
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(pga)，现场可编程门阵列(fpga)等。
[0224]
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示
例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
[0225]
尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。
[0226]
以上是对本发明的较佳实施进行了具体说明，但本发明并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做出种种的等同变形或替换，这些等同的变形或替换均包含在本技术权利要求所限定的范围内。