基于硬件同步信号与预标定时延补偿的多模态同步数据采集方法与流程

摘要: 本发明涉及数据采集领域,尤其是涉及一种基于硬件同步信号与预标定时延补偿的多模态同步数据采集方法。背景技术:1、机器人学习与控制领域需要高质量的多模态数据集,通常...
基于硬件同步信号与预标定时延补偿的多模态同步数据采集方法与流程

本发明涉及数据采集领域,尤其是涉及一种基于硬件同步信号与预标定时延补偿的多模态同步数据采集方法。

背景技术:

1、机器人学习与控制领域需要高质量的多模态数据集,通常包含视觉图像(rgb相机)和机械臂关节状态(位置、速度、力矩)。这两类数据由独立的硬件设备产生,其采集时刻存在不确定的时间偏差,主要体现在以下几方面:

2、1.软件触发延迟不确定性

3、现有方案通常由上位机(pc)通过软件调用依次向相机和机械臂控制器发送采集指令,受操作系统任务调度、网络协议栈延迟、usb驱动延迟等因素影响,两路指令到达时刻的差值可达数毫秒甚至数十毫秒,无法实现精确同步。

4、2.相机曝光起始时刻不透明

5、工业相机通常以帧率回调方式向主机传递数据,帧数据到达主机的时刻与实际曝光起始时刻之间存在mipi/usb传输延迟,且该延迟受分辨率、帧率、总线负载等因素影响,若不加以标定,将引入系统性时间误差。

6、3.时间戳来源不统一

7、相机时间戳与机械臂控制器时间戳通常基于各自独立的本地时钟,缺乏共同的时间基准,难以在后处理阶段完成精确对齐。

8、4.摄像头数量较少

9、现有方案摄像头视角普遍只有腕部和第三视角,继续增加视角会受到usb总线负载及带宽影响,使得摄像头数量上限极低,难以扩展多视角采集的场景。

技术实现思路

1、本发明的目的就是为了提供一种多模态数据同步精度更高的基于硬件同步信号与预标定时延补偿的多模态同步数据采集方法。

2、本发明的目的可以通过以下技术方案来实现:

3、一种基于硬件同步信号与预标定时延补偿的多模态同步数据采集方法,所述采集方法基于对应采集系统实现,采集系统包括包含相机控制器的相机、包含机械臂控制器的机械臂本体、上位机、以太网和触发模块,所述采集方法包括以下步骤:

4、相机控制器和机械臂控制器上电,进行全局初始化,其中,初始化过程中获取了相机启动延迟均值,并将其设置为机械臂控制器在收到触发事件后的等待时间;

5、上位机向触发模块发送触发指令,并记录发送完成时刻的时间戳t_host_send,以经触发模块同步触发相机控制器和机械臂控制器进入触发事件;

6、在进入触发事件后,开启相机流,并在相机控制器等待所述相机启动延迟均值的时间后完成曝光采集,同时机械臂控制器在所述等待时间后采集关节数据,最后连同各自的采集延时进行封装并上传至上位机,等待下一次触发;

7、上位机接收封装有采集延时的采集数据,结合所述时间戳t_host_send进行延时补偿以实现数据对齐,输出对齐数据对;

8、当上位机下发停止触发指令后,相机控制器和机械臂控制器停止采集,完成采集过程。

9、进一步地,所述相机控制器的初始化步骤包括:

10、相机控制器上电后,以太网驱动初始化,获取相机控制器的ip地址并建立与上位机之间的网络通信;

11、启动udp配置服务,周期广播announce包;

12、v4l2设备初始化,配置摄像头模组;

13、jpeg编码器初始化,分配用于缓存的环形缓冲区;

14、启动延迟标定循环,以在设定的循环次数下循环启动多次相机流采集,计算出相机启动延迟均值calibrated_startup_delay_us和延迟抖动标准差startup_delay_jitter_us;

15、awb暖机,以在相机流启动后的设定时间间隔后关闭相机流,等待白平衡收敛;

16、启动http流媒体服务器,配置gpio触发引脚,任意边沿可触发中断,绑定gpio中断服务程序isr以捕获触发信号的上升沿和下降沿,注册帧回调函数video_frame_callback,并通过udp广播公告将所述相机启动延迟均值calibrated_startup_delay_us和延迟抖动标准差startup_delay_jitter_us上报给上位机,完成相机控制器的初始化过程,等待触发事件。

17、进一步地,所述相机启动延迟均值和延迟抖动标准差的获取步骤包括:

18、设定总循环次数n;

19、在每次循环中,记录开启流的时间戳t_streamon,开启相机流vidioc_streamon,当捕获到首帧图像时,触发帧回调事件,保存首帧时间戳t_firstframe,得到本次延迟样本delay_i=t_firstframe-t_streamon,最后关闭相机流vidioc_streamoff,并等待设定时间,完成一次循环下的相机流采集;

20、循环序号自增1,并判断是否小于总循环次数n,若是,则进行下一次循环下的相机流采集,若否,则退出所述延迟标定循环;

21、基于获取的n组延迟样本delay_i,计算出相机启动延迟均值calibrated_startup_delay_us和延迟抖动标准差startup_delay_jitter_us。

22、进一步地,所述机械臂控制器的初始化步骤包括:

23、机械臂控制器上电后,以太网初始化,获取机械臂控制器的ip地址并建立与上位机之间的网络通信;

24、检测连接的机械臂类型,自动选择对应的适配通信通道,并与机械臂本体握手确认,完成机械臂控制器与机械臂本体之间的通信链路建立;

25、机械臂控制器通过以太网接收上位机下发的相机启动延迟均值calibrated_startup_delay_us,并设置相同的延迟时长t_wait=calibrated_startup_delay_us,所述延迟时长t_wait为机械臂控制器在收到触发事件后的等待时间;

26、配置gpio触发引脚,任意边沿可触发中断,绑定gpio中断服务程序isr以捕获触发信号的上升沿和下降沿,完成机械臂控制器的初始化过程,等待触发事件。

27、进一步地,当检测到机械臂类型为串口类时,初始化usb转串口硬件,加载dynamixel舵机总线协议,建立串口机械臂网络通信通道;

28、当检测到机械臂类型为以太网类ur5e时,初始化以太网tcp客户端,采用urscript协议和ur5e机械臂建立网络通信通道;

29、当检测到机械臂类型为以太网类xarm时,初始化以太网tcp客户端,调用xarmsdkapi协议,建立与xarm机械臂的网络通信通道。

30、进一步地,所述进入触发事件的步骤包括:

31、对于相机控制器和机械臂控制器,当触发模块完整收到所述触发指令后,解析后立即将同时连接相机控制器和机械臂控制器各自gpio引脚的同一条gpio触发线拉高,产生的上升沿触发各自gpio中断;

32、相机控制器和机械臂控制器以本地计时器timer各自记录上升沿到达时刻t_trigger_cam、t_trigger_arm,并置位stream_active=true、g_trigger_active=true、camera_stream_started=true、arm_active=true,其中t_trigger_cam为相机侧上升沿触发时刻时间戳,t_trigger_arm为机械臂侧上升沿触发时刻时间戳,stream_active为相机侧的采集激活标志位,g_trigger_active为相机侧触发事件对应的采集任务激活标志位,camera_stream_started为相机流启动标志位,arm_active为机械臂侧的采集激活标志位,true表示激活操作。

33、进一步地,在进入触发事件后,所述相机控制器的采集步骤包括:

34、(1)在camera_stream_started=true条件下,开启相机流vidioc_streamon后启动第一帧曝光,等待约所述相机启动延迟均值的时间后,待第一帧曝光完成,v4l2设备驱动释放帧就绪信号量frame_ready_sem,相机控制器调用信号量获取函数xsemaphoretake(frame_ready_sem)阻塞获取frame_ready_sem,确认第一帧曝光完成;

35、(2)判断v4l2设备驱动提供的硬件帧时间戳v4l2_ts是否可用,若是,则采用所述硬件帧时间戳v4l2_ts,计算采集延时capture_time_us=v4l2_ts-t_trigger_cam,若否,则不采用所述硬件帧时间戳v4l2_ts,启用帧回调函数获取帧回调时间戳callback_ts,并利用所述帧回调时间戳callback_ts计算采集延时capture_time_us=callback_ts-传输常数,

36、(3)jpeg编码,将所述采集延时capture_time_us与帧序号frame_num嵌入jpeg的app4段中,以将时间同步信息绑定在图像帧内;

37、(4)将携带app4段的图像帧存入环形缓冲区,并推流至上位机;

38、(5)判断所述stream_active是否仍为true,若是,返回步骤(1),循环等待下一帧就绪,进行持续采集,若否,则停止采集,并执行步骤(6);

39、(6)根据触发状态查询函数app_trigger_is_active()返回值判断是否已激活新出触发事件,若是,则保留相机状态和环形缓冲区,跳过资源清理,等待一下帧采集,若否,则关闭相机流vidioc_streamoff,清空环形缓冲区。

40、进一步地,在进入触发事件后,所述机械臂控制器的采集步骤包括:

41、启动本地定时器,等待所述等待时间结束,以将机械臂本体的采样时刻与相机首帧时刻时间对齐;

42、根据建立的机械臂控制器与机械臂本体之间的通信链路,向机械臂本体发送关节状态查询指令;

43、记录本次采样时间戳t_arm_sample,接收机械臂本体返回的关节数据,所述关节数据包括关节位置、关节速度和关节力矩;

44、将t_arm_sample与t_trigger_arm之间的差值作为机械臂本次采集延时acquisition_delay_us,并连同所述关节数据组装成封装数据通过以太网上报至上位机;

45、判断所述arm_active是否还为true,若是,重复上述所有步骤进行一下轮采集,若否,则触发结束,清理资源并等待下次采集任务触发。

46、进一步地,所述输出对齐数据对的步骤包括:

47、对于相机侧的封装数据,解析出jpeg的app4段,提取出相机侧的采集延时capture_time_us与帧序号frame_num;

48、获取机械臂侧的封装数据(acquisition_delay_us,joint_data),acquisition_delay_us为机械臂侧的采集延时,joint_data为关节数据;

49、基于所述时间戳t_host_send,结合所述采集延时capture_time_us、acquisition_delay_us,分别计算相机侧和机械臂侧的采集物理时刻,得到对齐数据对,其中计算表达式为:

50、t_physical_cam=t_host_send+capture_time_us

51、t_physical_arm=t_host_send+acquisition_delay_us

52、式中,t_physical_cam为相机侧的采集物理时刻,表示完成帧曝光的物理时刻,t_physical_arm为机械臂侧的采集物理时刻,表示关节数据的物理采样时刻;

53、判断所述相机侧和机械臂侧的采集物理时刻之间的差值绝对值是否小于设定阈值threshold,若是,则判定数据已对齐,输出对齐数据对(frame_num,t_physical_cam,jpeg_image,joint_positions,joint_velocities,joint_torques),若否,则表示未对齐,其中,jpeg_image为采集的图像帧,joint_positions为关节位置,joint_velocities为关节速度,joint_torques为关节力矩。

54、进一步地,当触发模块收到所述停止触发指令后,立即将同时连接相机控制器和机械臂控制器各自gpio引脚的同一条gpio触发线拉低,产生下降沿,并将g_trigger_active置为false,向触发事件队列发送触发停止事件标识trigger_event_stop,stream_active置为false,调用信号量获取函数xsemaphoretake(frame_ready_sem)释放帧循环的阻塞等待,以停止相机侧的采集任务,同时,机械臂侧也停止采集任务。

55、与现有技术相比,本发明具有以下有益效果:

56、(1)同步精度更高:本发明中触发模块将上位机下发的触发指令作为触发相机控制器和机械臂控制器的同步触发信号,延时补偿下沉至各采集控制器本地,相机利用标定得到的相机启动延迟均值补偿相机启动-曝光固有延时,两种控制器分别计算相对于触发上升沿的延时,适配相机、机械臂各自不同的硬件时延特性,提升多源数据时间对齐精度。

57、(2)鲁棒性提升:本发明触发模块仅输出硬件脉冲不参与计时,相机控制器、机械臂控制器各自通过本地定时器捕获同一触发信号的上升沿,独立完成计时与延时计算;区别于单mcu集中计时架构,单路控制器异常不会造成整套同步完全失效,同时降低触发模块对时钟精度、算力的要求。

58、(3)降低上位机压力,避免配对错位:本发明相机将采集延时、帧序号嵌入jpeg的app4段,机械臂随采样数据包携带采集延时,同步信息内嵌于数据包;上位机解析数据包内置延时即可还原物理采样时刻,无需对两路分离数据流进行序列号配对,规避传输抖动引发的配对错误,减轻上位机运算负荷。

59、(4)启停逻辑稳定可靠:本发明采用gpio硬件边沿中断捕获触发,结合多级状态标志与trigger_event_stop异步停止事件,将硬件中断与上层业务处理解耦,抑制电平抖动干扰,可靠控制单次采集任务的启动与停止。