PVT代表的是位置position、速度velocity、时间time的缩写即通过该算法可以获得接收机的这三个信息量其核心思想是以全球卫星导航系统GNSS如GPS、北斗为基础通过LSQ/KALMAN算法进行最优估计从而在各种环境下输出连续、可靠、准确的定位结果。在之前文章中提到过伪距的计算公式在此可以看出伪距、接收机位置rcv_pos、接收机钟差clkerr_rcv与卫星位置sat_pos、卫星钟差clkerr_sat的数学关系。不论是基于冷启动还是热启动伪距量可以通过初始化本地时间减去帧同步获得的发射时间得到该信息量包含了所有的误差项卫星位置和钟差可以通过星历解算得到电离层误差和对流层等误差项可以通过常规建模消除掉最终通过一定的数学方法求解得到接收机位置和时间信息。1. 最小二乘估计算法最小二乘估计算法Least squares estimationLSQ是一种数学优化技术用于在观测数据多于未知数即方程数多于变量数且存在误差的情况下使得所有观测方程的残差平方和最小的情况下寻找未知数的最优解。GNSS导航定位中所使用的伪距方程是关于位置pos钟差clkerr的一个非线性方程存在平方根所以第一步需要将其线性化线性化所使用的工具是泰勒展开公式即在一个已知的初始起点(x0、y0、z0clkerr0)对该函数进行一阶泰勒展开计算对上述公式的偏导数进行计算在初始点(x0、y0、z0)处这个表达式变为x、y、z的推导公式一样则对于y、z的表达式为对于钟差单位秒来说位置信息和其求导没有关系只和光速有关假设该部分求解的钟差单位为m的话求导的关系直接为1为保持未知参数单位的一致性后续将钟差的单位都默认为m综上可以将伪距方程转换为即可以转换为经典的伪距解算方程1此处伪距方程的左边为伪距残差H为转换矩阵为求解的未知量但是此时的仅仅为一个相对于(x0、y0、z0clkerr0)的变化量叠加后形成的(x1、y1、z1clkerr1)才为最终的结果输出。1.1 伪距残差的计算伪距残差是实际观测到的原始伪距与当前状态估计出的理论伪距之间的差异。1原始伪距本地时间tow-发射时间transmite*C其包含所有误差的伪距。2模型预测值卫地距r 和钟差部分clkerr_rcv-clkerr_sat以及各项传播时延误差项则对于多次迭代计算该数值部分中模型误差部分保持不变所以在整个LSQ迭代前端可以直接将该数值计算好便于后续计算。各项传播误差项除了电离层误差、对流层误差外一般还有TGD误差以及地球自转影响误差。1.1.1 电离层模型误差电离层是地球上空约60-1000公里高度的大气层受太阳辐射影响部分气体分子被电离形成大量自由电子和离子。当GNSS卫星信号穿过电离层时与自由电子相互作用导致信号传播路径和速度改变其对信号的影响与频率的平方成反比。电离层模型是基于经验模型建立的常用的有Klobuchar模型、BDS格网点电离层模型、北斗三代的BDGIM电离层模型。#define IONOOPT_OFF 0 /* ionosphere option: correction off */ #define IONOOPT_BRDC 1 /* ionosphere option: broadcast model */ #define IONOOPT_SBAS 2 /* ionosphere option: SBAS model */ #define IONOOPT_IFLC 3 /* ionosphere option: L1/L2 or L1/L5 iono-free LC */ #define IONOOPT_EST 4 /* ionosphere option: estimation */ #define IONOOPT_TEC 5 /* ionosphere option: IONEX TEC model */ #define IONOOPT_QZS 6 /* ionosphere option: QZSS broadcast model */ #define IONOOPT_LEX 7 /* ionosphere option: QZSS LEX ionospehre */ #define IONOOPT_STEC 8 /* ionosphere option: SLANT TEC model */ /* ionosphere model ------------------------------------------------------------ * compute ionospheric delay by broadcast ionosphere model (klobuchar model) * args : gtime_t t I time (gpst) * double *ion I iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} * double *pos I receiver position {lat,lon,h} (rad,m) * double *azel I azimuth/elevation angle {az,el} (rad) * return : ionospheric delay (L1) (m) *-----------------------------------------------------------------------------*/ extern double ionmodel(gtime_t t, const double *ion, const double *pos, const double *azel) { const double ion_default[]{ /* 2004/1/1 */ 0.1118E-07,-0.7451E-08,-0.5961E-07, 0.1192E-06, 0.1167E06,-0.2294E06,-0.1311E06, 0.1049E07 }; double tt,f,psi,phi,lam,amp,per,x; int week; if (pos[2]-1E3||azel[1]0) return 0.0; if (norm(ion,8)0.0) ionion_default; /* earth centered angle (semi-circle) */ psi0.0137/(azel[1]/PI0.11)-0.022; /* subionospheric latitude/longitude (semi-circle) */ phipos[0]/PIpsi*cos(azel[0]); if (phi 0.416) phi 0.416; else if (phi-0.416) phi-0.416; lampos[1]/PIpsi*sin(azel[0])/cos(phi*PI); /* geomagnetic latitude (semi-circle) */ phi0.064*cos((lam-1.617)*PI); /* local time (s) */ tt43200.0*lamtime2gpst(t,week); tt-floor(tt/86400.0)*86400.0; /* 0tt86400 */ /* slant factor */ f1.016.0*pow(0.53-azel[1]/PI,3.0); /* ionospheric delay */ ampion[0]phi*(ion[1]phi*(ion[2]phi*ion[3])); perion[4]phi*(ion[5]phi*(ion[6]phi*ion[7])); ampamp 0.0? 0.0:amp; perper72000.0?72000.0:per; x2.0*PI*(tt-50400.0)/per; return CLIGHT*f*(fabs(x)1.57?5E-9amp*(1.0x*x*(-0.5x*x/24.0)):5E-9); }RTKLIB中介绍了很多电离层模型在量产算法的使用最多、最基础的还是Klobuchar模型其主要依据8个参数计算接收机至卫星连线与电离层交点的电离层垂直延迟改正通过导航电文播发其为一个经验模型基于“电离层延迟在地方时夜间相对稳定且较小在白天尤其是中午前后达到最大并随纬度升高而总体减弱”。注意点1根据Klobuchar模型计算的为L1/B1I频点的电离层误差假设要计算B2I频点需要乘以频率系数2由于其对于全球范围内建模它在某些特定区域如低纬度的表现可能差于中纬度所以在使用双频接收机的话可以直接使用无电离层组合几乎可以完全消除99%一阶电离层延迟Klobuchar模型不再需要。如果对电离层误差或者高程信息严格要求代码本身空间足够的情况下可以使用BDS 格网点电离层模型。电离层格网覆盖范围为东经70~145 度北纬7.5~55 度按经纬度5×2.5度进行划分形成320 个格网点。每个格网点电离层信息包括格网点垂直延迟dτ和误差指数GIVEI目标就是将接收机点划分到以4个点为边界的网格中从而计算电离层模型误差具体可以参见BDS ICD文件。TEC计算公式总电子含量指在截面为1平方米的圆柱体内沿信号从卫星到接收机路径上的总自由电子数。卫星信号传播路径上的总电子含量TEC是利用电离层延迟的色散特性与频率平方成反比。使用同一卫星同一历元的两个频率的观测值进行组合常用单位为TECU1TECU10^16 个电子/平方米。双频伪距公式以及TEC和I的关系令则这里的TGD如何处理是该公式的又一重点TEC是斜路径上的电子含量一般会转换为VTEC存储计算。是信号路径在电离层单层模型穿刺点处的天顶角。VTEC是Septentrio和天宝电离层闪烁接收机的计算原理但是TEC计算出来为一个很大的数值一般以滑动窗60个历元取其数值与均值变化的差值或者std数值来判断是否发生电离层闪烁。1.1.2 对流层模型误差对流层是地面以上约0-12公里的大气层。信号在此传播的延迟由干延迟和湿延迟两部分组成干延迟 (约90%)主要由非水汽的干空气氮气、氧气等引起。这部分延迟比较稳定可以通过地面气压很好地预测和建模。湿延迟 (约10%)主要由大气中的水汽引起。水汽时空分布极不均匀、变化剧烈分钟级、公里级是主要的误差来源。常见的模型如 Saastamoinen、Hopfield、UNB模型、UNB3m等。前两者需要实测气象参数的模型后两者不需要实测气象参数的模型只需要提供高程、纬度和年积日即可计算对流层延迟量。RTKLIB中介绍了两种对流层模型量产算法中使用比较多的还是Saastamoinen模型。/* troposphere model ----------------------------------------------------------- * compute tropospheric delay by standard atmosphere and saastamoinen model * args : gtime_t time I time * double *pos I receiver position {lat,lon,h} (rad,m) * double *azel I azimuth/elevation angle {az,el} (rad) * double humi I relative humidity * return : tropospheric delay (m) *-----------------------------------------------------------------------------*/ extern double tropmodel(gtime_t time, const double *pos, const double *azel, double humi) { const double temp015.0; /* temparature at sea level */ double hgt,pres,temp,e,z,trph,trpw; if (pos[2]-100.0||1E4pos[2]||azel[1]0) return 0.0; /* standard atmosphere */ hgtpos[2]0.0?0.0:pos[2]; pres1013.25*pow(1.0-2.2557E-5*hgt,5.2568); temptemp0-6.5E-3*hgt273.16; e6.108*humi*exp((17.15*temp-4684.0)/(temp-38.45)); /* saastamoninen model */ zPI/2.0-azel[1]; trph0.0022768*pres/(1.0-0.00266*cos(2.0*pos[0])-0.00028*hgt/1E3)/cos(z); trpw0.002277*(1255.0/temp0.05)*e/cos(z); return trphtrpw; }上面也提到了UNB3模型其核心思想与Klobuchar模型类似用一个预先定义好的、参数化的全球平均气候模型为全球任意地点的用户提供对流层延迟估计而用户无需输入任何实时气象数据。其并不是一个全新的物理公式而是一个“Saastamoinen计算框架 内置全球平均气象参数数据库”的组合具体参见《周命端,郭际明,孟祥广.GPS对流层延迟改正UNB3模型及其精度分析[J]》的详细介绍。1.1.3地球自转改正在信号从卫星传播到接收机的几十毫秒内地球坐标系发生了旋转导致基于“信号发射时刻”坐标系计算的卫星位置与“信号接收时刻”的坐标系不再对齐从而引入了一个几何偏差具体计算公式为为地球自转角速度C为光速x_sat、y_sat为卫星位置x_rcv、y_rcv为接收机位置RTKLIB中直接将其作为卫地距的修正直接附加在r中修改掉这也是一种常用的方式。/* geometric distance ---------------------------------------------------------- * compute geometric distance and receiver-to-satellite unit vector * args : double *rs I satellilte position (ecef at transmission) (m) * double *rr I receiver position (ecef at reception) (m) * double *e O line-of-sight vector (ecef) * return : geometric distance (m) (0:error/no satellite position) * notes : distance includes sagnac effect correction *-----------------------------------------------------------------------------*/ extern double geodist(const double *rs, const double *rr, double *e) { double r; int i; if (norm(rs,3)RE_WGS84) return -1.0; for (i0;i3;i) e[i]rs[i]-rr[i]; rnorm(e,3); for (i0;i3;i) e[i]/r; return rOMGE*(rs[0]*rr[1]-rs[1]*rr[0])/CLIGHT; }1.1.4 TGD修正星上设备时延time group delay指从卫星的时间基准到发射天线相位中心的时延即卫星上不同频率信号从卫星钟参考点到天线相位中心的传播时间存在差异。如果是双频甚至三频观测量一起参与定位的话需要将其拉到同一时间轴上解算该数值必须消除掉。1BDS以B3I 信号为基准B1I、B2I的TGD1、TGD2在D1/D2的导航电文中播发用于补偿B1C 导频分量、B2a 导频分量的时延差TGDB1Cp 和TGDB2ap 在B-CNAV1 电文中播发用于补偿B2b 信号 I 支路的时延差TGDB2bI在B-CNAV3 电文中播发即2GPSL1CA导航电文中播发的是L1P和L2P之间的群延迟TGD具体也可参考《广伟,李玮,袁海波.TGD改正对北斗授时性能的影响[C]》3GALILEOINAV中播发BGD(E1,E5A)和BGD(E1,E5B)FNAV中播发BGD(E1,E5A)假设以E1为基础计算E5A和E5B计算公式为或代码如下#define SYS_NONE 0x00 /* navigation system: none */ #define SYS_GPS 0x01 /* navigation system: GPS */ #define SYS_GAL 0x08 /* navigation system: Galileo */ #define SYS_QZS 0x10 /* navigation system: QZSS */ #define SYS_CMP 0x20 /* navigation system: BeiDou */ #define CODE_NONE 0 /* obs code: none or unknown */ #define CODE_L1C 1 /* obs code: L1C/A,G1C/A,E1C (GPS,GLO,GAL,QZS,SBS) */ #define CODE_L1P 2 /* obs code: L1P,G1P (GPS,GLO) */ #define CODE_L1B 11 /* obs code: E1B (GAL) */ #define CODE_L2C 14 /* obs code: L2C/A,G1C/A (GPS,GLO) */ #define CODE_L2P 19 /* obs code: L2P,G2P (GPS,GLO) */ #define CODE_L5Q 25 /* obs code: L5/E5aQ (GPS,GAL,QZS,SBS) */ #define CODE_L7I 27 /* obs code: E5bI,B2I (GAL,CMP) */ #define CODE_L7Q 28 /* obs code: E5bQ,B2Q (GAL,CMP) */ #define CODE_L2I 40 /* obs code: B1I (CMP) */ #define CODE_L6I 42 /* obs code: B3I (CMP) */ #define CODE_L1P 50 /* obs code: B1C (CMP) * #define CODE_L5P 51 /* obs code: B2A (CMP) * #define CODE_L7P 52 /* obs code: B2B (CMP) * double tgd[5]; /* group delay parameters */ /* GPS/QZS:tgd[0]TGD */ /* GAL :tgd[0]BGD E5a/E1,tgd[1]BGD E5b/E1 */ /* CMP :tgd[0]BGD1,tgd[1]BGD2 tgd[2]B1C BGD,tgd[3]B2A BGD,tgd[4]B2B BGD*/ double get_sys_tgd(int sys, int code_id, eph_t gbg_eph){ double tgd0.0,gamma; if(SYS_GPS sys || SYS_QZS sys ){ if(CODE_L1C code_id || CODE_L1P code_id) tgdgbg_eph.tgd[0]; if(CODE_L2C code_id || CODE_L2P code_id){ gamma(FREQ1/FREQ2)^2; tgdgamma*gbg_eph.tgd[0]; } } else if(SYS_CMP sys){ if(CODE_L2I code_id) tgdgbg_eph.tgd[0]; else if(CODE_L7I code_id) tgdgbg_eph.tgd[1]; else if(CODE_L1P code_id) tgdgbg_eph.tgd[2]; else if(CODE_L5P code_id) tgdgbg_eph.tgd[3]; else if(CODE_B7b code_id) tgdgbg_eph.tgd[4]; } else if(SYS_GAL sys){ if(CODE_L1B code_id) tgdgbg_eph.tgd[0]; else if(CODE_L5Q code_id){ gamma(FREQ1/FREQ5)^2; tgdgamma*gbg_eph.tgd[0]; } else if(CODE_L7Q code_id){ gamma(FREQ1/FREQ7)^2; tgdgamma*gbg_eph.tgd[1]; } } return tgd; }1.2 LSQ迭代过程对于公式1伪距残差l和转换矩阵H已知对于解算函数对其求解x2RTKLIB中LSQ解算代码/* least square estimation ----------------------------------------------------- * least square estimation by solving normal equation (x(A*A)^-1*A*y) * args : double *A I transpose of (weighted) design matrix (n x m) * double *y I (weighted) measurements (m x 1) * int n,m I number of parameters and measurements (nm) * double *x O estmated parameters (n x 1) * double *Q O esimated parameters covariance matrix (n x n) * return : status (0:ok,0:error) * notes : for weighted least square, replace A and y by A*w and w*y (wW^(1/2)) * matirix stored by column-major order (fortran convention) *-----------------------------------------------------------------------------*/ extern int lsq(const double *A, const double *y, int n, int m, double *x, double *Q) { double *Ay; int info; if (mn) return -1; Aymat(n,1); matmul(NN,n,1,m,1.0,A,y,0.0,Ay); /* AyA*y */ matmul(NT,n,n,m,1.0,A,A,0.0,Q); /* QA*A */ if (!(infomatinv(Q,n))) matmul(NN,n,1,n,1.0,Q,Ay,0.0,x); /* xQ^-1*Ay */ free(Ay); return info; }1.2.1 退出LSQ迭代的条件是什么从伪距方程的线性化方程来讲其主要是基于初始点做了一阶展开后续高阶阶数并未展开即当前的整个方程具有一定近似性只有当每次迭代出的结果足够小的情况下的时候才能一步步的消除由于泰勒展开所丢失的高阶部分所以大部分算法中要求的输出门限是的条件下终止整个循环可以举例说明理解为迭代结果泰勒展开的起始点第一次10102010初始点第二次0.10.10.20.1第一次第三次0.00020.00020.00040.0002第二次结束可以理解在一次大的LSQ解算过程中每一次迭代的结果都作为下一次泰勒展开的初始点当迭代出的结果越来越小的时候说明前一次和本次的结果相近在考虑到耗时以及精度的前提下可以结束本次LSQ结束。在考虑迭代出满足条件的结果之前还需要考虑迭代过程耗时和运行效率的影响即一次LSQ迭代循环里最多可以迭代运算几次一般情况下在卫星数量少或者首次定位的时候迭代次数≤10后续的迭代次数≤5。1.2.2 LSQ中相关计算1加权最小二乘算法导航接收机所接收到的卫星信号传播路径不同其所受到的各部分误差影响也不同针对这一情况在实际运算中会对每一个卫星根据高度角、载噪比、基带参数、系统型号分配不一样的权重即卫星定权P阵引入最小二乘解算中即加权最小二乘算法具体公式为3RTKLIB.C lsq() /* weight by variance */ for (j0;jnv;j) { sigsqrt(var[j]); v[j]/sig; for (k0;kNX;k) H[kj*NX]/sig; }2LSQ计算指标验前伪距残差一般使用验前伪距残差来做剔星/定权操作即在进入LSQ之前基于上一个定位点和本次伪距量计算出所有观测量的伪距残差假设本环节存在粗差情况就可以剔除可以作为是一个预处理环节。GNSS模组上车路测环节会配备一个高精度POS设备可以依据该高精结果反算伪距残差来做基础校验。验后伪距残差验前剔星只是作为一个预处理环节在真正LSQ环节中第一次迭代出可以依据更新本地位置再重新计算伪距残差部分依据该部分再次做粗差探测剔除异常观测量。3LSQ质量控制下面介绍的相关指标都可作为迭代结果的有效性判断指标输出到RTK侧或者INS侧来判断当前定位是否有效。残差平方和RSS方差一般来说在LSQ迭代后伪距残差较小所以粗略门限可以设置为粗差探测极大值删除法、聚类法、中值检验法、标准化残差法。当前阶段的难点伪距残差呈现两个极端聚类情况分不清到底哪端到底是可以使用的是正确的会引入更多的剔星方法、基于高精计算残差、考虑当前定权是否分配合理如果都无法解决就尝试分别使用一端做定位查看定位结果再考虑如何处理。残差检验方法卡方检验是在LSQ中用于检验伪距残差的合理性RTKLIBchisqr()函数即比较残差平方和与理论方差来评估定位结果的可靠性当残差平方和超过基于自由度的卡方分布临界值则标记本次迭代结果不正常不采用本次结果。卡方检验中的显著性水平影响整个结果的判断在衡量伪距、载波、多普勒的定位、定速结果的时候要作以区分以及在针对不同运动场景、运动状态下也要做好区分。问题简单的高度角定权对卡方检验是否有影响如果只用简单高度角分段方法或正弦模型定权即高度角越来越大权重越高其改变了残差的相对贡献当方差大→权大高高度角→贡献变小当方差小→权小高度角低→贡献越小 会模糊残差对卡方检验的贡献计算的定权残差平方和可能会偏离理论期望导致卡方检验做出错误的判断。协方差矩阵一般会直接转为标准差的方式评价即可以看出Q阵的每个元素代表的都是未知数之间的作用关系一般只使用对角线元素即在LSQ迭代过程或者结束后也会对std的数值进行判断是否满足条件。由于P权重对于Q的影响较大所以在定权的时候要做好衡量在定位异常/STD异常的时候要去判断权重是否分配合理是否有接近0的情况避免出现nan/inf数值。注意点该处使用的是ECEF坐标系如果需要使用ENU坐标系则需要做一定转换。//RTKLIB rtkcmn.c ECEF坐标(xyz)转换为ENU坐标(llh) extern void ecef2pos(const double *r, double *pos) { double e2FE_WGS84*(2.0-FE_WGS84),r2dot(r,r,2),z,zk,vRE_WGS84,sinp; for (zr[2],zk0.0;fabs(z-zk)1E-4;) { zkz; sinpz/sqrt(r2z*z); vRE_WGS84/sqrt(1.0-e2*sinp*sinp); zr[2]v*e2*sinp; } pos[0]r21E-12?atan(z/sqrt(r2)):(r[2]0.0?PI/2.0:-PI/2.0); pos[1]r21E-12?atan2(r[1],r[0]):0.0; pos[2]sqrt(r2z*z)-v; } //转换矩阵E(R) extern void xyz2enu(const double *pos, double *E) { double sinpsin(pos[0]),cospcos(pos[0]),sinlsin(pos[1]),coslcos(pos[1]); E[0]-sinl; E[3]cosl; E[6]0.0; E[1]-sinp*cosl; E[4]-sinp*sinl; E[7]cosp; E[2]cosp*cosl; E[5]cosp*sinl; E[8]sinp; }精度衰减因子dop数值计算dop值是衡量卫星几何构型对定位精度影响的无量纲标量描述卫星与GNSS接收机之间的空间几何关系如何放大或缩小观测误差。当卫星卫星分散在天空各处计算dop值较小误差放大效应小当卫星聚集在天空一角计计算dop值较大误差放大效应大。a依据高度角方位角构造H阵构造方法为b依据协方差矩阵计算c垂直精度衰减因子水平精度衰减因子位置精度衰减因子几精度衰减因子/*RTKLIB rtkcmn.c 更新dop数值*/ extern void dops(int ns, const double *azel, double elmin, double *dop) { double H[4*MAXSAT],Q[16],cosel,sinel; int i,n; for (i0;i4;i) dop[i]0.0; for (in0;insiMAXSAT;i) { if (azel[1i*2]elmin||azel[1i*2]0.0) continue; coselcos(azel[1i*2]); sinelsin(azel[1i*2]); H[ 4*n]cosel*sin(azel[i*2]); H[14*n]cosel*cos(azel[i*2]); H[24*n]sinel; H[34*n]1.0; } if (n4) return; matmul(NT,4,4,n,1.0,H,H,0.0,Q); if (!matinv(Q,4)) { dop[0]SQRT(Q[0]Q[5]Q[10]Q[15]); /* GDOP */ dop[1]SQRT(Q[0]Q[5]Q[10]); /* PDOP */ dop[2]SQRT(Q[0]Q[5]); /* HDOP */ dop[3]SQRT(Q[10]); /* VDOP */ } }注意1如果在预处理模块对卫星进行高度角定权的部分占很大比重的话即低仰角卫星的权重降低其实是对dop数值存在很大的影响所以在整个定权模块要尽量采用多种定权方式结合的方式以至于dop更真实地反映实际定位中采用的策略。2判断指标当前定位算法中一般评价hdop和pdop比较多在dop2.0的时候认为当前是一个很好的卫星分布情况越大6.0越异常。