如何获取及分析发那科机器人轨迹运行时的实际速度

先贴结论

前提	获取及分析方法
有Roboguide仿真软件	1、Roboguide内置速度记录功能，可以在运行轨迹上显示实际速度值；2、Roboguide Motion Pro选项功能
无仿真软件	1、通过读取系统变量的方法来获取机器人实际运行速度；可通过karel程序的方式将速度值按某个时间间隔写入R寄存器中，进而通过寄存器的VA文件导入excel中形成速度曲线；2、发那科机器人有Speed out功能，可以输出速度值至AO、GO、R寄存器等

一、实际速度与指令速度存在差异吗？

答案是肯定的，各家的工业机器人在示教程序点位时，均存在“转角半径”“逼近距离”或者与之类似的概念，用于描述经过某个点的时候的轨迹圆滑过渡程度。可以以人的运动来类比理解这一点，跑步的时候遇到小的拐弯，你会自动减速；如果需要拐一个直角，或者折返跑，那在直角的位置或者折返的位置，速度会变得很慢。

工业机器人也是这样的，这就导致机器人的实际运行速度一般会低于指令速度，比如经过统计，在某些车身涂胶应用中，轨迹实际运行的平均速度只能达到指令速度的60%-70%。这对某些使用者会造成困扰，比如明明增加了程序运动指令的速度值，但实际运行的时候机器人节拍并没有改善。

当然也想过是不是厂家在设计时可以通过算法的优化，让实际运行速度接近轨迹速度，让实际运行点位尽量接近示教点位，但如下图所示，这样的设计逻辑又会导致新的问题，就是中间的轨迹有可能不可预见，甚至与环境的实体发生干涉。

二、有必要分析实际运行速度吗

我觉得是有必要，也没有必要。没有必要是指，不深入研究分析该问题，也能通过参数的调整，经验的积累来调试出满足要求的工艺效果；有必要是指，如果想要深入地分析机器人实际运行速度，是有必要定量地来研究。本文解决的就是如何来获取及分析发那科机器人的实际运行速度。
关于工艺的具体应用，举例如下，在某些涂胶场合中，需要保证转角的位置胶条膜厚、宽度等无明显差异，这就需要对机器人速度的平滑性提出更高的要求了。

三、使用Roboguide分析机器人实际运行速度

如果安装有仿真软件，可以通过将现场机器人导入仿真单元的方式，在仿真中对机器人实际运行轨迹进行模拟及分析，进而针对性优化。需要注意的是，仿真中机器人速度值与实际机器人速度值可能存在差异，但基本能作为参考。

1、分析方法1——Roboguide内置速度记录功能

Roboguide软件内置了速度记录功能，使用【Run Panel】执行机器人程序，勾选上“Collect TCP Trace”选项，生成轨迹后，在【TCP Traces】界面，选择“Color by Speed”即可在轨迹上看到各处机器人速度。从下图可以明显看到各个拐弯处机器人速度明显降低。

2、分析方法2——Motion Pro选项功能

Roboguide软件安装时可以选择安装Motion Pro插件（应该需要单独授权），根据EDOC介绍，该插件能够优化节拍、优化轨迹、优化减速机寿命和减少电能消耗，由于未使用过该插件，不做具体介绍。

四、不使用仿真软件前提下分析机器人速度

1、分析方法3——机器人速度输出功能

该功能为选装项，需要向发那科公司采购；由于未使用过该插件，不做具体介绍。

2、分析方法4——Karel程序法

1、相关系统变量

系统变量如下，设置SCR_GRP[1].M_POS_ENB和M_DST_ENB为TRUE，则在自动运行程序时，下方的MCH_POS变量会实时更新TCP位置；下方的MCH_SPD变量则会实时更新TCP速度。注意一定是自动运行的时候才会更新，示教模式下手动运行程序并不会更新。
需要注意的是，如果只是实时查看变量的话，有可能由于机器人速度太快而看不清楚具体运动到某处位置的时候的速度值。这种情况可以将机器人override调低至10%-20%，然后查看变量的值，将此时运行的时候的速度值乘以相应的比例（比如是10%则x10倍），就可以得到机器人在Override=100%速度下运行时的实际速度值。

2、Karel程序输出速度值至寄存器

编写Karel程序，可以将机器人速度输出至寄存器，进而可以通过NUMREG.VA文件将速度列表复制到excel表格中，生成速度曲线图，该方法可用于实际机器人。

3、分析方法5——状态-生产界面实时查看

某些应用场合，机器人菜单栏会有一个状态-生产页面，某些条件下，在该页面可以查看机器人TCP的实时速度。

五、应用案例举例

1、T公司内板密封胶项目

使用Roboguide内置速度记录功能，分析不同指令示教S形轨迹的运行速度。发现L指令调试难度太大，示教点需要远离轨迹；C指令能较好拟合形状，但拐弯处速度降低明显；A指令基本能满足要求，需要精细调整每个点的坐标。最终使用A指令调试优化，满足客户工艺要求。

2、T公司裙边胶项目

使用Karel程序方法分析CNT值对机器人实际运行速度的影响，确认示教裙边侧面轨迹时，需要使用CNT95—100（最好100），以满足2000mm/s的涂胶速度要求。

3、CNT对机器人加速度影响分析

使用Karel程序方法，测试轨迹完全一致时，不同CNT值对速度的影响。从图中可以看出，在使用不同的CNT值时，机器人加速度（曲线斜率）完全一致，由此可以对机器人运动控制有更好的理解。
PARAM_GRP[1].ACCEL_TIME1[9]和PARAM_GRP[1].ACCEL_TIME2[9]有对机器人各轴加速时间的定义。

六、输出速度值的karel程序分享

程序说明及代码如下，具体的karel程序如何编写及使用，待后续介绍。

1、程序说明

1）该程序用于自动运行时，将机器人运行速度存储至寄存器列表。

2）使用该程序前，需要设置R[31]—R[35]寄存器。
R[31]：速度界限，单位mm/s。低于该速度时按照R[32]间隔采样速度值；高于该速度时按照R[33]间隔采样速度值。
R[32]：采样间隔1，单位ms。
R[33]：采样间隔2，单位ms。
R[34]：寄存器序号，即从哪个寄存器开始存储速度列表。
R[35]：触发信号，即使用哪个DO信号=ON作为开始存储速度列表的触发；默认设置为0时，使用UO[3]Program Run信号作为触发。如果只需要统计某一段轨迹的速度，可以设置触发信号。

2）Load该程序后，将系统变量$KAREL_ENB设置为1，就可以在Karel程序列表找到SPEED_OUTPUT程序；使用时只需要在程序开始行，添加 RUN SPEED_OUTPUT指令即可。

2、程序代码

PROGRAM speed_output
%NOLOCKGROUP
%COMMENT= 'Speed output to register'
%ALPHABETIZE

VAR
cur_speed:REAL
speed:INTEGER
delay_time1:INTEGER
delay_time2:INTEGER
delay_time:INTEGER
t_clock:INTEGER
cur_time:INTEGER
temp:INTEGER
reg_num:INTEGER
trigger_do:INTEGER
cur_time_str:STRING[8]
r_val:REAL
r_flg:BOOLEAN
signal_flag:BOOLEAN

BEGIN
	r_flg=FALSE
	signal_flag=FALSE
	GET_REG(31,r_flg,speed,r_val,temp)
	GET_REG(32,r_flg,delay_time1,r_val,temp)
	GET_REG(33,r_flg,delay_time2,r_val,temp)
	GET_REG(34,r_flg,reg_num,r_val,temp)
	GET_REG(35,r_flg,trigger_do,r_val,temp)
IF trigger_do>0 THEN
		signal_flag=TRUE
	ENDIF
	
	t_clock=0
	CONNECT TIMER TO t_clock
	Wait FOR UOUT[3]=ON
		
	WHILE UOUT[3]=ON  DO
		IF signal_flag=TRUE THEN
			WAIT FOR DOUT[trigger_do]=ON
		ENDIF		
		cur_speed=$SCR_GRP[1].$MCH_SPD
		IF cur_speed<speed  THEN
			delay_time=delay_time1
		ELSE
			delay_time=delay_time2
		ENDIF
		cur_time=t_clock
		CNV_INT_STR(cur_time,1,0,cur_time_str)
		SET_REG_CMT(reg_num,cur_time_str,temp)
		SET_REAL_REG(reg_num,cur_speed,temp)
		reg_num=reg_num+1
		DELAY delay_time
	ENDWHILE