车辆行驶过程中如何获得准确位置信息？——GNSS PVT POS 算法（3）

四. 历元间差分算法

在之前的文章中有提到双核或者双任务场景下，针对RTK耗时较长、内存空间存储有限等问题，会在PVT任务中基于RTK上报的高精度定位点信息差分出实时高频的定位结果信息，在此过程中使用到的算法就是历元间差分算法。

4.1 载波历元间差分算法（TDCP，Time-Difference Carrier-Phase）

对于伪距、载波都可以使用其来做历元间差分，考虑到定位精度基本要和RTK定位精度保持一致，所以一般情况下采用的都是载波历元间差分算法。

从载波观测量公式入手：

简化为：

忽略电离层I、对流层T以及多路径等其他因素的影响，将历元间做差的载波计算公式整理为：

其中，最难确定的部分是整周模糊度部分，所以在整个计算过程期间，前后两个历元要针对其是否存在半周跳、周跳等现象做严格判断和限制；假设在无周跳无半周跳的情况下，前后历元的N应该是相等的，所以上述公式整理为：

其中，为已知值，待求量为；

将公式整理为：

该公式的左边可以为未知数部分，右侧为可以计算部分，列为，并将公式左侧展开：

该公式变种后与伪距求解位的最小二乘公式的形式基本一致，可将所求在处进行泰勒展开，则将上述公式更改为：

其所求参数也与伪距求解计算量一致，即：

，考虑到此处是近似化展开，所以还是需要多次LSQ迭代达到退出条件，才能最终输出结果。

4.2 文字端的具体实现

举例：假设实时运行频率为5hz，即PVT时间点为0.2、0.4、0.6、0.8、1.0……，RTK只有在整秒处触发，在低优先级或者在其他核运行，其运行时间为380ms，则其在第t+0.38s的时候输出的是第t+0s的定位点结果，那就可以在t+0.4ms的时候使用，即可开始进行TDCP算法的整个维持。

实时条件1：第1个RTK点是固定点，即第0.38s的时候输出的是第0s的定位点，可以在第0.4s的时候使用，那么第0.4s、0.6、0.8、1.0、1.2、1.4如何维持？

（1）第0.2spvt计算后，基于pvt_vel速度*时间信息更新delta_X_0.2，将其存储;

（2）第0.4spvt计算后开始TDCP解算，基于X_pvt_0.4计算TDCP_X_0.4，基于X_rtk_0.0更新：

X_rtk_0.0+delta_X_0.2+TDCP_X_0.4=X_0.4，并且将TDCP_X_0.4存储；

（3）第0.6s，先pvt再TDCP解算，基于X_pvt_0.4计算TDCP_X_0.6，基于X_rtk_0.0更新：

X_rtk_0.0+delta_X_0.2+TDCP_X_0.4+TDCP_X_0.6=X_0.6，并且将TDCP_X_0.6存储；

》》由此类推到第1.2s

（4）第1.2s，先pvt再TDCP解算，基于X_pvt_1.0计算TDCP_X_1.2，基于X_rtk_0.0更新：X_rtk_0.0+delta_X_0.2+TDCP_X_0.4+TDCP_X_0.6+TDCP_X_0.8+TDCP_X_1.0+TDCP_X_1.2=X_1.2，并且将TDCP_X_1.2存储；

》》第1.4s的时候拿到第1.38s计算的第1.0s的RTK结果！！

（5）第1.4s，先pvt再TDCP解算，基于X_pvt_1.2计算TDCP_X_1.4，基于X_rtk_1.0更新：X_rtk_1.0+TDCP_X_1.0+TDCP_X_1.2+TDCP_X_1.4=X_1.4，并且将TDCP_X_1.4存储；

（5）第1.6s，先pvt再TDCP解算，基于X_pvt_1.2计算TDCP_X_1.6，基于X_rtk_1.0更新：X_rtk_1.0+TDCP_X_1.0+TDCP_X_1.2+TDCP_X_1.4+TDCP_X_1.6=X_1.6，并且将TDCP_X_1.6存储；

后续和上述过程一致，即依据PVT的结果更新本历元的TDCP结果，依据最近的RTK定位结果以及时间差范围内的TDCP结果更新最终的定位结果。

实时条件2：在第0.38s的时候输出第1个RTK固定点，在1.38s和第2.38s时RTK均是单点/不定位状态的解，那如何设置使用第一个点去类推的范围呢？

（1）一般情况下设置在5s以内均可，假设长时间内一直都没有再固定，需要将存储的点信息清除，期间的解状态可以为固定，也可以设置为其他；

（2）一般情况下需要暂存5次RTK定位点信息，防止假设在1.38s更新的非固定点，还是要使用第0.38s的点去类推的情况。

实时条件3：在第0.38s的时候输出第1个RTK固定点，在1.38s和第2.38s时RTK均是浮点解/dgnss解，那是否要使用呢？

（1）使用，但是在计算的时候需要基于存储的几个RTK输出的点，基于时间差范围内的TDCP解都分别累计出一个解，比如在第2.6s计算，依据X_RTK_0.0（从第0.0累积到2.6）计算出X_1，依据X_RTK_1.0（从第1.0累积到2.6）计算出X_2，依据X_RTK_2.0（从第2.0累积到2.6）计算出X_3，对X_1、X_2、X_3根据定位解以及时间差的大小可以分配不一样的置信度，比如X_2..6=0.4*X_1+0.3*X_2+0.3*X_3，但是解状态最新的RTK解状态一致；

（2）不使用，假设是车辆运行变化状态比较快的状态下，还是要使用前几秒的信息去更新迭代，会存在一定的滞后性，可以直接使用最近的RTK解信息和PVT解信息的融合使用，也是和（1）一样，使用不一样的置信度比例；

实时条件4：比如没有周跳的卫星数量较少，有效星历较少，残差值异常，导致实时中的某个历元TDCP LSQ迭代解失败，那本历元的TDCP_X如何计算呢？

（1）依据pvt计算的速度、加速度信息去维持，但是要确保定速的质量！

（2）如果有其他传感器，比如IMU信息，可以使用IMU输出的速度信息去维持；

实时条件5：LSQ解跳的太严重，噪声较大，该如何处理？

（1）引入pos kalman滤波，定权R模块和Q模块可以直接类比于PVT 部分；

（2）从上述了解可以看到，TDCP失败后使用的还是PVT的速度类推，可以在估算模块引入加速度估算；

4.3 代码实现

注：该部分代码仅为个人梳理实现，并不保证代码的可正确运行性！！！

typedef struct { unsigned char spp_valid;//1:valid unsigned char spp_quality;//1:valid; double vel_xyz[3]; double acc_xyz[3]; double pos_xyz[3]; double vel_std[3]; double tow; } spp_sol_t,*pspp_sol_t; typedef struct { unsigned char rtk_valid;//1:valid unsigned char rtk_status;//1:spp,2:dgnss;4:float;5:fix double pos_xyz[3]; double rtk_std[3]; double tow; } rtk_sol_t,*prtk_sol_t; typedef struct { unsigned char ins_valid;//1:valid double vel_xyz[3]; double acc_xyz[3]; double pos_xyz[3]; double vel_std[3]; double tow; } ins_sol_t,*pins_sol_t; typedef struct { unsigned char tdcp_valid;//1:valid double delta_xyz[3]; double tow; } tdcp_lsq_t,*ptdcp_lsq_t; typedef struct { unsigned char sol_status; unsigned char tdcp_iter; spp_sol_t spp_sol; ins_sol_t ins_sol; rtk_sol_t rtk_sol[5];//save recent rtk sol info,[0]表示的是最新的rtk结果 tdcp_lsq_t tdcp_lsq[50];//save 50 tdcp sol info/pvt vel info tdcp_lsq_t tdcp_lsq_cur;//current tdcp lsq info double pos_std[3]; double pos_xyz[3]; double tow; } tdcp_sol_t,*ptdcp_sol_t; // unsigned char tdcp_sol_pro(tdcp_sol_t tdcp_sol){ unsigned char valid=0; //可以直接参照RTKLIB部分 //1.判断前后历元的卫星载波是否存在周跳,获取有效卫星数量 //2.计算卫星位置和钟差 //3.计算L1-L2的部分相关信息，最终得到delta_L //4.lsq解算模块 valid= tdcp_lsq(); return valid; } unsigned char tdcp_preocess(double obs_delta_t, tdcp_sol_t tdcp_sol){ unsigned char i, j, k, tdcp_source=0,sol_status[5], store_iter=0, sol_suceess=0; double delta_t, delta_xyz[5][3],pos_xyz[3],scale[5]; memset(sol_status,0,sizeof(sol_status)); memset(delta_xyz,0,sizeof(delta_xyz)); memset(scale,0,sizeof(scale)); memset(tdcp_sol.pos_xyz,0,sizeof( tdcp_sol.pos_xyz)); tdcp_sol.sol_status=0; if(tdcp_sol.tow<=0)return 0; //更新本历元的tdcp信息 memset(tdcp_sol.tdcp_lsq[tdcp_sol.tdcp_iter],0,sizeof(tdcp_sol.tdcp_lsq[tdcp_sol.tdcp_iter])); if(tdcp_sol_pro(tdcp_sol)){ memcpy(tdcp_sol.tdcp_lsq[tdcp_sol.tdcp_iter],tdcp_sol.tdcp_lsq_cur,sizeof(tdcp_sol.tdcp_lsq_cur)); } else{ if(tdcp_sol.spp_sol.spp_valid && tdcp_sol.spp_sol.spp_quality){ for(i=0;i<3;i++){ tdcp_sol.tdcp_lsq[tdcp_sol.tdcp_iter].delta_xyz[i] = tdcp_sol.spp_sol.vel_xyz[i]*obs_delta_t; } tdcp_sol.tdcp_lsq[tdcp_sol.tdcp_iter].tdcp_valid=1; tdcp_source=2; } else if(tdcp_sol.ins_sol.ins_valid){ for(i=0;i<3;i++){ tdcp_sol.tdcp_lsq[tdcp_sol.tdcp_iter].delta_xyz[i] = tdcp_sol.ins_sol.vel_xyz[i]*delta_t; } tdcp_sol.tdcp_lsq[tdcp_sol.tdcp_iter].tdcp_valid=1; tdcp_source=3; } } //判断本历元tdcp的来源 if(tdcp_source>=1){ tdcp_sol.tdcp_lsq[tdcp_sol.tdcp_iter].tow=tdcp_sol.tow; tdcp_sol.tdcp_iter++; if(tdcp_sol.tdcp_iter>=30)tdcp_sol.tdcp_iter-=30; } else { //如果本历元tdcp lsq失败并且其他均无法提供有效信息 tdcp_sol.sol_status =0; //需要重新开始存储暂存信息 tdcp_sol.tdcp_iter=0; return 0; } //根据存储的rtk固定点与tdcp之间的时间范围内的解累加 for(i=0;i<5;i++){ if(tdcp_sol.rtk_sol[i].rtk_status>1){ store_iter++; for(j=0;j<5;j++){ if(tdcp_sol.tdcp_lsq[j].tdcp_valid){ delta_t=tdcp_sol.tdcp_lsq[j].tow-tdcp_sol.rtk_sol[i].tow; if(delta_t>-0.001 && delta_t<=5.0+0.0001){ delta_xyz[i][0]+=tdcp_sol.tdcp_lsq[j].delta_xyz[0]; delta_xyz[i][1]+=tdcp_sol.tdcp_lsq[j].delta_xyz[1]; delta_xyz[i][2]+=tdcp_sol.tdcp_lsq[j].delta_xyz[2]; sol_status[i]|=1; } } } } } //表示没有暂存的可用的RTK定位点 if(0 == store_iter){ return 0; } //依据存储点整理定位点 for(i=0;i<5;i++){ if(sol_status[i]){ for(j=0;j<3;j++){ delta_xyz[i][j]+=tdcp_sol.rtk_sol[i].pos_xyz[j]; } } } //判断最近的一个rtk解状态,其为固定点，可以直接叠加存储的信息输出 if(1 == sol_status[0] && (4 == tdcp_sol.rtk_sol[0].rtk_status)){ for(i=0;i<3;i++){ tdcp_sol.pos_xyz[i]=delta_xyz[0][i]; } tdcp_sol.sol_status=tdcp_sol.rtk_sol[0].rtk_status; sol_suceess=1; } //上一步没有更新解，判断存储的RTK解数值的状态，并对其分配比例处理 if(0 == sol_suceess){ if(1 == store_iter){ scale[0]=1.0; } else if(2 == store_iter){ scale[0]=0.8; scale[1]=0.2; } else if(store_iter >= 3){ scale[0]=0.7; scale[1]=0.2; scale[2]=0.1; } k=0; for(i=0;i<5;i++){ if(1 == sol_status[i]){ for(j=0;j<3;j++){ tdcp_sol.pos_xyz[j]+=scale[k]*delta_xyz[i][j]; } if(0 == k){ tdcp_sol.sol_status=tdcp_sol.rtk_sol[k].rtk_status; } k++; } } } return 1; }