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

权利要求书

1.一种基于速度最优控制的空间轨迹平滑方法，其特征在于，包括如下步骤：

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

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

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

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

2.如权利要求1所述的基于速度最优控制的空间轨迹平滑方法，其特征在于，在所述步骤S2中，采用下式判断所述预设转弯区半径r是否超过所述第一简单轨迹的长度的一半，

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

3.如权利要求2所述的基于速度最优控制的空间轨迹平滑方法，其特征在于，在所述步骤S3中，当选取的转弯方式为等长度转弯时，计算S、M和E点坐标包括如下步骤：

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

4.如权利要求2所述的基于速度最优控制的空间轨迹平滑方法，其特征在于，在所述步骤S3中，当选取的转弯方式为等时间转弯时，包括如下步骤：

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

t＝min(t₁，t₂)，

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

5.如权利要求1所述的基于速度最优控制的空间轨迹平滑方法，其特征在于，在所述步骤S4中，所述计算所述空间轨迹在所述局部坐标系下的转弯区的轨迹方程，包括如下步骤：

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

A=cosθ-sinθsinθcosθ,]]>

其中，θ为坐标轴进行旋转变换的角度，A为旋转矩阵，

步骤S42，导出向量坐标和包括如下步骤：

其中，为SM在所述局部坐标系下的表示,为ME在所述局部坐标系下的表示，

步骤43，计算所述空间轨迹在所述局部坐标系下的转弯区的轨迹方程y为：

y＝ax²，

其中，a为所述轨迹方程对应的二次曲线的参数，Q＝S-As，O为所述局部坐标系的坐标原来在全局坐标系下的表示，为坐标变换过程中的平移部分，s(x_s，y_s)，m(x_m，y_m)，e(x_e，y_e)分别为转弯区的起点、中间点、终点坐标。