[发明专利]基于不满足Pieper准则的六自由度串联机器人的控制方法

权利要求书

1.基于不满足Pieper准则的六自由度串联机器人的控制方法，其特征在于：包括以下步骤：

步骤一：将不满足Pieper准则的六自由度串联机器人进行同等变化，使其变换到满足Pieper准则的六自由度串联机器人模型上；

虽然该类型串联机器人不满足Pieper准则，但将六轴串联机械机器人的末端执行臂进行变换，经过将末端转到关节轴线进行平移变形之后，使得末端最后三个转动关节轴线的延长线相交于一点；

步骤二：在对所述六自由度串联机器人进行正运动学分析的时候，求出偏置的矢量方向参数；

机器人正运动学分析就是已知杆件几何参数和关节角矢量，求机器人末端执行器相对于固定参考坐标的位置和姿态；

在进行机器人正运动学分析之前，建立坐标系{i}相对于坐标系{i-1}的变换；一般这个变换是由四个参数构成的；对任意给定的机器人，这个变换是只有一个变量的函数，另外三个参数是由机械系统来确定的；该六自由度串联机器人的连杆间变换参数如下表所示；

其中：

α_i为绕第i坐标系下的X_i轴，从第i坐标系下的Z_i轴旋转到第i+1坐标系下的Z_i+1轴的角度；

a_i为沿第i坐标系下的X_i轴，从第i坐标系下的Z_i轴方向移动到第i+1坐标系下Z_i+1轴的距离；

d_i为沿第i坐标系下的Z_i轴，从第i-1坐标系下的X_i-1轴移动到第i坐标系下的X_i轴的距离；

θ_i为绕第i坐标系下的Z_i轴，从第i-1坐标系下的X_i-1轴旋转到第i坐标系下的X_i轴的角度；

用齐次坐标变换矩阵来表示机器人的正解运算：表示得是从i坐标到j坐标系之间的4×4的传递矩阵；式中：将sθ_i表示的是sinθ_i；cθ_i表示的是cosθ₁；且i＝1,2,3...6；

其中：

θ₂₃＝θ₂+θ₃

n_x＝cθ₁cθ₂₃cθ₄cθ₅+sθ₁sθ₄cθ₅+cθ₁sθ₂₃sθ₅

n_y＝sθ₁sθ₂₃sθ₅+sθ₁cθ₂₃cθ₄cθ₅-cθ₁sθ₄cθ₅

n_z＝cθ₂₃sθ₅-sθ₂₃cθ₄cθ₅

o_x＝sθ₁cθ₄-cθ₁cθ₂₃sθ₄

o_y＝-cθ₁cθ₄-sθ₁cθ₂₃sθ₄

o_z＝sθ₂₃sθ₄

a_x＝cθ₁cθ₂₃cθ₄sθ₅-cθ₁sθ₂₃cθ₅+sθ₁sθ₄sθ₅

a_y＝sθ₁cθ₂₃cθ₄sθ₅-sθ₁sθ₂₃cθ₅-cθ₁sθ₄sθ₅

a_z＝-sθ₂₃cθ₄sθ₅-cθ₂₃cθ₅

p_x＝-d₄cθ₁sθ₂₃+l₂cθ₁cθ₂+d₃sθ₁-d₂sθ₁+d₆a_x+l₆o_x

p_y＝-d₄sθ₁sθ₂₃+l₂sθ₁cθ₂+d₂cθ₁-d₃cθ₁+d₆a_y+l₆o_y

p_z＝-d₄cθ₂₃-l₂sθ₂+d₆a_z+l₆o_z

将θ₆＝0带入上述方程中发现O_x、O_y、O_z这三个参数描述的正是偏执所在的矢量方向；其中，p_x’、p_y’、p_z’表示的是末端执行器的坐标位置，n_x、n_y、n_z、o_x、o_y、o_z、a_x、a_y、a_z表示的是末端执行器的坐标姿态；

步骤三：根据之前求得的偏置的矢量方向参数，对所述六自由度串联机器人进行坐标变化，将其变换到满足Pieper准则的六自由度串联机器人；

对于逆运动学分析而言，末端坐标系姿态的坐标是已知的，也就是说各项参数已知；但是由于θ₆的不确定性，无法通过已知，由来得出变换矩阵；采取“标定法”来解决这样的问题；

o_x5、o_y5、o_z5与步骤计算得出来的O_x、O_y、O_z参数数值是一致的，将上述正运动学分析求解出来的得出的o_x5、o_y5、o_z5加入到整个逆运动学分析中来，并对已知数据进行预处理：

其中：p_x'、p_y'、p_z'为已知参数，将改动后的p_x、p_y、p_z带入矩阵中；

该串联机器人不满足Pieper准则；该类型多轴串联机器人的末端三个关节的关节轴线的延长线未能相交于一点；其在末端执行器上的某一个位置上多出一个的相对的横向偏置；该偏置长度的位置状态具有一定的不确定性，采取对其末端坐标参数p_x'、p_y'、p_z'进行预先处理的方法，以消除由上述偏置不确定对求解问题所造成的影响；

步骤四：基于解析法对其进行逆运动学分析和计算；

由公式可得：

对于第一个轴关节角度，由上述方程两边的元素(2,4)相等，得到：

式中：

对于第三个关节角度，由上述方程两边的元素(1,4)、(3,4)相等，得到：

其中：

对于第二个关节角度，由上述方程两边的元素(1,4)(2,4)相等，得到：

那么，由θ₂₃＝θ₂+θ₃可得：

根据θ₁和θ₃解的四种可能组合，由上式得到相应的四种可能值，于是得到θ₂的四种可能解；

对于第四个关节角度，由上述方程两边的元素(1,3)、(3,3)相等可得：

θ₄＝Atan2(sθ₁a_x-cθ₁a_y，cθ₁cθ₂₃a_x+sθ₁cθ₂₃a_y-sθ₂₃a_z)

当sθ₅＝0时，机器人处于奇异位形，满足退化条件；此时，关节轴4和6重合，只能解出θ₆与θ₄的和与差；在奇异位形时，任意选取θ₄的角度，计算相应θ₆的值：

对于第五个轴关节角度，由上述方程两端的元素(1,3)、(3,3)相等可得：

θ₅＝Atan2(±M₁，M₂)

其中：

M₁＝(cθ₁cθ₂₃cθ₄+sθ₁sθ₄)a_x+(sθ₁cθ₂₃cθ₄-cθ₁sθ₄)a_y-sθ₂₃cθ₄a_z

M₂＝-cθ₁sθ₂₃a_x-sθ₁sθ₂₃a_y-cθ₂₃a_z

对于第六个关节角度而言，可得：

其中：

n＝cθ₁cθ₂₃cθ₄cθ₅+sθ₁sθ₄cθ₅+cθ₁sθ₂₃sθ₅

o＝sθ₁cθ₄-cθ₁cθ₂₃sθ₄

步骤五：对所述六自由度串联机器人在进行逆运动学分析过程中存在的问题进行相关补充和说明；

1)对一般情况下机器人退化问题的分析

当sθ₅＝0时，机器人处于奇异位形，满足退化条件；

在实际求解的过程中，存在当sθ₅＝0机器人满足退化条件的情况；下面，就这种退化情况进行详细的分析；

以关节轴4为基础坐标而言，将sθ₅＝0,cθ₅＝±1求得的转换矩阵：

令(1,2)、(3,2)相等可得：

θ₄±θ₆＝Atan2(o_xsθ₁-o_ycθ₁,-o_xcθ₁cθ₂₃-o_ysθ₁cθ₂₃+o_zsθ₂₃)

由上述提到的o_x5、o_y5、o_z5参数，求出θ₆：

θ₆＝Atan2(o_x，o_x5)

综上可得：

其中：上式正负性的选择由cθ₅的正负来判断；

2)对特殊情况处于退化状态下的分析

前述对所述六自由度串联机器人的奇异位形进行了一般情况下分析；实际上，当该机器人运动到某一特殊点的时候，也会出现奇异的情况；由于这种情况仅存在与某一类位置点的状态；因此将其称为特殊情况下的奇异位形；

情况1：当d₂＝d₃，且关节轴1和4的轴线重合的时候，这种状态下无法解出θ₁和θ₄；这种情况会在两种情况下出现：一是该类位置点为运动轨迹的途经点；二是该类位置点是运动轨迹起始点；

如果该类位置点为运动轨迹的途经点的话，采用“继承法”来解决这个问题；θ₁的值默认的继承了附近位置点状态下θ₁的值，进而对θ₄进行求解；

如果该类位置点为运动轨迹的起始点的话，采用“重构法”来解决这个问题；θ₁的值默认为0，以便之后对θ₄进行求解；

情况2：当d₂＝d₃，且关节轴1和6的轴线重合的时候，这种状态下无法解出θ₁和θ₆；这种情况只存在于变换后的机器人模型之中，在原有的机器人模型中不存在奇异位形问题；但在上述逆运动学分析中，仍然会出现这类奇异位形的状态；

对于这种情况，采用“投影法”来解决这个问题；通过机器人在xoy平面上形成的投影来求出θ₁的数值；

步骤六：把求解出的运动参数，发到运动控制卡，对不满足Pieper准则六自由度串联机器人进行控制。