CN105082134A - 一种基于多判据的六自由度串联机器人奇异性处理方法 - Google Patents

Abstract

本发明提出了一种基于多判据的六自由度串联机器人奇异性处理方法，包括如下步骤：采用S形加减速算法规划机器人末端的笛卡尔轨迹，对笛卡尔轨迹进行分析得到第一至第三奇异性判据，包括根据空间坐标变换算法计算出笛卡尔轨迹的空间速度，计算机器人的关节空间速度到笛卡尔空间速度的映射矩阵；计算第一至第三奇异性判据；对第一至第三奇异性判据进行判断和分析，根据分析结果对机器人采用对应的奇异性处理措施。本发明在奇异性判断上采用三种判据组合的方式，可以对六自由度串联机器人的奇异性进行更细致更准确的划分，提高奇异性处理的准确性，可适应多种实际运行工况的要求。

Description

一种基于多判据的六自由度串联机器人奇异性处理方法

技术领域

本发明涉及机器人控制技术领域，特别涉及一种基于多判据的六自由度串联机器人奇异性处理方法。

背景技术

奇异性是6自由度工业机器人的固有特性，这是由其本身的机械结构决定的。当机器人的构型靠近奇异位形时，其末端执行器的空间运动能力会极大的降低，甚至导致机器人振动或者停机，严重影响机器人的正常工作。因此如何准确的判断机器人是否靠近奇异位形，并针对不同的情形给出时给出合理的奇异性处理方案，直接关系到机器人整体的空间作业性能。6自由度工业机器人的奇异性处理问题可分解为两个子问题：(1)判断是否需要进行奇异性处理；(2)如果需要，采用何种处理方案。

现有技术中常见的奇异性处理方法有以下三种：

(1)通过判断关节速度是否超过设定值来近似判断奇异性：一旦判断关机速度超限，则立即以安全的方式停止机器人运动。这种方式虽然最容易实现，但极大的限制了机器人的空间作业范围，在一些需要复杂连续运动的场合不适用，并且无法预测的突然停机可能会对设备造成损坏。

(2)在奇异点附近采用关节插补的方式运动：通过雅克比矩阵或者其他方式计算机器人的灵活度指标，结合末端实时轨迹规划的数据，判断机器人是否靠近奇异点，如果靠近，则采用关节插补的方式通过奇异点。这种处理方案虽然可以保证机器人连续运动，但是末端轨迹会偏离预期的轨迹，在要求机器人末端轨迹一直保持精确的场合是不适用的。

(3)通过提前计算避开奇异点：通过引入轨迹前瞻功能或者专家系统来获得机器人奇异点的信息，在轨迹规划时避开这些奇异点。但是这种方法会限制机器人的工作空间范围，降低机器人的适应性，同时由于需要提前计算，在控制器负荷较大时，可能会影响系统的实时性。

除此之外，由于工业机器人属于高度灵活的可编程设备，所面对的应用环境和工况多种多样，上述三种方案仅能处理某种特定的奇异性问题，适应性较差。

发明内容

本发明的目的旨在至少解决所述技术缺陷之一。

为此，本发明的目的在于提出一种基于多判据的六自由度串联机器人奇异性处理方法，在奇异性判断上采用三种判据组合的方式，可以对六自由度串联机器人的奇异性进行更细致更准确的划分，提高奇异性处理的准确性，根据三种判据的不同组合提出了不同的处理方法，可适应多种实际运行工况的要求。

为了实现上述目的，本发明的实施例提供一种基于多判据的六自由度串联机器人奇异性处理方法，包括如下步骤：

步骤S1，采用S形加减速算法规划机器人末端的笛卡尔轨迹，对所述笛卡尔轨迹进行分析得到第一至第三奇异性判据，包括如下步骤：

步骤S11，根据空间坐标变换算法计算出所述笛卡尔轨迹的空间速度v(P)，计算所述机器人的关节空间速度到笛卡尔空间速度v(P)的映射矩阵，即雅克比矩阵J(q)；

步骤S12，计算所述雅克比矩阵J(q)的奇异值得到第一奇异性判据μ，其中，μ为当前雅克比矩阵灵活度指标；

步骤S13，将经由所述雅克比矩阵J(q)映射后得到的关节速度与预设速度进行比较，得到第二奇异性判据OP，其中，OP为关节速度与预设速度的超速比；

步骤S14，根据前瞻缓冲队列中5轴的角度值θ₅，判断该角度值θ₅的绝对值是否小于预设角度值γ，将判断结果η作为第三奇异性判据；

步骤S2，对所述第一至第三奇异性判据进行判断和分析，根据分析结果对所述机器人采用对应的奇异性处理措施，包括：

当η＝1时，将机器人的插补方式由笛卡尔空间插补改为关节空间插补以通过奇异域；

当OP中至少一个数值大于1，η＝0，μ＞σ时，采用降速算法通过奇异域，其中，σ为预设灵活度阈值；

当OP中数值均不大于1或满足η＝0且μ＞σ条件时，对所述机器人不作奇异性处理。

进一步，所述笛卡尔轨迹的空间速度 $v\left(P\right)=\left(\begin{array}{c}v\\ \mathrm{\ω}\end{array}\right),$ 其中，

$v=\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right],\mathrm{\ω}=\left[\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]$

v为所述笛卡尔轨迹的线速度，ω为所述笛卡尔轨迹的角速度。

进一步，所述雅克比矩阵J(q)＝(ξ_i')＝(ξ₁',ξ₂',ξ₃',ξ₄',ξ₅',ξ₆')，

${\mathrm{\ξ}}_i^{\′}={\mathrm{Ad}}_{e^{{\widehat{\mathrm{\ξ}}}_1{\mathrm{\θ}}_1}{e}^{{\widehat{\mathrm{\ξ}}}_2{\mathrm{\θ}}_2}...e^{{\widehat{\mathrm{\ξ}}}_{i-1\mathrm{\θ}i-1}}}{\mathrm{\ξ}}_i,$

其中，ξ_i′为当前位形运动旋量，ξ_i为初始参考位形运动旋量，Ad为将ξ_i变换到ξ_i′的伴随矩阵，为第i个关节的期望角速度，i＝2,…,6。

进一步，在所述步骤S12中，计算所述雅克比矩阵J(q)的奇异值为：

D＝sqrt(abs(eig(J(q)^TJ(q))))，

其中，D为当前雅克比矩阵J(q)的奇异值向量，eig为计算矩阵的特征值操作，abs为取绝对值操作，sqrt为开方操作；

根据所述雅克比矩阵J(q)的奇异值计算第一奇异性判据μ，其中，

$\mathrm{\μ}=\frac{min\left(D\right)}{max\left(D\right)},\mathrm{\μ}\∈\[0,1\].$

进一步，在所述步骤S13中，

将笛卡尔空间的速度映射到关节速度为：

$\stackrel{\·}{q}=J{\left(q\right)}^{-1}\×v\left(P\right)=\left({\stackrel{\·}{\mathrm{\θ}}}_i\right)=\left(\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ {\stackrel{\·}{\mathrm{\θ}}}_3\\ {\stackrel{\·}{\mathrm{\θ}}}_4\\ {\stackrel{\·}{\mathrm{\θ}}}_5\\ {\stackrel{\·}{\mathrm{\θ}}}_6\end{array}\right),$

其中，为第i轴通过雅克比矩阵映射得到的期望关节速度；

根据关节速度与预设速度计算所述第二奇异性判据OP为：

OP＝(op_i)＝(op₁,op₂,op₃,op₄,op₅,op₆)，

其中，op_i为预设的第i轴最大速度。

进一步，在所述步骤S14中，所述第三奇异性判据η为：

$\mathrm{\&eta;}=\left\{\begin{array}{cc}1,& \left|{\mathrm{\&theta;}}_5\right|<\mathrm{\&gamma;}\\ 0,& \left|{\mathrm{\&theta;}}_5\right|>\mathrm{\&gamma;}\end{array}\right..$

进一步，在所述步骤S2中，

当所述第二奇异性判据OP中的每个数值均不大于1时，对所述机器人不作奇异性处理，所述机器人正常运行；

当满足η＝0且μ＜σ条件时，对所述机器人不作奇异性处理，控制所述机器人停机。

进一步，在所述步骤S2中，当OP中至少一个数值大于1，η＝0，μ＞σ时，所述机器人的各轴采用降速算法同比例降速以通过奇异域。

根据本发明实施例的基于多判据的六自由度串联机器人奇异性处理方法，通过计算出三种不同的奇异性判定条件，可针对不同的情况自动选用最佳的奇异性处理算法，可较好的解决机器人在奇异点附近因速度突变导致的运行不稳定问题，增强机器人的笛卡尔空间作业能力。本发明在奇异性判断上采用三种判据组合的方式，可以对六自由度串联机器人的奇异性进行更细致更准确的划分，提高奇异性处理的准确性，根据三种判据的不同组合提出了不同的处理方法，可适应多种实际运行工况的要求。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1为根据发明一个实施例的基于多判据的六自由度串联机器人奇异性处理方法的流程图；

图2为根据本发明实施例的计算第一至第三奇异性判据的流程图；

图3为根据发明实施例的S形加减速规划示意图；

图4为根据发明另一个实施例的基于多判据的六自由度串联机器人奇异性处理方法的流程图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

如图1所示，本发明实施例的基于多判据的六自由度串联机器人奇异性处理方法，包括如下步骤：

步骤S1，采用S形加减速算法规划机器人末端的笛卡尔轨迹，对笛卡尔轨迹进行分析得到第一至第三奇异性判据。

如图2所示，步骤S2具体包括如下步骤：

步骤S11，根据空间坐标变换算法计算出笛卡尔轨迹的空间速度v(P)，计算机器人的关节空间速度到笛卡尔空间速度v(P)的映射矩阵，即雅克比矩阵J(q)。

图3为根据发明实施例的S形加减速规划示意图。

使用S形加减速算法规划机器人末端的笛卡尔轨迹，结合空间坐标变换算法得到轨迹的空间速度v(P)为：

$v\left(P\right)=\left(\begin{array}{c}v\\ \mathrm{\&omega;}\end{array}\right),---\left(1\right)$

其中， $v=\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right],\mathrm{\&omega;}=\left[\begin{array}{c}{\mathrm{\&omega;}}_x\\ {\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_z\end{array}\right]$

v为笛卡尔轨迹的线速度，ω为笛卡尔轨迹的角速度。

在本发明的一个实施例中，在计算出轨迹的空间速度之后，采用轨迹前瞻算法提前计算多个空间位置点，保存上述空间位置点。

然后，采用旋量法计算关节空间速度到笛卡尔空间速度的映射矩阵，即雅克比矩阵

J(q)＝(ξ_i')＝(ξ₁',ξ₂',ξ₃',ξ₄',ξ₅',ξ₆')，(2)

其中，ξ_i′为当前位形运动旋量，ξ_i为初始参考位形运动旋量，Ad为将ξ_i变换到ξ_i′的伴随矩阵，与(θ₁,θ₂…θ_i-1)有关，为第i个关节的期望角速度，i＝2,…,6。

步骤S12，计算雅克比矩阵J(q)的奇异值得到第一奇异性判据μ，其中，μ为当前雅克比矩阵灵活度指标。

计算雅克比矩阵J(q)的奇异值为：

D＝sqrt(abs(eig(J(q)^TJ(q))))，(3)

其中，D为当前雅克比矩阵J(q)的奇异值向量，eig为计算矩阵的特征值操作，abs为取绝对值操作，sqrt为开方操作。

根据雅克比矩阵J(q)的奇异值计算第一奇异性判据μ，其中，

$\mathrm{\&mu;}=\frac{min\left(D\right)}{max\left(D\right)},\mathrm{\&mu;}\&Element;\&lsqb;0,1\&rsqb;,---\left(4\right)$

步骤S13，将经由雅克比矩阵J(q)映射后得到的关节速度与预设速度进行比较，得到第二奇异性判据OP，其中，OP为关节速度与预设速度的超速比。

将笛卡尔空间的速度映射到关节速度为：

$\stackrel{\&CenterDot;}{q}=J{\left(q\right)}^{-1}\&times;v\left(P\right)=\left({\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_i\right)=\left(\begin{array}{c}{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_1\\ {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_2\\ {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_3\\ {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_4\\ {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_5\\ {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_6\end{array}\right),---\left(5\right)$

其中，为第i轴通过雅克比矩阵映射得到的期望关节速度。

根据关节速度与预设速度计算第二奇异性判据OP为：

OP＝(op_i)＝(op₁,op₂,op₃,op₄,op₅,op₆)，(6)

其中，op_i为预设的第i轴最大速度， 为预设的i轴最大速度。

步骤S14，根据前瞻缓冲队列中5轴的角度值θ₅，判断该角度值θ₅的绝对值是否小于预设角度值γ，将判断结果η作为第三奇异性判据。

第三奇异性判据η为：

$\mathrm{\&eta;}=\left\{\begin{array}{cc}1,& \left|{\mathrm{\&theta;}}_5\right|<\mathrm{\&gamma;}\\ 0,& \left|{\mathrm{\&theta;}}_5\right|>\mathrm{\&gamma;}\end{array}\right.,---\left(7\right)$

步骤S2，对第一至第三奇异性判据进行判断和分析，根据分析结果对机器人采用对应的奇异性处理措施。

根据第一至第三奇异性判据的数值和组合的不同，可以对机器人采取不同的奇异性处理措施：

(1)当η＝1时，则从前瞻计算出的插补点队列中找出η由1变为0的点，在这两个位置之间将机器人的插补方式由笛卡尔空间插补改为关节空间插补以通过奇异域。

(2)当OP中至少一个数值大于1，即op_ii＝1,2...6至少有一个大于1，且满足η＝0，μ＞σ时，机器人的各轴采用降速算法同比例降速以通过奇异域，其中，σ为预设灵活度阈值。

(3)当OP中数值均不大于1或满足η＝0且μ＜σ条件时，对机器人不作奇异性处理。

(3.1)当第二奇异性判据OP中的每个数值均不大于1时，则认为机器人当前所处的位形不存在奇异性，不做奇异性处理，机器人正常运行。

(3.2)当满足η＝0且μ＜σ条件时，则机器人靠近除腕部奇异性之外的其他奇异位形，

无法使用关节空间插补通过，且如果使用降速算法，损失的速度过大，因此这种情况下不做奇异性处理，直接停机。对机器人不作奇异性处理，控制机器人停机。

图4为根据发明另一个实施例的基于多判据的六自由度串联机器人奇异性处理方法的流程图。

步骤S401，设置机器人的插补方式为笛卡尔空间插补。

步骤S402，逆解，求出机器人6轴的角度值θ₁～θ₆。

步骤S403，计算空间速度伪向量v_x、v_y、v_z、ω_x、ω_y、ω_z。

步骤S404，计算第一至第三奇异性判据η、OP和μ。

步骤S405，判断η是否等于1，如果是，则执行步骤S406，否则执行步骤S407。

步骤S406，将机器人的插补方式修改为关节空间插补，以通过奇异域。

步骤S407，判断μ是否大于σ，如果是则执行步骤S408，否则执行步骤S409。

步骤S408，判断OP中每个数值是否均大于1，如果是，则执行步骤S411，否则执行步骤S410。

步骤S409，奇异性过大，控制机器人安全停机。

步骤S410，非奇异区域状态正常赋值退出(RETURNNORMAL)。

步骤S411，采用各轴同比例降速，然后根据情况分别执行步骤S412、S413和S414。

步骤S412，靠近奇异点，报错退出(RETURNSING)。

步骤S413，降速后1个插补周期内移动的距离没有超过当前正常插补点角度(RETURNSIN_THIS_STEP)。

步骤S414，降速后1个插补周期内移动的距离超过当前正常插补点角度(RETURNSIN_NEXT_STEP)。

根据本发明实施例的基于多判据的六自由度串联机器人奇异性处理方法，通过计算出三种不同的奇异性判定条件，可针对不同的情况自动选用最佳的奇异性处理算法，可较好的解决机器人在奇异点附近因速度突变导致的运行不稳定问题，增强机器人的笛卡尔空间作业能力。本发明在奇异性判断上采用三种判据组合的方式，可以对六自由度串联机器人的奇异性进行更细致更准确的划分，提高奇异性处理的准确性，根据三种判据的不同组合提出了不同的处理方法，可适应多种实际运行工况的要求。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。本发明的范围由所附权利要求极其等同限定。

Claims (8)

1.一种基于多判据的六自由度串联机器人奇异性处理方法，其特征在于，包括如下步骤：

步骤S1，采用S形加减速算法规划机器人末端的笛卡尔轨迹，对所述笛卡尔轨迹进行分析得到第一至第三奇异性判据，包括如下步骤：

步骤S11，根据空间坐标变换算法计算出所述笛卡尔轨迹的空间速度v(P)，计算所述机器人的关节空间速度到笛卡尔空间速度v(P)的映射矩阵，即雅克比矩阵J(q)；

步骤S12，计算所述雅克比矩阵J(q)的奇异值得到第一奇异性判据μ，其中，μ为当前雅克比矩阵灵活度指标；

步骤S13，将经由所述雅克比矩阵J(q)映射后得到的关节速度与预设速度进行比较，得到第二奇异性判据OP，其中，OP为关节速度与预设速度的超速比；

步骤S14，根据前瞻缓冲队列中5轴的角度值θ₅，判断该角度值θ₅的绝对值是否小于预设角度值γ，将判断结果η作为第三奇异性判据；

步骤S2，对所述第一至第三奇异性判据进行判断和分析，根据分析结果对所述机器人采用对应的奇异性处理措施，包括：

当η＝1时，将机器人的插补方式由笛卡尔空间插补改为关节空间插补以通过奇异域；

当OP中至少一个数值大于1，η＝0，μ＞σ时，采用降速算法通过奇异域，其中，σ为预设灵活度阈值；

当OP中数值均不大于1或满足η＝0且μ＜σ条件时，对所述机器人不作奇异性处理。

2.如权利要求1所述的基于多判据的六自由度串联机器人奇异性处理方法，其特征在于，所述笛卡尔轨迹的空间速度 $v\left(P\right)=\left(\begin{array}{c}v\\ \mathrm{\&omega;}\end{array}\right),$ 其中，

$v=\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right],\mathrm{\&omega;}=\left[\begin{array}{c}{\mathrm{\&omega;}}_x\\ {\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_z\end{array}\right]$

v为所述笛卡尔轨迹的线速度，ω为所述笛卡尔轨迹的角速度。

3.如权利要求1所述的基于多判据的六自由度串联机器人奇异性处理方法，其特征在于，所述雅克比矩阵J(q)＝(ξ′_i)＝(ξ′₁,ξ′₂,ξ′₃,ξ′₄,ξ′₅,ξ′₆)，

${\mathrm{\&xi;}}_i^{\&prime;}={\mathrm{Ad}}_{e^{{\widehat{\mathrm{\&xi;}}}_1{\mathrm{\&theta;}}_1}{e}^{{\widehat{\mathrm{\&xi;}}}_2{\mathrm{\&theta;}}_2}...e^{{\widehat{\mathrm{\&xi;}}}_{i-1\mathrm{\&theta;}i-1}}}{\mathrm{\&xi;}}_i,$

其中，ξ′_i为当前位形运动旋量，ξ_i为初始参考位形运动旋量，Ad为将ξ_i变换到ξ′_i的伴随矩阵，为第i个关节的期望角速度，i＝2,…,6。

4.如权利要求1所述的基于多判据的六自由度串联机器人奇异性处理方法，其特征在于，在所述步骤S12中，计算所述雅克比矩阵J(q)的奇异值为：

D＝sqrt(abs(eig(J(q)^TJ(q))))，

其中，D为当前雅克比矩阵J(q)的奇异值向量，eig为计算矩阵的特征值操作，abs为取绝对值操作，sqrt为开方操作；

根据所述雅克比矩阵J(q)的奇异值计算第一奇异性判据μ，其中，

$\mathrm{\&mu;}=\frac{min\left(D\right)}{max\left(D\right)},\mathrm{\&mu;}\&Element;\&lsqb;0,1\&rsqb;.$

5.如权利要求1所述的基于多判据的六自由度串联机器人奇异性处理方法，其特征在于，在所述步骤S13中，

将笛卡尔空间的速度映射到关节速度为：

$\stackrel{\&CenterDot;}{q}=J{\left(q\right)}^{-1}\&times;v\left(P\right)=\left({\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_i\right)=\left(\begin{array}{c}{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_1\\ {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_2\\ {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_3\\ {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_4\\ {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_5\\ {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_6\end{array}\right),$

其中，为第i轴通过雅克比矩阵映射得到的期望关节速度；

根据关节速度与预设速度计算所述第二奇异性判据OP为：

OP＝(op_i)＝(op₁,op₂,op₃,op₄,op₅,op₆)，

其中，op_i为预设的第i轴最大速度。

6.如权利要求1所述的基于多判据的六自由度串联机器人奇异性处理方法，其特征在于，在所述步骤S14中，所述第三奇异性判据η为：

$\mathrm{\&eta;}=\left\{\begin{array}{cc}1,& \left|{\mathrm{\&theta;}}_5\right|<\mathrm{\&gamma;}\\ 0,& \left|{\mathrm{\&theta;}}_5\right|>\mathrm{\&gamma;}\end{array}\right..$

7.如权利要求1所述的基于多判据的六自由度串联机器人奇异性处理方法，其特征在于，在所述步骤S2中，

当所述第二奇异性判据OP中的每个数值均不大于1时，对所述机器人不作奇异性处理，所述机器人正常运行；

当满足η＝0且μ＜σ条件时，对所述机器人不作奇异性处理，控制所述机器人停机。

8.如权利要求1所述的基于多判据的六自由度串联机器人奇异性处理方法，其特征在于，在所述步骤S2中，当OP中至少一个数值大于1，η＝0，μ＞σ时，所述机器人的各轴采用降速算法同比例降速以通过奇异域。