[0059] 当选取的转弯方式为等长度转弯时，计算S、M和E点坐标包括如下步骤：
[0060] 等长度方式转弯要求转弯区起点到转弯区中间点、转弯区中间点到转弯区终点长 度相等，g卩ΡΙ?_Ιβ根据等长度转弯需要满足《議it，采用向量叠加方法，由中点 M(中点M即为点Pm)的坐标分别叠加 ξ?、^两个方向上长度为r的向量，即可得转弯 区的起点S的坐标、终点E的坐标。
[0061]
[0062]
[0063] 当选取的转弯方式为等时间转弯时，包括如下步骤：
[0064] 等时间转弯方式要求转弯区起点到转弯区中间点、转弯区中间点到转弯区终点用 时相等，即tl = t2,其中，tl为SM段转弯时间，t2为ME段转弯时间，
[0065] t = min (tp t2)， ( 4 )
[0066] 代入速度规划，计算得到S、E点距M点距离dists、dists，
[0067]
[0068]
[0069] 根据步骤S3计算得到的S、M、E三点坐标，建立局部坐标系，通过坐标变换将该问 题转移到局部坐标系下解决，使得求得的轨迹在局部坐标系下的转弯区的轨迹方程是简单 的二次曲线。下面为计算满足平滑条件的转弯区曲线方程的求解过程。
[0070] 步骤S4,根据计算得到S、M和E点的坐标建立局部坐标系，计算空间轨迹在局部 坐标系下的转弯区的轨迹方程，即转弯区轨迹方程。其中，轨迹方程为平滑的转弯区的二次 曲线。
[0071] 在算法流程中，对二次曲线方程的表述建立在局部坐标系下，与全局坐标系通过 线性变换进行映射。
[0072] 步骤S41，计算坐标变换参数Θ和A，包括如下步骤：
[0075] 其中，Θ为坐标轴进行旋转变换的角度，A为旋转矩阵，
[0076] 步骤S42,导出向量坐标_和織,包括如下步骤：
[0077]
[0078]
[0079] 其中，11为?:在局部坐标系下的表示，为在局部坐标系下的表示，坐标 变换可解耦为平移与旋转两部分。对于向量的变换而言，坐标变换的平移部分被消去了，只 剩下旋转部分，因此两坐标下的表示可由变换矩阵A直接联系起来。
[0080] 步骤S43,计算空间轨迹在局部坐标系下的转弯区的轨迹方程y为：
[0081] y = ax2，
[0082] 其中，
a为轨迹方程对应的二次曲线的参数， 爲:》*微1, 〇 = S-As，0为局部坐标系的坐标原来在全局坐标系下的表示，为坐标变换过程中 的平移部分，s(xs，ys)，m(xm，ym)，ek，yj，分别为转弯区的起点、中间点、终点坐标。
[0083] 图3根据本发明另一个实施例的基于速度最优控制的空间轨迹平滑方法的流程 图。
[0084] 步骤 S301，输入 Ps、Pm、r、flag_dist。
[0085] 步骤S302,对转弯半径进行判断，不超过任一简单轨迹的一半。
[0086] 具体地，判断转弯区半径r没有超过第一简单轨迹的长度的一半，且没有超过的 第二简单轨迹的长度的一半，
[0087]
[0088] 步骤S303,采用等长度转弯策略。
[0089] 等长度方式转弯要求转弯区起点到转弯区中间点、转弯区中间点到转弯区终点长 度相等，即丨
[0090] 步骤S304,采用等时间转弯策略。
[0091] 等时间转弯方式要求转弯区起点到转弯区中间点、转弯区中间点到转弯区终点用 时相等，即tl = t2。
[0092] 需要说明的是，步骤S303和步骤S304可以择一执行。
[0093] 步骤S305,得到S、M、E三点坐标。
[0094] 步骤S306，求解坐标变换矩阵。
[0096] 步骤S307,求解SM、ME向量的局部坐标。
[0098] 步骤S308,由公式导出抛物线参数，各点坐标，局部坐标系原点相对坐标。
[0099] 步骤S309,输出轨迹参数和坐标变换参数。
[0100] y = · ax2，
[0101] 其中，
a为轨迹方程对应的二次曲线的参数， &?;似^ 0 = S-As，0为局部坐标系的坐标原来在全局坐标系下的表示，为坐标变换过程 中的平移部分，S(xs，ys)，m(xm，ym)，eUy yj，分别为转弯区的起点、中间点、终点坐标。
[0102] 根据本发明实施例的基于速度最优控制的空间轨迹平滑方法，可以将机器人机械 臂运行空间轨迹的二次曲线经坐标变换生成平滑转弯区表达式，从而保证速度方向的连续 性，通过平滑过渡两直线轨迹，结合S型速度规划，得到加速度、速度、位移平滑的转弯方 案，轨迹简单直观，可方便定位机械臂的末端执行器在任意时刻的空间位置，便于控制。同 时由于轨迹为简单的二次曲线，因此可简便计算出障碍规避空间。此外，本发明可以适应等 距与等时两种转弯需求，用户可根据实际情况，方便地比较与选取更优的方案。
[0103] 在本说明书的描述中，参考术语"一个实施例"、"一些实施例"、"示例"、"具体示 例"、或"一些示例"等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特 点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不 一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何 的一个或多个实施例或示例中以合适的方式结合。
[0104] 尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例 性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨 的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。本发明的范围 由所附权利要求及其等同限定。
【主权项】
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的坐标分别叠加 PLIS两个方向上长度为r的向量，即可得转弯区的起点坐标S、终点坐标E，其中，中 点M即为点Pm，计算出点S和E点坐标如下：4. 如权利要求2所述的基于速度最优控制的空间轨迹平滑方法，其特征在于，在所述 步骤S3中，当选取的转弯方式为等时间转弯时，包括如下步骤： 根据等时间转弯需要满足tl=t2,其中，tl为SM段转弯时间，t2为ME段转弯时间， t=min(t1?t2) ? 代入速度规划，计算得到S、E点距M点距离dists、distE，5.如权利要求1所述的基于速度最优控制的空间轨迹平滑方法，其特征在于，在所述 步骤S4中，所述计算所述空间轨迹在所述局部坐标系下的转弯区的轨迹方程，包括如下步 骤： 步骤S41，计算坐标变换参数0和A，包括如下步骤：其中，0为坐标轴进行旋转变换的角度，A为旋转矩阵， 步骤S42，导出向量坐标g和包括如下步骤：其中，編为SM在所述局部坐标系下的表示，鶸:为ME在所述局部坐标系下的表示， 步骤43,计算所述空间轨迹在所述局部坐标系下的转弯区的轨迹方程y为： 2 y=ax? 其中，EI为所述轨迹方程对应的二次曲线的参数，B?;皿1,Q=S-As，0为所述局部坐标系的坐标原来在全局坐标系下的表示，为坐标变换过 程中的平移部分，S(xs，ys)，m(xm，ym)，eUyyj分别为转弯区的起点、中间点、终点坐标。
【专利摘要】本发明提出了一种基于速度最优控制的空间轨迹平滑方法，包括如下步骤：获取机器人机械臂运行的空间轨迹的起点、中点和终点、预设转弯区半径和预设参数；判断所述预设转弯区半径r是否超过所述第一简单轨迹的长度的一半；选取所述机器人的转弯方式为等长度转弯或等时间转弯，计算得到第一和第二简单轨迹相交形成的M、S和E点的坐标；根据计算得到S、M和E点的坐标建立局部坐标系，计算所述空间轨迹在所述局部坐标系下的转弯区的轨迹方程，其中，所述轨迹方程为平滑的转弯区的二次曲线。本发明可以保证速度方向的连续性，可方便定位机械臂的末端执行器在任意时刻的空间位置，便于控制。
【IPC分类】G06F19/00, B25J9/16, B25J13/00
【公开号】CN105082156
【申请号】CN201510493101
【发明人】王志峰
【申请人】珞石（北京）科技有限公司
【公开日】2015年11月25日
【申请日】2015年8月12日