其中,d为传感器的安装角度,即在水平面中传感器探测范围的中心方向与车辆行驶方向间的夹角,posx和posy为传感器在车辆坐标系中的安装位置;xveh,vxveh,axveh,yveh,vyveh,ayveh分别指车辆坐标系下目标与本车之间的纵向距离,相对纵向速度,相对纵向加速度,横向距离,相对横向速度和相对横向加速度;xsen,vxsen,axsen,ysen,vysen,aysen分别指传感器坐标系下传感器检测到的目标与传感器之间的纵向距离,相对纵向速度,相对纵向加速度,横向距离,相对横向速度和相对横向加速度;
根据融合中心接收到传感器数据的时刻与融合中心当前工作时刻之间的差值,对车辆坐标系下目标和本车之间的纵向距离,相对纵向速度,相对纵向加速度,横向距离,相对横向速度和相对横向加速度在这段时间差内发生的变化进行补偿,得到车辆坐标系下补偿之后目标与本车之间的纵向距离,相对纵向速度,相对纵向加速度,横向距离,相对横向速度和相对横向加速度构成的目标运动状态;
对各个传感器检测到的所有目标进行全局跟踪,并对重复检测到的目标进行融合,使融合中心能够得到各个传感器检测范围内的所有目标的连续运动轨迹,即运动状态,具体包含如下步骤:
将各个传感器检测到的每个目标都记为测量目标,并将每个测量目标分别与全部历史目标进行关联匹配并判断;关联匹配成功的测量目标将会用于与之匹配的历史目标的运动状态更新,关联失败的测量目标均记为备选新目标;
对于与历史目标关联匹配成功的测量目标,根据其所关联的历史目标的运动状态估计值应用卡尔曼滤波原理对该历史目标在本周期的运动状态和协方差进行估计,而且对于同时与多个测量目标关联匹配成功的历史目标还需要进行目标融合;
必一运动官网
对本周期的关联失败所形成的备选新目标和历史目标中还未判断其航迹生成成功的目标进行航迹生成判断,其中判断航迹生成成功的原则是:该目标连续3个周期与与其对应的历史目标关联匹配成功;
对于航迹生成成功的目标,之后的每个周期都要对其进行航迹消亡判断,其中航迹消亡成功的判断准则是:该目标连续5个周期与与其对应的历史目标关联匹配失败;
在全体目标中删除航迹生成失败和航迹消亡成功的目标,得到航迹管理后本周期的所有目标及其运动状态;
将航迹管理后得到的目标作为下一周期的历史目标,根据ca运动学模型对这些目标在下一周期的运动状态和协方差进行预测,并将预测结果用于步骤二中作为下一周期的目标关联,运动状态和协方差的预测公式如下:
其中,p(k+1k)分别为根据本周期的目标运动状态预测得到的历史目标的下一周期的运动状态和协方差。
以航迹管理模块输出的本周期所有目标及其运动状态作为输入,筛除其中的备选新目标,即航迹生成判断还没完成的目标;将其余目标及其运动状态作为本周期融合中心最终输出的多传感器融合的障碍物检测结果。
2.根据权利要求1所述的一种智能汽车的目标级多传感器融合方法,其特征在于所述步骤一中的时间同步是将目标和本车在融合中心接收传感器数据的时刻与融合中心当前工作时刻之间的时间差所造成的运动状态变化使用ca模型进行补偿,补偿公式如下:
其中,xcom,vxcom,axcom,ycom,vycom,aycom分别指车辆坐标系下补偿之后目标与本车之间的纵向距离,相对纵向速度,相对纵向加速度,横向距离,相对横向速度和相对横向加速度;δt表示融合中心接收传感器数据的时刻与融合中心当前工作时刻之间的时间差。
3.根据权利要求2所述的一种智能汽车的目标级多传感器融合方法,其特征在于所述步骤二的目标关联中关联匹配判断的具体过程如下:
采用马氏距离计算各个测量目标分别与前一时刻全体历史目标之间的距离,并以矩阵的形式记录,其中该距离矩阵的行数代表传感器的测量目标个数m,列数代表历史目标个数n,得到各个传感器测量目标与前一时刻全体历史目标的距离矩阵如下:
其中,g是每个历史目标的距离门限值,当某个测量目标与某个历史目标之间的马氏距离小于或等于该距离门限值时才会被关联;cij表示距离矩阵中第i行第j列元素的取值,i=1,2,...,m,j=1,2,...,n;cmax是人为设定的一个常数值,代表该测量目标在历史目标的距离门限值g之外;表示第i个测量目标与第j个历史目标之间的马氏距离值;
然后采用kuhn-munkres分配算法,以全局距离代价最小为目标,将所有满足距离门限值g的测量目标分配给历史目标,使得每个历史目标与与其对应的测量目标一一关联匹配;由于存在多个传感器,因此对每个传感器的每个测量目标都重复上述过程,若一个历史目标与多个测量目标都关联匹配成功,则该历史目标需要进行后续的融合过程,关联匹配失败的测量目标则作为备选新目标;
所述的第i个历史目标与某个传感器第j个测量目标之间马氏距离的求取方法如下:
其中,t表示转置运算,zj为第j个测量目标的运动状态,即第j个测量目标的xcom,vxcom,axcom,ycom,vycom,aycom构成的目标运动状态;zj-hxi(kk-1)表示残差,残差的协方差为s(k)=hp(kk-1)ht+r,其中h为测量矩阵,r为传感器噪声矩阵,p(kk-1)为历史目标预测值的协方差。
4.根据权利要求3所述的一种智能汽车的目标级多传感器融合方法,其特征在于所述步骤二的状态估计与新目标生成中运动状态的估计和目标融合的具体过程如下:
卡尔曼滤波中使用的运动模型为ca运动模型,故历史目标的运动状态的状态空间方程可表示为如下形式,以常加速度运动,
其中,x为历史目标在本周期的运动状态向量(包括纵向距离x,横向距离y,相对纵向速度vx,相对横向速度vy,相对纵向加速度ax和相对横向加速度ay,即x=[x,y,vx,vy,ax,ay]t);z为车辆坐标系下传感器对该历史目标运动状态向量的测量值,即z=[xcom,ycom,vxcom,vycom,axcom,aycom]t;传感器测量噪声为v(k)服从n(0,r)的正态分布,ca运动模型噪声w(k)服从n(0,q)的正态分布,q,r分别为模型噪声和传感器测量噪声的协方差矩阵,各个矩阵的具体值如下,
其中a为ca运动模型的状态转移矩阵,γ为模型噪声增益矩阵,h为测量矩阵,ts为融合中心的运行周期;
根据上述状态空间方程,使用卡尔曼滤波的方法对关联成功的历史目标在本周期的运动状态进行估计,运动状态估计的过程如下,
其中,k为卡尔曼增益矩阵,p(kk-1)分别为上一周期的历史目标的运动状态预测值和协方差预测值,p(kk)分别为本周期状态估计得到的历史目标运动状态估计值和协方差估计值;
若存在与多个测量目标都关联匹配成功的历史目标还需要对这个历史目标进行航迹融合,将与多个测量目标都关联匹配成功的历史目标的运动状态估计值融合为一个,航迹融合的公式如下,
其中,为与多个传感器测量目标都关联匹配成功的历史目标分别与关联匹配成功的第1...n个传感器测量目标所得到的本周期的运动状态估计值,p1...pn分别为的协方差;;p为融航迹融合之后的历史目标在本周期的运动状态估计值和协方差;对于关联失败的历史目标,则将其运动状态估计值作为本周期的运动状态;对于关联失败的测量目标,将其作为备选新目标,并将该测量目标对应的运动状态测量值作为该目标的初始运动状态。
本发明属于传感器融合技术领域,具体涉及一种智能汽车的目标级多传感器融合方法;将智能汽车配备的多个传感器对目标的目标级检测信息进行时空配准,即目标的位置、速度、加速度信息,通过对多传感器检测到的所有目标进行全局跟踪,并对重复检测到的目标进行融合,使融合中心能够得到各个传感器检测范围内的所有目标的连续运动轨迹,再对全体目标进行航迹管理,筛除航迹管理后本周期所有目标中的备选新目标,即航迹生成判断还没完成的目标,将其余目标及其运动状态作为本周期融合中心最终输出的多传感器融合的障碍物检测结果。本发明可实现对车辆周围目标进行稳定、准确地检测和跟踪,并且可避免对重叠检测区域的目标重复检测等问题。
无人驾驶车辆运营管理系统、总控平台、分控平台、车载计算装置和计算机可读存储介质的制造方法