本发明涉及一种用于机器人的传送带同步跟踪方法、装置和系统。所述方法包括：获取传送带上的目标物体的初始位置信息，实时估计目标物体的位置和速度。基于机器人的起始位置、预设开始跟踪界限和所估计的目标物体的位置和速度，规划机器人的运动轨迹，使得机器人在目标物体到达第一位置时从起始位置开始运动将与目标物体同时到达开始跟踪位置并且在开始跟踪位置处具有与目标物体相同的运动速度。响应于目标物体到达第一位置，控制机器人以所规划的运动轨迹运动；以及基于机器人和目标物体的实时位置，控制机器人从开始跟踪位置同步跟踪目标物体。本公开的实施例能够实现快速目标物体的定位和跟踪，减小速度的波动，提高工业系统的精度和工作效率。
1.一种用于机器人的传送带同步跟踪的方法，其特征在于，包括：获取传送带上的目标物体的初始位置信息；实时估计所述目标物体的位置和速度；至少部分地基于机器人的起始位置、预设开始跟踪界限和所估计的所述目标物体的位置和速度，规划所述机器人的运动轨迹，所述规划包括：确定所述目标物体的第一位置，其中所述机器人在所述目标物体到达所述第一位置时从所述起始位置开始运动将与所述目标物体同时到达所述预设开始跟踪界限；以及基于所述第一位置、所述目标物体的位置和速度，规划所述机器人的运动轨迹，以使得所述机器人与所述目标物体同时到达与所述预设开始跟踪界限相对应的开始跟踪位置并且在所述开始跟踪位置处具有与所述目标物体相同的运动速度；
响应于所述目标物体到达所述第一位置，控制所述机器人从所述起始位置朝向所述开始跟踪位置以所规划的运动轨迹运动；以及基于所述机器人和所述目标物体的实时位置，控制所述机器人从所述开始跟踪位置同步跟踪所述目标物体，其中，规划所述机器人的运动轨迹包括：
基于所述预设开始跟踪界限xRs、所述机器人的所述起始位置以及预设的所述机器人的运动速度和加速度，计算所述机器人到达所述预设开始跟踪界限的第一时间值t1；
根据下式计算所述目标物体的所述第一位置的X轴坐标值xRt0，所述第一位置为所述目标物体从其开始经过所述第一时间值到达所述预设开始跟踪界限的位置:xRt0＝xRs-Vct*t1*cos(θ)，
其中Vct为所述传送带的速度，θ为机器人空间坐标与传送带空间的坐标之间的夹角；
根据下式得到第一开始跟踪位置PRs的Y轴坐标值yRs，
其中yRt0为所述目标物体的所述第一位置的Y轴坐标值；以及基于所述机器人的所述起始位置、在所述起始位置处的速度和加速度以及所述第一开始跟踪位置、在所述第一开始跟踪位置处的速度和加速度，规划所述机器人的第一平滑运动轨迹。
2.根据权利要求1所述的方法，其特征在于，其中获取所述目标物体的所述初始位置信息包括：响应于来自视觉系统的指示，保存所述传送带的编码器所反馈的第一编码器值，所述第一编码器值与所述初始位置信息相关联；以及从所述视觉系统获取所述目标物体的初始位置坐标，所述初始位置坐标包括所述目标物体的初始位置在视觉系统空间或者机器人空间之中任一空间的坐标。
3.根据权利要求1所述的方法，其特征在于，其中实时估计所述目标物体的位置包括：实时读取所述传送带的编码器所反馈的编码器值Ct；以及
根据下式计算所述目标物体在所述机器人空间中的实时位置PRt：其中，xRt和yRt分别为PRt的X轴坐标值和Y轴坐标值，xR0和yR0分别为所述目标物体的初始位置的X轴坐标值和Y轴坐标值，C0为所述目标物体的初始位置相应的编码器值，s为单位编码器值传送带前进的距离，θ为机器人空间坐标与传送带空间坐标之间的夹角。
4.根据权利要求1所述的方法，其特征在于，其中控制所述机器人从所述起始位置朝向所述第一开始跟踪位置运动包括：响应于所述目标物体到达所述第一位置，控制所述机器人以所述第一平滑运动轨迹开始运动至所述第一开始跟踪位置。
5.根据权利要求3所述的方法，其特征在于，其中规划所述机器人的运动轨迹还包括：选取第二时间值tm，所述第二时间值tm小于所述第一时间值t1；
计算在所述第二时间值tm处所述目标物体的第二位置PRtm；
根据下式计算第二开始跟踪位置P′Rs，以替代所述第一开始跟踪位置作为所述开始跟踪位置:其中x′Rs和y′Rs分别为P′Rs的X轴坐标值和Y轴坐标值,xRtm和yRtm分别为PRtm的X轴坐标值和Y轴坐标值；以及基于所述机器人在所述第二时间值处的位置、速度和加速度以及所述第二开始跟踪位置、在所述第二开始跟踪位置的速度和加速度，规划所述机器人的第二平滑运动轨迹。
6.根据权利要求5所述的方法，其特征在于，其中控制所述机器人从所述起始位置朝向所述第一开始跟踪位置运动包括：响应于所述目标物体到达所述第一位置，控制所述机器人以所述第一平滑运动轨迹开始运动；以及响应于所述目标物体到达所述第二位置，控制所述机器人以所述第二平滑运动轨迹开始运动至所述第二开始跟踪位置。
7.根据权利要求5所述的方法，其特征在于，其中规划所述机器人的第二平滑运动轨迹包括：利用多项式插值算法对所述机器人的运动轨迹进行规划。
8.根据权利要求1所述的方法，其特征在于，其中控制所述机器人同步跟踪所述目标物体包括：基于所述目标物体的实时位置和所述机器人的实时位置之间的偏差，利用比例-积分-微分算法计算所述机器人的位置调整量；以及基于所述位置调整量，控制所述机器人同步跟踪所述目标物体。
9.根据权利要求1所述的方法，其特征在于，其中规划所述机器人的第一平滑运动轨迹包括：利用多项式插值算法对所述机器人的运动轨迹进行规划。
10.一种用于机器人的传送带同步跟踪的装置，其特征在于，包括：获取单元，用于获取传送带上的目标物体的初始位置信息；
运动轨迹规划单元，用于基于机器人的起始位置、预设开始跟踪界限和所估计的所述目标物体的位置和速度，规划所述机器人的运动轨迹，所述规划包括：确定所述目标物体的第一位置，其中所述机器人在所述目标物体到达所述第一位置时从所述起始位置开始运动将与所述目标物体同时到达所述预设开始跟踪界限；以及基于所述第一位置、所述目标物体的位置和速度，规划所述机器人的运动轨迹，以使得所述机器人与所述目标物体同时到达与所述预设开始跟踪界限相对应的开始跟踪位置并且在所述开始跟踪位置处具有与所述目标物体相同的运动速度；
第一控制单元，用于响应于所述目标物体到达所述第一位置，控制所述机器人从所述起始位置朝向所述开始跟踪位置以所规划的运动轨迹运动；以及第二控制单元，用于基于所述机器人和所述目标物体的实时位置，控制所述机器人从所述开始跟踪位置同步跟踪所述目标物体，所述运动轨迹规划单元包括：
第二计算单元，用于基于所述预设开始跟踪界限xRs、所述机器人的所述起始位置以及预设的所述机器人的运动速度和加速度，计算所述机器人到达所述预设开始跟踪界限的第一时间值t1；第三计算单元，用于根据下式计算所述目标物体的所述第一位置的X轴坐标值xRt0，所述第一位置为所述目标物体从其开始经过所述第一时间值到达所述预设开始跟踪界限的位置:xRt0＝xRs-Vct*t1*cos(θ)，
其中Vct为所述传送带的速度，θ为机器人空间坐标与传送带空间的坐标之间的夹角；
位置获得单元，用于根据下式得到第一开始跟踪位置PRs的Y轴坐标值yRs，yRs＝yRt0+Vct*t1*sin(θ)，其中yRt0为所述目标物体的所述第一位置的Y轴坐标值；以及第一轨迹规划单元，用于基于所述机器人的所述起始位置、在所述起始位置处的速度和加速度以及所述第一开始跟踪位置、在所述第一开始跟踪位置处的速度和加速度，规划所述机器人的第一平滑运动轨迹。
11.根据权利要求10所述的装置，其特征在于，所述获取单元包括：保存单元，用于响应于来自视觉系统的指示，保存所述传送带的编码器所反馈的第一编码器值，所述第一编码器值与所述初始位置信息相关联；以及坐标获取单元，用于从所述视觉系统获取所述目标物体的初始位置坐标，所述初始位置坐标包括所述目标物体的初始位置在视觉系统空间或者所述机器人空间之中任一空间的坐标。
12.根据权利要求10所述的装置，其特征在于，所述位置估计单元包括：读取单元，用于实时读取所述传送带的编码器所反馈的编码器值Ct；以及第一计算单元，用于根据下式计算所述目标物体在所述机器人空间中的实时位置PRt：其中，xRt和yRt分别为PRt的X轴坐标值和Y轴坐标值，xR0和yR0分别为所述目标物体的初始位置的X轴坐标值和Y轴坐标值，C0为所述目标物体的初始位置相应的编码器值，s为单位编码器值传送带前进的距离，θ为机器人空间坐标与传送带空间的坐标之间的夹角。
13.根据权利要求10所述的装置，其特征在于，所述第一控制单元包括：第一运动轨迹控制单元，用于响应于所述目标物体到达所述第一位置而控制所述机器人以所述第一平滑运动轨迹开始运动至所述第一开始跟踪位置。
14.根据权利要求12所述的装置，其特征在于，所述运动轨迹规划单元还包括：时间选取单元，用于选取第二时间值tm，所述第二时间值tm小于所述第一时间值t1；
第四计算单元，用于计算在所述第二时间值tm处所述目标物体的第二位置PRtm；
第五计算单元，用于根据下式计算第二开始跟踪位置P′Rs，以替代所述第一开始跟踪位置作为所述开始跟踪位置:其中x′Rs和y′Rs分别为P′Rs的X轴坐标值和Y轴坐标值,xRtm和yRtm分别为PRtm的X轴坐标值和Y轴坐标值；以及第二轨迹规划单元，用于基于所述机器人在所述第二时间值处的位置、速度和加速度以及所述第二开始跟踪位置、在所述第二开始跟踪位置的速度和加速度，规划所述机器人的第二平滑运动轨迹。
15.根据权利要求14所述的装置，其特征在于，所述第一控制单元包括：第二运动轨迹控制单元，用于响应于所述目标物体到达所述第一位置，控制所述机器人以所述第一平滑运动轨迹开始运动；以及第三运动轨迹控制单元，用于响应于所述目标物体到达所述第二位置，控制所述机器人以所述第二平滑运动轨迹开始运动至所述第二开始跟踪位置。
16.根据权利要求14所述的装置，所述第二轨迹规划单元用于利用多项式插值算法对所述机器人的运动轨迹进行规划。
17.根据权利要求10所述的装置，其特征在于，所述第二控制单元包括：位置偏差计算单元，用于基于所述目标物体的实时位置和所述机器人的实时位置之间的偏差，利用比例-积分-微分算法计算所述机器人的位置调整量；以及跟踪控制单元，用于基于所述位置调整量，控制所述机器人同步跟踪所述目标物体。
18.根据权利要求10所述的装置，所述第一轨迹规划单元用于利用多项式插值算法对所述机器人的运动轨迹进行规划。
19.一种机器人系统，其特征在于，包括机器人和根据权利要求10-18中任一项所述的装置。
存储器，其存储有所述处理器可读的指令，所述指令在由所述处理器执行时使得所述处理器执行根据权利要求1-9中任一项所述的方法。
21.一种计算机可读存储介质，其特征在于，其存储有机器可读的指令，所述指令在由所述机器执行时使得所述机器执行根据权利要求1-9中任一项所述的方法。
一种用于机器人的传送带同步跟踪方法、装置及系统
技术领域
[0001] 本公开属于机器人控制技术领域，具体涉及一种用于机器人的传送带同步跟踪方法、装置及系统。
背景技术
[0002] 为了提高工作效率，降低生产成本，保障产品质量，机器视觉和工业机器人的结合已逐步应用在流水线上的分拣、包装和在线检测等工作。在实际应用中，传送带是移动的，工业机器人需要根据视觉系统和传感器获得的位置，对目标物体进行位置的实时同步和跟踪，最终完成分拣、包装和在线检测等工作。
[0003] 传送带动态抓取的一般方法是利用机器视觉技术获取传送带上目标物体的位置信息，并通过安装在传送带上的编码器获取传送带的位置和速度，计算目标物体任意时刻的位置；一旦目标物体进入机器人的工作空间，就会让机器人去动态地抓取传送带上的物体。传统的传送带动态抓取大多采取拦截式的抓取方式，即通过传送带的速度预测目标物体在将来某一时刻的位置，然后控制机器人运动，和目标物体同时到达该位置，完成拦截式的抓取。另外传统的方法假设传送带的运动是匀速的，但是传送带的速度存在波动，不完全是匀速的，所以会造成在直接抓取时存在一定的误差，且误差会随着目标物体运动距离的增加、运动速度的提高而变大。
发明内容
[0004] 针对上述问题，本公开的实施例提供一种用于机器人的传送带同步跟踪的方法、装置及系统，能够快速的实现目标物体的定位和跟踪，减小速度的波动，提高工业系统的精度和工作效率。
[0005] 在本公开的第一方面，提供一种用于机器人的传送带同步跟踪的方法。该方法包括：：获取传送带上的目标物体的初始位置信息；实时估计目标物体的位置；基于机器人的起始位置、预设开始跟踪界限和所估计的目标物体的位置和速度，规划机器人的运动轨迹，规划包括：确定目标物体的第一位置，其中机器人在目标物体到达第一位置时从起始位置开始运动将与目标物体同时到达预设开始跟踪界限；以及基于第一位置、目标物体的位置和速度，规划机器人的运动轨迹，以使得机器人与目标物体同时到达与预设跟踪界限相对应的开始跟踪位置并且在开始跟踪位置处具有与目标物体相同的运动速度；响应于目标物体到达第一位置，控制机器人从起始位置朝向开始跟踪位置以所规划的运动轨迹运动；以及基于机器人和目标物体的实时位置，控制机器人从开始跟踪位置同步跟踪目标物体。
[0006] 在某些实施例中，获取目标物体的初始位置信息包括：响应于来自视觉系统的指示，保存传送带的编码器所反馈的第一编码器值，第一编码器值与初始位置信息相关联；以及从视觉系统获取目标物体的初始位置坐标，初始位置坐标包括目标物体的初始位置在视觉系统空间或者机器人空间之中任一空间的坐标。
[0007] 在某些实施例中，估计目标物体在机器人空间中的实时位置包括：实时读取传送带的编码器所反馈的编码器值Ct；以及根据下式计算所述目标物体在所述机器人空间中的实时位置PRt：
[0008]
[0009] 其中，xRt和yRt分别为PRt的X轴坐标值和Y轴坐标值，xR0和yR0分别为所述目标物体的初始位置的X轴坐标值和Y轴坐标值，C0为所述目标物体的初始位置相应的编码器值，s为单位编码器值传送带前进的距离，θ为机器人空间坐标与传送带空间的坐标之间的夹角。
[0010] 在某些实施例中，规划机器人的运动轨迹包括：基于所述预设开始跟踪界限xRs、所述机器人的所述起始位置PRw以及预设的所述机器人的运动速度和加速度，计算所述机器人到达所述预设开始跟踪界限的第一时间值t1；根据下式计算所述目标物体的所述第一位置的X轴坐标值xRt0，所述第一位置为所述目标物体从其开始经过所述第一时间值到达所述预设开始跟踪界限的位置:
[0011] xRt0＝xRs-Vct*t1*cos(θ)
[0012] 其中Vct为所述传送带的速度，θ为机器人空间坐标与传送带空间的坐标之间的夹角；根据下式得到第一开始跟踪位置PRs的Y轴坐标值yRs，
[0013] yRs＝yRtO+Vct*t1*sin(θ)
[0014] 其中yRt0为所述目标物体的所述第一位置的Y轴坐标值；以及基于机器人的起始位置、在起始位置处的速度和加速度以及第一开始跟踪位置、在第一开始跟踪位置处的速度和加速度，规划机器人的第一平滑运动轨迹。
[0015] 在某些实施例中，控制机器人从起始位置朝向第一开始跟踪位置运动包括：响应于目标物体到达第一位置，控制机器人以第一平滑运动轨迹开始运动至第一开始跟踪位置。
[0016] 在某些实施例中，规划机器人的运动轨迹还包括：选取第二时间值tm，所述第二时间值小于所述第一时间值t1；计算在所述第二时间值tm处所述目标物体的第二位置PRtm；根据下式计算第二开始跟踪位置P’Rs，以替代所述第一开始跟踪位置作为所述开始跟踪位置:
[0017]
[0018] 其中，x’Rs和y’Rs分别为P’Rs的X轴坐标值和Y轴坐标值,xRtm和yRtm分别为PRtm的X轴坐标值和Y轴坐标值；以及基于机器人在第二时间值处的位置、速度和加速度以及第二开始跟踪位置、在第二开始跟踪位置的速度和加速度，规划机器人的第二平滑运动轨迹。
[0019] 在某些实施例中，控制机器人从起始位置朝向第一开始跟踪位置运动包括：响应于目标物体到达第一位置，控制机器人以第一平滑运动轨迹开始运动；以及响应于目标物体到达第二位置，控制机器人以第二平滑运动轨迹开始运动至第二开始跟踪位置。
[0020] 在某些实施例中，控制机器人同步跟踪目标物体包括：基于目标物体的实时位置和机器人的实时位置之间的偏差，利用比例-积分-微分算法计算机器人的位置调整量；以及基于位置调整量，控制机器人同步跟踪目标物体。
[0021] 在某些实施例中，规划所述机器人的平滑运动轨迹包括：利用多项式插值算法对所述机器人的运动轨迹进行规划。
[0022] 在本公开的第二方面，提供一种用于机器人的传送带同步跟踪装置。该装置包括：获取单元，用于获取传送带上的目标物体的初始位置信息；位置估计单元，用于实时估计目标物体的位置；运动轨迹规划单元，用于基于机器人的起始位置、预设开始跟踪界限和所估计的目标物体的位置和速度，规划机器人的运动轨迹，规划包括：确定目标物体的第一位置，其中机器人在目标物体到达第一位置时从起始位置开始运动将与目标物体同时到达预设开始跟踪界限；以及基于第一位置、目标物体的位置和速度，规划机器人的运动轨迹，以使得机器人与目标物体同时到达与预设跟踪界限相对应的开始跟踪位置并且在开始跟踪位置处具有与目标物体相同的运动速度；第一控制单元，用于响应于目标物体到达第一位置，控制机器人从起始位置朝向开始跟踪位置以所规划的运动轨迹运动；以及第二控制单元，用于基于机器人和目标物体的实时位置，控制机器人从开始跟踪位置同步跟踪目标物体。
[0023] 在某些实施例中，所述位置估计单元包括：读取单元，用于实时读取所述传送带的编码器所反馈的编码器值Ct；以及第一计算单元，用于根据下式计算所述目标物体在所述机器人空间中的实时位置PRt：
[0024]
[0025] 其中，xRt和yRt分别为PRt的X轴坐标值和Y轴坐标值，xR0和yR0分别为所述目标物体的初始位置的X轴坐标值和Y轴坐标值，C0为所述目标物体的初始位置相应的编码器值，s为单位编码器值传送带前进的距离，θ为机器人空间坐标与传送带空间的坐标之间的夹角。
[0026] 在某些实施例中，所述运动轨迹规划单元包括：第二计算单元，用于基于所述预设开始跟踪界限xRs、所述机器人的所述起始位置以及预设的所述机器人的运动速度和加速度，计算所述机器人到达所述预设开始跟踪界限的第一时间值t1；第三计算单元，用于根据下式计算所述目标物体的所述第一位置的X轴坐标值xRt0，所述第一位置为所述目标物体从其开始经过所述第一时间值到达所述预设开始跟踪界限的位置:
[0027] xRt0＝xRs-Vct*t1*cos(θ)
[0028] 其中Vct为所述传送带的速度，θ为机器人空间坐标与传送带空间的坐标之间的夹角；根据下式得到第一开始跟踪位置PRs的Y轴坐标值yRs，
[0029] yRs＝yRt0+Vct*t1*sin(θ)
[0030] 其中yRt0为所述目标物体的所述第一位置的Y轴坐标值；以及第一轨迹规划单元，用于基于所述机器人的所述起始位置、在所述起始位置处的速度和加速度以及所述第一开始跟踪位置、在所述第一开始跟踪位置处的速度和加速度，规划所述机器人的第一平滑运动轨迹。
[0031] 在某些实施例中，所述第一控制单元包括：第一运动轨迹控制单元，用于响应于所述目标物体到达所述第一位置而控制所述机器人以所述第一平滑运动轨迹开始运动至所述第一开始跟踪位置。
[0032] 在某些实施例中，所述运动轨迹规划单元还包括：时间选取单元，用于选取第二时间值tm，所述第二时间值tm小于所述第一时间值t1；第四计算单元，用于计算在所述第二时间值tm处所述目标物体的第二位置PRtm；第五计算单元，用于根据下式计算第二开始跟踪位置P′Rs，以替代所述第一开始跟踪位置作为所述开始跟踪位置:
[0033]
[0034] 其中，x′Rs和y’Rs分别为P′Rs的X轴坐标值和Y轴坐标值,xRtm和yRtm分别为PRtm的X轴坐标值和Y轴坐标值；以及第二轨迹规划单元，用于基于所述机器人在所述第二时间值处的位置、速度和加速度以及所述第二开始跟踪位置、在所述第二开始跟踪位置的速度和加速度，规划所述机器人的第二平滑运动轨迹。
[0035] 在某些实施例中，所述第一控制单元包括：第二运动轨迹控制单元，用于响应于所述目标物体到达所述第一位置，控制所述机器人以所述第一平滑运动轨迹开始运动；以及第三运动轨迹控制单元，用于响应于所述目标物体到达所述第二位置，控制所述机器人以所述第二平滑运动轨迹开始运动至所述第二开始跟踪位置。
[0036] 在某些实施例中，所述第二控制单元包括：位置偏差计算单元，用于基于所述目标物体的实时位置和所述机器人的实时位置之间的偏差，利用比例-积分-微分算法计算所述机器人的位置调整量；以及跟踪控制单元，用于基于所述位置调整量，控制所述机器人同步跟踪所述目标物体。
[0037] 在某些实施例中，所述第一轨迹规划单元或所述第二轨迹规划单元用于利用多项式插值算法对所述机器人的运动轨迹进行规划。
[0038] 在本公开的第三方面，提供一种机器人系统。该系统包括机器人和根据本公开的第二方面所描述的装置。
[0039] 在本公开的第四方面，提供一种机器人系统。该系统包括机器人、处理器以及存储器，存储器存储有处理器可读的指令，指令在由处理器执行时使得处理器执行根据本公开的第一方面所描述的方法。
[0040] 在本公开的第五方面，提供一种计算机可读存储介质，其存储有机器可读的指令，指令在由机器执行时使得机器执行根据本公开的第一方面所描述的方法。
[0041] 本公开的实施例提出的用于机器人的传送带同步跟踪的方法、装置及系统，能够快速的实现目标物体的定位和跟踪，并适应传送带的实时速度，提高工业系统的精度和工作效率。
附图说明
[0042] 图1示出本发明实施例可以应用于其中的系统示意图；
[0043] 图2示出视觉系统、机器人及传送带之间的位置关系示意图；
[0044] 图3示出根据本公开的一个实施例的传送带同步跟踪方法的流程图；
[0045] 图4示出根据本公开的一个实施例的视觉模块和机器人控制模块的交互过程图；以及
[0046] 图5示出根据本公开的一个实施例的传送带同步跟踪装置示意图。
具体实施方式
[0047] 为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。但本领域技术人员知晓，本发明并不局限于附图和以下实施例。
[0048] 如本文中所述，术语“包括”及其各种变体可以被理解为开放式术语，其意味着“包括但不限于”。术语“基于”可以被理解为“至少部分地基于”。术语“一个实施例”可以被理解为“至少一个实施例”。术语“另一实施例”可以被理解为“至少一个其它实施例”。
[0049] 如前所述，现有的对传送带上目标物体的抓取等操作存在无法准确、快速实现作业目的的缺陷，本发明实施例提出了一种用于机器人的传送带同步跟踪的方法、装置及系统，能够快速的实现目标物体的定位和跟踪，减小速度的波动，提高工业系统的精度和工作效率。
[0050] 下面结合附图对本公开实施例作进一步描述。
[0051] 图1示出了本公开的实施例可以应用于其中的系统示意图。如图所示，系统主要由三个子系统构成：传送带子系统、视觉子系统和机器人子系统。在一个实施例中，作为示意，传送带子系统可以包括传送带110以及传送带110的编码器111，视觉子系统可以包括视觉模块120，机器人子系统可以包括机器人控制模块130、机械臂131以及与编码器111对接的硬件接口132。
[0052] 视觉模块120例如可以包括相机等用以捕获传送带110上的目标物体140的装置，附加地还可以包括信息处理装置。视觉模块120获取目标物体140的位姿信息，并且可以对获取的信息进行各种适当的处理。传送带110以一定的速度运载目标物体140进行运动，并且能通过编码器111测量传送带110的相对位置和速度。机器人控制模块130通过与视觉模块120和传送带110的编码器111的信息交互，产生控制信息以控制机械臂131对目标物体140进行同步跟踪。
[0053] 可以理解，为了便于描述本公开的实施例，图1中未具体示出和描述所熟知的机器人控制系统其他部件和过程以免不必要地模糊本公开实施例的方面。此外，在附图中以机械臂作为机器人的一种具体示例而描述，其他形式的机器人也适用于本公开的实施例。
[0054] 图2示出了系统100中各模块的位置关系图200。如图所示，视觉模块120的相机具有相机视野范围210，在此范围内视觉模块120可以捕获目标物体140的图像。传送带110作单方向直线运动，坐标系为OCXCYC,XC方向为传送带的运动方向，机器人坐标系为ORXRYR。这两个坐标系之间的夹角为θ。机器人具有工作区域220，在此区域内机器人可以正常进行作业。
[0055] 此外，根据本公开的实施例，可以为机器人(即机械臂131)在工作区域220内选择合适的开始跟踪界限221和结束跟踪界限222。为了方便下文描述，分别记为机器人坐标系中的xRs和xRf，对应于开始跟踪位置和结束跟踪位置。开始跟踪界限221和结束跟踪界限222限定了机械臂131进行有效同步跟踪的范围。
[0056] 图3示出了根据本公开的一个实施例的传送带同步跟踪方法300的流程图。方法300可以在机器人系统中执行，例如在机器人控制模块130处实现。
[0057] 在310，获取传送带上的目标物体的初始位置信息。然后在320，实时估计目标物体的位置和速度。在本公开的一个实施例中，目标物体的初始位置可以是当目标物体140出现在传送带110的入口处，视觉模块120捕获到目标物体140时所对应的位置。目标物体的初始位置信息可以包括目标物体的初始位置的坐标、该初始位置相应的传送带的位置信息、速度信息等。机器人控制模块130可以部分地基于该初始位置信息，实时估计目标物体的位置和速度。
[0058] 在图4中示出了根据本公开的一个实施例的步骤310和步骤320(共同以315标示)的一种实施方式。图4还示出了视觉模块120与机器人控制模块130各自的操作以及交互过程。
[0059] 如图4所示，在410，视觉模块120检测得到目标物体140的视觉系统坐标，例如，视觉相机以连续采样的方式捕捉传送带110入口处的目标物体140的视觉系统坐标PV。在获取到视觉系统坐标PV的同时，发送指令到机器人控制模块130，指示其保存当前时刻传送带编码器111反馈的编码器值C0。相应地，在420，响应于保存指令，机器人控制模块130保存视觉模块120检测到目标物体140时传送带的编码器111反馈的位置信息，即编码器值C0。
[0060] 接着，在430，视觉模块120通过视觉标定和传送带标定，可以得到视觉系统坐标和机器人系统坐标的转换关系TVR，通过式(1)可以得到视觉系统坐标PV对应的机器人系统坐标PR0，
[0061] PR0＝TVRPV (1)
[0062] 关于视觉坐标系统与机器人坐标系统的转换，可以采用例如四点法等常规方法，本公开在此方面不做限制。
[0063] 在440，视觉模块120将得到的目标物体坐标PR0(xR0,yR0,zR0)发送到机器人控制模块130，此坐标对应的编码器值为C0，将此目标物体的初始位置信息记为(PR0，C0)。机器人控制模块130在得到该初始位置信息后，即可在450处实时估计目标物体的位置以及速度。
[0064] 机器人控制模块130实时读取编码器111所反馈的位置值和速度值，记t时刻的编码器位置值为Ct，速度值为Vct。基于以上数值、机器人系统坐标和传送带系统坐标的夹角θ、以及传送带标定得到的传送带比例因子s(即单位编码器值传送带前进的距离)，根据以下式(2)可以实时估计t时刻目标物体坐标(PRt，Ct)，其中PRt(xRt,yRt,zRt)：
[0065]
[0066] 应当理解，结合图4所描述的在系统100中机器人控制模块130估计或预测目标物体的实时位置和速度的过程仅是示例，其他变型方式是可能的。例如，视觉系统120可以直接将视觉坐标系统中的位置值连同指令在410一起发送至机器人控制模块130，由机器人控制模块130以上述方法计算得到目标物体在机器人坐标系统中的初始位置坐标。此外，本公开的实施例关注传送带的同步跟踪，因此对传送带所在平面也即各坐标系中XY平面做具体分析，对于Z向坐标可以根据具体应用而容易得到。
[0067] 下面返回图3。在330，基于机械臂的起始位置、预设开始跟踪界限和所估计的目标物体的位置和速度，规划机械臂的运动轨迹。规划运动轨迹可以包括确定目标物体的第一位置，其中机械臂在目标物体到达第一位置时从起始位置开始运动将与目标物体同时到达预设开始跟踪界限；以及基于第一位置、目标物体的位置和速度，规划机械臂的运动轨迹，以使得机械臂与目标物体同时到达与预设跟踪界限相对应的开始跟踪位置并且在开始跟踪位置处具有与目标物体相同的运动速度。
[0068] 在一个实施例中，机器人(或机械臂131)起始位置位于等待位置PRw(xRw,yRw,zRw)。根据机器人设定的速度和加速度可以得到机器人从起始位置到达开始跟踪位置需要的时间为t1。根据目标物体实时的位置PRt(xRt,yRt,zRt)以及t1和开始跟踪界限xRs，可以根据下式(3)判断在目标物体140到达位置PRt0(xRt0,yRt0,zRt0)(称为第一位置)时机器人从等待位置开始运动，经过t1时间和目标物体140同时到达开始跟踪位置PRs(xRs,yRs,zRs)：
[0069] xRt0＝xRs-Vct*t1*cos(θ) (3)
[0070] 并且根据式(4)，可计算出开始跟踪位置的Y向坐标yRs,
[0071] yRs＝yRt0+Vct*t1*sin(θ) (4)
[0072] 当目标物体140的x方向坐标到达xRt0时，机械臂131需要从等待位置(即起始位置)PRw开始运动时间t1，并与目标物体140同时到达开始跟踪位置PRs，并在此处与目标物体140保持相同的速度。在本公开的实施例中，考虑机器人到达开始跟踪位置时和目标物体同速的策略，目标物体的速度即传送带运动的速度Vct，到达开始跟踪位置时机器人加速度为0。由此，对机械臂的运动轨迹进行规划，以实现该目的。
[0073] 根据一个实施例，对运动轨迹的规划可以采用多项式插值方法。假定机器人的当前位置、速度和加速度为(q0,v0,a0)，目标点位置、速度和加速度为(q1,v1,a1)，运行时长为T。根据以上已知条件，可以建立位置q和时间t的多项式如下式(5)所示：
[0074]
[0075] 将已知条件带入式(5)，可以求得系数的值：
[0076]
[0077] 由得到的式(6)系数，即可以规划出一条连接两点之间的弧线，由此得到平滑运动轨迹。
[0078] 再次回到图3，上述轨迹规划完成后，在340，当机器人控制模块130判断目标物体140到达第一位置，可以控制机械臂131从起始位置朝向开始跟踪位置以所规划的运动轨迹运动机器人可以向开始跟踪位置运动。
[0079] 在一个实施例中，考虑到在机器人向开始跟踪位置运动的过程中，传送带的速度会有波动，可能并不是完全的匀速运动，使得当机器人到达开始跟踪位置时和目标物体有较大的偏差，最终会导致机器人在跟踪同步目标物体时速度波动较大，跟踪上目标物体的时间较长等问题，本公开针对该问题提出了以下解决方案。
[0080] 在机器人从等待位置向开始跟踪位置运动的过程中，可以得到机器人的实时位置qt，实时速度VRt，实时加速度at。为了减小传送带速度波动对开始跟踪时误差的影响，在此段运动将要结束前，即机器人轨迹运行了时间tm(0
[0081]
[0082] 可以理解，上述对开始跟踪位置的微调过程可以根据实际应用而进行多次，以便进一步降低机械臂和目标物体同时到达开始跟踪位置的误差。此外，本公开仅以多项式插值方法为例对轨迹规划进行描述，也可以采用其他方法规划满足符合运动条件的轨迹。
[0083] 接下来，当机械臂和目标物体同时到达开始跟踪位置，在350，基于机械臂和目标物体的实时位置，控制机械臂从开始跟踪位置同步跟踪目标物体。
[0084] 在一个实施例中，目标物体140的实时位置由式(2)可计算得到为PRt(xRt,yRt,zRt)，同时机器人控制模块130可以实时获取机器人的位置设为PR0(xR0,yR0,zR0)。机器人t时刻将要到达的位置，即发送给机器人的位置指令为PRk(xRk,yRk,zRk)，采用比例-积分-微分(PID)算法对目标物体进行跟踪，如下式：
[0085] ε(t)＝PRt(t)-PR0(t) (8)
[0086]
[0087] PRk＝PR0+m(t) (10)
[0088] 其中m(t)是机器人在t时刻的调整量，ε(t)是机器人当前位置和目标物体位置的偏差，Kp是比例系数，Ti为积分时间常数，Td为微分时间常数。使用上式(9)，每隔一个采样周期△t，更新一次PRk，从而控制机器人跟踪目标物体运动。
[0089] 在同步跟踪的过程中，当机器人跟踪上目标物体，例如两者位置的偏差足够小或者达到一定的跟踪时间，机器人可以对目标物体进行相关处理，如抓取、分拣、打包或在线检测等等。
[0090] 最后，在360，当检测到机器人运动到结束跟踪界限时，根据机器人当前的位置，速度和加速度，以及等待位置的坐标位置，机器人控制模块130进行轨迹规划，并控制机械臂回到等待位置，等待下一个目标物体经过传送带。
[0091] 采用本公开实施例提供的传送带同步跟踪方法，可以有效地对传送带上的目标物体进行实时同步跟踪，准确率高，并且实施灵活。
[0092] 图5示出了根据本公开的一个实施例的传送带同步跟踪装置500的示意图，装置500可以包括例如系统100中的机器人控制模块130。如图所示，装置500包括：获取单元510，用于获取传送带上的目标物体的初始位置信息；位置估计单元520，用于实时估计目标物体的位置和速度；运动轨迹规划单元，用于基于机械臂的起始位置、预设开始跟踪界限和所估计的目标物体的位置和速度，规划机械臂的运动轨迹。
[0093] 在一个实施例中，规划轨迹包括：确定目标物体的第一位置，其中机械臂在目标物体到达第一位置时从起始位置开始运动将与目标物体同时到达预设开始跟踪界限；以及基于第一位置、目标物体的位置和速度，规划机械臂的运动轨迹，以使得机械臂与目标物体同时到达与预设跟踪界限相对应的开始跟踪位置并且在开始跟踪位置处具有与目标物体相同的运动速度；
[0094] 装置500还包括第一控制单元540，用于响应于目标物体到达第一位置，控制机械臂从起始位置朝向开始跟踪位置以所规划的运动轨迹运动；以及第二控制单元550，用于基于机械臂和目标物体的实时位置，控制机械臂从开始跟踪位置同步跟踪目标物体。
[0095] 应当理解，装置500中的每个单元分别与参考图1至图4结合若干实施例描述的方法300和400中的每个步骤相对应。因此，上文结合图1至图4描述的操作和特征同样适用于装置500及其中包含的单元，并且具有同样的效果，具体细节不再赘述。
[0096] 本公开的实施例还提供一种机器人系统。该系统可以包括机器人和前述的装置500。
[0097] 本公开的实施例还提供一种机器人系统。该系统包括处理器以及存储器，存储器存储有处理器可读的指令，指令在由处理器执行时使得处理器执行如前所述的方法。
[0098] 本公开的实施例还提供一种计算机可读存储介质，其存储有机器可读的指令，指令在由机器执行时使得所述机器执行根据本公开所描述的方法。
[0099] 本领域技术人员可以理解，在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。
[0100] 计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式光盘只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。
[0101] 应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或它们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。
[0102] 在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
[0103] 以上，对本发明的实施方式进行了说明。但是，本发明不限定于上述实施方式。凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
