CN103909522A - 一种六自由度工业机器人通过奇异域的方法 - Google Patents

Abstract

本发明公开了一种六自由度工业机器人通过奇异域的方法，包括以下步骤:步骤1、对六自由度工业机器人进行笛卡尔空间的运动轨迹规划；步骤2、将运动轨迹中各个插值点的坐标值及其姿态进行运动学逆解，得到各插值点处六自由度工业机器人各个关节的角位移、角速度和角加速度关节变量；步骤3、对六自由度工业机器人进行奇异域的设置，对各插值点处各关节变量进行计算，并判别该插值点是否位于奇异域内，若位于奇异域内，则进一步判别奇异位形的种类；步骤4、使六自由度工业机器人通过奇异域。具有简单易行、能很好地解决目前六自由度工业机器人在遇到奇异域时出现关节角速度突变而导致运行不稳定的问题等优点。

Description

一种六自由度工业机器人通过奇异域的方法

技术领域

本发明涉及一种六自由度工业机器人轨迹规划中奇异域通过技术，特别涉及一种六自由度工业机器人通过奇异域的方法。

背景技术

奇异位形是机器人机构的一个重要的运动学特性，它是指机械手的工作空间中，手部参考点不能实现沿任意方向的微小位移或转动时相应的机械手位形。奇异是机器人的固有性质，奇异的存在容易造成机械臂的振动和冲击，因此分析和研究机器人奇异问题和奇异域的通过方法是非常重要的。工业机器人的奇异主要分为姿态奇异与位置奇异两种。目前对工业机器人奇异问题的处理方法主要有以下几种：通过预测奇异位形可能出现的位置而回避奇异位形；利用降秩雅克比矩阵求伪逆解而得到运动学近似逆解；以协调控制法回避机械手末端姿态奇异位形。（1）回避机器人的奇异位形。通过预测奇异位形可能出现的位置而回避奇异位形。然而当机器人的几何结构比较复杂时，就会产生极其复杂和不可预测的奇异位形，并且此方法限制了机器人在其工作空间的活动范围和运动路径，使得机器人在奇异位形处及其邻域成了运动轨迹的盲点。（2）利用降秩雅克比矩阵求运动学近似反解。利用矩阵论中的伪逆矩阵理论，通过定义一种伪逆雅克比矩阵，将雅克比矩阵降秩处理，求解近似运动学反解的方法，但是伪逆解意味着从精确解到近似解存在着不连续性，而且在奇异位形附近的关节速度和加速度的伪逆解往往很大，计算方面也很繁琐。（3）以协调控制法回避机械手奇异位形。末端手部姿态只与(θ₄+θ₆)有关，因此在保持(θ₄+θ₆)不变的情况下,让θ₄和θ₆按相反的方向转一定的角度，而关节5的轴线方向就随着变化,这样就使得机器人的雅克比矩阵J(q)≠0,从而避开了奇异形位，但是此方法只能解决姿态奇异现象，在协调运动过程中会在奇异位形处停留一段时间，且存在协调运动前后关节速度会出现不连续现象的问题。由此可见，现有技术主要有以下缺点与不足之处：

1、回避机器人奇异位形的方法，限制了机器人在其工作空间的活动范围和运动路径，使得机器人在奇异位形处及其邻域成了运动轨迹的盲点。

2、降秩雅克比矩阵求运动学近似反解的方法，意味着从精确解到近似解存在着不连续性，而且在奇异位形附近的关节速度和加速度的伪逆解往往很大，计算方面也很繁琐。

3、协调运动法，只能解决姿态奇异现象，且在协调运动过程中在奇异位形处有停留，协调运动前后关节速度不连续而导致运行不稳定。

发明内容

本发明的目的在于克服现有技术的缺点与不足，提供一种六自由度工业机器人通过奇异域的方法，该方法克服了工业机器人在工作过程中遇到奇异位形时出现关节角速度突变而运行不稳定的问题，使机器人各关节角位移及角速度进行平滑过渡以通过奇异域。

本发明的目的通过下述技术方案实现：一种六自由度工业机器人通过奇异域的方法，包括以下步骤：

步骤1、对六自由度工业机器人进行运动学分析，利用离线仿真软件对六自由度工业机器人进行笛卡尔空间的运动轨迹规划；

步骤2、将步骤1所述的运动轨迹中各个插值点的坐标值及其姿态进行运动学逆解，得到各插值点处六自由度工业机器人各个关节的角位移关节变量、角速度关节变量和角加速度关节变量；

步骤3、对六自由度工业机器人进行奇异域的设置，对步骤2所述的各插值点处各关节变量进行计算，并判别该插值点是否位于奇异域内，若位于奇异域内，则进一步判别奇异位形的种类；

步骤4、将步骤3所述的位于奇异域内的运动轨迹进行关节空间的重新规划，构造带有抛物线拟合的线性过渡函数，实现各关节角位移及角速度的平滑过渡，使六自由度工业机器人通过奇异域。

所述步骤1包括以下步骤：

步骤A、对六自由度工业机器人建立D-H坐标系，并根据D-H坐标系及其参数计算得工业六自由度工业机器人的各变换矩阵，对所述的变换矩阵进行运动学顺解及逆解分析；

步骤B、对笛卡尔空间的运动轨迹进行初步规划，使所述笛卡尔空间的运动轨迹的加速和减速的时间相等，并对指定的空间路径进行离散插值，获得各个离散插值点处的笛卡尔空间的位置和姿态，所述笛卡尔空间的运动轨迹的末端速度采用梯形法进行规划。

所述步骤3包括以下步骤：

步骤Ⅰ、对六自由度工业机器人进行雅克比矩阵分析，对应于雅克比矩阵奇异的不同情形，六自由度工业机器人奇异位形大体上分为前臂内部奇异、边界奇异和腕部奇异三种情况：设以腕部为参考点的雅可比矩阵为J_w，根据雅可比矩阵的定义，计算出腕部为参考点的雅可比矩阵：

$\begin{array}{c}{J}_w=\left[\begin{array}{ccccc}{z}_1\×(p_w-p_1)& ...& 0_{3\×1}& 0_{3\×1}& 0_{3\×1}\\ z_1& & z_{n-3}& z_{n-2}& z_{n-1}\end{array}\right]\\ =\left[\begin{array}{cc}{J}_{11}& 0_{3\×3}\\ J_{21}& J_{22}\end{array}\right]\end{array},$

其中，J₁₁，J₂₁，J₂₂为雅可比矩阵的分块矩阵；

六自由度工业机器人奇异形位大体上分为边界奇异、前臂内部奇异和腕部奇异3种情况，分别对应于det(J₁₁)=0和det(J₂₂)=0的情况，即：

$\left\{\begin{array}{c}\det \left(J_{11}\right)=a_2\×(a_3\·s_3+d_4\·c_3)\×(a_3\·c_{23}-d_4\·s_{23}+a_2\·c_2+a_1)\\ \det \left(J_{22}\right)=-s_5\end{array}\right.,,$

由det(J₁₁)=0得：sing1={θ∈R⁶|a₃·s₃+d₄·c₃=0}，

sing2={θ∈R⁶|a₂·c₂₃-d₄·s₂₃+a₂·c₂+a₁=0}，

式中：sing1为边界奇异；sing2为前臂内部奇异；

由det(J22)=0得：sing3={θ∈R⁶|-s₅=0}，即θ₅=0°，

式中：sing3为腕部奇异；

故六自由度工业机器人的奇异形位为sing={sing1∪sing2∪sing3}；

对边界奇异的区域、前臂内部奇异的区域和腕部奇异的区域进行参数设置，前臂内部奇异域的判别因子、边界奇异域的判别因子和腕部奇异域的判别因子分别设置为ε₁、ε₂和ε₃；

步骤Ⅱ、六自由度工业机器人是否进入奇异域由奇异因子参数k₁、k₂和k₃来确定，前臂内部奇异的奇异因子、边界奇异的奇异因子和腕部奇异的奇异因子分别为k₁、k₂和k₃，其中，k₁、k₂和k₃的值由步骤2所述的各插值点处的各关节变量及六自由度工业机器人的相关参数计算得出：

内部奇异因子k₁的计算公式为：k₁=a₃·c₂₃-d₄·s₂₃+a₂·c₂+a₁,

边界奇异因子k₂的计算公式为：k₂=a₃·s₃+d₄·c₃，

腕部奇异因子k₃的计算公式为：k₃=s₅，

式中：

c₂₃=cos(θ₂)·cos(θ₃)-sin(θ₂)·sin(θ₃)，

s₂₃=cos(θ₂)·sin(θ₃)+sin(θ₂)·cos(θ₃)，

c₂=cos(θ₂)，

s₂=sin(θ₂)，

c₃=cos(θ₃)，

s₃=sin(θ₃)，

s₅=sin(θ₅)，

其中，θ₂、θ₃和θ₅分别为六自由度工业机器人的关节二、关节三和关节五的关节变量，a₁、a₂、a₃和d₄为六自由度工业机器人连杆参数：a₁、a₂和a₃分别为连杆一的连杆长度、连杆二的连杆长度和连杆三的连杆长度，d₄为关节三和关节四的关节距离；

步骤Ⅲ、将步骤Ⅱ所述的奇异因子k₁、k₂和k₃与设置的相应奇异域的判别因子ε₁、ε₂和ε₃进行比较，当|k₁|<ε₁时，六自由度工业机器人位于内部奇异位形；当|k₂|<ε₂时，六自由度工业机器人位于边界奇异位形；当|k₃|<ε₃时，六自由度工业机器人位于腕部奇异位形。

所述步骤4包括以下步骤：

步骤ⅰ、若指定的空间路径中各点均未经过奇异域，则可直接将原规划所得的轨迹直接输出至六自由度工业机器人控制器；

步骤ⅱ、若指定的空间路径途经奇异域，则对经过奇异域的运动轨迹进行关节空间的重新规划，以六自由度工业机器人在进入奇异域及离开奇异域区间内各关节角位移、角速度及角加速度的关节变量构造一条带有抛物线拟合的线性过渡函数，使各关节角位移及角速度平滑过渡；

步骤ⅲ、将重新规划后的运动轨迹输出至六自由度工业机器人控制器，使六自由度工业机器人通过奇异域。

该技术方案可具体描述如下：

当六自由度工业机器人的位置及姿态是在空间相互正交的轴上测量，即在直角坐标系中测量时，这个空间称为笛卡尔空间。对六自由度工业机器人，它的所有连杆位置可由一组6个关节变量加以确定，这样的一组变量被称为6×1的关节矢量，所有关节矢量组成的空间称为关节空间。

对奇异域的参数进行设置，六自由度工业机器人的内部奇异、边界奇异和腕部奇异分别由奇异因子k₁、k₂和k₃参数来确定，当奇异因子等于0时，六自由度工业机器人位于相应的奇异点，其关节速度理论上趋于无穷大，而实际应用中电机的速度具有上限而引起六自由度工业机器人无法运行。在六自由度工业机器人中，奇异因子的计算公式分别为：

内部奇异因子：k₁=a₃·c₂₃-d₄·s₂₃+a₂·c₂+a₁，

边界奇异因子：k₂=a₃·s₃+d₄·c₃，

腕部奇异因子：k₃=s₅，

上述公式中：

c₂₃=cos(θ₂)·cos(θ₃)-sin(θ₂)·sin(θ₃)，

s₂₃=cos(θ₂)·sin(θ₃)+sin(θ₂)·cos(θ₃)，

c₂=cos(θ₂)，

s₂=sin(θ₂)，

c₃=cos(θ₃)，

s₃=sin(θ₃)，

s₅=sin(θ₅)，

其中，θ₂、θ₃和θ₅分别为六自由度工业机器人的关节二、关节三和关节五的关节变量，a₁、a₂、a₃和d₄为六自由度工业机器人连杆参数：a₁、a₂和a₃为连杆一的连杆长度、连杆二的连杆长度和连杆三的连杆长度，d₄为关节三和关节四的关节距离。

设置奇异域的阈值，ε₁、ε₂和ε₃分别为六自由度工业机器人内部奇异、边界奇异和腕部奇异的判别阈值，当|k₁|<ε₁时，六自由度工业机器人位于内部奇异位形，当|k₂|<ε₂时，六自由度工业机器人位于边界奇异位形，当|k₃|<ε₃时，六自由度工业机器人位于腕部奇异位形。

在仿真后对所记录的各个时刻各关节的角位移进行计算，判别六自由度工业机器人是否位于奇异域内并记录其进入奇异域和离开奇异域时各关节的角位移、角速度及角加速度三个关节变量。对经过奇异域的运动轨迹在关节空间进行新的规划，采用与抛物线拟合的线性函数进行各关节角位移和角速度的平滑过渡，达到顺利通过奇异域的目的。为了生成一条角位移和角速度都连续的平滑运动轨迹，在进入奇异域和离开奇异域处对各关节的角位移曲线增加一段抛物线拟合区域，两段抛物线之间为线性插值区域，则可由直线函数和两个抛物线函数组合成一条完整的位置与速度均连续的路径。

在运动轨迹开始的抛物线拟合区段内，将使用恒定的加速度（即所记录的进入奇异域时刻的加速度）改变速度，使速度由初始状态值（即所记录的进入奇异域时刻的速度）平滑地变化至设定值；此后进入直线拟合区段，此时加速度为零，而速度保持恒定，在运动轨迹末段的抛物线拟合区段内，同样使用恒定的加速度（即所记录的离开奇异域时刻的加速度）平滑地改变速度，使速度匀速变化至终止状态值（即所记录的离开奇异域时刻的速度）。这三段拟合区段的总位移即为六自由度工业机器人位于奇异域内的角位移，至此可根据上述拟合方法建立方程组并求解，所列方程组如下：

v₁=v₀+a₀·(t₁-t₀)，

v_f=v₁-a_f·(t_f-t₂)，

$\mathrm{\&Delta;}{\mathrm{\&theta;}}_1=v_0\&CenterDot;(t_1-t_0)+\frac{1}{2}\&CenterDot;a_0\&CenterDot;{(t_1-t_0)}^2,$

Δθ₂=v₁·(t₂-t₁)，

$\mathrm{\&Delta;}{\mathrm{\&theta;}}_3=v_1\&CenterDot;(t_f-t_2)-\frac{1}{2}\&CenterDot;a_f\&CenterDot;{(t_f-t_2)}^2,$

Δθ₁+Δθ₂+Δθ₃=θ_f-θ₀，

其中，θ₀为初始角位移，θ_f为终止角位移，v₀为初始角速度，v_f为终止角速度，a₀为初始角加速度，a_f为终止角加速度，t₀为初始时刻，t_f为终止时刻，则上述各量可由记录得到。t₁为运动轨迹开始的抛物线拟合区段运动终止时刻，t₂为直线拟合段运动终止时刻，v₁为直线段运动的角速度，Δθ₁为运动轨迹开始的抛物线拟合区段的运动位移，Δθ₂为直线拟合段运动位移，Δθ₃为运动轨迹末段的抛物线拟合区段的运动位移，上述六个变量为未知量。

求解上述方程组即可得到在关节空间由直线函数和两个抛物线函数拟合而成的一条完整的位置与速度均连续的路径，在求解出上文中六个未知变量Δθ₁、Δθ₂、Δθ₃、t₁、t₂、v₁后，其关节空间轨迹规划后的描述方程可表述为：

$\mathrm{\&theta;}=\left\{\begin{array}{cc}{\mathrm{\&theta;}}_0+v_0\&CenterDot;(t-t_0)+\frac{1}{2}\&CenterDot;a_0\&CenterDot;{(t-t_0)}^2& t\&Element;[t_0,t_1)\\ {\mathrm{\&theta;}}_0+\mathrm{\&Delta;}{\mathrm{\&theta;}}_1+v_1\&CenterDot;(t-t_1)& t\&Element;[t_1,t_2)\\ {\mathrm{\&theta;}}_0+\mathrm{\&Delta;}{\mathrm{\&theta;}}_1+\mathrm{\&Delta;}{\mathrm{\&theta;}}_2+v_1\&CenterDot;(t-t_2)-\frac{1}{2}\&CenterDot;a_f\&CenterDot;{(t-t_2)}^2& t\&Element;[t_2,t_f]\end{array}\right.$

其中，θ为对应于t时刻的关节角位移。

将该关节空间的轨迹方程与笛卡尔空间的轨迹规划相结合，重新进行运动轨迹的规划则可以达到六自由度工业机器人顺利通过奇异域的目的。

本发明的工作原理：在离线仿真软件中对工业机器人进行笛卡尔空间的轨迹规划，并记录路径中各点的坐标值及各点处的各关节角位移、角速度及角加速度这三个关节的变量，以获得其运动路径中每个点的坐标值及各点处机器人各关节的角位移、角速度及角加速度；对六自由度工业六自由度工业机器人的奇异域进行设置，在离线仿真后对所记录的各个时刻各关节的关节变量进行计算，判别机器人是否位于奇异域内，再对经过奇异域的运动轨迹进行关节空间的重新规划，提取出其进入奇异域和离开奇异域时各关节的关节变量，构造带有抛物线拟合的线性过渡函数实现各关节角位移及角速度的平滑过渡，得到新的运动轨迹，以达到机器人顺利通过奇异域的目的。

本发明相对于现有技术具有如下的优点及效果：

1、通过离线仿真可预先判别工业机器人是否位于奇异位形，并对位于奇异位形内的运动轨迹进行重新规划，再将结果写入机器人控制器中以控制机器人顺利通过奇异域。

2、本发明的方法简单易行，能很好地解决目前工业机器人在遇到奇异域时出现关节角速度突变而导致运行不稳定的问题。

附图说明

图1是基于离线仿真的通过奇异域算法流程图。

图2是六自由度工业机器人D-H坐标系及参数示意图。

图3a是关节角位移与时间的关系图。

图3b是关节角速度与时间的关系图。

图3c是关节角加速度与时间的关系图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

实施例

如图1所示，在离线仿真软件中对工业机器人进行笛卡尔空间的运动轨迹规划，获得其运动路径中每个点的坐标值及各点处机器人各关节的角位移、角速度及角加速度。对所获得的各关节变量进行计算并判别机器人是否位于奇异域内，如果机器人的路径并未经过奇异域则可直接输出，如果机器人的路径途径奇异域则对经过奇异域的运动轨迹进行关节空间的重新规划，构造带有抛物线拟合的线性过渡函数实现各关节角位移及角速度的平滑过渡以达到机器人顺利通过奇异域的目的。（当机器人的位置及姿态是在空间相互正交的轴上测量，即在直角坐标系中测量时，这个空间称为笛卡尔空间。对六自由度工业机器人，它的所有连杆位置可由一组6个关节变量加以确定，这样的一组变量被称为6×1的关节矢量，所有关节矢量组成的空间称为关节空间。）

如图2所示，对六自由度工业机器人建立D-H坐标系，图中坐标系{1}（X₁，Y₁，Z₁）为关节1坐标系，坐标系{2}（X₂，Y₂，Z₂）为关节2坐标系，坐标系{3}（X₃，Y₃，Z₃）为关节3坐标系，关节4、5和6的轴线相交于同一点，交点与坐标系{4}（X₄，Y₄，Z₄）、坐标系{5}（X₅，Y₅，Z₅）、坐标系{6}（X₆，Y₆，Z₆）的原点重合，图中a₁，a₂，a₃，d₄为六自由度工业机器人的连杆参数：a₁、a₂和a₃为连杆一、二、三的连杆长度，d₄为关节三和关节四的关节距离。根据D-H坐标系及其参数计算得六自由度工业机器人的各变换矩阵，对其进行运动学顺解及逆解。

六自由度工业机器人的雅可比矩阵J(q)是从关节空间速度向笛卡尔空间速度映射的线性变换，根据数学公式推算六自由度工业机器人的雅克比矩阵。

由于六自由度工业机器人末端与腕部是相对固定的，为便于能简单的进行奇异分析和通过奇异域算法的有效性和可行性研究，这里简化为以腕部为参考点的运动学问题。设以腕部为参考点的雅可比矩阵为J_w，根据雅可比矩阵的定义，计算出腕部为参考点的雅可比矩阵：

$\begin{array}{c}{J}_w=\left[\begin{array}{ccccc}{z}_1\&times;(p_w-p_1)& ...& 0_{3\&times;1}& 0_{3\&times;1}& 0_{3\&times;1}\\ z_1& & z_{n-3}& z_{n-2}& z_{n-1}\end{array}\right]\\ =\left[\begin{array}{cc}{J}_{11}& 0_{3\&times;3}\\ J_{21}& J_{22}\end{array}\right]\end{array},$

其中，J₁₁，J₂₁，J₂₂为雅可比矩阵的分块矩阵。

六自由度工业机器人奇异形位大体上分为边界奇异、前臂内部奇异和腕部奇异3种情况，分别对应于det(J₁₁)=0和det(J₂₂)=0的情况，即

$\left\{\begin{array}{c}\det \left(J_{11}\right)=a_2\&times;(a_3\&CenterDot;s_3+d_4\&CenterDot;c_3)\&times;(a_3\&CenterDot;c_{23}-d_4\&CenterDot;s_{23}+a_2\&CenterDot;c_2+a_1)\\ \det \left(J_{22}\right)=-s_5\end{array}\right.,,$

由det(J₁₁)=0得：sing1={θ∈R⁶|a₃·s₃+d₄·c₃=0}，

sing2={θ∈R⁶|a₂·c₂₃-d₄·s₂₃+a₂·c₂+a₁=0}，

式中：sing1为边界奇异；sing2为前臂内部奇异。

由det(J₂₂)=0得：sing3={θ∈R⁶|-s₅=0}，即θ₅=0°，

式中：sing3为腕部奇异。

故六自由度工业机器人的奇异形位为sing={sing1∪sing2∪sing3}，

对奇异域的参数进行设置，六自由度工业机器人的内部奇异、边界奇异和腕部奇异分别由奇异因子k₁、k₂和k₃参数来确定，当奇异因子等于0时，六自由度工业机器人位于相应的奇异点，其关节速度理论上趋于无穷大，而实际应用中电机的速度具有上限而引起六自由度工业机器人无法运行。在六自由度工业机器人中，奇异因子的计算公式及奇异域的判别因子分别为：

内部奇异因子：k₁=a₃·c₂₃-d₄·s₂₃+a₂·c₂+a₁,

其判别因子：ε₁，当|k₁|<ε₁时，六自由度工业机器人位于内部奇异位形。

边界奇异因子：k₂=a₃·s₃+d₄·c₃，

其判别因子：ε₂，当|k₂|<ε₂时，六自由度工业机器人位于边界奇异位形。

腕部奇异因子：k₃=s₅，

其判别因子：ε₃，当|k₃|<ε₃时，六自由度工业机器人位于腕部奇异位形。

上述公式中，

c₂₃=cos(θ₂)·cos(θ₃)-sin(θ₂)·sin(θ₃)，

s₂₃=cos(θ₂)·sin(θ₃)+sin(θ₂)·cos(θ₃)，

c₂=cos(θ₂)，

s₂=sin(θ₂)，

c₃=cos(θ₃)，

s₃=sin(θ₃)，

s₅=sin(θ₅)，

其中，θ₂、θ₃和θ₅分别为六自由度工业机器人的关节二、关节三和关节五的关节变量，a₁、a₂、a₃和d₄为六自由度工业机器人连杆参数：a₁、a₂和a₃分别为连杆一的连杆长度、连杆二的连杆长度和连杆三的连杆长度，d₄为关节三和关节四的关节距离。

在离线仿真软件中对六自由度工业机器人进行笛卡尔空间的运动轨迹规划，其末端速度采用梯形法进行规划，其加速和减速的时间相等，对各插值点进行运动学逆解，获得运动路径中各个插值点的坐标及逆解计算后所得的各点处各关节角位移、角速度及角加速度三个关节变量并通过延时模块记录上一时刻所输出的各关节的运动变量。

仿真后对所记录的各个时刻各关节的角位移进行计算，判别六自由度工业机器人是否位于奇异域内并提取出其进入奇异域和离开奇异域时各关节的角位移、角速度及角加速度三个关节变量。在关节空间对位于奇异域内的运动轨迹进行重新规划，采用与抛物线拟合的线性函数进行各关节角位移和角速度的平滑过渡，达到顺利通过奇异域的目的。如图3a所示，图中，θ-t为关节角位移与时间的关系图，如图3b所示，图中，v-t为关节角速度与时间的关系图，如图3c所示，图中，a-t为关节角加速度与时间的关系图。为了生成一条角位置和角速度都连续的平滑运动轨迹，在进入奇异域和离开奇异域处对各关节的角位移曲线增加一段抛物线拟合区域，两段抛物线之间为线性插值区域，则可由直线函数和两个抛物线函数组合成一条完整的位置与速度均连续的运动轨迹。

在运动轨迹开始的抛物线拟合区段内（即由t₀到t₁时间内），将使用恒定的加速度a₀改变速度，使速度由初始状态值v₀平滑地变化至设定值v₁；此后进入直线拟合区段（即由t₁到t₂时间内），此时加速度为零，而速度保持恒定，在运动轨迹末段的抛物线拟合区段内（即由t₂到t_f时间内），同样使用恒定的加速度a_f平滑地改变速度，使速度匀速变化至终止状态值v_f。这三段拟合区段的总位移即为六自由度工业机器人位于奇异域内的角位移(θ_f-θ₀),至此可根据上述拟合方法建立方程组并求解。

列方程组如下：

v₁=v₀+a₀·(t₁-t₀)，

v_f=v₁-a_f·(t_f-t₂)，

$\mathrm{\&Delta;}{\mathrm{\&theta;}}_1=v_0\&CenterDot;(t_1-t_0)+\frac{1}{2}\&CenterDot;a_0\&CenterDot;{(t_1-t_0)}^2,$

Δθ₂=v₁·(t₂-t₁)，

$\mathrm{\&Delta;}{\mathrm{\&theta;}}_3=v_1\&CenterDot;(t_f-t_2)-\frac{1}{2}\&CenterDot;a_f\&CenterDot;{(t_f-t_2)}^2,$

Δθ₁+Δθ₂+Δθ₃=θ_f-θ₀，

其中，θ₀为初始角位移，θ_f为终止角位移，v₀为初始角速度，v_f为终止角速度，a₀为初始角加速度，a_f为终止角加速度，t₀为初始时刻，t_f为终止时刻，则上述各量可由记录得到。t₁为运动轨迹开始的抛物线拟合区段运动终止时刻，t₂为直线拟合段运动终止时刻，v₁为直线段运动的角速度，Δθ₁为运动轨迹开始的抛物线拟合区段的运动位移，Δθ₂为直线拟合段运动位移，Δθ₃为运动轨迹末段的抛物线拟合区段的运动位移，上述六个变量为未知量。

求解上述方程组即可得到在关节空间由直线函数和两个抛物线函数拟合而成的一条完整的位置与速度均连续的路径，在求解出上文中六个未知变量Δθ₁、Δθ₂、Δθ₃、t₁、t₂、v₁后，其关节空间轨迹规划后的描述方程可表述为：

$\mathrm{\&theta;}=\left\{\begin{array}{cc}{\mathrm{\&theta;}}_0+v_0\&CenterDot;(t-t_0)+\frac{1}{2}\&CenterDot;a_0\&CenterDot;{(t-t_0)}^2,& t\&Element;[t_0,t_1),\\ {\mathrm{\&theta;}}_0+\mathrm{\&Delta;}{\mathrm{\&theta;}}_1+v_1\&CenterDot;(t-t_1),& t\&Element;[t_1,t_2),\\ {\mathrm{\&theta;}}_0+\mathrm{\&Delta;}{\mathrm{\&theta;}}_1+\mathrm{\&Delta;}{\mathrm{\&theta;}}_2+v_1\&CenterDot;(t-t_2)-\frac{1}{2}\&CenterDot;a_f\&CenterDot;{(t-t_2)}^2,& t\&Element;[t_2,t_f],\end{array}\right.$

其中，θ为对应于t时刻的关节角位移。

将该关节空间的轨迹方程与笛卡尔空间的轨迹规划相结合，则可以实现通过离线仿真使六自由度工业机器人顺利通过奇异域的目的。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (5)

1.一种六自由度工业机器人通过奇异域的方法，其特征在于，包括以下步骤：

步骤1、对六自由度工业机器人进行运动学分析，利用离线仿真软件对六自由度工业机器人进行笛卡尔空间的运动轨迹规划；

步骤2、将步骤1中所述的运动轨迹中各个插值点的坐标值及其姿态进行运动学逆解，得到各插值点处六自由度工业机器人各个关节的角位移关节变量、角速度关节变量和角加速度关节变量；

步骤3、对六自由度工业机器人进行奇异域的设置，对步骤2中所述的各插值点处各关节变量进行计算，并判别该插值点是否位于奇异域内，若位于奇异域内，则进一步判别奇异位形的种类；

步骤4、将步骤3中所述的位于奇异域内的运动轨迹进行关节空间的重新规划，构造带有抛物线拟合的线性过渡函数，实现各关节角位移及角速度的平滑过渡，使六自由度工业机器人通过奇异域。

2.如权利要求1所述的六自由度工业机器人通过奇异域的方法，其特征在于，所述步骤1包括以下步骤：

步骤A、对六自由度工业机器人建立D-H坐标系，并根据D-H坐标系及其参数计算得到工业六自由度工业机器人的各变换矩阵，对所述的变换矩阵进行运动学顺解及逆解分析；

步骤B、对笛卡尔空间的运动轨迹进行初步规划，使所述笛卡尔空间的运动轨迹的加速和减速的时间相等，并对指定的空间路径进行离散插值，获得各个离散插值点处的笛卡尔空间的位置和姿态，所述笛卡尔空间的运动轨迹的末端速度采用梯形法进行规划。

3.如权利要求1所述的六自由度工业机器人通过奇异域的方法，其特征在于，所述步骤3包括以下步骤：

步骤Ⅰ、对六自由度工业机器人进行雅克比矩阵分析，六自由度工业机器人的奇异位形分为前臂内部奇异、边界奇异和腕部奇异；设以腕部为参考点的雅可比矩阵为J_w，根据雅可比矩阵的定义，计算出腕部为参考点的雅可比矩阵：

$\begin{array}{c}{J}_w=\left[\begin{array}{ccccc}{z}_1\&times;(p_w-p_1)& ...& 0_{3\&times;1}& 0_{3\&times;1}& 0_{3\&times;1}\\ z_1& & z_{n-3}& z_{n-2}& z_{n-1}\end{array}\right]\\ =\left[\begin{array}{cc}{J}_{11}& 0_{3\&times;3}\\ J_{21}& J_{22}\end{array}\right],\end{array}$

其中，J₁₁，J₂₁，J₂₂为雅可比矩阵的分块矩阵；

六自由度工业机器人奇异形位分为边界奇异、前臂内部奇异和腕部奇异，对应于det(J₁₁)=0和det(J₂₂)=0的情况，即：

$\left\{\begin{array}{c}\det \left(J_{11}\right)=a_2\&times;(a_3\&CenterDot;s_3+d_4\&CenterDot;c_3)\&times;(a_3\&CenterDot;c_{23}-d_4\&CenterDot;s_{23}+a_2\&CenterDot;c_2+a_1)\\ \det \left(J_{22}\right)=-s_5\end{array}\right.,,$

由det(J₁₁)=0得：sing1={θ∈R⁶|a₃·s₃+d₄·c₃=0}，

sing2={θ∈R⁶|a₂·c₂₃-d₄·s₂₃+a₂·c₂+a₁=0}，

式中：sing1为边界奇异；sing2为前臂内部奇异；

由det(J22)=0得：sing3={θ∈R⁶|-s₅=0}，即：θ₅=0°，

式中：sing3为腕部奇异；

得到六自由度工业机器人的奇异形位为：

sing={sing1∪sing2∪sing3}；

对边界奇异的区域、前臂内部奇异的区域和腕部奇异的区域进行参数设置，前臂内部奇异域的判别因子、边界奇异域的判别因子和腕部奇异域的判别因子分别设置为ε₁、ε₂和ε₃；

步骤Ⅱ、六自由度工业机器人是否进入奇异域由奇异因子参数k₁、k₂和k₃来确定，前臂内部奇异的奇异因子、边界奇异的奇异因子和腕部奇异的奇异因子分别为k₁、k₂和k₃，其中，k₁、k₂和k₃的值由步骤2所述的各插值点处的各关节变量及六自由度工业机器人的相关参数计算得出：

内部奇异因子k₁的计算公式为：k₁=a₃·c₂₃-d₄·s₂₃+a₂·c₂+a₁,

边界奇异因子k₂的计算公式为：k₂=a₃·s₃+d₄·c₃，

腕部奇异因子k₃的计算公式为：k₃=s₅，

式中：

c₂₃=cos(θ₂)·cos(θ₃)-sin(θ₂)·sin(θ₃)，

s₂₃=cos(θ₂)·sin(θ₃)+sin(θ₂)·cos(θ₃)，

c₂=cos(θ₂)，

s₂=sin(θ₂)，

c₃=cos(θ₃)，

s₃=sin(θ₃)，

s₅=sin(θ₅)，

其中，θ₂、θ₃和θ₅分别为六自由度工业机器人的关节二、关节三和关节五的关节变量，a₁、a₂、a₃和d₄为六自由度工业机器人的连杆参数：a₁、a₂和a₃分别为连杆一的连杆长度、连杆二的连杆长度和连杆三的连杆长度，d₄为关节三和关节四之间的关节距离；

步骤Ⅲ、将步骤Ⅱ所述的奇异因子k₁、k₂和k₃与设置的相应奇异域的判别因子ε₁、ε₂和ε₃进行比较，当|k₁|<ε₁时，六自由度工业机器人位于内部奇异位形；当|k₂|<ε₂时，六自由度工业机器人位于边界奇异位形；当|k₃|<ε₃时，六自由度工业机器人位于腕部奇异位形。

4.如权利要求1所述的六自由度工业机器人通过奇异域的方法，其特征在于，所述步骤4包括以下步骤：

步骤ⅰ、若指定的空间路径中各点均未经过奇异域，则可直接将原规划所得的轨迹直接输出至六自由度工业机器人控制器；

步骤ⅱ、若指定的空间路径途经奇异域，则对经过奇异域的运动轨迹进行关节空间的重新规划，以六自由度工业机器人在进入奇异域及离开奇异域区间内各关节角位移、角速度及角加速度的关节变量构造一条带有抛物线拟合的线性过渡函数，使各关节角位移及角速度平滑过渡；

步骤ⅲ、将重新规划后的运动轨迹输出至六自由度工业机器人控制器，使六自由度工业机器人通过奇异域。

5.如权利要求4所述的六自由度工业机器人通过奇异域的方法，其特征在于，所述步骤ⅱ包括以下步骤：

①在运动轨迹开始的由t₀到t₁时间内的抛物线拟合区段内，使用恒定的加速度a₀改变速度，使速度由初始状态值v₀平滑地变化至设定值v₁；

②进入由t₁到t₂时间内的直线拟合区段内，此时加速度为零，而速度保持恒定，在运动轨迹末段的由t₂到t_f时间内的抛物线拟合区段内，使用恒定的加速度a_f平滑地改变速度，使速度匀速变化至终止状态值v_f；由t₀到t₁时间内的抛物线拟合区段、由t₁到t₂时间内的直线拟合区段和由t₂到t_f时间内的抛物线拟合区段的总位移即为六自由度工业机器人位于奇异域内的角位移(θ_f-θ₀)；

③建立方程组并求解：

建立方程组如下：

v₁=v₀+a₀·(t₁-t₀)，

v_f=v₁-a_f·(t_f-t₂)，

$\mathrm{\&Delta;}{\mathrm{\&theta;}}_1=v_0\&CenterDot;(t_1-t_0)+\frac{1}{2}\&CenterDot;a_0\&CenterDot;{(t_1-t_0)}^2,$

Δθ₂=v₁·(t₂-t₁)，

$\mathrm{\&Delta;}{\mathrm{\&theta;}}_3=v_1\&CenterDot;(t_f-t_2)-\frac{1}{2}\&CenterDot;a_f\&CenterDot;{(t_f-t_2)}^2,$

Δθ₁+Δθ₂+Δθ₃=θ_f-θ₀，

其中，θ₀为初始角位移，θ_f为终止角位移，v₀为初始角速度，v_f为终止角速度，a₀为初始角加速度，a_f为终止角加速度，t₀为初始时刻，t_f为终止时刻；t₁为运动轨迹开始的抛物线拟合区段运动终止时刻，t₂为直线拟合段运动终止时刻，v₁为直线段运动的角速度，Δθ₁为运动轨迹开始的抛物线拟合区段的运动位移，Δθ₂为直线拟合段运动位移，Δθ₃为运动轨迹末段的抛物线拟合区段的运动位移，变量Δθ₁、Δθ₂、Δθ₃、t₁、t₂和v₁均为未知量；

求解上述方程组，得到在关节空间由直线函数和两个抛物线函数拟合而成的一条完整的位置与速度均连续的路径，求解出所述Δθ₁、Δθ₂、Δθ₃、t₁、t₂和v₁后，六自由度工业机器人的关节空间轨迹规划后的描述方程表述为：

$\mathrm{\&theta;}=\left\{\begin{array}{cc}{\mathrm{\&theta;}}_0+v_0\&CenterDot;(t-t_0)+\frac{1}{2}\&CenterDot;a_0\&CenterDot;{(t-t_0)}^2,& t\&Element;[t_0,t_1),\\ {\mathrm{\&theta;}}_0+\mathrm{\&Delta;}{\mathrm{\&theta;}}_1+v_1\&CenterDot;(t-t_1),& t\&Element;[t_1,t_2),\\ {\mathrm{\&theta;}}_0+\mathrm{\&Delta;}{\mathrm{\&theta;}}_1+\mathrm{\&Delta;}{\mathrm{\&theta;}}_2+v_1\&CenterDot;(t-t_2)-\frac{1}{2}\&CenterDot;a_f\&CenterDot;{(t-t_2)}^2,& t\&Element;[t_2,t_f],\end{array}\right.$

其中，θ为对应于t时刻的关节角位移；

④将所述的关节空间轨迹方程与笛卡尔空间的轨迹规划相结合，得到重新规划的运动轨迹。