智能网联车即插即用组网模型及决策融合算法

周晓春 梁 军 陈 龙 王亚飞 龚进峰

1.江苏大学汽车工程研究院，镇江，2120132.上海交通大学机械与动力工程学院，上海，200240 3.中国汽车技术研究中心汽车工程研究院，天津，300300

0 引言

高级驾驶辅助系统[1](advanced driver assistance system, ADAS)以及行车主动服务系统[2](driving active service system, DASS)的发展推动着汽车安全与服务的智能化。智能网联车(intelligent connected vehicle, ICV)已成为汽车行业未来的主要发展方向，而环境感知传感器[3](environmental sensing sensor, ESS)作为ICV的重要组成部分，在智能出行过程中发挥着重要作用。针对单点探测存在因环境条件及噪声干扰导致的感知精度低的局限[4]，国内外学者重点研究了多传感器信息融合，信息融合具体可分为数据级、特征级、决策级三部分[5]。文献[6]提出了一种基于传感器融合的障碍物检测和识别系统，使用毫米波雷达检测障碍物的位置和速度，并利用边界框回归算法精确定位和识别障碍物，结果显示该系统的检出率高达91.6%。文献[7]提出了一种基于改进的联合概率数据关联的车用多传感器跟踪融合算法并进行实车试验，结果表明前方检测目标能被稳定跟踪且具备较好的检测精度。文献[8-9]设计了两种不同的卡尔曼滤波器，分别对雷达及摄像机探测目标进行跟踪并基于贝叶斯估计进行目标融合，结果表明卡尔曼滤波可显著降低测量噪声且融合算法可提高估计精度。

上述研究都是基于固定传感器组合的，未考虑融合网络中传感器的动态可扩展，无法实现传感器的即插即用(plug-and-play，PnP)，包括组件PnP、功能PnP、通信PnP。当新增ESS以及ESS失效更新时，需进行重复繁琐的组网流程，包括搜索、识别及标定。针对上述组网难点，文献[10]提出了一种支持PnP的物联网(internet of things, IoT)设备的架构方案，即SensPnP，可实现传感器与IoT设备的无缝集成，并提出了可用于IoT自动管理所接传感器驱动程序的算法，结果表明此方案在降低成本的同时还可提高效率。文献[11]通过设计SpaceFibre光网络的网络管理器并利用宽度遍历算法检索网络设备，从而实现节点设备在总线网络的即插即用，并通过仿真试验证明了算法具有更优的分包机制及网络延时。

在上述文献基础上，本文提出了一种面向PnP传感器组网模型，通过网络自搜索(self-search，SS)-自识别(self-identification，SI)-自标定(self-calibration，SC)(简称“S-SIC”)实现ESS在ADAS域的自动接入与主动标定，并基于K-means聚类与动态加权融合算法(简称“K-m.AW”)准确预测出目标轨迹融合坐标集。该模型对降低组网难度，实现ESS即插即用，提高ADAS感知系统鲁棒性，建立ICV即插即用嵌入式软件平台具有重要意义。

1 S-SIC模型

图1所示为S-SIC模型框架,包括SS、SI及SC三部分。SS模型通过深度优先搜索规划(depth-first search, DFS)算法搜索即插ESS，并给定ESS目标ID作为下一次搜索的依据。SI模型利用搜索路径将ESS基础配置信息推送至域控制器，用于该ESS信息记录以及资源协调。SC模型以车体坐标系(vehicle body coordinate system, VBCS)为转换媒介，分别建立相机系统(camera system, CS)、毫米波雷达坐标系(millimeter wave radar coordinate system, MWRCS)、激光雷达坐标系(lidar coordinate system, LCS)的转换矩阵，实现三者之间的外参求解以及位置姿态误差补偿。

图1 S-SIC模型框架

1.1 自搜索SS算法设计

1.1.1初始化

新ESS节点接入总线网络后，域控制器DCU将自动创建一个Visited数组，用于记录所有被访问的ESS节点，其中DCU本身为初始节点。

1.1.2模型建立

将ICV上的ESS与ADAS域控制器DCU分别用字符Si与C表示。字符C表示DCU，是负责搜索的发起节点；Si(i=1,2,…,n)表示车辆上已有的ESS节点；Sm(m=n+1)表示目标节点，等待被访问。图2所示为基于DFS算法的ESS搜索模型(六节点)，黑色圆圈表示搜索的发起节点以及访问节点，灰色圆圈表示搜索的待访问节点，而白色圆圈则表示尚未被访问的节点。每进行一次搜索则代表一处节点被访问，当目标节点被访问完成后则停止搜索。表1显示了搜索完毕后各ESS节点参数值的变化情况。当除DCU的ESS参数值全为0时，表示搜索未开始；当ESS节点出现0-1变化时，表示搜索正在进行；当参数值全为1时，则表示搜索完成。具体算法流程如下：

图2 基于DFS的ICV传感器搜索模型(六节点)

表1 ESS参数值变化过程

(1)初始化DCU节点C与各ESS节点Si，其中节点C作为搜索发起节点，其值为1，其他未被访问节点的值为0。

(2)DCU节点C首次搜寻其邻近未被访问的ESS节点Sj，并对该节点进行访问，此时节点Sj值由0变化到1；以节点Sj为新起始节点进行下一轮搜索，直至访问过的节点不再有未被访问的邻近节点为止。

(3)返回前一个已访问过的且拥有未被访问邻近点的节点，继续访问该节点的下一个未被访问的邻接节点。

(4)重复步骤(2)、步骤(3)，直至目标节点Sn被访问，搜索结束。

流程图见图3。

1.2 ESS参数自识别

ESS经总线搜索后推送本身基础配置信息报文至域控制器，域控制器在获取ESS的当前状态参数后确定其功能及服务，表2所示为部分典型基础识别参数。为实现可即插即用的ESS组网，DCU须识别即插ESS位置及方位等预设参数。因存在行车振动、复杂天气等不可控性以及人为因素，使得部分参数(如时序精度Ac、采样率Sr等参数)产生区域波动进而直接影响决策级融合的准确性，如权值分配Wd及数据更新Du。适应K-m.AW决策级融合算法的二次参数Ac定义如下：

表2 典型基础参数识别列表

(1)

Sr={α，β，γ，…}

(2)

式中，Me为ESS在[T1,T2]时间序列下的最大误差；Fm为ESS满程测量范围；α,β,γ,…为Sr的多触发状态。

1.3 自标定矩阵设计

多传感器标定[12-13]是指通过一定方法寻找不同种类或是相同种类下不同探测角度传感器在时空上的对应关系，即求解传感器间的外参[14]。NOBRE等[15]提出了一种在预先不知道系统初始状态、无需标定目标或特殊初始化序列的状态下，对多传感器的内外标定参数进行定时自标定和变化检测的系统解决方案，并以相机IMU外部标定为对象验证了该系统的高可靠性。PANDEY等[16]提出了一种基于信息论、以数据驱动且无需任何基准物的自标定算法，并实验验证了该算法在机器人应用中的鲁棒性和准确性。同理，在ICV上，ESS坐标系随外界变化的不确定性会导致外参在预定范围部分微调，对该微调进行补偿是非常必要的。如图4所示，Apollo 3.5系统存在多种ESS且预安装位置分散，角度多变。研究ESS特征差异及其输出参数所依赖的坐标系成为确定转换矩阵函数及补偿算法的重点。为适应ESS即插即用及K-m.AW算法，选择VBCS为中间坐标系作为转换媒介，其他坐标系涉及CS、MWRCS以及LCS等ESS坐标系。

图4 Apollo 3.5多传感器安装位置和坐标系的俯视图

1.3.1CS与VBCS转换

VBCS是描述ICV当前道路场景及交通路况最直观的坐标体系，空间坐标可表示为(XV,YV,ZV)，其中,原点为后车桥中心位置，XV指向车辆右侧，YV指向车辆前方，ZV指向天空。CS为一种泛称，根据光心、图像位置的不同又可分为像素坐标系、图像坐标系以及相机坐标系。像素坐标系(pixel coordinate system, PCS)可表示为(u,v)，其中原点为生成图像的左上方，u指向图像的右侧，v指向图像的下方；图像坐标系(image coordinate system, ICS)可表示为(x,y)，其中,原点为相机光轴与图像坐标系的交点处，x指向图像的右侧，y指向图像的下方；相机坐标系(camera coordinate system, CCS)可表示为(XC,YC,ZC)，其中,原点OC为相机光心，XC指向光心即相机右侧，YC指向光心上方，ZC指向光心前方。转换过程如下：

(1)PCS转化为ICS，存在平移与伸缩变换(相机內CCD传感器的存在)，不涉及旋转变换，即

(3)

(2)ICS转换为CCS，存在空间维数的变化，利用透视投影实现转化过程，即

(4)

式中，f0为相机焦距，描述光心到图像中心的距离。

(3)CCS转化为VBCS，存在旋转变换及平移变换，利用旋转矩阵R和平移矩阵T实现变换，即

(5)

R=R1R2R3

(6)

(7)

(8)

式中，R1、R2、R3为绕XC、YC、ZC轴旋转θ、φ、ω角度的矩阵。

综上，转换矩阵为

[XVYVZV1]T=

(9)

1.3.2LCS与VBCS转换

VBCS转换同上，不再赘述。LCS空间坐标表示为(XL,YL,ZL)，其中,原点OL为雷达中心，XL指向输出电缆的反方向，若雷达安装在车辆后方，则YL指向车辆右侧，ZL指向天空。LCS与VBCS只存在旋转及平移变换，参考1.3.1节，转换矩阵如下：

(10)

1.3.3MWRCS与VBCS转换

VBCS转换同上，不再赘述。MWRCS与上述两种ESS的主要区别在于它是基于二维极坐标系的目标检测，原点OM为雷达扫射平面的中心点，LM为监测范围内目标物与中心点的相对距离，αM为目标物与中心点的相对角度。直角坐标系转换如下：

(11)

式中，Y0为转换后XMOMZM平面至XVOVZV平面的相对距离。

故转换矩阵如下：

(12)

1.3.4误差补偿算法设计

为补偿行车过程中因外力因素产生的传感器偏移，设计一种基于坐标反算的补偿算法，具体如下：

(13)

式中,dx、dy、dz、dθ、dφ、dω为相对预设位置的偏移量；(Xij,Yij,Zij)、(Xik,Yik,Zik)为待补偿模块i在探测范围内探测到的第j、k个目标；n为模块总探测数。

2 基于K-m.AW的决策级融合算法

如表3所示，决策级信息融合在通信带宽、环境要求、向量维数、计算资源方面体现出其他融合方式无可比拟的优势。目前，常见的用于决策级数据融合估计算法的有贝叶斯估计[17]、D-S证据理论[18]和模糊集理论[19]等。然而，贝叶斯估计属于参数化估计，其结果的准确性很大程度依赖先期假设的概率分布，若假设存在偏差,会严重影响最后的目标估计；D-S证据理论虽可避免求解先验概率，但在证据获取及计算量方面存在困难；模糊集理论通过引入隶属函数对数字化信息进行宽松建模，但算法直观性较差且运算相对复杂。针对上述不足，本文利用K-means聚类与动态加权函数(K-m.AW)进行信息融合估计。

表3 三类信息融合方法比较

聚类属于无监督学习，相比于分类，聚类不依赖预定义的类和类标号的训练实例[20]。在ICV环境感知过程中，ESS会生成一系列均匀离散的轨迹预测点，且不可避免地会造成预测偏差，无法保证感知精度，通过K-means聚类算法[21]的多次聚类可对预测集进行预测值分类并形成多类簇。在此基础上，利用基于时间序列的动态加权函数对簇预测均值进行权值分配并确定融合目标点坐标，如图5所示。

图5 基于K-m.AW融合算法的目标定位估计流程

(1)假设第i(i=1,2,…,n)个ESS在tu(u=1,2,…,N)时刻的目标坐标值为Ki(tu)，在tc(c=1，2，…，N)时刻，从Camera(CA)、Rader(RA)、Lider(LI)预测集群中随机选取3个元素作为初始预测簇中心，记为K′1、K′2、K′3。

(2)利用ADAS域控制器计算全部ESS预测对象Ki(tc)到三类簇中心K′1、K′2、K′3的相异度(dissimilarity, DI)，并将全部元素划归至相异度最低的簇。由于预测值仅存在三维坐标值的差异，故该相异度可利用不同ESS预测对象至K′1、K′2、K′3的欧几里得距离(Euclid distance, ED)来区别，定义如下：

(14)

m=1,2,3

(3)根据聚类结果，重新计算3个新簇中心，记为K″1、K″2、K″3，其三维坐标值由簇内预测值的平均数得到。

(4)将预测集中全部元素根据新簇中心点K″1、K″2、K″3重新聚类。

(5)重复步骤(4)，直到聚类结果不再变化，将结果输出，此时三类簇中心分别为K1、K2、K3。

tc时刻，三类簇中心平均值为

(15)

(16)

累计三类簇在时间序列上前tc时测量值的平均误差为

(17)

式中，y为前tc时的测量次数。

由式(14)可知，在tc时刻三类簇测量误差的标准差为

(18)

式(18)所求标准差近似为簇中心的均方差，由此可知在tc时刻，K1、K2、K3在加权融合时的权值为

(19)

由此可知最终预测点为

(20)

3 组网实验

3.1 实验设计

为验证ESS组网模型效果，包括1.1节SS算法的准确性、实时性以及1.3节SP矩阵的补偿性能，采用PreScan传感器模型中主流ESS，在相同场景下进行搜索实验并人为设置ESS位置、姿态误差以测试SP矩阵补偿性能，记录搜索结果并标定误差，具体流程如下。

(1)选取PreScan中4类ESS，包括Camera(CA)、Rader(RA)、Lider(LI)、Ultrasonic(UI)，并进行编号，记为N1、N2、N3、N4。

(2)将CA接入ESS网络，利用DFS算法进行全局搜索，若搜索成功，则记录时间T；反之记录时间为∞。重复搜索10次，记录搜索成功次数δ1、搜索失败次数δ2及时间Ti(i=1,2,…,10)。

(21)

(22)

j=1,2,3,4

(4)为模拟环境及车体造成的影响，人为设置4类ESS定误差ΔF，p个探测目标(p≥2)，基于补偿算法求解误差补偿值∂F，ESS参数设置如表4所示，统计相对误差Re，并取相对平均误差M(Re)为评价层标准，定义如下：

表4 ESS参数设置

(23)

(24)

式中，ΔF[n]为矩阵ΔF=[ΔxΔyΔzΔθΔφΔω]的第n个参数；F[n]为预设位置姿态矩阵∂F=[∂x∂y∂z∂θ∂φ∂ω]的第n个参数；N(Re)为Re数组量，取N(Re)=6。

实验框架如图6所示。

图6 ESS组网实验框架

3.2 实验结果及分析

表5 10组实验搜索结果

(a)摄像头CA

补偿误差结果如图8所示，其中：

图8 ESS相对误差Re(SC)

(25)

n=1,2,…,6

符合标准设定，使得补偿算法在预测∂F=[∂x∂y∂z∂θ∂φ∂ω]多维度上具备良好的补偿性能且矩阵M处于M(Re)标准值中段，使自标定SC存在一定的冗余性，保证误差在可控范围。其中，矩阵M为

(26)

4 融合估计实验

4.1 实验设计

为验证面向PnP的K-m.AW算法的性能，采用PreScan-Simulink仿真平台，利用PreScan传感器模型中典型的CA、LI、RA三类ESS搭配在特定场景及工况下进行前方目标轨迹估计，场景设置见图9，包含直道S1、弯道S2、上下坡S3三类典型场景，实验参数设置如表6所示。

表6 实验参数设置表

图9 PreScan场景示意图

为模拟环境要素对RA、LI的影响，实验引入高斯噪声G，其中，方位角GaAzimuth=0.80°,方程GElevtion=0.50°,范围GRange=0.7 m。

为比较算法的适用程度，在相同场景工况下采用两类对比算法，包括贝叶斯估计、模糊集理论，其中，先验概率P(i)={0.1，0.3，0.6},i=CA,RA,LI，隶属度函数置为LR型。

如图9所示，目标车辆(intention vehicle, IV)依照场景及工况设置由路段节点1行驶至节点7，其中，1～2、3～4、5～6为直道场景；2～3、6～7为弯道场景；4～5为上下坡场景。ICV搭载上述配置传感器进行前方目标车辆的轨迹估计，并保证目标车辆全程在ICV监测范围内，从Simulink后端采集目标车辆的实际行车轨迹及预测轨迹。为测试即插即用组网模型在轨迹估计方面的有效性及优越性，利用上述随机模块在全路段接入(connect, C)或断连(disconnect, D)三类模块，并统计有效测试结果及实验过程中三类ESS在相同时序前Tc不同场景下的平均误差率Ar|t=Tc及不同时序前Tc同一场景下的平均误差率Ar|t=Tc，i(i=1,2,…,n)，匹配多场景下的Tc和Ar的最优区间。

4.2 实验结果及分析

(a)CA直道场景(A1) (b)CA弯道场景(B1)(c)CA上下坡场景(C1)

表7 PnP统计数量及平均误差率

图11描述了10 s内Tc与Ar的匹配值及局部最优区间，结果显示直道场景下Tc对Ar的影响最小，表明该类场景下ESS探测的误差较小且趋近稳定；而弯道及上下坡场景下不同Tc对Ar影响较为明显，其中ESS探测弯道及上下坡场景的Tc的局部最优区间分别为[2.1 s,3.3 s]、[1.6 s,2.8 s]，表明在弯道及上下坡的场景工况下ESS探测平均误差较直道场景工况波动较大，可推断时间序列中初始时间戳误差估计数据集对当前时间戳的融合误差影响较为明显，应降低其所占权重，而末端时间戳的误差数据集影响较小，所占权重应适当增加。结果验证了在不同场景下K-m.AW算法在动态权值选取时的合理性。

(a)直道场景

5 结论

(1)建立了S-SIC组网模型，通过自搜索(SS)-自识别(SI)-自标定(SC)实现环境感知传感器(ESS)的组网流程与即插即用，并在此基础上提出了基于时间序列上平均误差的K-m.AW融合算法，提高了目标轨迹估计的准确性。

(2)以S-SIC模型为基础，依据4类PreScan常用ESS进行了即插即用的SS及SC补偿实验，并利用直道-弯道-上下坡进行了多场景的融合估计验证，结果表明，该模型可有效提高ESS的组网效率，搜索成功率与搜索平均时间控制在92%和1.79s；以前方单目标轨迹估计为例，该算法在融合估计中较对比算法体现出较为明显的优势；实验统计出的多场景Tc最优区间对算法动态权值的选取具有重要参考价值；同时在多场景中随机实现即插即用，进一步验证了ESS组网模型的适用性及K-m.AW算法在轨迹估计中的有效性。

(3)在后续研究中，可进行多种类、多数量ESS横纵向对比实验去验证ESS组网模型的稳定性以及分析研究K-m.AW融合算法在多角度、多目标轨迹估计预测的准确性及可靠性。