一种控制机械臂末端执行空间曲线动作的方法与流程

本发明涉及自动控制技术领域，特别是一种控制机械臂末端执行空间曲线动作的方法。

背景技术：

随着产业升级和企业技术进步的加快，机械臂成为了机器人技术领域中被得到广泛实际应用的自动化机械装置，此类机械臂具有多自由度，允许在三维空间进行运动，通过接收控制指令以完成各种作业。

工业机器人的作业包括喷涂、切割等等，涉及到机械臂末端在空间进行圆周动作。从原理上讲，空间任意曲线都可以用分段空间圆弧去逼近拟合，然而在计算机中做圆弧点的逼近拟合计算，首先要确定空间圆弧曲线的表达函数，但该函数复杂且难于对空间园弧上任意点坐标做求解运算，更谈不上用分段空间圆弧逼近方法来实现圆弧真实轨迹的跟踪了。

申请号为cn201510163464.9的中国发明专利公开了一种机器人的控制方法及机器人，涉及到了采用几何解析的方法计算运动学逆解，获取到机械臂末端位姿在世界坐标系中的描述，进而采用位姿信息插补，规划机械臂末端的运行轨迹，最终采用插补的形式约束机械臂末端的运行轨迹，实现精确的轨迹控制。上述方案并不适用于机械臂末端执行空间曲线动作，因为无法确定空间圆弧曲线的表达函数以及对空间园弧上任意点坐标做求解运算。

技术实现要素：

本发明的主要目的在于提供一种控制机械臂末端进行空间曲线动作的方法，基于空间圆弧曲线的表达函数，即机械臂待执行空间曲线动作的轨迹表达式对机械臂末端的运动轨迹进行规划控制。

为实现上述目的，本发明包括步骤：

a、确定机械臂待执行空间曲线动作的轨迹表达式；

b、基于所述表达式，将所述待执行空间曲线动作的轨迹分别投影至水平和垂直的两平面中；

c、分别在所述水平和垂直的两平面中进行圆弧轨迹还原，并以还原结果输出控制指令；

d、机械臂执行所述控制指令以完成曲线动作。

其中，所述轨迹表达式为：式中g(x，y，z)表示以机械臂末端为坐标原点的空间二次曲面；(x，y，z)表示机械臂待执行空间曲线动作的轨迹上任一一点的坐标；(ax，ay，az)，(bx，by，bz)，(cx，cy，cz)分别表示机械臂待执行空间曲线动作的轨迹上已知的三个位置的坐标。

由上，通过上述正交变换可以将复杂的空间二次曲面的标准式转化为矩阵方程形式，由于有了空间曲线动作的轨迹上上表示其轮廓的三个位置的坐标，通过该矩阵方程可以更简便的、采用更少运算量求出空间二次曲面的圆心，以及经过该圆心与上述a、b、c三点所确定的平面。

其中，所述步骤b包括：采用高斯消元对以机械臂末端为坐标原点的空间二次曲面g(x，y，z)进行计算，所得的在两个坐标系下的椭圆方程组表示为:式中，x’、y’分别表示在水平坐标系下的横坐标和纵坐标，b1、a1分别表示在水平坐标系下椭圆的长轴和短轴数值；y”、z’分别表示在垂直坐标系下的横坐标和纵坐标，b2、a2分别表示在垂直坐标系下椭圆的长轴和短轴数值。

由上，将空间二次曲面原表达式g(x，y，z)中的三维运算降维成二维，从而减少了运算量。

其中，步骤c包括：

c1：计算待执行空间曲线动作的轨迹上任一点到空间坐标原点的距离r，所述任一点的坐标表示为(xi，yi，zi)，空间坐标原点的坐标表示为(0，0，0)；

c2：确定机械臂待执行空间曲线动作轨迹的圆心，将所述任一点和所述圆心分别投影至所述水平和垂直的两平面中；所述圆心坐标表示为(x0，y0，z0)；

c3：分别在所述两平面的坐标系下，计算投影到当前坐标系下的所述任一点的与所述圆心的距离，在水平面上，该距离表示为rxoy，在垂直面上，该距离表示为ryoz；

c4：分别比较rxoy²与r²的大小以及ryoz²与r²的大小，

当rxoy²≥r²时，输出控制指令以使机械臂末端向x轴的负方向进给一步以进行调整；

当ryoz²＜r²时，输出控制指令以使机械臂末端向y轴的正方向进给一步以进行调整。

由上，采用两个平面坐标系的圆弧还原转换为对空间坐标系的圆弧还原，实现了机械臂末端执行空间曲线动作。

其中，所述距离r的计算式包括：r²＝xi²+yi²+zi²。

由上，基于勾股定理的原理计算出空间曲线动作的轨迹上任一点到空间坐标原点的距离。

附图说明

图1为本发明的流程图。

具体实施方式

下面参见图1对本发明所述的控制机械臂末端执行空间曲线动作的方法进行详细说明。

本申请实施例以机械臂末端执行圆周运动为例进行说明，即机械臂的操作对象为圆形，例如机械臂执行轮胎喷漆工作。如图1所示，包括以下步骤：

s100：确定操作对象相对于机械臂的位置表达式。

以机械臂末端为坐标原点，获取操作对象上表示其轮廓的至少三个位置的坐标，即获取三个位置的坐标即可确定一个圆面，由此可使机械臂控制端获取操作对象相对于机械臂末端的位置和大小。

获取操作对象的三个位置的坐标点以及将操作对象划归至以机械臂末端所在的坐标系中已属现有技术，本申请不再赘述。

以操作对象上表示其轮廓的三个位置分别为a、b、c为例说明，a、b、c三个位置的坐标分别表示为(ax，ay，az)，(bx，by，bz)，(cx，cy，cz)。

空间二次曲面(即对应本实施例的空间任意位置的圆形轮胎)的标准式表示为ax²+by²+cz²+dxy+eyx+fzx+gx+hy+iz+j＝0，其中a、b、……、j分别表示系数常量，对上述空间二次曲面的标准式进行正交变换，转换为下列表达式：

式中(x，y，z)表示操作对象上任一一点的坐标，g(x，y，z)表示以机械臂末端为坐标原点的空间二次曲面。

通过上述正交变换可以将复杂的空间二次曲面的标准式转化为矩阵方程形式，由于有了操作对象上表示其轮廓的三个位置的坐标，通过任意两点的中垂线便可以求出圆心o点(x0，y0，z0)。通过该矩阵方程可以表示出空间二次曲面的圆心，以及经过该圆心与上述a、b、c三点所确定的平面。

s200：对步骤s100中的表达式进行消元处理。

将所述表达式g(x，y，z)表达的空间二次曲面分别在xoy、yoz两个坐标系下投影，空间二次曲面投影后分别在上述两个坐标系下呈现为椭圆。

具体的，采用高斯消元对步骤s100所变换后的空间二次曲面g(x，y，z)进行计算，所得的在两个坐标系下的椭圆方程组表示为:式中，x’、y’分别表示在xoy坐标系下的横坐标和纵坐标，b1、a1分别表示在xoy坐标系下椭圆的长轴和短轴数值；y”、z’分别表示在yoz坐标系下的横坐标和纵坐标，b2、a2分别表示在yoz坐标系下椭圆的长轴和短轴数值。

本步骤的目的是将空间二次曲面原表达式g(x，y，z)中的三维运算降维成二维，从而减少了运算量。

步骤s300：针对步骤s200中消元处理后的方程进行圆弧轨迹还原。

首先以xoy坐标系为例，进行说明。

假设在空间二次曲面g(x，y，z)上的任意点为a(xi，yi，zi)，a点到坐标原点(0，0，0)的距离为d，d＝r，r表示空间二次曲面的半径，则有：d²＝r²＝xi²+yi²+zi²。

将上述a点和圆心o点分别投影至xoy坐标系下，即令zi＝z0＝0。则投影后的a点与投影后的圆心o点的距离rxoy表达式为：rxoy²＝b1²+a1²。

计算rxoy²与r²的大小，以δx表示，即δx＝rxoy²-r²，当δx≥0表示圆弧轨迹的还原落在圆弧曲线外或其上，则需要向x轴的负方向进给一步以进行调整。

对应的，再将上述a点和圆心o分别投影至yoz坐标系下，即令xi＝x0＝0。则投影后的a点与投影后的圆心o点的距离ryoz表达式为：ryoz²＝b2²+a2²。

计算ryoz²与r²的大小，以δy表示，即δy＝ryoz²-r²，当δy＜0表示圆弧轨迹的还原落在圆弧曲线内，则需要向y轴的正方向进给一步以进行调整。

此后，对空间二次曲面g(x，y，z)上a点之后的b点进行还原，b点坐标表示为(xi+1，yi+1，zi+1)，还原原理与a点相同。如此循环，直至将整个圆环全部进行轨迹还原，即还原至n点，n点坐标表示为(xi-1，yi-1，zi-1)。

步骤s400：基于步骤s300的圆弧轨迹还原结果输出控制指令，以调整机械臂末端的运动方向和轨迹。

步骤s300中，分别在xoy和yoz两坐标系下进行圆弧轨迹的还原，而针对机械臂需要将计算结果转换到空间中时，则需要将两投影面分别转换至空间中。仍以a点为例，从xoy坐标系转换回空间时，将a点和圆心o点的z轴坐标zi和z0恢复；从yoz坐标系转换回空间时，将a点和圆心o点的x轴坐标xi和x0恢复。机械臂接收到上述转换后的空间坐标，根据调整的坐标进行空间圆弧轨迹运动，从而可以执行喷涂等工作。

具体对于机械臂末端的控制包括以下子步骤：

s401：贮存计算数据、传送至机械臂主控制器单元。

s402：机械臂主控制器单元调用位姿转换矩阵生成机械臂各轴旋转角控制指令，分别对应发送至机械臂各轴子控制单元。

s403：机械臂各轴子控制单元基于所述控制指令，调用系统驱动编译程序生成各轴执行代码。

s404：调用机械臂各轴驱动程序对各轴发出脉冲指令。

s405：结束运动返回至程序零点位置。

本申请基于空间圆弧曲线的表达函数，即机械臂待执行空间曲线动作的轨迹表达式，采用两个平面坐标系的圆弧还原转换为对空间坐标系的圆弧还原，实现了机械臂末端执行空间曲线动作。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。