CN110146910B - Positioning method and device based on data fusion of GPS and laser radar - Google Patents

Positioning method and device based on data fusion of GPS and laser radar Download PDF

Info

Publication number
CN110146910B
CN110146910B CN201910403394.8A CN201910403394A CN110146910B CN 110146910 B CN110146910 B CN 110146910B CN 201910403394 A CN201910403394 A CN 201910403394A CN 110146910 B CN110146910 B CN 110146910B
Authority
CN
China
Prior art keywords
gps
positioning
road
laser radar
longitude
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
Application number
CN201910403394.8A
Other languages
Chinese (zh)
Other versions
CN110146910A (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.)
Chongqing University
Original Assignee
Chongqing University
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 Chongqing University filed Critical Chongqing University
Priority to CN201910403394.8A priority Critical patent/CN110146910B/en
Publication of CN110146910A publication Critical patent/CN110146910A/en
Application granted granted Critical
Publication of CN110146910B publication Critical patent/CN110146910B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO 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
    • G01S17/00Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06Systems determining position data of a target
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO 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
    • G01S17/00Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/50Systems of measurement based on relative movement of target
    • G01S17/58Velocity or trajectory determination systems; Sense-of-movement determination systems
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO 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/00Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39Determining 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/42Determining position
    • G01S19/48Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

The invention discloses a positioning method based on data fusion of a GPS and a laser radar, which comprises the following steps: acquiring a GPS positioning coordinate; acquiring road environment information, and establishing a road environment characteristic model; dividing the map into grid maps according to a certain resolution ratio according to the GPS error, and determining a key grid area according to the GPS positioning coordinate; matching the road environment characteristic model with road key point characteristic parameters in the key grid area; calculating to obtain longitude and latitude coordinates of the vehicle according to the longitude and latitude coordinates of the successfully matched road key points and the coordinates of the road key points relative to the laser radar; and performing data fusion on the GPS positioning coordinate, the vehicle longitude and latitude coordinate, the vehicle speed and the course angle by adopting an extended Kalman filter to obtain a final positioning result. The invention combines the advantages of GPS in global positioning and the advantages of laser radar in local positioning, and greatly improves the positioning accuracy of the GPS positioning system.

Description

一种基于GPS与激光雷达数据融合的定位方法及装置A positioning method and device based on GPS and laser radar data fusion

技术领域technical field

本发明属于定位技术领域,具体涉及一种基于GPS与激光雷达数据融合的定位方法及装置。The invention belongs to the technical field of positioning, and in particular relates to a positioning method and device based on fusion of GPS and laser radar data.

背景技术Background technique

车辆定位在自动驾驶汽车领域的研究中是一个关键问题,扮演着极为重要的角色。一个高精度的定位效果不仅仅使自动驾驶汽车的轨迹跟随控制变得更容易,还能基于位置共享实现车与车(V2V),车与路边基础设施(V2I)以及车与城市网络的互联,是实现智能交通的必由之路。Vehicle localization is a key issue in research in the field of autonomous vehicles and plays an extremely important role. A high-precision positioning effect not only makes the trajectory following control of autonomous vehicles easier, but also enables vehicle-to-vehicle (V2V), vehicle-to-roadside infrastructure (V2I) and vehicle-to-city network interconnection based on location sharing , is the only way to realize intelligent transportation.

近年来导航定位技术不断发展,按照所使用的传感器设备不同可以分为基于磁传感器阵列的定位技术、航迹推算(Dead reckoning,DR)、惯性导航(Inertial navigation)、卫星定位、视觉定位、基于激光雷达的定位技术等。而在具体系统实现过程中,各种定位技术可以相互独立使用,也可以使用多种技术相互组合使用。In recent years, navigation and positioning technology has continued to develop. According to the different sensor devices used, it can be divided into positioning technology based on magnetic sensor arrays, dead reckoning (DR), inertial navigation (Inertial navigation), satellite positioning, visual positioning, based on Laser radar positioning technology, etc. In the process of implementing a specific system, various positioning technologies can be used independently of each other, or can be used in combination with each other.

由车辆定位技术的研究和应用现状可知,现有的各种定位方法仍然存在许多不足之处。对于单一传感器定位领域,GPS定位应用便捷但由于其测量原理存在较大的误差;差分GPS定位虽然能提高定位精度,但是需要一定范围内具有基准站,难以普及;基于惯导系统的定位方法存在不可忽视的累积误差;基于磁传感器阵列的导航方法虽然较为精确,但是需要提前铺设,灵活度极差。现有的多传感器融合定位方法中,GPS与无线局域网或4G网络的融合定位方法,能够有效的解决室内GPS信号遮挡的问题,实现无缝定位,但对于定位精度的提高十分有限;其余如GPS与惯导系统、GPS与视觉传感等的融合定位方法虽然能够提升一定的定位精度,但是仍然不能满足未来自动驾驶汽车的定位需求。According to the research and application status of vehicle positioning technology, there are still many deficiencies in the existing positioning methods. For the field of single-sensor positioning, GPS positioning is convenient to apply, but there are large errors in its measurement principle; although differential GPS positioning can improve positioning accuracy, it requires a reference station within a certain range, which is difficult to popularize; positioning methods based on inertial navigation systems exist The cumulative error cannot be ignored; although the navigation method based on the magnetic sensor array is more accurate, it needs to be laid in advance, and the flexibility is extremely poor. Among the existing multi-sensor fusion positioning methods, the fusion positioning method of GPS and wireless local area network or 4G network can effectively solve the problem of indoor GPS signal occlusion and realize seamless positioning, but the improvement of positioning accuracy is very limited; others such as GPS Although the fusion positioning method with inertial navigation system, GPS and visual sensing can improve the positioning accuracy to a certain extent, it still cannot meet the positioning requirements of future self-driving cars.

发明内容Contents of the invention

鉴于以上所述现有技术的缺点,本发明提供一种基于GPS与激光雷达数据融合的定位方法及装置,以解决现有的定位技术精度不够高的缺陷。In view of the shortcomings of the prior art described above, the present invention provides a positioning method and device based on fusion of GPS and laser radar data to solve the defect that the accuracy of the existing positioning technology is not high enough.

为实现上述目的及其他相关目的,本发明提供一种基于GPS与激光雷达数据融合的定位方法,所述定位方法包括:In order to achieve the above purpose and other related purposes, the present invention provides a positioning method based on fusion of GPS and laser radar data. The positioning method includes:

获取GPS定位坐标;Obtain GPS positioning coordinates;

获取道路环境信息,建立道路环境特征模型;Obtain road environment information and establish a road environment characteristic model;

根据GPS误差将地图按一定的分辨率分割成为栅格地图,根据所GPS定位坐标确定出关键栅格区域;According to the GPS error, the map is divided into a grid map according to a certain resolution, and the key grid area is determined according to the GPS positioning coordinates;

将所述道路环境特征模型与所述关键栅格区域中的道路关键点特征参数进行匹配;Matching the road environment feature model with the road key point feature parameters in the key grid area;

根据匹配成功的道路关键点的经纬度坐标和该道路关键点相对于激光雷达的坐标计算得到车辆经纬度坐标;According to the longitude and latitude coordinates of the successfully matched road key points and the coordinates of the road key points relative to the lidar, the longitude and latitude coordinates of the vehicle are calculated;

采用扩展卡尔曼滤波器对所述GPS定位坐标、所述车辆经纬度坐标、车辆速度v和航向角

Figure BDA0002060489470000022
进行数据融合,得到最终的定位结果。Using an extended Kalman filter to perform the GPS positioning coordinates, the latitude and longitude coordinates of the vehicle, the vehicle speed v and the heading angle
Figure BDA0002060489470000022
Perform data fusion to obtain the final positioning result.

可选地,所述采用2D激光雷达进行检测,获取道路环境信息,建立道路环境特征模型,具体包括:Optionally, the 2D lidar is used for detection, the road environment information is obtained, and the road environment characteristic model is established, which specifically includes:

获取2D激光雷达数据,以激光雷达装置位置为坐标原点,环境数据为点簇数据;Obtain 2D lidar data, take the location of the lidar device as the coordinate origin, and the environmental data as point cluster data;

对所述点簇数据进行预处理;Preprocessing the point cluster data;

采用split-merge算法对预处理后的点簇数据进行聚类,建模。The split-merge algorithm is used to cluster and model the preprocessed point cluster data.

可选地,所述对所述点簇数据进行预处理,具体包括:Optionally, the preprocessing of the point cluster data specifically includes:

对点簇数据进行中值滤波,得到滤波后的点簇数据;Perform median filtering on the point cluster data to obtain filtered point cluster data;

对激光雷达数据作误差补偿;Error compensation for lidar data;

以车辆中心为原点、横轴为车辆水平方向、纵轴为车辆直行方向将极坐标形式的激光雷达数据转换为直角坐标。Taking the center of the vehicle as the origin, the horizontal axis as the horizontal direction of the vehicle, and the vertical axis as the straight direction of the vehicle, the lidar data in polar coordinates is converted into rectangular coordinates.

可选地,将所述道路环境特征模型与所述关键栅格区域中的道路关键点特征参数进行匹配,具体包括:Optionally, matching the road environment feature model with the road key point feature parameters in the key grid area specifically includes:

以道路环境模型中关键点{Xj,Yj}为原点,按顺时针方向提取道路环境特征模型中的特征角度θ和距离d得到特征集合{θ123,d14,d2…};Taking the key point {X j , Y j } in the road environment model as the origin, extract the characteristic angle θ and distance d in the road environment feature model clockwise to obtain the feature set {θ 123 ,d 1 , θ 4 ,d 2 ...};

将所述特征集合{θ123,d14,d2…}与所述关键栅格区域中的道路关键点信息依次进行比对,在一定阈值范围内均匹配成功则认为道路关键点匹配成功。Compare the feature set {θ 1 , θ 2 , θ 3 , d 1 , θ 4 , d 2 ...} with the road key point information in the key grid area in sequence, and match within a certain threshold range If successful, it is considered that the key points of the road are successfully matched.

可选地,所述车辆经纬度坐标(longL,latL)表示为:Optionally, the latitude and longitude coordinates (long L , lat L ) of the vehicle are expressed as:

Figure BDA0002060489470000021
Figure BDA0002060489470000021

其中,longk表示道路关键点经度坐标,latk表示道路关键点纬度坐标,d为关键点与激光雷达的欧氏距离;α位车辆行驶方位角,ARC为地球平均半径。Among them, longk represents the longitude coordinates of key points on the road, latk represents the latitude coordinates of key points on the road, d is the Euclidean distance between the key point and the lidar; α is the azimuth of the vehicle, and ARC is the average radius of the earth.

可选地,采用扩展卡尔曼滤波器对所述GPS定位坐标、所述车辆经纬度坐标、车辆速度v和航向角

Figure BDA0002060489470000031
进行数据融合,得到最终的定位结果,其中,扩展卡尔曼滤波器的状态方程和观测方程如下:Optionally, the GPS positioning coordinates, the vehicle latitude and longitude coordinates, the vehicle speed v and the heading angle are analyzed using an extended Kalman filter
Figure BDA0002060489470000031
Carry out data fusion to obtain the final positioning result, among which, the state equation and observation equation of the extended Kalman filter are as follows:

Figure BDA0002060489470000032
Figure BDA0002060489470000032

式中:f()是非线性系统的状态转移函数,H()是非线性系统的观测函数,w(k)和v(k)为系统噪声;U(k)是输入值,X(k)是状态变量,Z(k)是观测值;Where: f() is the state transition function of the nonlinear system, H() is the observation function of the nonlinear system, w(k) and v(k) are system noises; U(k) is the input value, X(k) is State variable, Z(k) is the observed value;

则定位系统模型如下:Then the positioning system model is as follows:

Figure BDA0002060489470000033
Figure BDA0002060489470000033

式中:t为采样间隔时间,xk,yk分别为所求定位经度、纬度。In the formula: t is the sampling interval time, x k , y k are the longitude and latitude of the desired location respectively.

为实现上述目的及其他相关目的,本发明还提供一种基于GPS与激光雷达数据融合的定位装置,该装置包括:In order to achieve the above purpose and other related purposes, the present invention also provides a positioning device based on fusion of GPS and laser radar data, which includes:

坐标获取模块,用于获取GPS定位坐标;A coordinate acquisition module, configured to acquire GPS positioning coordinates;

模型建立模块,用于获取道路环境信息,建立道路环境特征模型;The model building module is used to obtain road environment information and establish a road environment characteristic model;

特征提取模块,用于根据GPS误差将地图按一定的分辨率分割成为栅格地图,根据所GPS定位坐标确定出关键栅格区域;The feature extraction module is used to divide the map into a grid map according to a certain resolution according to the GPS error, and determine the key grid area according to the GPS positioning coordinates;

特征匹配模块,用于将所述道路环境特征模型与所述关键栅格区域中的道路关键点特征参数进行匹配;A feature matching module, configured to match the road environment feature model with the road key point feature parameters in the key grid area;

坐标计算模块,用于根据匹配成功的道路关键点的经纬度坐标和该道路关键点相对于激光雷达的坐标计算得到车辆经纬度坐标;The coordinate calculation module is used to calculate the longitude and latitude coordinates of the vehicle according to the latitude and longitude coordinates of the successfully matched road key points and the coordinates of the road key points relative to the laser radar;

数据融合模块,用于采用扩展卡尔曼滤波器对所述GPS定位坐标、所述车辆经纬度坐标、车辆速度v和航向角

Figure BDA0002060489470000034
进行数据融合,得到最终的定位结果。The data fusion module is used to adopt the extended Kalman filter to the GPS positioning coordinates, the vehicle latitude and longitude coordinates, vehicle speed v and heading angle
Figure BDA0002060489470000034
Perform data fusion to obtain the final positioning result.

为实现上述目的及其他相关目的,本发明还提供一种可读存储介质,存储计算机程序,所述计算机程序被处理器运行时执行所述的定位方法。To achieve the above object and other related objects, the present invention also provides a readable storage medium storing a computer program, and the computer program executes the positioning method when run by a processor.

为实现上述目的及其他相关目的,本发明还提供一种电子终端,包括:处理器及存储器;To achieve the above object and other related objects, the present invention also provides an electronic terminal, including: a processor and a memory;

所述存储器用于存储计算机程序,所述处理器用于执行所述存储器存储的计算机程序,以使所述终端执行所述的定位方法。The memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory, so that the terminal executes the positioning method.

如上所述,本发明的一种基于GPS与激光雷达数据融合的定位方法、装置,具有以下有益效果:As mentioned above, a positioning method and device based on the fusion of GPS and laser radar data of the present invention has the following beneficial effects:

本发明一种基于GPS与激光雷达数据融合的定位方法,结合了GPS在全局定位的优势和激光雷达在局部定位的优势,运用GPS先进行粗定位确定大致区域范围,再利用激光雷达对附近环境进行建模、提取特征,与历史环境信息进行匹配,得到局部精确定位,最后将多种传感器的数据进行融合,得到最终定位结果,大幅度提高了GPS定位系统的定位精度。A positioning method based on GPS and laser radar data fusion in the present invention combines the advantages of GPS in global positioning and laser radar in local positioning, uses GPS to perform rough positioning to determine the approximate area, and then uses laser radar to analyze the surrounding environment Carry out modeling, extract features, match with historical environmental information, obtain local accurate positioning, and finally fuse data from various sensors to obtain the final positioning result, which greatly improves the positioning accuracy of the GPS positioning system.

附图说明Description of drawings

图1是本发明实施例的一种基于GPS与激光雷达数据融合的定位方法的流程图;Fig. 1 is a flow chart of a positioning method based on fusion of GPS and laser radar data according to an embodiment of the present invention;

图2是本发明实施例的激光雷达数据修正几何示意图;Fig. 2 is a geometrical schematic diagram of laser radar data correction according to an embodiment of the present invention;

图3是本发明实施例的激光雷达数据split-merge聚类算法示意图;3 is a schematic diagram of a split-merge clustering algorithm for laser radar data according to an embodiment of the present invention;

图4是本发明实施例的道路环境特征模型图;Fig. 4 is a road environment feature model diagram of an embodiment of the present invention;

图5是本发明实施例的特征提取示意图;Fig. 5 is a schematic diagram of feature extraction according to an embodiment of the present invention;

图6是本发明实施例的经纬度计算示意图;Fig. 6 is a schematic diagram of latitude and longitude calculation according to an embodiment of the present invention;

图7是本发明实施例的基于EKF的数据融合定位;Fig. 7 is the EKF-based data fusion positioning of the embodiment of the present invention;

图8是本发明实施例的数据融合的三个阶段。Fig. 8 shows three stages of data fusion in the embodiment of the present invention.

具体实施方式Detailed ways

以下通过特定的具体实例说明本发明的实施方式,本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用,本说明书中的各项细节也可以基于不同观点与应用,在没有背离本发明的精神下进行各种修饰或改变。需说明的是,在不冲突的情况下,以下实施例及实施例中的特征可以相互组合。Embodiments of the present invention are described below through specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific implementation modes, and various modifications or changes can be made to the details in this specification based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that, in the case of no conflict, the following embodiments and features in the embodiments can be combined with each other.

需要说明的是,以下实施例中所提供的图示仅以示意方式说明本发明的基本构想,遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制,其实际实施时各组件的型态、数量及比例可为一种随意的改变,且其组件布局型态也可能更为复杂。It should be noted that the diagrams provided in the following embodiments are only schematically illustrating the basic ideas of the present invention, and only the components related to the present invention are shown in the diagrams rather than the number, shape and shape of the components in actual implementation. Dimensional drawing, the type, quantity and proportion of each component can be changed arbitrarily during actual implementation, and the component layout type may also be more complicated.

如图1所示,本实施例的一种基于GPS与激光雷达数据融合的定位方法,提高了GPS定位精度,包括如下步骤:As shown in Figure 1, a positioning method based on GPS and lidar data fusion in this embodiment improves the positioning accuracy of GPS, including the following steps:

步骤S1、获取GPS定位坐标;Step S1, obtaining GPS positioning coordinates;

具体地,将GPS原始数据从WGS-84坐标系转换为BD-09坐标系数据(longG,latG),其中longG为经度,latG为纬度,则GPS定位坐标表示为(longG,latG)。Specifically, the original GPS data is converted from the WGS-84 coordinate system to the BD-09 coordinate system data (long G , lat G ), where long G is the longitude and lat G is the latitude, then the GPS positioning coordinates are expressed as (long G , lat G ).

步骤S2、获取道路环境信息,建立道路环境特征模型;具体地,采用2D激光雷达进行检测获取道路环境信息。Step S2, acquiring road environment information, and establishing a road environment feature model; specifically, using 2D laser radar for detection to obtain road environment information.

更加具体地,步骤S2包括以下子步骤:More specifically, step S2 includes the following sub-steps:

步骤S21:获取2D激光雷达数据,以激光雷达装置位置为坐标原点,激光雷达数据为点簇数据{(Lii)|i=1,2,3,...,n},其中点簇数据为极坐标形式,θi表示角度,Li表示距离。Step S21: Obtain 2D lidar data, take the location of the lidar device as the coordinate origin, and the lidar data is point cluster data {(L ii )|i=1,2,3,...,n}, where The point cluster data is in the form of polar coordinates, θ i represents the angle, and Li represents the distance.

步骤S22:对激光雷达点簇数据进行预处理,预处理步骤包括:Step S22: Preprocessing the lidar point cluster data, the preprocessing steps include:

步骤S221:对点簇数据进行中值滤波,得到滤波后的点簇数据{(Lii)|j=1,2,3,...,m,m<n},算法如下:Step S221: Perform median filtering on the point cluster data to obtain filtered point cluster data {(L ii )|j=1,2,3,...,m,m<n}, the algorithm is as follows:

fori=1to n;fori=1to n;

j=1;j = 1;

Lj=mid(Li,Li+1,Li+2);L j = mid(L i ,L i+1 ,L i+2 );

j++;j++;

步骤S222:激光雷达扫描一帧数据大约100ms时间,在这100ms中车辆处于移动过程中,激光束扫描过程中原点发生了变化,需要对激光雷达数据作误差补偿,如图2所示,公式如下:Step S222: The lidar scans a frame of data for about 100ms. During this 100ms, when the vehicle is moving and the origin changes during the laser beam scanning process, it is necessary to make error compensation for the lidar data, as shown in Figure 2. The formula is as follows :

for j=1to mfor j=1to m

Figure BDA0002060489470000051
Figure BDA0002060489470000051

Figure BDA0002060489470000052
Figure BDA0002060489470000052

式中:T为激光雷达扫描周期;γ为激光雷达扫描的角度分辨率;v为车辆行驶速度;St为当前时刻激光雷达数据中序号为i的所测距离;S(t-T)为上一时刻激光雷达数据中序号为i的所测距离;Θj为所测障碍物与横坐标轴的夹角;Sreal为修正后的激光雷达数据。In the formula: T is the laser radar scanning period; γ is the angular resolution of the laser radar scanning; v is the vehicle speed; S t is the measured distance with the serial number i in the current laser radar data; S (t - T) is The measured distance with the sequence number i in the lidar data at the last moment; Θ j is the angle between the measured obstacle and the abscissa axis; S real is the corrected lidar data.

步骤S223:将极坐标形式的激光雷达数据转换为直角坐标,以车辆中心为原点,横轴为车辆水平方向,纵轴为车辆直行方向。Step S223: Convert the lidar data in polar coordinates into rectangular coordinates, with the center of the vehicle as the origin, the horizontal axis as the horizontal direction of the vehicle, and the vertical axis as the straight direction of the vehicle.

Figure BDA0002060489470000061
Figure BDA0002060489470000061

式中:l为车辆纵向长度。In the formula: l is the longitudinal length of the vehicle.

步骤S23:采用split-merge算法对激光雷达数据进行聚类,建模;拟合方法采用最小二乘法,如图3所示,算法如下:Step S23: Use the split-merge algorithm to cluster and model the lidar data; the fitting method uses the least square method, as shown in Figure 3, the algorithm is as follows:

其中,采用IEPF(Iterative End Point Fit)方法进行线段类特征提取,Among them, the IEPF (Iterative End Point Fit) method is used for line segment feature extraction,

(1)取出激光雷达点簇数据,放入ListA中;(1) Take out the lidar point cluster data and put it into ListA;

(2)取A中首尾两点拟合成直线l1(2) Take the first and last two points in A and fit them into a straight line l 1 ;

(3)搜索ListA中距离直线l1最远的点,得到距离d;(3) Search for the point farthest from the straight line l +1 in ListA to obtain the distance d;

(4)如果d小于分割阈值dmax,则执行步骤(6);(4) If d is less than the segmentation threshold d max , then perform step (6);

(5)否则,将l1分割为l2和l3,则执行步骤(2);(5) Otherwise, divide l 1 into l 2 and l 3 , then perform step (2);

(6)取各分割段间所有点,用最小二乘法拟合,最小二乘拟合公式如下:(6) Take all the points between the segments, and use the least squares method to fit them. The least squares fitting formula is as follows:

Figure BDA0002060489470000062
Figure BDA0002060489470000062

Figure BDA0002060489470000063
Figure BDA0002060489470000063

式中:In the formula:

Figure BDA0002060489470000064
为数据算术平均值;
Figure BDA0002060489470000064
is the arithmetic mean of the data;

β01为回归系数,

Figure BDA0002060489470000065
为参数β01的最小二乘估计值.β 0 , β 1 are regression coefficients,
Figure BDA0002060489470000065
is the least squares estimate of parameters β 0 , β 1 .

经步骤(6)得到线段集合{l1,l2,l3......ln},确定线段端点,如图4所示,得到最终道路环境特征模型如下:After step (6), the set of line segments {l 1 , l 2 , l 3 ...... l n } is obtained, and the endpoints of the line segments are determined, as shown in Figure 4. The final road environment characteristic model is obtained as follows:

Figure BDA0002060489470000066
Figure BDA0002060489470000066

其中,

Figure BDA0002060489470000067
为线段端点,(xj,yj)为线段交点,in,
Figure BDA0002060489470000067
is the end point of the line segment, (x j ,y j ) is the intersection point of the line segment,

步骤S3:GPS误差最大约为30m,根据GPS误差将地图按一定的分辨率分割成为栅格地图,根据步骤S1获得的GPS定位坐标(longG,latG)确定出关键栅格区域。Step S3: The maximum GPS error is about 30m. According to the GPS error, the map is divided into a grid map with a certain resolution, and the key grid area is determined according to the GPS positioning coordinates (long G , lat G ) obtained in step S1.

步骤S4:采用步骤S2中建立的道路环境特征模型与关键栅格区域中的道路关键点信息进行匹配。Step S4: Use the road environment feature model established in step S2 to match the road key point information in the key grid area.

步骤S41:以道路环境特征模型中关键点{Xj,Yj}为原点,按顺时针方向提取道路环境特征模型中可用特征角度θ和距离d得到{θ123,d14,d2…},如图5所示,公式如下:Step S41: Taking the key point {X j , Y j } in the road environment feature model as the origin, extract the available feature angle θ and distance d in the road environment feature model in a clockwise direction to obtain {θ 1 , θ 2 , θ 3 , d 14 ,d 2 ...}, as shown in Figure 5, the formula is as follows:

Figure BDA0002060489470000071
Figure BDA0002060489470000071

Figure BDA0002060489470000072
Figure BDA0002060489470000072

式中:k为直线斜率;d为原点到直线的距离;θ为从原点作各直线的垂线之间的夹角。In the formula: k is the slope of the straight line; d is the distance from the origin to the straight line; θ is the angle between the perpendicular lines drawn from the origin.

步骤S42:按上述特征提取后得到特征集合{θ123,d14,d2…},与关键栅格区域中记录的道路关键点特征参数依次进行比对,在一定阈值范围内均匹配成功则认为关键点匹配成功,否则重复步骤S2、S3、S4。Step S42: Get the feature set {θ 1 , θ 2 , θ 3 , d 1 , θ 4 , d 2 ...} after extracting the above features, and compare them with the key point feature parameters of the road recorded in the key grid area, If the matching of key points is successful within a certain threshold range, it is considered that the key point matching is successful, otherwise, steps S2, S3, and S4 are repeated.

步骤S5:根据匹配成功的道路关键点经纬度坐标(longk,latk)和关键点对于激光雷达的坐标(xj,yj)计算得到车辆经纬度定位坐标(longL,latL),如图6所示,公式如下:Step S5: Calculate the longitude and latitude positioning coordinates (long L , lat L ) of the vehicle according to the latitude and longitude coordinates (long k , lat k ) of the key points on the road that have been successfully matched and the coordinates (x j , y j ) of the key points for the lidar, as shown in the figure 6, the formula is as follows:

Figure BDA0002060489470000073
Figure BDA0002060489470000073

式中:d为关键点与激光雷达的欧氏距离;α位车辆行驶方位角,以正北方向为0°;ARC为地球平均半径,约为6371.393千米;(longk,latk)为关键点经纬度坐标;(longL,latL)为求取的车辆经纬度坐标。In the formula: d is the Euclidean distance between the key point and the lidar; the azimuth angle of the vehicle at α position is 0° in the direction of true north; ARC is the average radius of the earth, which is about 6371.393 kilometers; (long k ,lat k ) is The latitude and longitude coordinates of key points; (long L , lat L ) is the obtained vehicle latitude and longitude coordinates.

步骤S6:经上述步骤得到由GPS获得的经纬度坐标(longG,latG),由激光雷达获得的经纬度坐标(longL,latL),结合由车辆直接得到的速度v和航向角

Figure BDA0002060489470000074
采用扩展卡尔曼滤波器(EKF)对以上数据进行融合,获得最终的定位结果。如图7所示。EKF的状态方程和观测方程如下:Step S6: Obtain the latitude and longitude coordinates (long G , lat G ) obtained by GPS through the above steps, and the latitude and longitude coordinates (long L , lat L ) obtained by laser radar, combined with the velocity v and heading angle directly obtained by the vehicle
Figure BDA0002060489470000074
The extended Kalman filter (EKF) is used to fuse the above data to obtain the final positioning result. As shown in Figure 7. The state equation and observation equation of EKF are as follows:

Figure BDA0002060489470000081
Figure BDA0002060489470000081

式中:f()是非线性系统的状态转移函数;H()是非线性系统的观测函数;w(k)和v(k)为系统噪声;U(k)是输入值,X(k)是状态变量,Z(k)是观测值。In the formula: f() is the state transition function of the nonlinear system; H() is the observation function of the nonlinear system; w(k) and v(k) are the system noise; U(k) is the input value, X(k) is The state variable, Z(k) is the observed value.

本发明的组合定位系统模型如下:The combined positioning system model of the present invention is as follows:

Figure BDA0002060489470000082
Figure BDA0002060489470000082

式中:v为车辆速度;t为采样间隔时间;θ为车辆航向方位角,以正北为0°;ARC为地球平均半径;w(k)为系统噪声;xk,yk分别为所求定位经度、纬度。In the formula: v is the vehicle speed; t is the sampling interval time; θ is the heading and azimuth angle of the vehicle, taking true north as 0°; ARC is the average radius of the earth; w( k ) is the system noise; Find the location longitude and latitude.

基于扩展卡尔曼滤波器数据融合的具体计算公式如下:The specific calculation formula based on the extended Kalman filter data fusion is as follows:

Figure BDA0002060489470000083
Figure BDA0002060489470000083

Figure BDA0002060489470000084
Figure BDA0002060489470000084

Figure BDA0002060489470000085
Figure BDA0002060489470000085

Figure BDA0002060489470000086
Figure BDA0002060489470000086

Figure BDA0002060489470000087
Figure BDA0002060489470000087

X(k)=X(k|k-1)+K(k)×(Z(k)-H(X(k|k-1)))X(k)=X(k|k-1)+K(k)×(Z( k )-H(X(k|k -1 )))

P(k|k)=P(k|k-1)-K(k)P(k|k-1)P(k|k)=P(k|k-1)-K(k)P(k|k-1)

式中:Aj为函数f的Jacobian矩阵,Hj为函数H的Jacobian矩阵;f为非线性系统的状态转移函数,H是非线性系统的观测函数;v(k)为系统噪声;P为状态变量协方差矩阵;Q为过程噪声协方差矩阵;R为观测噪声协方差矩阵;K()为卡尔曼增益;v为车辆速度;t为采样间隔时间;θ为车辆航向方位角,以正北为0°;ARC为地球平均半径。In the formula: A j is the Jacobian matrix of the function f, H j is the Jacobian matrix of the function H; f is the state transition function of the nonlinear system, H is the observation function of the nonlinear system; v(k) is the system noise; P is the state Variable covariance matrix; Q is process noise covariance matrix; R is observation noise covariance matrix; K() is Kalman gain; v is vehicle speed; t is sampling interval time; is 0°; ARC is the average radius of the earth.

本发明将数据融合过程分为三个阶段,如图8所示。The present invention divides the data fusion process into three stages, as shown in FIG. 8 .

阶段一:检测到关键点之前,该阶段由于激光雷达还未检测到道路特征关键点,无法与地图进行匹配,因此经纬度坐标数据来源只有来自于GPS数据。该阶段的观测值为GPS获取的经纬度坐标(longG,latG),EKF的具体参数如下表:Phase 1: Before the key points are detected, the lidar has not detected the key points of the road features and cannot be matched with the map at this stage, so the source of the latitude and longitude coordinate data only comes from GPS data. The observations at this stage are the latitude and longitude coordinates (long G , lat G ) obtained by GPS, and the specific parameters of EKF are as follows:

Figure BDA0002060489470000091
Figure BDA0002060489470000091

阶段二:检测到关键点,该阶段激光雷达能够检测到道路特征关键点,通过本文基于GPS和激光雷达的精确定位方法可以获取到相对精确的经纬度坐标(longL,latL),将该数据作为观测值,EKF的具体参数如下表:Stage 2: Key points are detected. At this stage, lidar can detect key points of road features. Through the precise positioning method based on GPS and lidar in this paper, relatively accurate latitude and longitude coordinates (long L , lat L ) can be obtained. As the observed value, the specific parameters of EKF are as follows:

Figure BDA0002060489470000092
Figure BDA0002060489470000092

阶段三:离开关键点,该阶段虽然失去了道路关键点,但上一时刻的最优估计坐标对后续一段时间的数据具有一定的修正作用,将GPS数据(longG,latG)作为观测值,EKF具体参数如下表:Stage 3: Leaving the key point. Although the key point of the road is lost in this stage, the optimal estimated coordinates at the previous moment have a certain correction effect on the data of the subsequent period of time. The GPS data (long G , lat G ) is used as the observation value , EKF specific parameters are as follows:

Figure BDA0002060489470000093
Figure BDA0002060489470000093

本发明一种基于GPS与激光雷达数据融合的定位方法,采用GPS进行全局定位确定大致区域,再利用激光雷达在局部进行精确定位,最后将二者数据融合获得定位结果,能够有效地提高GPS定位系统的定位精度,而且GPS与激光雷达均为自动驾驶汽车必备传感器,不需额外添加设备。因此,本发明一种基于GPS与激光雷达数据融合的定位方法具有较高的实用性。The present invention is a positioning method based on the fusion of GPS and laser radar data. GPS is used for global positioning to determine a rough area, and then laser radar is used for local precise positioning. Finally, the two data are fused to obtain a positioning result, which can effectively improve GPS positioning. The positioning accuracy of the system, and GPS and lidar are essential sensors for self-driving cars, no additional equipment is required. Therefore, a positioning method based on fusion of GPS and laser radar data in the present invention has high practicability.

本实施例还提供一种基于GPS与激光雷达数据融合的定位装置,该装置包括:This embodiment also provides a positioning device based on fusion of GPS and laser radar data, the device comprising:

坐标获取模块,用于获取GPS定位坐标;A coordinate acquisition module, configured to acquire GPS positioning coordinates;

模型建立模块,用于获取道路环境信息,建立道路环境特征模型;The model building module is used to obtain road environment information and establish a road environment characteristic model;

特征提取模块,用于根据GPS误差将地图按一定的分辨率分割成为栅格地图,根据所GPS定位坐标确定出关键栅格区域;The feature extraction module is used to divide the map into a grid map according to a certain resolution according to the GPS error, and determine the key grid area according to the GPS positioning coordinates;

特征匹配模块,用于将所述道路环境特征模型与所述关键栅格区域中的道路关键点信息进行匹配;A feature matching module, configured to match the road environment feature model with road key point information in the key grid area;

坐标计算模块,用于根据匹配成功的道路关键点的经纬度坐标和该道路关键点相对于激光雷达的坐标计算得到车辆经纬度坐标;The coordinate calculation module is used to calculate the longitude and latitude coordinates of the vehicle according to the latitude and longitude coordinates of the successfully matched road key points and the coordinates of the road key points relative to the laser radar;

数据融合模块,用于采用扩展卡尔曼滤波器对所述GPS定位坐标、所述车辆经纬度坐标、车辆速度v和航向角

Figure BDA0002060489470000101
进行数据融合,得到最终的定位结果。The data fusion module is used to adopt the extended Kalman filter to the GPS positioning coordinates, the vehicle latitude and longitude coordinates, vehicle speed v and heading angle
Figure BDA0002060489470000101
Perform data fusion to obtain the final positioning result.

需要说明的是,由于装置部分的实施例与方法部分的实施例相互对应,因此装置部分的实施例的内容请参见方法部分的实施例的描述,这里暂不赘述。It should be noted that, since the embodiment of the device part corresponds to the embodiment of the method part, please refer to the description of the embodiment of the method part for the content of the embodiment of the device part, and details will not be repeated here.

本发明还提供一种存储介质,存储计算机程序,所述计算机程序被处理器运行时执行前述的定位方法。The present invention also provides a storage medium storing a computer program, and the computer program executes the aforementioned positioning method when executed by a processor.

本发明还提供一种电子终端,包括:The present invention also provides an electronic terminal, including:

存储器,用于存储计算机程序;memory for storing computer programs;

处理器,用于执行所述存储器存储的计算机程序,以使所述设备执行前述的定位方法。A processor, configured to execute the computer program stored in the memory, so that the device executes the aforementioned positioning method.

所述处理器可以是中央处理单元(Central Processing Unit,CPU),还可以是其他通用处理器、数字信号处理器(Digital Signal Processor,DSP)、专用集成电路(Application Specific Integrated Circuit,ASIC)、现场可编程门阵列(FieldProgrammable Gate Array,FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。The processor can be a central processing unit (Central Processing Unit, CPU), and can also be other general-purpose processors, digital signal processors (Digital Signal Processor, DSP), application specific integrated circuits (Application Specific Integrated Circuit, ASIC), on-site Programmable gate array (Field Programmable Gate Array, FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor, or the processor may be any conventional processor, or the like.

所述存储器可以是内部存储单元或外部存储设备,例如插接式硬盘,智能存储卡(Smart Media Card,SMC),安全数字卡(Secure Digital,SD),闪存卡(Flash Card)等。进一步地,所述存储器还可以既包括内部存储单元,也包括外部存储设备。所述存储器用于存储所述计算机程序以及其他程序和数据。所述存储器还可以用于暂时地存储己经输出或者将要输出的数据。The memory may be an internal storage unit or an external storage device, such as a plug-in hard disk, a smart memory card (Smart Media Card, SMC), a secure digital card (Secure Digital, SD), a flash memory card (Flash Card) and the like. Further, the memory may also include both an internal storage unit and an external storage device. The memory is used to store the computer program as well as other programs and data. The memory can also be used to temporarily store data that has been output or will be output.

所属领域的技术人员可以清楚地了解到,为了描述的方便和简洁,仅以上述各功能单元、模块的划分进行举例说明,实际应用中,可以根据需要而将上述功能分配由不同的功能单元、模块完成,即将所述装置的内部结构划分成不同的功能单元或模块,以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个单元中,上述集成的单元既可以采用硬件的形式实现,也可以采用软件功能单元的形式实现。另外,各功能单元、模块的具体名称也只是为了便于相互区分,并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程,可以参考前述方法实施例中的对应过程,在此不再赘述。Those skilled in the art can clearly understand that for the convenience and brevity of description, only the division of the above-mentioned functional units and modules is used for illustration. In practical applications, the above-mentioned functions can be assigned to different functional units, Completion of modules means that the internal structure of the device is divided into different functional units or modules to complete all or part of the functions described above. Each functional unit and module in the embodiment may be integrated into one processing unit, or each unit may exist separately physically, or two or more units may be integrated into one unit, and the above-mentioned integrated units may adopt hardware It can also be implemented in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing each other, and are not used to limit the protection scope of the present application. For the specific working process of the units and modules in the above system, reference may be made to the corresponding process in the foregoing method embodiments, and details will not be repeated here.

在上述实施例中,对各个实施例的描述都各有侧重,某个实施例中没有详述或记载的部分,可以参见其它实施例的相关描述。In the above-mentioned embodiments, the descriptions of each embodiment have their own emphases, and for parts that are not detailed or recorded in a certain embodiment, refer to the relevant descriptions of other embodiments.

本领域普通技术人员可以意识到,结合本文中所公开的实施例描述的各示例的单元及算法步骤,能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行,取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能,但是这种实现不应认为超出本发明的范围。Those skilled in the art can appreciate that the units and algorithm steps of the examples described in conjunction with the embodiments disclosed herein can be implemented by electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are executed by hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art may use different methods to implement the described functions for each specific application, but such implementation should not be regarded as exceeding the scope of the present invention.

在本发明所提供的实施例中,应该理解到,所揭露的装置/终端设备和方法,可以通过其它的方式实现。例如,以上所描述的装置/终端设备实施例仅仅是示意性的,例如,所述模块或单元的划分,仅仅为一种逻辑功能划分,实际实现时可以有另外的划分方式,例如多个单元或组件可以结合或者可以集成到另一个系统,或一些特征可以忽略,或不执行。另一点,所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口,装置或单元的间接耦合或通讯连接,可以是电性,机械或其它的形式。In the embodiments provided in the present invention, it should be understood that the disclosed apparatus/terminal equipment and method may be implemented in other ways. For example, the device/terminal device embodiments described above are only illustrative. For example, the division of the modules or units is only a logical function division. In actual implementation, there may be other division methods, such as multiple units Or components may be combined or may be integrated into another system, or some features may be omitted, or not implemented. In another point, the mutual coupling or direct coupling or communication connection shown or discussed may be through some interfaces, and the indirect coupling or communication connection of devices or units may be in electrical, mechanical or other forms.

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place, or may be distributed to multiple network units. Part or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

另外,在本发明各个实施例中的各功能单元可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现,也可以采用软件功能单元的形式实现。In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, each unit may exist separately physically, or two or more units may be integrated into one unit. The above-mentioned integrated units can be implemented in the form of hardware or in the form of software functional units.

所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读取存储介质中。基于这样的理解,本发明实现上述实施例方法中的全部或部分流程,也可以通过计算机程序来指令相关的硬件来完成,所述的计算机程序可存储于一计算机可读存储介质中,该计算机程序在被处理器执行时,可实现上述各个方法实施例的步骤。其中,所述计算机程序包括计算机程序代码,所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括:能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM,Read-Only Memory)、随机存取存储器((RAM,Random Access Memory)、电载波信号、电信信号以及软件分发介质等。If the integrated module/unit is realized in the form of a software function unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the present invention realizes all or part of the processes in the methods of the above embodiments, and can also be completed by instructing related hardware through a computer program. The computer program can be stored in a computer-readable storage medium, and the computer When the program is executed by the processor, the steps in the above-mentioned various method embodiments can be realized. Wherein, the computer program includes computer program code, and the computer program code may be in the form of source code, object code, executable file or some intermediate form. The computer-readable medium may include: any entity or device capable of carrying the computer program code, a recording medium, a USB flash drive, a removable hard disk, a magnetic disk, an optical disk, a computer memory, and a read-only memory (ROM, Read-Only Memory) , Random Access Memory (RAM, Random Access Memory), electrical carrier signal, telecommunication signal and software distribution medium, etc.

上述实施例仅例示性说明本发明的原理及其功效,而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下,对上述实施例进行修饰或改变。因此,举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变,仍应由本发明的权利要求所涵盖。The above-mentioned embodiments only illustrate the principles and effects of the present invention, but are not intended to limit the present invention. Anyone skilled in the art can modify or change the above-mentioned embodiments without departing from the spirit and scope of the present invention. Therefore, all equivalent modifications or changes made by those skilled in the art without departing from the spirit and technical ideas disclosed in the present invention should still be covered by the claims of the present invention.

Claims (8)

1. A positioning method based on data fusion of a GPS and a laser radar is characterized by comprising the following steps:
acquiring a GPS positioning coordinate;
detecting by adopting a 2D laser radar, acquiring road environment information, and establishing a road environment characteristic model;
dividing the map into grid maps according to a certain resolution ratio according to the GPS error, and determining a key grid area according to the GPS positioning coordinate;
matching the road environment characteristic model with road key point characteristic parameters in the key grid area;
calculating to obtain longitude and latitude coordinates of the vehicle according to the longitude and latitude coordinates of the successfully matched road key points and the coordinates of the road key points relative to the laser radar;
adopting an extended Kalman filter to carry out positioning on the GPS positioning coordinate, the longitude and latitude coordinate of the vehicle, the vehicle speed v and the course angle
Figure FDA0003995869050000011
And performing data fusion to obtain a final positioning result, wherein the state equation and the observation equation of the extended Kalman filter are as follows:
Figure FDA0003995869050000012
in the formula: f () is the state transfer function of the nonlinear system, H () is the observation function of the nonlinear system, and w (k) and v (k) are the system noise; u (k) is an input value, X (k) is a state variable, and Z (k) is an observed value;
the positioning system model is as follows:
Figure FDA0003995869050000013
in the formula: t is the sampling interval time, x k ,y k Respectively, the longitude and latitude of the determined location.
2. The positioning method based on GPS and lidar data fusion according to claim 1,
adopt 2D laser radar to detect, acquire road environment information, establish road environment characteristic model, specifically include:
acquiring 2D laser radar data, wherein the position of a laser radar device is taken as a coordinate origin, and environmental data is taken as point cluster data;
preprocessing the point cluster data;
and clustering the preprocessed point cluster data by adopting a split-merge algorithm, and modeling.
3. The positioning method based on GPS and lidar data fusion according to claim 2,
the preprocessing the point cluster data specifically includes:
carrying out median filtering on the point cluster data to obtain filtered point cluster data;
performing error compensation on the laser radar data;
and converting the laser radar data in a polar coordinate form into rectangular coordinates by taking the center of the vehicle as an origin, the horizontal axis as the horizontal direction of the vehicle and the vertical axis as the straight direction of the vehicle.
4. The positioning method based on GPS and lidar data fusion according to claim 1,
matching the road environment feature model with the road key point feature parameters in the key grid area, specifically comprising:
by key points { X in the road environment model j ,Y j Taking the angle theta as the original point, and extracting the characteristic angle theta and the distance d in the road environment characteristic model in the clockwise direction to obtain a characteristic set { theta 123 ,d 14 ,d 2 …};
Set the features { theta } 123 ,d 14 ,d 2 … is compared with the road key point information in the key grid area in sequence, and if matching is successful within a certain threshold value range, the matching of the road key points is considered to be successful.
5. The positioning method based on GPS and lidar data fusion according to claim 1,
the vehicle longitude and latitude coordinates (Long) L ,lat L ) Expressed as:
Figure FDA0003995869050000021
the method comprises the following steps that (1) longk represents longitude coordinates of key points of a road, latk represents latitude coordinates of the key points of the road, and d is the Euclidean distance between the key points and a laser radar; and the alpha vehicle driving azimuth angle and ARC is the average radius of the earth.
6. A positioning device based on GPS and laser radar data fusion is characterized in that the device comprises:
the coordinate acquisition module is used for acquiring a GPS positioning coordinate;
the model establishing module is used for detecting by adopting a 2D laser radar, acquiring road environment information and establishing a road environment characteristic model;
the characteristic extraction module is used for dividing the map into a grid map according to a certain resolution ratio according to the GPS error and determining a key grid area according to the GPS positioning coordinate;
the characteristic matching module is used for matching the road environment characteristic model with the road key point characteristic parameters in the key grid area;
the coordinate calculation module is used for calculating the longitude and latitude coordinates of the vehicle according to the longitude and latitude coordinates of the successfully matched road key points and the coordinates of the road key points relative to the laser radar;
a data fusion module for adopting an extended Kalman filter to carry out positioning on the GPS positioning coordinate, the longitude and latitude coordinate of the vehicle, the vehicle speed v and the course angle
Figure FDA0003995869050000022
And performing data fusion to obtain a final positioning result, wherein the state equation and the observation equation of the extended Kalman filter are as follows:
Figure FDA0003995869050000031
in the formula: f () is a state transfer function of the nonlinear system, H () is an observation function of the nonlinear system, and w (k) and v (k) are system noise; u (k) is an input value, X (k) is a state variable, and Z (k) is an observed value;
the positioning system model is as follows:
Figure FDA0003995869050000032
in the formula: t is the sampling interval time, x k ,y k Respectively, the longitude and latitude of the determined location.
7. A storage medium storing a computer program, characterized in that the computer program, when executed by a processor, performs the positioning method according to any one of claims 1 to 5.
8. An electronic terminal, comprising: a processor and a memory;
the memory is configured to store a computer program, and the processor is configured to execute the computer program stored by the memory to cause the terminal to perform the positioning method according to any one of claims 1 to 5.
CN201910403394.8A 2019-05-15 2019-05-15 Positioning method and device based on data fusion of GPS and laser radar Active CN110146910B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910403394.8A CN110146910B (en) 2019-05-15 2019-05-15 Positioning method and device based on data fusion of GPS and laser radar

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910403394.8A CN110146910B (en) 2019-05-15 2019-05-15 Positioning method and device based on data fusion of GPS and laser radar

Publications (2)

Publication Number Publication Date
CN110146910A CN110146910A (en) 2019-08-20
CN110146910B true CN110146910B (en) 2023-02-28

Family

ID=67595467

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910403394.8A Active CN110146910B (en) 2019-05-15 2019-05-15 Positioning method and device based on data fusion of GPS and laser radar

Country Status (1)

Country Link
CN (1) CN110146910B (en)

Families Citing this family (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110780325B (en) * 2019-08-23 2022-07-19 腾讯科技(深圳)有限公司 Method and device for positioning moving object and electronic equipment
CN110515382A (en) * 2019-08-28 2019-11-29 锐捷网络股份有限公司 A kind of smart machine and its localization method
CN112525184A (en) * 2019-08-28 2021-03-19 深圳拓邦股份有限公司 Method and system for acquiring initial data of scrubber and scrubber
CN110749328B (en) * 2019-08-30 2021-12-28 华为技术有限公司 Positioning method and system, electronic device, vehicle and storage medium
CN110796707B (en) * 2019-09-10 2022-05-06 腾讯科技(深圳)有限公司 Calibration parameter calculation method, calibration parameter calculation device and storage medium
CN113009414B (en) * 2019-12-20 2024-03-19 中移(成都)信息通信科技有限公司 Signal source position determining method and device, electronic equipment and computer storage medium
CN111257903B (en) * 2020-01-09 2022-08-09 广州微牌智能科技有限公司 Vehicle positioning method and device, computer equipment and storage medium
CN111241224B (en) * 2020-01-10 2021-05-11 福瑞泰克智能系统有限公司 Method, system, computer device and storage medium for target distance estimation
CN111724599B (en) * 2020-06-30 2021-07-16 暨南大学 Method, device, equipment and medium for obtaining safety driving behavior evaluation data
CN111947666B (en) * 2020-08-21 2022-10-28 广州高新兴机器人有限公司 Automatic retrieving method for loss of outdoor laser navigation position
CN112731320B (en) * 2020-12-29 2024-06-21 福瑞泰克智能系统有限公司 Estimation method, device, equipment and storage medium of vehicle-mounted radar error data
CN114814780A (en) * 2021-01-22 2022-07-29 长沙智能驾驶研究院有限公司 Cargo platform identification method and device, electronic equipment and computer storage medium
CN112987061B (en) * 2021-02-08 2023-12-29 福州大学 Fuzzy fusion positioning method based on GPS and laser radar
CN113295179B (en) * 2021-06-04 2022-07-05 清智汽车科技(苏州)有限公司 Vehicle course angle correction method and device based on laser sensor
CN114594467B (en) * 2022-02-28 2025-05-09 浙江大华技术股份有限公司 A method, device, electronic device and storage medium for determining heading angle
CN114786199B (en) * 2022-04-21 2024-06-14 中国联合网络通信集团有限公司 Method, device, equipment and storage medium for determining network problem point

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2005107693A (en) * 2003-09-29 2005-04-21 Denso Corp Device and program for detecting travel position of one's own vehicle
CN102819951A (en) * 2011-06-09 2012-12-12 无锡物联网产业研究院 Road target detection system, road target detection method and road target detection device
CN104520732A (en) * 2012-02-10 2015-04-15 Isis创新有限公司 Method of locating sensor and related apparatus
CN105137412A (en) * 2015-08-19 2015-12-09 重庆大学 Accurate fitting method of line segment features in 2D laser radar distance image
CN107091648A (en) * 2017-05-11 2017-08-25 江苏保千里视像科技集团股份有限公司 A kind of data fusion method and system of laser radar and differential GPS
CN208149311U (en) * 2018-04-03 2018-11-27 北京智行者科技有限公司 A kind of context aware systems for automatic Pilot passenger car

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8473187B2 (en) * 2009-06-01 2013-06-25 Robert Bosch Gmbh Method and apparatus for combining three-dimensional position and two-dimensional intensity mapping for localization
EP3006953B1 (en) * 2013-05-29 2019-11-13 Mitsubishi Electric Corporation Laser radar device and method for generating laser image

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2005107693A (en) * 2003-09-29 2005-04-21 Denso Corp Device and program for detecting travel position of one's own vehicle
CN102819951A (en) * 2011-06-09 2012-12-12 无锡物联网产业研究院 Road target detection system, road target detection method and road target detection device
CN104520732A (en) * 2012-02-10 2015-04-15 Isis创新有限公司 Method of locating sensor and related apparatus
CN105137412A (en) * 2015-08-19 2015-12-09 重庆大学 Accurate fitting method of line segment features in 2D laser radar distance image
CN107091648A (en) * 2017-05-11 2017-08-25 江苏保千里视像科技集团股份有限公司 A kind of data fusion method and system of laser radar and differential GPS
CN208149311U (en) * 2018-04-03 2018-11-27 北京智行者科技有限公司 A kind of context aware systems for automatic Pilot passenger car

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
车载LiDAR扫描系统安置误差角检校;田祥瑞 等;《红外与激光工程》;20141031;第43卷(第10期);全文 *
道路交通流状态的多参数融合预测方法;马庆禄 等;《物理学报》;20121231;第61卷(第16期);全文 *

Also Published As

Publication number Publication date
CN110146910A (en) 2019-08-20

Similar Documents

Publication Publication Date Title
CN110146910B (en) Positioning method and device based on data fusion of GPS and laser radar
CN110160542B (en) Method and device for positioning lane line, storage medium and electronic device
CN114184200B (en) A Multi-source Fusion Navigation Method Combined with Dynamic Mapping
WO2021073656A1 (en) Method for automatically labeling image data and device
CN111272180B (en) Method and apparatus for estimating a location on a map
CN109791052B (en) Method and system for classifying data points of a point cloud using a digital map
JP6595182B2 (en) Systems and methods for mapping, locating, and attitude correction
JP6197393B2 (en) Lane map generation device and program
Brenner Extraction of features from mobile laser scanning data for future driver assistance systems
WO2020224305A1 (en) Method and apparatus for device positioning, and device
US11846520B2 (en) Method and device for determining a vehicle position
CN114111775B (en) Multi-sensor fusion positioning method and device, storage medium and electronic equipment
CN111220143B (en) Method and device for determining position and posture of imaging equipment
CN108303103A (en) The determination method and apparatus in target track
CN107728175A (en) The automatic driving vehicle navigation and positioning accuracy antidote merged based on GNSS and VO
JP2016157197A (en) Self-position estimation device, self-position estimation method, and program
CN112455502B (en) Train positioning method and device based on laser radar
CN112136021A (en) System and method for constructing landmark-based high-definition map
US20180211111A1 (en) Unsupervised online learning of overhanging structure detector for map generation
CN113838129A (en) Method, device and system for obtaining pose information
CN115564865A (en) Construction method and system of crowdsourcing high-precision map, electronic equipment and vehicle
CN113379915A (en) Driving scene construction method based on point cloud fusion
CN115409910A (en) A semantic map construction method, visual positioning method and related equipment
Jiang et al. Precise vehicle ego-localization using feature matching of pavement images
CN113227713A (en) Method and system for generating environment model for positioning

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