一种基于速度最优控制的空间轨迹平滑方法
【技术领域】
[0001] 本发明涉及机器人控制技术领域，特别涉及一种基于速度最优控制的空间轨迹平 滑方法。
【背景技术】
[0002] 空间轨迹平滑是空间运动学的一个分支。空间轨迹可由一系列简单轨迹拼接而 成。简单轨迹分为直线和圆弧两种，其起点和终点称为节点。需进行操作任务的节点称为停 止点，无操作任务的节点称为过渡点。在运动过程中，由速度为零至达到制定速度、由制定 速度减速至零，都是十分耗费时间的。为了提高整体运动速度，提升工作效率，就要想方设 法免去不必要的加减速过程，尽可能保持高速运行。过渡点的存在往往是为了绕开障碍物。 沿轨迹行进过程中，无需精确抵达过渡点，因此提出转弯区平滑的概念，即在过渡点所处的 轨迹相接处，采取平滑曲线过渡，保持较高速度通过新的平滑轨迹，从而提升转弯速度。
[0003] 现有轨迹平滑方案主要有以下两类。
[0004] (1)基于速度规划曲线，按照等时的原则（即按照原速度规划，转弯起点到转弯中 间点的时间与中间点到终点的时间相等）进行轨迹的平滑规划。由速度规划曲线计算转弯 路径上各点速度，进行位移的空间向量叠加。
[0005] (2)基于速度规划曲线，按照等距原则（即按照原速度规划，转弯起点到转弯中间 点的距离与中间点到终点的距离相等）进行轨迹的平滑规划。由速度规划曲线计算转弯路 径上各点速度，进行位移的空间向量叠加。
[0006] 现有的上述两种方案的缺陷在于：没有显式的轨迹表达式，或轨迹表达式过于复 杂，不是简单曲线，难以完成对机械臂轨迹规划的精确控制，也不利于计算障碍规避空间。 同时，按照上述方案执行，得到的在欧几里得空间中的轨迹并不是简单平滑曲线，其轨迹不 直观，不利于操作者的对运行结果的直接观察与评价。
【发明内容】
[0007] 本发明的目的旨在至少解决所述技术缺陷之一。
[0008] 为此，本发明的目的在于提出一种基于速度最优控制的空间轨迹平滑方法，可以 保证速度方向的连续性，可方便定位机械臂的末端执行器在任意时刻的空间位置，便于控 制。
[0009] 为了实现上述目的，本发明的实施例提供一种基于速度最优控制的空间轨迹平滑 方法，包括如下步骤：
[0010] 步骤S1，获取机器人机械臂运行的空间轨迹的起点Ps、中点Pm、终点Pe、预设转弯 区半径r和预设参数flag_dist，其中，所述起点Ps和中点Pm之间构成第一简单轨迹，所述 中点Pm和终点Pe之间构成第二简单轨迹；
[0011] 步骤S2,将所述预设转弯区半径r与所述第一简单轨迹和所述第二简单轨迹的长 度进行比较，如果所述预设转弯区半径r没有超过所述第一简单轨迹的长度的一半，且没 有超过所述的第二简单轨迹的长度的一半，则执行步骤S3 ;
[0012] 步骤S3,选取所述机器人的转弯方式为等长度转弯或等时间转弯，计算得到第一 和第二简单轨迹相交形成的过渡点M的坐标、所述第一和第二简单轨迹分别与预设圆形相 交的S和E点的坐标；
[0013] 步骤S4,根据计算得到S、M和E点的坐标建立局部坐标系，计算所述空间轨迹在 所述局部坐标系下的转弯区的轨迹方程，其中，所述轨迹方程为平滑的转弯区的二次曲线。
[0014] 进一步，在所述步骤S2中，采用下式判断所述预设转弯区半径r是否超过所述第 一简单轨迹的长度的一半，
[0015]
[0016] 其中，ξξ是由起点Ps指向中点Pm的向量，:ξξ是由中点Pm指向终点Pe的向 量。
[0017] 进一步，在所述步骤S3中，当选取的转弯方式为等长度转弯时，计算S、M和E点坐 标包括如下步骤：
[0018] 根据等长度转弯需要满足IPI #瞬1|，采用向量叠加方法，由中点1的坐标分别叠 加两个方向上长度为r的向量，即可得转弯区的起点坐标S、终点坐标E，其中，中 点M即为点Pm，计算出点S和E点坐标如下：
[0019]
[0020]
[0021] 进一步，在所述步骤S3中，当选取的转弯方式为等时间转弯时，包括如下步骤：
[0022] 根据等时间转弯需要满足tl = t2,其中，tl为SM段转弯时间，t2为ME段转弯时 间，
[0023] t = mimUp t2)，
[0024] 代入速度规划，计算得到S、E点距M点距离dists，distE，
[0025]
[0026]
[0027] 进一步，在所述步骤S4中，所述计算所述空间轨迹在所述局部坐标系下的转弯区 的轨迹方程，包括如下步骤：
[0028] 步骤S41，计算坐标变换参数Θ和A，包括如下步骤：
[0031] 其中，Θ为坐标轴进行旋转变换的角度，A为旋转矩阵，
[0032] 步骤S42,导出向量坐标和,包括如下步骤：
[0033]
[0034]
[0035] 其中，艤为靈在所述局部坐标系下的表示，为議在所述局部坐标系下的表 示，
[0036] 步骤43,计算所述空间轨迹在所述局部坐标系下的转弯区的轨迹方程y为：
[0037] y = ax2,
[0038] 其中，
a为所述轨迹方程对应的二次曲线的参数，
為, 0 = S-As，0为所述局部坐标系的坐标原来在全局坐标系下的表示，为坐标变换过 程中的平移部分，s(xs，ys)，m(xm，ym)，eUy yj分别为转弯区的起点、中间点、终点坐标。
[0039] 根据本发明实施例的基于速度最优控制的空间轨迹平滑方法，可以将机器人机械 臂运行空间轨迹的二次曲线经坐标变换生成平滑转弯区表达式，从而保证速度方向的连续 性，通过平滑过渡两直线轨迹，结合S型速度规划，得到加速度、速度、位移平滑的转弯方 案，轨迹简单直观，可方便定位机械臂的末端执行器在任意时刻的空间位置，便于控制。同 时由于轨迹为简单的二次曲线，因此可简便计算出障碍规避空间。此外，本发明可以适应等 距与等时两种转弯需求，用户可根据实际情况，方便地比较与选取更优的方案。
[0040] 本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变 得明显，或通过本发明的实践了解到。
【附图说明】
[0041] 本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变 得明显和容易理解，其中：
[0042] 图1为根据本发明一个实施例的基于速度最优控制的空间轨迹平滑方法的流程 图；
[0043] 图2为根据本发明实施例的第一和第二简单轨迹的示意图；
[0044] 图3根据本发明另一个实施例的基于速度最优控制的空间轨迹平滑方法的流程 图。
【具体实施方式】
[0045] 下面详细描述本发明的实施例，实施例的示例在附图中示出，其中自始至终相同 或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描 述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
[0046] 本发明提一种基于速度最优控制的空间轨迹平滑方法，利用二次曲线的平滑特 性，根据两直线路径起点、终点及所夹转弯区的转弯半径，生成二次曲线，平滑连接两直线 路径，将二次曲线经坐标变换生成平滑转弯区表达式，从而保证速度方向的连续性。
[0047] 如图1所示，本发明实施例的基于速度最优控制的空间轨迹平滑方法，包括如下 步骤：
[0048] 步骤S1，获取机器人机械臂运行的空间轨迹的起点Ps、中点Pm、终点Pe、预设转弯 区半径r和预设参数flag_dist。其中，起点Ps和中点Pm之间构成第一简单轨迹，中点Pm 和终点Pe之间构成第二简单轨迹。预设参数flag_dist为采取等距转弯或等时转弯的参 数，其值为1时为等距转弯，否则为等时转弯。
[0049] 图2为根据本发明实施例的第一和第二简单轨迹的示意图。
[0050] 首先对转弯区的定义进行说明：在两个简单轨迹相交形成的过渡点M处，以过渡 点为原点，以某一特定值R为半径做圆，相交两简单轨迹于S，E两点，由线段SM，ME，圆弧ES 围成的区域即为转弯区。参考图2,第一简单轨迹的起点为Ps，终点为过渡点M;第二简单 轨迹的起点为M，终点为Pe。第一和第二简单轨迹均为直线轨迹。
[0051] 在开始规划转弯区轨迹之前，需由两相交简单轨迹确定当前路径所在的平面，并 建立二维平面与三维空间的坐标转换方程，然后确定各关键点在二维平面的坐标。后续步 骤均在该平面上进行。
[0052] 步骤S2,将预设转弯区半径r与第一简单轨迹和第二简单轨迹的长度进行比较， 如果预设转弯区半径r没有超过第一简单轨迹的长度的一半，且没有超过的第二简单轨迹 的长度的一半，则执行步骤S3。
[0053] 在本步骤中，采用下式判断预设转弯区半径r是否超过第一简单轨迹的长度的一 半，
[0054]
[0055] 其中，1?是由起点Ps指向中点Pm的向量，即原轨迹中的第一简单轨迹，1?是 由中点Pm指向终点Pe的向量，即原轨迹中的第二简单轨迹。
[0056] 步骤S3,选取机器人的转弯方式为等长度转弯或等时间转弯，计算得到第一和第 二简单轨迹相交形成的过渡点M的坐标、第一和第二简单轨迹分别与预设圆形相交的S和 E点的坐标。
[0057] 具体地，根据起点Ps、中点Pm、终点Pe、预设转弯区半径r进行等距规划或等时规 划。其中，等距规划可以由转弯半径r直接求得。对于等时规划，由S型速度规划得到转弯 起点到过渡点的时间，以此计算转弯终点位置，由此得到转弯区起点终点坐标。即，本发明 可以基于S型速度规划且同时支持等时与等距两种规划的转弯区算法。
[0058] 下面分别对等距规划和等时规划进行说明。