下载文档原格式
工业机器人的路径规划与运动控制方法与技巧工业机器人在现代制造业中扮演着重要的角色，它们能够自动执行各种繁重、重复的工作任务，提高生产效率和产品质量。
而路径规划与运动控制是工业机器人实现高效自动化的关键技术。
本文将介绍工业机器人的路径规划与运动控制方法与技巧。
路径规划是指确定工业机器人从起始位置到目标位置的最佳路径。
一个有效的路径规划算法能够提高机器人的运动效率和安全性。
目前常用的路径规划方法包括位姿插补、分段直线插补和样条插补。
位姿插补是最基本的路径规划方法，它通过在每个关节轴上逐渐改变位姿来实现机器人的运动。
位姿插补的优点是简单易行，但在实际应用中可能会出现抖动和不平滑的问题。
分段直线插补是另一种常用的路径规划方法，它将机器人的运动路径分成若干个直线段，并在每个直线段上进行插补计算。
分段直线插补能够有效地减少机器人的振动，并提高运动的平稳性。
样条插补是一种更加精细的路径规划方法，它利用数学模型对机器人的运动进行插补计算。
样条插补能够实现连续平滑的运动轨迹，并提高机器人的运动精度。
除了路径规划，工业机器人的运动控制方法也非常重要。
运动控制是指实现机器人按照路径规划结果进行准确控制的技术。
常见的运动控制方法包括开环控制和闭环控制。
开环控制是一种简单直接的控制方法，它根据路径规划结果直接控制机器人的执行器进行运动。
开环控制的优点是响应速度快，但它对于外界干扰和机器人自身参数变化非常敏感，容易出现运动误差。
闭环控制是一种更加精确的控制方法，它通过传感器获取机器人的实际位姿信息，并与路径规划结果进行对比，然后根据误差进行调整。
闭环控制能够提高机器人的运动精度和稳定性。
除了路径规划和运动控制方法，还有一些其他的技巧可以进一步提高工业机器人的运动性能。
例如，合理选择机器人的控制系统和传感器，以确保系统能够快速响应并准确感知环境。
此外，对机器人进行动力学建模和参数标定也非常重要，它们能够提供机器人运动控制所需的准确输入。
![机器人插补算法基础[宝典]](https://img.taocdn.com/s1/m/fbea78cc6e1aff00bed5b9f3f90f76c660374c5e.png)
机器人插补算法基础运动系统轮廓控制的主要问题，就是怎样根据来自存储器的运动信息来控制机械运动部件的运动轨迹。
根据第十一届机器人论坛所述，一般情况是已知运动轨迹的起点坐标、终点坐标、曲线类型和走向，由运动控制系统实时地算出各个中间点的坐标。
即需要“插入、补上”运动轨迹各个中间点的坐标，通常将这个过程称为“插补”。
插补结果是输出运动轨迹的中间点坐标值，位控系统根据此坐标值控制各个坐标轴相互协调运动，走出预定轨迹。
插补可以用硬件或者软件实现。
早期都是采用硬件的数字逻辑电路来完成插补工作。
在运动控制系统中，采用了电压脉冲作为插补点坐标增量输出，其中每一脉冲都是在相应的坐标轴上产生一个基本长度单位的运动，即每一脉冲对应着一个基本长度单位。
这些脉冲可用来驱动开环控制系统中的步进电动机，也可驱动闭环系统中的直流伺服电动机。
运动控制系统每输出一个脉冲，执行部件移动一个基本长度单位，称之为脉冲当量。
脉冲当量的大小决定了运动精度，发送给每一坐标轴的脉冲数目决定了相对运动距离，而脉冲频率代表了坐标轴速度。
目前插补工作一般由软件完成，也有用软件进行粗插补，用硬件进行细插补的运动控制系统。
软件插补方法分为两类，即基准脉冲插补法和数据采样插补法。
基准脉冲插补法是模拟硬件插补的原理，即把每次插补运算产生的指令脉冲输出到伺服系统，以驱动机械部件运动。
该方法插补程序比较简单，但由于输出脉冲的最大速度取决于执行一次运算所需的时间，所以进给速度受到了一定的限制。
这种插补方法一般用于进给速度不是很高的运动控制系统。
基准脉冲插补有多种方法，最常用的是逐点比较插补法和数字积分插补法。
由于基准脉冲插补法在进给速度方面受到一定的限制。
因此，目前运动控制系统普遍采用数据采样插补算法。
数据采样的基本思想是进行时间分割，根据编程进给速度，将轮廓曲线分割为采样周期的进给段，即“轮廓步长”。
插补程序在每一个采样周期中（也可以是两个或者两个以上周期中）算出在一个轮廓步长里零件的轮廓曲线在各个坐标的增长段，即理论上需要传递给坐标轴的进给量。
工业机器人的轨迹规划与运动控制算法研究工业机器人在现代制造业中发挥着重要的作用，它能够自动执行复杂的任务，提高生产效率和质量。
轨迹规划和运动控制算法是实现机器人自动化的关键技术，本文将对此进行研究和探讨。
一、轨迹规划轨迹规划是指确定机器人从起始位置到目标位置的路径，使得机器人能够在规定的约束条件下安全、高效地完成任务。
常见的轨迹规划方法包括规划点插值法、直线插值法、样条插值法等。
1. 规划点插值法规划点插值法是一种简单且常用的轨迹规划方法。
它将机器人的路径划分为若干离散的规划点，然后通过插值算法确定规划点之间的路径。
这种方法计算简便，但可能导致机器人移动时出现抖动或曲线过于锐利的问题。
2. 直线插值法直线插值法是指将机器人的路径划分为若干直线段，然后通过线性插值得到每个直线段上的点。
这种方法的优点是计算简单，路径平滑，适用于一些简单的轨迹规划问题。
3. 样条插值法样条插值法是一种基于曲线的轨迹规划方法，它能够生成更加平滑的路径。
通过使用样条曲线进行插值，可以得到平滑的机器人轨迹，提高机器人的运动控制性能。
样条插值法相对于前两种方法来说计算更加复杂，但更适用于一些复杂的轨迹规划问题。
二、运动控制算法运动控制算法是指机器人根据规划得到的路径执行运动时的控制方法。
常见的运动控制算法包括PID控制、模糊控制、自适应控制等。
1. PID控制PID控制是一种常用的控制方法，它通过调节比例、积分和微分三个参数来实现对机器人的控制。
PID控制具有结构简单、调节灵活等优点，适用于对机器人位置和速度进行控制。
2. 模糊控制模糊控制是一种基于模糊逻辑的控制方法，它可以处理非线性和模糊的控制问题。
模糊控制通过将输入和输出变量模糊化，并使用一系列的模糊规则进行控制决策，实现对机器人的运动控制。
3. 自适应控制自适应控制是一种能够根据外部环境变化自动调整控制参数的控制方法。
它通过建立机器人与环境的数学模型，利用自适应算法实时调整控制器参数，以适应不同的工作条件。
工业机器人的路径规划算法与实践技巧工业机器人是当今制造业中的重要工具，它们能够自动执行繁重、危险或重复性的任务，提高生产效率和质量。
而在工业机器人的使用过程中，路径规划是一项至关重要的技术。
路径规划是指机器人在给定环境中找到一条有效路径以实现指定任务的过程。
本文将介绍工业机器人的路径规划算法与实践技巧。
一、路径规划算法1.1 A*算法A*算法是一种常用的启发式搜索算法，广泛应用于路径规划中。
该算法通过估计当前节点到目标节点的代价函数，综合考虑节点距离和启发函数的值，选择最佳路径。
A*算法在工业机器人路径规划中的优势在于快速找到最优路径，并且具有较低的计算复杂度。
1.2 RRT算法RRT(快速随机树)算法是一种无模型、无领域信息的路径规划算法。
RRT树是由一系列节点和连边构成的树状结构。
RRT算法通过随机采样和树的生长来搜索可行路径，根据树的生长方向来生成最佳路径。
RRT算法适用于复杂环境下的路径规划，并且具有较高的计算效率。
1.3 D*算法D*算法是一种增量式最优路径规划算法，它能够在路径规划过程中动态调整路径。
D*算法通过不断更新路径距离和启发函数的值，实现最短路径的搜索。
D*算法适用于有动态环境变化的路径规划，如避让障碍物或更新环境信息。
二、路径规划实践技巧2.1 环境建模在进行路径规划之前，需要对工作环境进行建模。
环境建模可以使用传感器等设备获取工作区域的地图信息，并将其转化为机器人可识别的模型。
准确的环境建模有助于机器人更好地感知环境，提高路径规划的效果。
2.2 碰撞检测碰撞检测是路径规划中的关键步骤，用于检测规划的路径是否与环境中的障碍物发生碰撞。
通过合理选择碰撞检测算法和参数设置，可以提高路径规划的可靠性和安全性。
2.3 优化路径在得到初始路径后，可以通过优化算法对路径进行优化。
优化路径可以减少机器人的移动距离、降低能耗，并提高生产效率。
常见的优化算法包括迭代最优化算法和模拟退火算法等。
关节空间轨迹的插值计算关节空间轨迹的插值计算是机器人学中的一个重要问题，它可以用于机器人的路径规划和轨迹生成。
在机器人的运动控制中，关节空间轨迹插值的目的是通过一系列关节坐标点的插值来实现机器人的平滑运动。
插值计算的基本原理是通过已知的关节坐标点来计算中间位置的关节坐标，从而实现整个轨迹的平滑插值。
下面将介绍几种常用的关节空间轨迹插值方法。
1. 线性插值（Linear Interpolation）线性插值是最简单和最直接的插值方法之一。
假设已知起始坐标点q1和结束坐标点q2，线性插值可以通过以下公式计算中间位置的关节坐标点：q(t) = (1-t)q1 + tq2其中，t为取值范围为[0,1]的系数，表示插值在两个坐标点间的位置。
2. 二次插值（Quadratic Interpolation）二次插值是在线性插值的基础上引入二次多项式的插值方法。
它可以通过以下公式计算中间位置的关节坐标点：q(t) = (1-t)^2q1 + 2t(1-t)q + t^2q2其中，q为参数，通常取0.5。
3. Bezier曲线插值Bezier曲线是一种常用的平滑曲线插值方法，它可以通过控制点来定义一条曲线。
对于三个控制点q1、q2和q3，Bezier曲线可以通过以下公式计算中间位置的关节坐标点：q(t) = (1-t)^2q1 + 2(1-t)tq2 + t^2q3其中，t为参数，取值范围为[0,1]。
4. 样条曲线插值样条曲线是一种通过多个控制点相连而成的平滑曲线。
它可以通过公式计算中间位置的关节坐标点，其中每段曲线由四个控制点定义：q(t) = [t^3, t^2, t, 1] * M * Q其中，M为样条曲线的矩阵，Q为控制点矩阵。
除了上述插值方法，还可以使用其他高阶插值方法如样条插值、B样条插值等来实现关节空间轨迹的插值计算。
这些方法可以根据具体的应用场景和要求选择合适的插值方法。
总结起来，关节空间轨迹的插值计算是机器人运动控制中的一个重要问题，通过使用线性插值、二次插值、Bezier曲线插值和样条曲线插值等方法，可以实现机器人的平滑运动和轨迹生成。
工业机器人的实时轨迹插补算法李天友 ,孟正大 ,陈勍奇（东南大学自动化学院，江苏南京 210096）摘要：提出了一种实现工业机器人实时轨迹插补的规划算法。
该算法既能满足时间上的实时性，又能够在完成机器人当前轨迹插补的同时，实现在线调整插补参数，改变机器人当前插补方程，从而改变机器人运动轨迹与状态。
而对于不同插补类型，只要找准对应线长的表示，不需要对算法本身进行修改，就可以完成相应的轨迹插补。
本算法应用于“昆山一号”焊接机器人中，表明其满足焊接实时性和可调速性要求。
关键词：工业机器人；实时插补；算法；轨迹规划示教再现方式下的轨迹插补算法是工业机器人的一个传统课题[1]，技术和方法比较成熟有效。
文献[2-4]分别解决了直线、圆弧、样条曲线等单一类型的轨迹插补，文献[5,6]讨论了复杂曲线在编程时用分段直线或圆弧进行拟合插补的方法，文献[7]研究了关节空间和笛卡儿空间的通用插补算法，把插补段分为加速段、匀速段、减速段进行插补, 但算法复杂，运算量大，且不能进行实时控制。
此外，时间上满足实时性的轨迹插补方法也得到了研究[3,4]。
但是既满足实时性要求又能够进行平滑调速并且能够同时完成关节空间和笛卡儿空间各种类型插补的通用轨迹插补算法却比较少见。
本文介绍工业机器人的实时轨迹插补算法。
它是为满足“昆山一号”焊接机器人的实时性而设计的，实时性包含两层涵义，一是满足时间上的实时性，即在一个采样周期内能够完成一次轨迹插补，多数算法能够满足这层要求；而实时性第二层涵义是系统能够在完成机器人当前轨迹插补的同时，实现在线调整插补参数，改变机器人当前插补方程，从而改变机器人运动轨迹与状态，本文的算法很好地完成了这层实时性的要求。
并且这种算法能够完成PTP （点到点）、多点关节空间、直线、圆弧、样条曲线、FlyBy [8,9]等多种类型的轨迹插补。
本文首先介绍实时轨迹插补算法的提出背景，然后给出了算法基本原理的详细说明，并且运用流程图进一步明确了基本算法，对算法的实时性和调速控制进行了分析，最后阐述了算法在多种插补类型中的实际应用。
运行时间周期化工业机器人模型迭代寻优nurbs轨迹插补2023-10-26contents •引言•运行时间周期化工业机器人模型•迭代寻优算法•nurbs轨迹插补算法•实验与分析•结论与展望目录01引言工业机器人是实现工业自动化的重要工具，提高其运动轨迹的精度和效率对于提高工业生产效率和质量具有重要意义。
现有的轨迹规划方法往往存在精度不高、计算量大等问题，因此需要研究新的轨迹规划方法。
研究背景与意义现有的轨迹规划方法主要包括直线插补和圆弧插补，但这些方法在处理复杂轨迹时存在计算量大、精度不高的问题。
近年来，NURBS（非均匀有理B样条）插补方法逐渐成为研究热点，它具有较高的计算精度和灵活性。
研究现状与发展研究内容本研究旨在研究基于NURBS的工业机器人轨迹规划方法，通过迭代寻优的方式实现高精度、高效率的轨迹插补。
研究方法首先，建立工业机器人的运动模型，包括运动学和动力学模型；其次，基于NURBS方法进行轨迹规划，并采用遗传算法进行迭代寻优；最后，通过实验验证方法的可行性和优越性。
研究内容与方法02运行时间周期化工业机器人模型工业机器人定义工业机器人是一种自动化机器，可以在各种环境中执行一系列重复或复杂的任务，如搬运、焊接、装配、检测等。
工业机器人模型概述工业机器人分类根据应用领域和功能，工业机器人可分为几类，如水平多关节机器人（LAMA）、垂直多关节机器人（VAMA）、码垛机器人等。
模型构建需求为提高工业机器人的运行效率和精度，需要构建一个能够描述和预测机器人行为的模型。
模型构建方法运行时间周期化模型是一种有效的建模方法，通过将机器人的运行时间划分为一系列周期，并分析每个周期内的运动规律，从而建立描述机器人行为的模型。
运行时间周期化模型包括以下元素将机器人的运行时间划分为一系列固定长度的周期，每个周期对应一个任务。
描述机器人在一个周期内从起始点到终止点的路径。
决定机器人运动轨迹的参数，如关节角度、速度等。
机器人运动规划中的轨迹生成算法机器人运动规划是指描述和控制机器人在给定环境中实现特定任务的过程。
其中，轨迹生成算法是机器人运动规划中的关键环节。
本文将介绍几种常用的机器人轨迹生成算法，包括直线轨迹生成算法、插补轨迹生成算法和优化轨迹生成算法。
一、直线轨迹生成算法直线轨迹生成算法是最简单和基础的轨迹生成算法。
它通过给定机器人的起始位置和目标位置，计算机器人在二维平面上的直线路径。
该算法可以通过简单的公式求解，即直线方程，将机器人从起始点移动到目标点。
首先，根据起始点和目标点的坐标计算直线的斜率和截距。
然后，根据斜率和截距计算机器人在每个时间步骤上的位置。
最后，将计算得到的位置点连接起来，形成直线轨迹。
直线轨迹生成算法的优点是简单直观，计算效率高。
然而，该算法无法应对复杂的环境和机器人动力学模型，因此在实际应用中有着较大的局限性。
二、插补轨迹生成算法插补轨迹生成算法是一种基于离散路径点的轨迹生成算法。
它通过在起始位置和目标位置之间插补一系列路径点，使机器人在这些路径点上运动，并最终到达目标位置。
常用的插补轨迹生成算法包括线性插值算法和样条插值算法。
线性插值算法将起始点和目标点之间的轨迹划分为多个小段，每个小段的位置可以通过线性方程求解。
样条插值算法则通过引入额外的控制点，使得轨迹更加光滑。
插补轨迹生成算法的优点是适用于复杂环境和机器人动力学模型。
它可以在运动过程中改变速度和加速度，从而实现更加灵活的路径规划。
不过，插补轨迹生成算法的计算量较大，需要更多的计算资源。
三、优化轨迹生成算法优化轨迹生成算法通过优化目标函数来生成最优的机器人轨迹。
它将机器人运动规划问题转化为优化问题，通过调整机器人轨迹上的参数，使得目标函数达到最小或最大值。
常见的优化轨迹生成算法包括遗传算法、粒子群算法和模拟退火算法。
这些算法主要通过搜索机器人轨迹参数的空间来寻找最优解。
遗传算法模拟生物进化过程，粒子群算法模拟鸟群觅食行为，模拟退火算法则模拟物体在不同温度下的热力学过程。
工业机器人的实时轨迹插补算法李天友 ,孟正大 ,陈勍奇（东南大学自动化学院，江苏南京 210096）摘要：提出了一种实现工业机器人实时轨迹插补的规划算法。
该算法既能满足时间上的实时性，又能够在完成机器人当前轨迹插补的同时，实现在线调整插补参数，改变机器人当前插补方程，从而改变机器人运动轨迹与状态。
而对于不同插补类型，只要找准对应线长的表示，不需要对算法本身进行修改，就可以完成相应的轨迹插补。
本算法应用于“昆山一号”焊接机器人中，表明其满足焊接实时性和可调速性要求。
关键词：工业机器人；实时插补；算法；轨迹规划示教再现方式下的轨迹插补算法是工业机器人的一个传统课题[1]，技术和方法比较成熟有效。
文献[2-4]分别解决了直线、圆弧、样条曲线等单一类型的轨迹插补，文献[5,6]讨论了复杂曲线在编程时用分段直线或圆弧进行拟合插补的方法，文献[7]研究了关节空间和笛卡儿空间的通用插补算法，把插补段分为加速段、匀速段、减速段进行插补, 但算法复杂，运算量大，且不能进行实时控制。
此外，时间上满足实时性的轨迹插补方法也得到了研究[3,4]。
但是既满足实时性要求又能够进行平滑调速并且能够同时完成关节空间和笛卡儿空间各种类型插补的通用轨迹插补算法却比较少见。
本文介绍工业机器人的实时轨迹插补算法。
它是为满足“昆山一号”焊接机器人的实时性而设计的，实时性包含两层涵义，一是满足时间上的实时性，即在一个采样周期内能够完成一次轨迹插补，多数算法能够满足这层要求；而实时性第二层涵义是系统能够在完成机器人当前轨迹插补的同时，实现在线调整插补参数，改变机器人当前插补方程，从而改变机器人运动轨迹与状态，本文的算法很好地完成了这层实时性的要求。
并且这种算法能够完成PTP （点到点）、多点关节空间、直线、圆弧、样条曲线、FlyBy[8,9]等多种类型的轨迹插补。
本文首先介绍实时轨迹插补算法的提出背景，然后给出了算法基本原理的详细说明，并且运用流程图进一步明确了基本算法，对算法的实时性和调速控制进行了分析，最后阐述了算法在多种插补类型中的实际应用。
1 实时轨迹插补算法1.1 提出背景“昆山一号”机器人是我国第一台自主研制的全自动工业焊接机器人，其核心技术“自动控制软件系统”第一版在轨迹插补方面，不仅实现了常见的PTP、直线、圆弧这些插补方式，而且实现了样条曲线插补和多点笛卡尔坐标空间插补等新型插补方式，丰富了机器人的轨迹插补功能，这些插补算法在实际系统运行中，操作简单方便，机器人运行平稳[10]。
先前的算法是把插补段分为加速段、匀速段、减速段，分别对位置和姿态进行定时插补计算，然后把所有的插补点位姿全部求解出来形成位置和姿态链表。
通过对插补点位置和姿态链表进行依次读取执行，来完成各种类型的插补。
这种方法虽然能够完成姿态的插补，但是在已知插补类型的情况下，不能对机器人运动轨迹与状态进行控制，也就是不能满足实时性第二层内容的要求，当然也就不能满足“昆山一号”焊接机器人在实际应用中的电弧跟踪实基金项目：江苏省科技成果转化专项资金项目（BA2007058）和国家高技术研究发展计划资助项目(2006AA040202)作者简介：李天友（1983-），男，河南民权人，硕士生，litianyouseu@时偏差补偿和插补过程中进行调速的需要，所以我们提出了实时轨迹插补算法。
1.2 基本算法机器人控制系统计算点列并形成轨迹的过程叫插补，插补工作有三个要素：插补的参数、插补的方程、插补形成的轨迹（点列）。
只有插补的参数、插补的方程都合乎操作者的需求时，产生的点列才是符合要求的。
其中把插补的参数分成两部分：第一部分参数一般包括示教点的位姿如始点位姿和终点位姿、示教点间的插补方式如直线、圆弧等，一般能够规定插补的轨迹方程，第二部分参数一般包括设定的匀速段速度、变速段加速度、始点速度和终点速度等，一般能够规定机器人工具手末端的运动状态。
为方便说明问题，我们定义空间两点始点A和终点B以及它们之间的线段为直线段（实际上两点之间可以为圆弧、样条曲线、FlyBy等任意形状的线段），算法原理图如图1所示。
图1 实时轨迹插补算法原理示意图图2 实时轨迹插补算法关键步骤说明示意图由图1可知第一部分参数有：始点A位姿startAuxiT6，终点B位姿endAuxiT6，该图例表示的插补类型为直线；第二部分参数有：始点A的速度为Vstart、终点B的速度为Vend、始点A和终点B之间的匀速段速度velo和变速段加速度accel。
定义当前插补点的位姿为currentAuxiT6、速度为Vcurrent和距离A的线长Scurrent，下一个插补点的位姿为nextAuxiT6、速度Vnext和距离A的线长Snext。
定义A和B总线长为S，SA为A附近的变速距离（Vstart按设定加速度变化到velo所需要的线长），SB为B附近的变速距离（velo按设定加速度变化到Vend所需要的线长），SAB为总的变速距离。
定义SAECE为当前插补点处于末段变速状态时所需的变速距离（Vcurrent按设定加速度变化到Vend所需要的线长），SCE为当前插补点到B的距离。
输入变量有：startAuxiT6、 Vstart、endAuxiT6、Vend、currentAuxiT6、Vcurrent、Scurrent （以上三个变量由上次插补算法计算得到）、velo、accel、调速控制量Vrate（通过控制该变量完成机器人的调速，集中反映了该算法通过控制输入变量对机器人的插补状态实行控制）。
输出变量有： Vnext、Snext、nextAuxiT6(以上三个量作为下次轨迹插补的输入控制量)。
算法的核心思想：由以上实时性的分析，可以通过控制输入变量对当前插补点状态施加控制，这样就把整段的连续插补变为可以控制插补状态（如插补的速度和姿态）的单步插补。
在完成当前插补点轨迹插补的同时输出Vnext和Snext，通过计算得到nextAuxiT6，以上三个变量作为下次轨迹插补的输入变量对下次轨迹插补的状态进行控制。
所以现在讨论的问题就变为如何通过控制当前插补点输入变量求解下一插补点的Vnext和Snext上。
在两个插补点之间，存在两个关键判断点：一是比较SCE和SAECE，若SCE不大于SAECE，当前插补点进入末段变速段；若SCE大于SAECE，则进入非末段的变速段；这就相当于直观的把整条线段分成了两个部分，末段变速段和非末段变速段。
第二个关键判断点是速度的对比。
若当前插补点进入末段变速段，首先比较velo和Vend，确定变速段是加速状态还是减速状态，然后比较Vcurrent和Vend，确定当前插补要进行相应的运动还是匀速运动。
而当前插补点处于非末段变速段的处理方法与处于末段变速段类似，首先比较velo 和Vstart ，确定本段是加速状态还是减速状态，然后比较Vcurrent 和Vstart ，确定当前插补要进行相应的运动还是匀速运动。
其关键步骤说明示意图见图2。
所以实时轨迹插补算法处理流程如下：step1：初始化操作，进行速度、加速度、距离合法性检验。
求解SA 和SB ，求和得SAB ；比较SAB 与S 的大小，判别S 的合法性；对插补点末点进行处理；step2：求解处理关键判断点一。
求解SAECE 和SCE ；比较SCE 和SAECE ；若SCE 不大于SAECE ，则进入末段的变速段，进入step3；若SCE 大于SAECE ，则进入非末段的变速段，进入step6；accel Vend Vcurrent fabs SAECE 2/)(22−= (1) step3：处理末段变速段的关键判断点二。
比较velo 和Vend ，若velo 小于Vend ，则本段为加速段，进入step4；若velo 大于Vend ，则本段为减速段，进入step5；step4：比较Vcurrent 和Vend 。
若Vcurrent 小于Vend ，加速操作，算出Vnext 、Snext ；Ts accel Vcurrent Vnext *+= (2)2**5.0*Ts accel Ts Vcurrent Scurrent Snext ++= (3) 其它情况，加速过程提前到达末点速度，则做匀速运动；Vend Vnext = (4) Ts Vcurrent Scurrent Snext *+= (5) step5：比较Vcurrent 和Vend 。
若Vcurrent 大于Vend ，减速操作，算出Vnext 、Snext ；Ts accel Vcurrent Vnext *−= (6)2**5.0*Ts accel Ts Vcurrent Scurrent Snext −+= (7)其它情况，减速过程中提前达到末点速度，则做匀速运动，计算公式同(4)(5)。
step6：处理非末段变速段的关键判断点二。
比较velo 和Vstart ，若Vstart 小于velo ，则本段为加速段，进入step7；若Vstart 不小于velo ，则本段为减速段，进入step8；图3 实时轨迹插补算法的流程示意图 图4 直线在线实时插补的仿真效果示意图 step7：比较Vcurrent 和velo 。
若Vcurrent 小于velo ，加速操作，算出Vnext 、Snext ，计算公式同(2)(3)。
若Vcurrent 达到或暂时超过velo ，则需要将速度保持或调整为velo ，计算公式如下：velo Vnext = (8) Ts Vcurrent Scurrent Snext *+= (9) step8：比较Vcurrent 和velo 。
若Vcurrent 大于velo ，减速操作，算出Vnext 、Snext ，计算公式同(6)(7)。
若Vcurrent 达到或暂时超过velo ，则需要将速度保持或调整为velo ，计算公式同 (8)(9)。
step9：实时轨迹插补算法结束。
根据以上的分析推导过程可以得到如图3所示的实时轨迹插补算法流程图。
1.3 算法的实时性和调速控制分析要实现调速控制，引入调速控制量Vrate （其中有效范围：0-100%），作为插补点的输入变量，对轨迹插补的速度进行控制。
由Sstep Scurrent Snext += (10) 其中Sstep 为单步插补步长；要实现速度的调节，既把V 调整为V*Vrate ；设Ssteprate 为速度调整后的步长,由dt V Sstept t ∫=21 (11)调整为 Vrate dt V dt Vrate V Ssteprate t t t t *)()*(2121∫∫== (12)所以对速度V 的调节转化为对步长Sstep 的调节，得到引入Vrate 后的控制方程：Vrate Sstep Scurrent Snext *+= (13) 由以上推理可知，要实现速度的平滑调节，以公式(3)为例，计算公式如下：Vrate Ts accel Ts Vcurrent Scurrent Snext *)**5.0*(2++= (14) 要实现电弧跟踪实时偏差补偿，由以上算法得到Vnext 和Snext ，计算得到nextAuxiT6，再由电弧跟踪得到的偏差补偿量进行姿态调整，求得补偿后的新的nextAuxiT6位姿量，然后把这三个量作为中间变量，等待下一步调用，这样就完成了电弧跟踪偏差的实时补偿。
