[发明专利]一种基于速度最优控制的空间轨迹平滑方法

说明书

技术领域

本发明涉及机器人控制技术领域，特别涉及一种基于速度最优控制的空间轨迹平滑方法。

背景技术

空间轨迹平滑是空间运动学的一个分支。空间轨迹可由一系列简单轨迹拼接而成。简单轨迹分为直线和圆弧两种，其起点和终点称为节点。需进行操作任务的节点称为停止点，无操作任务的节点称为过渡点。在运动过程中，由速度为零至达到制定速度、由制定速度减速至零，都是十分耗费时间的。为了提高整体运动速度，提升工作效率，就要想方设法免去不必要的加减速过程，尽可能保持高速运行。过渡点的存在往往是为了绕开障碍物。沿轨迹行进过程中，无需精确抵达过渡点，因此提出转弯区平滑的概念，即在过渡点所处的轨迹相接处，采取平滑曲线过渡，保持较高速度通过新的平滑轨迹，从而提升转弯速度。

现有轨迹平滑方案主要有以下两类。

(1)基于速度规划曲线，按照等时的原则(即按照原速度规划，转弯起点到转弯中间点的时间与中间点到终点的时间相等)进行轨迹的平滑规划。由速度规划曲线计算转弯路径上各点速度，进行位移的空间向量叠加。

(2)基于速度规划曲线，按照等距原则(即按照原速度规划，转弯起点到转弯中间点的距离与中间点到终点的距离相等)进行轨迹的平滑规划。由速度规划曲线计算转弯路径上各点速度，进行位移的空间向量叠加。

现有的上述两种方案的缺陷在于：没有显式的轨迹表达式，或轨迹表达式过于复杂，不是简单曲线，难以完成对机械臂轨迹规划的精确控制，也不利于计算障碍规避空间。同时，按照上述方案执行，得到的在欧几里得空间中的轨迹并不是简单平滑曲线，其轨迹不直观，不利于操作者的对运行结果的直接观察与评价。

发明内容

本发明的目的旨在至少解决所述技术缺陷之一。

为此，本发明的目的在于提出一种基于速度最优控制的空间轨迹平滑方法，可以保证速度方向的连续性，可方便定位机械臂的末端执行器在任意时刻的空间位置，便于控制。

为了实现上述目的，本发明的实施例提供一种基于速度最优控制的空间轨迹平滑方法，包括如下步骤：

步骤S1，获取机器人机械臂运行的空间轨迹的起点Ps、中点Pm、终点Pe、预设转弯区半径r和预设参数flag_dist，其中，所述起点Ps和中点Pm之间构成第一简单轨迹，所述中点Pm和终点Pe之间构成第二简单轨迹；

步骤S2，将所述预设转弯区半径r与所述第一简单轨迹和所述第二简单轨迹的长度进行比较，如果所述预设转弯区半径r没有超过所述第一简单轨迹的长度的一半，且没有超过所述的第二简单轨迹的长度的一半，则执行步骤S3；

步骤S3，选取所述机器人的转弯方式为等长度转弯或等时间转弯，计算得到第一和第二简单轨迹相交形成的过渡点M的坐标、所述第一和第二简单轨迹分别与预设圆形相交的S和E点的坐标；

步骤S4，根据计算得到S、M和E点的坐标建立局部坐标系，计算所述空间轨迹在所述局部坐标系下的转弯区的轨迹方程，其中，所述轨迹方程为平滑的转弯区的二次曲线。

进一步，在所述步骤S2中，采用下式判断所述预设转弯区半径r是否超过所述第一简单轨迹的长度的一半，

其中，是由起点Ps指向中点Pm的向量，是由中点Pm指向终点Pe的向量。

进一步，在所述步骤S3中，当选取的转弯方式为等长度转弯时，计算S、M和E点坐标包括如下步骤：

根据等长度转弯需要满足采用向量叠加方法，由中点M的坐标分别叠加两个方向上长度为r的向量，即可得转弯区的起点坐标S、终点坐标E，其中，中点M即为点Pm，计算出点S和E点坐标如下：

进一步，在所述步骤S3中，当选取的转弯方式为等时间转弯时，包括如下步骤：

根据等时间转弯需要满足t1＝t2，其中，t1为SM段转弯时间，t2为ME段转弯时间，

t＝mim(t₁，t₂)，

代入速度规划，计算得到S、E点距M点距离dist_s，dist_E，

进一步，在所述步骤S4中，所述计算所述空间轨迹在所述局部坐标系下的转弯区的轨迹方程，包括如下步骤：

步骤S41，计算坐标变换参数θ和A，包括如下步骤：