A road scene based positioning system and method

Abstract

The application provides a positioning system and method based on a road scene. The system comprises a computing unit, a GNSS satellite positioning unit, an inertial measurement unit, a laser radar and a binocular camera installed on a vehicle; the computing unit is used to establish a road scene map, obtain position coordinates of the vehicle by integrating real-time output data of the inertial measurement unit for a period of time, and calculate real-time position coordinates of the vehicle based on matching of the position coordinates obtained by integration and the road scene map; the sensor data comprises position coordinates of the vehicle obtained by the GNSS satellite positioning unit, attitude angles of the vehicle output by the inertial measurement unit, laser point clouds of a front road output by the laser radar during fan-shaped scanning, depth point clouds and RGB images of the front road output by the binocular camera. The application realizes real-time positioning by using the road scene map, and can realize accurate positioning without dependence on satellites and other reference devices.

Description

Technical Field

[0001] This invention belongs to the field of navigation and positioning technology, specifically relating to a positioning system and method based on road scenarios. Background Technology

[0002] Accurate and stable positioning and attitude measurement are crucial for vehicles to complete specific tasks, such as autonomous driving, lane keeping, and high-speed cruise in special vehicles. Currently, high-precision vehicle positioning mainly relies on sensors such as GNSS, high-precision IMU, and odometers, as well as combinations thereof. However, current positioning systems and methods still have significant shortcomings. For example, when GNSS signals are shielded by tunnels or are themselves interfered with, global positioning information can become invalid. Although sensors such as IMU and odometers can meet the requirements for accurate pose measurement in short periods, their inherent errors such as zero bias and scale factor cause positioning and attitude measurement errors to diverge over time, failing to meet the requirements for long-term stable navigation.

[0003] In view of this, the present invention proposes a positioning system and method based on road scenes. Relying on vehicle-mounted image sensors, the semantic features of the road scene are parsed and matched with the semantic map established during historical driving to achieve high-precision and stable positioning results. Summary of the Invention

[0004] To address the aforementioned problems in the existing technology, this invention provides a positioning system and method based on road scenarios, which can effectively solve the problems of unstable vehicle positioning information and error divergence over time.

[0005] To achieve the above objectives, the present invention adopts the following technical solution.

[0006] In a first aspect, the present invention provides a road scene-based positioning system, comprising: a computing unit installed on a vehicle and a GNSS satellite positioning unit, an inertial measurement unit, a lidar, and a binocular camera connected to the computing unit; the computing unit is used to build a road scene map based on sensor data obtained during historical driving, to obtain the vehicle's position coordinates by integrating the real-time output data of the inertial measurement unit over a period of time, and to calculate the vehicle's real-time position coordinates by matching the position coordinates obtained by integration with the road scene map; the sensor data includes: the vehicle's position coordinates expressed in longitude, latitude, and elevation obtained by the GNSS satellite positioning unit, the vehicle's attitude angle output by the inertial measurement unit, the laser point cloud of the road ahead output by the lidar during fan-shaped scanning, and the depth point cloud and RGB image of the road ahead output by the binocular camera.

[0007] Furthermore, the system also includes a synchronization controller. When the synchronization controller detects the rising edge of the pulse signal output by the GNSS satellite positioning unit, it simultaneously outputs four identical synchronization pulse signals to the inertial measurement unit, lidar, binocular camera, and computing unit, respectively, so that the inertial measurement unit, lidar, binocular camera, and computing unit work synchronously based on the rising edge of the synchronization pulse signal.

[0008] Furthermore, methods for building road scene maps based on sensor data obtained during historical driving include:

[0009] The RGB images output by the binocular cameras during historical driving are input into the semantic segmentation neural network to obtain the type and R, G, B values ​​of each pixel. Pixels {i,j} are denoted as P. ij ={r,g,b,S k}, where r, g, and b are the R, G, and B values ​​of pixel {i,j}, respectively, and S k There are 8 pixel types, denoted as {S}, where {i,j} is the pixel type. k} = {lane lines, road boundary lines, road markings, traffic lights, road signs, roadside buildings, poles, overpasses}, k = 1, 2, ..., 8;

[0010] The laser point cloud from the lidar and the depth point cloud from the binocular camera are registered to the nearest location, and the depth data of the two point clouds are weighted and summed to obtain the true depth of each pixel.

[0011] Calculate the coordinates of each pixel in the inertial measurement unit coordinate system. The formula is:

[0012]

[0013] In the formula, π C [R] C t C ] are the intrinsic and extrinsic parameter matrices of the stereo camera, respectively, R C t C λ represents the attitude angle and position deviation of the binocular camera relative to the inertial measurement unit. ij Let {i,j} be the actual depth of pixel point {i,j};

[0014] Calculate the coordinates of each pixel in the navigation coordinate system. The formula is:

[0015]

[0016] In the formula, R W T is the attitude angle measured by the inertial measurement unit. WConvert the longitude, latitude, and elevation data obtained from GNSS satellite positioning units into values ​​in the navigation coordinate system;

[0017] Further processing is performed on the point cloud for each pixel type to obtain point cloud data for each pixel type. Continuous point cloud data is obtained by superimposing point cloud data from n consecutive acquisition times. The collection of continuous point cloud data of 8 pixel types constitutes the road scene map P. map , is represented as:

[0018]

[0019] In the formula, For pixel type S k Point cloud data, t1~t n For n consecutive data collection times.

[0020] Furthermore, continuous point cloud data of road boundaries The calculation methods include:

[0021] Gaussian filtering and downsampling are applied to the road surface point cloud data to maintain the shape of the road surface point cloud and reduce the number of road surface point clouds.

[0022] The elevation coordinate components of the processed road surface point cloud are removed from its three-dimensional coordinates to obtain a two-dimensional planar point cloud. The convex hull of this two-dimensional planar point cloud is then detected using a two-dimensional convex hull method to obtain the closed road boundary line at each data acquisition moment. After removing the front and rear boundaries determined by the measurement range of the binocular camera, only the road boundary lines on both sides of the road are obtained.

[0023] For t1~t n Closed road boundary line at n consecutive acquisition times By overlaying the data, continuous point cloud data of the road boundary line is obtained.

[0024] Furthermore, methods for calculating the real-time location coordinates of vehicles based on road scene maps include:

[0025] The video images output in real time from the binocular camera are input into the trained semantic segmentation neural network to obtain the type and R, G, B values ​​of each pixel. Pixels {i,j} are denoted as P. ij ={r,g,b,S k};

[0026] Calculate the coordinates of each pixel in the inertial measurement unit coordinate system according to equation (1). (1) λ ij Let {i,j} be the depth of the pixel points obtained by the stereo camera;

[0027] By integrating the real-time output data of the inertial measurement unit over a period of time, the coordinates at each moment relative to the starting moment of integration are obtained. position change T I and attitude angle change R I ;

[0028] The formula for calculating the coordinates of each pixel in the navigation coordinate system at n time points within a time interval Δt is as follows:

[0029]

[0030] In the formula, Let {i,j} be the coordinates of pixel {i,j} in the navigation coordinate system at n time points within Δt;

[0031] Using the ICP algorithm, with the current time T I R I Given the initial values ​​of the translation matrix T and the rotation value matrix R, iterative calculations are used to obtain T and R that minimize the optimization function, thereby achieving... P with road scene map point cloud map {q1,q2,…q n Alignment; the optimized function for iterative operations is:

[0032]

[0033] In the formula, E(R,T) is the value of the optimization function;

[0034] T is calculated based on the integral at the current time t. I R I Calculate the position coordinates T of the stereo camera at the current time t. t and attitude angle R t The formula is:

[0035]

[0036] Based on T t R t And the field of view of the binocular camera, in the road scene map P map The matching region is divided into segments, and the ICP algorithm is used again, with T... t R t Using the initial values, the optimal coordinate translation matrix T′ and rotation value matrix R′ that minimize the optimization function are obtained through iterative calculations;

[0037] The formula for calculating the current position and attitude angle is:

[0038]

[0039] In the formula, T t ′、R t ′ represents the current position and attitude angle, respectively.

[0040] Secondly, the present invention provides a method for vehicle positioning using the system, comprising the following steps performed by a computing unit:

[0041] A road scene map is built based on sensor data obtained during historical driving. The sensor data includes: vehicle position coordinates output by GNSS satellite positioning unit, vehicle attitude angle output by inertial measurement unit, laser point cloud output by lidar, depth point cloud and RGB image output by binocular camera.

[0042] The vehicle's position coordinates are obtained by integrating the data output from the inertial measurement unit over a period of time, after acquiring sensor data in real time.

[0043] The vehicle's position coordinates are calculated in real time by matching the location coordinates obtained through integration with the road scene map.

[0044] Furthermore, the system also includes a synchronization controller. When the synchronization controller detects the rising edge of the pulse signal output by the GNSS satellite positioning unit, it simultaneously outputs four identical synchronization pulse signals to the inertial measurement unit, lidar, binocular camera, and computing unit, respectively, so that the inertial measurement unit, lidar, binocular camera, and computing unit work synchronously based on the rising edge of the synchronization pulse signal.

[0045] Furthermore, methods for building road scene maps based on sensor data obtained during historical driving include:

[0046] The RGB images output by the binocular cameras during historical driving are input into the semantic segmentation neural network to obtain the type and R, G, B values ​​of each pixel. Pixels {i,j} are denoted as P. ij ={r,g,b,S k}, where r, g, and b are the R, G, and B values ​​of pixel {i,j}, respectively, and S k There are 8 pixel types, denoted as {S}, where {i,j} is the pixel type. k} = {lane lines, road boundary lines, road markings, traffic lights, road signs, roadside buildings, poles, overpasses}, k = 1, 2, ..., 8;

[0047] The laser point cloud from the lidar and the depth point cloud from the binocular camera are registered to the nearest location, and the depth data of the two point clouds are weighted and summed to obtain the true depth of each pixel.

[0048] Calculate the coordinates of each pixel in the inertial measurement unit coordinate system. The formula is:

[0049]

[0050] In the formula, π C [R] C t C ] are the intrinsic and extrinsic parameter matrices of the stereo camera, respectively, R C t C λ represents the attitude angle and position deviation of the binocular camera relative to the inertial measurement unit. ij Let {i,j} be the actual depth of pixel point {i,j};

[0051] Calculate the coordinates of each pixel in the navigation coordinate system. The formula is:

[0052]

[0053] In the formula, R W T is the attitude angle measured by the inertial measurement unit. W Convert the longitude, latitude, and elevation data obtained from GNSS satellite positioning units into values ​​in the navigation coordinate system;

[0054] Further processing is performed on the point cloud for each pixel type to obtain point cloud data for each pixel type. Continuous point cloud data is obtained by superimposing point cloud data from n consecutive acquisition times. The collection of continuous point cloud data of 8 pixel types constitutes the road scene map P. map , is represented as:

[0055]

[0056] In the formula, For pixel type S k Point cloud data, t1~t n For n consecutive data collection times.

[0057] Furthermore, continuous point cloud data of road boundaries The calculation methods include:

[0058] The laser point cloud from the lidar and the depth point cloud from the binocular camera are registered to the nearest location, and the depth data of the two point clouds are weighted and summed to obtain the true depth of each pixel.

[0059] The elevation coordinate components of the processed road surface point cloud are removed from its three-dimensional coordinates to obtain a two-dimensional planar point cloud. The convex hull of this two-dimensional planar point cloud is then detected using a two-dimensional convex hull method to obtain the closed road boundary line at each data acquisition moment. After removing the front and rear boundaries determined by the measurement range of the binocular camera, only the road boundary lines on both sides of the road are obtained.

[0060] For t1~t n Closed road boundary line at n consecutive acquisition times By overlaying the data, continuous point cloud data of the road boundary line is obtained.

[0061] Furthermore, methods for calculating the real-time location coordinates of vehicles based on road scene maps include:

[0062] The video images output in real time from the binocular camera are input into the trained semantic segmentation neural network to obtain the type and R, G, B values ​​of each pixel. Pixels {i,j} are denoted as P. ij ={r,g,b,S k};

[0063] Calculate the coordinates of each pixel in the inertial measurement unit coordinate system according to equation (1). (1) λ ij Let {i,j} be the depth of the pixel points obtained by the stereo camera;

[0064] By integrating the real-time output data of the inertial measurement unit over a period of time, the coordinates at each moment relative to the starting moment of integration are obtained. position change T I and attitude angle change R I ;

[0065] The formula for calculating the coordinates of each pixel in the navigation coordinate system at n time points within a time interval Δt is as follows:

[0066]

[0067] In the formula, Let {i,j} be the coordinates of pixel {i,j} in the navigation coordinate system at n time points within Δt;

[0068] Using the ICP algorithm, with the current time T I R I Given the initial values ​​of the translation matrix T and the rotation value matrix R, iterative calculations are used to obtain T and R that minimize the optimization function, thereby achieving... P with road scene map point cloud map {q1,q2,…q n Alignment; the optimized function for iterative operations is:

[0069]

[0070] In the formula, E(R,T) is the value of the optimization function;

[0071] T is calculated based on the integral at the current time t. I R I Calculate the position coordinates T of the stereo camera at the current time t. t and attitude angle R t The formula is:

[0072]

[0073] Based on T t R t And the field of view of the binocular camera, in the road scene map P map The matching region is divided into segments, and the ICP algorithm is used again, with T... t R t Using the initial values, the optimal coordinate translation matrix T′ and rotation value matrix R′ that minimize the optimization function are obtained through iterative calculations;

[0074] The formula for calculating the current position and attitude angle is:

[0075]

[0076] In the formula, T t ′、R t ′ represents the current position and attitude angle, respectively.

[0077] Compared with the prior art, the present invention has the following beneficial effects.

[0078] This invention utilizes a computing unit installed on a vehicle, along with a GNSS satellite positioning unit, an inertial measurement unit, a lidar, and a binocular camera connected to the computing unit. The computing unit builds a road scene map based on sensor data acquired during historical driving. The vehicle's position coordinates are obtained by integrating the real-time output data from the inertial measurement unit over a period of time. The vehicle's real-time position coordinates are then calculated by matching the integrated position coordinates with the road scene map, thus achieving real-time positioning based on the road scene. This invention achieves real-time positioning using a road scene map, enabling precise positioning without relying on satellites or other reference devices, and solving problems such as poor positioning accuracy and difficulty in maintaining position when satellite signals are denied. Attached Figure Description

[0079] Figure 1 This is a block diagram of a road scene-based positioning system according to an embodiment of the present invention.

[0080] In the diagram: 1-Computing unit, 2-GNSS satellite positioning unit, 3-Inertial measurement unit, 4-LiDAR, 5-Binocular camera.

[0081] Figure 2 This is a schematic diagram of the hardware installation structure of the system.

[0082] Figure 3 This is a flowchart illustrating a method for vehicle positioning using the system according to an embodiment of the present invention. Detailed Implementation

[0083] To make the objectives, technical solutions, and advantages of this invention clearer and more understandable, the invention will be further described below in conjunction with the accompanying drawings and specific embodiments. Obviously, the described embodiments are merely some embodiments of this invention, and not all embodiments. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this invention.

[0084] Figure 1 This is a block diagram of a road scene-based positioning system according to an embodiment of the present invention, including: a computing unit 1 installed on a vehicle, and a GNSS satellite positioning unit 2, an inertial measurement unit 3, a lidar 4, and a binocular camera 5 connected to the computing unit 1; the computing unit 1 is used to build a road scene map based on sensor data obtained during historical driving, to obtain the vehicle's position coordinates by integrating the real-time output data of the inertial measurement unit 3 over a period of time, and to calculate the vehicle's real-time position coordinates by matching the position coordinates obtained by integration with the road scene map; the sensor data includes: the vehicle's position coordinates expressed in longitude, latitude, and elevation obtained by the GNSS satellite positioning unit 2, the vehicle's attitude angle output by the inertial measurement unit 3, the laser point cloud of the road ahead output by the lidar 4 during fan-shaped scanning, and the depth point cloud and RGB image of the road ahead output by the binocular camera 5.

[0085] The system described in this embodiment mainly consists of a computing unit 1, a GNSS satellite positioning unit 2, an inertial measurement unit 3, a lidar 4, and a binocular camera 5. A schematic diagram of the system's hardware installation structure is shown below. Figure 2As shown. Except for the inertial measurement unit 3, all components in the system are mounted on the bracket with screws. The inertial measurement unit 3 is installed in the cavity of the lower half of the bracket and fixed to the bottom of the bracket with screws. The binocular camera 5 is installed at the front of the bracket, extending a certain distance out of the bracket via an L-shaped connecting device. The measurement direction of the binocular camera 5 is horizontal and forward. The computing unit 1 and the lidar 4 are installed inside the cavity of the upper half of the bracket. The computing unit 1 is completely enclosed within the cavity, and the lidar 4 is installed at the upper front of the bracket, with its sensing part extending out of the cavity to scan targets in front. The bracket is made of aluminum alloy, and a piece of cast iron is mounted on the upper surface of the cavity with screws. The GNSS satellite positioning unit 2 is attached to the cast iron with a magnet. A square cable routing hole is provided at the rear of the bracket cavity to lead out the cables inside the space. All units are rigidly connected.

[0086] In this embodiment, the computing unit 1 is connected to the GNSS satellite positioning unit 2, the inertial measurement unit 3, the lidar 4, and the binocular camera 5. The lidar 4, the inertial measurement unit 3, and the binocular camera 5 transmit sensing information to the computing unit 1 through a communication interface. The GNSS satellite positioning unit 2 typically uses the GPS or BeiDou system to acquire the vehicle's position coordinates in real time, including longitude, latitude, and elevation data. The inertial measurement unit 3 outputs the vehicle's attitude data, including azimuth, pitch, and roll angles. The lidar 4 outputs the laser point cloud of the road ahead, which it scans. The binocular camera 5 outputs the depth point cloud and RGB image of the road ahead. The computing unit 1 is the control and data processing center of the system, used to coordinate the work of each unit and complete necessary data processing tasks. The data processing tasks completed by the computing unit 1 mainly include: establishing a road scene map based on sensor data obtained during historical driving; when the satellite signal is not ideal (such as obstruction), the GNSS satellite positioning unit 2 cannot output reliable vehicle positioning data. The computing unit 1 integrates the real-time output data of the inertial measurement unit 3 for a period of time (when the time is short, the inertial measurement unit 3 can obtain high-precision positioning data, and when the time is long, the positioning accuracy will decrease due to accumulated errors), obtains the vehicle's position coordinates, and calculates the vehicle's real-time position coordinates based on the position coordinates obtained by integration and the road scene map.

[0087] This embodiment achieves real-time positioning by utilizing road scene maps, enabling accurate positioning without relying on satellites or other reference devices. This solves the problems of poor positioning accuracy and difficulty in maintaining positioning when satellite signals are denied.

[0088] As an optional embodiment, the system further includes a synchronization controller. When the synchronization controller detects the rising edge of the pulse signal output by the GNSS satellite positioning unit 2, it simultaneously outputs four identical synchronization pulse signals to the inertial measurement unit 3, the lidar 4, the binocular camera 5, and the computing unit 1, respectively, so that the inertial measurement unit 3, the lidar 4, the binocular camera 5, and the computing unit 1 work synchronously based on the rising edge of the synchronization pulse signal.

[0089] This embodiment provides a technical solution for achieving synchronized operation of the system. To achieve synchronized operation of the various units, this embodiment includes a synchronization controller connected to the inertial measurement unit 3, the lidar 4, the binocular camera 5, and the computing unit 1. The installation location of the synchronization controller is as follows... Figure 2 As shown, the synchronization controller, along with the computing unit 1 and the lidar 4, is installed inside the cavity in the upper part of the support. The system achieves synchronized operation by outputting synchronization pulses from the synchronization controller to other units. The source of the synchronization pulse is the GNSS satellite positioning unit 2, and the synchronization pulse is a narrow pulse with a frequency of 1Hz and an amplitude of 5V. After the synchronization controller acquires the rising edge of the narrow pulse, it synchronously outputs four 1Hz narrow pulses, which are respectively sent to the lidar 4, the inertial measurement unit 3, the binocular camera 5, and the computing unit 1. After detecting the rising edge of the pulse, each unit calibrates its internal timer to achieve time synchronization between the individual units.

[0090] As an optional embodiment, the method for building a road scene map based on sensor data obtained during historical driving includes:

[0091] The RGB images output by the binocular camera 5 during the historical driving process are input into the semantic segmentation neural network to obtain the type and R, G, B values ​​of each pixel. Pixels {i,j} are denoted as P. ij ={r,g,b,S k}, where r, g, and b are the R, G, and B values ​​of pixel {i,j}, respectively, and S k There are 8 pixel types, denoted as {S}, where {i,j} is the pixel type. k} = {lane lines, road boundary lines, road markings, traffic lights, road signs, roadside buildings, poles, overpasses}, k = 1, 2, ..., 8;

[0092] The laser point cloud of LiDAR 4 and the depth point cloud of binocular camera 5 are registered based on the closest position, and the depth and distance data of the two point clouds are weighted and summed to obtain the true depth of each pixel.

[0093] Calculate the coordinates of each pixel in the inertial measurement unit's 3-coordinate system. The formula is:

[0094]

[0095] In the formula, π C [R] C t C [R] represents the intrinsic and extrinsic parameter matrices of the stereo camera 5. C t C λ represents the attitude angle deviation and position deviation of the binocular camera 5 relative to the inertial measurement unit 3. ij Let {i,j} be the actual depth of pixel point {i,j};

[0096] Calculate the coordinates of each pixel in the navigation coordinate system. The formula is:

[0097]

[0098] In the formula, R W The attitude angle T is measured by inertial measurement unit 3. W The longitude, latitude, and elevation data obtained by GNSS satellite positioning unit 2 are converted into values ​​in the navigation coordinate system;

[0099] Further processing is performed on the point cloud for each pixel type to obtain point cloud data for each pixel type. Continuous point cloud data is obtained by superimposing point cloud data from n consecutive acquisition times. The collection of continuous point cloud data of 8 pixel types constitutes the road scene map P. map , is represented as:

[0100]

[0101] In the formula, For pixel type S k Point cloud data, t1~t n For n consecutive data collection times.

[0102] This embodiment presents a technical solution for establishing a road scene map. A road scene map is a collection of positional coordinate data of pixels that constitute lane lines and road boundary lines. Since calculating the positional coordinates of pixels involves coordinate transformations between various coordinate systems, the various coordinate systems involved in this embodiment are introduced before describing the technical solution for establishing the road scene map. Navigation coordinate system O1X1Y1Z1: The origin is selected as a point on the ground, the X1 axis points due east, the Y1 axis points due north, and the Z1 axis is defined according to the right-hand rule. Inertial measurement unit coordinate system O2X2Y2Z2: The origin is at the geometric center of the inertial measurement unit 3, the X2 axis points directly in front of the vehicle (also directly in front of the binocular camera 5), ​​the Z2 axis points upward in the longitudinal plane of the inertial measurement unit 3, and the Y2 axis is defined according to the right-hand rule. Camera coordinate system O3X3Y3Z3: The origin is defined at the geometric center of the camera, the X3 axis points directly in front of the camera's field of view, the Z3 axis points upward in the longitudinal plane of the camera, and the Y3 axis is defined according to the right-hand rule. It also involves the lidar coordinate system and the image coordinate system, which will not be introduced in detail here.

[0103] This embodiment establishes a road scene map based on road image data obtained during historical driving. First, the RGB images output by the binocular camera 5 during historical driving are input into a trained semantic segmentation neural network, which outputs the type and R, G, and B values ​​of each pixel. This embodiment has eight pixel types: lane lines, road boundary lines, road markings, traffic lights, road signs, roadside buildings, poles, and overpasses. Pixels with coordinates {i,j} are denoted as P. ij ={r,g,b,S k}, where r, g, and b are the R, G, and B values ​​of pixel {i,j}, respectively, and S k The type of pixel is determined by the laser point cloud of the road ahead obtained by the lidar 4 and the depth point cloud of the binocular camera 5. The registration method is to first convert the two point cloud data to the same coordinate system (such as the camera coordinate system), then align the two point clouds with the closest coordinates, and then sum the depth and distance data of the two registered point clouds by weighted summation to obtain the high-precision depth data of each pixel, which is called the true depth. Then, the coordinates of each pixel in the coordinate system of the inertial measurement unit 3 are calculated according to equation (1). And according to formula (2) Convert to coordinates in the navigation coordinate system (2) R W The attitude angle T is measured by inertial measurement unit 3. W The formula for converting the longitude, latitude, and elevation [lon,lat,alt] obtained from GNSS satellite positioning unit 2 to coordinate values ​​in the navigation coordinate system is as follows:

[0104] T W=[r×(lon-lon0)cos(lat),r×(lat-lat0),alt-alt0]

[0105] In the formula, [lon0,lat0,alt0] represents the longitude, latitude, and elevation of the origin of the navigation coordinate system, and r represents the Earth's radius.

[0106] Next, we will further process the point cloud data for each pixel type to obtain point cloud data for each pixel type. Continuous point cloud data is then obtained by superimposing point cloud data from n consecutive acquisition times. A road scene map P is a collection of continuous point cloud data consisting of eight pixel types. map It can be expressed as shown in equation (3). It is worth noting that among the eight types of point cloud data in equation (3), the most important are the first two types, namely lane lines and road boundary lines. Lane lines and road boundary lines are continuous and can be used to provide continuous and stable positioning; other point cloud data are used to improve accuracy. The boundary processing method for lane lines and road boundary lines is also different from that for other point cloud data. When processing lane lines and road boundary lines, the boundaries at both ends are removed.

[0107] As an alternative embodiment, continuous point cloud data of road boundary lines The calculation methods include:

[0108] Gaussian filtering and downsampling are applied to the road surface point cloud data to maintain the shape of the road surface point cloud and reduce the number of road surface point clouds.

[0109] The elevation coordinate components of the processed point cloud are removed from its three-dimensional coordinates to obtain a two-dimensional planar point cloud. The convex hull of this two-dimensional planar point cloud is then detected using a two-dimensional convex hull method to obtain the closed road boundary line at each data acquisition moment. After removing the front and rear boundaries determined by the measurement range of the binocular camera 5, only the road boundary lines on both sides of the road are obtained.

[0110] For t1~t n Closed road boundary line at n consecutive acquisition times By overlaying the data, continuous point cloud data of the road boundary line is obtained.

[0111] This embodiment presents a technical solution for obtaining continuous point cloud data of road boundary lines. First, the point cloud data is preprocessed: Gaussian filtering is used to downsample the 3D semantic point cloud of the road surface, preserving the point cloud morphology and reducing the amount of point cloud data. Then, road boundary extraction is performed: a planar assumption is adopted for road boundary extraction, discarding the height component of the 3D point cloud coordinates, transforming it into a 2D planar point cloud. A 2D convex hull is used to detect the convex hull of the 2D planar point cloud, obtaining the closed boundary of the road surface area. The closed boundary includes a front boundary and a rear boundary, located at the near and far ends of the effective measurement range defined in the camera coordinate system. The coordinates are transformed to the navigation coordinate system using a coordinate transformation matrix. Closed boundary points along the vehicle direction that are smaller than the near end value or larger than the far end value are removed, leaving the point cloud of the road boundary on both sides. Finally, the closed road boundary lines are obtained from n consecutive acquisition times. By overlaying the data, continuous point cloud data of the road boundary line is obtained.

[0112] As an optional embodiment, the method for calculating the real-time location coordinates of a vehicle based on a road scene map includes:

[0113] The video images output in real time from the binocular camera 5 are input into the trained semantic segmentation neural network to obtain the type and R, G, and B values ​​of each pixel. Pixels {i,j} are denoted as P. ij ={r,g,b,S k};

[0114] Calculate the coordinates of each pixel in the coordinate system of the inertial measurement unit 3 according to equation (1). (1) λ ij Let {i,j} be the depth of the pixel points obtained by the binocular camera 5;

[0115] By integrating the real-time output data of inertial measurement unit 3 over a period of time, the coordinates at each moment relative to the start of the integration are obtained. position change T I and attitude angle change R I ;

[0116] The formula for calculating the coordinates of each pixel in the navigation coordinate system at n time points within a time interval Δt is as follows:

[0117]

[0118] In the formula, Let {i,j} be the coordinates of pixel {i,j} in the navigation coordinate system at n time points within Δt;

[0119] Using the ICP algorithm, with the current time T I R IGiven the initial values ​​of the translation matrix T and the rotation value matrix R, iterative calculations are used to obtain T and R that minimize the optimization function, thereby achieving... P with road scene map point cloud map {q1,q2,…q n Alignment; the optimized function for iterative operations is:

[0120]

[0121] In the formula, E(R,T) is the value of the optimization function;

[0122] T is calculated based on the integral at the current time t. I R I Calculate the position coordinates T of the stereo camera 5 at the current time t. t and attitude angle R t The formula is:

[0123]

[0124] Based on T t R t And the field of view of the binocular camera 5, in the road scene map P map The matching region is divided into segments, and the ICP algorithm is used again, with T... t R t Using the initial values, the optimal coordinate translation matrix T′ and rotation value matrix R′ that minimize the optimization function are obtained through iterative calculations;

[0125] The formula for calculating the current position and attitude angle is:

[0126]

[0127] In the formula, T t ′、R t ′ represents the current position and attitude angle, respectively.

[0128] This embodiment presents a technical solution for calculating the real-time position coordinates of vehicles based on a road scene map. First, the video image output in real-time from the binocular camera 5 is input into a trained semantic segmentation neural network to obtain the type and R, G, and B values ​​of each pixel. Pixels {i,j} are denoted as P... ij ={r,g,b,S k Then, calculate the coordinates of each pixel in the coordinate system of the inertial measurement unit 3 according to equation (1). It is worth noting that λ in equation (1) ijThe depth of pixel {i,j} obtained by the binocular camera 5 is used, rather than the true depth obtained by fusing the depth data from the binocular camera 5 and the lidar 4. In other words, the lidar 4 can be inactive during real-time positioning, and the computing center does not need to process the lidar point cloud data output by the lidar 4. This improves data processing speed and enables better real-time positioning calculations. When satellite signals are normal, the system achieves real-time positioning based on the output data of the GNSS satellite positioning unit 2. When satellite signals are weak or interrupted, existing technologies generally achieve real-time positioning by integrating the output data of the inertial measurement unit 3. Since integration involves cumulative errors, which increase over longer periods, this method cannot be used for high-precision real-time positioning over extended periods. Therefore, this embodiment integrates the output data of the inertial measurement unit 3 over a shorter period to obtain the vehicle's position coordinates. The integrated position coordinates are then matched with the road scene map to obtain the vehicle's high-precision position coordinates, thereby improving the vehicle's real-time positioning accuracy. This embodiment employs a double ICP (Iterative Closest Point) algorithm to align the integrated position coordinates with the road scene map point cloud data. The first ICP algorithm can divide the matching area from the road scene map and obtain the initial coordinate translation matrix T and rotation value matrix R. The second ICP algorithm can obtain the optimal coordinate translation matrix T′ and rotation value matrix R′. Based on T′ and R′, the precise position and attitude angle at the current moment can be calculated according to equation (7). The second matching only uses one frame of data for matching, and the matching scene is smaller, so the accuracy is higher, but it is easy to make a matching error. Therefore, the first matching is needed to find a low-precision but reliable result for the second matching.

[0129] Figure 3 This is a flowchart of a method for vehicle positioning using the system according to an embodiment of the present invention, including the following steps:

[0130] Step 101: Establish a road scene map based on sensor data obtained during historical driving. The sensor data includes: vehicle position coordinates output by GNSS satellite positioning unit 2, vehicle attitude angle output by inertial measurement unit 3, laser point cloud output by lidar 4, depth point cloud and RGB image output by binocular camera 5.

[0131] Step 102: Acquire sensor data in real time, and obtain the vehicle's position coordinates by integrating the output data of the inertial measurement unit 3 over a period of time;

[0132] Step 103: Based on the location coordinates obtained through integration, match them with the road scene map to calculate the vehicle's location coordinates in real time.

[0133] The method in this embodiment is similar to... Figure 1 The implementation principle and technical effects of the device embodiments shown are similar to those of the embodiments described above, and will not be repeated here. The same applies to the subsequent embodiments, which will not be described in detail.

[0134] As an optional embodiment, the system further includes a synchronization controller. When the synchronization controller detects the rising edge of the pulse signal output by the GNSS satellite positioning unit 2, it simultaneously outputs four identical synchronization pulse signals to the inertial measurement unit 3, the lidar 4, the binocular camera 5, and the computing unit 1, respectively, so that the inertial measurement unit 3, the lidar 4, the binocular camera 5, and the computing unit 1 work synchronously based on the rising edge of the synchronization pulse signal.

[0135] As an optional embodiment, the method for building a road scene map based on sensor data obtained during historical driving includes:

[0136] The RGB images output by the binocular camera 5 during the historical driving process are input into the semantic segmentation neural network to obtain the type and R, G, B values ​​of each pixel. Pixels {i,j} are denoted as P. ij ={r,g,b,S k}, where r, g, and b are the R, G, and B values ​​of pixel {i,j}, respectively, and S k There are 8 pixel types, denoted as {S}, where {i,j} is the pixel type. k} = {lane lines, road boundary lines, road markings, traffic lights, road signs, roadside buildings, poles, overpasses}, k = 1, 2, ..., 8;

[0137] The laser point cloud of LiDAR 4 and the depth point cloud of binocular camera 5 are registered, and the depth and distance data of the two point clouds are weighted and summed to obtain the true depth of each pixel.

[0138] Calculate the coordinates of each pixel in the inertial measurement unit's 3-coordinate system. The formula is:

[0139]

[0140] In the formula, π C [R] C t C [R] represents the intrinsic and extrinsic parameter matrices of the stereo camera 5. C t C λ represents the attitude angle deviation and position deviation of the binocular camera 5 relative to the inertial measurement unit 3. ij Let {i,j} be the actual depth of pixel point {i,j};

[0141] Calculate the coordinates of each pixel in the navigation coordinate system. The formula is:

[0142]

[0143] In the formula, R W The attitude angle T is measured by inertial measurement unit 3. W The longitude, latitude, and elevation data obtained by GNSS satellite positioning unit 2 are converted into values ​​in the navigation coordinate system;

[0144] Further processing is performed on the point cloud for each pixel type to obtain point cloud data for each pixel type. Continuous point cloud data is obtained by superimposing point cloud data from n consecutive acquisition times. The collection of continuous point cloud data of 8 pixel types constitutes the road scene map P. map , is represented as:

[0145]

[0146] In the formula, For pixel type S k Point cloud data, t1~t n For n consecutive data collection times.

[0147] As an alternative embodiment, continuous point cloud data of road boundary lines The calculation methods include:

[0148] Gaussian filtering and downsampling are applied to the road surface point cloud data to maintain the shape of the road surface point cloud and reduce the number of road surface point clouds.

[0149] The elevation coordinate components of the processed road surface point cloud are removed from its three-dimensional coordinates to obtain a two-dimensional planar point cloud. The convex hull of this two-dimensional planar point cloud is then detected using a two-dimensional convex hull method to obtain the closed road boundary line at each data acquisition moment. After removing the front and rear boundaries determined by the measurement range of the binocular camera 5, only the road boundary lines on both sides of the road are obtained.

[0150] For t1~t n Closed road boundary line at n consecutive acquisition times By overlaying the data, continuous point cloud data of the road boundary line is obtained.

[0151] As an optional embodiment, the method for calculating the real-time location coordinates of a vehicle based on a road scene map includes:

[0152] The video images output in real time from the binocular camera 5 are input into the trained semantic segmentation neural network to obtain the type and R, G, and B values ​​of each pixel. Pixels {i,j} are denoted as P. ij ={r,g,b,Sk};

[0153] Calculate the coordinates of each pixel in the coordinate system of the inertial measurement unit 3 according to equation (1). (1) λ ij Let {i,j} be the depth of the pixel points obtained by the binocular camera 5;

[0154] By integrating the real-time output data of inertial measurement unit 3 over a period of time, the coordinates at each moment relative to the start of the integration are obtained. position change T I and attitude angle change R I ;

[0155] The formula for calculating the coordinates of each pixel in the navigation coordinate system at n time points within a time interval Δt is as follows:

[0156]

[0157] In the formula, Let {i,j} be the coordinates of pixel {i,j} in the navigation coordinate system at n time points within Δt;

[0158] Using the ICP algorithm, with the current time T I R I Given the initial values ​​of the translation matrix T and the rotation value matrix R, iterative calculations are used to obtain T and R that minimize the optimization function, thereby achieving... P with road scene map point cloud map {q1,q2,…q n Alignment; the optimized function for iterative operations is:

[0159]

[0160] In the formula, E(R,T) is the value of the optimization function;

[0161] T is calculated based on the integral at the current time t. I R I Calculate the position coordinates T of the stereo camera 5 at the current time t. t and attitude angle R t The formula is:

[0162]

[0163] Based on T t R t And the field of view of the binocular camera 5, in the road scene map P map The matching region is divided into segments, and the ICP algorithm is used again, with T... t R tUsing the initial values, the optimal coordinate translation matrix T′ and rotation value matrix R′ that minimize the optimization function are obtained through iterative calculations;

[0164] The formula for calculating the current position and attitude angle is:

[0165]

[0166] In the formula, T t ′、R t ′ represents the current position and attitude angle, respectively.

[0167] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A positioning system based on road scenes, characterized in that, include: A computing unit installed on the vehicle, and a GNSS satellite positioning unit, an inertial measurement unit, a lidar, and a binocular camera connected to the computing unit; The computing unit is used to build a road scene map based on sensor data obtained during historical driving. When the satellite signal is weak or interrupted, real-time positioning is achieved by integrating the output data of the inertial measurement unit. The vehicle's position coordinate data is obtained by integrating the real-time output data of the inertial measurement unit over a period of time. The vehicle's real-time position coordinates are obtained by matching the position coordinate data obtained by integration with the road scene map. The location coordinate data obtained by integration is aligned with the point cloud data of the road scene map by employing two ICP algorithms; The sensor data includes: the vehicle's position coordinates expressed in longitude, latitude, and elevation obtained by the GNSS satellite positioning unit; the vehicle's attitude angle output by the inertial measurement unit; the laser point cloud of the road ahead output by the lidar during fan-shaped scanning; and the depth point cloud and RGB image of the road ahead output by the binocular camera.

2. The positioning system based on road scenes according to claim 1, characterized in that, The system also includes a synchronization controller. When the synchronization controller detects the rising edge of the pulse signal output by the GNSS satellite positioning unit, it simultaneously outputs four identical synchronization pulse signals to the inertial measurement unit, lidar, binocular camera, and computing unit, respectively, so that the inertial measurement unit, lidar, binocular camera, and computing unit work synchronously based on the rising edge of the synchronization pulse signal.

3. The positioning system based on road scenes according to claim 1, characterized in that, Methods for building road scene maps based on sensor data obtained during historical driving include: The RGB images output by the binocular cameras during historical driving are input into the semantic segmentation neural network to obtain the type and R, G, B values ​​of each pixel. Pixels {i,j} are denoted as P. ij ={r,g,b,S k }, where r, g, and b are the R, G, and B values ​​of pixel {i,j}, respectively, and S k There are 8 pixel types, denoted as {S}, where {i,j} is the pixel type. k } = {lane lines, road boundary lines, road markings, traffic lights, road signs, roadside buildings, poles, overpasses}, k = 1, 2, ..., 8; The laser point cloud from the lidar and the depth point cloud from the binocular camera are registered to the nearest location, and the depth data of the two point clouds are weighted and summed to obtain the true depth of each pixel. Calculate the coordinates of each pixel in the inertial measurement unit coordinate system. The formula is: In the formula, π C [R] C t C ] are the intrinsic and extrinsic parameter matrices of the stereo camera, respectively, R C t C λ represents the attitude angle and position deviation of the binocular camera relative to the inertial measurement unit. ij Let {i,j} be the actual depth of pixel point {i,j}; Calculate the coordinates of each pixel in the navigation coordinate system. The formula is: In the formula, R W T is the attitude angle measured by the inertial measurement unit. W Convert the longitude, latitude, and elevation data obtained from GNSS satellite positioning units into values ​​in the navigation coordinate system; Further processing is performed on the point cloud for each pixel type to obtain point cloud data for each pixel type. Continuous point cloud data is obtained by superimposing point cloud data from n consecutive acquisition times. The collection of continuous point cloud data of 8 pixel types constitutes the road scene map P. map , is represented as: In the formula, For pixel type S k Point cloud data, t1~t n For n consecutive data collection times.

4. The positioning system based on road scenes according to claim 3, characterized in that, Continuous point cloud data of road boundary lines The calculation methods include: Gaussian filtering and downsampling are applied to the road surface point cloud data to maintain the shape of the road surface point cloud and reduce the number of road surface point clouds. The elevation coordinate components of the processed road surface point cloud are removed from its three-dimensional coordinates to obtain a two-dimensional planar point cloud. The convex hull of this two-dimensional planar point cloud is then detected using a two-dimensional convex hull method to obtain the closed road boundary line at each data acquisition moment. After removing the front and rear boundaries determined by the measurement range of the binocular camera, only the road boundary lines on both sides of the road are obtained. For t1~t n Closed road boundary line at n consecutive acquisition times By overlaying the data, continuous point cloud data of the road boundary line is obtained.

5. The positioning system based on road scenes according to claim 3, characterized in that, Methods for calculating the real-time location coordinates of vehicles based on road scene maps include: The video images output in real time from the binocular camera are input into the trained semantic segmentation neural network to obtain the type and R, G, B values ​​of each pixel. Pixels {i,j} are denoted as P. ij ={r,g,b,S k }; Calculate the coordinates of each pixel in the inertial measurement unit coordinate system according to equation (1). (1) λ ij Let {i,j} be the depth of the pixel points obtained by the stereo camera; By integrating the real-time output data of the inertial measurement unit over a period of time, the coordinates at each moment relative to the starting moment of integration are obtained. position change T I and attitude angle change R I ; The formula for calculating the coordinates of each pixel in the navigation coordinate system at n time points within a time interval Δt is as follows: In the formula, Let {i,j} be the coordinates of pixel {i,j} in the navigation coordinate system at n time points within Δt; Using the ICP algorithm, with the current time T I R I Given the initial values ​​of the translation matrix T and the rotation value matrix R, iterative calculations are used to obtain T and R that minimize the optimization function, thereby achieving... P with road scene map point cloud map {q1,q2,L q n Alignment; the optimized function for iterative operations is: In the formula, E(R,T) is the value of the optimization function; T is calculated based on the integral at the current time t. I R I Calculate the position coordinates T of the stereo camera at the current time t. t and attitude angle R t The formula is: Based on T t R t And the field of view of the binocular camera, in the road scene map P map The matching region is divided into segments, and the ICP algorithm is used again, with T... t R t Using the initial values, the optimal coordinate translation matrix T' and rotation value matrix R' that minimize the optimization function are obtained through iterative calculations; The formula for calculating the current position and attitude angle is: In the formula, T t ′、R′ t These represent the current position and attitude angle, respectively.

6. A method for vehicle positioning using the system of claim 1, comprising the following steps performed in a computing unit: A road scene map is built based on sensor data obtained during historical driving. The sensor data includes: The vehicle's position coordinates are output by the GNSS satellite positioning unit, the vehicle's attitude angle is output by the inertial measurement unit, the laser point cloud is output by the lidar, and the depth point cloud and RGB image are output by the binocular camera. The vehicle's position coordinates are obtained by integrating the data output from the inertial measurement unit over a period of time, after acquiring sensor data in real time. The vehicle's position coordinates are calculated in real time by matching the location coordinates obtained through integration with the road scene map.

7. The method according to claim 6, characterized in that, The system also includes a synchronization controller. When the synchronization controller detects the rising edge of the pulse signal output by the GNSS satellite positioning unit, it simultaneously outputs four identical synchronization pulse signals to the inertial measurement unit, lidar, binocular camera, and computing unit, respectively, so that the inertial measurement unit, lidar, binocular camera, and computing unit work synchronously based on the rising edge of the synchronization pulse signal.

8. The method according to claim 6, characterized in that, Methods for building road scene maps based on sensor data obtained during historical driving include: The RGB images output by the binocular cameras during historical driving are input into the semantic segmentation neural network to obtain the type and R, G, B values ​​of each pixel. Pixels {i,j} are denoted as P. ij ={r,g,b,S k }, where r, g, and b are the R, G, and B values ​​of pixel {i,j}, respectively, and S k There are 8 pixel types, denoted as {S}, where {i,j} is the pixel type. k } = {lane lines, road boundary lines, road markings, traffic lights, road signs, roadside buildings, poles, overpasses}, k = 1, 2, ..., 8; The laser point cloud from the lidar is registered with the depth point cloud from the binocular camera, and the depth data of the two point clouds are weighted and summed to obtain the true depth of each pixel. Calculate the coordinates of each pixel in the inertial measurement unit coordinate system. The formula is: In the formula, π C [R] C t C ] are the intrinsic and extrinsic parameter matrices of the stereo camera, respectively, R C t C λ represents the attitude angle and position deviation of the binocular camera relative to the inertial measurement unit. ij Let {i,j} be the actual depth of pixel point {i,j}; Calculate the coordinates of each pixel in the navigation coordinate system. The formula is: In the formula, R W T is the attitude angle measured by the inertial measurement unit. W Convert the longitude, latitude, and elevation data obtained from GNSS satellite positioning units into values ​​in the navigation coordinate system; Further processing is performed on the point cloud for each pixel type to obtain point cloud data for each pixel type. Continuous point cloud data is obtained by superimposing point cloud data from n consecutive acquisition times. The collection of continuous point cloud data of 8 pixel types constitutes the road scene map P. map , is represented as: In the formula, For pixel type S k Point cloud data, t1~t n For n consecutive data collection times.

9. The method according to claim 8, characterized in that, Continuous point cloud data of road boundary lines The calculation methods include: Gaussian filtering and downsampling are applied to the road surface point cloud data to maintain the shape of the road surface point cloud and reduce the number of road surface point clouds. The elevation coordinate components of the processed road surface point cloud are removed from its three-dimensional coordinates to obtain a two-dimensional planar point cloud. The convex hull of this two-dimensional planar point cloud is then detected using a two-dimensional convex hull method to obtain the closed road boundary line at each data acquisition moment. After removing the front and rear boundaries determined by the measurement range of the binocular camera, only the road boundary lines on both sides of the road are obtained. For t1~t n Closed road boundary line at n consecutive acquisition times By overlaying the data, continuous point cloud data of the road boundary line is obtained.

10. The method according to claim 7, characterized in that, Methods for calculating the real-time location coordinates of vehicles based on road scene maps include: The video images output in real time from the binocular camera are input into the trained semantic segmentation neural network to obtain the type and R, G, B values ​​of each pixel. Pixels {i,j} are denoted as P. ij ={r,g,b,S k }; Calculate the coordinates of each pixel in the inertial measurement unit coordinate system according to equation (1). (1) λ ij Let {i,j} be the depth of the pixel points obtained by the stereo camera; By integrating the real-time output data of the inertial measurement unit over a period of time, the coordinates at each moment relative to the starting moment of integration are obtained. position change T I and attitude angle change R I ; The formula for calculating the coordinates of each pixel in the navigation coordinate system at n time points within a time interval Δt is as follows: In the formula, Let {i,j} be the coordinates of pixel {i,j} in the navigation coordinate system at n time points within Δt; Using the ICP algorithm, with the current time T I R I Given the initial values ​​of the translation matrix T and the rotation value matrix R, iterative calculations are used to obtain T and R that minimize the optimization function, thereby achieving... P with road scene map point cloud map {q1,q2,L q n Alignment; the optimized function for iterative operations is: In the formula, E(R,T) is the value of the optimization function; T is calculated based on the integral at the current time t. I R I Calculate the position coordinates T of the stereo camera at the current time t. t and attitude angle R t The formula is: Based on T t R t And the field of view of the binocular camera, in the road scene map P map The matching region is divided into segments, and the ICP algorithm is used again, with T... t R t Using the initial values, the optimal coordinate translation matrix T' and rotation value matrix R' that minimize the optimization function are obtained through iterative calculations; The formula for calculating the current position and attitude angle is: In the formula, T t ′、R′ t These represent the current position and attitude angle, respectively.

Images