一种机械臂的轨迹规划方法与流程

本发明属于机器人控制相关技术领域，更具体地，涉及一种机械臂的轨迹规划方法。

背景技术：

工业机器人作业主要包括两种，即点位作业和连续路径作业。在操作之前规划好机器人的运动轨迹属于该领域的顶层设计。目前就工业机器人的各类轨迹规划方式而言，通常可在关节空间或笛卡尔空间内实施。其中前者计算简单，但是机器人末端轨迹不可控，因而在实践中特别是要求末端具有明确轨迹的场合下，越来越多地采用了后者。

然而，虽然笛卡尔空间内实施可使机器人末端轨迹误差较小，但是计算量往往很大，且需要在速度环和加速度环对机器人进行控制。更具体来说，一方面，这种轨迹规划方法的计算复杂，在小型机械臂的规划上代价大，效率偏低；另一方面，一些机械臂关节的速度和加速度控制在实际工况中具有很大的困难，如舵机在控制周期内先是全速运行，到达控制位置后停止并等待下一控制周期控制指令，进而导致笛卡尔空间轨迹规划效果及精度较差。相应地，本领域亟需寻找更为完善的解决方案，以便更好地满足日益增长的实际需求。

技术实现要素：

针对现有技术的以上不足或改进需求，本发明提供了一种机械臂的轨迹规划方法，其中通过结合机械臂自身结构及实际作业的特点，并构建特定算法来研究改进其轨迹规划的具体操作过程，测试表明可确保改善控制精度，并大大减少总体计算量、减少计算误差，同时显著提高轨迹规划的效率，因而尤其适用于譬如六轴机械臂之类的小型机械臂高精度轨迹规划应用场合。

为实现上述目的，按照本发明，提供了一种驱动机械臂的轨迹规划方法，其特征在于，该方法包括下列步骤：

(1)针对作为规划对象的机械臂，获取包括与它的末端相关的多个参数，包括初始位姿，终点位姿和预定轨迹；然后计算获得对应的初始关节向量和终点关节向量；

(2)在所述预定轨迹上选择一系列的特征点，并将机械臂末端的当前特征点位姿记为yi，对应的关节向量记计为oi，下一个特征点位姿记为yi+1，对应的下一特征点关节向量计为oi+1，其中i＝1,2,3,…且表示各特征点的次序编号；

(3)计算δo＝oi+1-oi；

(4)依照步骤(3)的计算结果，执行以下子步骤：

(41)当δo中绝对值最大的元素小于或等于预设的第一阈值b0时，首先对oi和oi+1两者之间的运行段执行第一插值操作，由此获得多个中间关节向量oi1,oi2,…,oiy，其中y表示第一插值操作过程中插值点的总数量；接着，依照所获得的多个中间关节向量oi1,oi2,…,oiy，使机械臂运行至下一特征点，然后转入步骤(5)；当δo中绝对值最大的元素大于预设的第一阈值b0时，直接转入步骤(42)；

(42)当δo中前k个元素的绝对值均小于或等于预设的第二阈值b1时，首先对oi和oi+1的前k个元素进行第二插值操作，由此获得前k个关节的关节向量ok1,ok2,…,oky，同时确定第k+1个关节所对应的位姿yk1,yk2,…,yky，其中y表示第二插值操作过程中插值点的总数量；接着，对yi和yi+1两者之间执行插值点总数量同样为y的第三插值操作，并获得多个插值点yi1,yi2,…,yiy；最后，分别针对yk1,yk2,…,yky和yi1,yi2,…,yiy，计算获得第k+1个关节到最后一个关节的关节向量，并将其与对应的ok1,ok2,…,oky结合以获得一系列完整的关节向量，并依照该一系列关节向量完整的使机械臂运行至下一特征点，然后转入步骤(5)；

(5)使i＝i+1，并判断机械臂的末端是否到达终点位姿，若否，则返回至步骤(3)予以循环，直至机械臂的末端到达终点位姿为止，由此完成整体的轨迹规划过程。

作为进一步优选地，在步骤(2)中，所述特征点的选择优选被设定为使得机械臂关节角的变化单调。

作为进一步优选地，所述机械臂优选为舵机驱动的机械臂。作为进一步优选地，在步骤(41)中，所述第一阈值b0优选被设定等于舵机在一个控制周期内的最大角位移。

作为进一步优选地，在步骤(42)中，所述第二阈值b1优选被设定等于舵机在一个控制周期内的最大角位移的2.5倍。

作为进一步优选地，在步骤(42)中，所述k的数值优选被设定等于机械臂的关节数减去三。

作为进一步优选地，对于上述第一插值操作而言，其优选采用线性插值的方式；此外，对于所述第二和第三插值操作而言，两者优选采用相同的插值方式。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，主要具备以下的技术优点：

1、本发明中针对笛卡尔空间内实施可使机器人末端轨迹误差较小，但是计算量往往很大和一些机械臂关节的速度和加速度控制在实际工况中具有很大的困难的问题，相应构建了特定算法并采取多次空间插补操作来进行改善，相应不仅可缩短极限插补周期，还能够大大减少计算量，降低对处理器运算速度的要求，同时有效降低了数值计算误差，并显著提高了机械臂轨迹规划的效率和精度；

2、在以上算法及轨迹规划的基础上，本发明还进一步对其中一些关键参数譬如第一阈值、第二阈值和k值等作出了研究和设计，较多的实际测试表明，其能够在整体算法的效率与最终可获得的轨迹精度之间取得很好的平衡，并具备便于操控和计算处理的优点；

3、按照本发明的轨迹规划方法能够很好地融合笛卡尔空间和关节空间各自的优点，同时具备高效率、高精度和适应性强等特点，因而尤其适用于譬如六轴机械臂之类的小型机械臂轨迹规划应用场合。

附图说明

图1是按照本发明所构建的轨迹规划方法的整体工艺流程示意图；

图2更为具体地显示了图1中所示规划子算法的具体处理流程。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

图1是按照本发明所构建的轨迹规划方法的工艺流程示意图，。如图1和图2中所示，该工艺主要包括下列流程步骤：

步骤一，针对作为规划对象的机械臂，获取包括与它的末端相关的多个参数，包括初始位姿，终点位姿和预定轨迹；然后譬如通过本领域常规可知的运动学反解等方式，来计算获得对应的初始关节向量和终点关节向量；

步骤二，在所述预定轨迹上选择一系列的特征点，并将机械臂末端的当前特征点位姿记为yi，对应的关节向量记计为oi，下一个特征点位姿记为yi+1，对应的下一特征点关节向量计为oi+1，其中i＝1,2,3,…且表示各特征点的次序编号。

步骤三，计算δo＝oi+1-oi，该步骤也表示关节的角位移，并依照计算结果执行以下算法处理过程，也即作为本发明关键改进所在的步骤四。

步骤四具体包括以下子步骤：

子步骤(41)——首先判断δo中绝对值最大的元素是否小于或等于预设的第一阈值b0，若是，则对oi和oi+1两者之间的运行段执行第一插值操作(也即执行关节空间轨迹规划处理)，由此获得多个中间关节向量oi1,oi2,…,oiy，其中y表示第一插值操作过程中插值点的总数量；接着，依照所获得的多个中间关节向量oi1,oi2,…,oiy，使机械臂运行至下一特征点，然后转入步骤五；其中考虑到b0的取值会影响算法的效率和轨迹的误差，相应经过实际对比测试，最终优选设定为等于舵机在一个控制周期内的最大角位移；若否，则转入下面的子步骤(42)；

子步骤(42)——继续判断δo中前k个元素的绝对值是否均小于或等于预设的第二阈值b1。若是，则执行所谓的规划子算法处理过程，具体也即对oi和oi+1的前k个元素进行第二插值操作，由此获得前k个关节的关节向量ok1,ok2,…,oky，同时譬如通过本领域公知的运动学正解方式来确定第k+1个关节所对应的位姿yk1,yk2,…,yky，其中y表示第二插值操作过程中插值点的总数量；接着，对yi和yi+1两者之间执行插值点总数量同样为y的第三插值操作，并获得多个插值点yi1,yi2,…,yiy；最后，分别针对yk1,yk2,…,yky和yi1,yi2,…,yiy，譬如采用运动学逆解的方式来计算获得第k+1个关节到最后一个关节的新的关节向量，并依照这些新的关节向量，使机械臂运行至下一特征点，然后转入步骤五；

需要解释说明的是，作为本发明的关键改进所在，本发明正是通过对上述多次的空间插补及相关操作，相应来获得更为优化的轨迹规划效果的。若上述两次计算结果均为否，也即δo中绝对值最大的元素大于预设的第一阈值b0并且δo中前k个元素的绝对值也大于预设的第二阈值b1，则说明在步骤二中特征点的选择过疏，相应无法实现后续的计算及多次空间插补操作；在此情况下，可重新调整该步骤中的特征点密度，并使得所选择的特征点更为密集，然后依照上述的算法依次执行后续的计算及空间插补操作即可。

此外，同样考虑到b1、k取值均会影响算法的效率和轨迹的误差，本发明中经过实际对比测试，最终优选将其设定为舵机在一个控制周期内的最大角位移的2.5倍，k的值可选取为机械臂关节数减去三。

步骤五，使i＝i+1，并判断机械臂的末端是否到达终点位姿，若否，则返回至前面的步骤三予以循环，直至机械臂的末端到达终点位姿为止，由此完成整体的轨迹规划过程。

下面将以六轴机械臂为例，更为具体地解释说明按照本发明的规划方法的实施过程。

首先，给定机械臂末端初始位姿y00，终点位姿y11和预定轨迹，并可通过运动学反解获得始末关节向量o00＝(θ001,θ002,θ003,θ004,θ005,θ006)和o01＝(θ011,θ012,θ013,θ014,θ015,θ016)。

接着，在预定轨迹上选择一系列特征点y。并记机器人当前位姿为yi，关节向量为oi＝(θi1,θi2,θi3,θi4,θi5,θi6)，下一个特征点位姿为yi+1，可通过运动学反解求得下一特征点关节向量为oi+1＝(θ(i+1)1,θ(i+1)2,θ(i+1)3,θ(i+1)4,θ(i+1)5,θ(i+1)6)。

接着，计算δo＝oi+1-oi，δo＝(δθ1,δθ2,δθ3,δθ4,δθ5,δθ6)，并根据计算处理执行以下空间轨迹规划处理及规划子算法处理：

在此过程中，首先判断δo中绝对值最大的元素是否小于或等于预设的第一阈值b0，若max(|δθ1|,|δθ2|,|δθ3|,|δθ4|,|δθ5|,|δθ6|)≤b0，则对oi和oi+1进行插值，获得一系列中间关节向量oi1,oi2,…,oiy；

否则的话，则转入判断δo中前k个元素的值是否均小于预设的第二阈值b1，若|θ(i+1)j-θij|≤b1，j＝1,2,3，则执行规划子流程，该规划子流具体程如下：首先，对oi和oi+1的前k＝3个元素进行插值，得到前3个关节的关节向量ok1,ok2,…,oky，其中oky＝(θky1,θky2,θky3)；然后，由前三个轴的关节向量，可通过运动学正解可得到第四个关节的位置向量yk1，yk1……yky；然后，对yi和yi+1进行插值，获得yi1,yi2,…,yiy；接着，可采用运动学逆解的方式对yk1,yk2,…,yky和yi1,yi2,…,yiy进行计算，获得第k+1个关节到最后一个关节的关节向量oe1,oe2,…,oey，其中oej＝(θey4,θey5,θey6)；最后，结合ok1,ok2,…,oky和oe1,oe2,…,oey，获得一系列关节向量oi1,oi2,…,oiy。其中oiy＝(θiy1,θiy2,θiy3,θey4,θey5,θey6)。

接着，按照所得的一系列完整的关节向量，使机械臂运行至下一特征点，即控制机械臂每一个关节角度为oiy所指示的角度，按照顺序进行运动。

最后，若机械臂到达终点位姿y11，则完成轨迹规划，否则返回至计算δo＝oi+1-oi的步骤并予以循环。

综上，按照本发明的按照本发明的轨迹规划方法能够很好地融合笛卡尔空间和关节空间各自的优点，同时具备高效率、高精度和适应性强等特点，因而尤其适用于譬如六轴机械臂之类的小型机械臂轨迹规划应用场合。

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