Positioning system and method based on road scene

Abstract

The invention provides a positioning system and method based on a road scene. The system comprises a calculation unit, a GNSS satellite positioning unit, an inertial measurement unit, a laser radar and a binocular camera which are installed on a vehicle. The calculation unit is used for establishing a road scene map, performing integration on the real-time output data of the inertial measurement unit for a period of time to obtain the position coordinates of the vehicle, and performing matching on the position coordinates obtained through integration and the road scene map to calculate the real-time position coordinates of the vehicle; the sensor data comprises the position coordinates of the vehicle obtained by the GNSS satellite positioning unit, the attitude angle of the vehicle output by the inertial measurement unit, the laser point cloud of the front road output by the laser radar during sector scanning, and the depth point cloud and the RGB image of the front road output by the binocular camera. According to the invention, real-time positioning is realized by using the road scene map, and accurate positioning can be realized without depending on satellites and other reference equipment.

Description

Technical Field

[0001] The present invention belongs to the technical field of navigation and positioning, and in particular relates to a positioning system and method based on road scenes. Background Art

[0002] Accurate and stable positioning and attitude measurement are critical for vehicles to complete specific tasks, such as special vehicle automatic driving, lane keeping, high-speed cruising, etc. At present, high-precision positioning of vehicles is mainly achieved by relying on sensors such as GNSS, high-precision IMU, odometers and their combination, but the current positioning system and its method still have major defects. For example, when the GNSS signal is shielded by a tunnel or it is interfered with, the global positioning information will be invalid. Although sensors such as IMU and odometer can meet the short-term precise attitude measurement, due to their inherent zero bias, scale factor and other errors, the positioning attitude measurement error diverges over time and cannot meet the long-term stable navigation needs.

[0003] In view of this, the present invention proposes a road scene-based positioning system and method, which relies on vehicle-mounted image sensors to parse the semantic features of road scenes and match them with the semantic maps established during historical driving processes to achieve high-precision and stable positioning results. Summary of the invention

[0004] In order to solve the above problems existing in the prior art, the present invention provides a positioning system and method based on road scenes, which can effectively solve the problems of unstable vehicle positioning information and error divergence over time.

[0005] In order to achieve the above object, the present invention adopts the following technical solutions.

[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 laser radar and a binocular camera connected to the computing unit; the computing unit is used to establish a road scene map based on sensor data obtained during historical driving, obtain the position coordinates of the vehicle by integrating the real-time output data of the inertial measurement unit for a period of time, and calculate the real-time position coordinates of the vehicle based on the position coordinates obtained by the integration and the road scene map by matching; the sensor data includes: the position coordinates of the vehicle expressed in longitude, latitude and elevation obtained by the GNSS satellite positioning unit, the attitude angle of the vehicle output by the inertial measurement unit, the laser point cloud of the road ahead output by the laser radar when performing a sector scan, 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 4 identical synchronization pulse signals and sends them 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, the method for establishing a road scene map based on sensor data obtained during historical driving includes:

[0009] The RGB image output by the binocular camera during the historical driving process is input into the semantic segmentation neural network to obtain the type and R, G, B values ​​of each pixel. The pixel {i, j} is recorded as P ij = {r,g,b,S k}, where r, g, and b are the R, G, and B values ​​of the pixel {i, j} respectively, and S k is the type of pixel {i,j}, there are 8 pixel types in total, represented by {S k} = {lane line, road boundary line, road surface marking, traffic light, road sign, roadside building, pole, overpass}, k = 1, 2, ..., 8;

[0010] The laser point cloud of the laser radar and the depth point cloud of the binocular camera are aligned, 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 internal parameter matrix and external parameter matrix of the binocular camera, R C ,t C represents the attitude angle deviation and position deviation of the binocular camera relative to the inertial measurement unit, λ ij is the true depth of pixel {i,j};

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

[0015]

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

[0017] Further process the point cloud of each pixel type to obtain the point cloud data of each pixel type. Superimpose the point cloud data of n consecutive acquisition moments to obtain continuous point cloud data The collection of continuous point cloud data of 8 pixel types is the road scene map P map , expressed as:

[0018]

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

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

[0021] Perform Gaussian filtering on the road point cloud data and perform downsampling processing to keep the road point cloud shape unchanged and reduce the number of road point clouds;

[0022] The elevation coordinate component is removed from the processed three-dimensional coordinates of the road surface point cloud to obtain a two-dimensional plane point cloud. The convex hull of the two-dimensional plane point cloud is detected using a two-dimensional convex hull to obtain the closed road boundary line at each data collection moment. After eliminating the front boundary and the rear boundary determined by the measurement range of the binocular camera, the road boundary line including only the boundaries on both sides of the road is obtained.

[0023] For t1~t n Closed road boundary lines at n consecutive acquisition moments Overlay to obtain continuous point cloud data of road boundary lines

[0024] Furthermore, the method for calculating the real-time position coordinates of the vehicle based on the road scene map includes:

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

[0026] Calculate the coordinates of each pixel in the inertial measurement unit coordinate system according to formula (1): (1) ij is the depth of pixel {i,j} obtained by the binocular camera;

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

[0028] Calculate the coordinates of each pixel point in the navigation coordinate system at n moments within a period of time Δt. The formula is:

[0029]

[0030] In the formula, is the coordinate of the pixel point {i, j} in the navigation coordinate system at n moments within Δt;

[0031] Using the ICP algorithm, the current time T I , R I is the initial value of the translation matrix T and the rotation value matrix R. Through iterative calculation, T and R that minimize the optimization function are obtained, so that With the road scene map point cloud P map {q1,q2,…q n}Alignment; the optimization function of iterative operation is:

[0032]

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

[0034] T calculated based on the current time t I , R I , calculate the position coordinates T of the binocular 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 stereo camera, in the road scene map P map The matching area is divided in the , and the ICP algorithm is used again with T t , R t is the initial value, and the optimal coordinate translation matrix T′ and rotation value matrix R′ that minimize the optimization function are obtained through iterative calculation;

[0037] Calculate the current position and attitude angle using the following formula:

[0038]

[0039] Where, T t ′、R t ′ are the current position and attitude angle respectively.

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

[0041] Building a road scene map based on sensor data obtained during historical driving, the sensor data includes: vehicle position coordinates output by a GNSS satellite positioning unit, vehicle attitude angles output by an inertial measurement unit, laser point clouds output by a lidar, and depth point clouds and RGB images output by a binocular camera;

[0042] Acquire sensor data in real time and obtain the vehicle's position coordinates by integrating the inertial measurement unit output data over a period of time;

[0043] The position coordinates of the vehicle are calculated in real time based on matching the position 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 4 identical synchronization pulse signals and sends them 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, the method for establishing a road scene map based on sensor data obtained during historical driving includes:

[0046] The RGB image output by the binocular camera during the historical driving process is input into the semantic segmentation neural network to obtain the type and R, G, B values ​​of each pixel. The pixel {i, j} is recorded as P ij = {r,g,b,S k}, where r, g, and b are the R, G, and B values ​​of the pixel {i, j} respectively, and S k is the type of pixel {i,j}, there are 8 pixel types in total, represented by {S k} = {lane line, road boundary line, road surface marking, traffic light, road sign, roadside building, pole, overpass}, k = 1, 2, ..., 8;

[0047] The laser point cloud of the laser radar and the depth point cloud of the binocular camera are aligned, 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 internal parameter matrix and external parameter matrix of the binocular camera, R C ,t C represents the attitude angle deviation and position deviation of the binocular camera relative to the inertial measurement unit, λ ij is the true depth of pixel {i,j};

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

[0052]

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

[0054] Further process the point cloud of each pixel type to obtain the point cloud data of each pixel type. Superimpose the point cloud data of n consecutive acquisition moments to obtain continuous point cloud data The collection of continuous point cloud data of 8 pixel types is the road scene map P map , expressed as:

[0055]

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

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

[0058] The laser point cloud of the laser radar and the depth point cloud of the binocular camera are aligned, 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 component is removed from the processed three-dimensional coordinates of the road surface point cloud to obtain a two-dimensional plane point cloud. The convex hull of the two-dimensional plane point cloud is detected using a two-dimensional convex hull to obtain the closed road boundary line at each data collection moment. After eliminating the front boundary and the rear boundary determined by the measurement range of the binocular camera, the road boundary line including only the boundaries on both sides of the road is obtained.

[0060] For t1~t n Closed road boundary lines at n consecutive acquisition moments Overlay to obtain continuous point cloud data of road boundary lines

[0061] Furthermore, the method for calculating the real-time position coordinates of the vehicle based on the road scene map includes:

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

[0063] Calculate the coordinates of each pixel in the inertial measurement unit coordinate system according to formula (1): (1) ij is the depth of pixel {i,j} obtained by the binocular camera;

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

[0065] Calculate the coordinates of each pixel point in the navigation coordinate system at n moments within a period of time Δt. The formula is:

[0066]

[0067] In the formula, is the coordinate of the pixel point {i, j} in the navigation coordinate system at n moments within Δt;

[0068] Using the ICP algorithm, the current time T I , R I is the initial value of the translation matrix T and the rotation value matrix R. Through iterative calculation, T and R that minimize the optimization function are obtained, so that With the road scene map point cloud P map {q1,q2,…q n}Alignment; the optimization function of iterative operation is:

[0069]

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

[0071] T calculated based on the current time t I , R I , calculate the position coordinates T of the binocular 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 stereo camera, in the road scene map P map The matching area is divided in the , and the ICP algorithm is used again with T t , R t is the initial value, and the optimal coordinate translation matrix T′ and rotation value matrix R′ that minimize the optimization function are obtained through iterative calculation;

[0074] Calculate the current position and attitude angle using the following formula:

[0075]

[0076] Where, T t ′、R t ′ are the current position and attitude angle respectively.

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

[0078] The present invention realizes real-time positioning based on road scenes by setting up a computing unit installed on a vehicle and a GNSS satellite positioning unit, an inertial measurement unit, a laser radar and a binocular camera connected to the computing unit. The computing unit is used to establish a road scene map based on sensor data obtained during historical driving, and the position coordinates of the vehicle are obtained by integrating the real-time output data of the inertial measurement unit for a period of time, and the real-time position coordinates of the vehicle are calculated based on the position coordinates obtained by integration and the road scene map. The present invention realizes real-time positioning based on road scenes. The present invention realizes real-time positioning by using a road scene map, and can realize accurate positioning without relying on satellites and other reference devices, solving the problems of poor positioning accuracy and difficulty in maintaining positioning under satellite signal denial. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0080] In the figure: 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 system hardware installation structure.

[0082] Figure 3 The present invention is a flowchart of a method for positioning a vehicle using the system according to an embodiment of the present invention. DETAILED DESCRIPTION

[0083] In order to make the purpose, technical solution and advantages of the present invention clearer and more understandable, the present invention is further described below in conjunction with the accompanying drawings and specific implementation methods. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0084] Figure 1 The present invention is a block diagram of a positioning system based on a road scene according to an embodiment of the present invention, comprising: a computing unit 1 installed on a vehicle and a GNSS satellite positioning unit 2, an inertial measurement unit 3, a laser radar 4 and a binocular camera 5 connected to the computing unit 1; the computing unit 1 is used to establish a road scene map based on sensor data obtained during historical driving, obtain the position coordinates of the vehicle by integrating the real-time output data of the inertial measurement unit 3 for a period of time, and calculate the real-time position coordinates of the vehicle based on the position coordinates obtained by the integration and the road scene map by matching; the sensor data comprises: the position coordinates of the vehicle expressed in longitude, latitude and elevation obtained by the GNSS satellite positioning unit 2, the attitude angle of the vehicle output by the inertial measurement unit 3, the laser point cloud of the road ahead output by the laser radar 4 when performing a sector scan, 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 is mainly composed of a computing unit 1, a GNSS satellite positioning unit 2, an inertial measurement unit 3, a laser radar 4 and a binocular camera 5. The hardware installation structure diagram of the system is shown in FIG. Figure 2As shown. In the system, except for the inertial measurement unit 3, all other components are installed on the bracket by screws, wherein 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 by screws. The binocular camera 5 is installed at the front of the bracket and extends a certain distance from the bracket through the L-shaped connection device. The measurement direction of the binocular camera 5 is horizontal and forward. The computing unit 1 and the laser radar 4 are installed inside the cavity of the upper half of the bracket, wherein the computing unit 1 is completely enclosed in the cavity, and the laser radar 4 is installed in the front and upper part of the bracket. The sensing part of the laser radar 4 extends out of the cavity to scan the target in front. The bracket is made of aluminum alloy, and a piece of cast iron is installed on the upper surface of the cavity by screws. The GNSS satellite positioning unit 2 is adsorbed on the cast iron by magnets. A square wiring hole is set at the rear of the bracket cavity to lead out the cables inside the space. The 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 laser radar 4 and the binocular camera 5 respectively, and the laser radar 4, the inertial measurement unit 3 and the binocular camera 5 transmit the perception information to the computing unit 1 through the communication interface. The GNSS satellite positioning unit 2 generally adopts the GPS or Beidou system, which is used to obtain the position coordinates of the vehicle in real time, including longitude, latitude and elevation data. The inertial measurement unit 3 is used to output the posture data of the vehicle, including azimuth, pitch angle and roll angle. The laser radar 4 is used to output the laser point cloud of the road ahead scanned by itself. The binocular camera 5 is used to output 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, which is used to coordinate the work of each unit and complete the necessary data processing tasks. The data processing tasks completed by the computing unit 1 mainly include: establishing a road scene map based on the sensor data obtained during the historical driving process; when the satellite signal is not ideal (for example, there is an obstruction), the GNSS satellite positioning unit 2 cannot output reliable vehicle positioning data, and 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 after a long time, the positioning accuracy decreases 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 matching them with the road scene map.

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

[0088] As an optional embodiment, 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 2, it simultaneously outputs 4 identical synchronization pulse signals and sends them 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 realizing the synchronous operation of the system. In order to realize the synchronous operation of each unit, this embodiment sets a synchronous controller connected to the inertial measurement unit 3, the laser radar 4, the binocular camera 5 and the computing unit 1. The installation position of the synchronous controller is as follows: Figure 2 As shown, the synchronization controller is installed together with the computing unit 1 and the laser radar 4 inside the cavity of the upper part of the bracket. The system realizes synchronous 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 collects the rising edge of the narrow pulse, it synchronously outputs 4 narrow pulses of 1Hz, which are respectively given to the laser radar 4, the inertial measurement unit 3, the binocular camera 5, and the computing unit 1. After the rising edge of the pulse is detected inside each unit, its internal timer is calibrated to realize time synchronization between each single machine.

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

[0091] The RGB image output by the binocular camera 5 during the historical driving process is input into the semantic segmentation neural network to obtain the type and R, G, B values ​​of each pixel point. The pixel point {i, j} is recorded as P ij = {r,g,b,S k}, where r, g, and b are the R, G, and B values ​​of the pixel {i, j} respectively, and S k is the type of pixel {i,j}, there are 8 pixel types in total, represented by {S k} = {lane line, road boundary line, road surface marking, traffic light, road sign, roadside building, pole, overpass}, k = 1, 2, ..., 8;

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

[0093] Calculate the coordinates of each pixel in the IMU 3 coordinate system The formula is:

[0094]

[0095] In the formula, π C , [R C t C ] are the internal parameter matrix and external parameter matrix of the binocular camera 5, R C ,t C represents the attitude angle deviation and position deviation of the binocular camera 5 relative to the inertial measurement unit 3, λ ij is the true depth of pixel {i,j};

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

[0097]

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

[0099] Further process the point cloud of each pixel type to obtain the point cloud data of each pixel type. Superimpose the point cloud data of n consecutive acquisition moments to obtain continuous point cloud data The collection of continuous point cloud data of 8 pixel types is the road scene map P map , expressed as:

[0100]

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

[0102] This embodiment provides a technical solution for establishing a road scene map. The road scene map is a collection of position coordinate data of pixel points that constitute lane lines and road boundary lines. Since coordinate conversion between multiple different coordinate systems is involved when calculating the position coordinates of pixel points, before introducing the technical solution for establishing a road scene map, the various coordinate systems involved in this embodiment are introduced. Navigation coordinate system O1X1Y1Z1: The coordinate origin is selected as a point on the ground, the X1 axis points to the east, the Y1 axis points to the north, and the Z1 axis is defined according to the right-hand rule. Inertial measurement unit coordinate system O2X2Y2Z2: The coordinate origin is at the geometric center of the inertial measurement unit 3, the X2 axis points to the front of the vehicle (also the 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 coordinate origin is defined at the geometric center of the camera, the X3 axis points to the front of the camera field of view, the Z3 axis points upward in the longitudinal plane of the camera, and Y3 is defined according to the right-hand rule. It also involves the lidar coordinate system and image coordinate system, which will not be introduced one by one.

[0103] This embodiment establishes a road scene map based on the image data of the road obtained during the historical driving process. First, the RGB image output by the binocular camera 5 during the historical driving process is input into the trained semantic segmentation neural network, and the type and R, G, B values ​​of each pixel are output. There are 8 types of pixel points in this embodiment, namely lane lines, road boundary lines, road surface markings, traffic lights, road signs, roadside buildings, poles and overpasses. The pixel point with coordinates {i, j} is denoted as P ij = {r,g,b,S k}, where r, g, and b are the R, G, and B values ​​of the pixel {i, j} respectively, and S k is the type of pixel point. Then, the laser point cloud of the road ahead obtained by the laser radar 4 is registered with 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 weightedly sum the depth of the two registered point clouds, i.e., the distance data, to obtain high-precision depth data for each pixel point, which is called the true depth. Then, the coordinates of each pixel point in the coordinate system of the inertial measurement unit 3 are calculated according to formula (1). According to formula (2), Convert to the coordinates of the navigation coordinate system (2) R in the formula W is the attitude angle measured by the inertial measurement unit 3, T W The longitude, latitude and altitude [lon, lat, alt] obtained by the GNSS satellite positioning unit 2 are converted into coordinate values ​​in the navigation coordinate system. The conversion formula is:

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

[0105] Where [lon0, lat0, alt0] is the longitude, latitude and altitude of the origin of the navigation coordinate system, and r is the radius of the earth.

[0106] Next, the point cloud data of each pixel type is obtained by further processing the point cloud of each pixel type. Then, continuous point cloud data is obtained by superimposing the point cloud data of n consecutive acquisition moments. The set of continuous point cloud data of 8 pixel types is the road scene map P map , which can be expressed as the expression shown in formula (3). It is worth noting that the most important of the 8 types of point cloud data in formula (3) are the first two types of point cloud data, 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 of lane lines and road boundary lines is also different from that of other point cloud data. When processing lane lines and road boundary lines, the boundaries at both ends are removed.

[0107] As an optional embodiment, the continuous point cloud data of the road boundary line The calculation methods include:

[0108] Perform Gaussian filtering on the road point cloud data and perform downsampling processing to keep the road point cloud shape unchanged and reduce the number of road point clouds;

[0109] The elevation coordinate component is removed from the three-dimensional coordinates of the processed point cloud to obtain a two-dimensional plane point cloud. The convex hull of the two-dimensional plane point cloud is detected using a two-dimensional convex hull to obtain the closed road boundary line at each data collection moment. After eliminating the front boundary and the rear boundary determined by the measurement range of the binocular camera 5, the road boundary line including only the boundaries on both sides of the road is obtained.

[0110] For t1~t n Closed road boundary lines at n consecutive acquisition moments Overlay to obtain continuous point cloud data of road boundary lines

[0111] This embodiment provides a technical solution for obtaining continuous point cloud data of road boundary lines. First, the point cloud data is preprocessed: the three-dimensional semantic point cloud of the road surface is downsampled using Gaussian filtering to keep the point cloud morphology unchanged and reduce the amount of point cloud data; then the road boundary is extracted: a plane assumption is used for road boundary extraction, the height component of the three-dimensional coordinates of the point cloud is discarded, and it is converted into a two-dimensional plane point cloud. The convex hull of the two-dimensional plane point cloud is detected using the two-dimensional convex hull to obtain the closed boundary of the road surface area. The closed boundary includes the front boundary and the rear boundary, which are located at the proximal and distal ends of the effective range of the camera measurement defined in the camera coordinate system. The coordinates are converted to the navigation coordinate system through the coordinate transformation matrix, and the closed boundary points that are less than the proximal value or greater than the distal value along the direction of the vehicle body are eliminated, and the boundary point clouds on both sides of the remaining road are obtained. Finally, the closed road boundary lines at n consecutive acquisition moments are analyzed. Overlay to obtain continuous point cloud data of road boundary lines

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

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

[0114] Calculate the coordinates of each pixel in the inertial measurement unit 3 coordinate system according to formula (1): (1) ij is the depth of pixel {i, j} obtained by binocular camera 5;

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

[0116] Calculate the coordinates of each pixel point in the navigation coordinate system at n moments within a period of time Δt. The formula is:

[0117]

[0118] In the formula, is the coordinate of the pixel point {i, j} in the navigation coordinate system at n moments within Δt;

[0119] Using the ICP algorithm, the current time T I , R Iis the initial value of the translation matrix T and the rotation value matrix R. Through iterative calculation, T and R that minimize the optimization function are obtained, so that With the road scene map point cloud P map {q1,q2,…q n}Alignment; the optimization function of iterative operation is:

[0120]

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

[0122] T calculated based on the current time t I , R I , calculate the position coordinates T of the binocular 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 area is divided in the , and the ICP algorithm is used again with T t , R t is the initial value, and the optimal coordinate translation matrix T′ and rotation value matrix R′ that minimize the optimization function are obtained through iterative calculation;

[0125] Calculate the current position and attitude angle using the following formula:

[0126]

[0127] Where, T t ′、R t ′ are the current position and attitude angle respectively.

[0128] This embodiment provides a technical solution for calculating the real-time position coordinates of a vehicle based on a road scene map. First, the video image output by the binocular camera 5 in real time is input into the trained semantic segmentation neural network to obtain the type and R, G, B values ​​of each pixel point, and the pixel point {i, j} is recorded as P ij = {r,g,b,S k}. Then, the coordinates of each pixel in the inertial measurement unit 3 coordinate system are calculated according to formula (1): It is worth noting that λ in formula (1) ijIt is the depth of the pixel {i, j} obtained by the binocular camera 5, rather than the real depth obtained by the fusion of the depth data obtained by the binocular camera 5 and the laser radar 4. That is to say, the laser radar 4 may not work during the real-time positioning process, and the computing center does not need to process the laser point cloud data output by the laser radar 4, which can improve the data processing speed to better realize the real-time positioning calculation. When the satellite signal is normal, the system realizes real-time positioning based on the output data of the GNSS satellite positioning unit 2; when the satellite signal is weak or interrupted, the prior art generally realizes real-time positioning by integrating the output data of the inertial measurement unit 3. Since there is a cumulative error in the integration, the longer the time, the greater the cumulative error, so this method cannot be used for high-precision real-time positioning for a long time. To this end, this embodiment integrates the output data of the inertial measurement unit 3 in a short period of time to obtain the position coordinate data of the vehicle, and uses the position coordinate data obtained by integration to match with the road scene map to obtain the high-precision position coordinates of the vehicle, thereby improving the real-time positioning accuracy of the vehicle. This embodiment adopts the ICP (iterative closest point) algorithm twice to realize the alignment of the position coordinate data obtained by integration with the point cloud data of the road scene map. Through the first ICP algorithm, the matching area can be divided from the road scene map, and the preliminary coordinate translation matrix T and rotation value matrix R can be obtained; through the second ICP algorithm, the optimal coordinate translation matrix T′ and rotation value matrix R′ can be obtained. Based on T′ and R′, the precise position and attitude angle at the current moment can be calculated according to formula (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 match wrong, so the first matching is needed to find a low-precision but highly reliable result for the second matching.

[0129] Figure 3 This is a flow chart of a method for using the system to locate a vehicle according to an embodiment of the present invention, comprising the following steps:

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

[0131] Step 102, acquiring sensor data in real time, and obtaining the position coordinates of the vehicle by integrating the output data of the inertial measurement unit 3 for a period of time;

[0132] Step 103, based on matching the position coordinates obtained by integration with the road scene map, the position coordinates of the vehicle are calculated in real time.

[0133] The method of this embodiment, Figure 1 Compared with the technical solution of the device embodiment shown in the figure, its implementation principle and technical effect are similar, which will not be described in detail here. The same is true for the following embodiments, which will not be described in detail.

[0134] As an optional embodiment, 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 2, it simultaneously outputs 4 identical synchronization pulse signals and sends them 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, a method for establishing a road scene map based on sensor data obtained during historical driving includes:

[0136] The RGB image output by the binocular camera 5 during the historical driving process is input into the semantic segmentation neural network to obtain the type and R, G, B values ​​of each pixel point. The pixel point {i, j} is recorded as P ij = {r,g,b,S k}, where r, g, and b are the R, G, and B values ​​of the pixel {i, j} respectively, and S k is the type of pixel {i,j}, there are 8 pixel types in total, represented by {S k} = {lane line, road boundary line, road surface marking, traffic light, road sign, roadside building, pole, overpass}, k = 1, 2, ..., 8;

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

[0138] Calculate the coordinates of each pixel in the IMU 3 coordinate system The formula is:

[0139]

[0140] In the formula, π C , [R C t C ] are the internal parameter matrix and external parameter matrix of the binocular camera 5, R C ,t C represents the attitude angle deviation and position deviation of the binocular camera 5 relative to the inertial measurement unit 3, λ ij is the true depth of pixel {i,j};

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

[0142]

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

[0144] Further process the point cloud of each pixel type to obtain the point cloud data of each pixel type. Superimpose the point cloud data of n consecutive acquisition moments to obtain continuous point cloud data The collection of continuous point cloud data of 8 pixel types is the road scene map P map , expressed as:

[0145]

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

[0147] As an optional embodiment, the continuous point cloud data of the road boundary line The calculation methods include:

[0148] Perform Gaussian filtering on the road point cloud data and perform downsampling processing to keep the road point cloud shape unchanged and reduce the number of road point clouds;

[0149] The elevation coordinate component is removed from the three-dimensional coordinates of the processed road surface point cloud to obtain a two-dimensional plane point cloud. The convex hull of the two-dimensional plane point cloud is detected using a two-dimensional convex hull to obtain the closed road boundary line at each data collection moment. After eliminating the front boundary and the rear boundary determined by the measurement range of the binocular camera 5, the road boundary line including only the boundaries on both sides of the road is obtained.

[0150] For t1~t n Closed road boundary lines at n consecutive acquisition moments Overlay to obtain continuous point cloud data of road boundary lines

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

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

[0153] Calculate the coordinates of each pixel in the inertial measurement unit 3 coordinate system according to formula (1): (1) ij is the depth of pixel {i, j} obtained by binocular camera 5;

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

[0155] Calculate the coordinates of each pixel point in the navigation coordinate system at n moments within a period of time Δt. The formula is:

[0156]

[0157] In the formula, is the coordinate of the pixel point {i, j} in the navigation coordinate system at n moments within Δt;

[0158] Using the ICP algorithm, the current time T I , R I is the initial value of the translation matrix T and the rotation value matrix R. Through iterative calculation, T and R that minimize the optimization function are obtained, so that With the road scene map point cloud P map {q1,q2,…q n}Alignment; the optimization function of iterative operation is:

[0159]

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

[0161] T calculated based on the current time t I , R I , calculate the position coordinates T of the binocular 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 area is divided in the , and the ICP algorithm is used again with T t , R tis the initial value, and the optimal coordinate translation matrix T′ and rotation value matrix R′ that minimize the optimization function are obtained through iterative calculation;

[0164] Calculate the current position and attitude angle using the following formula:

[0165]

[0166] Where, T t ′、R t ′ are the current position and attitude angle respectively.

[0167] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by a person skilled in the art within the technical scope disclosed by the present invention should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection 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 laser radar and a binocular camera connected to the computing unit; The calculation unit is used to establish a road scene map based on sensor data obtained during historical driving, obtain the vehicle's position coordinates by integrating the real-time output data of the inertial measurement unit for a period of time, and calculate the vehicle's real-time position coordinates based on the position coordinates obtained by integration and 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 laser radar when performing a sector scan, and the depth point cloud and RGB image of the road ahead output by the binocular camera.

2. The road scene-based positioning system 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 and sends them to the inertial measurement unit, the laser radar, the binocular camera and the computing unit respectively, so that the inertial measurement unit, the laser radar, the binocular camera and the computing unit work synchronously based on the rising edge of the synchronization pulse signal.

3. The road scene-based positioning system according to claim 1, characterized in that: The method of establishing a road scene map based on sensor data obtained during historical driving includes: The RGB image output by the binocular camera during the historical driving process is input into the semantic segmentation neural network to obtain the type and R, G, B values ​​of each pixel. The pixel {i, j} is recorded as P ij = {r,g,b,S k }, where r, g, and b are the R, G, and B values ​​of the pixel {i, j} respectively, and S k is the type of pixel {i,j}, there are 8 pixel types in total, represented by {S k } = {lane line, road boundary line, road surface marking, traffic light, road sign, roadside building, pole, overpass}, k = 1, 2, ..., 8; The laser point cloud of the laser radar and the depth point cloud of the binocular camera are aligned, and the depth data of the two point clouds are weighted and summed to obtain the true depth of each pixel; Calculate the coordinates P of each pixel in the inertial measurement unit coordinate system ij C , the formula is: In the formula, π C , [R C t C ] are the internal parameter matrix and external parameter matrix of the binocular camera, R C ,t C represents the attitude angle deviation and position deviation of the binocular camera relative to the inertial measurement unit, λ ij is the true depth of pixel {i,j}; Calculate the coordinates of each pixel in the navigation coordinate system The formula is: In the formula, R W is the attitude angle measured by the inertial measurement unit, T W The longitude, latitude and elevation data obtained by the GNSS satellite positioning unit are converted into values ​​in the navigation coordinate system; Further process the point cloud of each pixel type to obtain the point cloud data of each pixel type. Superimpose the point cloud data of n consecutive acquisition moments to obtain continuous point cloud data The collection of continuous point cloud data of 8 pixel types is the road scene map P map , expressed as: In the formula, The pixel type is S k Point cloud data, t1~t n is n consecutive collection moments.

4. The road scene-based positioning system according to claim 3, characterized in that: Continuous point cloud data of road boundary lines The calculation methods include: Perform Gaussian filtering on the road point cloud data and perform downsampling processing to keep the road point cloud shape unchanged and reduce the number of road point clouds; The elevation coordinate component is removed from the processed three-dimensional coordinates of the road surface point cloud to obtain a two-dimensional plane point cloud. The convex hull of the two-dimensional plane point cloud is detected using a two-dimensional convex hull to obtain the closed road boundary line at each data collection moment. After eliminating the front boundary and the rear boundary determined by the measurement range of the binocular camera, the road boundary line including only the boundaries on both sides of the road is obtained. For t1~t n Closed road boundary lines at n consecutive acquisition moments Overlay to obtain continuous point cloud data of road boundary lines 5. The road scene-based positioning system according to claim 3, characterized in that: The method for calculating the real-time position coordinates of a vehicle based on a road scene map includes: The video image output by the binocular camera in real time is input into the trained semantic segmentation neural network to obtain the type and R, G, B values ​​of each pixel. The pixel {i, j} is recorded as P ij = {r,g,b,S k }; Calculate the coordinates of each pixel in the inertial measurement unit coordinate system according to formula (1): (1) ij is the depth of pixel {i,j} obtained by the binocular camera; By integrating the real-time output data of the inertial measurement unit for a period of time, the coordinates of each moment relative to the start time of integration are obtained. The position change T I and attitude angle change R I ; Calculate the coordinates of each pixel point in the navigation coordinate system at n moments within a period of time Δt. The formula is: In the formula, is the coordinate of the pixel point {i, j} in the navigation coordinate system at n moments within Δt; Using the ICP algorithm, the current time T I , R I is the initial value of the translation matrix T and the rotation value matrix R. Through iterative calculation, T and R that minimize the optimization function are obtained, so that With the road scene map point cloud P map {q1,q2,…q n }Alignment; the optimization function of iterative operation is: Where E(R,T) is the value of the optimization function; T calculated based on the current time t I , R I , calculate the position coordinates T of the binocular 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 stereo camera, in the road scene map P map The matching area is divided in the , and the ICP algorithm is used again with T t , R t is the initial value, and the optimal coordinate translation matrix T′ and rotation value matrix R′ that minimize the optimization function are obtained through iterative calculation; Calculate the current position and attitude angle using the following formula: Where, T t ′、R t ′ are the current position and attitude angle respectively.

6. A method for vehicle positioning using the system of claim 1, comprising the following steps performed by a computing unit: A road scene map is established based on sensor data obtained during historical driving, wherein 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; Acquire sensor data in real time and obtain the vehicle's position coordinates by integrating the inertial measurement unit output data over a period of time; The position coordinates of the vehicle are calculated in real time based on matching the position 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 and sends them to the inertial measurement unit, the laser radar, the binocular camera and the computing unit respectively, so that the inertial measurement unit, the laser radar, the binocular camera and the computing unit work synchronously based on the rising edge of the synchronization pulse signal.

8. The method according to claim 6, characterized in that The method of establishing a road scene map based on sensor data obtained during historical driving includes: The RGB image output by the binocular camera during the historical driving process is input into the semantic segmentation neural network to obtain the type and R, G, B values ​​of each pixel. The pixel {i, j} is recorded as P ij = {r,g,b,S k }, where r, g, and b are the R, G, and B values ​​of the pixel {i, j} respectively, and S k is the type of pixel {i,j}, there are 8 pixel types in total, represented by {S k } = {lane line, road boundary line, road surface marking, traffic light, road sign, roadside building, pole, overpass}, k = 1, 2, ..., 8; The laser point cloud of the lidar is registered with the depth point cloud of 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 internal parameter matrix and external parameter matrix of the binocular camera, R C ,t C represents the attitude angle deviation and position deviation of the binocular camera relative to the inertial measurement unit, λ ij is the true depth of pixel {i,j}; Calculate the coordinates of each pixel in the navigation coordinate system The formula is: In the formula, R W is the attitude angle measured by the inertial measurement unit, T W The longitude, latitude and elevation data obtained by the GNSS satellite positioning unit are converted into values ​​in the navigation coordinate system; Further process the point cloud of each pixel type to obtain the point cloud data of each pixel type. Superimpose the point cloud data of n consecutive acquisition moments to obtain continuous point cloud data The collection of continuous point cloud data of 8 pixel types is the road scene map P map , expressed as: In the formula, The pixel type is S k Point cloud data, t1~t n is n consecutive collection moments.

9. The method according to claim 8, characterized in that Continuous point cloud data of road boundary lines The calculation methods include: Perform Gaussian filtering on the road point cloud data and perform downsampling processing to keep the road point cloud shape unchanged and reduce the number of road point clouds; The elevation coordinate component is removed from the processed three-dimensional coordinates of the road surface point cloud to obtain a two-dimensional plane point cloud. The convex hull of the two-dimensional plane point cloud is detected using a two-dimensional convex hull to obtain the closed road boundary line at each data collection moment. After eliminating the front boundary and the rear boundary determined by the measurement range of the binocular camera, the road boundary line including only the boundaries on both sides of the road is obtained. For t1~t n Closed road boundary lines at n consecutive acquisition moments Overlay to obtain continuous point cloud data of road boundary lines 10. The method according to claim 7, characterized in that The method for calculating the real-time position coordinates of a vehicle based on a road scene map includes: The video image output by the binocular camera in real time is input into the trained semantic segmentation neural network to obtain the type and R, G, B values ​​of each pixel. The pixel {i, j} is recorded as P ij = {r,g,b,S k }; Calculate the coordinates of each pixel in the inertial measurement unit coordinate system according to formula (1): (1) ij is the depth of pixel {i,j} obtained by the binocular camera; By integrating the real-time output data of the inertial measurement unit for a period of time, the coordinates of each moment relative to the start time of integration are obtained. The position change T I and attitude angle change R I ; Calculate the coordinates of each pixel point in the navigation coordinate system at n moments within a period of time Δt. The formula is: In the formula, is the coordinate of the pixel point {i, j} in the navigation coordinate system at n moments within Δt; Using the ICP algorithm, the current time T I , R I is the initial value of the translation matrix T and the rotation value matrix R. Through iterative calculation, T and R that minimize the optimization function are obtained, so that With the road scene map point cloud P map {q1,q2,…q n }Alignment; the optimization function of iterative operation is: Where E(R,T) is the value of the optimization function; T calculated based on the current time t I , R I , calculate the position coordinates T of the binocular 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 stereo camera, in the road scene map P map The matching area is divided in the , and the ICP algorithm is used again with T t , R t is the initial value, and the optimal coordinate translation matrix T′ and rotation value matrix R′ that minimize the optimization function are obtained through iterative calculation; Calculate the current position and attitude angle using the following formula: Where, T t ′、R t ′ are the current position and attitude angle respectively.

Images