CN113551669A - 基于短基线的组合导航定位方法及装置 - Google Patents

Abstract

本发明提供了本发明基于短基线的组合导航定位方法及装置，方法包括获取多模接收机的PPS信号，并通过RTK方式进行定位；将PPS信号中的定位信息作为INS的初值，进行INS计算，得到INS信息；将成功定位对应的PPS信号及所述INS信息通过卡尔曼滤波组合，得到最优的姿态位置信息。本发明采用短基线，基线长度仅有320mm，两个接收天线，重量仅为5kg，设备小巧，移动方便，GNSS和INS组合导航，在载体受到遮挡时可以用INS快速定位，采用了高精度的INS惯性元件，同时用卡尔曼滤波优化惯性元件的误差，并且GNSS的定位信息能够校正INS惯性元件的偏差，保证了系统的定位精度和可靠性。

Description

基于短基线的组合导航定位方法及装置

技术领域

本发明涉及惯性导航技术领域，尤其是一种基于短基线的组合导航定位方法及装置。

背景技术

现有的惯导系统主要为GPS/INS(GPS，Global Positioning System全球定位系统；INS，Inertial Navigation System，惯性导航系统)组合导航系统。它是一种无框架系统，由三个陀螺仪、三个加速度计和微型计算机组成，陀螺仪和加速度计直接固定连接在载体上，姿态数据通过微型计算机计算陀螺仪和加速度计的输出值得到。

如图1所示，加速度计测得沿载体坐标系三个轴的加速度，经误差补偿计算后，进行坐标变换计算，加速度与重力加速度求和并积分得到速度，加速度的双重积分得到载体的位置值。陀螺仪测得沿载体坐标系三个轴的角速度，经误差补偿计算后进行坐标系转换和姿态矩阵计算，姿态矩阵随时间的变化而不断变化，从姿态矩阵中可以确定载体的姿态角。最后得到载体的三个方向角、速度和位置信息。

由于惯性元件直接承受载体的振动、冲击及温度波动等环境条件，惯性元件的输出信息会产生严重的动态误差，这类误差难以补偿，且长期使用会累积，造成误差较大，导航系统精度降低。现有的组合导航系统为了保证定位精度高，通常会采用多个天线，且天线接收机间的基线通常大于1m，重量大于10kg，造成了设备体积大、笨重且移动不方便等问题。

发明内容

本发明提供了基于短基线的组合导航定位方法及装置，用于解决现有设备体积大、移动不方便的问题。

为实现上述目的，本发明采用下述技术方案：

本发明第一方面提供了基于短基线的组合导航定位方法，所述方法包括以下步骤：

获取多模接收机的PPS信号，并通过RTK方式进行定位；

将PPS信号中的定位信息作为INS的初值，进行INS计算，得到INS信息；

将成功定位对应的PPS信号及所述INS信息通过卡尔曼滤波组合，得到最优的姿态位置信息。

进一步地，所述多模式接收机包括前天线接收机和后天线接收机，前天线接收机将观测数据发送给后天线接收机，所述后天线接收机利用自身的观测数据和差分信息，建立载波双差方程进行相对定位。

进一步地，所述通过RTK方式进行定位的具体过程为：

设前天线接收机和后天线接收机的共视卫星有k+1颗，线性化后的载波相位双差方程为ξ＝Aη+BN+v,式中：ξ∈R^2k是载波相位双差观测值与伪距双差观测值构成的向量，η∈R³表示基线向量，N∈Z^k表示载波相位双差整周模糊度向量，v∈R^2k表示观测噪声，A∈R^2k×3表示基线常系数矩阵，B∈R^2k×k表示模糊度常系数矩阵；

对所述载波相位双差方程进行求解，若已知前天线接收机坐标为η^a，则后天线接收机坐标η^b的差分估计值为

为基线坐标向量。

进一步地，所述INS信息包括载体的经纬度、海拔信息以及姿态角。

进一步地，所述进行INS计算，得到INS信息的具体过程为：

基于陀螺仪输出的载体相对于惯性坐标系的角速度和

得到载体坐标系相对于导航坐标系的角速度

将该角速乘采样时间得到当前转动的角度矢量值，将东北天三个方向的角度矢量值进行归一化处理，得到姿态角；

将加速度计的输出值乘采样时间得到当前速度矢量值，将所述速度矢量值进行四元数变换得到载体相对于导航坐标系的速度，经坐标转换，将载体对于地理坐标系的角速度和速度进行归一化处理，将因科里奥利力引起的速度增量修正，计算出东北天三个方向的速度，将前后两次的速度积分计算得到位置增量，与前次位置相加得到当前的经纬度、海拔信息。

进一步地，所述通过卡尔曼滤波组合，得到最优的姿态位置信息的具体过程为：

将所述定位信息作为卡尔曼滤波估计系统定位信息状态，建立INS信息的卡尔曼滤波方程，得到组合导航系统的状态向量x及状态方程x(t)；

将PPS信号中的载体速度信息和位置信息作为量测，得到组合导航系统的量测方程，经离散化处理，得到观测方程；

对随机状态变量X在时刻k进行卡尔曼滤波计算，得到载体在k+1时刻位置信息的最优估计。

进一步地，所述进行卡尔曼滤波计算的具体过程为：

利用UT变换，得到一组状态采集点及对应的权值；

通过对所述状态采集点的预测值加权求和，得到系统状态的一步预测及协方差矩阵；

对一步预测值进行UT变换，产生新的状态采集点；

将所述新的状态采集点代入观测方程，得到预测的观测值，通过对所述观测值进行加权求和，得到预测的均值及协方差；

计算卡尔曼增益矩阵，更新状态和协方差，所述更新状态即载体在k+1时刻位置信息的最优估计。

进一步地，所述定位成功的判断具体为：

接收到的PPS信号中包含完整的定位信息，且天线接收机的卫星可视星数大于4颗。

本发明第二方面提供了基于短基线的组合导航定位装置，所述装置包括多模接收机和INS形成的组合导航系统，以及微型计算机，所述多模接收机用于获取PPS信号，所述微型计算机利用所述PPS信号通过RTK方式进行定位，并计算INS信息，将将成功定位对应的PPS信号及所述INS信息通过卡尔曼滤波组合，得到最优的姿态位置信息。

进一步地，所述多模接收机的基线长度为320毫米。

发明内容中提供的效果仅仅是实施例的效果，而不是发明所有的全部效果，上述技术方案中的一个技术方案具有如下优点或有益效果：

本发明采用短基线，基线长度仅有320mm，两个接收天线，重量仅为5kg，设备小巧，移动方便，GNSS和INS组合导航，在载体受到遮挡时可以用INS快速定位，采用了高精度的INS惯性元件，同时用卡尔曼滤波优化惯性元件的误差，并且GNSS的定位信息能够校正INS惯性元件的偏差，保证了系统的定位精度和可靠性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是现有组合惯导系统的原理示意图；

图2是本发明所述方法的流程示意图；

图3是本发明所述组合导航系统的原理示意图；

图4是本发明所述方法实施例的流程示意图；

图5是本发明仿真位置值的对比示意图；

图6是本发明偏差值的对比示意图；

图7是现有惯导系统航向角输出值的曲线图；

图8为本发明所述系统航向角输出值的曲线图；

图9为现有惯导系统俯仰角输出值的曲线图；

图10为本发明所述系统俯仰角输出值的曲线图。

具体实施方式

为能清楚说明本方案的技术特点，下面通过具体实施方式，并结合其附图，对本发明进行详细阐述。下文的公开提供了许多不同的实施例或例子用来实现本发明的不同结构。为了简化本发明的公开，下文中对特定例子的部件和设置进行描述。此外，本发明可以在不同例子中重复参考数字和/或字母。这种重复是为了简化和清楚的目的，其本身不指示所讨论各种实施例和/或设置之间的关系。应当注意，在附图中所图示的部件不一定按比例绘制。本发明省略了对公知组件和处理技术及工艺的描述以避免不必要地限制本发明。

如图2所示，本发明基于短基线的组合导航定位方法，包括以下步骤：

S1,获取多模接收机的PPS信号，并通过RTK方式进行定位；

S2,将PPS信号中的定位信息作为INS的初值，进行INS计算，得到INS信息；

S3,将成功定位对应的PPS信号及所述INS信息通过卡尔曼滤波组合，得到最优的姿态位置信息。

如图3、4所示，微型计算机通过GPS/BDS多模接收机接收PPS(秒脉冲)信号，收到PPS信号后采用RTK定位方式获取位置信息，接收到完整的定位信息后，将获取的速度、经纬度和海拔信息赋给INS信息的初值(包括UTC时间，经纬度，海拔高度，航向角，东北天三个方向的角速度)，根据接收机收到卫星信号的强度确定接收卫星的星数，当信号强度大于15dBm认为卫星信号有效，当GPS/BDS的卫星可视星数大于4时，判断定位信息有效，否则重新接收PPS信号再定位，定位成功后将GPS/BDS信息和上述计算得到的INS信息通过卡尔曼滤波组合获得最优的姿态和位置信息。

步骤S1中所述多模式接收机包括前天线接收机和后天线接收机，前天线接收机将观测数据发送给后天线接收机，所述后天线接收机利用自身的观测数据和差分信息，建立载波双差方程进行相对定位。

通过RTK方式进行定位的具体过程为：

设前天线接收机和后天线接收机的共视卫星有k+1颗，线性化后的载波相位双差方程为ξ＝Aη+BN+v,式中：ξ∈R^2k是载波相位双差观测值与伪距双差观测值构成的向量，η∈R³表示基线向量，N∈Z^k表示载波相位双差整周模糊度向量，v∈R^2k表示观测噪声，A∈R^2k×3表示基线常系数矩阵，B∈R^2k×k表示模糊度常系数矩阵；

对所述载波相位双差方程进行求解，若已知前天线接收机坐标为η^a，则后天线接收机坐标η^b的差分估计值为

为基线坐标向量。

对所述载波相位双差方程进行求解的具体过程为：

第一步，用最小二乘法求解η和N的浮点解(浮点解的精度在0.1～0.5米之间)，即

浮点解的协方差矩阵为

矩阵中四个量表示η和N两两之间的协方差。

第二步，固定模糊度浮点解(固定解的精度可达1厘米，精度提高)，即

第三步，根据

和协方差

求出基线向量坐标，即

依据上述RTK差分算法，若已知前天线坐标为η^a，后天线坐标η^b的差分估计值为

步骤S2中所述INS信息包括载体的经纬度、海拔信息以及姿态角。

所述进行INS计算，得到INS信息的具体过程为：

S21,基于陀螺仪输出的载体相对于惯性坐标系的角速度和

得到载体坐标系相对于导航坐标系的角速度

将该角速乘采样时间得到当前转动的角度矢量值，将东北天三个方向的角度矢量值进行归一化处理，得到姿态角；

S22,将加速度计的输出值乘采样时间得到当前速度矢量值，将所述速度矢量值进行四元数变换得到载体相对于导航坐标系的速度，经坐标转换，将载体对于地理坐标系的角速度和速度进行归一化处理，将因科里奥利力引起的速度增量修正，计算出东北天三个方向的速度，将前后两次的速度积分计算得到位置增量，与前次位置相加得到当前的经纬度、海拔信息。

步骤S21的具体实现过程为：

取得陀螺仪的输出值

即载体相对于惯性坐标系的角速度，根据GPS定位得到的纬度求得载体导航坐标系下的地球自转角速度

其中ω_ie为赤道位置的地球自转角速度，λ为载体所在的纬度；

位置速率

由载体沿导航坐标系的地速决定，首先根据载体所在位置的纬度计算出此时在子午面内以及与子午面垂直的平面内的地球曲率半径R_M和R_N，根据载体东北天三个方向的速度、所在位置的曲率半径、纬度和高度可以计算得出

v_e是载体东向速度，v_n是载体北向速度，h是载体所在的海拔高度；

得到载体在地理坐标系的角速度值；

得出导航坐标系下导航坐标系相对于惯性坐标系的角速度；

根据加速度计的输出值和重力加速度计算出载体的横滚角和俯仰角，根据GPS定位得到载体的航向角，将这三个角转换成四元数q0,q1,q2,q3,通过姿态坐标转换成导航坐标系相对于惯性坐标系的角速度

q*表示四元数的共轭。

得到载体坐标系相对于导航坐标系的角速度

ω_nb_b＝ω_ib_b-ω_ib_n。

根据

乘采样时间得到此刻转动的角度矢量值，采样频率越高，角度的精度越高，将三个方向的角度进行归一化处理计算后得到了高精度的姿态角。

步骤S22的具体实现过程为：

由加速度计输出值乘采样时间得出此时的速度矢量值，采样频率越高，速度的精度越高，将速度矢量值进行四元数变换得到载体相对于导航坐标系的速度v_n＝q×v_b×q*；

根据地球曲率半径和载体纬度海拔高度计算得出载体所在位置的重力加速度，

g₀＝9.780318×(1+(5.3024×10-3)×sin2(λ)-(5.9×10-6)×sin2(2λ))；

g＝g₀/((1+h/R_N)2)；

g₀是赤道位置的重力加速度值，λ为载体所在位置纬度，h为载体所在海拔高度。

天向的速度矢量需要去掉重力加速度的影响误差，所以天向速度u_d＝u_d+g×t(t为采样时间)。

将载体对于地理坐标系的角速度和速度进行归一化处理，将因科里奥利力引起的速度增量计算修正，计算出三个方向的速度，将前后两次的速度积分计算得到位置增量，与前次位置相加得到此刻的经纬度海拔信息。

步骤S3中，所述通过卡尔曼滤波组合，得到最优的姿态位置信息的具体过程为：

S31,将所述定位信息作为卡尔曼滤波估计系统定位信息状态，建立INS信息的卡尔曼滤波方程，得到组合导航系统的状态向量x及状态方程x(t)。

组合导航系统的状态向量x为：

v_e、v_n、v_n表示三个方向的速度，

分别表示经纬度和高度，φe、φn和φu表示三个方向的角度，d_e、d_n、d_u表示三个方向的移动距离，a_e、a_n、a_u表示三个方向的加速度。

组合导航系统的状态方程为：

其中f[·]为非线性连续函数，w(t)为系统噪声。

S32,将PPS信号中的载体速度信息和位置信息作为量测，得到组合导航系统的量测方程，经离散化处理，得到观测方程。

选取GPS/BDS输出的载体速度信息v_egv_ngv_ug，位置信息

作为量测Z，

则获得组合导航系统的量测方程为：Z_k＝H(x_k,v_k)

将系统模型离散化，处理后得到的系统方程和观测方程分别为：

X(k+1)＝ΦX(k)+ΓW(k)

Z(k)＝HX(k)+V(k)

其中，k为离散时间，系统在时刻k的状态为X(k)∈Rⁿ；Z(k)∈R^m为对应状态的观测信号；W(k)∈R^r为输入的白噪声；V(k)∈R^m为观测噪声，Ф为状态转移矩阵，Γ为噪声驱动矩阵，H为观测矩阵。

构成非线性系统如下：

X(k+1)＝f(x(k),W(k))

Z(k)＝h(x(k),V(k))

f是非线性状态方程函数；h是非线性观测方程函数。

S33,对随机状态变量X在时刻k进行卡尔曼滤波计算，得到载体在k+1时刻位置信息的最优估计。

运用UT变换，设一个非线性变换y＝f(x)，状态向量x为n维随机变量，并且已知其均值

和方差P，通过下面的UT变换得到2n+1个sigma点X和相应的权值ω来计算y的统计特征：

计算2n+1个sigma点，即采样点，n是系统状态维数。

式中，

表示矩阵方根的第i列。

是状态向量x进行UT变换后的2n+1个sigma点集X的均值。

计算这些采样点相应的权值

式中，下标m为均值，c为协方差，上标为第i个采样点。参数λ＝a²(n+κ)-ns是一个缩放比例参数，用来降低总的预测误差，a的选取控制了采样点的分布状态，κ是待选参数，具体取值没有界限，通常应保证矩阵(n+λ)P为半正定矩阵。待选参数β是非负的权系数，可以合并方程中高阶项的动差，这样可以把高阶项的影响计算在内。

利用上述UT变换获得一组状态采样点(称为sigma点集)及对应权值。

计算2n+1个sigma点集的下一个时刻的预测，i＝1,2,...2n+1

X⁽ⁱ⁾(k+1|k)＝f[k,X⁽ⁱ⁾(k|k)]

计算系统状态的一步预测及协方差矩阵，由sigma点集的预测值加权求和得到，其中权值ω⁽ⁱ⁾通过上述UT变换的计算方式可得。

根据一步预测值再次进行UT变换，产生新的sigma点集。

将新的sigma点集代入观测方程，得到预测的观测量，i＝1,2,...2n+1。

Z⁽ⁱ⁾(k+1|k)＝h[X⁽ⁱ⁾(k+1|k)]

由此得到sigma点集的观测预测值，通过加权求和得到系统预测的均值及协方差。

计算卡尔曼增益矩阵。

计算系统的状态更新和协方差更新

P(k+1|k+1)是经过卡尔曼滤波后得到的协方差，

即是卡尔曼滤波后得到的载体在k+1时刻的位置信息的最优估计，通过以上步骤计算得到最优的载体姿态位置信息。

下面对上述方法进行仿真计算。

如图5和图6所示，图5分别为仿真位置的真实值、惯导直接计算得出的测量值和惯导经由卡尔曼滤波后得出的最优估计值，图6为惯导直接计算与实际值的偏差和卡尔曼滤波后的偏差。从仿真图可以看出卡尔曼滤波与惯导直接计算出的值相比，大大降低了偏差，比较接近真实值。虽然卡尔曼滤波后误差并没有完全消失，但它尽可能地逼近真实值，降低了误差的影响，从而提高了导航定位系统的精度。

以定向所得航向角为例进行比较，图7为普通惯性导航系统航向角长时间输出值的曲线图，图8为本方案采用的组合导航系统航向角输出值的曲线图。由图可以看出，普通惯性导航系统定向精度不高，航向角偏差最大能达到15度，而本方案采取的算法得出的航向角偏差仅为1度多，精度很高，同时长时间测量也不会发生大的偏差，可靠性好。

图9和图10为惯导在摇摆台正弦运动时的俯仰角输出值的曲线图。图9为普通惯导系统的输出值，图10是本方案的组合导航系统俯仰角的输出值。由图中曲线比较可以得知，本方案采用的算法的动态精度也很高，保证系统设备在运动过程中定向也能精准。

本发明还提供了基于短基线的组合导航定位装置，装置包括多模接收机和INS形成的组合导航系统，以及微型计算机，所述多模接收机用于获取PPS信号，所述微型计算机利用所述PPS信号通过RTK方式进行定位，并计算INS信息，将将成功定位对应的PPS信号及所述INS信息通过卡尔曼滤波组合，得到最优的姿态位置信息。所述多模接收机的基线长度为320毫米。

本发明克服了传统惯性导航设备体积大、定位不稳、容易受到干扰，可靠性差的缺点，由于采用了短基线设计，因此设备体积小，易携带，天线之间距离短，定位的经纬度和海拔高度与载体位置之间的误差小，计算相对简单。本发明采用了GNSS/INS组合导航定位，定位快速，在载体受到遮挡无法接收卫星信号进行GPS/BDS定位的情况下，也能够根据INS信息单独定位，可靠性相对于普通惯性导航系统高，采用了卡尔曼滤波综合GPS/BDS和INS的定位信息获得最优位置信息，采用算法校正陀螺仪和加速度计的偏差，使得本系统定位的精度也比一般的惯性导航系统要高。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (10)

1.基于短基线的组合导航定位方法，其特征是，所述方法包括以下步骤：

获取多模接收机的PPS信号，并通过RTK方式进行定位；

将PPS信号中的定位信息作为INS的初值，进行INS计算，得到INS信息；

将成功定位对应的PPS信号及所述INS信息通过卡尔曼滤波组合，得到最优的姿态位置信息。

2.根据权利要求1所述基于短基线的组合导航定位方法，其特征是，所述多模式接收机包括前天线接收机和后天线接收机，前天线接收机将观测数据发送给后天线接收机，所述后天线接收机利用自身的观测数据和差分信息，建立载波双差方程进行相对定位。

3.根据权利要求2所述基于短基线的组合导航定位方法，其特征是，所述通过RTK方式进行定位的具体过程为：

设前天线接收机和后天线接收机的共视卫星有k+1颗，线性化后的载波相位双差方程为ξ＝Aη+BN+v,式中：ξ∈R^2k是载波相位双差观测值与伪距双差观测值构成的向量，η∈R³表示基线向量，N∈Z^k表示载波相位双差整周模糊度向量，v∈R^2k表示观测噪声，A∈R^2k×3表示基线常系数矩阵，B∈R^2k×k表示模糊度常系数矩阵；

对所述载波相位双差方程进行求解，若已知前天线接收机坐标为η^a，则后天线接收机坐标η^b的差分估计值为

为基线坐标向量。

4.根据权利要求1所述基于短基线的组合导航定位方法，其特征是，所述INS信息包括载体的经纬度、海拔信息以及姿态角。

5.根据权利要求4所述基于短基线的组合导航定位方法，其特征是，所述进行INS计算，得到INS信息的具体过程为：

基于陀螺仪输出的载体相对于惯性坐标系的角速度和

得到载体坐标系相对于导航坐标系的角速度

将该角速乘采样时间得到当前转动的角度矢量值，将东北天三个方向的角度矢量值进行归一化处理，得到姿态角；

将加速度计的输出值乘采样时间得到当前速度矢量值，将所述速度矢量值进行四元数变换得到载体相对于导航坐标系的速度，经坐标转换，将载体对于地理坐标系的角速度和速度进行归一化处理，将因科里奥利力引起的速度增量修正，计算出东北天三个方向的速度，将前后两次的速度积分计算得到位置增量，与前次位置相加得到当前的经纬度、海拔信息。

6.根据权利要求4所述基于短基线的组合导航定位方法，其特征是，所述通过卡尔曼滤波组合，得到最优的姿态位置信息的具体过程为：

将所述定位信息作为卡尔曼滤波估计系统定位信息状态，建立INS信息的卡尔曼滤波方程，得到组合导航系统的状态向量x及状态方程x(t)；

将PPS信号中的载体速度信息和位置信息作为量测，得到组合导航系统的量测方程，经离散化处理，得到观测方程；

对随机状态变量X在时刻k进行卡尔曼滤波计算，得到载体在k+1时刻位置信息的最优估计。

7.根据权利要求6所述基于短基线的组合导航定位方法，其特征是，所述进行卡尔曼滤波计算的具体过程为：

利用UT变换，得到一组状态采集点及对应的权值；

通过对所述状态采集点的预测值加权求和，得到系统状态的一步预测及协方差矩阵；

对一步预测值进行UT变换，产生新的状态采集点；

将所述新的状态采集点代入观测方程，得到预测的观测值，通过对所述观测值进行加权求和，得到预测的均值及协方差；

计算卡尔曼增益矩阵，更新状态和协方差，所述更新状态即载体在k+1时刻位置信息的最优估计。

8.根据权利要求1所述基于短基线的组合导航定位方法，其特征是，所述定位成功的判断具体为：

接收到的PPS信号中包含完整的定位信息，且天线接收机的卫星可视星数大于4颗。

9.基于短基线的组合导航定位装置，其特征是，所述装置包括多模接收机和INS形成的组合导航系统，以及微型计算机，所述多模接收机用于获取PPS信号，所述微型计算机利用所述PPS信号通过RTK方式进行定位，并计算INS信息，将将成功定位对应的PPS信号及所述INS信息通过卡尔曼滤波组合，得到最优的姿态位置信息。

10.根据权利要求9所述基于短基线的组合导航定位装置，其特征是，所述多模接收机的基线长度为320毫米。

Images