CN115327588A - 一种基于网络rtk的无人自动化作业特种车高精度定位方法 - Google Patents

一种基于网络rtk的无人自动化作业特种车高精度定位方法 Download PDF

Info

Publication number
CN115327588A
CN115327588A CN202210820920.2A CN202210820920A CN115327588A CN 115327588 A CN115327588 A CN 115327588A CN 202210820920 A CN202210820920 A CN 202210820920A CN 115327588 A CN115327588 A CN 115327588A
Authority
CN
China
Prior art keywords
formula
ambiguity
difference
unmanned
special
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202210820920.2A
Other languages
English (en)
Inventor
赵忠义
崔永志
王慎玉
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Liaoning University of Technology
Original Assignee
Liaoning University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Liaoning University of Technology filed Critical Liaoning University of Technology
Priority to CN202210820920.2A priority Critical patent/CN115327588A/zh
Publication of CN115327588A publication Critical patent/CN115327588A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/40—Correcting position, velocity or attitude
    • G01S19/41—Differential correction, e.g. DGPS [differential GPS]
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/13—Receivers
    • G01S19/35—Constructional details or hardware or software details of the signal processing chain
    • G01S19/37—Hardware or software details of the signal processing chain
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于网络RTK的无人自动化作业特种车高精度定位方法，包括以下步骤：步骤1：建立GNSS差分观测方程，消除卫星钟差、大气延迟误差和接收机钟差等误差；步骤2：对整周模糊度进行估计；步骤3：对整周模糊度进行降相关与搜索；步骤4：对模糊度进行检验；步骤5：周跳的探测与修复；步骤6：对无人自动化作业特种车的姿态信息进行感知，同时将运动测量实时信息转换到导航坐标体系下，并对位置和速度实现更新。本发明能够提高定位的精度以及可靠性，极大地满足无人自动化作业特种车在实际应用中的要求。

Description

一种基于网络RTK的无人自动化作业特种车高精度定位方法
技术领域
本发明涉及定位技术领域，具体涉及一种基于网络RTK的无人自动化作业特种车高精度定位方法。
背景技术
RTK，即实时动态差分定位技术(Real-time kinematic)，是利用GPS载波相位观测值对定位结果进行修正，从而进行实时动态相对定位的技术。RTK的基本工作原理：基准站和附近的流动用户之间建立数据通信链路，流动站通过通信链路接收到基准站的载波相位观测值和基准站的位置信息，再利用静态相对测量的处理方法求解基线长度，最后得到待测点的位置信息。随着RTK技术的不断发展，其已经广泛应用到交通运输、工程测绘、城市规划、农业机械和地质灾害监测等领域，并逐渐取代了传统的导航定位方法。
现代社会对导航的定位精度要求越来越高，单一的导航技术会因为其自身的缺陷，导致定位精度不高，难以适应复杂的工作环境，从而达不到现代高精度定位的需求。目前，常用的定位方式主要有：卫星定位、惯性导航定位、视觉里程算法定位、超宽带无线定位等。
美国的GPS系统起步最早，发展也最为完善，与俄罗斯的格洛纳斯导航卫星系统(GLONASS)，我国的北斗导航卫星系统(BDS)以及欧盟的GALILEO系统，统称为全球导航卫星系统。目前，GNSS的位置服务已经渗透到人类社会生活中的方方面面，如自动驾驶、交通运输、工程测绘、农业机械和地质灾害监测等。虽然广泛应用的GNSS定位系统具有覆盖区域广、观测时间短，且在可视卫星数目良好的情况下可以提供稳定的和较高精度的定位等优点，但是一旦进入森林峡谷、高楼林立的城市和结构复杂多变的室内环境中，由于可视卫星数目不足，最终会导致GNSS定位精度很低，甚至出现定位失效的情况。
惯性导航系统(Inertial Navigation System，INS)由惯性测量单元(InertialMeasurement Unit，IMU)和计算机构成，惯性测量单元集中了陀螺仪，加速度计和地磁传感器等基本元件，是现代惯性导航系统的核心。惯性导航是运用牛顿力学原理，测得运载体的加速度，再运用数学中的微分计算的方法得到载体的位置和速度。惯性导航的优点就是，它可以不受外部环境的影响，在传输过程中也不依靠外部的导航电台或者无线电。不过因为惯性元件自身也存在着一定的制造误差，使得惯性导航的定位偏差也会随着时间而逐渐积累。目前由于生产工艺的进展,陀螺仪的准确度也在日益提升,因此平台式的惯性导航系统也正在逐步地被捷联型的惯性导航系统所替代。
视觉里程算法定位就是使用机器视觉获取周围环境的图像，将采集到的图像通过算法处理，提取特征值，最后通过不断地迭代的方式，采用累积求和形成视觉里程计，得到运载体的位置信息。由于摄像头安装在车身，当无人车以比较快地速度运动时，会使摄像头拍摄的图像变得模糊，将导致提取特征值的算法难度增加，甚至使其失效。
超宽带(Ultra Wideband，UWB)无线定位是通过发送和接收具有纳秒级，甚至是低于纳秒级的极窄脉冲来传输数据，从而具有GHz量级的带宽，主要是利用到达基站的时间差进行定位。如果有三个已知坐标点的基站收到信号，发送的脉冲信号距离三个基站的间隔不同，那么这三个基站收到信号的时间点是不一样的。超宽带系统具有较强的穿透力、较好的抗多径效果和很高的安全性等优点，主要应用于室内定位。
无人特种作业车主要用于对观察区域周围的图像、声音、空气质量、目标温度等进行判断的无人作业平台，而如果只是单纯依靠一种导航定位，难以满足无人特种作业车对高精度的要求，而且每种导航方式都存在一定缺陷。就应用最为广泛的惯性导航和卫星导航而言，惯性导航不受外界环境的干扰，但是其误差会随着时间不断积累，而卫星导航则受环境影响比较大，一旦进入遮蔽环境中，很容易造成卫星信号失锁，从而造成定位丢失。因此，实际工程应用中，尤其是针对无人特种作业车，需要一种高精度、高可靠性的定位方法。
发明内容
本发明的目的在于提供一种基于网络RTK的无人自动化作业特种车高精度定位方法，用以解决现有定位方法无法满足无人自动化作业特种车在实际应用中对定位精度、可靠性的要求的问题。
为实现上述目的，本发明的技术方案为：
一种基于网络RTK的无人自动化作业特种车高精度定位方法，包括以下步骤：
步骤1：建立GNSS差分观测方程，消除卫星钟差、大气延迟误差和接收机钟差等误差；
步骤2：对整周模糊度进行估计；
步骤3：对整周模糊度进行降相关与搜索；
步骤4：对模糊度进行检验；
步骤5：周跳的探测与修复；
步骤6：对无人自动化作业特种车的姿态信息进行感知，同时将运动测量实时信息转换到导航坐标体系下，并对位置和速度实现更新。
进一步地，作为优选技术方案，所述步骤1的具体过程为：
步骤1-1：统一时空基准，包括时间系统的统一和坐标系统的统一，统一关系如下：
GPST＝UTC+1s×n-19s (1)
BDT＝GPST-14s (2)
式中，GPST为GPS时，表示GPS主控站的原子钟控制的专用的时间系统；UTC表示协调世界时；n表示调整参数，具体数值由国际地球自转服务局发布；BDT表示北斗时；
步骤1-2：建立伪距和载波相位观测方程：
Figure BDA0003742385530000031
Figure BDA0003742385530000032
式中，上标s表示观测卫星，下标r表示接收机；p和
Figure BDA0003742385530000033
分别为伪距和载波相位观测量；ρ为卫星信号发射时刻与接收机的真实距离；c为真空中的光速；dtr和dts分别为接收机钟差和卫星钟差；T为信号在大气中传播过程中的对流层误差；I为信号在大气中传播过程中的电离层误差；λ为载波相位波长；N为以周为单位的整周模糊度；εp和
Figure BDA0003742385530000034
分别为伪距和载波相位的其他误差(包括噪声误差)；
步骤1-3：建立单差观测模型，基准站r与移动站q进行站间差分，基准站接收机r和移动站接收机q对同一颗卫星s进行差分计算，其中，基准站r的伪距和载波相位观测方程为式(3)和(4)，而移动站q的伪距和载波相位观测方程如下：
Figure BDA0003742385530000035
Figure BDA0003742385530000036
式(3)与式(5)作差，以及式(4)与式(6)作差可以卫星钟差dtq，从而得到单差模型：
Figure BDA0003742385530000041
Figure BDA0003742385530000042
式(7)和式(8)中，Δ表示单差，对于短基线情况下，基准站和移动站的电离层延迟误差和对流层延迟误差相差很小，所以可以近似将式(7)和式(8)中对流层和电离层的差值等于0，可以得到：
Figure BDA0003742385530000043
Figure BDA0003742385530000044
步骤1-4：建立双差观测模型，设参考卫星y，则基准站r和移动站q的单差方程与式(9)和(10)类似，
Figure BDA0003742385530000045
Figure BDA0003742385530000046
将式(9)和(11)，(10)和(12)作差，消除了接收机钟差项得到双差观测方程：
Figure BDA0003742385530000047
Figure BDA0003742385530000048
式(13)、(14)中，
Figure BDA0003742385530000049
表示双差符号，双差整周模糊度保留了整数特性，并且可以有效分离；
设有N个可用卫星，则载波相位和伪距的观测方程的个数为2(N-1)个，其中，未知量包括基线双差伪距的3个方向坐标，还有N-1个双差模糊度，所以根据数学上求解方程的原则：方程数大于等于未知数。为了求解模糊度的浮点解，则需要：2(N-1)≥3+(N-1)，解得N＝4，所以同时观察到4颗可视卫星，是单系统实现差分定位的最低要求；
当以4颗卫星a，b，c，d进行解算时，加入伪距双差观测量，可以建立一个6维的方程组，以大高度角的卫星a作为主星，则得到：
Figure BDA0003742385530000051
其中，[l m n]为卫星到接收机的单位向量，[δX δY δZ]为接收机概略坐标改正数，▽ΔR为双差伪距观测值，将式(15)写成矩阵形式：
Y＝HX (16)
式(16)中，H为接收机到卫星的单位矢量构成的设计矩阵，X为待求的基线矢量和模糊度解，利用最小二乘法可以解算出模糊度的浮点解：
Figure BDA0003742385530000052
式(17)中，
Figure BDA0003742385530000053
为模糊度的浮点解，令基线矢量
Figure BDA0003742385530000054
双差整周模糊度矢量
Figure BDA0003742385530000055
最小二乘结果可以表示为：
Figure BDA0003742385530000056
Figure BDA0003742385530000057
在式(19)中，Q表示向量X的协方差矩阵，
Figure BDA0003742385530000058
表示基线矢量的协方差矩阵，
Figure BDA0003742385530000059
表示模糊度矢量的协方差矩阵，
Figure BDA00037423855300000510
和
Figure BDA00037423855300000511
表示基线矢量和模糊度矢量的互协方差矩阵。
进一步地，作为优选技术方案，所述步骤2的具体过程为：
步骤2-1：将浮点解
Figure BDA00037423855300000512
分解为基线矢量和整周模糊度矢量的形式，得到：
Figure BDA00037423855300000513
式(20)中，y为伪距和载波相位双差观测量，
Figure BDA00037423855300000514
为基线矢量，M为基线矢量的系数矩阵，N为整周模糊度系数矩阵，
Figure BDA00037423855300000515
为双差整周模糊度，ε为未建模的误差量；
步骤2-2：将求解模糊度问题转化为最小二乘问题：
Figure BDA00037423855300000516
式(21)中，Qy是y的协方差矩阵；
步骤2-3：通过式(18)和式(19)得到整周模糊度浮点解
Figure BDA0003742385530000061
和协方差矩阵Q，然后利用模糊度矢量浮点解
Figure BDA0003742385530000062
和协方差矩阵
Figure BDA0003742385530000063
估计模糊度的整数解
Figure BDA0003742385530000064
使模糊度残余平方和最小，得到：
Figure BDA0003742385530000065
进一步地，作为优选技术方案，所述步骤3的具体过程为：
设变换矩阵z满足：
Figure BDA0003742385530000066
则其对应的协方差矩阵为：
Figure BDA0003742385530000067
式(24)中，Z为一个模为1的整数矩阵，该矩阵的所有元素均为整数，且模值为1，|det(Z)|＝1，降到各个模糊度的相关性可以通过选择合适的Z矩阵，变换后，使目标函数Ω最小；
Figure BDA0003742385530000068
得到满足式(25)的整数矩阵最优解，设置搜索空间χ2，即为：
Figure BDA0003742385530000069
搜索空间是一个多维椭球体，形状与协方差阵
Figure BDA00037423855300000610
有关，其大小由常量χ决定。
进一步地，作为优选技术方案，所述步骤4的具体过程为：
设搜索到的残差最小值为Ωmin，则残差次最小值为Ωsec，则可以得到：
Figure BDA00037423855300000611
式(27)中，k为经验常数，不能随模型强度、测量环境等的变化而变化，通常k＝3作为检验的判断条件，ratio≥3，则认为搜索得到的最优解是正确解；
通过模糊度搜索，得到整周模糊度固定解
Figure BDA00037423855300000612
通过逆变换，可以得到原始双差模糊度的固定解
Figure BDA00037423855300000613
Figure BDA00037423855300000614
通过整周模糊度求解得到的双差整周模糊度矢量
Figure BDA0003742385530000071
利用模糊度的整数特性，进一步提高基线矢量估计精度，由LAMBDA搜索得到的
Figure BDA0003742385530000072
可以得到：
Figure BDA0003742385530000073
Figure BDA0003742385530000074
上式中，
Figure BDA0003742385530000075
为基线矢量改正数的固定解，
Figure BDA0003742385530000076
为
Figure BDA0003742385530000077
的协方差矩阵。
根据上述结果可以得到
Figure BDA0003742385530000078
流动站的精确三维坐标(Xq,Yq,Zq)T为：
(Xq,Yq,Zq)T＝(Xr0,Yr0,Zr0)T+(δXr,δYr,δZr)T (31)。
进一步地，作为优选技术方案，所述步骤6的具体过程为：
步骤6-1：建立无人自动化作业特种车的SINS坐标系，包括载体坐标系、惯性坐标系以及地理坐标系；
步骤6-2：通过采用四元数法计算出无人自动化作业特种车的姿态参数，包括无人特种作业车的航向角、俯仰角、横滚角，并对捷联式惯性导航的姿态进行更新；
步骤6-3：对捷联式惯性导航的速度进行更新；
步骤6-4：对捷联式惯性导航的位置进行更新。
进一步地，作为优选技术方案，所述步骤6-2的具体过程为：
四元数为一个四维数组，由四个元素构成，包含三个虚单位和一个实单位，其数学表达式如下所示：
Figure BDA0003742385530000079
式(32)中，q0、q1、q2和q3为任意实数，
Figure BDA00037423855300000710
和
Figure BDA00037423855300000711
是互相垂直的单位向量；四元数的矩阵式为：
Figure BDA00037423855300000712
式(33)中，θ为角度，
Figure BDA0003742385530000081
可以得到四元数表示的旋转矩阵为：
Figure BDA0003742385530000082
由n旋转到b的过程中，由于两个坐标系中的三条坐标轴两两互相垂直，
Figure BDA0003742385530000083
和
Figure BDA0003742385530000084
表示了两个互逆的坐标系旋转过程，二者的关系如下：
Figure BDA0003742385530000085
式(35)中，ψ为无人特种作业车的航向角；θ为无人特种作业车的俯仰角；γ为无人特种作业车的横滚角；联立式(33)和式(34)得：
Figure BDA0003742385530000086
γ＝arcsin(-C13)＝arcsin(-2(q1q3-q0q2)) (37)
Figure BDA0003742385530000087
观察上式可以得到，只要得到四元数q0、q1、q2和q3的数值，就能计算出无人特种作业车的姿态角ψ、俯仰角θ和横滚角γ的值。
进一步地，作为优选技术方案，所述步骤6-3的具体过程为
根据比力方程得到速度方程为：
Figure BDA0003742385530000088
在式(39)中，fb为加速度计输出；
Figure BDA0003742385530000089
为哥氏加速度；
Figure BDA00037423855300000810
为牵引加速度，通过积分运算可以完成速度更新，具体表达式为：
Figure BDA00037423855300000811
式(40)中，Vm-1为前一时刻的速度；ΔVsmf为比力积分增量；ΔVg/corm为有害加速度引起的速度增量；
Figure BDA0003742385530000091
从式(39)至式(41)即为惯性导航的速度更新。
进一步地，作为优选技术方案，所述步骤6-4的具体过程为：
通过导航坐标系下的无人自动化作业特种车的速度，经过积分运算得到无人车所在的经纬度和高度，具体公式如下：
Figure BDA0003742385530000092
Figure BDA0003742385530000093
Figure BDA0003742385530000094
其中，L代表纬度，Lm代表m时刻纬度，Lm-1代表m-1时刻纬度；λm代表m时刻经度；λm-1代表m-1时刻经度；hm代表无人车m时刻高度，hm-1代表无人车m-1时刻高度；tm代表m时刻，tm-1代表m-1时刻；
Figure BDA0003742385530000095
代表x方向速度n次方；
Figure BDA0003742385530000096
代表y方向速度n次方；
Figure BDA0003742385530000097
代表z方向速度n次方；Rn代表地球半径；h代表卫星高度。
进一步地，作为优选技术方案，还包括初始粗对准步骤，具体为：由于陀螺零偏远大于地球自转角速度，所以陀螺很难感知到地球的自转角速度，因此无法估计初始航向角的大小；由于加速度计有比较高的精度，可以通过加速度计来感知重力加速度的分离通过加速度计来估算初始水平姿态角：俯仰角θ和横滚角γ；
Figure BDA0003742385530000098
式(45)左边三个元素为载体坐标系下加速度计的输出；而右边中括号里的三个元素为导航坐标系下重力加速度的投影，在静态的情形下，一般粗对准中重力加速度只在Z轴中有投影为-g，其他轴的投影均为0，即为gn＝[0 0 -g]T；已知
Figure BDA0003742385530000099
带入式(2.50)得到：
Figure BDA00037423855300000910
Figure BDA00037423855300000911
Figure BDA0003742385530000101
g为重力加速度，航向角ψ为未知量，可以反解出：
Figure BDA0003742385530000102
Figure BDA0003742385530000103
本发明相对于现有技术，具有如下有益效果：
(1)本发明利用GPS双天线和MEMS惯性单元进行组合导航定位，通过创建推导单差和双差观测方程，消除了主要误差，再结合整周模糊度的搜索与检验，以及周跳的探测与修复，有效提高了定位的精度以及可靠性，能够极大地满足了无人自动化作业特种车在实际应用中的要求。
(2)本发明通过采用初始粗对准估算出捷联惯性导航系统一定精度的初始姿态，便于进一步确定初始姿态转换矩形，从而尽可能减小起始点的误差，避免出现导航系统后续的精度越来越差的情况。
附图说明
图1为本发明的方法流程示意图；
图2为本发明的单差观测模型；
图3为本发明的双差观测模型；
图4为本发明的整周模糊度的搜索流程示意图；
图5为本发明的相位周跳示意图；
图6为捷联惯性导航系统结构示意图。
具体实施方式
下面结合实施例及附图，对本发明作进一步的详细说明，但本发明的实施方式不限于此。
实施例
如图1所示，本实施例所示的一种基于网络RTK的无人自动化作业特种车高精度定位方法，包括以下步骤：
步骤1：建立GNSS差分观测方程，消除卫星钟差、大气延迟误差和接收机钟差等误差；
步骤2：对整周模糊度进行估计；
步骤3：对整周模糊度进行降相关与搜索；
步骤4：对模糊度进行检验；
步骤5：周跳的探测与修复；
步骤6：对无人自动化作业特种车的姿态信息进行感知，同时将运动测量实时信息转换到导航坐标体系下，并对位置和速度实现更新。
本实施例的具体方法如下：
首先是时空基准的统一，包括时间系统和坐标系统的统一：
(1)时间系统的统一
GPS建立了由GPS主控站的原子钟控制的专用的时间系统，即为GPS时(GPS Time，GPST)，GPST与协调世界时(Universal Time Coordinated，UTC)的时刻规定，起始历元为UTC的1986年1月6日00时00分00秒，以后时间不断地积累，GPST和UTC之间相差为秒的整数倍，GPST与UTC之间的关系为：
GPST＝UTC+1s×n-19s (1)
BDT＝GPST-14s (2)
式中，GPST为GPS时，表示GPS主控站的原子钟控制的专用的时间系统；UTC表示协调世界时；n表示调整参数，具体数值由国际地球自转服务局发布；BDT表示北斗时；北斗时(BeiDou Time，BDT)的起始历元为2006年1月1日00时00分00秒，采用国际单位制(SI)秒为基本单位连续累计，不闰秒。BDT溯源到中国科学院国家授时中心(National Time ServiceCenter，NTSC)维持的协调世界时。GPST比BDT快了14整秒，是因为两者的起始历元不同。而对于坐标系统的统一，WGS-84坐标系与CGCS2000坐标系是一致的，在4个基本常数中，只有扁率略有差异，扁率的差异不影响精度的变化，影响大地纬度和大地高度产生0.1mm左右的差异，然而在目前的定位精度要求的情况下，这种差异几乎可以忽略不计，故WGS-84坐标系和CGCS2000坐标系可以认为是一致的。
在GNSS数据处理中，接收机的原始观测量和影响信号传播的各种因素之间的函数关系式就是GNSS观测方程，在GNSS定位中卫星不断播发导航信号，接收机对收到的伪距、载波相位和多普勒等信息进行算法计算，实现导航定位。伪距和载波相位观测量是两类原始观测量，其观测方程如下：
Figure BDA0003742385530000121
Figure BDA0003742385530000122
式中，上标s表示观测卫星，下标r表示接收机；p和
Figure BDA0003742385530000123
分别为伪距和载波相位观测量；ρ为卫星信号发射时刻与接收机的真实距离；c为真空中的光速；dtr和dts分别为接收机钟差和卫星钟差；T为信号在大气中传播过程中的对流层误差；I为信号在大气中传播过程中的电离层误差；λ为载波相位波长；N为以周为单位的整周模糊度；εp和
Figure BDA0003742385530000124
分别为伪距和载波相位的其他误差(包括噪声误差)；
建立单差观测模型。基准站r与移动站q进行站间差分，基准站接收机r和移动站接收机q对同一颗卫星s进行差分计算，单差观测模型如图2所示。其中，基准站r的伪距和载波相位观测方程为式(3)和(4)，而移动站q的伪距和载波相位观测方程如下：
Figure BDA0003742385530000125
Figure BDA0003742385530000126
式(3)与式(5)作差，以及式(4)与式(6)作差可以卫星钟差dtq，从而得到单差模型：
Figure BDA0003742385530000127
Figure BDA0003742385530000128
式(7)和式(8)中，Δ表示单差，对于短基线情况下，基准站和移动站的电离层延迟误差和对流层延迟误差相差很小，所以可以近似将式(7)和式(8)中对流层和电离层的差值等于0，可以得到：
Figure BDA0003742385530000129
Figure BDA00037423855300001210
建立双差观测模型。如图3所示，双差是再对另一颗参考卫星进行星间差分的方法，大气延迟误差和接收机钟差都可以通过双差观测模型消除，设参考卫星y，则基准站r和移动站q的单差方程与式(9)和(10)类似，
Figure BDA00037423855300001211
Figure BDA0003742385530000131
将式(9)和(11)，(10)和(12)作差，消除了接收机钟差项得到双差观测方程：
Figure BDA0003742385530000132
Figure BDA0003742385530000133
式(13)、(14)中，
Figure BDA0003742385530000134
表示双差符号，双差整周模糊度保留了整数特性，并且可以有效分离；
设有N个可用卫星，则载波相位和伪距的观测方程的个数为2(N-1)个，其中，未知量包括基线双差伪距的3个方向坐标，还有N-1个双差模糊度，所以根据数学上求解方程的原则：方程数大于等于未知数。为了求解模糊度的浮点解，则需要：2(N-1)≥3+(N-1)，解得N＝4，所以同时观察到4颗可视卫星，是单系统实现差分定位的最低要求；
当以4颗卫星a，b，c，d进行解算时，加入伪距双差观测量，可以建立一个6维的方程组，以大高度角的卫星a作为主星，则得到：
Figure BDA0003742385530000135
其中，[l m n]为卫星到接收机的单位向量，[δX δY δZ]为接收机概略坐标改正数，
Figure BDA0003742385530000136
为双差伪距观测值，将式(15)写成矩阵形式：
Y＝HX (16)
式(16)中，H为接收机到卫星的单位矢量构成的设计矩阵，X为待求的基线矢量和模糊度解，利用最小二乘法可以解算出模糊度的浮点解：
Figure BDA0003742385530000137
式(17)中，
Figure BDA0003742385530000138
为模糊度的浮点解，令基线矢量
Figure BDA0003742385530000139
双差整周模糊度矢量
Figure BDA00037423855300001310
最小二乘结果可以表示为：
Figure BDA0003742385530000141
Figure BDA0003742385530000142
在式(19)中，Q表示向量X的协方差矩阵，
Figure BDA0003742385530000143
表示基线矢量的协方差矩阵，
Figure BDA0003742385530000144
表示模糊度矢量的协方差矩阵，
Figure BDA0003742385530000145
和
Figure BDA0003742385530000146
表示基线矢量和模糊度矢量的互协方差矩阵。
本实施例的整周模糊度估计的具体过程为：
将浮点解
Figure BDA0003742385530000147
分解为基线矢量和整周模糊度矢量的形式，得到：
Figure BDA0003742385530000148
式(20)中，y为伪距和载波相位双差观测量，
Figure BDA0003742385530000149
为基线矢量，M为基线矢量的系数矩阵，N为整周模糊度系数矩阵，
Figure BDA00037423855300001410
为双差整周模糊度，ε为未建模的误差量；
将求解模糊度问题转化为最小二乘问题：
Figure BDA00037423855300001411
式(21)中，Qy是y的协方差矩阵；
若模糊度相互独立，则模糊度的协方差阵为对角阵，其最优解可以通过就近取整的方法获得。但是，一般情况下，因模糊度之间存在较强的相关性，模糊度协方差阵并非对角阵，这就导致通过四舍五入方法的成功率较低。通过式(18)和式(19)得到整周模糊度浮点解
Figure BDA00037423855300001412
和协方差矩阵Q，然后利用模糊度矢量浮点解
Figure BDA00037423855300001413
和协方差矩阵
Figure BDA00037423855300001414
估计模糊度的整数解
Figure BDA00037423855300001415
使模糊度残余平方和最小，得到：
Figure BDA00037423855300001416
本实施例的模糊度降相关与搜索的具体过程为：
如图4所示，通过Z变换来实现将
Figure BDA00037423855300001417
变成对角矩阵，使模糊度搜索变得简单，并且缩短了搜索时间，设变换矩阵z满足：
Figure BDA00037423855300001418
则其对应的协方差矩阵为：
Figure BDA00037423855300001419
式(24)中，Z为一个模为1的整数矩阵，该矩阵的所有元素均为整数，且模值为1，|det(Z)|＝1，降到各个模糊度的相关性可以通过选择合适的Z矩阵，变换后，使目标函数Ω最小；
Figure BDA0003742385530000151
得到满足式(25)的整数矩阵最优解，设置搜索空间χ2，即为：
Figure BDA0003742385530000152
搜索空间是一个多维椭球体，形状与协方差阵
Figure BDA0003742385530000153
有关，其大小由常量χ决定，若χ赋值过大，那么搜索空间中的取值就会过多，导致搜索效率降低；反之，若χ的值取的过小，搜索空间中可能无法包含完整的模糊度整数解，所以χ的取值至关重要。
本实施例的模糊度检验的具体过程为：
设搜索到的残差最小值为Ωmin，则残差次最小值为Ωsec，则可以得到：
Figure BDA0003742385530000154
式(27)中，k为经验常数，不能随模型强度、测量环境等的变化而变化，通常k＝3作为检验的判断条件，ratio≥3，则认为搜索得到的最优解是正确解；
通过模糊度搜索，得到整周模糊度固定解
Figure BDA0003742385530000155
通过逆变换，可以得到原始双差模糊度的固定解
Figure BDA0003742385530000156
Figure BDA0003742385530000157
通过整周模糊度求解得到的双差整周模糊度矢量
Figure BDA0003742385530000158
利用模糊度的整数特性，进一步提高基线矢量估计精度，由LAMBDA搜索得到的
Figure BDA0003742385530000159
可以得到：
Figure BDA00037423855300001510
Figure BDA00037423855300001511
上式中，
Figure BDA00037423855300001512
为基线矢量改正数的固定解，
Figure BDA00037423855300001513
为
Figure BDA00037423855300001514
的协方差矩阵。
根据上述结果可以得到
Figure BDA00037423855300001515
流动站的精确三维坐标(Xq,Yq,Zq)T为：
(Xq,Yq,Zq)T＝(Xr0,Yr0,Zr0)T+(δXr,δYr,δZr)T (31)。
本实施例中，在接收机进行连续的载波相位测量的过程中，往往由于接收机所处的环境会导致信号被遮挡、接收机发生急剧的抖动和信号的信噪比过低等原因，会导致整周计数发生错误，出现整周跳变的现象，这就是周跳(Cycle Slip)，如图5所示，原本相位值是随时间变化的连续曲线函数，当发生周跳时，该曲线会发生显著变化，t1历元对应载波相位观测值
Figure BDA0003742385530000161
和
Figure BDA0003742385530000162
随后的相位观测仍然包括丢失的周跳值。
由于周跳对GNSS数据质量会产生重要影响，所以在GNSS数据处理中，周跳的探测也是一个比较重要的问题。同时，由于周跳发生的任意性和噪声等因素的影响，所以周跳相关的问题也是比较难解决的。目前常用的周跳探测与修复方法有许多种，但通常可以分为以下三类：
(1)利用观测值伴随时间变化的方法，如多项式拟合法；
(2)利用组合不同的观测值的方法，如电离层残差法；
(3)利用观测值估值残差的方法，如卡尔曼滤波法和历元间差分观测值参数估计法等。
捷联式惯性导航系统SINS由于具有体积小、可靠性高和结构简单等特点，所以发展非常迅速。其主要分为位置更新、速度更新和姿态更新三大部分。姿态更新是SINS的核心，其不仅是感知载体姿态信息，还将运动测量实时信息转换到导航坐标系下，实现位置与速度更新。具体解算过程如下：通过初始对准，能够得到载体的初始姿态、位置和速度等信息，并以此作为导航计算的初始信息；再对导航坐标系下的加速度进行积分运算，得到实时速度和位置信息，然后求解更新后的方向余弦矩阵或四元数，最终得出无人车姿态信息，捷联惯导系统原理图如图6所示。
在无人自动化作业特种车的导航系统中，由于传感器测量值和导航系统最终输出的姿态观测值所参考的坐标系不同，本SINS所使用的坐标系包括：
(1)载体坐标系(b)：顾名思义，载体坐标系是惯导模块安装在运动载体上的坐标系，与载体的运动状态保持一致，其定义如下：
原点O：无人特种作业车的质心；
X轴：平行于地面并指向无人特种作业车的前进方向；
Z轴：通过无人特种作业车的质心竖直指向上方；
Y轴：遵循右手定则且垂直于XOZ平面。
(2)惯性坐标系(i)：属于空间固定坐标系，在惯性坐标系中，空间点的位置由(X,Y,Z)来表示；其定义如下：
原点O：地球地心；
X轴：指向春分点；
Z轴：指向地球北极；
Y轴：遵循右手定则且垂直于XOZ平面，平面XOY位于赤道平面。
(3)地理坐标系(g)：选用东北天坐标系；
原点O：无人特种作业车的重心；
X轴：指向东方向，即为E；
Z轴：沿地垂线指向天，即为U；
Y轴：指向北方向，即为N。
本实施例中，捷联式惯性导航系统的姿态包括航向角、俯仰角和横滚角。无人车以东北天坐标系为参考系的方向信息，姿态矩阵
Figure BDA0003742385530000171
表示无人车(载体)坐标系到东北天坐标系的旋转关系。目前，常用的姿态解决方法有欧拉角法、方向余弦矩阵和四元数法三种方法，由于四元数法是以4个变量来表征姿态信息，相较于另外两种方法，在姿态信息的实时更新方面十分方便，而且四元数算法与基于泊松方程设计的算法相比，可以将计算量平均降低30％，所以选择四元数法作为惯性传感器获取姿态的主要方法。
从数学的角度来看四元数本质上就是一个四维数组，由四个元素构成，包含三个虚单位和一个实单位。其数学表达式如下所示：
Figure BDA0003742385530000172
式(32)中，q0、q1、q2和q3为任意实数，
Figure BDA0003742385530000173
和
Figure BDA0003742385530000174
是互相垂直的单位向量；四元数的矩阵式为：
Figure BDA0003742385530000175
式(33)中，θ为角度，
Figure BDA0003742385530000176
可以得到四元数表示的旋转矩阵为：
Figure BDA0003742385530000177
由n旋转到b的过程中，由于两个坐标系中的三条坐标轴两两互相垂直，
Figure BDA0003742385530000181
和
Figure BDA0003742385530000182
表示了两个互逆的坐标系旋转过程，二者的关系如下：
Figure BDA0003742385530000183
式(35)中，ψ为无人特种作业车的航向角；θ为无人特种作业车的俯仰角；γ为无人特种作业车的横滚角；联立式(33)和式(34)得：
Figure BDA0003742385530000184
γ＝arcsin(-C13)＝arcsin(-2(q1q3-q0q2)) (37)
Figure BDA0003742385530000185
观察上式可以得到，只要得到四元数q0、q1、q2和q3的数值，就能计算出无人特种作业车的姿态角ψ、俯仰角θ和横滚角γ的值。
本实施例的捷联式惯性导航系统实现速度更新的具体过程为：
根据比力方程得到速度方程为：
Figure BDA0003742385530000186
在式(39)中，fb为加速度计输出；
Figure BDA0003742385530000187
为哥氏加速度；
Figure BDA0003742385530000188
为牵引加速度，通过积分运算可以完成速度更新，具体表达式为：
Figure BDA0003742385530000189
式(40)中，Vm-1为前一时刻的速度；ΔVsmf为比力积分增量；ΔVg/corm为有害加速度引起的速度增量；
Figure BDA00037423855300001810
从式(39)至式(41)即为惯性导航的速度更新。
本实施例捷联式惯性导航系统实现位置更新的具体过程为：
通过导航坐标系下的无人自动化作业特种车的速度，经过积分运算得到无人车所在的经纬度和高度，具体公式如下：
Figure BDA0003742385530000191
Figure BDA0003742385530000192
Figure BDA0003742385530000193
其中，L代表纬度，Lm代表m时刻纬度，Lm-1代表m-1时刻纬度；λm代表m时刻经度；λm-1代表m-1时刻经度；hm代表无人车m时刻高度，hm-1代表无人车m-1时刻高度；tm代表m时刻，tm-1代表m-1时刻；
Figure BDA0003742385530000194
代表x方向速度n次方；
Figure BDA0003742385530000195
代表y方向速度n次方；
Figure BDA0003742385530000196
代表z方向速度n次方；Rn代表地球半径；h代表卫星高度。
捷联惯导系统定位精度的好坏，很大一部分原因取决于初始对准的效果，如果没有初始对准，起始点的误差越大，会导致导航系统后续的精度越差，为此，本实施例还包括初始粗对准步骤，具体为：由于陀螺零偏远大于地球自转角速度，所以陀螺很难感知到地球的自转角速度，因此无法估计初始航向角的大小；由于加速度计有比较高的精度，可以通过加速度计来感知重力加速度的分离通过加速度计来估算初始水平姿态角：俯仰角θ和横滚角γ；
Figure BDA0003742385530000197
式(45)左边三个元素为载体坐标系下加速度计的输出；而右边中括号里的三个元素为导航坐标系下重力加速度的投影，在静态的情形下，一般粗对准中重力加速度只在Z轴中有投影为-g，其他轴的投影均为0，即为gn＝[0 0 -g]T；已知
Figure BDA0003742385530000198
带入式(2.50)得到：
Figure BDA0003742385530000199
Figure BDA00037423855300001910
Figure BDA00037423855300001911
g为重力加速度，航向角ψ为未知量，可以反解出：
Figure BDA00037423855300001912
Figure BDA0003742385530000201
本发明采用上述方法，通过实现对BDS和GPS的时间系统和坐标系统的统一，并推导出单差和双差观测方程，再结合整周模糊度的检索与检验，以及周跳的探测与修复，有效提高了定位的精准度以及可靠性，能够极大地满足了无人自动化作业特种车在实际应用中的要求。
虽然，上文中已经用一般性说明及具体实施例对本发明作了详尽的描述，但在本发明基础上，可以对之作一些修改或改进，这对本领域技术人员而言是显而易见的。因此，在不偏离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。

Claims (10)

1.一种基于网络RTK的无人自动化作业特种车高精度定位方法，其特征在于，包括以下步骤：
步骤1：建立GNSS差分观测方程，消除卫星钟差、大气延迟误差和接收机钟差等误差；
步骤2：对整周模糊度进行估计；
步骤3：对整周模糊度进行降相关与搜索；
步骤4：对模糊度进行检验；
步骤5：周跳的探测与修复；
步骤6：对无人自动化作业特种车的姿态信息进行感知，同时将运动测量实时信息转换到导航坐标体系下，并对位置和速度实现更新。
2.根据权利要求1所述的一种基于网络RTK的无人自动化作业特种车高精度定位方法，其特征在于，所述步骤1的具体过程为：
步骤1-1：统一时空基准，包括时间系统的统一和坐标系统的统一，其中时间系统的统一关系如下：
GPST＝UTC+1s×n-19s (1)
BDT＝GPST-14s (2)
式中，GPST为GPS时，表示GPS主控站的原子钟控制的专用的时间系统；UTC表示协调世界时；n表示调整参数，具体数值由国际地球自转服务局发布；BDT表示北斗时；
步骤1-2：建立伪距和载波相位观测方程：
Figure FDA0003742385520000011
Figure FDA0003742385520000012
式中，上标s表示观测卫星，下标r表示接收机；p和
Figure FDA0003742385520000013
分别为伪距和载波相位观测量；ρ为卫星信号发射时刻与接收机的真实距离；c为真空中的光速；dtr和dts分别为接收机钟差和卫星钟差；T为信号在大气中传播过程中的对流层误差；I为信号在大气中传播过程中的电离层误差；λ为载波相位波长；N为以周为单位的整周模糊度；εp和
Figure FDA0003742385520000021
分别为伪距和载波相位的其他误差(包括噪声误差)；
步骤1-3：建立单差观测模型，基准站r与移动站q进行站间差分，基准站接收机r和移动站接收机q对同一颗卫星s进行差分计算，其中，基准站r的伪距和载波相位观测方程为式(3)和(4)，而移动站q的伪距和载波相位观测方程如下：
Figure FDA0003742385520000022
Figure FDA0003742385520000023
式(3)与式(5)作差，以及式(4)与式(6)作差可以卫星钟差dtq，从而得到单差模型：
Figure FDA0003742385520000024
Figure FDA0003742385520000025
式(7)和式(8)中，Δ表示单差，对于短基线情况下，基准站和移动站的电离层延迟误差和对流层延迟误差相差很小，所以可以近似将式(7)和式(8)中对流层和电离层的差值等于0，可以得到：
Figure FDA0003742385520000026
Figure FDA0003742385520000027
步骤1-4：建立双差观测模型，设参考卫星y，则基准站r和移动站q的单差方程与式(9)和(10)类似，
Figure FDA0003742385520000028
Figure FDA0003742385520000029
将式(9)和(11)，(10)和(12)作差，消除了接收机钟差项得到双差观测方程：
Figure FDA00037423855200000210
Figure FDA00037423855200000211
式(13)、(14)中，▽Δ表示双差符号，双差整周模糊度保留了整数特性，并且可以有效分离；
设有N个可用卫星，则载波相位和伪距的观测方程的个数为2(N-1)个，其中，未知量包括基线双差伪距的3个方向坐标，还有N-1个双差模糊度，所以根据数学上求解方程的原则：方程数大于等于未知数；为了求解模糊度的浮点解，则需要：2(N-1)≥3+(N-1)，解得N＝4，所以同时观察到4颗可视卫星，是单系统实现差分定位的最低要求；
当以4颗卫星a，b，c，d进行解算时，加入伪距双差观测量，可以建立一个6维的方程组，以大高度角的卫星a作为主星，则得到：
Figure FDA0003742385520000031
其中，[l m n]为卫星到接收机的单位向量，[δX δY δZ]为接收机概略坐标改正数，▽ΔR为双差伪距观测值，将式(15)写成矩阵形式：
Y＝HX (16)
式(16)中，H为接收机到卫星的单位矢量构成的设计矩阵，X为待求的基线矢量和模糊度解，利用最小二乘法可以解算出模糊度的浮点解：
Figure FDA0003742385520000032
式(17)中，
Figure FDA0003742385520000033
为模糊度的浮点解，令基线矢量
Figure FDA0003742385520000034
双差整周模糊度矢量
Figure FDA0003742385520000035
最小二乘结果可以表示为：
Figure FDA0003742385520000036
Figure FDA0003742385520000037
在式(19)中，Q表示向量X的协方差矩阵，
Figure FDA0003742385520000038
表示基线矢量的协方差矩阵，
Figure FDA0003742385520000039
表示模糊度矢量的协方差矩阵，
Figure FDA00037423855200000310
和
Figure FDA00037423855200000311
表示基线矢量和模糊度矢量的互协方差矩阵。
3.根据权利要2所述的一种基于网络RTK的无人自动化作业特种车高精度定位方法，其特征在于，所述步骤2的具体过程为：
步骤2-1：将浮点解
Figure FDA00037423855200000312
分解为基线矢量和整周模糊度矢量的形式，得到：
Figure FDA00037423855200000313
式(20)中，y为伪距和载波相位双差观测量，
Figure FDA0003742385520000041
为基线矢量，M为基线矢量的系数矩阵，N为整周模糊度系数矩阵，
Figure FDA0003742385520000042
为双差整周模糊度，ε为未建模的误差量；
步骤2-2：将求解模糊度问题转化为最小二乘问题：
Figure FDA0003742385520000043
式(21)中，Qy是y的协方差矩阵；
步骤2-3：通过式(18)和式(19)得到整周模糊度浮点解
Figure FDA0003742385520000044
和协方差矩阵Q，然后利用模糊度矢量浮点解
Figure FDA0003742385520000045
和协方差矩阵
Figure FDA0003742385520000046
估计模糊度的整数解
Figure FDA0003742385520000047
使模糊度残余平方和最小，得到：
Figure FDA0003742385520000048
4.根据权利要求3所述的一种基于网络RTK的无人自动化作业特种车高精度定位方法，其特征在于，所述步骤3的具体过程为：
设变换矩阵z满足：
Figure FDA0003742385520000049
则其对应的协方差矩阵为：
Figure FDA00037423855200000410
式(24)中，Z为一个模为1的整数矩阵，该矩阵的所有元素均为整数，且模值为1，|det(Z)|＝1，降到各个模糊度的相关性可以通过选择合适的Z矩阵，变换后，使目标函数Ω最小；
Figure FDA00037423855200000411
得到满足式(25)的整数矩阵最优解，设置搜索空间χ2，即为：
Figure FDA00037423855200000412
搜索空间是一个多维椭球体，形状与协方差阵
Figure FDA00037423855200000413
有关，其大小由常量χ决定。
5.根据权利要求4所述的一种基于网络RTK的无人自动化作业特种车高精度定位方法，其特征在于，所述步骤4的具体过程为：
设搜索到的残差最小值为Ωmin，则残差次最小值为Ωsec，则可以得到：
Figure FDA00037423855200000414
式(27)中，k为经验常数，不能随模型强度、测量环境等的变化而变化，通常k＝3作为检验的判断条件，ratio≥3，则认为搜索得到的最优解是正确解；
通过模糊度搜索，得到整周模糊度固定解
Figure FDA0003742385520000051
通过逆变换，可以得到原始双差模糊度的固定解
Figure FDA0003742385520000052
Figure FDA0003742385520000053
通过整周模糊度求解得到的双差整周模糊度矢量
Figure FDA0003742385520000054
利用模糊度的整数特性，进一步提高基线矢量估计精度，由LAMBDA搜索得到的
Figure FDA0003742385520000055
可以得到：
Figure FDA0003742385520000056
Figure FDA0003742385520000057
上式中，
Figure FDA0003742385520000058
为基线矢量改正数的固定解，
Figure FDA0003742385520000059
为
Figure FDA00037423855200000510
的协方差矩阵。
根据上述结果可以得到
Figure FDA00037423855200000511
流动站的精确三维坐标(Xq,Yq,Zq)T为：
(Xq,Yq,Zq)T＝(Xr0,Yr0,Zr0)T+(δXr,δYr,δZr)T (31)。
6.根据权利要求5所述的一种基于网络RTK的无人自动化作业特种车高精度定位方法，其特征在于，所述步骤6的具体过程为：
步骤6-1：建立无人自动化作业特种车的SINS坐标系，包括载体坐标系、惯性坐标系以及地理坐标系；
步骤6-2：通过采用四元数法计算出无人自动化作业特种车的姿态参数，包括无人特种作业车的航向角、俯仰角、横滚角，并对捷联式惯性导航的姿态进行更新；
步骤6-3：对捷联式惯性导航的速度进行更新；
步骤6-4：对捷联式惯性导航的位置进行更新。
7.根据权利要求6所述的一种基于网络RTK的无人自动化作业特种车高精度定位方法，其特征在于，所述步骤6-2的具体过程为：
四元数为一个四维数组，由四个元素构成，包含三个虚单位和一个实单位，其数学表达式如下所示：
Figure FDA00037423855200000512
式(32)中，q0、q1、q2和q3为任意实数，
Figure FDA00037423855200000513
和
Figure FDA00037423855200000514
是互相垂直的单位向量；四元数的矩阵式为：
Figure FDA0003742385520000061
式(33)中，θ为角度，
Figure FDA0003742385520000062
可以得到四元数表示的旋转矩阵为：
Figure FDA0003742385520000063
由n旋转到b的过程中，由于两个坐标系中的三条坐标轴两两互相垂直，
Figure FDA0003742385520000064
和
Figure FDA0003742385520000065
表示了两个互逆的坐标系旋转过程，二者的关系如下：
Figure FDA0003742385520000066
式(35)中，ψ为无人特种作业车的航向角；θ为无人特种作业车的俯仰角；γ为无人特种作业车的横滚角；联立式(33)和式(34)得：
Figure FDA0003742385520000067
γ＝arcsin(-C13)＝arcsin(-2(q1q3-q0q2)) (37)
Figure FDA0003742385520000068
观察上式可以得到，只要得到四元数q0、q1、q2和q3的数值，就能计算出无人特种作业车的姿态角ψ、俯仰角θ和横滚角γ的值。
8.根据权利要求7所述的一种基于网络RTK的无人自动化作业特种车高精度定位方法，其特征在于，所述步骤6-3的具体过程为
根据比力方程得到速度方程为：
Figure FDA0003742385520000069
在式(39)中，fb为加速度计输出；
Figure FDA0003742385520000071
为哥氏加速度；
Figure FDA0003742385520000072
为牵引加速度，通过积分运算可以完成速度更新，具体表达式为：
Figure FDA0003742385520000073
式(40)中，Vm-1为前一时刻的速度；ΔVsmf为比力积分增量；ΔVg/corm为有害加速度引起的速度增量；
Figure FDA0003742385520000074
从式(39)至式(41)即为惯性导航的速度更新。
9.根据权利要求8所述的一种基于网络RTK的无人自动化作业特种车高精度定位方法，其特征在于，所述步骤6-4的具体过程为：
通过导航坐标系下的无人自动化作业特种车的速度，经过积分运算得到无人车所在的经纬度和高度，具体公式如下：
Figure FDA0003742385520000075
Figure FDA0003742385520000076
Figure FDA0003742385520000077
其中，L代表纬度，Lm代表m时刻纬度，Lm-1代表m-1时刻纬度；λm代表m时刻经度；λm-1代表m-1时刻经度；hm代表无人车m时刻高度，hm-1代表无人车m-1时刻高度；tm代表m时刻，tm-1代表m-1时刻；
Figure FDA0003742385520000078
代表x方向速度n次方；
Figure FDA0003742385520000079
代表y方向速度n次方；
Figure FDA00037423855200000710
代表z方向速度n次方；Rn代表地球半径；h代表卫星高度。
10.根据权利要求9所述的一种基于网络RTK的无人自动化作业特种车高精度定位方法，其特征在于，还包括初始粗对准步骤，具体为：由于陀螺零偏远大于地球自转角速度，所以陀螺很难感知到地球的自转角速度，因此无法估计初始航向角的大小；由于加速度计有比较高的精度，可以通过加速度计来感知重力加速度的分离通过加速度计来估算初始水平姿态角：俯仰角θ和横滚角γ；
Figure FDA0003742385520000081
式(45)左边三个元素为载体坐标系下加速度计的输出；而右边中括号里的三个元素为导航坐标系下重力加速度的投影，在静态的情形下，一般粗对准中重力加速度只在Z轴中有投影为-g，其他轴的投影均为0，即为gn＝[0 0 -g]T；已知
Figure FDA0003742385520000082
带入式(2.50)得到：
Figure FDA0003742385520000083
Figure FDA0003742385520000084
Figure FDA0003742385520000085
g为重力加速度，航向角ψ为未知量，可以反解出：
Figure FDA0003742385520000086
Figure FDA0003742385520000087
CN202210820920.2A 2022-07-12 2022-07-12 一种基于网络rtk的无人自动化作业特种车高精度定位方法 Pending CN115327588A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210820920.2A CN115327588A (zh) 2022-07-12 2022-07-12 一种基于网络rtk的无人自动化作业特种车高精度定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210820920.2A CN115327588A (zh) 2022-07-12 2022-07-12 一种基于网络rtk的无人自动化作业特种车高精度定位方法

Publications (1)

Publication Number Publication Date
CN115327588A true CN115327588A (zh) 2022-11-11

Family

ID=83917942

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210820920.2A Pending CN115327588A (zh) 2022-07-12 2022-07-12 一种基于网络rtk的无人自动化作业特种车高精度定位方法

Country Status (1)

Country Link
CN (1) CN115327588A (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116030646A (zh) * 2023-03-30 2023-04-28 南昌航天广信科技有限责任公司 一种路况信息播报方法、系统、计算机及存储介质
CN116540278A (zh) * 2023-07-06 2023-08-04 中国科学院空天信息创新研究院 一种云边端协同的基准动态维持方法和系统
CN116699665A (zh) * 2023-08-08 2023-09-05 山东科技大学 一种适用于海上光伏电厂环境的无人船定位系统及方法
CN117250646A (zh) * 2023-11-17 2023-12-19 毫厘智能科技(江苏)有限公司 基于芯片的测向方法及装置、芯片模组及存储介质
CN118091718A (zh) * 2024-04-17 2024-05-28 中国科学院国家授时中心 通过低轨卫星下行导航信号改善ut1解算精度的方法

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116030646A (zh) * 2023-03-30 2023-04-28 南昌航天广信科技有限责任公司 一种路况信息播报方法、系统、计算机及存储介质
CN116540278A (zh) * 2023-07-06 2023-08-04 中国科学院空天信息创新研究院 一种云边端协同的基准动态维持方法和系统
CN116540278B (zh) * 2023-07-06 2023-09-08 中国科学院空天信息创新研究院 一种云边端协同的基准动态维持方法和系统
CN116699665A (zh) * 2023-08-08 2023-09-05 山东科技大学 一种适用于海上光伏电厂环境的无人船定位系统及方法
CN117250646A (zh) * 2023-11-17 2023-12-19 毫厘智能科技(江苏)有限公司 基于芯片的测向方法及装置、芯片模组及存储介质
CN117250646B (zh) * 2023-11-17 2024-02-02 毫厘智能科技(江苏)有限公司 基于芯片的测向方法及装置、芯片模组及存储介质
CN118091718A (zh) * 2024-04-17 2024-05-28 中国科学院国家授时中心 通过低轨卫星下行导航信号改善ut1解算精度的方法

Similar Documents

Publication Publication Date Title
CN101743453B (zh) 任务后高精确度定位和定向系统
CN115327588A (zh) 一种基于网络rtk的无人自动化作业特种车高精度定位方法
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN108344415B (zh) 一种组合导航信息融合方法
US20090093959A1 (en) Real-time high accuracy position and orientation system
JP2015111133A (ja) 受信機の位置決定
CN106405592B (zh) 车载北斗载波相位周跳检测与修复方法及系统
CN111965685A (zh) 一种基于多普勒信息的低轨卫星/惯性组合导航定位方法
Yi et al. Tightly-coupled GPS/INS integration using unscented Kalman filter and particle filter
Yi On improving the accuracy and reliability of GPS/INS-based direct sensor georeferencing
Chi et al. Enabling robust and accurate navigation for UAVs using real-time GNSS precise point positioning and IMU integration
Wen et al. 3D LiDAR aided GNSS real-time kinematic positioning
CN115683094A (zh) 一种复杂环境下车载双天线紧耦合定位方法及系统
Du et al. Integration of PPP GPS and low cost IMU
CN108205151B (zh) 一种低成本gps单天线姿态测量方法
Farkas et al. Small UAV’s position and attitude estimation using tightly coupled multi baseline multi constellation GNSS and inertial sensor fusion
Ellum et al. A mobile mapping system for the survey community
CN116878498A (zh) 一种多频bds/ins组合空中加油相对导航系统和方法
Hutton et al. 10 years of direct georeferencing for airborne photogrammetry
Hein et al. High‐Precision Kinematic GPS Differential Positioning and Integration of GPS with a Ring Laser Strapdown Inertial System
Consoli et al. A multi-antenna approach for UAV's attitude determination
CN111288990A (zh) 一种架空检修机器人组合姿态测量方法
Kjørsvik et al. Tightly coupled precise point positioning and inertial navigation systems
Vana Continuous urban navigation with next-generation, mass market navigation sensors and adaptive filtering
CN117782080B (zh) 基于PPP-B2b/INS的实时空基导航系统及方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination