CN117406259B - 一种基于北斗的智慧工地车辆定位方法及*** - Google Patents
一种基于北斗的智慧工地车辆定位方法及*** Download PDFInfo
- Publication number
- CN117406259B CN117406259B CN202311715088.0A CN202311715088A CN117406259B CN 117406259 B CN117406259 B CN 117406259B CN 202311715088 A CN202311715088 A CN 202311715088A CN 117406259 B CN117406259 B CN 117406259B
- Authority
- CN
- China
- Prior art keywords
- data
- particle
- vehicle
- pseudo
- pose
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 155
- 238000010276 construction Methods 0.000 title claims abstract description 30
- 238000012937 correction Methods 0.000 claims abstract description 85
- 238000012545 processing Methods 0.000 claims abstract description 39
- 238000004364 calculation method Methods 0.000 claims abstract description 17
- 239000002245 particle Substances 0.000 claims description 193
- 230000008569 process Effects 0.000 claims description 63
- 238000006073 displacement reaction Methods 0.000 claims description 45
- 238000005070 sampling Methods 0.000 claims description 36
- 238000012360 testing method Methods 0.000 claims description 27
- 239000011159 matrix material Substances 0.000 claims description 26
- 230000009466 transformation Effects 0.000 claims description 22
- 238000009826 distribution Methods 0.000 claims description 20
- 230000004927 fusion Effects 0.000 claims description 20
- 239000005433 ionosphere Substances 0.000 claims description 20
- 230000008859 change Effects 0.000 claims description 16
- 238000004590 computer program Methods 0.000 claims description 13
- 238000012952 Resampling Methods 0.000 claims description 10
- 238000001514 detection method Methods 0.000 claims description 10
- 239000005436 troposphere Substances 0.000 claims description 8
- 238000011156 evaluation Methods 0.000 claims description 5
- 230000035515 penetration Effects 0.000 claims description 4
- 230000000717 retained effect Effects 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 2
- DMBHHRLKUKUOEG-UHFFFAOYSA-N diphenylamine Chemical compound C=1C=CC=CC=1NC1=CC=CC=C1 DMBHHRLKUKUOEG-UHFFFAOYSA-N 0.000 claims 1
- 239000003673 groundwater Substances 0.000 claims 1
- 230000008030 elimination Effects 0.000 abstract description 3
- 238000003379 elimination reaction Methods 0.000 abstract description 3
- 238000004891 communication Methods 0.000 description 8
- 230000005540 biological transmission Effects 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 3
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 3
- 238000003491 array Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000007689 inspection Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000002093 peripheral effect Effects 0.000 description 2
- 239000007787 solid Substances 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000004566 building material Substances 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000012854 evaluation process Methods 0.000 description 1
- 238000000802 evaporation-induced self-assembly Methods 0.000 description 1
- 239000003574 free electron Substances 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 150000002500 ions Chemical class 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000009827 uniform distribution Methods 0.000 description 1
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/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
-
- 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
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)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明提供了一种基于北斗的智慧工地车辆定位方法及***,所述方法包括对北斗卫星观测数据进行误差矫正处理;计算第一伪矩观测量的第一检验值与第二检验值,对伪矩观测量进行噪声剔除;对智慧工地车辆进行坐标联合解算;将第一定位数据、惯性数据与里程数据进行融合,以得到融合数据;将融合数据进行粒子转化,以得到位姿粒子集,对位姿粒子集进行权重更新与采样处理,以得到智慧工地车辆的第二定位数据,本发明通过对观测数据进行误差矫正与噪声剔除,可保证最后得到的伪矩观测量的准确度以提升后续解算坐标时的精度,同时本发明可避免出现定位偏移的情况,进一步提升车辆定位的精度。
Description
技术领域
本发明属于车辆定位的技术领域,具体地涉及一种基于北斗的智慧工地车辆定位方法及***。
背景技术
智慧工地平台主要利用智能终端、物联网、移动互联等技术,实时采集建设过程数据,利用大数据、人工智能技术对建设过程的数据进行实时分析,并对建设过程进行控制。
对于现有的智慧工地而言,物料运送车辆会逐渐趋于无人化,即采用AGV车辆完成的建筑物料的运输,对于无人运输的AGV车辆而言,需要实时获取车辆的实时位置,以便于在全局方面对不同的运输车辆进行运输管控与导航定位,避免车辆在运输过程中发生干涉的情况,但基于现有的车辆定位而言,其在定位环境复杂的条件下无法实现车辆的准确定位,同时对于北斗卫星定位中存在的误差未修正,进一步影响了AGV物料运输车辆的定位精度。
发明内容
为了解决上述技术问题,本发明提供了一种基于北斗的智慧工地车辆定位方法及***,用于解决现有技术中的技术问题。
一方面,本发明提供以下技术方案,一种基于北斗的智慧工地车辆定位方法,包括:
获取北斗卫星观测数据,对所述北斗卫星观测数据进行误差矫正处理,以得到矫正观测数据;
基于所述矫正观测数据确定第一伪矩观测量,计算所述第一伪矩观测量的第一检验值与第二检验值,基于所述第一检验值与所述第二检验值对所述伪矩观测量进行噪声剔除,以得到第二伪矩观测量;
基于所述第二伪矩观测量对智慧工地车辆进行坐标联合解算,以得到所述智慧工地车辆的第一定位数据;
获取所述智慧工地车辆的惯性数据与里程数据,将所述第一定位数据、所述惯性数据与所述里程数据进行融合,以得到融合数据;
将所述融合数据进行粒子转化,以得到位姿粒子集,对所述位姿粒子集进行权重更新与采样处理,以得到所述智慧工地车辆的第二定位数据。
相比现有技术,本申请的有益效果为:本申请首先获取北斗卫星观测数据,对所述北斗卫星观测数据进行误差矫正处理,以得到矫正观测数据;之后基于所述矫正观测数据确定第一伪矩观测量,计算所述第一伪矩观测量的第一检验值与第二检验值,基于所述第一检验值与所述第二检验值对所述伪矩观测量进行噪声剔除,以得到第二伪矩观测量;而后基于所述第二伪矩观测量对智慧工地车辆进行坐标联合解算,以得到所述智慧工地车辆的第一定位数据;然后获取所述智慧工地车辆的惯性数据与里程数据,将所述第一定位数据、所述惯性数据与所述里程数据进行融合,以得到融合数据;最后将所述融合数据进行粒子转化,以得到位姿粒子集,对所述位姿粒子集进行权重更新与采样处理,以得到所述智慧工地车辆的第二定位数据,本发明通过对观测数据进行误差矫正与噪声剔除,可保证最后得到的伪矩观测量的准确度以提升后续解算坐标时的精度,同时本发明对第一定位数据进行融合、粒子转化、权重更新与采样处理,以此可避免出现定位偏移的情况,进一步提升车辆定位的精度。
较佳的,所述对所述北斗卫星观测数据进行误差矫正处理,以得到矫正观测数据的步骤包括:
基于第一预设公式对所述北斗卫星观测数据进行第一误差矫正处理,其中,第一预设公式为:
;
式中,为卫星钟差修正值,/>为时钟偏差,/>、/>分别为卫星信号发射时刻、卫星信号接收时刻,/>为时钟漂移,/>为频率漂移,/>为光速,/>为卫星轨道偏心角,/>为卫星轨道偏近点角,/>为引力常数,/>为卫星轨道半长轴;
基于第二预设公式对所述北斗卫星观测数据进行第二误差矫正处理,其中,第二预设公式为:
;
;
式中,为电离层修正值,/>、/>分别为预设改正模型中的第/>个第一网络参数、第/>个第二网络参数,/>为电离层穿刺点的纬度,/>为电离层穿刺点的地方时,/>、/>分别为第一修正常数与第二修正常数,/>为观测站的地理纬度,/>为卫星高度角,/>为地球半径,/>为电离层高度,/>为卫星方位角;
基于第三预设公式对所述北斗卫星观测数据进行第三误差矫正处理,其中,第三预设公式为:
;
式中,为对流层修正值,/>为第三修正常数,/>为地面气压,/>为地面水气压,为地面湿度,/>分别为测站大地高度、干大气层顶高度、湿大气层顶高度。
较佳的,所述基于所述矫正观测数据确定第一伪矩观测量的步骤包括:
通过第四预设公式并基于所述矫正观测数据确定第一伪矩观测量:
;
式中,为车载接收机至北斗卫星之间的真实距离,/>为光速,/>分别为卫星钟差修正值、电离层修正值、对流层修正值,/>为车载接收机钟差,/>为其他误差。
较佳的,所述计算所述第一伪矩观测量的第一检验值与第二检验值,基于所述第一检验值与所述第二检验值对所述伪矩观测量进行噪声剔除,以得到第二伪矩观测量的步骤包括:
计算所述第一伪矩观测量的第一检验值:
;
式中,分别为北斗卫星、车载接收机的第一码偏差,/>为北斗卫星、车载接收机的第一码偏差对应的时变量,/>为第一观测噪声;
计算所述第一伪矩观测量的第二检验值;
;
式中,分别为北斗卫星、车载接收机的第二码偏差,/>为北斗卫星、车载接收机的第二码偏差对应的时变量,/>为第二观测噪声,/>为电离层时延参数与误差项;
判断所述第一检验值是否大于第一剔除阈值/>以及所述第二检验值是否大于第二剔除阈值/>,其中,/>;
若所述第一检验值不大于第一剔除阈值/>且所述第二检验值/>不大于第二剔除阈值/>,则将对应的第一伪矩观测量进行保留,若所述第一检验值/>大于第一剔除阈值/>和/或所述第二检验值/>大于第二剔除阈值/>,则将对应的第一伪矩观测量进行剔除,以得到第二伪矩观测量。
较佳的,所述基于所述第二伪矩观测量对智慧工地车辆进行坐标联合解算,以得到所述智慧工地车辆的第一定位数据的步骤包括:
基于所述第二伪矩观测量建立坐标解算方程:
;
式中,为第一定位数据的坐标,/>为第/>个第二伪矩观测量,为第/>个第二伪矩观测量对应的北斗卫星坐标,/>为车载接收机钟差;
将所述坐标解算方程进行泰勒展开,以得到线性解算方程;
采用加权最小二乘法对所述线性解算方程进行求解,以得到第一定位数据。
较佳的,所述获取所述智慧工地车辆的惯性数据与里程数据,将所述第一定位数据、所述惯性数据与所述里程数据进行融合,以得到融合数据的步骤包括:
获取所述智慧工地车辆的惯性数据与里程数据,并基于所述第一定位数据、所述惯性数据与所述里程数据确定第时刻的智慧工地车辆的初始位姿矩阵/>:
;
式中,为第/>时刻的智慧工地车辆在全局坐标下的x轴坐标、y轴坐标、z轴坐标,/>为第/>时刻的智慧工地车辆在全局坐标下的角度,/>分别为第/>时刻的智慧工地车辆的全局线速度、全局角速度;
对所述初始位姿矩阵进行运动转化,以得到转化位姿矩阵/>:
;
式中,为第/>时刻的智慧工地车辆在全局坐标下的x轴坐标、y轴坐标、z轴坐标,/>为第/>时刻的智慧工地车辆在全局坐标下的角度,分别为第/>时刻的智慧工地车辆的全局线速度、全局角速度,/>为高斯分布噪声;
将所述转化位姿矩阵、所述惯性数据与所述里程数据进行融合,以得到融合数据/>:
;
式中,、/>分别为里程噪声、惯性噪声。
较佳的,所述对所述位姿粒子集进行权重更新与采样处理,以得到所述智慧工地车辆的第二定位数据的步骤包括:
将第时刻的位姿粒子集/>均匀分布至全局空间中,其中,,/>为第/>个表示第/>时刻的位姿的粒子,/>为粒子/>对应的粒子权重;
通过第时刻的位姿粒子集迭代估计第/>时刻的粒子分布,以得到估计位姿粒子/>:
;
式中,分别为第/>时刻智慧工地车辆的x方向估计位移、y方向估计位移、z方向估计位移、角度估计值,/>、/>、/>、/>为智慧工地车辆在第/>时刻至第/>时刻的x方向位移变化量、y方向位移变化量、z方向位移变化量、角度变化量,/>分别为x方向位移噪声、y方向位移噪声、z方向位移噪声、角度噪声;
基于第时刻的粒子分布对位姿粒子集/>中每个粒子权重进行评估更新,以得到更新权重/>:
;
式中,为观测结果,/>为给定估计位姿粒子/>的条件下采样得到/>的理想概率密度;
基于所述更新权重对所述位姿粒子集中的所有粒子进行重新采样,以得到采样权重/>与采样粒子/>,基于所述采样权重/>计算有效粒子数量/>;
;
判断所述有效粒子数量是否大于粒子阈值,若所述有效粒子数量/>大于粒子阈值,则第二定位数据/>,若所述有效粒子数量/>不大于粒子阈值,则在所述位姿粒子集添加若干数量的随机粒子,以得到更新粒子集,重复对所述更新粒子集进行粒子分布估计、权重更新与重新采样直至有效粒子数量/>大于粒子阈值,输出对应的迭代采样权重/>与迭代采样粒子/>,则第二定位数据/>,其中,/>为更新粒子集中的粒子数量。
第二方面,本发明提供以下技术方案,一种基于北斗的智慧工地车辆定位***,所述***包括:
获取模块,用于获取北斗卫星观测数据,对所述北斗卫星观测数据进行误差矫正处理,以得到矫正观测数据;
检验模块,用于基于所述矫正观测数据确定第一伪矩观测量,计算所述第一伪矩观测量的第一检验值与第二检验值,基于所述第一检验值与所述第二检验值对所述伪矩观测量进行噪声剔除,以得到第二伪矩观测量;
解算模块,用于基于所述第二伪矩观测量对智慧工地车辆进行坐标联合解算,以得到所述智慧工地车辆的第一定位数据;
融合模块,用于获取所述智慧工地车辆的惯性数据与里程数据,将所述第一定位数据、所述惯性数据与所述里程数据进行融合,以得到融合数据;
更新模块,用于将所述融合数据进行粒子转化,以得到位姿粒子集,对所述位姿粒子集进行权重更新与采样处理,以得到所述智慧工地车辆的第二定位数据。
第三方面,本发明提供以下技术方案,一种计算机,包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序,所述处理器执行所述计算机程序时实现如上述的基于北斗的智慧工地车辆定位方法。
第四方面,本发明提供以下技术方案,一种存储介质,所述存储介质上存储有计算机程序,所述计算机程序被处理器执行时实现如上述的基于北斗的智慧工地车辆定位方法。
附图说明
为了更清楚地说明本发明实施例中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为本发明实施例一提供的基于北斗的智慧工地车辆定位方法的流程图;
图2为本发明实施例一提供的基于北斗的智慧工地车辆定位方法中步骤S1的详细流程图;
图3为本发明实施例一提供的基于北斗的智慧工地车辆定位方法中步骤S22的详细流程图;
图4为本发明实施例一提供的基于北斗的智慧工地车辆定位方法中步骤S3的详细流程图;
图5为本发明实施例一提供的基于北斗的智慧工地车辆定位方法中步骤S4的详细流程图;
图6为本发明实施例一提供的基于北斗的智慧工地车辆定位方法中步骤S5的详细流程图;
图7为本发明实施例二提供的基于北斗的智慧工地车辆定位***的结构框图;
图8为本发明另一实施例提供的计算机的硬件结构示意图。
以下将结合附图对本发明实施例作进一步说明。
具体实施方式
下面详细描述本发明的实施例,所述实施例的示例在附图中示出,其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的,旨在用于解释本发明的实施例,而不能理解为对本发明的限制。
在本发明实施例的描述中,需要理解的是,术语“长度”、“宽度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系,仅是为了便于描述本发明实施例和简化描述,而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作,因此不能理解为对本发明的限制。
此外,术语“第一”、“第二”仅用于描述目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明实施例的描述中,“多个”的含义是两个或两个以上,除非另有明确具体的限定。
在本发明实施例中,除非另有明确的规定和限定,术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解,例如,可以是固定连接,也可以是可拆卸连接,或成一体;可以是机械连接,也可以是电连接;可以是直接相连,也可以通过中间媒介间接相连,可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言,可以根据具体情况理解上述术语在本发明实施例中的具体含义。
实施例一
在本发明的实施例一中,如图1所示,一种基于北斗的智慧工地车辆定位方法,包括:
S1、获取北斗卫星观测数据,对所述北斗卫星观测数据进行误差矫正处理,以得到矫正观测数据;
具体的,对于北斗卫星的观测数据而言,在观测数据中会存在卫星端、中间传输以及车载接收机的误差,如不对误差进行修正处理,则会影响最终的坐标解算过程,因此在该步骤中,对北斗卫星观测数据进行误差矫正处理,可得到矫正观测数据,以提升智慧工地定位的精度。
如图2所示,其中,所述步骤S1包括:
S11、基于第一预设公式对所述北斗卫星观测数据进行第一误差矫正处理,其中,第一预设公式为:
;
式中,为卫星钟差修正值,/>为时钟偏差,/>、/>分别为卫星信号发射时刻、卫星信号接收时刻,/>为时钟漂移,/>为频率漂移,/>为光速,/>为卫星轨道偏心角,/>为卫星轨道偏近点角,/>为引力常数,/>为卫星轨道半长轴;
具体的,第一次误差矫正处理过程是对卫星端的误差进行校正处理,通过上述第一预设公式可对卫星钟与北斗***时间之间的偏差进行校正处理。
S12、基于第二预设公式对所述北斗卫星观测数据进行第二误差矫正处理,其中,第二预设公式为:
;
;
式中,为电离层修正值,/>、/>分别为预设改正模型中的第/>个第一网络参数、第/>个第二网络参数,/>为电离层穿刺点的纬度,/>为电离层穿刺点的地方时,/>、/>分别为第一修正常数与第二修正常数,/>为观测站的地理纬度,/>为卫星高度角,/>为地球半径,/>为电离层高度,/>为卫星方位角;
其中,第一修正常数为5×10-9,第二修正常数为50400,第二次误差矫正处理是对电离层进行误差矫正,而电离层误差是是在地面上空50~100km之间产生的,北斗卫星发出的信号在电离层中受到正离子和自由电子的影响,信号的传播途径发生偏折产生的,电离层误差可利用导航电文中预设改正模型进行改正,其中预设改正模型为Klobuchar模型。
S13、基于第三预设公式对所述北斗卫星观测数据进行第三误差矫正处理,其中,第三预设公式为:
;
式中,为对流层修正值,/>为第三修正常数,/>为地面气压,/>为地面水气压,为地面湿度,/>分别为测站大地高度、干大气层顶高度、湿大气层顶高度;
其中,第三修正常数为155.2×10-7,第三次误差矫正处理是对对流层误差进行矫正处理,对流层误差是在地面上空40kmm以下产生的,对流层与地面相连,故而具有一定的温度差,信号经过对流层时,传播路径发射偏折,从而产生对流层延迟,在本申请中,通过第三预设公式进行第三误差矫正处理。
S2、基于所述矫正观测数据确定第一伪矩观测量,计算所述第一伪矩观测量的第一检验值与第二检验值,基于所述第一检验值与所述第二检验值对所述伪矩观测量进行噪声剔除,以得到第二伪矩观测量;
具体的,在得到矫正观测数据之后,便可从中获取计算得到第一伪矩观测量,但对于实际得到的第一伪矩观测量而言,其中会存在粗差的情况,且粗车可能会达到百米、千米量级,如不剔除相应的第一伪矩观测量,则会导致后续解算第一定位数据时存在较大误差。
同时,所述步骤S2包括:S21、基于所述矫正观测数据确定第一伪矩观测量;S22、计算所述第一伪矩观测量的第一检验值与第二检验值,基于所述第一检验值与所述第二检验值对所述伪矩观测量进行噪声剔除,以得到第二伪矩观测量。
其中,所述步骤S21具体为:
通过第四预设公式并基于所述矫正观测数据确定第一伪矩观测量:
;
式中,为车载接收机至北斗卫星之间的真实距离,/>为光速,/>分别为卫星钟差修正值、电离层修正值、对流层修正值,/>为车载接收机钟差,/>为其他误差;
其中,对于车载接收机至北斗卫星之间的真实距离而言,,其中,/>为卫星在地球坐标系中的坐标,/>为接收机在地球坐标系中的坐标。
如图3所示,其中,所述步骤S22包括:
S221、计算所述第一伪矩观测量的第一检验值:
;
式中,分别为北斗卫星、车载接收机的第一码偏差,/>为北斗卫星、车载接收机的第一码偏差对应的时变量,/>为第一观测噪声。
S222、计算所述第一伪矩观测量的第二检验值;
;
式中,分别为北斗卫星、车载接收机的第二码偏差,/>为北斗卫星、车载接收机的第二码偏差对应的时变量,/>为第二观测噪声,/>为电离层时延参数与误差项;
其中,北斗卫星的第一码偏差、第二码偏差分别为北斗卫星至中间传输基站的码偏差与中间传输基站至车载接收机的码偏差,车载接收机的第一码偏差、第二码偏差分别为车载接收机至中间传输基站的码偏差、中间传输基站至北斗卫星的码偏差。
S223、判断所述第一检验值是否大于第一剔除阈值/>以及所述第二检验值/>是否大于第二剔除阈值/>,其中,/>。
S224、若所述第一检验值不大于第一剔除阈值/>且所述第二检验值不大于第二剔除阈值/>,则将对应的第一伪矩观测量进行保留,若所述第一检验值大于第一剔除阈值/>和/或所述第二检验值/>大于第二剔除阈值/>,则将对应的第一伪矩观测量进行剔除,以得到第二伪矩观测量。
S3、基于所述第二伪矩观测量对智慧工地车辆进行坐标联合解算,以得到所述智慧工地车辆的第一定位数据;
如图4所示,其中,所述步骤S3包括:
S31、基于所述第二伪矩观测量建立坐标解算方程:
;
式中,为第一定位数据的坐标,/>为第/>个第二伪矩观测量,为第/>个第二伪矩观测量对应的北斗卫星坐标,/>为车载接收机钟差。
S32、将所述坐标解算方程进行泰勒展开,以得到线性解算方程;
其中,线性解算方程为:
;
其中,,/>为第/>颗卫星与接受机之间的近似距离,/>为接收机每一次求解时的近似坐标,/>、/>、/>为求解时对应的坐标变量值。
S33、采用加权最小二乘法对所述线性解算方程进行求解,以得到第一定位数据。
S4、获取所述智慧工地车辆的惯性数据与里程数据,将所述第一定位数据、所述惯性数据与所述里程数据进行融合,以得到融合数据;
如图5所示,其中,所述步骤S4包括:
S41、获取所述智慧工地车辆的惯性数据与里程数据,并基于所述第一定位数据、所述惯性数据与所述里程数据确定第时刻的智慧工地车辆的初始位姿矩阵/>:
;
式中,为第/>时刻的智慧工地车辆在全局坐标下的x轴坐标、y轴坐标、z轴坐标,/>为第/>时刻的智慧工地车辆在全局坐标下的角度,/>分别为第/>时刻的智慧工地车辆的全局线速度、全局角速度。
S42、对所述初始位姿矩阵进行运动转化,以得到转化位姿矩阵/>:
;
式中,为第/>时刻的智慧工地车辆在全局坐标下的x轴坐标、y轴坐标、z轴坐标,/>为第/>时刻的智慧工地车辆在全局坐标下的角度,分别为第/>时刻的智慧工地车辆的全局线速度、全局角速度,/>为高斯分布噪声;
其中,为里程数据的观测矩阵,/>为惯性数据的观测矩阵。
S43、将所述转化位姿矩阵、所述惯性数据与所述里程数据进行融合,以得到融合数据/>:
;
式中,、/>分别为里程噪声、惯性噪声。
S5、将所述融合数据进行粒子转化,以得到位姿粒子集,对所述位姿粒子集进行权重更新与采样处理,以得到所述智慧工地车辆的第二定位数据;
如图6所示,其中,所述步骤S5包括:
S51、将第时刻的位姿粒子集/>均匀分布至全局空间中,其中,,/>为第/>个表示第/>时刻的位姿的粒子,/>为粒子/>对应的粒子权重;
其中,在位姿粒子集分布的过程中,采用均匀分布概率策略将位姿粒子集/>中的所有粒子分布至全局空间中。
S52、通过第时刻的位姿粒子集迭代估计第/>时刻的粒子分布,以得到估计位姿粒子/>:/>
;
式中,分别为第/>时刻智慧工地车辆的x方向估计位移、y方向估计位移、z方向估计位移、角度估计值,/>、/>、/>、/>为智慧工地车辆在第/>时刻至第/>时刻的x方向位移变化量、y方向位移变化量、z方向位移变化量、角度变化量,分别为x方向位移噪声、y方向位移噪声、z方向位移噪声、角度噪声;
具体的,通过迭代的方法可根据上一时刻粒子中的粒子以及运动信息,可用于估计下一时刻的先验粒子分布,同时对于而言,其均满足高斯分布。
S53、基于第时刻的粒子分布对位姿粒子集/>中每个粒子权重进行评估更新,以得到更新权重/>:
;
式中,为观测结果,/>为给定估计位姿粒子/>的条件下采样得到/>的理想概率密度;
具体的,对粒子权重进行评估更新过程,即为后验评估过程,而其中的观测结果具体为激光雷达所得到的观测数据。
S54、基于所述更新权重对所述位姿粒子集中的所有粒子进行重新采样,以得到采样权重/>与采样粒子/>,基于所述采样权重/>计算有效粒子数量/>;
;
具体的,在实际的粒子分布估计与权重更新的情况,可能会出现粒子数量不够或者耗尽的情况,如此会影响定位的精度以及定位的速度,因此通过计算有效粒子数量并与粒子阈值对比,通过对比结果判断是否需要添加随机粒子。
S55、判断所述有效粒子数量是否大于粒子阈值,若所述有效粒子数量/>大于粒子阈值,则第二定位数据/>,若所述有效粒子数量/>不大于粒子阈值,则在所述位姿粒子集添加若干数量的随机粒子,以得到更新粒子集,重复对所述更新粒子集进行粒子分布估计、权重更新与重新采样直至有效粒子数量/>大于粒子阈值,输出对应的迭代采样权重/>与迭代采样粒子/>,则第二定位数据/>,其中,为更新粒子集中的粒子数量;
具体的,当有效粒子数量足够时,则此时第二定位数据,若有效粒子不够时,则说明粒子数量不够用,容易出现粒子耗尽的情况,因此通过在粒子集中添加对应的随机粒子,之后得到了一个更新粒子集,对于更新粒子集重复执行步骤S51-S54的过程,直至满足有效粒子数量/>大于粒子阈值的条件,之后便可输出对应的迭代采样权重与迭代采样粒子/>,此时第二定位数据/>。/>
本发明实施例一提供的基于北斗的智慧工地车辆定位方法,本申请首先获取北斗卫星观测数据,对所述北斗卫星观测数据进行误差矫正处理,以得到矫正观测数据;之后基于所述矫正观测数据确定第一伪矩观测量,计算所述第一伪矩观测量的第一检验值与第二检验值,基于所述第一检验值与所述第二检验值对所述伪矩观测量进行噪声剔除,以得到第二伪矩观测量;而后基于所述第二伪矩观测量对智慧工地车辆进行坐标联合解算,以得到所述智慧工地车辆的第一定位数据;然后获取所述智慧工地车辆的惯性数据与里程数据,将所述第一定位数据、所述惯性数据与所述里程数据进行融合,以得到融合数据;最后将所述融合数据进行粒子转化,以得到位姿粒子集,对所述位姿粒子集进行权重更新与采样处理,以得到所述智慧工地车辆的第二定位数据,本发明通过对观测数据进行误差矫正与噪声剔除,可保证最后得到的伪矩观测量的准确度以提升后续解算坐标时的精度,同时本发明对第一定位数据进行融合、粒子转化、权重更新与采样处理,以此可避免出现定位偏移的情况,进一步提升车辆定位的精度。
实施例二
如图7所示,在本发明的实施例二提供了一种基于北斗的智慧工地车辆定位***,所述***包括:
获取模块1,用于获取北斗卫星观测数据,对所述北斗卫星观测数据进行误差矫正处理,以得到矫正观测数据;
检验模块2,用于基于所述矫正观测数据确定第一伪矩观测量,计算所述第一伪矩观测量的第一检验值与第二检验值,基于所述第一检验值与所述第二检验值对所述伪矩观测量进行噪声剔除,以得到第二伪矩观测量;
解算模块3,用于基于所述第二伪矩观测量对智慧工地车辆进行坐标联合解算,以得到所述智慧工地车辆的第一定位数据;
融合模块4,用于获取所述智慧工地车辆的惯性数据与里程数据,将所述第一定位数据、所述惯性数据与所述里程数据进行融合,以得到融合数据;
更新模块5,用于将所述融合数据进行粒子转化,以得到位姿粒子集,对所述位姿粒子集进行权重更新与采样处理,以得到所述智慧工地车辆的第二定位数据。
所述获取模块1包括:
第一矫正子模块,用于基于第一预设公式对所述北斗卫星观测数据进行第一误差矫正处理,其中,第一预设公式为:
;
式中,为卫星钟差修正值,/>为时钟偏差,/>、/>分别为卫星信号发射时刻、卫星信号接收时刻,/>为时钟漂移,/>为频率漂移,/>为光速,/>为卫星轨道偏心角,/>为卫星轨道偏近点角,/>为引力常数,/>为卫星轨道半长轴;
第二矫正子模块,用于基于第二预设公式对所述北斗卫星观测数据进行第二误差矫正处理,其中,第二预设公式为:
;/>
;
式中,为电离层修正值,/>、/>分别为预设改正模型中的第/>个第一网络参数、第/>个第二网络参数,/>为电离层穿刺点的纬度,/>为电离层穿刺点的地方时,/>、/>分别为第一修正常数与第二修正常数,/>为观测站的地理纬度,/>为卫星高度角,/>为地球半径,/>为电离层高度,/>为卫星方位角;
第三矫正子模块,用于基于第三预设公式对所述北斗卫星观测数据进行第三误差矫正处理,其中,第三预设公式为:
;
式中,为对流层修正值,/>为第三修正常数,/>为地面气压,/>为地面水气压,为地面湿度,/>分别为测站大地高度、干大气层顶高度、湿大气层顶高度。
所述检验模块2用于:
通过第四预设公式并基于所述矫正观测数据确定第一伪矩观测量:
;
式中,为车载接收机至北斗卫星之间的真实距离,/>为光速,/>分别为卫星钟差修正值、电离层修正值、对流层修正值,/>为车载接收机钟差,/>为其他误差。
所述检验模块2包括:
第一计算子模块,用于计算所述第一伪矩观测量的第一检验值:
;
式中,分别为北斗卫星、车载接收机的第一码偏差,/>为北斗卫星、车载接收机的第一码偏差对应的时变量,/>为第一观测噪声;
第二计算子模块,用于计算所述第一伪矩观测量的第二检验值;
;
式中,分别为北斗卫星、车载接收机的第二码偏差,/>为北斗卫星、车载接收机的第二码偏差对应的时变量,/>为第二观测噪声,/>为电离层时延参数与误差项;
第一判断子模块,用于判断所述第一检验值是否大于第一剔除阈值/>以及所述第二检验值/>是否大于第二剔除阈值/>,其中,/>;
剔除子模块,用于若所述第一检验值不大于第一剔除阈值/>且所述第二检验值/>不大于第二剔除阈值/>,则将对应的第一伪矩观测量进行保留,若所述第一检验值/>大于第一剔除阈值/>和/或所述第二检验值/>大于第二剔除阈值/>,则将对应的第一伪矩观测量进行剔除,以得到第二伪矩观测量。/>
所述解算模块3包括:
建立子模块,用于基于所述第二伪矩观测量建立坐标解算方程:
;
式中,为第一定位数据的坐标,/>为第/>个第二伪矩观测量,为第/>个第二伪矩观测量对应的北斗卫星坐标,/>为车载接收机钟差;
张开子模块,用于将所述坐标解算方程进行泰勒展开,以得到线性解算方程;
求解子模块,用于采用加权最小二乘法对所述线性解算方程进行求解,以得到第一定位数据。
所述融合模块4包括:
初始位姿矩阵确定子模块,用于获取所述智慧工地车辆的惯性数据与里程数据,并基于所述第一定位数据、所述惯性数据与所述里程数据确定第时刻的智慧工地车辆的初始位姿矩阵/>:
;
式中,为第/>时刻的智慧工地车辆在全局坐标下的x轴坐标、y轴坐标、z轴坐标,/>为第/>时刻的智慧工地车辆在全局坐标下的角度,/>分别为第/>时刻的智慧工地车辆的全局线速度、全局角速度;
转化位姿矩阵确定子模块,用于对所述初始位姿矩阵进行运动转化,以得到转化位姿矩阵/>:
;
式中,为第/>时刻的智慧工地车辆在全局坐标下的x轴坐标、y轴坐标、z轴坐标,/>为第/>时刻的智慧工地车辆在全局坐标下的角度,分别为第/>时刻的智慧工地车辆的全局线速度、全局角速度,/>为高斯分布噪声;
融合子模块,用于将所述转化位姿矩阵、所述惯性数据与所述里程数据进行融合,以得到融合数据/>:/>
;
式中,、/>分别为里程噪声、惯性噪声。
所述更新模块5包括:
分布子模块,用于将第时刻的位姿粒子集/>均匀分布至全局空间中,其中,/>,/>为第/>个表示第/>时刻的位姿的粒子,/>为粒子/>对应的粒子权重;
估计子模块,用于通过第时刻的位姿粒子集迭代估计第/>时刻的粒子分布,以得到估计位姿粒子/>:
;
式中,分别为第/>时刻智慧工地车辆的x方向估计位移、y方向估计位移、z方向估计位移、角度估计值,/>、/>、/>、/>为智慧工地车辆在第/>时刻至第/>时刻的x方向位移变化量、y方向位移变化量、z方向位移变化量、角度变化量,/>分别为x方向位移噪声、y方向位移噪声、z方向位移噪声、角度噪声;
更新子模块,用于基于第时刻的粒子分布对位姿粒子集/>中每个粒子权重进行评估更新,以得到更新权重/>:
;
式中,为观测结果,/>为给定估计位姿粒子/>的条件下采样得到/>的理想概率密度;
采样子模块,用于基于所述更新权重对所述位姿粒子集中的所有粒子进行重新采样,以得到采样权重/>与采样粒子/>,基于所述采样权重/>计算有效粒子数量/>;
;
第二判断子模块,用于判断所述有效粒子数量是否大于粒子阈值,若所述有效粒子数量/>大于粒子阈值,则第二定位数据/>,若所述有效粒子数量/>不大于粒子阈值,则在所述位姿粒子集添加若干数量的随机粒子,以得到更新粒子集,重复对所述更新粒子集进行粒子分布估计、权重更新与重新采样直至有效粒子数量/>大于粒子阈值,输出对应的迭代采样权重/>与迭代采样粒子/>,则第二定位数据/>,其中,/>为更新粒子集中的粒子数量。/>
在本发明的另一些实施例中,本发明实施例提供以下技术方案,一种计算机,包括存储器102、处理器101以及存储在所述存储器102上并可在所述处理器101上运行的计算机程序,所述处理器101执行所述计算机程序时实现如上所述的基于北斗的智慧工地车辆定位方法。
具体的,上述处理器101可以包括中央处理器(CPU),或者特定集成电路(Application Specific Integrated Circuit,简称为ASIC),或者可以被配置成实施本申请实施例的一个或多个集成电路。
其中,存储器102可以包括用于数据或指令的大容量存储器。举例来说而非限制,存储器102可包括硬盘驱动器(Hard Disk Drive,简称为HDD)、软盘驱动器、固态驱动器(Solid State Drive,简称为SSD)、闪存、光盘、磁光盘、磁带或通用串行总线(UniversalSerial Bus,简称为USB)驱动器或者两个或更多个以上这些的组合。在合适的情况下,存储器102可包括可移除或不可移除(或固定)的介质。在合适的情况下,存储器102可在数据处理装置的内部或外部。在特定实施例中,存储器102是非易失性(Non-Volatile)存储器。在特定实施例中,存储器102包括只读存储器(Read-Only Memory,简称为ROM)和随机存取存储器(Random Access Memory,简称为RAM)。在合适的情况下,该ROM可以是掩模编程的ROM、可编程ROM(Programmable Read-Only Memory,简称为PROM)、可擦除PROM(ErasableProgrammable Read-Only Memory,简称为EPROM)、电可擦除PROM(Electrically ErasableProgrammable Read-Only Memory,简称为EEPROM)、电可改写ROM(ElectricallyAlterable Read-Only Memory,简称为EAROM)或闪存(FLASH)或者两个或更多个以上这些的组合。在合适的情况下,该RAM可以是静态随机存取存储器(Static Random-AccessMemory,简称为SRAM)或动态随机存取存储器(Dynamic Random Access Memory,简称为DRAM),其中,DRAM可以是快速页模式动态随机存取存储器(Fast Page Mode DynamicRandom Access Memory,简称为FPMDRAM)、扩展数据输出动态随机存取存储器(ExtendedDate Out Dynamic Random Access Memory,简称为EDODRAM)、同步动态随机存取内存(Synchronous Dynamic Random-Access Memory,简称SDRAM)等。
存储器102可以用来存储或者缓存需要处理和/或通信使用的各种数据文件,以及处理器101所执行的可能的计算机程序指令。
处理器101通过读取并执行存储器102中存储的计算机程序指令,以实现上述基于北斗的智慧工地车辆定位方法。
在其中一些实施例中,计算机还可包括通信接口103和总线100。其中,如图8所示,处理器101、存储器102、通信接口103通过总线100连接并完成相互间的通信。
通信接口103用于实现本申请实施例中各模块、装置、单元和/或设备之间的通信。通信接口103还可以实现与其他部件例如:外接设备、图像/数据采集设备、数据库、外部存储以及图像/数据处理工作站等之间进行数据通信。
总线100包括硬件、软件或两者,将计算机设备的部件彼此耦接在一起。总线100包括但不限于以下至少之一:数据总线(Data Bus)、地址总线(Address Bus)、控制总线(Control Bus)、扩展总线(Expansion Bus)、局部总线(Local Bus)。举例来说而非限制,总线100可包括图形加速接口(Accelerated Graphics Port,简称为AGP)或其他图形总线、增强工业标准架构(Extended Industry Standard Architecture,简称为EISA)总线、前端总线(Front Side Bus,简称为FSB)、超传输(Hyper Transport,简称为HT)互连、工业标准架构(Industry Standard Architecture,简称为ISA)总线、无线带宽(InfiniBand)互连、低引脚数(Low Pin Count,简称为LPC)总线、存储器总线、微信道架构(Micro ChannelArchitecture,简称为MCA)总线、***组件互连(Peripheral Component Interconnect,简称为PCI)总线、PCI-Express(PCI-X)总线、串行高级技术附件(Serial AdvancedTechnology Attachment,简称为SATA)总线、视频电子标准协会局部(Video ElectronicsStandards Association Local Bus,简称为VLB)总线或其他合适的总线或者两个或更多个以上这些的组合。在合适的情况下,总线100可包括一个或多个总线。尽管本申请实施例描述和示出了特定的总线,但本申请考虑任何合适的总线或互连。
该计算机可以基于获取到基于北斗的智慧工地车辆定位***,执行本申请的基于北斗的智慧工地车辆定位方法,从而实现智慧工地车辆的定位方法。
在本发明的再一些实施例中,结合上述的基于北斗的智慧工地车辆定位方法,本发明实施例提供以下技术方案,一种存储介质,所述存储介质上存储有计算机程序,所述计算机程序被处理器执行时实现上述的基于北斗的智慧工地车辆定位方法。
本领域技术人员可以理解,在流程图中表示或在此以其他方式描述的逻辑和/或步骤,例如,可以被认为是用于实现逻辑功能的可执行指令的定序列表,可以具体实现在任何计算机可读介质中,以供指令执行***、装置或设备(如基于计算机的***、包括处理器的***或其他可以从指令执行***、装置或设备取指令并执行指令的***)使用,或结合这些指令执行***、装置或设备而使用。就本说明书而言,“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行***、装置或设备或结合这些指令执行***、装置或设备而使用的装置。
可读介质的更具体的示例(非穷尽性列表)包括以下:具有一个或多个布线的电连接部(电子装置),便携式计算机盘盒(磁装置),随机存取存储器(RAM),只读存储器(ROM),可擦除可编辑只读存储器(EPROM或闪速存储器),光纤装置,以及便携式光盘只读存储器(CDROM)。另外,计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质,因为可以例如通过对纸或其他介质进行光学扫描,接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序,然后将其存储在计算机存储器中。
应当理解,本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中,多个步骤或方法可以用存储在存储器中且由合适的指令执行***执行的软件或固件来实现。例如,如果用硬件来实现,和在另一实施方式中一样,可用本领域公知的下列技术中的任一项或它们的组合来实现:具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路,具有合适的组合逻辑门电路的专用集成电路,可编程门阵列(PGA),现场可编程门阵列(FPGA)等。
以上所述实施例的各技术特征可以进行任意的组合,为使描述简洁,未对上述实施例中的各个技术特征所有可能的组合都进行描述,然而,只要这些技术特征的组合不存在矛盾,都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干变形和改进,这些都属于本申请的保护范围。因此,本申请专利的保护范围应以所附权利要求为准。
Claims (8)
1.一种基于北斗的智慧工地车辆定位方法,其特征在于,包括:
获取北斗卫星观测数据,对所述北斗卫星观测数据进行误差矫正处理,以得到矫正观测数据;
基于所述矫正观测数据确定第一伪距观测量,计算所述第一伪距观测量的第一检验值与第二检验值,基于所述第一检验值与所述第二检验值对所述伪距观测量进行噪声剔除,以得到第二伪距观测量;
基于所述第二伪距观测量对智慧工地车辆进行坐标联合解算,以得到所述智慧工地车辆的第一定位数据;
获取所述智慧工地车辆的惯性数据与里程数据,将所述第一定位数据、所述惯性数据与所述里程数据进行融合,以得到融合数据;
将所述融合数据进行粒子转化,以得到位姿粒子集,对所述位姿粒子集进行权重更新与采样处理,以得到所述智慧工地车辆的第二定位数据;
所述获取所述智慧工地车辆的惯性数据与里程数据,将所述第一定位数据、所述惯性数据与所述里程数据进行融合,以得到融合数据的步骤包括:
获取所述智慧工地车辆的惯性数据与里程数据,并基于所述第一定位数据、所述惯性数据与所述里程数据确定第时刻的智慧工地车辆的初始位姿矩阵/>:
;
式中,为第/>时刻的智慧工地车辆在全局坐标下的x轴坐标、y轴坐标、z轴坐标,/>为第/>时刻的智慧工地车辆在全局坐标下的角度,/>分别为第/>时刻的智慧工地车辆的全局线速度、全局角速度;
对所述初始位姿矩阵进行运动转化,以得到转化位姿矩阵/>:
;
式中,为第/>时刻的智慧工地车辆在全局坐标下的x轴坐标、y轴坐标、z轴坐标,/>为第/>时刻的智慧工地车辆在全局坐标下的角度,/>分别为第/>时刻的智慧工地车辆的全局线速度、全局角速度,/>为高斯分布噪声;
将所述转化位姿矩阵、所述惯性数据与所述里程数据进行融合,以得到融合数据/>:
;
式中,、/>分别为里程噪声、惯性噪声;
所述对所述位姿粒子集进行权重更新与采样处理,以得到所述智慧工地车辆的第二定位数据的步骤包括:
将第时刻的位姿粒子集/>均匀分布至全局空间中,其中,/>,/>为第/>个表示第/>时刻的位姿的粒子,/>为粒子/>对应的粒子权重;
通过第时刻的位姿粒子集迭代估计第/>时刻的粒子分布,以得到估计位姿粒子/>:
;
式中,分别为第/>时刻智慧工地车辆的x方向估计位移、y方向估计位移、z方向估计位移、角度估计值,/>、/>、/>、/>为智慧工地车辆在第/>时刻至第/>时刻的x方向位移变化量、y方向位移变化量、z方向位移变化量、角度变化量,/>分别为x方向位移噪声、y方向位移噪声、z方向位移噪声、角度噪声;
基于第时刻的粒子分布对位姿粒子集/>中每个粒子权重进行评估更新,以得到更新权重/>:
;
式中,为观测结果,/>为给定估计位姿粒子/>的条件下采样得到/>的理想概率密度;
基于所述更新权重对所述位姿粒子集中的所有粒子进行重新采样,以得到采样权重与采样粒子/>,基于所述采样权重/>计算有效粒子数量/>;
;
判断所述有效粒子数量是否大于粒子阈值,若所述有效粒子数量/>大于粒子阈值,则第二定位数据/>,若所述有效粒子数量/>不大于粒子阈值,则在所述位姿粒子集添加若干数量的随机粒子,以得到更新粒子集,重复对所述更新粒子集进行粒子分布估计、权重更新与重新采样直至有效粒子数量/>大于粒子阈值,输出对应的迭代采样权重/>与迭代采样粒子/>,则第二定位数据/>,其中,/>为更新粒子集中的粒子数量。
2.根据权利要求1所述的基于北斗的智慧工地车辆定位方法,其特征在于,所述对所述北斗卫星观测数据进行误差矫正处理,以得到矫正观测数据的步骤包括:
基于第一预设公式对所述北斗卫星观测数据进行第一误差矫正处理,其中,第一预设公式为:
;
式中,为卫星钟差修正值,/>为时钟偏差,/>、/>分别为卫星信号发射时刻、卫星信号接收时刻,/>为时钟漂移,/>为频率漂移,/>为光速,/>为卫星轨道偏心角,/>为卫星轨道偏近点角,/>为引力常数,/>为卫星轨道半长轴;
基于第二预设公式对所述北斗卫星观测数据进行第二误差矫正处理,其中,第二预设公式为:
;
;
式中,为电离层修正值,/>、/>分别为预设改正模型中的第/>个第一网络参数、第/>个第二网络参数,/>为电离层穿刺点的纬度,/>为电离层穿刺点的地方时,/>、/>分别为第一修正常数与第二修正常数,/>为观测站的地理纬度,/>为卫星高度角,/>为地球半径,/>为电离层高度,/>为卫星方位角;
基于第三预设公式对所述北斗卫星观测数据进行第三误差矫正处理,其中,第三预设公式为:
;
式中,为对流层修正值,/>为第三修正常数,/>为地面气压,/>为地面水气压,/>为地面湿度,/>分别为测站大地高度、干大气层顶高度、湿大气层顶高度。
3.根据权利要求1所述的基于北斗的智慧工地车辆定位方法,其特征在于,所述基于所述矫正观测数据确定第一伪距观测量的步骤包括:
通过第四预设公式并基于所述矫正观测数据确定第一伪距观测量:
;
式中,为车载接收机至北斗卫星之间的真实距离,/>为光速,/>分别为卫星钟差修正值、电离层修正值、对流层修正值,/>为车载接收机钟差,/>为其他误差。
4.根据权利要求1所述的基于北斗的智慧工地车辆定位方法,其特征在于,所述计算所述第一伪距观测量的第一检验值与第二检验值,基于所述第一检验值与所述第二检验值对所述伪距观测量进行噪声剔除,以得到第二伪距观测量的步骤包括:
计算所述第一伪距观测量的第一检验值:
;
式中,分别为北斗卫星、车载接收机的第一码偏差,/>为北斗卫星、车载接收机的第一码偏差对应的时变量,/>为第一观测噪声;
计算所述第一伪距观测量的第二检验值;
;
式中,分别为北斗卫星、车载接收机的第二码偏差,/>为北斗卫星、车载接收机的第二码偏差对应的时变量,/>为第二观测噪声,/>为电离层时延参数与误差项;
判断所述第一检验值是否大于第一剔除阈值/>以及所述第二检验值/>是否大于第二剔除阈值/>,其中,/>;
若所述第一检验值不大于第一剔除阈值/>且所述第二检验值/>不大于第二剔除阈值/>,则将对应的第一伪距观测量进行保留,若所述第一检验值/>大于第一剔除阈值/>和/或所述第二检验值/>大于第二剔除阈值/>,则将对应的第一伪距观测量进行剔除,以得到第二伪距观测量。
5.根据权利要求1所述的基于北斗的智慧工地车辆定位方法,其特征在于,所述基于所述第二伪距观测量对智慧工地车辆进行坐标联合解算,以得到所述智慧工地车辆的第一定位数据的步骤包括:
基于所述第二伪距观测量建立坐标解算方程:
;
式中,为第一定位数据的坐标,/>为第/>个第二伪距观测量,/>为第/>个第二伪距观测量对应的北斗卫星坐标,/>为车载接收机钟差,/>为光速;
将所述坐标解算方程进行泰勒展开,以得到线性解算方程;
采用加权最小二乘法对所述线性解算方程进行求解,以得到第一定位数据。
6.一种基于北斗的智慧工地车辆定位***,其特征在于,所述***包括:
获取模块,用于获取北斗卫星观测数据,对所述北斗卫星观测数据进行误差矫正处理,以得到矫正观测数据;
检验模块,用于基于所述矫正观测数据确定第一伪距观测量,计算所述第一伪距观测量的第一检验值与第二检验值,基于所述第一检验值与所述第二检验值对所述伪距观测量进行噪声剔除,以得到第二伪距观测量;
解算模块,用于基于所述第二伪距观测量对智慧工地车辆进行坐标联合解算,以得到所述智慧工地车辆的第一定位数据;
融合模块,用于获取所述智慧工地车辆的惯性数据与里程数据,将所述第一定位数据、所述惯性数据与所述里程数据进行融合,以得到融合数据;
更新模块,用于将所述融合数据进行粒子转化,以得到位姿粒子集,对所述位姿粒子集进行权重更新与采样处理,以得到所述智慧工地车辆的第二定位数据;
所述融合模块包括:
初始位姿矩阵确定子模块,用于获取所述智慧工地车辆的惯性数据与里程数据,并基于所述第一定位数据、所述惯性数据与所述里程数据确定第时刻的智慧工地车辆的初始位姿矩阵/>:
;
式中,为第/>时刻的智慧工地车辆在全局坐标下的x轴坐标、y轴坐标、z轴坐标,/>为第/>时刻的智慧工地车辆在全局坐标下的角度,/>分别为第/>时刻的智慧工地车辆的全局线速度、全局角速度;
转化位姿矩阵确定子模块,用于对所述初始位姿矩阵进行运动转化,以得到转化位姿矩阵/>:
;
式中,为第/>时刻的智慧工地车辆在全局坐标下的x轴坐标、y轴坐标、z轴坐标,/>为第/>时刻的智慧工地车辆在全局坐标下的角度,/>分别为第/>时刻的智慧工地车辆的全局线速度、全局角速度,/>为高斯分布噪声;
融合子模块,用于将所述转化位姿矩阵、所述惯性数据与所述里程数据进行融合,以得到融合数据/>:
;
式中,、/>分别为里程噪声、惯性噪声;
所述更新模块包括:
分布子模块,用于将第时刻的位姿粒子集/>均匀分布至全局空间中,其中,/>,/>为第/>个表示第/>时刻的位姿的粒子,/>为粒子/>对应的粒子权重;
估计子模块,用于通过第时刻的位姿粒子集迭代估计第/>时刻的粒子分布,以得到估计位姿粒子/>:
;
式中,分别为第/>时刻智慧工地车辆的x方向估计位移、y方向估计位移、z方向估计位移、角度估计值,/>、/>、/>、/>为智慧工地车辆在第/>时刻至第/>时刻的x方向位移变化量、y方向位移变化量、z方向位移变化量、角度变化量,/>分别为x方向位移噪声、y方向位移噪声、z方向位移噪声、角度噪声;
更新子模块,用于基于第时刻的粒子分布对位姿粒子集/>中每个粒子权重进行评估更新,以得到更新权重/>:
;
式中,为观测结果,/>为给定估计位姿粒子/>的条件下采样得到/>的理想概率密度;
采样子模块,用于基于所述更新权重对所述位姿粒子集中的所有粒子进行重新采样,以得到采样权重/>与采样粒子/>,基于所述采样权重/>计算有效粒子数量/>;
;
第二判断子模块,用于判断所述有效粒子数量是否大于粒子阈值,若所述有效粒子数量/>大于粒子阈值,则第二定位数据/>,若所述有效粒子数量/>不大于粒子阈值,则在所述位姿粒子集添加若干数量的随机粒子,以得到更新粒子集,重复对所述更新粒子集进行粒子分布估计、权重更新与重新采样直至有效粒子数量/>大于粒子阈值,输出对应的迭代采样权重/>与迭代采样粒子/>,则第二定位数据/>,其中,/>为更新粒子集中的粒子数量。
7.一种计算机,包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序,其特征在于,所述处理器执行所述计算机程序时实现如权利要求1至5任一项所述的基于北斗的智慧工地车辆定位方法。
8.一种存储介质,其特征在于,所述存储介质上存储有计算机程序,所述计算机程序被处理器执行时实现如权利要求1至5任一项所述的基于北斗的智慧工地车辆定位方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311715088.0A CN117406259B (zh) | 2023-12-14 | 2023-12-14 | 一种基于北斗的智慧工地车辆定位方法及*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311715088.0A CN117406259B (zh) | 2023-12-14 | 2023-12-14 | 一种基于北斗的智慧工地车辆定位方法及*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN117406259A CN117406259A (zh) | 2024-01-16 |
CN117406259B true CN117406259B (zh) | 2024-03-22 |
Family
ID=89498316
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202311715088.0A Active CN117406259B (zh) | 2023-12-14 | 2023-12-14 | 一种基于北斗的智慧工地车辆定位方法及*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN117406259B (zh) |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109633718A (zh) * | 2018-12-12 | 2019-04-16 | 上海无线电设备研究所 | 一种归一化加权最小二乘导航定位方法 |
CN110196044A (zh) * | 2019-05-28 | 2019-09-03 | 广东亿嘉和科技有限公司 | 一种基于gps闭环检测的变电站巡检机器人建图方法 |
CN110243358A (zh) * | 2019-04-29 | 2019-09-17 | 武汉理工大学 | 多源融合的无人车室内外定位方法及*** |
WO2020048623A1 (en) * | 2018-09-07 | 2020-03-12 | Huawei Technologies Co., Ltd. | Estimation of a pose of a robot |
CN111272165A (zh) * | 2020-02-27 | 2020-06-12 | 清华大学 | 一种基于特征点标定的智能车定位方法 |
CN114459470A (zh) * | 2022-01-19 | 2022-05-10 | 国网江西省电力有限公司超高压分公司 | 基于多传感器融合的巡检机器人定位方法 |
CN115265551A (zh) * | 2021-08-26 | 2022-11-01 | 北京理工大学 | 一种无人驾驶复杂场景下的多传感器融合定位方法和*** |
CN115900708A (zh) * | 2022-10-14 | 2023-04-04 | 西北工业大学 | 基于gps引导式粒子滤波的机器人多传感器融合定位方法 |
WO2023131048A1 (zh) * | 2022-01-06 | 2023-07-13 | 上海安亭地平线智能交通技术有限公司 | 位姿信息的确定方法、装置、电子设备和存储介质 |
-
2023
- 2023-12-14 CN CN202311715088.0A patent/CN117406259B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2020048623A1 (en) * | 2018-09-07 | 2020-03-12 | Huawei Technologies Co., Ltd. | Estimation of a pose of a robot |
CN109633718A (zh) * | 2018-12-12 | 2019-04-16 | 上海无线电设备研究所 | 一种归一化加权最小二乘导航定位方法 |
CN110243358A (zh) * | 2019-04-29 | 2019-09-17 | 武汉理工大学 | 多源融合的无人车室内外定位方法及*** |
CN110196044A (zh) * | 2019-05-28 | 2019-09-03 | 广东亿嘉和科技有限公司 | 一种基于gps闭环检测的变电站巡检机器人建图方法 |
CN111272165A (zh) * | 2020-02-27 | 2020-06-12 | 清华大学 | 一种基于特征点标定的智能车定位方法 |
CN115265551A (zh) * | 2021-08-26 | 2022-11-01 | 北京理工大学 | 一种无人驾驶复杂场景下的多传感器融合定位方法和*** |
WO2023131048A1 (zh) * | 2022-01-06 | 2023-07-13 | 上海安亭地平线智能交通技术有限公司 | 位姿信息的确定方法、装置、电子设备和存储介质 |
CN114459470A (zh) * | 2022-01-19 | 2022-05-10 | 国网江西省电力有限公司超高压分公司 | 基于多传感器融合的巡检机器人定位方法 |
CN115900708A (zh) * | 2022-10-14 | 2023-04-04 | 西北工业大学 | 基于gps引导式粒子滤波的机器人多传感器融合定位方法 |
Non-Patent Citations (7)
Title |
---|
A Multi-Source Fusion Navigation System to Overcome GPS Interruption of Unmanned Ground Vehicles;Kuibao Zhu;IEEE Access;20230602;全文 * |
北斗***对流层延迟改正模型精度分析;张瑜;雷达科学与技术;20180830;第2节 * |
室外变电站巡检机器人自主导航研究;于志浩;中国硕士学位论文全文数据库信息科技辑;20220515;全文 * |
室外移动机器人的定位和避障研究与***实现;郭恒宇;中国硕士学位论文全文数据库信息科技辑;20200715;全文 * |
李征航,黄劲松.GPS测量与数据处理.武汉大学出版社,2005,第3章. * |
水下石油管道漏油检测定位的粒子滤波SLAM算法;袁赣南;王丹丹;魏延辉;洪伟;;中国惯性技术学报;20130415(第02期);全文 * |
郭斐.GPS精密单点定位质量控制与分析的相关理论和方法研究.武汉大学出版社,2016,第3.4节. * |
Also Published As
Publication number | Publication date |
---|---|
CN117406259A (zh) | 2024-01-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108732603B (zh) | 用于定位车辆的方法和装置 | |
EP3875985B1 (en) | Method, apparatus, computing device and computer-readable storage medium for positioning | |
US8326532B2 (en) | Arrangement for and method of two dimensional and three dimensional precision location and orientation determination | |
US9886849B2 (en) | Toll object detection in a GNSS system using particle filter | |
JP2020515816A (ja) | Gnss受信機保護レベル | |
US20090138188A1 (en) | Method, device and system for modeling a road network graph | |
CN110208823B (zh) | 确定用于基于卫星确定车辆位置的数据简档的方法 | |
CN114624741A (zh) | 一种定位精度评估方法及装置 | |
CN114264301B (zh) | 车载多传感器融合定位方法、装置、芯片及终端 | |
CN117406259B (zh) | 一种基于北斗的智慧工地车辆定位方法及*** | |
CN116794695A (zh) | 基于双重优化的高程辅助低轨卫星机会信号定位方法 | |
CN115616637B (zh) | 一种基于三维格网多径建模的城市复杂环境导航定位方法 | |
KR20220039709A (ko) | 적어도 하나의 환경-특정 gnss 프로파일을 나타내기 위한 모델을 결정하는 방법 | |
CN111276000A (zh) | 一种基于车路协同的定位方法及***、介质和车载设备 | |
CN110794434A (zh) | 一种位姿的确定方法、装置、设备及存储介质 | |
No et al. | Machine learning based overbound modeling of multipath error for safety critical urban environment | |
US20240159914A1 (en) | Method for taking provided gnss-relevant route information into account in the gnss-based localization of vehicles | |
AU2008351425B2 (en) | GPS filter algorithm | |
CN112629553B (zh) | 一种智能网联环境下车辆协同定位方法、***及装置 | |
CN111399019B (zh) | 一种gnss中卫星定位方法和装置 | |
Mok et al. | GPS vehicle location tracking in dense high-rise environments with the minimum range error algorithm | |
US20240085572A1 (en) | Method for providing gnss-relevant route information | |
Bakırcı | Reducing GPS impreciseness by odometer sensor reading to improve positioning accuracy | |
US20240118435A1 (en) | Post-processing of global navigation satellite system (gnss) data | |
Sharma et al. | Multipath Error Modelling and Position Error Over-bounding for Precise RTK Positioning using GNSS Raw Measurements from Smartphone for Automotive Navigation |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |