基于FPGA的多传感器融合同步授时系统设计

黄战华,李晓伟,王康年,董联欣

(1.天津大学精密仪器与光电子工程学院,光电信息技术教育部重点实验室,天津 300072;2.天津大学四川创新研究院,四川成都 610200)

0 引言

多传感器融合[1-2]是指对多传感器信息的采集、匹配以及其内在联系进行综合处理与优化的技术,它是从全方位多信息的角度出发对数据信息进行对应与处理优化,得到数据信息的内在联系与规律的过程。多传感器的融合可以剔除掉一些错误的和无用的信息,而保留正确的和有用的信息,因而极大提高了数据信息的可靠性与对应匹配的精准性,避免了单一传感器获取的数据信息有限以及缺乏各传感器之间的内在对应关系和信息交互联系的弊端影响。多传感器融合由于其独特的优势,提高了信息的可靠性与系统的精确性,被广泛应用于军事、航天、智能制造、目标检测与图像处理等领域[3-4]。

目前,多传感器融合同步授时系统的稳定性与鲁棒性容易受到GPS信号等环境因素变化的影响[5],环境适用性不强;其次,多传感器信息融合的精确性容易受到传感器自身延迟的影响,导致信息融合的有效性不佳[6-7]。

针对上述问题,本文提出一种基于FPGA的多传感器融合同步授时系统,包括系统框架设计与同步授时方法。系统可以克服GPS信号不稳定等环境因素变化的影响,同时可以精确补偿传感器对应的触发延迟时间,提高了系统的稳定性与环境适应性。实验证明,基于FPGA的多传感器融合同步授时系统可以实现多传感器的同步授时与信息融合,且各传感器信息融合的鲁棒性较好,满足同步定位与建图(simultaneous localization and mapping,SLAM)[8]等应用场景对多传感器融合的实现要求。

1 基于FPGA的多传感器融合同步授时系统设计

本文设计的基于FPGA的多传感器融合同步授时系统采用现场可编程逻辑门阵列(FPGA)作为主控,以FPGA系统时钟作为基准时钟,以秒脉冲(pulse per second,PPS)校准模块产生的标准可控的触发信号作为传感器的标准触发信号,传感器触发校准模块以该标准触发信号作为基准信号结合各传感器自身的工作频率产生触发传感器工作的唯一且精确的触发信号,同时触发校准模块内部设置有延时补偿参数,可以精确补偿传感器对应的触发延迟时间,避免传感器的触发延迟对信息融合有效性的影响,并且可以方便获得各传感器触发时刻对应的精确时间戳。FPGA将每路传感器数据与各自时间戳进行标记与拼接处理之后,置于各传感器对应的先进先出数据缓存器(FIFO)内,并通过优先级管理单元将各传感器拼接后的数据包上传至上位机,优先级管理单元将并行的数据转换成串行的数据,并按照预先设计的优先级顺序将各缓冲区内的传感器数据依次上传至上位机,以此方式避免了各传感器数据上传至上位机时发生的并行转串行相互抢占的问题,且该优先级顺序可以根据系统不同功能的需求进行自定义调整。

此外,当传感器在野外采集数据时,由于受到GPS信号不稳定的影响,难免会出现数据缺失或者漏采的情况,会导致多传感器融合的鲁棒性降低。而基于FPGA的多传感器融合系统可以通过FPGA内部的PPS校准模块校准不稳定的PPS脉冲信号,产生精确且稳定的标准触发信号,从而避免了PPS秒脉冲信号易受环境变化影响的问题,并且即使在无GPS信号的环境下,通过FPGA内部的系统计数器也可以使各传感器正常工作,系统的鲁棒性并不会因此而降低,因而提高了多传感器系统的适应性、多传感器融合的鲁棒性和同步授时的精确性。

基于FPGA的多传感器融合同步授时系统框架如图1所示。系统包括全球卫星导航系统(global navigation satellite system,GNSS)授时模块、惯性测量单元(inertial measurement unit,IMU)、激光雷达、相机、FPGA以及上位机(即工控机)。

GNSS授时模块用于每隔1 s产生1次PPS脉冲以及NMEA-0183协议的数据报文。FPGA内部的UTC解析模块会对产生的NMEA-0183协议数据报文进行解析,获取当前时刻的协调世界时(universal time coordinated,UTC),并将其与对应时间戳标记拼接处理之后保存至对应的FIFO缓冲区4内。PPS校准模块内通过设置标准触发频率(如200 Hz)和标准计数器对GNSS产生的不稳定的PPS脉冲信号进行校准,并会依据设置的标准触发频率输出精确且稳定的标准触发信号。触发校准模块为产生各传感器的精确触发信号的模块,其根据标准触发信号结合各传感器自身的工作频率输出触发传感器工作的唯一且精确的触发信号。时间戳、数据解析与拼接模块用于对各传感器的数据和纳秒时间戳进行标记与拼接处理,并将处理后的数据帧放置于各传感器对应的FIFO缓冲区内。并串转换与优先级管理模块根据预先设定的优先级顺序生成访问信号对各传感器的缓冲区依次进行访问,并生成串行数据包,通过USB协议将串行数据包上传至上位机中。

以下分别说明UTC时间的解析与各传感器的同步触发过程。

1.1 UTC时间的解析

GNSS授时模块每隔1 s以NMEA-0183协议输出1次数据包,而每个数据包中均包含以GPGGA、GPGSA、GPGSV、GPRMC、GPVTG、GPGLL等为帧头信息的多帧数据。每帧数据所包含的信息均存在差异,而本文只需要从包含UTC时间的数据帧中提取出UTC时间即可,即先获取包含UTC时间的一个数据帧,再从该数据帧中提取出UTC时间。本文选用的是以GPRMC为帧头的“推荐定位信息”,从该数据帧中提取出UTC时间。该数据帧中的UTC时间由时分秒时间与年月日时间组成,其处于数据帧中的不同位置,需要将时分秒时间与年月日时间提取出来,并与该UTC时间对应的纳秒时间戳进行拼接与打标签等解析操作,并将解析后的数据帧通过移位缓冲区以单字节的方式发送至预先设定好的FIFO缓冲区4中,其中解析后的数据帧包括3字节识别码“FF_00_FF”、4字节纳秒时间戳、10字节年月日时间与10字节时分秒时间、1字节结束位“FF”。通过ModelSim软件对UTC时间解析程序进行仿真,时序仿真图如图2所示。

图3 UTC时间数据帧上传时序仿真图

图4为UTC时间上传至上位机时的上板实验结果,由时序仿真结果和上板实验结果可知,UTC时间可以正常提取拼接并成功上传至上位机。

图4 UTC时间上传至上位机结果

1.2 惯性测量单元IMU的同步触发

惯性测量单元IMU是用来测量物体三轴姿态角及加速度的装置,是自动驾驶与三维重建等领域不可或缺的传感器之一。本文选用九轴IMU传感器ML7600,分别用来测量X轴、Y轴和Z轴的角度、加速度和角速度信息。系统设定该IMU的工作频率为100 Hz,即每1 s产生100次触发指令,同时返回100次角度等相关数据信息。IMU模块的同步触发框图如图5所示。

图5 IMU模块同步触发框图

为了避免GNSS授时模块产生的PPS秒脉冲不稳定且存在由于信号问题导致有时PPS秒脉冲会消失的情况,因而需要通过PPS校准模块对不稳定的PPS秒脉冲进行校准。具体实现方式是在校准模块内部设置标准触发频率(本文均以200 Hz为例进行说明)和标准计数器,标准触发频率用于规定模块输出的精确稳定的标准触发信号的频率;标准计数器用于对模块输入的PPS脉冲信号进行半周期的延时,延时时间为Td,单位为μs,其值等于设定的标准触发频率对应的周期值的1/2。延时后的信号即为输出的精确且稳定的标准触发信号,其频率为设定的200 Hz。PPS校准模块对应的时序原理如图6所示。

图6 PPS校准模块对应的时序图

PPS校准模块输出的标准触发信号是对PPS脉冲信号进行了半周期的延时,半周期对应的时间应大于PPS脉冲信号波动周期对应的半周期值,这样输出的标准触发信号避免了PPS脉冲信号波动周期的影响,能精确且稳定地输出信号。如图6所示,标准计数器采用先减后增的计数方式,其计数最大值设为CMAX,计数标志信号用于判断标准计数器处于增加状态或减小状态,当其为1时,标准计数器处于减小状态,当其为0时,标准计数器处于增加状态。当标准计数器计数到1且计数标志信号为1时,产生标准触发信号。设FPGA系统时钟频率为fCLK,标准触发频率为fST,其单位均为Hz,FPGA的系统时钟对应的系统计数器最大值为TMAX,则:

TMAX=fCLK-1

(1)

CMAX=(fCLK/fST)/2

(2)

Td=(1/fST)/2

(3)

由于标准计数器的置位条件取决于PPS秒脉冲信号的到来或者系统计数器计数到最大值,即意味着标准触发信号的产生将不唯一依赖于PPS秒脉冲信号的上升沿。因此,即使在无GPS信号的环境下,仍可以通过FPGA内部的系统计数器使标准触发信号正常产生,这样就避免了PPS秒脉冲信号产生不稳定且因此会对传感器的触发信号造成影响的不良情况,从而实现了对PPS秒脉冲信号的校准,同时也提高了系统的稳定性与适应性。

IMU触发校准模块以PPS校准模块输出的标准触发信号为基准,结合标准触发信号频率与IMU频率(即100 Hz)的比值关系产生唯一且精确的IMU触发信号,该IMU触发信号为单周期的高电平信号,其用于触发生成IMU的触发指令。当触发指令生成模块接收到该IMU触发信号之后产生IMU触发指令,该IMU触发指令为8字节的数据,并以RS232通信协议将该触发指令发送至IMU模块,用于触发IMU模块开始工作。此外,IMU触发校准模块内设置有延时补偿参数,该延时补偿参数对应的值即为传感器的延迟时间,其目的是用于补偿传感器自身的延迟时间,避免传感器的延时时间对传感器信息融合的有效性造成影响,提高了系统的鲁棒性和模块的复用性。图7为IMU模块同步触发的时序示意图,其中包括标准触发信号与IMU触发信号的比例关系以及经过补偿后的IMU触发信号与未经补偿的IMU触发信号的相对位置关系(图例显示的补偿后IMU触发信号仅用于示例,其对应的比例关系并不代表IMU模块真实的延迟时间)。

图7 IMU模块同步触发时序示意图

补偿后IMU触发信号会相对于未补偿IMU触发信号提前一段时间产生,该时间对应于IMU模块的触发延时时间,如图7所示,即

T=t1+t2

(4)

式中:t2为IMU模块接收完整触发指令所用时间,μs;t1为从FPGA发送完成请求指令到IMU模块开始输出数据时的延时时间,μs。

图8为用示波器测量IMU模块的触发延时。

图8 IMU模块的触发延时测量结果

IMU数据包接收模块用于以RS232通信协议接收IMU模块采集的数据,并将其传输至时间戳和数据解析与拼接模块,IMU时间戳和数据解析与拼接模块以数据帧为单位对接收的单字节数据进行组合,同时与该帧数据对应的纳秒时间戳进行拼接。当FPGA接收到IMU模块传回的第一个字节时,该时刻对应的FPGA系统计数器的计数值即为该帧数据对应的纳秒时间戳。同时对拼接后的数据帧添加标识符与结束位,即得到完整的解析处理后的一帧IMU数据,其中包括3字节的标识符“FF_01_FF”、4字节时间戳、41字节IMU数据与1字节结束位“FF”。数据移位缓冲区将处理后的数据帧以单字节的方式发送至预先设定好的FIFO缓冲区1中。图9为IMU数据帧解析与拼接的时序仿真图。

图9 IMU数据帧解析与拼接的时序仿真图

由图9可以看出:可以成功地对IMU数据和纳秒时间戳进行标记与拼接处理,并将拼接后的数据帧置于buffer空间中,同时数据移位缓冲区将拼接后的数据帧以单字节的方式发送至预先设定好的FIFO缓冲区1中,并经过优先级管理模块最终上传至上位机。

数据帧上传时序仿真结果如图10所示,由仿真结果可知,IMU数据和对应时间戳可以正常获取拼接并上传至上位机,在上位机中完成IMU数据帧的解析。

图10 IMU数据帧上传时序仿真图

多传感器融合系统中,IMU的工作频率为100 Hz,即FPGA 1 s产生100次触发指令,同时获取100次IMU模块触发时刻对应的纳秒时间戳。图11为在任意一段时间间隔内,上位机解析出的IMU模块触发时刻对应的纳秒时间戳与系统时间的对应关系,其中横轴表示系统时间,纵轴表示IMU模块的纳秒时间戳,系统时间每秒对应100个纳秒时间戳。由图11可以看出,IMU纳秒时间戳可以稳定地产生和获取,即IMU模块可以正常且稳定地被触发。

图11 IMU模块时间戳与系统时间的对应关系

1.3 相机的同步触发

工业相机作为机器视觉系统的重要组成部分,被广泛应用于视觉检测、自动驾驶与图像处理等领域。本文通过同步脉冲触发的方式实现对相机的同步触发,相机模块同步触发的系统框图如图12所示。根据系统的需求,设定相机的工作频率为5 Hz。相机的触发信号是由触发校准模块将该模块输入的标准触发信号作为基准结合相机的工作频率产生的。

图12 相机模块同步触发系统框图

图13为标准触发信号与相机触发信号的时序关系,同时也为相机同步触发的时序示意图。

图13 相机同步触发时序示意图

图13中的标准触发信号即为PPS校准模块产生的频率为200 Hz的信号,触发校准模块以该信号作为基准,结合标准触发信号频率与相机自身工作频率的比值关系产生5 Hz的传感器触发信号,该传感器触发信号为单周期的高电平信号,其信号周期t3=200 ms,对应于相机的工作频率5 Hz。由于触发相机工作的信号有脉宽限制,脉冲宽度由相机自身特性决定,故需要将单周期的传感器触发信号进行脉冲展宽。脉冲扩展模块实现对单周期的传感器触发信号进行展宽,输出占空比为τ的相机触发信号,且该占空比可调节,本文以相机触发信号的脉宽对应时间t4=100 μs为例进行说明,即占空比τ为

(5)

实现多传感器融合系统中相机同步授时的核心步骤是获取相机触发时刻对应的纳秒时间戳,如图13所示,相机触发信号与传感器触发信号的上升沿须处于对齐状态,便于精准获取相机触发时刻的纳秒时间戳,且当相机触发信号的上升沿产生时,该时刻系统计数器的计数值即为相机触发时刻的纳秒时间戳,需要将其提取出来,并在相机时间戳解析与拼接模块内进行标记与拼接处理,即对提取出的时间戳添加3字节的标识符“FF_02_FF”和1字节的结束位“FF”。图14为相机触发信号的产生和相机纳秒时间戳的提取与拼接时序仿真图。

图14 相机触发信号产生与相机时间戳的提取拼接时序仿真图

由图14可知,相机触发信号可以正常产生,且触发信号的脉宽符合设定的脉冲宽度,同时相机触发时刻对应的纳秒时间戳可以正常提取与拼接。拼接后的相机时间戳长度共8字节,包括3字节的标识符、4字节纳秒时间戳和1字节结束位,且放置于buffer空间中。数据移位缓冲区将拼接处理后的相机时间戳以单字节的形式发送至预先设定好的存放相机时间戳的FIFO缓冲区3中,便于后续优先级管理模块进行访问。优先级管理模块根据预先设定的优先级顺序生成访问信号对各传感器缓冲区依次进行访问,并生成串行数据包,以此方式最终将相机时间戳通过USB协议发送至上位机。图15为相机时间戳上传至上位机的时序仿真结果,由时序仿真结果可知,相机时间戳可以正常获取拼接并上传至上位机,在上位机中完成相机时间戳的解析。

图15 相机时间戳上传时序仿真图

多传感器融合系统中,相机的工作频率为5 Hz,即FPGA每秒产生5次相机触发信号,同时获取5次相机触发时刻对应的纳秒时间戳。图16为在任意一段时间间隔内,上位机解析出的相机模块触发时刻对应的纳秒时间戳与系统时间的对应关系,系统时间每秒对应5个相机纳秒时间戳。由上述时间戳上传时序仿真结果与相机时间戳的显示结果可知,相机纳秒时间戳可以稳定地产生和获取,并上传至上位机,最终在上位机中完成时间戳的解析,即相机模块可以正常且稳定地实现同步触发。

图16 相机时间戳与系统时间的对应关系

1.4 激光雷达的同步触发

系统所选用的激光雷达型号为Livox Mid-70,支持3种同步方式,分别为IEEE 1588-2008同步(即PTP协议同步)、PPS脉冲同步以及GPS同步,本文选用的同步方式是通过PPS脉冲进行同步,其同步触发系统框图如图17所示。通过以PPS脉冲信号为基准输出的激光雷达触发信号实现激光雷达的同步触发,且激光雷达触发信号的产生频率为1 Hz。

激光雷达收到触发信号,将以10 Hz的工作频率采集点云数据,并将采集的点云数据直接通过网口上传至上位机中,而激光雷达触发时刻对应的纳秒时间戳通过FPGA上传至上位机。图18为PPS脉冲信号与激光雷达触发信号以及激光雷达工作频率的对应时序关系。其中PPS脉冲信号即为GNSS授时模块产生的频率为1 Hz的信号,传感器触发信号为触发校准模块以标准触发信号作为基准结合激光雷达的触发频率产生的频率为1 Hz的信号,且传感器触发信号为单周期的高电平信号。由于触发激光雷达工作的信号有脉宽限制,因而需要对单周期的高电平信号进行脉冲展宽,激光雷达触发信号即为脉冲扩展模块对传感器触发信号展宽之后输出的触发激光雷达工作的信号。而激光雷达接收到触发信号之后,将以10 Hz的频率开始采集数据,对应于图18中的激光雷达工作频率时序。

图18 激光雷达同步触发时序示意图

激光雷达与IMU模块同步的核心在于数据时间系对齐,即获取激光雷达触发时间戳是实现激光雷达与IMU同步的核心所在,激光雷达触发时间戳对应于激光雷达触发信号的上升沿产生时,该时刻系统计数器的计数值。如图18所示,激光雷达触发信号与传感器触发信号的上升沿须处于对齐状态,即保证了提取的纳秒时间戳精确对应于激光雷达触发时刻,从而不会因此对同步授时的精确性产生相应影响。提取出的激光雷达触发时间戳需要在激光雷达时间戳解析与拼接模块内进行标记与拼接处理,即对提取出的时间戳添加3字节的标识符“FF_03_FF”和1字节的结束位“FF”。

图19为激光雷达触发信号的产生和激光雷达触发纳秒时间戳的提取与拼接时序仿真图。

图19 激光雷达触发信号产生与对应时间戳的提取拼接时序仿真图

由图19可知,激光雷达触发信号可以正常产生,且其上升沿与传感器触发信号的上升沿处于对齐状态,符合系统设计要求,同时激光雷达触发时刻对应的纳秒时间戳可以正常提取与拼接。拼接后的激光雷达触发时间戳长度共8字节,包括3字节的标识符、4字节纳秒时间戳和1字节结束位,放置于该路数据通道的buffer空间中。数据移位缓冲区从buffer空间中将拼接处理后的激光雷达触发时间戳以单字节的形式发送至预先设定好的存放激光雷达纳秒时间戳的FIFO缓冲区2中,便于后续优先级管理模块进行访问。优先级管理模块根据预先设定的优先级顺序生成访问信号对各传感器缓冲区依次进行访问,并生成串行数据包,以此方式最终实现将激光雷达触发时间戳通过USB协议发送至上位机。

图20为激光雷达触发时间戳上传至上位机的时序仿真结果,由时序仿真结果可知,激光雷达触发时间戳可以正常获取并上传至上位机,最终在上位机中完成时间戳的解析。

图20 激光雷达触发时间戳上传时序仿真图

多传感器融合系统中,激光雷达的工作频率为10 Hz,即收到FPGA发出的触发信号时,激光雷达都会以10 Hz的频率开始采集点云数据,同时激光雷达会自动保存每次采集点云数据时刻的纳秒时间戳,该时间戳为激光雷达采集时间戳,其频率为1 s产生10次,即上位机中每秒会获取10次激光雷达采集点云数据时刻的纳秒时间戳。

图21为在任意一段时间间隔内,上位机解析出的激光雷达采集时间戳与系统时间的对应关系,系统时间每秒对应10个激光雷达采集时间戳。由上述时间戳上传时序仿真结果与激光雷达采集时间戳的显示结果可知,激光雷达时间戳可以稳定地产生和获取,并上传至上位机,即激光雷达可以正常且稳定地实现同步触发。

图21 激光雷达采集时间戳与系统时间的对应关系

2 多传感器融合同步授时系统实验验证

2.1 实验平台

为了验证上述多传感器融合同步授时系统的鲁棒性和精确性,设计了如图22所示的实验平台。实验平台包括FPGA驱动板、电源模块、上位机、激光雷达、惯性测量单元IMU和相机等部分,FPGA芯片采用的是Cyclone IV系列的EP4CE10F17C8型号芯片,FPGA驱动板中包含GNSS授时模块、电源转换模块、数据接收与发送模块以及与各传感器相连的符合传感器通信协议的多种通信接口。图22中所示电源模块为整个系统的供电来源,电源模块输出22 V电压,经过降压模块降压后输出12 V电压至FPGA驱动板,以此实现对驱动板和各传感器供电,同时驱动板输出12 V电压至上位机,驱动上位机工作。该实验平台体积小、质量轻,便于手持外出采集实验数据。

图22 系统实验平台

2.2 实验验证

为了验证多传感器信息融合的有效性和同步授时的精确性,设计了如下实验:各传感器的触发信号或触发指令均由FPGA根据标准触发信号结合各传感器自身的工作频率产生,传感器接收到FPGA发出的触发信号或触发指令后开始工作,FPGA获取传感器触发时精确的纳秒时间戳和传感器采集的数据,将两者进行拼接处理,并通过USB协议上传至上位机,在上位机中实现对各传感器的纳秒时间戳与对应传感器数据拼接后的数据帧的解析与匹配。

由于各传感器的触发信号或触发指令均以FPGA内部PPS校准模块产生的标准触发信号为基准,且传感器触发时的纳秒时间戳均对应于FPGA内部系统计数器的计数值,故各传感器的数据信息处于同一时间坐标系下。同时,由于各传感器的工作频率不一致,因而上位机中应按照优先级管理模块中设定的优先级顺序进行数据的更新。

系统上电后,即使在没有GPS信号的环境下,FPGA会按照各传感器自身的工作频率输出触发信号或触发指令,各传感器也会相继开始工作。FPGA将各传感器对应FIFO缓冲区内的数据帧按照预先设定好的优先级顺序依次上传至上位机中,在上位机中对数据包进行解析,即可获得实际的时间戳与角度位姿等传感器数据信息。图23为上位机中解析出的各传感器实际工作频率下的纳秒时间戳与系统时间的对应关系,由此可知,各传感器对应的时间戳可以正常稳定地产生与获取,即各传感器可以稳定地实现同步触发。

图23 各传感器时间戳与系统时间的对应关系

同时,为了验证多传感器信息融合的有效性,上位机利用FPGA上传的数据包与激光雷达采集的点云数据对建筑物进行三维重建,三维重建效果如图24所示,由图24可知,三维重建的效果良好,多传感器信息融合的有效性和鲁棒性较高。

(a)建筑物三维重建俯视图

3 结束语

为了提高多传感器融合同步授时的精确性和信息融合的有效性与鲁棒性,同时避免户外无GPS信号环境对多传感器融合系统的同步授时乃至正常工作产生影响,设计了基于FPGA的多传感器融合同步授时系统与方法。系统以FPGA作为主控单元,通过其内部的PPS校准模块校准GNSS产生的PPS秒时间脉冲,产生精确且稳定的标准触发信号,触发校准模块以该标准触发信号作为基准结合各传感器自身工作频率产生同步脉冲触发各传感器工作,同时触发校准模块内设置延时补偿参数,可以精确补偿传感器对应的触发延迟时间。FPGA获取各传感器触发时精确的纳秒时间戳与传感器数据,拼接处理后通过优先级管理模块上传至上位机,在上位机中实现传感器数据与时间戳的精准匹配。基于FPGA的多传感器融合同步授时系统分别设计了激光雷达、IMU和相机的同步触发方案,以及多传感器同步授时与信息融合的实现方案,同时搭建了硬件系统平台,分别进行了多次仿真与实验验证。实验结果表明:本文提出的多传感器融合同步授时系统和方法可以实现多传感器的同步授时与同步数据采集,且系统工作稳定,易于实现,易于扩展,不易受GPS信号不佳等环境因素变化的影响,各传感器信息融合的有效性与鲁棒性较好,满足SLAM三维重建等应用场景对多传感器融合的实现要求。