技术领域
本发明属于工业机器人控制领域，具体的，为一种基于工业机器人腕部奇异点计算的控制方法。
背景技术：
工业机器人可以对多个运动轴，包括方向、位置和工作流程进行编程。工业机器人通常包括具有多个轴的机器人臂以及可编程控制器(控制装置)，控制器在运行中控制或调整工业机器人的运动过程。
运动学奇异是机械结构不可避免的固有特性。其不仅限制了机器人真正运行空间，当机器人运动到奇异点时，其雅可比矩阵不存在逆解，从而使关节跟踪速度和加速度无穷大，形成冲击进而将造成工业机器人稳定性变差，跟踪能力下降等问题，这些不足严重影响机器人的运动性能。
针对机器人运动学奇异位形问题，提出一种计算六轴机器人腕部奇异位形的方法。通过计算出机器人处于奇异位形时各关节的状态，使机器人远离奇异位形，提高工业机器人性能。
技术实现要素：
本发明提供了一种基于计算工业机器人奇异位形的控制方法，可以克服机器人在奇异位形时稳定性差的缺陷。
为实现上述目的，本发明采用如下技术方案，
一种基于工业机器人腕部奇异点计算的控制方法，包括以下步骤：
步骤一：定义基坐标系和机器人机械臂关节坐标系，且保证机器人关节坐标系与基坐标系方向相同，根据机器人运动参数，建立机器人运动学模型；
步骤二：根据机器人机械臂运动学模型的连杆变换，求取机器人运动学正解；
步骤三：求解机器人机械臂的速度雅可比矩阵，得到包括机器人关节平动速度和转动速度的速度雅可比矩阵；
步骤四：对求取的速度雅可比矩阵进行解耦运算，将该矩阵按照平动速度和转动速度分为多个矩阵；
步骤五：根据解耦后的速度雅可比矩阵，计算整个雅可比矩阵的行列式结果，在行列式结果为零时，则为机器人机械臂的奇异点；
步骤六：在得到机器人机械臂的奇异点后，根据绕开奇异点的轨迹对机器人机械臂的控制。
所述对机器人进行运动学模型的连杆变换，其变换矩阵为：
并计算机器人运动学正解，计算公式和结果为：
其中：
nx＝-s5(s1s4+c4(c1c2s3+c1c3s2))-c5(c1s2s3-c1c2c3)
ny＝s5(c1s4-c4(c2s1s3+c3s1s2))-c5(s1s2s3-c2c3s1)
nz＝-s23c5-c23c4s5
ox＝s6(c5(s1s4+c4(c1c2s3+c1c3s2))-s5(c1s2s3-c1c2c3))-c6(c4s1-s4(c1c2s3+c1c3s2))
oy＝c6(c1c4+s4(c2s1s3+c3s1s2))-s6(c5(c1s4-c4(c2s1s3+c3s1s2))+s5(s1s2s3-c2c3s1))
oz＝c23c6s4-s6(s23s5-c23c4c5)
ax＝s6(c4s1-s4(c1c2s3+c1c3s2))+c6(c5(s1s4+c4(c1c2s3+c1c3s2))-s5(c1s2s3-c1c2c3))
ay＝-s6(c1c4+s4(c2s1s3+c3s1s2))-c6(c5(c1s4-c4(c2s1s3+c3s1s2))+s5(s1s2s3-c2c3s1))
az＝-c6(s23s5-c23c4c5)-c23s4s6
Px＝c1(L1+L4c23+L3s23+L2s2)
Py＝s1(L1+L4c23+L3s23+L2s2)
Pz＝L3c23-L4s23+L2c2
其中,c1-c6,s1-s6依次表示机器人各关节变量θ1-θ6的正弦值和余弦值，c23，s23分别表示关节变量θ2和θ3之和的正弦值和余弦值；L1表示机器人关节1和关节2之间连杆在水平方向上的长度，L2表示机器人关节2和关节3之间连杆长度，L3表示机器人关节3和关节4之间连杆在垂直方向上的长度，L4表示机器人关节3和关节4之间连杆在水平方向上的长度；nx-nz，ox-oz，ax-az表示组成旋转矩阵的三个单位向量，每个参数都是旋转矩阵的一部分；Px，Py，Pz表示机器人6轴原点坐标在机器人基坐标系下的表示。
所述求解机器人的速度雅可比矩阵，包括如下步骤：
步骤一：确定机械臂体速度、机械臂的速度雅可比矩阵关系，得到方程：
其中ξ为机械臂体速度，J为机械臂的速度雅可比矩阵，q为关节位置变量，为q的导数，为关节速度向量，且：
其中0vn代表末端执行器的线速度，0ωn代表末端执行器的角速度，Jv为雅可比矩阵的上半部分，定义关节速度向量与0vn的关系，Jω为雅可比矩阵的下半部分，定义关节速度向量/>与0ωn的关系
步骤二：确定机械臂末端执行器的线速度，采用微分链式法则处理，计算方程为：
其中为机械臂末端执行器的线速度，qi代表机器人第i个关节的位置变量，/>代表导数，为关节速度向量
由此确定矩阵Jv的第i列结果为：
计算方程为：
其中代表相对于坐标系0，第i个关节的轴方向的单位向量，/>代表第i个关节坐标系原点坐标；
步骤三：确定机器人机械臂速度雅可比矩阵中的转动关节角速度矩阵，可得到矩阵第i列的角速度，为：
其中Jωi代表速度雅可比矩阵第i列的角速度向量矩阵，其中代表相对于坐标系0，第i个关节的轴方向的单位向量。
所述对机器人机械臂的速度雅可比矩阵做解耦处理，处理方式为按平动关节的平动速度和转动关节的转动速度分解速度雅可比矩阵，得到：
其中JO为转动关节速度列向量矩阵，JP为平动关节的速度列向量矩阵，J11和J22代表分块矩阵中的中间矩阵，定义JO为：
由于关节4-6交于同一点o，因此可选择坐标系，使得o4＝o5＝o6＝o7＝o，其中o4、o5、o6和o7代表关节坐标系原点，则转动关节速度列向量矩阵可表示为：
其中和/>分别为机器人机械臂转动关节4～6的角速度向量。
所述对机器人机械臂的速度雅可比矩阵做解耦处理，由于手腕轴线交于同一点o，因此解耦得到的矩阵J12为零矩阵，即：
所述计算整个雅可比矩阵的行列式结果，即：
detJ＝detJ11detJ22，
其中对于detJ11、detJ22中的一个为0，或者两者都为0时，矩阵J11和J22中对应的机器人机械臂关节平动速度和/或转动速度为奇异点。
若矩阵J11和J22的行列式为0，则矩阵中的列向量线性相关。
本发明具有以下有益效果和优点：
1.可以解决工业机器人在运动中遇到腕部奇异点不能通过的问题，通过计算出机器人处于奇异位形时各关节的状态，使机器人远离奇异位形，提高工业机器人性能。
2.针对机器人运动学奇异位形问题，提出一种根据机器人正向运动学结合速度雅可比矩阵计算六轴机器人腕部奇异位形的方法。通过计算出机器人处于奇异位形时各关节的状态，使机器人远离奇异位形，提高工业机器人性能。
附图说明
图1工业机器人奇异位形计算流程图；
图2工业机器人运动学建模示意图；
图3工业机器腕部奇异位形示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
如图1所示，本发明包括如下步骤：
1.建立工业机器人的运动学模型
2.求解工业机器人运动学正解
3.求解工业机器人的速度雅可比矩阵
4.对雅可比矩阵进行解耦运算
5.求解工业机器人腕部奇异位形
建立工业机器人的运动学模型，根据6轴工业机器人关节和连杆结构，建立机器人运动学模型。
所述机器人关节和连杆结构，关节坐标系定义如图2所示：
图中{1}、{2}、{3}、{4}、{5}、{6}代表各个关节处的坐标系，其坐标系方向均定义为与基坐标系相同。坐标系{i}(i＝1，2，3)的原点定义为机器人i轴轴心和i+1轴轴心公垂线与机器人i轴轴心的交点，腕关节坐标系{4}、{5}、{6}原点定义为机器人四轴轴心和五轴轴心交点。
所述机器人运动学模型的连杆变换矩阵如下：
计算机器人运动学正解
其中
nx＝-s5(s1s4+c4(c1c2s3+c1c3s2))-c5(c1s2s3-c1c2c3)
ny＝s5(c1s4-c4(c2s1s3+c3s1s2))-c5(s1s2s3-c2c3s1)
nz＝-s23c5-c23c4s5
ox＝s6(c5(s1s4+c4(c1c2s3+c1c3s2))-s5(c1s2s3-c1c2c3))-c6(c4s1-s4(c1c2s3+c1c3s2))
oy＝c6(c1c4+s4(c2s1s3+c3s1s2))-s6(c5(c1s4-c4(c2s1s3+c3s1s2))+s5(s1s2s3-c2c3s1))
oz＝c23c6s4-s6(s23s5-c23c4c5)
ax＝s6(c4s1-s4(c1c2s3+c1c3s2))+c6(c5(s1s4+c4(c1c2s3+c1c3s2))-s5(c1s2s3-c1c2c3))
ay＝-s6(c1c4+s4(c2s1s3+c3s1s2))-c6(c5(c1s4-c4(c2s1s3+c3s1s2))+s5(s1s2s3-c2c3s1))
az＝-c6(s23s5-c23c4c5)-c23s4s6
Px＝c1(L1+L4c23+L3s23+L2s2)
Py＝s1(L1+L4c23+L3s23+L2s2)
Pz＝L3c23-L4s23+L2c2
求解工业机器人的速度雅可比矩阵，雅可比矩阵是关节速度空间，到笛卡尔速度空间的映射，其逆矩阵，就是笛卡尔速度空间，到关节速度空间的映射。雅可比矩阵是时变的线性映射。
其中，ξ和J由下式给出
向量ξ被称为体速度，注意这个速度向量并非位置变量的倒数，这是由于角速度向量不是任何时变数量的倒数，矩阵J被称为机械臂的雅可比矩阵。J是一个6×n的矩阵，其中n是机器人中的连杆数量。
想要描述刚体的运动，需要6个变量，其中3个平移速度分量，3个旋转速度分量，想要确定这6个输出变量，至少需要6个输出，且雅可比矩阵的6个列向量必须线性无关，也就是说这个6个列向量可以张成一个6维的向量空间。
末端执行器的线速度是使用微分的链式法则，有
因此，矩阵Jv的第i列(记为Jvi)可由下式给出：
其中，矩阵的第i列Jvi为
雅可比矩阵的下半部分为
Jω＝(Jω1…Jωn)
其中，矩阵的第i列Jωi为
当雅可比矩阵的行列式等于零,该方阵是奇异矩阵，此情况下雅可比矩阵的逆矩阵不存在，不能通过笛卡尔速度求出对应的关节速度，通过求解雅可比矩阵行列式为零的情况，可以确定机器人的奇异位形。
一般情况下，难以求解非线性方程detJ(q)＝0，但是对于装备有球形手腕的机械臂，可以对雅可比矩阵进行解耦，求解奇异点位置。
通过解耦，可以分离出腕部奇异点的雅可比矩阵。
对于有球形手腕的6轴机器人，雅可比矩阵为6*6矩阵，当
detJ(q)＝0
时，位形q为奇异点
若将雅可比矩阵分解成如下3*3的矩阵块
最后三个关节总为转动关节
由于手腕轴线交于同一点o，若我们选择坐标系使得o4＝o5＝o6＝o7＝o，则
因此，雅可比矩阵有以下形式
其行列式为
detJ＝detJ11detJ22
手腕奇异点
因为是表式机器人4，5，6轴方向的三维向量，其方向由连杆变换矩阵T的旋转算子部分决定。
其中是3×3矩阵，是变换矩阵T的旋转算子部分
根据运动学正解，可以计算
其中代表第i个关节变换矩阵T的旋转算子部分，以上公式中，/>统一省略了左上角标0，/>为轴方向在轴所在坐标系中的表示
J22是一个3*3的方阵，当向量线性相关时，球形手腕处于奇异位形，即当θ5＝0或180°时，/>处于同一条直线上，球形手腕处于奇异位形，如图3所示。并在此基础上绕开奇异点。