[发明专利]非信控交叉口安全预警和管控方法和系统

权利要求书

1.非信控交叉口安全预警和管控方法，其特征在于，包括：

采用毫米波雷达和激光雷达获取目标的状态；对获取的目标的状态进行修正，获取修正后的目标的状态；

利用修正后的目标的状态训练获得优化后的预警模型，所述预警模型用于预测目标的最佳预警位置；

利用预警模型确定目标的最佳预警位置并根据目标的最佳预警位置发出预警信号；

对获取的目标的状态进行修正的方法具体包括以下步骤：

分别对于激光雷达以及毫米波雷达获取目标的状态进行初始化；

分别确定激光雷达以及毫米波雷达检测到的目标随时间变化的状态中不确定性程度；

利用卡尔曼滤波方法，根据激光雷达检测到的目标随时间变化的状态中不确定性程度对激光雷达感知的目标的状态进行修正获得修正后的目标的状态；

利用扩展卡尔曼滤波方法，根据毫米波雷达检测到的目标随时间变化的状态中不确定性程度对毫米波雷达感知的目标的状态进行修正获得修正后的目标的状态；

对于激光雷达以及毫米波雷达检测到的目标修正后状态趋同，则最终确定修正后的目标的状态；

其中所述目标为感知交叉口一机动车x，其实时状态为：

x＝(p_x,p_y,v_x,v_y)

其中，车辆距圆心距离用ρ表示，ρ与x方向的夹角用φ表示，距离ρ的变化速率用表示；

根据激光雷达的感知信息，其采用的是笛卡尔坐标系，能够检测到机动车x的位置，但没有其速度信息，其测量值为：

iz＝(p_x,p_y)

根据毫米波雷达的感知信息，其采用极坐标系，能够检测到机动车x的距离、角度、速度信息，其测量值为：

根据激光雷达首次感知到该机动车时的感知信息对机动车x状态进行初始化；

根据毫米波雷达在首次感知到该机动车时的感知信息对机动车x状态进行初始化；

此时激光雷达测得数据x_iz与毫米波雷达测得数据x_z之间是存在一定误差的，需对机动车x随时间变化的状态分别进行分析，才能实现有效融合；用到的公式如下：

x′＝Fx

x′指代机动车x随时间变化的状态，F 指代机动车x变化的方式，

运用激光雷达检测到的机动车x随时间变化的状态分析：

运用毫米波雷达检测到的机动车x随时间变化的状态分析：

下一步是将机动车x随时间变化的状态加以不确定性程度分析，用P′表示，其公式如下；

P′＝FPF^T+Q

P指代机动车x状态的不确定程度，以x的协方差表示，采用如下数据：

Q指代x′＝Fx中未能刻画的外界干扰，其公式如下：E表示单位矩阵：

Q＝E[cc^T]

c指代外界干扰Q的干扰量；在线性变化中，外界干扰项为加速度，干扰量c通过加速度a计算求得,其公式如下：

c服从高斯分布N(0,Q)，因此Q通过如下公式计算：

Q＝E[cc^T]＝E[Gcc^TG^T]＝GE[cc^T]G^T

激光雷达的干扰量Q_iz：

毫米波雷达的干扰量Q_z：

则激光雷达检测到的机动车x随时间变化的状态中不确定性程度P′_iz：

P′_iz＝FPF^T+Q_iz

毫米波检测到的机动车x随时间变化的状态中不确定性程度P_z′：

P_z′＝FPF^T+Q_z

将激光雷达感知的信息运用KF进行修正，首先计算出卡尔曼增益K_iz，其公式如下：

K_iz＝P′_izH^T(HP′_izH^T+R_lidar)^-1

其中，H表示激光雷达的状态空间到感知空间的线性映射；

R_lidar 表示噪声协方差矩阵，即激光雷达测量值的不确定度；

将K_iz计算出来后，利用卡尔曼增益对机动车x随时间变化的状态加以不确定性程度P′_iz进行更新，更新后的机动车x随时间变化的状态加以不确定性程度记为P″_iz，其公式如下：

P″_iz＝P′_iz-K_izHP′_iz

将更新后的机动车x随时间变化的状态加以不确定性程度记为P″_iz代入卡尔曼增益公式，更新后的卡尔曼增益记为K′_iz，其公式如下：

K′_iz＝P″_izH^T(HP″_izH^T+R_lidar)^-1

通过上述运算，更新后的机动车x的状态变量x″_iz如下：

将毫米波雷达感知的信息运用EKF进行修正，首先计算出卡尔曼增益K_z，其公式如下：

其中，H_j表示毫米波雷达的状态空间到感知空间的非线性映射f(x)线性化后的Jacob矩阵，

R_radar表示噪声协方差矩阵；

将K_z计算出来后，利用卡尔曼增益对机动车x随时间变化的状态加以不确定性程度P′_z进行更新，更新后的机动车x随时间变化的状态加以不确定性程度记为P″_z，其公式如下：

P″_z＝P_z′-K_zHP_z′

将更新后的机动车x随时间变化的状态加以不确定性程度记为P″_z代入卡尔曼增益公式，更新后的卡尔曼增益记为K′_z，其公式如下：

通过上述运算，更新后的机动车x的状态变量x″_z如下：

通过如上卡尔曼滤波与扩展卡尔曼滤波的过滤，x″_iz与x″_z应趋同，即: