CN117930833B

CN117930833B - 机器人轨迹的评估优化方法

Info

Publication number: CN117930833B
Application number: CN202311855537.1A
Authority: CN
Inventors: 魏洪兴; 赵永利; 唐亚伟; 苏彦业; 邵乔林; 卢晓宇
Original assignee: Aobo Jiangsu Robot Co ltd
Current assignee: Aobo Jiangsu Robot Co ltd
Priority date: 2023-12-28
Filing date: 2023-12-28
Publication date: 2024-12-27
Anticipated expiration: 2043-12-28
Also published as: CN117930833A

Abstract

本发明提供一种机器人轨迹的评估优化方法，方法包括：获取表征机器人所有轨迹的路点信息，并基于路点信息计算轨迹上所有轨迹点的末端线速度，其中，轨迹点包括路点；基于路点的末端线速度判断该路点是否合理，并在该路点不合理时修改路点信息，返回第一步，直至所有路点合理。由此，无需外部工具，能够计算路点的末端线速度，基于末端线速度对路点进行评估、优化，降低了调试成本，提高了调试效率，缩短了调试周期。

Description

机器人轨迹的评估优化方法

技术领域

本发明涉及机器人轨迹调试技术领域，具体涉及一种机器人轨迹的评估优化方法。

背景技术

目前，机器人在执行某些项目比如涂胶项目时，对机器人的运行轨迹要求较高，需要满足匀速且平滑等需求。用户在调试机器人运行轨迹时普遍都是通过肉眼观察或者配合胶枪等外部工具辅助观察，然后通过不断的运行调试，对于不满足需求的轨迹通过增加删除路点、修改路点的运动属性和位姿等方式。

然而，机器人的运行轨迹是否匀速很难用肉眼观察，只能配合胶枪等外部工具辅助观察轨迹是否匀速，这样会造成物力浪费，而且通过外部工具辅助观察并不能定位具体不合理的路点，只能通过不断运行调试找到具体不合理的路点，导致运行调试效率低，周期长，比如短则3～5天，多则1～2月。

发明内容

本发明为解决相关技术中研发周期长、成本高的问题，提出了如下技术方案。

本发明第一方面实施例提出了一种机器人轨迹的评估优化方法，包括：获取表征机器人所有轨迹的路点信息，并基于所述路点信息计算所述轨迹上所有轨迹点的末端线速度，其中，所述轨迹点包括路点；基于所述路点的末端线速度判断该路点是否合理，并在该路点不合理时修改路点信息，返回第一步，直至所有路点合理。

另外，根据本发明上述实施例的机器人轨迹的评估优化方法还可以具有如下附加的技术特征。

根据本发明的一个实施例，基于所述路点信息计算所述轨迹上所有轨迹点的末端线速度，包括：基于所述路点信息得到所述轨迹上每个轨迹点在机器人基坐标系下的位置；根据所述轨迹点的位置计算每个轨迹点的末端线速度。

根据本发明的一个实施例，通过以下公式计算所述轨迹点的末端线速度：

其中，Pos_i表示第i个轨迹点在机器人基坐标系下的位置，Pos_i-1表示第i-1个轨迹点在机器人基坐标系下的位置，i为大于0的整数，cycle表示轨迹点更新周期。

根据本发明的一个实施例，基于所述路点的末端线速度判断该路点是否合理，包括：获取机器人轨迹点的末端线速度标准值和末端线速度误差值；根据所述末端线速度标准值和所述末端线速度误差值计算末端线速度边界值，并根据所述末端线速度边界值得到末端线速度合理范围；如果所述路点的末端线速度不处于末端线速度合理范围，则确定路点不合理；如果所述轨迹曲线上路点的末端线速度处于末端线速度合理范围，则确定路点合理。

根据本发明的一个实施例，所述末端线速度标准值的数量为一个，所述末端线速度误差值的数量为两个，所述末端线速度合理范围包括各不相同的第一合理范围、第二合理范围和第三合理范围，通过以下公式计算末端线速度边界值：

L₂＝S+I₂

L₁＝S+I₁

T₁＝S-I₁

T₂＝S-I₂

其中，S为末端线速度标准值，I₁、I₂分别为末端线速度误差值，I₁小于I₂，I₁、I₂均大于0，L₁、L₂、T₂和T₂分别为末端线速度边界值；所述第一合理范围为(T₁，L₁)、第二合理范围为(T₂，T₁]以及第三合理范围为[L₁，L₂)。

根据本发明的一个实施例，在基于所述路点信息计算所述轨迹上所有轨迹点的末端线速度之后，还包括：基于所有所述轨迹点的位置、末端线速度绘制所述机器人的轨迹曲线，并显示所述轨迹曲线。

根据本发明的一个实施例，基于所有所述轨迹点的位置、末端线速度绘制所述机器人的轨迹曲线，包括：基于每个所述轨迹点的末端线速度确定该轨迹点的绘制颜色；基于所有所述轨迹点的位置、末端线速度和绘制颜色绘制所述机器人的所有轨迹点，以得到所述机器人的轨迹曲线。

根据本发明的一个实施例，基于每个所述轨迹点的末端线速度确定该轨迹点的绘制颜色，包括：获取机器人轨迹点的末端线速度标准值和末端线速度误差值；根据所述末端线速度标准值和所述末端线速度误差值计算末端线速度边界值，并根据所述末端线速度边界值得到末端线速度合理范围；确定每个所述末端线速度合理范围对应的绘制颜色、所述末端线速度不合理范围对应的绘制颜色；根据每个轨迹点的末端线速度、所述末端线速度合理范围及其对应的绘制颜色，确定该轨迹点的绘制颜色；确定路点的额外绘制颜色，以使每个路点的绘制颜色由两种不同的颜色混合而成。

根据本发明的一个实施例，通过以下公式确定轨迹点的绘制颜色：

其中，color为轨迹点的绘制颜色，color1、color2和color3分别为第一颜色、第二颜色和第三颜色，color1、color2和color3各不相同，v_i为第i个轨迹点的末端线速度，L₁、L₂、T₂和T₂分别为末端线速度边界值。

根据本发明的一个实施例，所述机器人的轨迹曲线包括机器人轨迹在机器人基坐标系的三维轨迹曲线、在机器人基坐标系xOy平面、xOz平面和yOz平面的二维轨迹曲线、以及机器人的轨迹点个数-末端线速度曲线。

基于所述轨迹点的位置、末端线速度和绘制颜色绘制所述机器人的轨迹点，以得到所述机器人的轨迹曲线，包括：在机器人基坐标系下根据所述轨迹点的位置、绘制颜色绘制所述机器人的轨迹点，以得到三维轨迹曲线；分别在机器人基坐标系xOy平面、xOz平面和yOz平面根据所述轨迹点的位置、绘制颜色绘制所述机器人的轨迹点，以得到二维轨迹曲线；建立以轨迹点个数为横轴、以轨迹点的末端线速度为纵轴的坐标系，并在该坐标系下根据所述机器人的轨迹点的末端线速度、绘制颜色绘制所述轨迹点，以得到轨迹点个数-末端线速度曲线。

根据本发明的一个实施例，基于所述轨迹点的位置、末端线速度和绘制颜色绘制所述机器人的轨迹点，以得到所述机器人的轨迹曲线，还包括：将所述三维轨迹曲线中路点的形状绘制为预设立体形状；将所述二维轨迹曲线、所述轨迹点个数-末端线速度曲线中路点的形状绘制为预设平面形状。

根据本发明的一个实施例，所述预设立体形状为球体，将所述三维轨迹曲线中路点的形状绘制为预设立体形状，包括：计算所述三维轨迹曲线中路点在模型视图矩阵下的位置向量；基于所述位置向量计算球体半径；根据所述球体半径将所述三维轨迹曲线中路点的形状绘制为球体。

根据本发明的一个实施例，通过以下公式计算路点在模型视图矩阵下的位置向量：

p＝M_mv(Pos×1000)

其中，p表示路点在模型视图矩阵下的位置向量，M_mv表示模型视图矩阵，Pos表示路点在机器人基坐标系下的位姿，Pos×1000表示路点在三维空间中的位置。

通过以下公式计算球体半径：

r＝|p|×θ

其中，r表示球体半径，|p|为p的长度，为路点到摄像机的距离，θ为球形大小系数。

根据本发明的一个实施例，机器人轨迹的评估优化方法，其特征在于，还包括：在路点不合理时计算所述路点的不合理分数，并将所述不合理分数进行显示。

根据本发明的一个实施例，通过以下公式计算所述不合理分数：

score＝(1-|v_不/S-1|)×100

其中，score为不合理分数，S为末端线速度标准值，v_不为不合理路点的末端线速度。

本发明实施例的技术方案，首先获取表征机器人所有轨迹的路点信息，并基于路点信息计算轨迹上所有轨迹点的末端线速度，其中，轨迹点包括路点，然后基于路点的末端线速度判断该路点是否合理，并在该路点不合理时修改路点信息，返回第一步，直至所有路点合理。由此，无需插件即可将DXF文件转换为机械臂运动轨迹，可以避免与插件开发商沟通带来的时间成本以及插件价格和维护费用，从而可以减少研发时间，降低研发成本。

附图说明

图1为本发明实施例的机器人轨迹的评估优化方法的流程图。

图2为本发明一个示例的显示满足需求的三维轨迹曲线的主界面示意图。

图3为本发明一个示例的主界面中参数设置区域的示意图。

图4为本发明一个示例的路点参数设置界面的示意图。

图5为本发明一个示例的显示不满足需求的三维轨迹曲线的主界面示意图。

图6为本发明一个示例的显示不满足需求的二维xOz曲线的主界面示意图。

图7为本发明一个示例的显示不满足需求的末端线速度曲线的主界面示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

图1为本发明实施例的机器人轨迹的评估优化方法的流程图。

本发明实施例的机器人可以是协作机器人。

如图1所示，该机器人轨迹的评估优化方法包括以下步骤S1和S2。

S1，获取表征机器人所有轨迹的路点信息，并基于路点信息计算轨迹上所有轨迹点的末端线速度，其中，轨迹点包括路点。

轨迹，是指机器人末端的运动轨迹。

路点，表示机器人末端要达到的位置点，通常机器人轨迹包括至少一个路点。

路点信息，可包括路点的名称、运动属性(直线、轴动、圆弧运动等)、关节速度、加速度、角加速度和位姿等信息。

轨迹点的末端线速度，是指轨迹点所对应的机器人末端的线速度，即机器人末端运动至该轨迹点时的线速度。

具体地，可首先通过任何可行方式获取机器人的路点信息，该路点信息表征机器人的轨迹，可对路点信息进行插补等处理后得到机器人每条轨迹以及每条轨迹上包含路点的所有轨迹点，然后计算每个轨迹点的末端线速度。

S2、基于路点的末端线速度判断该路点是否合理，并在该路点不合理时修改路点信息，返回第一步，直至所有路点合理。

计算出所有轨迹点的末端线速度后，获取其中路点的末端线速度，并基于此判断该路点是否合理，如果该路点不合理，则修改路点的运动属性、关节速度、加速度、角加速度和/或位姿等信息，并返回第一步即步骤S1，再次计算路点末端线速度、判断路点是否合理，如此循环执行步骤S1、S2，判断轨迹是否被优化，反复调整和充评估知道直至所有路点合理时，轨迹符合需求；如果所有路点合理，则保存路点信息，并将其在真机上进行验证或者使用。

相较于相关技术，本发明实施例无需肉眼或外部工具观察轨迹是否匀速，因此降低了成本；能够计算末端线速度，并基于末端线速度直接判断具体路点是否合理，从而可以直接定位到不合理的路点，并对路点信息进行调整后再次评估，提高了调试效率，缩短调试周期。

由此，本发明实施例的机器人轨迹的评估优化方法，无需外部工具，能够计算路点的末端线速度，基于末端线速度对路点进行评估、优化，降低了调试成本，提高了调试效率，缩短了调试周期。

在本发明的一个实施例中，步骤S1中的基于路点信息计算轨迹上所有轨迹点的末端线速度，可包括：基于路点信息得到轨迹上每个轨迹点在机器人基坐标系下的位置；根据轨迹点的位置计算每个轨迹点的末端线速度。

具体地，首先可选取并加载一个机器人示教器(比如AuboPE)在线编辑好的工程文件(比如涂胶工程文件)，该工程文件包括机器人的所有路点信息，对该工程文件进行解析得到其中的路点信息。然后可调用底层轨迹规划算法对路点信息完成轨迹规划，得到每条轨迹以及每条轨迹上的所有轨迹点在机器人基坐标系下的位置(三维坐标)，可标记其中路点的名称。最后根据第i个轨迹点的位置和第i-1个轨迹点的位置计算第i个轨迹点的末端线速度，即通过以下公式计算每个轨迹点的末端线速度：

计算出所有轨迹点包括路点的末端线速度之后，执行步骤S2，即基于路点的末端线速度判断该路点是否合理。

在本发明的一个实施例中，基于路点的末端线速度判断该路点是否合理，可包括：获取机器人轨迹点的末端线速度标准值和末端线速度误差值；根据末端线速度标准值和末端线速度误差值计算末端线速度边界值，并根据末端线速度边界值得到末端线速度合理范围；如果路点的末端线速度不处于末端线速度合理范围，则确定路点不合理；如果路点的末端线速度处于末端线速度合理范围，则确定路点合理。

其中，机器人轨迹点的末端线速度标准值是指机器人末端运动至轨迹点时满足需求的末端线速度，而末端线速度误差值是指在末端线速度标准值的基础上允许上下浮动的值。末端线速度标准值和线速度误差值可以是用户根据实际需求具体配置的，其数量均可以是至少一个，且两者数量可以相同也可以不同，比如一个标准值一个误差值、一个标准值两个误差值等。

进一步地，末端线速度标准值的数量为一个，末端线速度误差值的数量为两个，末端线速度合理范围包括各不相同的第一合理范围、第二合理范围和第三合理范围。

具体而言，首先获取用户事先配置的一个末端线速度标准值和两个末端线速度误差值，通过以下公式计算末端线速度边界值：

L₂＝S+I₂

L₁＝S+I₁

T₁＝S-I₁

T₂＝S-I₂

其中，S为末端线速度标准值，I₁、I₂分别为末端线速度误差值，I₁小于I₂，I₁、I₂均大于0，L₁、L₂、T₂和T₂分别为末端线速度边界值。

根据末端线速度边界值L₁、L₂、T₂和T₂得到末端线速度合理范围，其中，第一合理范围为(T₁，L₁)、第二合理范围为(T₂，T₁]以及第三合理范围为[L₁，L₂)，第一合理范围表示末端线速度满足需求，第二合理范围表示末端线速度可以接受，第三合理范围表示末端线速度也可以接受，对于不在这三个合理范围之内的末端线速度，即为不能接受的末端线速度。

然后对于每个路点，判断其末端线速度v处于三个合理范围之内还是之外，如果处于三个合理范围之内，即((T₁>v>L₁)∪(L₂>v>L₁]∪(T₂>v>T₁])，则确定路点合理；如果不处于三个合理范围之内，即(v>L₂)∪(v<T₂)，则确定路点不合理，实现对轨迹的评估。

最后在确定出不合理路点时，可将不合理路点的路点信息反馈给用户，用户可根据反馈的路点信息中的名称定位到具体路点，并可对该路点信息进行修改，也可以对其他路点比如附近路点的路点信息(运动属性、位姿等)进行修改，还可以对路点进行增删等修改，实现对轨迹的优化。基于修改后的路点信息重新评估优化轨迹，即重新计算所有轨迹点的末端线速度以及基于路点的末端线速度判断该路点是否合理，并在该路点不合理时优化轨迹。

如果所有路点合理，则保存路点信息，并将其写入步骤S1中加载的工程文件，在真机上使用AuboPE加载该工程并验证轨迹的运行效果。

由此，实现对机器人轨迹路点的循环评估、优化，其中在每次评估优化时，可评估出每个路点是否合理，且不合理时可快速定位到具体路点，并进行优化，针对性地实现评估优化，提高了调试效率，缩短了调试周期，且可以保证调试可靠性。

相关技术中，机器人轨迹不直观，没法肉眼观察，需要外部设备辅助观察，会造成物力浪费。为此，本发明实施例绘制轨迹进行显示，使轨迹及其轨迹点直观化，用户可以直接查看。

即在本发明的一个实施例中，在步骤S1之后，还可以包括：基于所有轨迹点的位置、末端线速度绘制机器人的轨迹曲线，并显示轨迹曲线。

在一个示例中，基于所有轨迹点的位置、末端线速度绘制机器人的轨迹曲线，可包括：基于每个轨迹点的末端线速度确定该轨迹点的绘制颜色；基于所有轨迹点的位置、末端线速度和绘制颜色绘制机器人的所有轨迹点，以得到机器人的轨迹曲线。

进一步地，基于每个轨迹点的末端线速度确定该轨迹点的绘制颜色，可包括：获取机器人轨迹点的末端线速度标准值和末端线速度误差值；根据末端线速度标准值和末端线速度误差值计算末端线速度边界值，并根据末端线速度边界值得到末端线速度合理范围；确定每个末端线速度合理范围对应的绘制颜色、末端线速度不合理范围对应的绘制颜色；根据每个轨迹点的末端线速度、末端线速度合理范围及其对应的绘制颜色，确定该轨迹点的绘制颜色；确定路点的额外绘制颜色，以使每个路点的绘制颜色由两种不同的颜色混合而成。

具体而言，首先可通过以下公式计算末端线速度边界值：

L₂＝S+I₂

L₁＝S+I₁

T₁＝S-I₁

T₂＝S-I₂

其中，S为末端线速度标准值，I₁、I₂分别为线速度误差值，I₁小于I₂，I₁、I₂均大于0，L₁、L₂、T₂和T₂分别为末端线速度边界值。第一合理范围为(T₁，L₁)、第二合理范围为(T₂，T₁]以及第三合理范围为[L₁，L₂)。

然后可随机确定三种不同的绘制颜色，一一对应第一合理范围、第二和第三合理范围、以及末端线速度不合理范围，以得到每个范围对应的绘制颜色，确定出所有轨迹点(包括路点)的末端线速度所在的范围后，即可确定每个轨迹点的轨迹点绘制颜色，即通过以下公式确定每个轨迹点的绘制颜色：

比如，第一颜色color1、第二颜色color2和color3第三颜色分别是绿色(green)、黄色(yellow)和红色(red)。

可见，每个轨迹点的绘制颜色为一种颜色。在该基础上，为了将轨迹点中的路点和非路点进行区分，可将路点的绘制颜色增加一种额外绘制颜色(比如蓝色)，即每个路点的绘制颜色由两种不同的颜色混和而成，比如，某一路点的绘制颜色为绿色、黄色或红色、加蓝色的的混合，分别为青色、绿色或紫色。

总结，将轨迹曲线根据轨迹点的位置、末端线速度进行绘制并显示于人机界面，不仅可以使用户直观地查看轨迹曲线，而且可以通过颜色使用户直观地查看每个轨迹点的末端线速度是否合理，对于非路点的轨迹点，当其显示为绿色时，用户可知该轨迹点满足需求；当其显示为黄色时，用户可知该轨迹点可以接受；当其显示为红色时，用户可知该轨迹点不能接受。对于路点，当其显示为青色时，用户可知该路点满足需求；当其显示为绿色时，用户可知该路点可以接受；当其显示为紫色时，用户可知该路点不能接受。

由此，用户可以直观看到轨迹及其所有轨迹点的末端线速度，不需要再通过外部工具辅助查看，可以节省很多人力物力。

在本发明的一个实施例中，机器人的轨迹曲线包括机器人轨迹在机器人基坐标系的三维轨迹曲线、在机器人基坐标系xOy平面、xOz平面和yOz平面的二维轨迹曲线、以及机器人的轨迹点个数-末端线速度曲线。

进一步地，基于轨迹点的位置、末端线速度和绘制颜色绘制机器人的轨迹点，以得到机器人的轨迹曲线，可包括：在机器人基坐标系下根据轨迹点的位置、绘制颜色绘制机器人的轨迹点，以得到三维轨迹曲线；分别在机器人基坐标系xOy平面、xOz平面和yOz平面根据轨迹点的位置、绘制颜色绘制机器人的轨迹点，以得到二维轨迹曲线；建立以轨迹点个数为横轴、以轨迹点的末端线速度为纵轴的坐标系，并在该坐标系下根据机器人的轨迹点的末端线速度、绘制颜色绘制轨迹点，以得到轨迹点个数-末端线速度曲线。

具体而言，根据上述得到的所有轨迹点的位置、绘制颜色，分别在机器人基坐标系下、基坐标系xOy平面、xOz平面和yOz平面绘制机器人的所有轨迹点，以得到带有绘制颜色的三维轨迹曲线、三个二维轨迹曲线，并将其进行显示，以供用户随时查看。同时曲线中的每个轨迹点携带自身信息，用户通过选中曲线中的轨迹点即可弹出该轨迹点的信息，比如名称、运动属性、位姿等，因此用户可根据路点的颜色选中不合理路点，弹出不合理路点的信息，用户可修该信息后，保存修该后的信息，然后重新绘制曲线，即可看到曲线是否被优化。

其中，可使用OpenGL的glColor4d接口设置绘制颜色、glDrawArrays绘制轨迹点。

以轨迹点个数为横轴、以轨迹点的末端线速度为纵轴建立一个平面坐标系，将计算出的轨迹点的末端线速度曲线根据其绘制颜色绘制到该平面坐标系中，得到轨迹点个数-末端线速度曲线。比如，机器人轨迹800个轨迹点，那么可以绘制出800个轨迹点的末端线速度，其中，每个轨迹点的末端线速度的颜色为以上得到的绘制颜色。用户通过查看曲线颜色即可知晓所有轨迹及路点是否合理，并在不合理时，通过选中该末端线速度即可弹出该末端线速度所属路点的路点信息，用户可修该信息后，保存修该后的信息，然后重新绘制曲线，即可看到曲线是否被优化。

除了显示曲线，还可以显示不合理路点列表。用户可在人际界面选择具体曲线进行查看，在显示具体曲线的同时，可显示该曲线上的不合理路点列表，该列表可包括路点名称、运动属性和末端线速度，同样用户可通过选中该列表中的路点，弹出该路点的信息，用户可修该信息后，保存修该后的信息，然后重新绘制曲线，即可看到曲线是否被优化。

其中，用户通过鼠标点击选中路点：使用射线拾取法实现鼠标选中路点，通过将鼠标在视口上点击的坐标换算成三维空间中的一条射线，计算路点到该射线的距离d，当d小于阈值t时表示选中该路点。

以上描述了各个曲线的颜色，为了使路点更加直观化，更容易被区分，可将路点绘制为一定形状，下面进行说明。

在本发明的一个实施例中，基于轨迹点的位置、绘制颜色绘制机器人的轨迹点，以得到机器人的轨迹曲线，还可包括：将三维轨迹曲线中路点的形状绘制为预设立体形状；将二维轨迹曲线、轨迹点个数-末端线速度曲线中路点的形状绘制为预设平面形状。

其中，预设立体形状的目的是为了使用户更好地查看路点，因此其可以是方形、球形等定位容易的形状。

进一步地，预设立体形状可为球体，将三维轨迹曲线中路点的形状绘制为预设立体形状，可包括：计算三维轨迹曲线中路点在模型视图矩阵下的位置向量；基于位置向量计算球体半径；根据球体半径将三维轨迹曲线中路点的形状绘制为球体。

具体而言，为了将三维轨迹中的路点绘制为小球，需要满足随着三维视角变化路点显示的大小不变(透视投影下遵循近大远小规律)，因此需要根据路点到摄像机(camera)的距离计算球型半径，首先通过以下公式计算每个路点在模型视图矩阵(ModelView)M_mv下的位置向量p：

p＝M_mv(Pos×1000)

其中，p表示路点在模型视图矩阵下的位置向量，M_mv表示模型视图矩阵，Pos表示路点在机器人基坐标系下的位姿，Pos×1000表示路点在三维空间中的位置，该公式没有做向量的齐次坐标处理。

再通过以下公式计算球体半径：

r＝|p|×θ

其中，r表示球体半径，|p|为向量p的长度，为路点到摄像机的距离，θ为球形大小系数，是一个自定义值用于控制球型大小，比如可以为0.003。

最后利用四边形逼近法完成球体的渲染。

适应性地，将二维轨迹曲线、轨迹点个数-末端线速度曲线中路点的形状绘制为圆形。

由此，实现了对轨迹曲线的绘制，用户既可以直观看到轨迹及其轨迹点，又可以看到不合理路点信息，通过修该路点信息实现对曲线的优化。

在本发明的一个实施例中，机器人的轨迹评估优化方法还可包括：在路点不合理时计算路点的不合理分数，并将不合理分数进行显示。

具体而言，被判定为不合理路点后，根据其名称可从步骤S1中解析的路点信息中找到对应的路点信息，运动属性、位姿信息、关节角信息等，可对其进行修改，还可基于该路点的末端线速度计算不合理分数，计算公式为：

score＝(1-|v_不/S-1|)×100

最后将路点信息和不合理分数均显示在不合理路点列表中，以供用户直观查看、评估以及优化。

基于以上方案，本发明实施例提出一款轨迹评估优化软件来辅助轨迹调试。该软件通过加载和评估轨迹工程文件比如涂胶工程，显示三维轨迹曲线、二维轨迹曲线和末端线速度曲线及曲线上的路点，曲线中分段标记满足需求(绿色)、可以接受(黄色)和不能接受(红色)的轨迹点或轨迹段，用户通过点击路点查看和修改路点信息，再次评估，反复如此循环直到轨迹满足需求。

下面介绍轨迹评估优化软件设计构思。

如图2所示，主界面分为曲线显示区域和参数设置区域。

1、参照图2，曲线显示区域用于显示三维轨迹曲线Trajectory_3D、轨迹在xOy平面、xOz平面、yOz平面的投影即二维轨迹曲线Trajectory_xOy、Trajectory_xOz、Trajectory_yOz以及末端线速度曲线realTimeTcpSpeed。其中曲线上的蓝点表示轨迹路点，可对应AuboPE在线编程的WayPoint(路点)。

2、参照图2，参数设置区域包括曲线选择区域，末端线速度标准值、误差值设置区域以及不合理路点列表，还包括重评估、保存、跳转到AuboPE的按钮。

曲线选择区域：通过点击该区域选择查看具体曲线，图2中显示的是满足需求的三维轨迹曲线Trajectory_3D。

末端线速度标准值、误差值设置区域：设置末端线速度的标准值以及两个误差区间，设置成功后将轨迹点末端线速度位于误差区间1内的曲线标识为绿色，末端速度位于误差区间2内的曲线标识为黄色，其余的曲线标识为红色，程序启动后需要先设置才能起作用。图2中，末端线速度标准值为120mm/s、误差值1为1mm/s，误差值2为2mm/s。

不合理路点列表：显示超出误差范围的路点名，格式为：move名/waypoint名。如图3所示，每个路点后都有一个分数表示对路点末端线速度距离标准值的远近程度，分数最大为100，越大表示越接近标准值，越小表示离标准值越远。参照图2，在所有路点均满足需求时，该列表为空。

可单击选中列表某个路点，路点信息将会显示在如图4所示的路点参数设置界面(显示各关节位姿、末端线速度、线加速度、角速度、角加速度等)，同时轨迹显示区域标记该路点为被选中状态。

重评估：修改选中轨迹点参数信息后，用于重新生成新的轨迹。

保存：保存工程Xml和脚本文件。

下面通过一个具体示例，说明通过轨迹评估优化软件进行轨迹优化评估的流程。

1、选择并加载一个AuboPE在线编程编辑好的涂胶工程，解析其中的轨迹运动属性和路点信息。

2、根据1中解析的结果调用底层轨迹规划算法完成轨迹规划，得到轨迹点，并标记路点以及路点名称。

3、计算每个轨迹点(包含了路点)的末端线速度v_i，计算方式为：

4、绘制三维轨迹曲线图。

该步骤需要实现绘制三维曲线，绘制三维路点等功能，绘制效果图5所示。

(1)绘制曲线，根据3中计算得到的轨迹点末端速度计算轨迹点绘制的颜色。首先根据界面设计中配置的末端线速度标准值S和两个误差值I₁、I₂计算不同颜色的边界值(从小到大依次为T₂、T₁、L₁、L₂)，如下：

L₂＝S+I₂

L₁＝S+I₁

T₁＝S-I₁

T₂＝S-I₂

再根据3中计算得到的v_i计算当前轨迹点显示的曲线颜色，公式如下：

最后使用OpenGL的glColor4d接口设置颜色，glDrawArrays绘制轨迹段或轨迹点。

(2)绘制路点，将2中标记为路点的轨迹点绘制为蓝色小球，需要满足随着三维视角变化路点显示的大小不变(透视投影下遵循近大远小规律)，因此需要根据路点到摄像机(camera)的距离计算球型半径，首先计算路点在ModelView矩阵M_mv下的位置向量p，公式如下：

p＝M_mv(Pos×1000)

其中，Pos表示路点在机器人基坐标系下的位姿，Pos×1000表示路点在三维空间中的位置，该公式没有做向量的齐次坐标处理。

再计算球型半径r，公式如下：

r＝|p|×0.003

其中，向量p的长度就是路点到camera的距离，0.003是一个自定义值用于控制球型大小。

最后利用四边形逼近法完成球体的渲染。

5、绘制轨迹在基坐标系xOy、xOz、yOz平面的二维轨迹曲线。

将轨迹点的xy、xz、yz坐标信息分别绘制到xOy平面坐标系、xOz平面坐标系、yOz平面坐标系中，绘制前同样需要计算每段轨迹绘制的颜色，计算方法同步骤4中的一样。再将步骤2中标记为路点的轨迹点绘制为蓝色圆形。绘制结果比如二维xOz曲线如图6所示。

6、绘制末端线速度曲线。

以轨迹点个数为横坐标，以步骤3中计算的轨迹点末端速度为纵坐标，将轨迹点的末端线速度曲线绘制到平面坐标系中，绘制前同样需要计算每段轨迹绘制的颜色，计算方法同步骤4中的计算方法一样绘制。再将步骤2中标记为路点的轨迹点绘制为蓝色圆形，结果如图7所示，图7中的曲线包含多个不合理路点。

7、计算不合理路点和分数，显示不合理路点。

对步骤2中标记为路点的轨迹点进行不合理路点的判定，判定方式为(v>L₂)∪(v<T₂)，被判定为不合理路点后根据路点名称从步骤1中解析的信息中找到move名、waypoint名、位姿信息、关节角信息或运动属性等。计算路点不合理分数，计算公式如下：

score＝(1-|v_不/S-1|)×100

最后将路点信息和分数显示在不合理路点列表中，参照图3、图5、图6和图7。

8、查看和调整路点信息。

使用射线拾取法通过鼠标选中曲线图中或者不合理路点列表中的路点(通过将鼠标在视口上点击的坐标换算成三维空间中的一条射线，计算路点到该射线的距离d，当d小于阈值t时表示选中该路点)，弹出图4所示的路点信息界面，在该界面修改路点位姿或者运动属性后点击“确认”保存修改信息。

9、重评估。

参照图5、6和7，路点参数修改完成后，点击参数配置界面上的“重评估”按钮，开始执行步骤2～7重新生成并显示所有的曲线，查看轨迹是否被优化，反复调整和重评估直到轨迹符合需求。

10、保存。

参照图2，轨迹符合需求后，点击“保存”按钮将修改后的路点信息写入步骤1中加载的工程文件，在真机上使用AuboPE加载该工程并验证轨迹的运行效果。

总的来说，软件通过加载已编辑好的轨迹工程，调用算法接口完成轨迹规划，将轨迹曲线和末端线速度曲线绘制在软件界面，在曲线上使用蓝色小球标识每个路点的位置。设置轨迹末端线速度标准值和两段误差值，分段标记轨迹曲线和末端线速度曲线，对于末端线速度在第一段误差内的曲线使用绿色显示，表示满足需求；在第一段和第二段误差之间的曲线使用黄色显示，表示可以接受，在第二段误差以外的曲线使用红色显示，表示不能接受。用户通过点击曲线上的路点查看和修改路点的运动属性和位姿信息等，修改好后再次评估，界面上将会生成新的曲线图。反复评估和调整直到轨迹满足需求。最后将修改好的工程文件保存并放到机器人中运行查看调试结果。

综上所述，本发明实施例通过轨迹评估优化软件工具可以直观看到轨迹的末端线速度，不需要再通过外部工具辅助查看，可以节省很多人力物力；在该软件工具上可以很快定位到不合理路点，并对路点进行调整，提高调试效率，缩短调试周期。

在本发明的描述中，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。“多个”的含义是两个或两个以上，除非另有明确具体的限定。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必针对相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本发明的实施例所属技术领域的技术人员所理解。

应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。

本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。此外，在本发明各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims

1.一种机器人轨迹的评估优化方法，其特征在于，包括以下步骤：

获取表征机器人所有轨迹的路点信息，并基于所述路点信息计算所述轨迹上所有轨迹点的末端线速度，其中，所述轨迹点包括路点；

基于所述路点的末端线速度判断该路点是否合理，并在该路点不合理时修改路点信息，返回第一步，直至所有路点合理，

基于所述路点信息计算所述轨迹上所有轨迹点的末端线速度，包括：

基于所述路点信息得到所述轨迹上每个轨迹点在机器人基坐标系下的位置；

根据所述轨迹点的位置计算每个轨迹点的末端线速度，

在基于所述路点信息计算所述轨迹上所有轨迹点的末端线速度之后，还包括：

基于所有所述轨迹点的位置、末端线速度绘制所述机器人的轨迹曲线，并显示所述轨迹曲线，

基于所有所述轨迹点的位置、末端线速度绘制所述机器人的轨迹曲线，包括：

基于每个所述轨迹点的末端线速度确定该轨迹点的绘制颜色；

基于所有所述轨迹点的位置、末端线速度和绘制颜色绘制所述机器人的所有轨迹点，以得到所述机器人的轨迹曲线，

基于每个所述轨迹点的末端线速度确定该轨迹点的绘制颜色，包括：

获取机器人轨迹点的末端线速度标准值和末端线速度误差值；

根据所述末端线速度标准值和所述末端线速度误差值计算末端线速度边界值，并根据所述末端线速度边界值得到末端线速度合理范围；

确定每个所述末端线速度合理范围对应的绘制颜色、所述末端线速度不合理范围对应的绘制颜色；

根据每个轨迹点的末端线速度、所述末端线速度合理范围及其对应的绘制颜色，确定该轨迹点的绘制颜色；

确定路点的额外绘制颜色，以使每个路点的绘制颜色由两种不同的颜色混合而成。

2.根据权利要求1所述的机器人轨迹的评估优化方法，其特征在于，通过以下公式计算所述轨迹点的末端线速度：

,

其中，表示第i个轨迹点在机器人基坐标系下的位置，表示第i-1个轨迹点在机器人基坐标系下的位置，i为大于0的整数，表示轨迹点更新周期。

3.根据权利要求1或2所述的机器人轨迹的评估优化方法，其特征在于，基于所述路点的末端线速度判断该路点是否合理，包括：

如果所述路点的末端线速度不处于所述末端线速度合理范围，则确定路点不合理；

如果所述路点的末端线速度处于所述末端线速度合理范围，则确定路点合理。

4.根据权利要求3所述的机器人轨迹的评估优化方法，其特征在于，所述末端线速度标准值的数量为一个，所述末端线速度误差值的数量为两个，所述末端线速度合理范围包括各不相同的第一合理范围、第二合理范围和第三合理范围，

通过以下公式计算末端线速度边界值：

,

其中，S为末端线速度标准值，I ₁、I ₂分别为末端线速度误差值，I ₁小于I ₂，I ₁、I ₂均大于0，L ₁、L ₂、T ₂和T ₂分别为末端线速度边界值；

所述第一合理范围为（T ₁，L ₁）、第二合理范围为（T ₂，T ₁]以及第三合理范围为[L ₁，L ₂）。

5.根据权利要求1所述的机器人轨迹的评估优化方法，其特征在于，通过以下公式确定轨迹点的绘制颜色：

,

其中，color为轨迹点的绘制颜色，color1、color2和color3分别为第一颜色、第二颜色和第三颜色，color1、color2和color3各不相同，v _i为第i个轨迹点的末端线速度，L ₁、L ₂、T ₂和T ₂分别为末端线速度边界值。

6.根据权利要求1所述的机器人轨迹的评估优化方法，其特征在于，所述机器人的轨迹曲线包括机器人轨迹在机器人基坐标系的三维轨迹曲线、在机器人基坐标系xOy平面、xOz平面和yOz平面的二维轨迹曲线、以及机器人的轨迹点个数-末端线速度曲线，

基于所述轨迹点的位置、末端线速度和绘制颜色绘制所述机器人的轨迹点，以得到所述机器人的轨迹曲线，包括：

在机器人基坐标系下根据所述轨迹点的位置、绘制颜色绘制所述机器人的轨迹点，以得到三维轨迹曲线；

分别在机器人基坐标系xOy平面、xOz平面和yOz平面根据所述轨迹点的位置、绘制颜色绘制所述机器人的轨迹点，以得到二维轨迹曲线；

建立以轨迹点个数为横轴、以轨迹点的末端线速度为纵轴的坐标系，并在该坐标系下根据所述机器人的轨迹点的末端线速度、绘制颜色绘制所述轨迹点，以得到轨迹点个数-末端线速度曲线。

7.根据权利要求6所述的机器人轨迹的评估优化方法，其特征在于，基于所述轨迹点的位置、末端线速度和绘制颜色绘制所述机器人的轨迹点，以得到所述机器人的轨迹曲线，还包括：

将所述三维轨迹曲线中路点的形状绘制为预设立体形状；

将所述二维轨迹曲线、所述轨迹点个数-末端线速度曲线中路点的形状绘制为预设平面形状。

8.根据权利要求7所述的机器人轨迹的评估优化方法，其特征在于，所述预设立体形状为球体，将所述三维轨迹曲线中路点的形状绘制为预设立体形状，包括：

计算所述三维轨迹曲线中路点在模型视图矩阵下的位置向量；

基于所述位置向量计算球体半径；

根据所述球体半径将所述三维轨迹曲线中路点的形状绘制为球体。

9.根据权利要求8所述的机器人轨迹的评估优化方法，其特征在于，通过以下公式计算路点在模型视图矩阵下的位置向量：

,

其中，表示路点在模型视图矩阵下的位置向量，表示模型视图矩阵，表示路点在机器人基坐标系下的位姿，表示路点在三维空间中的位置；

通过以下公式计算球体半径：

,

其中，表示球体半径，为的长度，为路点到摄像机的距离，为球形大小系数。

10.根据权利要求1所述的机器人轨迹的评估优化方法，其特征在于，还包括：

在路点不合理时计算所述路点的不合理分数，并将所述不合理分数进行显示。

11.根据权利要求10所述的机器人轨迹的评估优化方法，其特征在于，通过以下公式计算所述不合理分数：

，

其中，score为不合理分数，S为末端线速度标准值，v _不为不合理路点的末端线速度。