[发明专利]一种基于因子图的卫星雷达城市峡谷车辆互定位方法

权利要求书

1.一种基于因子图的卫星雷达城市峡谷车辆互定位方法，其特征在于，采集当前时刻当前车辆的GNSS观测信息及车载毫米波雷达的量测信息，根据当前时刻当前车辆的车速里程计输出数据构建连接当前车辆相邻时刻位置信息节点的第一类因子节点，根据当前时刻当前车辆的GNSS观测信息构建连接当前车辆位置信息节点的第二类因子节点，根据当前时刻当前车辆车载毫米波雷达的量测信息及当前时刻其它车辆的GNSS观测信息构建第三类因子节点，以初始时刻至当前时刻整个因子图的误差最小为目标确定当前车辆在各时刻的位置信息；

根据所述第一类因子节点构建的指标函数为：f^{车速里程计}(x_k,z_k)＝d[x_k-h(x_k-1,v_k)]，f^{车速里程计}(·)为根据第一类因子节点构建的指标函数，x_k为根据系统状态方程更新的k时刻当前车辆的位置信息，x_k＝F(v_k)x_k-1，F(·)为系统的状态转移函数矩阵，h(·)为量测模型，x_k-1为k-1时刻当前车辆的位置信息，v_k为k时刻当前车辆车速里程计输出的车速信息；

根据所述第二类因子节点构建的指标函数为：f^GNSS(·)为根据第二类因子节点构建的指标函数，x_k为k时刻当前车辆的位置信息，d[·]为差值运算，为k时刻当前车辆的GNSS观测信息，h^GNSS(x_k)为k时刻当前车辆位置信息的量测值；

根据所述第三类因子节点构建的指标函数为：f^CAR1(·)为根据第三类因子节点构建的指标函数，x_k为k时刻当前车辆的位置信息，为k时刻其它车辆位置信息的观测值，h^CAR1(x_k,ρ_k,θ_k)为其它车辆位置信息的量测值，ρ_k、θ_k分别为毫米波雷达输出的距离和角度，v^CAR1为k时刻的观测噪声。

2.根据权利要求1所述一种基于因子图的卫星雷达城市峡谷车辆互定位方法，其特征在于，初始时刻至当前时刻整个因子图的误差最小的表达式为：为从初始时刻至当前时刻内当前车辆在各时刻的位置信息的估计值，X从初始时刻至当前时刻内当前车辆在各时刻的位置信息，f_i(X_i)为根据与i时刻当前车辆位置信息节点相连的因子节点构建的指标函数。

3.根据权利要求2所述一种基于因子图的卫星雷达城市峡谷车辆互定位方法，其特征在于，采用基于珊瑚礁优化的岛屿模型算法求解初始时刻至当前时刻整个因子图的误差最小的目标函数。

4.根据权利要求3所述一种基于因子图的卫星雷达城市峡谷车辆互定位方法，其特征在于，采用基于珊瑚礁优化的岛屿模型算法求解初始时刻至当前时刻整个因子图的误差最小的目标函数的具体方法为：将当前车辆在各时刻的位置信息映射为礁石上的珊瑚，对每一个礁石上的珊瑚随机配对，在礁石上随机取点为每对珊瑚生成的子珊瑚确定生长位置，在子珊瑚健康度优于所选生长位置的原珊瑚时，将原珊瑚替代为子珊瑚，在所选生长位置已有珊瑚或子珊瑚健康度劣于原珊瑚时重新在礁石上取点，重新取点后仍不能确定生成位置时放弃该子珊瑚，经过多代子珊瑚繁殖后，将礁石上健康度高的子珊瑚随机迁移至其它礁石后重复配对生成子珊瑚、选取生长位置的操作，直至每个礁石上珊瑚的健康度最大值和最小值之差满足预设要求，所有礁石上健康度最高的珊瑚为当前车辆在各时刻的位置信息的最优解。

5.根据权利要求4所述一种基于因子图的卫星雷达城市峡谷车辆互定位方法，其特征在于，健康度根据初始时刻至当前时刻整个因子图的误差最小的表达式计算，计算健康度的表达式为：deg_health为健康度。