一种面向城市复杂环境的GNSS/UWB鲁棒因子图优化协同定位方法

本发明属于智能交通与车辆高精度定位，具体涉及一种面向城市复杂环境的gnss/uwb鲁棒因子图优化协同定位方法。

背景技术：

1、车辆定位作为智能交通系统（intelligent transportation systems，its）的重要组成部分，在自动驾驶、车道级导航和编队驾驶等应用中占据重要地位，准确可靠的位置信息是实现上述应用的基础。全球导航卫星系统（global navigation satellite system，gnss）凭借着高精度、全球覆盖和实时性强等优点被广泛应用于车辆定位，但在城市复杂环境下，gnss面临卫星数量减少、非视距和多路径效应严重等问题，导致其无法提供稳定可靠的定位结果。随着车载通信技术的快速发展，通过共享与融合多车观测数据的协同定位（cooperative positioning，cp）技术成为一项主要的研究方向。与单节点定位相比，cp方法可以从车辆之间的信息交互中获取更多的观测信息，从而突破个体观测的限制，实现更高的定位精度和可用性。

2、cp系统中包含多种信息来源，节点间的相对测距是其重要组成部分，通常由无线电测距设备获取。其中，超宽带（ultra wide band，uwb）技术通过纳秒级的非正弦波窄脉冲传输数据，有着数据传输速率快、成本低和测距精度高等优点，可为cp系统提供高效且精确的测距通信服务。在cp算法方面，基于扩展卡尔曼滤波（extended kalman filter，ekf）、置信度传播（belief propagation，bp）、因子图优化（factor graph optimization，fgo）等多种状态估计算法广泛应用到cp系统中。其中，fgo算法能够通过多次迭代重复线性化来降低非线性误差，且能重复利用测量数据之间的时间相关性，在定位精度方面展现出一定优势。zhuang c等人从几何约束的角度出发，利用协同车辆的位置信息拟合道路平面，将平面约束作为额外因子引入fgo框架中，有效缓解了车间测距中断带来的性能退化问题。zhang g等人提出了一种3d建筑模型辅助gnss的cp方法，利用射线追踪技术对nlos和多径误差进行修正，并通过fgo融合绝对定位、相对定位和位移约束以实现多节点联合优化，在城市峡谷场景中取得了优于传统gnss双差方法的定位精度。hongbo z等人提出了一种无人飞行载具（unmanned aerial vehicle，uav）辅助的聚类优化协同定位策略，通过引入无人机作为空中中继节点，解决了城市环境中因建筑物遮挡导致cp网络分裂为多个孤立子簇的问题。然而，上述方法大多侧重于增加观测约束信息，缺乏对观测数据异常值的处理，对于fgo的鲁棒性考虑不足。为提升城市复杂环境下cp系统的鲁棒性，可通过稳健估计策略来抑制异常值对状态估计的影响。m估计作为一种典型的稳健估计方法，采用稳健代价函数代替最小二乘中的二次代价以此来增加系统鲁棒性，其方法在gnss定位和cp系统中都得到了广泛的应用。然而，大多数的稳健代价函数如hubre函数、cauchy函数和geman-mcclure函数等都是非凸函数，直接将其优化目标函数会出现局部最优问题，即正常观测值被误判剔除、异常值反而被保留，导致状态估计向错误方向收敛。为降低陷入局部最优的风险，渐进非凸（graduate non-convexity，gnc)优化方法被应用于cp系统中。gnc算法通过引入控制参数来控制代价函数的非凸性，该控制参数随着迭代进行更新，逐渐将代价函数由凸函数逐渐恢复到初始的非凸函数，从而在保证系统鲁棒性的情况下有效降低陷入局部最优的风险。chitturi s等人在图像平均领域提出了基于图拉普拉斯谱特性的自适应gnc策略，有效提升了大规模问题解算效率与精度平衡，但该方法尚未被引入cp系统中。wang j等人将welsch代价函数替代fgo中传统的最小二乘代价，通过gnc方式规避局部最优，使cp系统在高异常值占比下仍能对异常值正确降权，显著提升了cp系统的鲁棒性。zhao h等人将截断最小二乘代价函数引入fgo协同定位框架，并采用gnc策略克服截断最小二乘代价函数非凸性带来的局部最优问题，在gnss/uwb多源融合cp系统中实现了对异常观测的有效抑制。但上述两种方法均采用固定因子的gnc方法，即按照预设的比例逐步递减控制参数，导致其无法在精度和计算效率方面取得平衡。

3、针对城市环境下cp系统易受测量异常值干扰导致定位精度与可靠性严重下降问题，本发明提出一种面向城市复杂环境的gnss/uwb鲁棒因子图协同定位方法。首先构造抗差代价函数，将残差分为正常、可疑和粗差三个区间，不同区间采用不同惩罚策略，实现fgo过程中异常值的降权处理；在此基础上将抗差代价函数融入gnc框架，并基于black-rangarajan对偶关系推导对应的抗差权函数，将稳健优化问题转化为加权最小二乘与权函数更新的交替迭代，有效降低了直接优化非凸代价时的局部最优问题；再根据残差分布特征构建融合hessian矩阵正定性回调机制的自适应gnc策略实现控制参数的动态调节，在维持优化稳定性的同时加速收敛进程。

技术实现思路

1、针对城市复杂环境下车辆协同定位易受gnss伪距异常、uwb测距误差及多路径效应影响，导致传统因子图优化方法鲁棒性不足、直接优化非凸抗差代价函数易陷入局部最优的问题，本发明提出一种面向城市复杂环境的gnss/uwb鲁棒因子图优化协同定位方法。该方法从理论层面实现了多源观测融合、鲁棒估计与自适应优化的深度结合，为城市复杂环境下协同车辆的高精度、可靠定位提供保障。

2、一种面向城市复杂环境的gnss/uwb鲁棒因子图优化协同定位方法包括以下步骤：

3、步骤1、构建gnss/uwb协同定位因子图模型，将滑动窗口内多历元、多车辆节点的三维位置作为全局待估状态，基于gnss伪距双差、车辆间uwb测距和相邻历元速度约束构建误差函数，并形成协同定位非线性最小二乘目标函数；

4、步骤2、构建抗差代价函数，根据标准化残差分布将观测划分为正常域、可疑域和粗差域，分别保留、降权或剔除观测值，并引入控制参数调节代价函数非凸程度，为后续渐进非凸优化提供鲁棒代价函数；

5、步骤3、通过black-rangarajan对偶关系构建抗差权函数，将鲁棒因子图优化转化为状态变量与观测权重的联合优化；设置观测初始权重，基于常规fgo获得初始状态，并依据初始残差完成异常触发判断与控制参数初始化；固定状态时更新观测权重，固定权重时更新车辆节点状态，实现异常观测降权或剔除；

6、步骤4、提出融合hessian矩阵正定性回调机制的自适应gnc策略，根据残差临界控制参数的百分位数生成候选参数，并利用hessian正定性或cholesky分解进行检验，动态接受或回调控制参数，直至输出最终协同定位结果。

7、步骤1所述的构建gnss/uwb协同定位因子图模型，将滑动窗口内多历元、多车辆节点的三维位置作为全局待估状态，基于gnss伪距双差、车辆间uwb测距和相邻历元速度约束构建误差函数，并形成协同定位非线性最小二乘目标函数；

8、具体步骤如下：

9、步骤1-1、在fgo中，将滑动窗口内多个历元的待估状态共同组成全局状态向量，定义为：

10、（1）

11、式中，代表当前历元，代表滑窗长度，代表历元的待估计状态，定义为：

12、（2）

13、式中，为车辆节点编号，表示当前历元下车辆节点在地心地固坐标系下的的三维位置。

14、步骤1-2、构建gnss伪距双差因子，车辆节点在历元的gnss伪距观测方程可表示为：

15、（3）

16、式中，表示卫星在频点上与车辆节点之间的伪距观测值，表示节点与卫星之间真实的几何距离，表示接收机钟差，表示卫星钟差，表示对流层延迟，表示电离层延迟，表示由非视距、多路径和伪距噪声引起的其他误差。

17、引入基准站，得到车辆节点即流动站与基准站伪距双差观测方程：

18、（4）

19、式中，表示双差运算，和表示参考卫星和非参考卫星。通过双差运算可以消除接收机钟差和卫星钟差。由于卫星到流动站和到基准站的gnss信号传播路径相似，双差处理能够显著削弱电离层延迟和对流层延迟的影响。当流动站与基准站之间的距离小于10公里时可忽略双差电离层延迟和双差对流层延迟。因此，在短基线情况下，伪距双差观测方程可简写为：

20、（5）

21、由式（5）可得车辆节点与基站之间双差伪距误差函数为：

22、（6）

23、式中，，表示参考卫星和非参考卫星在地心地固坐标系下的三维位置，可通过广播星历获取。

24、在协同车辆网络中，每台车辆也可以与其他车辆构成伪距双差观测方程，由式（5），（6）可推出车辆节点与车辆节点之间的伪距误差函数为：

25、（7）

26、需注意，在城市峡谷等复杂环境，伪距观测值易受到严重的非视距延迟和多路径效应影响，导致显著增大，双差处理并不能消除这类误差带来的影响。

27、步骤1-3、在协同车辆网络中，通过uwb获取车辆之间的距离构建uwb测距因子。对于节点和节点之间的uwb测距观测方程为：

28、（8）

29、式中，是通过uwb测量得到的车辆间距离，是车辆之间的真实距离，是由于非视距、多路径效应和uwb噪声引起的测距误差。

30、由式（8）可推出车辆节点和车辆节点之间的测距误差函数为：

31、（9）

32、当车辆之间存在遮挡时，uwb测距会受非视距和多路径效应影响严重，导致误差项会显著增大。

33、为在因子图中建立多个历元之间的时间相关性，引入gnss测速结果对相邻历元的车辆节点位置进行约束形成历元间速度约束因子。对于车辆节点，可列出观测方程如下：

34、（10）

35、式中，为gnss多普勒观测值测速结果，为相邻历元时间间隔。由式（10）可推出速度误差函数为：

36、（11）

37、结合上述所有误差函数，基于fgo的车辆协同定位的目标函数可表示为非线性最小二乘优化：

38、（12）

39、式中，代表全局状态的最优估计，代表所有共视卫星的集合，代表所有车辆节点的集合，和代表车辆节点与基站之间和车辆节点之间的双差伪距观测值的协方差矩阵，代表车辆节点之间距离观测值的协方差矩阵，代表历元间速度约束的协方差矩阵。

40、步骤2所述的构建抗差代价函数，根据标准化残差分布将观测划分为正常域、可疑域和粗差域，分别保留、降权或剔除观测值，并引入控制参数调节代价函数非凸程度，为后续渐进非凸优化提供鲁棒代价函数；

41、具体步骤如下：

42、步骤2-1、在城市复杂环境中，测量残差随着观测条件的变化呈现出不同的趋势：观测条件良好时残差分布接近噪声水平，随着观测条件的恶化残差也随之增大，部分情况下因非视距和多路径效应误差过大导致残差严重恶化，单一形式的鲁棒代价函数通常难以同时兼顾小残差区间的保留权重、中等残差区间的平滑降权以及大残差区间的强抑制需求。为解决该问题，本文将残差域划分为正常域、可疑域和粗差域三个区间，各区间采用不同的残差处理策略，形成抗差代价函数。同时抗差代价函数作为非凸函数，直接优化非凸代价存在陷入局部最优的风险，而gnc通过引入一个控制参数，将代价函数从凸函数出发，逐步恢复到原始非凸代价来降低陷入局部最优的风险。将引入控制参数的抗差代价函数替换式（12）中的二次代价函数，得到：

43、（13）

44、步骤2-2、表示添加控制参数后的可调非凸性的抗差代价函数。可调非凸性的抗差代价函数和抗差代价函数表示形式如下：

45、（14）

46、（15）

47、式中，表示标准化残差，，为两个判断阈值，一般按经验设定为，，为调节非凸性的控制参数，标准化残差可由下式计算得到：

48、（16）

49、式中，表示所有误差函数计算得到的残差，为当前所有残差的中误差，，代表取中值，为中位数绝对偏差的归一化系数。

50、步骤3所述的通过black-rangarajan对偶关系构建抗差权函数，将鲁棒因子图优化转化为状态变量与观测权重的联合优化；设置观测初始权重，基于常规fgo获得初始状态，并依据初始残差完成异常触发判断与控制参数初始化；固定状态时更新观测权重，固定权重时更新车辆节点状态，实现异常观测降权或剔除；

51、具体步骤如下：

52、步骤3-1、对于式（13）很难通过直接求解目标函数来实现优化，为此可借助black-rangarajan对偶性，可将式（13）中的鲁棒优化问题转化为：

53、（17）

54、式中，，，和分别代表车辆与基站之间双差伪距测量值，车辆之间双差伪距测量值，uwb测距测量值和历元间速度约束对应的权重，所有权重的取值范围都是0到1之间。代表所有权重组成的集合，表示所有权重的最优解。表示对权重的惩罚函数，其表达形式取决于代价函数可由black-rangarajan对偶性推导得到。

55、步骤3-2、在初始阶段将所有观测值权重设置为1，代表所有初始权重的集合，在进行自适应gnc解算之前，先进行一次fgo的非线性最小二乘解得到即初始全局状态，通过计算所有因子的标准化初始残差。

56、为避免在观测质量较好时引入不必要的鲁棒迭代，本文在自适应gnc优化前设置基于阈值的异常触发判据。基于对残差整体分布和局部异常的综合考虑，对按照从小到大排序，取第95百分位数与最大残差分别与，进行比较：

57、（18）

58、当满足式中任意条件时，认为当前历元中存在需要鲁棒处理的异常观测，进入自适应gnc优化过程；否则认为绝大多数观测仍处于的二次区间内，其权重保持为1，直接采用常规fgo解算结果作为当前历元输出。

59、控制代价函数非凸性的控制参数初始化为，为保证初始阶段所有残差位于抗差代价函数的凸函数部分需满足：

60、（19）

61、式中，表示因子残差的百分位数。因此，初始控制参数可取为：

62、（20）

63、此时所有残差都位于凸区间，目标函数近似为凸函数，有利于获得稳定的初始收敛方向，之后进行自适应gnc解算。

64、步骤3-3、在第次自适应gnc迭代中，固定上一轮估计得到的全局状态、控制参数，通过最小化式（17）所示目标函数中与权重变量相关的部分，更新各因子的权重。由于各因子权重变量之间相互独立，因此全局权重更新问题可分解为若干个独立的一维优化问题。由求解的所有因子残差其对应当前迭代权重的目标函数可表示为：

65、（21）

66、根据black-rangarajan对偶关系，最优权重满足：

67、（22）

68、式中，的一阶导数为：

69、（23）

70、则当前迭代最优权重即抗差权函数可表示为：

71、（24）

72、将带入式得到当前迭代最优权重。

73、权重更新完成之后，获得，，和，即各类因子所有观测值的权重，并在权重固定的条件下进行全局状态更新，目标函数为：

74、（25）

75、由于权重惩罚项仅与权重有关，独立于全局状态，因此在状态更新阶段可将其从目标函数中移除。此时，鲁棒优化问题转化为加权非线性最小二乘优化问题。

76、步骤4所述的提出融合hessian矩阵正定性回调机制的自适应gnc策略，根据残差临界控制参数的百分位数生成候选参数，并利用hessian正定性或cholesky分解进行检验，动态接受或回调控制参数，直至输出最终协同定位结果。

77、具体步骤如下：

78、步骤4-1、状态更新完成后需要根据当前残差分布对控制参数进行自适应更新，自适应gnc的核心在于，如何保证在优化状态向全局最优状态进行收敛的前提下，使尽可能快速的减小到1，恢复原始。

79、在第次状态更新完成后根据当前全局状态重新计算所有因子的标准化残差，对应任意因子残差定义其进入降权区间的临界控制参数为：

80、（26）

81、将所有因子对应的临界控制参数构成集合：

82、（27）

83、式中，表示所有因子残差集合。本文不采用缩减因子来直接缩小，而是根据集合的百分位数确定候选控制参数。令表示集合的第百分位数，则第次迭代的候选控制参数定义为：

84、（28）

85、步骤4-2、百分位数用于控制每次迭代中允许进入降权区间的残差比例。在初始阶段令此时：

86、（29）

87、上式结果与初始化阶段定义的结果相同，在实际计算时，初始化采用式（20）计算。

88、为避免控制参数下降过快导致目标函数非凸性过强，本文构建当前候选控制参数下的hessian矩阵并进行正定性检验，判断鲁棒目标函数在当前估计值附近是否保持稳定的局部二阶近似。hessian矩阵是鲁棒目标函数关于状态变量的局部二阶曲率矩阵，表示形式为：

89、（30）

90、式中，为所有因子的雅可比矩阵，为基于二阶导数构成的权重矩阵。当前候选控制参数对应的二阶导数可表示为：

91、（31）

92、通过hessian矩阵判断当前控制参数下目标函数在当前估计值附近是否保持局部正定表示形式为：

93、（32）

94、式中，为正定性判断阈值，若满足上式，则说明在候选控制参数下，目标函数在当前状态附近保持局部正定性，认为当前目标函数非凸性程度可以接受。在实际计算中，可通过cholesky分解进行等价判断：

95、（33）

96、若cholesky分解成功，则认为hessian矩阵满足正定性要求，此时接受候选控制参数，并在下一次迭代中继续减小百分位数：

97、（34）

98、（35）

99、式中，表示百分位数的最小取值，本文中其意义在于避免超过一半的观测因子在同一阶段被同时降权，能够保留多数观测对状态估计的约束作用，降低hessian矩阵非正定风险，提高自适应gnc过程的稳定性。表示前进步长，用于控制目标函数非凸性增强速度，本文中。

100、步骤4-3、当hessian矩阵不满足正定性条件或cholesky分解失败即：

101、（36）

102、当过小时，目标函数的非凸性过强，可能导致优化陷入局部最优情况。此时应回调百分位数，使候选控制参数增大：

103、（37）

104、式中，为上一次迭代已接受的百分位数上界，为回退步长，本文中。回调后通过式（30）重新计算，并再次进行hessian矩阵正定性检验，直到通过计算的hessian矩阵满足正定性要求。

105、当等于1时，采用原始的抗差代价函数进行权重更新和状态更新，认为优化完成，输出最终估计结果。

106、本发明有益效果：

107、1、为提高城市复杂环境下车辆协同定位的观测利用能力，本发明构建gnss/uwb协同定位因子图模型，将gnss伪距双差、车辆间uwb测距和相邻历元速度约束统一融合，充分利用多车辆、多历元观测信息，有效提升协同定位精度与可用性。

108、2、针对gnss伪距异常、uwb测距误差及多路径效应导致的定位结果偏差问题，本发明提出一种抗差代价函数，将标准化残差划分为正常域、可疑域和粗差域，对不同观测分别保留、平滑降权或剔除，有效抑制异常观测对状态估计的影响。

109、3、为降低直接优化非凸抗差代价函数易陷入局部最优的风险，本发明基于black-rangarajan对偶关系构建抗差权函数，将鲁棒因子图优化转化为状态变量与观测权重的联合优化，通过权重更新与状态更新交替迭代，提高了鲁棒优化过程的稳定性与可解性。

110、4、为兼顾定位精度与计算效率，本发明提出融合hessian矩阵正定性回调机制的自适应gnc策略，根据残差分布动态调节控制参数，并通过hessian正定性或cholesky分解检验控制非凸程度，在保证优化稳定性的同时减少迭代次数，有效提升城市复杂环境下协同定位的实时性与可靠性。