一种机器人末端连续轨迹规划过渡方法
【技术领域】
[0001] 本发明涉及一种机器人轨迹控制方法，尤其是指一种在笛卡尔空间下的多自由度 工业机器人末端连续轨迹规划过渡方法。
【背景技术】
[0002] 工业机器人轨迹规划算法是保证工业机器人实现稳定运动的核心技术。对于弧 焊、涂胶、水切割等机器人应用来说，通常要求机器人末端执行器尽量以接近给定速度的速 度沿着指定路径前进，并且保证相邻路径之间能够平滑过渡，因此过渡控制方法是重点和 难点。
[0003] 为解决路径平滑过渡这一难点问题，最早被应用的是基于样条曲线插补等方式的 关节空间轨迹规划算法。虽然这类算法具有约束条件少和计算速度快等优势，但同时存在 着空间轨迹不直观和轨迹形状随速度不同而改变等缺点，因此近年来国内外研究者大多将 注意力转向了笛卡尔空间轨迹规划算法。现有常用的算法主要有三类：
[0004] 第一类算法是卷积类算法（例如专利US4554497，US5434489)，通常是对路径进行 均匀离散化之后采用卷积计算实现加减速规划，然后基于给定重叠时间参数进行速度叠 加，其优点是算法简单，但存在诸多缺点，特别是难以精确控制过渡曲线形状。
[0005] 第二类算法则是混叠类算法，包括速度混叠算法(例如专利US5602968)和位置混 叠算法（Ustyan T , Jonsson V , Imp 1 ementation of a generic virtual robot controller[D],Master's Thesis，Chalmers University of Technology，Sweden，2011)〇 这类算法一般需要给定加减速时间，然后通过线性、多项式或者摆线等函数进行同伦处理 获得速度曲线或位置曲线。其缺点是加速度约束涉及时间变量，需要比较复杂的处理流程。
[0006] 第三类算法则是规划类算法，包括速度规划算法(例如专利US5740327)和位置规 划算法(例如专利US8290611)。这一类算法通过设计过渡曲线的数学表达式来实现。其中， 速度规划算法计算流程复杂，难以保证轨迹不随速度参数的更改而变化。而在位置规划算 法的研究中，Siciliano等将关节空间的位置规划算法扩展到笛卡尔空间，获得抛物线形状 的过渡轨迹，使得加速度约束不再取决于时间常数，但是仍存在加速度矢量方向突变的问 题（Siciliano B，Sciavicco L，Villani L,Robotics：Modelling,Planning and Control [M]，3rd Fdition，Springer，2010.)。林仕高等提出五次多项式位置规划过渡算法，但是因 加速度约束涉及时间变量而未能提出加速度约束处理方案(林仕高，刘晓麟，欧元贤，机械 手笛卡尔空间轨迹规划研究[J]，机械设计与制造，2013(3) :49-52)。还有一些研究首先选 取过渡路径并细分为微小路径段，再采用前瞻算法实现速度平滑和轨迹修形，但是算法复 杂，且容易产生速度毛刺和波动。
【发明内容】
[0007] 本发明的目的是克服现有技术中机器人末端平滑过渡的方法缺乏精确控制、处理 流程较为复杂的缺陷，提供一种机器人末端连续轨迹规划过渡方法。
[0008] 本发明的目的是通过下述技术方案予以实现：
[0009] -种机器人末端连续轨迹规划过渡方法，用于控制机器人末端沿着给定路径前 进，并在相连线段之间实现平滑过渡，包括以下步骤：
[0010] 步骤1，确定需要进行连续轨迹规划过渡的第一线段和第二线段，确定相连线段的 示教点，并确定示教点与第一线段的过渡距离和示教点与第二线段的过渡距离；
[0011] 步骤2,根据示教点与第一线段的过渡距离确定在第一线段上的第一过渡节点，根 据示教点与第二线段的过渡距离确定在第二线段上的第二过渡节点；
[0012] 步骤3,在坐标轴内计算幅值系数、相位系数和速度缩放系数，将幅值系数、相位系 数和速度缩放系数带入有限项正弦位置规划函数确定第一过渡节点和第二过渡节点之间 的过渡曲线表达式。
[0013] 作为一种优选方案，步骤1中的示教点与第一线段的过渡距离和示教点与第二线 段的过渡距离相等。
[0014] 作为一种优选方案，步骤3中的速度缩放系数若大于等于1，则应采用时间缩放过 渡曲线得到过渡曲线表达式。
[0015] 作为一种优选方案，步骤3中的有限项正弦位置规划函数包含至少三个正弦周期 函数，并且每个正弦周期函数的周期是给定周期的2"倍，而给定周期是从第一过渡节点到 第二过渡节点所需的过渡时间的倍数。
[0016] 作为一种优选方案，第一线段和第二线段为两个直线线段、一个直线线段和弧线 线段或两个弧线线段。
[0017]作为一种优选方案，步骤3具体为：
[0018]设T为过渡时间，Γ1为示教点与第一线段的过渡距离或示教点与第二线段的过渡 距离，则分别为第一线段和第二线段的巡行速度，h为过渡节点处待优化的速度比例 系数，将第一过渡节点P#PP2的位置矢量分别用PQ和ρτ表示；
[0019 ]将位置、速度和加速度矢量分别向坐标轴j投影以获得分量，设过渡节点的切向加 速度均为〇以使后续推导大为简化；
[0020] p〇处的位置、速度和加速度的坐标轴分量配置如下：
[0022] 其中Λ广h 4广α丨·<气，ai为在Pl处的加速度，^和4分别为PQ处切向 单位向量和法向单位向量，ej为坐标轴j的单位向量，坐标轴j = X，y，z ;
[0023] ρτ处的位置、速度和加速度的坐标轴分量配置如下：
[0025] 其中：& = .ν'2 · A ·，.~=〇片为ρτ处切向单位向量；
[0026] 由以上公式可知，如果能确定h，过渡节点的实际配置将随之确定。因此，如果能 够构造一种包含1^为参数的位置过渡函数，令其满足上述配置，同时根据运动学约束指标 或动力学约束指标来确定h，即可获得过渡区域的位置过渡函数。
[0027] 对于非过渡区域的轨迹只需采用经典的起止点之速度和加速度为给定值的直线 段或圆弧段的轨迹规划算法即可，例如多项式样条曲线方法。
[0028]对于每个坐标轴方向，引入基于有限项正弦级数的位置规划函数：
[0030] 则过渡节点运动学约束方程组如下：
[0031] p〇j = Pj(0) .PTj = Pj(T)
[0032] %=/),(())，~ = /),('0
[0033] 而/ =/:V:())，"i, = Α(7'),
[0034]为保证上述方程组的确定性，需方程数量与待定系数数量相同，选取ns = 3，ne = 5;
[0035] 不难看出，T并不影响过渡轨迹的形状，其实际数值可由用户为每一段过渡区域指 定，但为避免速度过大，应限定大于某下限值。综合考虑到两条共线直线段之间过渡等特殊 情形，定义Τ = 1α、，
[0036] 其中：s = k〇ri(vi+V2广Iko是用户可控参数且ko 2 4,将1 = 带入约束方程组，借 助正弦函数的特点可消去lu，得到：
[0037] Z = CV，
[0038] 其中：
[0039 ] Z = [/?，弋,,"~]T
[0040] C=[a3，a4，a5，03，04，&]
[0041 ] a, = [t)i0, q!t), 0,-plX, -0i?_, -qi2 ]T
[0042] Pi= [0，pi,0，0i,i，qi,i，0，-pi,2]T
[0043] 〇i,n = 2_n(i_1)un,u = 3is_1
[0044] Pi,n = Oi,nSin2_(l_1)3T,qi,n = Oi,nCOs2_(l_1)3T
[0045 ] V = [V3j，V4j，V5j，W3j，W4j，