A method and storage medium for integrating multi-focal-length visual SLAM with GPS

Abstract

The present invention claims protection for a method for fusing multi-focal-length visual SLAM with GPS, and belongs to the field of autonomous driving and intelligent automobile environment map construction and positioning. The method comprises the following steps: 1. Installing a multi-focal-length camera and a GPS module, with the short-focal-length and long-focal-length cameras placed horizontally. 2. Calibration between the multi-focal-length cameras, and simultaneously acquiring multi-focal-length images and GPS data. 3. According to the difference in focal length of the multi-focal-length cameras, image feature points are extracted based on the image pyramid and feature matching is performed. 4. The coordinates of the collected GPS positioning data are converted to the multi-focal-length visual SLAM world coordinate system. 5. The converted GPS data and the multi-focal-length visual SLAM data are timestamp aligned. 6. The GPS data and SLAM image data after timestamp alignment are used to construct a graph optimization model to optimize the posture. 7. The running trajectory generated by SLAM matches the corresponding map road trajectory on the map.

Description

Technical Field

[0001] The present invention belongs to the field of autonomous driving and intelligent automobile environment map construction and positioning, and specifically relates to a multi-focal-length visual SLAM and GPS fusion and implementation method. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) has been a research hotspot in computer vision and robotics for nearly two decades. It can construct maps of unknown environments and locate its own position in real time. Visual SLAM has become a hot area of ​​SLAM research due to its high resolution, rich environmental image information, portability, low hardware cost, high positioning accuracy, and the lack of prior environmental information. Existing binocular visual SLAM generally uses two identical short-focal-length cameras to achieve good visual map construction and positioning accuracy. This provides a wide field of view. However, in the field of automotive driving, cameras are more needed to perceive the surrounding environment. Using only short-focal-length cameras cannot obtain long-range scene information, nor can they detect and locate objects in long-range scenes. Therefore, a long-focal-length camera is needed to perceive the long-range environment. Multi-focal-length camera images can simultaneously capture a wide field of view and long-range information, and are more stable in cornering scenarios.

[0003] The Global Positioning System (GPS) is an indispensable technology for vehicle positioning today and plays a crucial role in intelligent driving positioning. The GPS system consists of 32 GPS satellites in space, one master control station, three data injection stations, five monitoring stations on the ground, and GPS receivers serving as user terminals. With at least three of these satellites, a user's location and altitude can be quickly determined. Currently, civilian GPS systems can achieve positioning accuracy of approximately ten meters. The GPS system uses low-frequency signals, which maintains high signal penetration even in adverse weather conditions. GPS positioning utilizes basic satellite triangulation principles, with GPS receivers measuring distance by measuring the transmission time of radio signals. Satellite distance measurements are subject to errors caused by satellite clock and transmission delays. Differential techniques are typically used to eliminate or reduce these errors, enabling GPS to achieve higher accuracy. However, autonomous vehicles operate in complex and dynamic environments, particularly in large cities, where GPS multipath reflections can be significant, resulting in GPS positioning information that can easily be inaccurate by several meters.

[0004] After searching, the closest existing technology is 201910849671.8, a method and device for simultaneous positioning and mapping. The method includes: extracting, matching and triangulating feature points of a binocular image sequence to obtain a three-dimensional landmark point cloud for each frame of binocular image; obtaining an affinity matrix based on the three-dimensional landmark point cloud of each frame of binocular image; performing hierarchical clustering based on the affinity matrix to determine the rigid body to which each landmark belongs; performing simultaneous positioning and mapping based on the three-dimensional landmark point cloud of each frame of binocular image and the landmarks included in each rigid body; wherein affinity is the logarithmic probability that two landmarks belong to the same rigid body. The method and device for simultaneous positioning and mapping provided by the above-mentioned embodiments of the invention perform hierarchical clustering based on affinity, can more accurately detect moving objects in the scene and model the motion and estimate the pose, and has a smaller amount of computation, thereby improving the accuracy and speed of simultaneous positioning and mapping.

[0005] The present invention uses a multi-focal length camera to acquire images, taking into account both a wide field of view and longer-distance information. The provided SLAM has stronger stability and accurately locates the road trajectory on the map. Summary of the Invention

[0006] The present invention aims to solve the above problems of the prior art. A method for fusing multi-focal visual SLAM with GPS is proposed. The technical solution of the present invention is as follows:

[0007] A method for fusing multi-focal-length visual SLAM with GPS comprises the following steps:

[0008] Step 1. Install the multi-focal length camera and GPS module. The multi-focal length visual SLAM consists of a short-focal length camera and a long-focal length camera, which are placed horizontally. Step 2. Calibrate the multi-focal length cameras, collect multi-focal length images and GPS data at the same time, and obtain multi-focal length image data and GPS data. Step 3. According to the difference in focal length of the multi-focal length cameras, extract image feature points based on the image pyramid and perform feature matching. Step 4. Convert the collected GPS positioning data into the multi-focal length visual SLAM world coordinate system. Step 5. Align the timestamps of the converted GPS data and the multi-focal length visual SLAM data. Step 6. Construct a graph optimization model to optimize the pose of the GPS data and SLAM image data after timestamp alignment. Step 7. Match the running trajectory generated by SLAM to the corresponding map road trajectory on the map.

[0009] Furthermore, the step 2 of calibrating the multi-focal cameras and simultaneously collecting the multi-focal images and GPS data to obtain the multi-focal image data and GPS data specifically includes:

[0010] Step 21, completing the multi-focal length camera calibration according to the installation positions of the short-focal length camera and the long-focal length camera;

[0011] Step 22: Synchronize the information of the short-focus camera and the long-focus camera. In terms of time, the short-focus camera and the long-focus camera are required to shoot synchronously. In terms of space, the short-focus camera and the long-focus camera are both installed in front of the roof, with the short-focus camera located on the left side of the front of the roof and the long-focus camera located on the right side of the front of the roof.

[0012] Step 23: The camera's image acquisition frequency remains constant, and the camera is set to automatically expose during the acquisition process; multi-focal length image data and GPS data are acquired.

[0013] Furthermore, step 3 extracts image feature points based on the image pyramid according to the difference in focal length of the multi-focal length camera, and performs feature matching, specifically including:

[0014] Step 31: construct an image pyramid for the image, and determine a scaling factor s for downsampling the image pyramid based on the difference in focal length between the short-focus camera and the long-focus camera.

[0015] Step 32, based on the difference of multi-focal length images, only the layers with the same image information size in the short focal length and long focal length image pyramids are selected to extract ORB feature points, such as the top layer of the short focal length image pyramid. dis The layer does not extract feature points, the long focal length camera image pyramid bottom l dis The layer does not extract feature points;

[0016] Step 33, obtaining the number of ORB feature points required to be extracted for the corresponding layer from which feature points need to be extracted;

[0017] Step 34, using a normalized cross-correlation algorithm to determine the region in the short-focus camera image that is identical to the region in the long-focus camera image, referred to as the ROI region;

[0018] Step 35: using a quadtree algorithm to increase the number of ORB feature points extracted within the ROI area of ​​a certain layer of the short focal length camera image pyramid, and reduce the number of ORB feature points extracted outside the ROI area;

[0019] Step 36: perform feature matching on the extracted feature points and filter out incorrect matches.

[0020] Furthermore, in step 33, the number of ORB feature points required to be extracted for the corresponding layer from which feature points need to be extracted is obtained as follows:

[0021]

[0022] Among them, s represents the image pyramid scaling ratio, l disIndicates the fixed level difference of the same size image features in the short focal length image pyramid and the long focal length image pyramid, n represents the number of image pyramid scaling layers, N represents the total number of ORB feature points that need to be extracted, α represents the layer from which ORB feature points need to be extracted, and N α Indicates the number of ORB feature points extracted by layer α in the image pyramid.

[0023] Furthermore, the normalized cross-correlation algorithm of step 34 is specifically expressed as:

[0024]

[0025] The area corresponding to the maximum value of ρ(x,y) is selected as the part of the ROI short-focus camera image. Where S is the short-focus camera image, m and n represent the length and width of the long-focus camera image, i and j represent the image pixel coordinate points, X and Y represent the area blocks of the same size in the short-focus camera image and the long-focus camera image, and g is the long-focus camera image. is the average grayscale of the short focal length camera image, Average grayscale of long focal length camera image.

[0026] Furthermore, step 4 converts the collected GPS positioning data into a multi-focal-length visual SLAM world coordinate system, specifically including:

[0027] Step 41, based on the multi-focal visual SLAM world coordinate system established with the first SLAM frame, which does not change during the entire multi-focal visual SLAM process, calculate the coordinates of the translation vector from the first multi-focal visual SLAM frame to the initial frame in the SLAM world coordinate system and the GPS coordinate system; the translation vector is described in the SLAM world coordinate system:

[0028] t w =R cw *(pos c,init -pos c,first )

[0029] t w Represents the world coordinate system, R cw Indicates the conversion from the camera coordinate system to the world coordinate system, pos c,init Indicates the camera pose at initialization, pos c,first Indicates the camera's first frame pose.

[0030] The GPS coordinate system is described as:

[0031] t GPS =pos GPS,init -pos GPS,first

[0032] tGPS Indicates the GPS coordinate system, pos GPS,init Indicates the GPS initialization posture, pos GPS,first Indicates the GPS first frame pose.

[0033] The rotation matrix between the GPS coordinate system and the multi-focal length binocular SLAM world coordinate system is obtained according to the coordinates of the two coordinate systems:

[0034] R GPSw *t GPS =t w

[0035] R GPSw Represents the rotation matrix from the GPS coordinate system to the camera's world coordinate system.

[0036] Furthermore, step 6 constructs a graph optimization model to optimize the pose of the GPS data and SLAM image data after the timestamps are aligned, specifically including:

[0037] Step 61, at time k, at position x k At this point, the sensor makes an observation and obtains data z k , the observation equation is:

[0038] z k =h(x k )

[0039] Since there are always errors in observations, define the error:

[0040] e k =z k -h(x k )

[0041] Step 62: Build a global pose graph. The poses to be solved are used as vertices of the optimization graph, and the observations are used as edges. The objective function is constructed. The observations include the camera's observations of the landmarks and GPS data:

[0042]

[0043] where e k Represents the error, including image reprojection error and GPS positioning data error; Ω k Represents the information matrix, which is the inverse of the covariance matrix and is a symmetric matrix.

[0044] In step 63, a graph optimizer is used to deploy the vertices and edges of the global pose graph and solve the optimal pose.

[0045] Furthermore, the step 7 of matching the running trajectory generated by SLAM to the corresponding map road trajectory on the map specifically includes:

[0046] Step 71: Establish a directed graph of the road network, obtain the node ID, coordinate information, and attribute information of the road, and thus establish a directed graph G(V, E) of the road network, where the elements of V are road endpoints and the elements of E are road segments;

[0047] Step 72: Generate a grid index. Use the grid index to divide the entire map into M×N grids. Pre-calculate the intersecting road segments in each grid using a containment analysis algorithm to obtain candidate road segments for the trajectory points. The grid index is associated with the road segment data via the road segment ID.

[0048] Step 73: Set a buffer zone with a radius of R for the trajectory point, determine the grid where the trajectory point is located based on the grid index, and query the K road segments closest to the trajectory point in a 3×3 grid centered on the grid as candidate road segments;

[0049] Step 74, calculate the emission probability, which is the probability of obtaining a certain observation value under a certain implicit state, taking into account the distance factor;

[0050] Step 75, calculating the transmission probability;

[0051] Step 76: Use the Viterbi algorithm (select the optimal path in each segment and delete other paths, and iterate until the end point) to find the optimal path.

[0052] Furthermore, the emission probability is calculated, that is, the probability of obtaining a certain observation value under a certain implicit state, taking into account the distance factor. The specific formula is:

[0053]

[0054] Among them, σ represents the standard deviation of the error in obtaining trajectory data, p i represents the trajectory point, represents the candidate road segment corresponding to the k-th point, Indicates the distance from the trajectory point to the candidate road segment.

[0055] Step 81: The transfer probability is based on the fact that the road is generally the shortest distance between two points. The smaller the road network distance between the front and rear road segments, the greater the transfer probability:

[0056]

[0057] in, and are the candidate road segments corresponding to the t+1th trajectory point and the tth trajectory point, respectively. a is the set coefficient, and D represents the actual road network distance between the candidate road segments.

[0058] A storage medium stores a computer program, wherein when the computer program is executed by a processor, the method for fusing multi-focal-distance visual SLAM and GPS is implemented.

[0059] The advantages and beneficial effects of the present invention are as follows:

[0060] The multi-focal length camera used in this invention realizes visual SLAM and integrates GPS information to improve the positioning accuracy of SLAM. At the same time, the trajectory formed by SLAM is matched with the map road to obtain accurate positioning on the map. Specifically, it includes the following aspects:

[0061] 1. Multi-focal length visual SLAM. Most previous stereo vision SLAMs use cameras with the same focal length to acquire images and implement feature extraction and matching. The limitation of cameras with the same focal length is that two cameras with the same focal length are only used to calculate the depth of feature points and obtain image depth information. The multi-focal length camera used in the present invention is composed of a short-focus camera and a long-focus camera. The short-focus camera can obtain a wider field of view, while the long-focus camera can obtain more long-distance information. The two cameras are not only used to obtain image depth information, but also to obtain more information that you want to use, such as a wider field of view in the left and right directions of the camera, and more feature information at a long distance. The present invention improves the image feature matching method based on the difference in focal length of the multi-focal length camera, determines the scaling factor s of the image pyramid downsampling based on the difference in focal length between the short-focus camera and the long-focus camera, extracts image feature points based on the image pyramid, performs feature matching on the pyramid layers of different long and short focal length images, thereby completing the long and short focal length image feature matching.

[0062] 2. Multi-focal image fusion with GPS positioning data. By performing coordinate conversion on GPS data and then aligning the timestamps of the converted GPS data with the visual SLAM image pose data, a sensor fusion model and pose graph optimization model are constructed. This approach utilizes visual cameras and GPS positioning data for simultaneous positioning and mapping, improving SLAM positioning and mapping accuracy.

[0063] 3. SLAM trajectory matching with the map: Use the SLAM trajectory generated above to match the road network map to reduce the GPS positioning error and correctly display the current location on the map. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 This is a framework diagram of the preferred embodiment of the present invention providing multi-focal-length visual SLAM and GPS fusion.

[0065] Figure 2 This is a schematic diagram of the installation positions of the multi-focal length camera and GPS receiver module.

[0066] Figure 3This is a schematic diagram of the conversion of GPS coordinates to the SLAM world coordinate system.

[0067] Figure 4 It is a diagram of graph optimization. DETAILED DESCRIPTION

[0068] The following will describe the technical solutions in the embodiments of the present invention in detail with reference to the accompanying drawings. The described embodiments are only a part of the embodiments of the present invention.

[0069] The technical solution of the present invention to solve the above technical problems is:

[0070] Step 1. Install the multi-focal length camera and GPS module. The multi-focal length visual SLAM consists of a short-focal length camera and a long-focal length camera, which are placed horizontally.

[0071] Step 2: Calibrate the multi-focal length cameras and simultaneously collect multi-focal length images and GPS data to obtain multi-focal length image data and GPS data.

[0072] Step 3. According to the difference in focal length of the multi-focal length camera, image feature points are extracted based on the image pyramid and feature matching is performed.

[0073] Step 4. Convert the collected GPS positioning data into the multi-focal-length visual SLAM world coordinate system.

[0074] Step 5. Align the timestamps of the converted GPS data and multi-focal visual SLAM data.

[0075] Step 6. Construct a graph optimization model for the time-aligned GPS data and SLAM image data to optimize the pose.

[0076] Step 7. The running trajectory generated by SLAM is matched to the corresponding map road trajectory on the map.

[0077] As a possible implementation of this embodiment, the step 1 installs a multi-focal length camera and a GPS module. The multi-focal length visual SLAM is composed of a short-focal length camera and a long-focal length camera. The short-focal length and long-focal length cameras are placed horizontally as shown in FIG. Figure 2 As shown, the following steps are included:

[0078] Step 11, install the multi-focal length camera. The multi-focal length camera consists of a short-focal length camera and a long-focal length camera. The short-focal length and long-focal length cameras are placed horizontally in front of the roof.

[0079] Step 12: The GPS positioning data acquisition module is installed in the middle of the roof.

[0080] As a possible implementation of this embodiment, step 2 includes the following steps:

[0081] Step 21: complete the multi-focal length camera calibration according to the installation positions of the short-focal length camera and the long-focal length camera.

[0082] Step 22, realizes information synchronization between the short-focus camera and the long-focus camera. In terms of time, the short-focus camera and the long-focus camera are required to shoot synchronously; in terms of space, the short-focus camera and the long-focus camera are both installed in front of the roof, with the short-focus camera located on the left side of the front of the roof, and the short-focus camera and the long-focus camera located on the right side of the front of the roof.

[0083] In step 23, the camera's image acquisition frequency should be kept constant, typically around 10 Hz. During acquisition, the camera is set to auto-exposure. Multi-focal length image data and GPS data are collected.

[0084] As a possible implementation of this embodiment, step 3 includes the following steps:

[0085] Step 31: construct an image pyramid for the image, and determine a scaling factor s for downsampling the image pyramid based on the difference in focal length between the short-focus camera and the long-focus camera.

[0086] Step 32, based on the difference of multi-focal length images, only the layers with the same image information size in the short focal length and long focal length image pyramids are selected to extract ORB feature points, such as the top layer of the short focal length image pyramid. dis The layer does not extract feature points, the long focal length camera image pyramid bottom l dis The layer does not extract feature points.

[0087] In step 33, the number of ORB feature points required to be extracted from the corresponding layer of feature points is:

[0088]

[0089] Among them, s represents the image pyramid scaling ratio, l dis Indicates the fixed level difference of the same size image features in the short focal length image pyramid and the long focal length image pyramid, n represents the number of image pyramid scaling layers, N represents the total number of ORB feature points that need to be extracted, α represents the layer from which ORB feature points need to be extracted, and N α Indicates the number of ORB feature points extracted by layer α in the image pyramid.

[0090] In step 34, a normalized cross-correlation algorithm is used to determine the region in the short-focus camera image that is identical to the long-focus camera image, which is called the ROI region. The algorithm is expressed as:

[0091]

[0092] The area corresponding to the maximum value of ρ(x,y) is selected as the part of the ROI short-focus camera image. Where S is the short-focus camera image, m and n represent the length and width of the long-focus camera image, i and j represent the image pixel coordinate points, X and Y represent the area blocks of the same size in the short-focus camera image and the long-focus camera image, and g is the long-focus camera image. is the average grayscale of the short focal length camera image, Average grayscale of long focal length camera image.

[0093] Step 35: Using a quadtree algorithm in the ROI region of a certain layer of the short focal length camera image pyramid, the number of ORB feature points extracted is increased, and the number of ORB feature points extracted outside the ROI region is reduced.

[0094] Step 36: perform feature matching on the extracted feature points and filter out incorrect matches.

[0095] As a possible implementation of this embodiment, step 4 converts the collected GPS positioning data into the multi-focal visual SLAM world coordinate system as follows: Figure 3 As shown, the following steps are included:

[0096] Step 41, based on the fact that the multi-focal visual SLAM world coordinate system is the first frame of the multi-focal SLAM and does not change during the entire multi-focal SLAM process, the coordinates of the translation vector from the first frame of the multi-focal SLAM to the initial frame in the SLAM world coordinate system and the GPS coordinate system are calculated. The translation vector is described in the SLAM world coordinate system as:

[0097] t w =R cw *(pos c,init -pos c,first )

[0098] t w Represents the world coordinate system, R cw Indicates the conversion from the camera coordinate system to the world coordinate system, pos c,init Indicates the camera pose at initialization, pos c,first Indicates the camera's first frame pose.

[0099] The GPS coordinate system is described as:

[0100] t GPS =pos GPS,init -pos GPS,first

[0101] t GPS Indicates the GPS coordinate system, pos GPS,init Indicates the GPS initialization posture, pos GPS,firstIndicates the GPS first frame pose.

[0102] The rotation matrix between the GPS coordinate system and the multi-focal length binocular SLAM world coordinate system is obtained according to the coordinates of the two coordinate systems:

[0103] R GPSw *t GPS =t w

[0104] R GPSw Represents the rotation matrix from the GPS coordinate system to the camera's world coordinate system.

[0105] As a possible implementation of this embodiment, step 5 includes the following steps:

[0106] Step 51: Different sensors have inconsistent data output frequencies. Civilian vehicle-mounted GPS receivers have an output frequency of approximately 50 Hz, while cameras have an output frequency of 10 Hz. To fuse sensor data, the data at different frequencies must be aligned. Aligning the camera and GPS data involves using the timestamp as the reference. The GPS positioning data at the moment the camera outputs the image is acquired. If no GPS data exists at that moment, the most recent GPS data is used as the current data.

[0107] As a possible implementation of this embodiment, the step 6 is as follows: Figure 4 As shown, the following steps are included:

[0108] Step 61, at time k, at position x k At this point, the sensor makes an observation and obtains data z k , the observation equation is:

[0109] z k =h(x k )

[0110] Since there are always errors in observations, define the error:

[0111] e k =z k -h(x k )

[0112] Step 62: Build a global pose graph. The poses to be solved are used as vertices of the optimization graph, and the observations (including camera observations of landmarks and GPS data) are used as edges. Build the objective function:

[0113]

[0114] where e k Represents the error, including image reprojection error and GPS positioning data error; Ω kRepresents the information matrix, which is the inverse of the covariance matrix and is a symmetric matrix.

[0115] Step 63: Use the g2o graph optimizer to solve the pose.

[0116] As a possible implementation of this embodiment, step 7 includes the following steps:

[0117] Step 71, establish a directed graph of the road network. The road network data is generally in shapefile format. By reading the shp file and dbf file, the node ID, coordinate information and attribute information of the road are obtained, thereby establishing a directed graph G(V, E) of the road network, where the elements of V are road endpoints and the elements of E are road sections.

[0118] Step 72 generates a grid index. This is required during the map matching process to obtain candidate road segments for trajectory points. Since simply traversing the entire road network is time-consuming, grid indexes are used to divide the entire map into M×N grids. Using a containment analysis algorithm, the road segments contained (intersecting) in each grid are pre-calculated, resulting in a 1000×1000 grid. Grid indexes are associated with road segment data via the road segment ID.

[0119] Step 73: Set a buffer zone with a radius of R for the trajectory point, determine the grid where the trajectory point is located based on the grid index, and query the K road segments closest to the trajectory point in a 3×3 grid centered on the grid as candidate road segments.

[0120] Step 74, calculate the emission probability. The emission probability is the probability of obtaining a certain observation value under a certain implicit state. Considering the distance factor, the formula is:

[0121]

[0122] Among them, σ represents the standard deviation of the error in obtaining trajectory data, p i represents the trajectory point, represents the candidate road segment corresponding to the k-th point, Indicates the distance from the trajectory point to the candidate road segment.

[0123] Step 75: The transfer probability is based on the fact that the road is generally the shortest distance between two points. The smaller the road network distance between the front and rear road segments, the greater the transfer probability:

[0124]

[0125] in, and are the candidate road segments corresponding to the t+1th trajectory point and the tth trajectory point respectively, a is the set coefficient, and D represents the actual road network distance between the candidate road segments.

[0126] Step 76: Use the Viterbi algorithm to find the optimal path.

[0127] The systems, devices, modules, or units described in the above embodiments may be implemented by computer chips or entities, or by products having certain functions. A typical implementation device is a computer. Specifically, the computer may be, for example, a personal computer, a laptop computer, a cellular phone, a camera phone, a smartphone, a personal digital assistant, a media player, a navigation device, an email device, a game console, a tablet computer, a wearable device, or a combination of any of these devices.

[0128] Computer-readable media includes permanent and non-permanent, removable and non-removable media that can be implemented by any method or technology to store information. The information can be computer-readable instructions, data structures, program modules or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, compact disc read-only memory (CD-ROM), digital versatile disc (DVD) or other optical storage, magnetic cassettes, magnetic tape, magnetic disk storage or other magnetic storage devices or any other non-transmission media that can be used to store information that can be accessed by a computing device. As defined herein, computer-readable media does not include transitory computer-readable media (transitory media), such as modulated data signals and carrier waves.

[0129] It should also be noted that the terms "comprises," "includes," or any other variations thereof are intended to encompass non-exclusive inclusion, such that a process, method, commodity, or apparatus that includes a series of elements includes not only those elements but also other elements not explicitly listed, or includes elements inherent to such process, method, commodity, or apparatus. In the absence of further limitations, an element defined by the phrase "comprises a ..." does not exclude the presence of other identical elements in the process, method, commodity, or apparatus that includes the element.

[0130] The above embodiments should be understood as merely illustrating the present invention and not as limiting the scope of protection of the present invention. After reading the contents of the present invention, technicians may make various changes or modifications to the present invention, and these equivalent changes and modifications also fall within the scope defined by the claims of the present invention.

Claims

1. A method for fusing multi-focal-length visual SLAM with GPS, characterized in that: The following steps are involved: Step 1. Install a multi-focal length camera and a GPS module. The multi-focal length visual SLAM consists of a short-focal length camera and a long-focal length camera, which are placed horizontally. Step 2. Calibrate the multi-focal length cameras, collect multi-focal length images and GPS data at the same time, and obtain multi-focal length image data and GPS data. Step 3. According to the difference in focal length of the multi-focal length cameras, extract image feature points based on the image pyramid and perform feature matching. Step 4. Convert the collected GPS positioning data to the multi-focal length visual SLAM world coordinate system. Step 5. Align the timestamps of the converted GPS data and the multi-focal length visual SLAM data. Step 6. Build a graph optimization model to optimize the pose of the GPS data and SLAM image data after timestamp alignment. Step 7. Match the running trajectory generated by SLAM to the corresponding map road trajectory on the map. Step 3 extracts image feature points based on the image pyramid according to the difference in focal lengths of the multi-focal cameras, and performs feature matching, specifically including: Step 31: construct an image pyramid for the image, and determine a scaling factor s for downsampling the image pyramid based on the difference in focal length between the short-focus camera and the long-focus camera. Step 32, based on the difference of multi-focal length images, only the layers with the same image information size in the short focal length and long focal length image pyramids are selected to extract ORB feature points, and the top layer of the short focal length image pyramid is l dis The layer does not extract feature points, the long focal length camera image pyramid bottom l dis The layer does not extract feature points; Step 33, obtaining the number of ORB feature points required to be extracted for the corresponding layer from which feature points need to be extracted; Step 34, using a normalized cross-correlation algorithm to determine the region in the short-focus camera image that is identical to the region in the long-focus camera image, referred to as the ROI region; Step 35: using a quadtree algorithm to increase the number of ORB feature points extracted within the ROI area of ​​a certain layer of the short focal length camera image pyramid, and reduce the number of ORB feature points extracted outside the ROI area; Step 36: perform feature matching on the extracted feature points and filter out incorrect matches.

2. The method for fusing multifocal visual SLAM with GPS according to claim 1, wherein The step 2 of calibrating the multi-focal cameras and simultaneously collecting multi-focal images and GPS data to obtain the multi-focal image data and GPS data specifically includes: Step 21, completing the multi-focal length camera calibration according to the installation positions of the short-focal length camera and the long-focal length camera; Step 22: Synchronize the information of the short-focus camera and the long-focus camera. In terms of time, the short-focus camera and the long-focus camera are required to shoot synchronously. In terms of space, the short-focus camera and the long-focus camera are both installed in front of the roof, with the short-focus camera located on the left side of the front of the roof and the long-focus camera located on the right side of the front of the roof. Step 23: The camera's image acquisition frequency remains constant, and the camera is set to automatically expose during the acquisition process; multi-focal length image data and GPS data are acquired.

3. The method of combining multi-focal-length visual SLAM and GPS according to claim 1, wherein In step 33, the number of ORB feature points required to be extracted for the corresponding layer where feature points need to be extracted is obtained as follows: Among them, s represents the image pyramid scaling ratio, l dis Indicates the fixed level difference of the same size image features in the short focal length image pyramid and the long focal length image pyramid, n represents the number of image pyramid scaling layers, N represents the total number of ORB feature points that need to be extracted, α represents the layer from which ORB feature points need to be extracted, and N α Indicates the number of ORB feature points extracted by layer α in the image pyramid.

4. The method of combining multi-focal-length visual SLAM and GPS according to claim 3, wherein: The normalized cross-correlation algorithm of step 34 is specifically expressed as: The area corresponding to the maximum value of ρ(x,y) is selected as the part of the ROI short-focus camera image; where S is the short-focus camera image, m and n represent the length and width of the long-focus camera image, i and j represent the image pixel coordinate points, X and Y represent the area blocks of the same size in the short-focus camera image and the long-focus camera image, and g is the long-focus camera image. is the average grayscale of the short focal length camera image, Average grayscale of long focal length camera image.

5. A method for fusing multifocal visual SLAM with GPS according to claim 4, characterized in that, Step 4 converts the collected GPS positioning data into the multi-focal visual SLAM world coordinate system, specifically including: Step 41, based on the multi-focal visual SLAM world coordinate system established with the first SLAM frame, which does not change during the entire multi-focal visual SLAM process, calculate the coordinates of the translation vector from the first multi-focal visual SLAM frame to the initial frame in the SLAM world coordinate system and the GPS coordinate system; the translation vector is described in the SLAM world coordinate system: t w =R cw *(pos c,init -pos c,first ) t w Represents the world coordinate system, R cw Indicates the conversion from the camera coordinate system to the world coordinate system, pos c,init Indicates the camera pose at initialization, pos c,first Indicates the camera's first frame pose; The GPS coordinate system is described as: t GPS =pos GPS,init -pos GPS,first t GPS Indicates the GPS coordinate system, pos GPS,init Indicates the GPS initialization posture, pos GPS,first Indicates the GPS first frame pose; The rotation matrix between the GPS coordinate system and the multi-focal length binocular SLAM world coordinate system is obtained according to the coordinates of the two coordinate systems: R GPSw *t GPS =t w R GPSw Represents the rotation matrix from the GPS coordinate system to the camera's world coordinate system.

6. A method for fusing multi-focal-length visual SLAM with GPS according to claim 5, characterized in that, The step 6 constructs a graph optimization model to optimize the pose of the GPS data and SLAM image data after the timestamps are aligned, specifically including: Step 61, at time k, at position x k At this point, the sensor makes an observation and obtains data z k , the observation equation is: z k =h(x k ) Since there are always errors in observations, define the error: e k =z k -h(x k ) Step 62: Build a global pose graph. The poses to be solved are used as vertices of the optimization graph, and the observations are used as edges. The objective function is constructed. The observations include the camera's observations of the landmarks and GPS data: where e k Represents the error, including image reprojection error and GPS positioning data error; Ω k represents the information matrix, which is the inverse of the covariance matrix and is a symmetric matrix; In step 63, a graph optimizer is used to deploy the vertices and edges of the global pose graph and solve the optimal pose.

7. A method for fusing multi-focal-length visual SLAM with GPS according to claim 6, characterized in that, Step 7 matches the running trajectory generated by SLAM to the corresponding map road trajectory on the map, specifically including: Step 71: Establish a directed graph of the road network, obtain the node ID, coordinate information, and attribute information of the road, and thus establish a directed graph G(V, E) of the road network, where the elements of V are road endpoints and the elements of E are road segments; Step 72: Generate a grid index. Use the grid index to divide the entire map into M×N grids. Pre-calculate the intersecting road segments in each grid using a containment analysis algorithm to obtain candidate road segments for the trajectory points. The grid index is associated with the road segment data via the road segment ID. Step 73: Set a buffer zone with a radius of R for the trajectory point, determine the grid where the trajectory point is located based on the grid index, and query the K road segments closest to the trajectory point in a 3×3 grid centered on the grid as candidate road segments; Step 74, calculate the emission probability, which is the probability of obtaining a certain observation value under a certain implicit state, taking into account the distance factor; Step 75, calculating the transmission probability; Step 76: Use the Viterbi algorithm to select the optimal path for each segment and delete other paths, and iterate until the optimal path is found.

8. A method for fusing multi-focal-length visual SLAM with GPS according to claim 7, characterized in that, The emission probability is calculated, that is, the probability of obtaining a certain observation value under a certain implicit state, taking into account the distance factor. The specific formula is: Among them, σ represents the standard deviation of the error in obtaining trajectory data, p i represents the trajectory point, represents the candidate road segment corresponding to the k-th point, Indicates the distance from the trajectory point to the candidate road segment; Step 81: The transfer probability is based on the shortest distance between two points on the road. The smaller the road network distance between the front and rear road segments, the greater the transfer probability: in, and are the candidate road segments corresponding to the t+1th trajectory point and the tth trajectory point respectively, a is the set coefficient, and D represents the actual road network distance between the candidate road segments.

9. A storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method for fusing multi-focal visual SLAM and GPS as described in any one of claims 1 to 8 is implemented.

Images