工业机器人的实时轨迹插补算法(精)
工业机器人的轨迹规划与运动控制算法研究
工业机器人的轨迹规划与运动控制算法研究工业机器人在现代制造业中发挥着重要的作用，它能够自动执行复杂的任务，提高生产效率和质量。
轨迹规划和运动控制算法是实现机器人自动化的关键技术，本文将对此进行研究和探讨。
一、轨迹规划轨迹规划是指确定机器人从起始位置到目标位置的路径，使得机器人能够在规定的约束条件下安全、高效地完成任务。
常见的轨迹规划方法包括规划点插值法、直线插值法、样条插值法等。
1. 规划点插值法规划点插值法是一种简单且常用的轨迹规划方法。
它将机器人的路径划分为若干离散的规划点，然后通过插值算法确定规划点之间的路径。
这种方法计算简便，但可能导致机器人移动时出现抖动或曲线过于锐利的问题。
2. 直线插值法直线插值法是指将机器人的路径划分为若干直线段，然后通过线性插值得到每个直线段上的点。
这种方法的优点是计算简单，路径平滑，适用于一些简单的轨迹规划问题。
3. 样条插值法样条插值法是一种基于曲线的轨迹规划方法，它能够生成更加平滑的路径。
通过使用样条曲线进行插值，可以得到平滑的机器人轨迹，提高机器人的运动控制性能。
样条插值法相对于前两种方法来说计算更加复杂，但更适用于一些复杂的轨迹规划问题。
二、运动控制算法运动控制算法是指机器人根据规划得到的路径执行运动时的控制方法。
常见的运动控制算法包括PID控制、模糊控制、自适应控制等。
1. PID控制PID控制是一种常用的控制方法，它通过调节比例、积分和微分三个参数来实现对机器人的控制。
PID控制具有结构简单、调节灵活等优点，适用于对机器人位置和速度进行控制。
2. 模糊控制模糊控制是一种基于模糊逻辑的控制方法，它可以处理非线性和模糊的控制问题。
模糊控制通过将输入和输出变量模糊化，并使用一系列的模糊规则进行控制决策，实现对机器人的运动控制。
3. 自适应控制自适应控制是一种能够根据外部环境变化自动调整控制参数的控制方法。
它通过建立机器人与环境的数学模型，利用自适应算法实时调整控制器参数，以适应不同的工作条件。
机器人运动规划中的轨迹生成算法
机器人运动规划中的轨迹生成算法机器人运动规划是指描述和控制机器人在给定环境中实现特定任务的过程。
其中，轨迹生成算法是机器人运动规划中的关键环节。
本文将介绍几种常用的机器人轨迹生成算法，包括直线轨迹生成算法、插补轨迹生成算法和优化轨迹生成算法。
一、直线轨迹生成算法直线轨迹生成算法是最简单和基础的轨迹生成算法。
它通过给定机器人的起始位置和目标位置，计算机器人在二维平面上的直线路径。
该算法可以通过简单的公式求解，即直线方程，将机器人从起始点移动到目标点。
首先，根据起始点和目标点的坐标计算直线的斜率和截距。
然后，根据斜率和截距计算机器人在每个时间步骤上的位置。
最后，将计算得到的位置点连接起来，形成直线轨迹。
直线轨迹生成算法的优点是简单直观，计算效率高。
然而，该算法无法应对复杂的环境和机器人动力学模型，因此在实际应用中有着较大的局限性。
二、插补轨迹生成算法插补轨迹生成算法是一种基于离散路径点的轨迹生成算法。
它通过在起始位置和目标位置之间插补一系列路径点，使机器人在这些路径点上运动，并最终到达目标位置。
常用的插补轨迹生成算法包括线性插值算法和样条插值算法。
线性插值算法将起始点和目标点之间的轨迹划分为多个小段，每个小段的位置可以通过线性方程求解。
样条插值算法则通过引入额外的控制点，使得轨迹更加光滑。
插补轨迹生成算法的优点是适用于复杂环境和机器人动力学模型。
它可以在运动过程中改变速度和加速度，从而实现更加灵活的路径规划。
不过，插补轨迹生成算法的计算量较大，需要更多的计算资源。
三、优化轨迹生成算法优化轨迹生成算法通过优化目标函数来生成最优的机器人轨迹。
它将机器人运动规划问题转化为优化问题，通过调整机器人轨迹上的参数，使得目标函数达到最小或最大值。
常见的优化轨迹生成算法包括遗传算法、粒子群算法和模拟退火算法。
这些算法主要通过搜索机器人轨迹参数的空间来寻找最优解。
遗传算法模拟生物进化过程，粒子群算法模拟鸟群觅食行为，模拟退火算法则模拟物体在不同温度下的热力学过程。
第4章 工业机器人运动轨迹规划
培养严谨认真、规范操作的意识。
培养合作学习、团结协作的精神。
任务1 轨迹规划问题与性能指标
【任务描述】 在本次任务中需要了解清楚轨迹规划的重要性，轨迹规划的基本概念和方式。路径 和轨迹规划与受到控制的机器人从一个位置移动到另一个位置的方法有关。路径与轨迹 规划既要用到机器人的运动学相关知识，也要用到机器人的动力学。本任务主要讨论机 器人的轨迹规划问题和性能指标。
任务2 常用机器人路径控制方式
【知识储备】 三、常用轨迹运动控制指令 2. MoveJ －通过关节移动移动机器人 当运动不必是直线的时候，MoveJ用来快速将机器人从一个点运动到另一个点，如 图4-6示意。机器人和外部轴沿着一个非直线的路径移动到目标点，所有轴同时到达目标
点。该指令只能用在主任务T_ROB1中，或者在多运动系统中的运动任务中。
任务3 机器人运动轨迹规划基本方法
【知识储备】 一、轨迹规划基本方法分类 在工业机器人末端执行工具的轨迹路径控制方法中，最常用的轨迹规划方法有两种： 第—种方法要求用户对于选定的轨迹结点(插值点)上的位姿、速度和加速度给出一组 显式约束(例如连续性和光滑程度等)，轨迹规划器从一类函数(例如n次多项式)中选取参
主要内容
1 2 3 4
轨迹规划问题与性能指标
常用机器人路径控制方式
机器人运动轨迹规划基本方法
机器人轨迹规划实例
2017/1/13
【学习目标】 1. 知识目标 了解机器人轨迹规划的基本概念。 熟悉机器人轨迹规划的性能指标。 掌握机器人的路径控制方式。 掌握机器人运动轨迹规划的基本方法。 2. 技能目标 能够进行点位运动轨迹示教及程序编写与调试。 能够进行连续路径轨迹示教及程序的编写与调试。 能够进行复杂轨迹的程序编写与调试。 3. 情感目标
机器人现场编程-川崎机器人示教-运动插补指令
3
二 、插补类型-各轴
各轴 9 3 1 1
结束点
起始点
• 说明：机器人TCP点以各轴插补类型，速度9、精度3、计时1、工具1的运 动参数从起始点运动到结束点，过程不考虑路径，以运动时间优先，一般情 况下为非直线运动。
4
二 、插补类型-直线
直线 7 3 1 1
结束点
起始点
• 说明：机器人TCP点以直线插补类型，速度7、精度3、计时1、工具1的运 动参数从起始点以直线轨迹运动到结束点。
5
二、插补类型-圆弧1/圆弧2
圆弧1 7 3 1 1 圆弧2 7 3 1 1
起始点
中间点
结束点
• 说明：机器人TCP点以圆弧插补类型，速度7、精度3、计时1、工具1的运 动参数从起始点开始，经过中间点，以圆弧轨迹运动到结束点。
6
三、 课程预告
• 工业机器人综合命令示教的操作方法。
7
大家好
川崎工业机器人示教
运动插补指令
一、 综合命令要素-插补
• 川崎工业机器人的通用插补类型如下表所示。
插补类型
说明
各 轴 当在两点之间，不考虑机器人的运动路径而以时间优先时，选择此类型。
直线 圆弧 1 圆弧 2
TCP 在两个示教点沿直线路径移动到目标点。
当 TCP 指定的 3 点以圆弧路径移动时，并要指定机器人在两点（开始和结 束点）之间的中间点的位姿时，选择此模式。 当 TCP 指定的 3 点以圆弧路径移动时，并要指定机器人在结束点的位姿 时，选择此模式。
《工业机器人轨迹规划算法的研究与实现》
《工业机器人轨迹规划算法的研究与实现》一、引言随着工业自动化技术的飞速发展，工业机器人已成为现代制造业不可或缺的重要工具。
轨迹规划作为机器人运动控制的核心技术之一，对于提高机器人的工作效率、精度和灵活性具有重要意义。
本文旨在研究工业机器人轨迹规划算法，探讨其实现方法，并分析其在工业领域的应用前景。
二、工业机器人轨迹规划算法概述工业机器人轨迹规划是指根据任务需求，为机器人规划出一条从起始点到目标点的最优路径。
这一过程涉及到机器人的运动学、动力学以及环境因素等多方面因素。
常见的轨迹规划算法包括插补法、优化法、学习法等。
1. 插补法插补法是一种基于数学模型的轨迹规划方法，通过在关键点之间插入适当的中间点，形成平滑的轨迹。
插补法具有计算简单、实时性好的特点，适用于对精度要求不高的场景。
2. 优化法优化法是一种以优化目标函数为手段的轨迹规划方法。
它通过考虑机器人的运动学、动力学约束以及任务需求，建立优化模型，并采用相应的优化算法求解。
优化法可以获得更优的轨迹，提高机器人的工作效率和精度。
3. 学习法学习法是一种基于机器学习技术的轨迹规划方法。
它通过学习历史数据和经验知识，自主生成适应新任务的轨迹。
学习法具有较高的自适应性和智能性，适用于复杂多变的工业环境。
三、工业机器人轨迹规划算法的实现实现工业机器人轨迹规划算法需要综合考虑机器人的运动学模型、动力学模型以及任务需求等因素。
下面以优化法为例，介绍轨迹规划算法的实现过程。
1. 建立优化模型根据任务需求和机器人的运动学、动力学约束，建立优化模型。
优化模型通常包括目标函数和约束条件两部分。
目标函数用于描述轨迹的优劣，如时间最短、能耗最低等；约束条件用于描述机器人的运动学、动力学约束以及安全要求等。
2. 选择优化算法根据优化模型的特点和需求，选择合适的优化算法进行求解。
常用的优化算法包括梯度下降法、遗传算法、模拟退火算法等。
在选择优化算法时，需要考虑算法的收敛速度、求解精度以及计算复杂度等因素。
工业机器人行走轨迹优化算法研究
工业机器人行走轨迹优化算法研究随着社会的发展，机器人在生产领域的应用越来越广泛。
在工业领域中，机器人在生产线上协同作业，提高了生产效率，缩短了生产周期。
然而，工业机器人的行走轨迹优化依然是生产线上的一个重要问题。
通过行走轨迹的优化，可以最大程度地提高机器人的工作效率，降低能耗和成本。
因此，本文着重探讨工业机器人行走轨迹优化算法研究的问题。
一、背景工业机器人行走轨迹优化是一个非常重要的问题，因为机器人在生产线上的移动速度和路径会直接影响工作效率和生产成本。
沿着最短路径行走可以提高生产效率和降低成本。
工业机器人在执行任务的过程中会出现转弯、停顿等不必要的操作，这对于生产效率和设备的寿命都会造成影响。
因此，有必要研究工业机器人的行走轨迹优化算法，以提高机器人的工作效率和生产效率。
二、研究内容工业机器人的行走轨迹优化算法主要通过优化机器人的行走路径和操作，以达到提高生产效率和降低成本的目的。
1. 路径规划路径规划是优化工业机器人行走轨迹的主要方法之一。
常用的路径规划算法有遗传算法、粒子群算法、模拟退火算法等。
这些算法通过计算机模拟人类智慧的思维方式，来求解机器人的最优行走路径。
2. 操作优化除了路径规划外，还可以通过优化操作来提高机器人的工作效率。
例如，对于重复性任务，可以通过编程实现自动化，减少人为干预的时间。
同时，还可以合理调整机器人的速度、加速度和转弯半径等参数，避免机器人重复做出不必要的操作，优化机器人的行走轨迹。
三、研究意义工业机器人是生产线上不可或缺的一部分，其运动轨迹的优化对于生产线的效率和生产成本有着极大的影响。
本研究将有助于提高工业机器人的工作效率和生产效率，也将为未来机器人技术的发展提供重要的基础。
通过对机器人轨迹的优化，还可以大大减少机器人对于生产环境的干扰，降低机器人使用成本。
四、总结工业机器人行走轨迹优化算法的研究是工业智能化发展的重要组成部分，其重要性不言而喻。
通过对机器人路径规划和操作优化的研究，可以提高机器人的工作效率和生产效率，降低成本。
工业机器人运动规划算法的使用方法与运动轨迹准确度分析
工业机器人运动规划算法的使用方法与运动轨迹准确度分析工业机器人运动规划是指通过算法的设计与实现，使机器人能够在工作空间内高效、准确地完成指定的任务。
在工业生产中，机器人的运动规划算法的应用非常重要，它直接影响着机器人的运动轨迹准确度和工作效率。
本文将介绍工业机器人运动规划算法的使用方法，并对其运动轨迹准确度进行分析。
首先，工业机器人运动规划算法的使用方法可以分为以下几个步骤：1. 建立工作空间模型：在进行运动规划之前，首先需要建立机器人的工作空间模型。
这包括对机器人的结构和约束条件进行建模，并设置工作空间的边界和障碍物等。
2. 确定运动目标：根据实际需求，确定机器人的运动目标。
这可以是一个位姿、路径或轨迹。
3. 选择运动规划算法：根据机器人的结构和工作环境的要求，选择适合的运动规划算法。
常见的算法包括关节空间规划算法、任务空间规划算法和混合规划算法等。
4. 运动规划：根据选择的算法，对机器人进行运动规划。
这包括对机器人的路径进行生成、优化和插补等。
5. 生成机器人控制指令：根据运动规划的结果，生成机器人的控制指令。
这些指令可以是关节角度或位姿的控制命令，以实现机器人的运动。
以上是工业机器人运动规划算法的使用方法的基本步骤。
不同的算法和应用场景可能会有所不同，具体的使用方法需根据实际情况进行调整。
其次，对于工业机器人运动规划算法的运动轨迹准确度，可以通过以下几个方面进行分析：1. 算法精度：不同的运动规划算法具有不同的精度。
一般来说，算法精度越高，生成的轨迹也越准确。
因此，在选择算法时应考虑算法的精度。
2. 器件精度：除了算法的影响外，机器人自身的精度也会对运动轨迹的准确度产生影响。
例如，关节的编码器精度、传感器的测量精度等。
因此，在实际运用中，需要对机器人的器件精度进行准确测量并考虑其影响。
3. 环境干扰：在生产环境中，机器人的运动轨迹可能会受到环境干扰的影响，例如重力、摩擦力、气压等。
这些干扰会导致机器人的轨迹偏离预期值。
工业机器人的实时高精度路径跟踪与轨迹规划
工 业 机 器 人 在 实 际 运 行 期 间 ， 手 部 的路 在着 一定的不足 ，希望我 国专业技术人员加强 径跟踪精准度提升 ，那么整个机器人 的首位两 对工业机器人实时 高精度路径跟踪与关节规划 端路径节 点就会有所 下降，如果节 点下降现象 工作 的研 究，只有这样才能保证工业机器人可 控 制 不 及 时 ，那 么 整 路 径 节 点就 会 出一 定 的弊 以在 各 个 领 域 中得 到 广 泛 的 应 用 ，采 集 我 国工
在 对 节 点控 制 期 间 ， 还要 沿 着指 定 的 关 节 轨迹
加 强工 业机 器人 高度 高精度 的控 制， 只有这 样 才 能保 证 工业 机 器
人可 以正 常运行 下去。基 于 此，
工业机器人实 时高精度 。对 于其 中的震荡性特
点来 说，还应 该做好节 点坐标值 的限制工作 ， 减少振 荡现 象的发生 ，降低振荡幅度 ，从而保 证工业机器 人实时 高精度路径跟踪与关节规划
点 体 现 出来 ，保 证 工 业 机 器 人 可 以正 常使 用 。然 而，工 业机 器人 在 实 际运 行 期 间常常会 因为 多种 原 因影 响着机 器人 的控制 性 能， 导致 其 不 能正常 运行 下去 要 想 从 根 本 上 解 决 这 一 问 题 ， 就 应 该
中重要组成部分 ， 做好工业机器人的控制工作，
如 说 ，Ｏ。 一 Ｘ０ Ｙ ０ ｚ 。 为工业 机器 人的基 础坐标 ， 业大学 ， ２ ０ １ ５ ． 【 ２ ］周 巍 ．煤 矿 井 下 搜 救 探 测 机 器人 的路 径 规
划 及 轨 迹 跟 踪 控 制研 究 ［ Ｄ ］ ．太 原 理 工 大 学， ２ ０ １ １ ．
的发展来说起到 了非常重要 的作用 。县手
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
工业机器人的实时轨迹插补算法李天友 ,孟正大 ,陈勍奇（东南大学自动化学院，江苏南京 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 *+= (22**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 *−= (62**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位姿量，然后把这三个量作为中间变量，等待下一步调用，这样就完成了电弧跟踪偏差的实时补偿。