WO2020135183A1 - 点云地图的构建方法、装置、计算机设备和存储介质 - Google Patents

点云地图的构建方法、装置、计算机设备和存储介质 Download PDF

Info

Publication number
WO2020135183A1
WO2020135183A1 PCT/CN2019/126328 CN2019126328W WO2020135183A1 WO 2020135183 A1 WO2020135183 A1 WO 2020135183A1 CN 2019126328 W CN2019126328 W CN 2019126328W WO 2020135183 A1 WO2020135183 A1 WO 2020135183A1
Authority
WO
WIPO (PCT)
Prior art keywords
point cloud
optimal
state
vehicle body
cloud data
Prior art date
Application number
PCT/CN2019/126328
Other languages
English (en)
French (fr)
Inventor
林渲竺
钟华
韩旭
Original Assignee
广州文远知行科技有限公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 广州文远知行科技有限公司 filed Critical 广州文远知行科技有限公司
Priority to US17/419,430 priority Critical patent/US20220057517A1/en
Publication of WO2020135183A1 publication Critical patent/WO2020135183A1/zh

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Definitions

  • This application relates to the field of computer technology, for example, to a method, device, computer device, and storage medium for constructing a point cloud map.
  • High-precision map technology can be applied in the field of mobile robots and autonomous driving. For example, it can achieve centimeter-level positioning of mobile robots with sensors such as LiDAR and cameras.
  • the method of constructing high-precision maps in related technologies is mainly based on a nonlinear optimization method.
  • the cost function is constructed by the state of the sensor, and then the cost function is optimized to obtain a high-precision map.
  • the nonlinear optimization tends to converge to the wrong local solution, which makes the constructed high-precision map have large errors.
  • the present application provides a method, device, computer equipment, and storage medium for constructing a point cloud map capable of reducing map errors.
  • a method for constructing a point cloud map includes:
  • the fusion processing of the vehicle body sensor data to obtain the optimal estimation state and corresponding uncertainty information of the vehicle body sensor data includes:
  • a Bayesian estimation method is used to obtain uncertainty information corresponding to the optimal estimated state.
  • the constructing a point cloud map corresponding to the point cloud data according to the optimal estimated state and corresponding uncertainty information includes:
  • a nonlinear optimization method is used to optimize the cost function to obtain the optimal state of the point cloud data
  • the cost function is constructed using the optimal estimated state and corresponding uncertainty information and the constraint relationship between point cloud data, including:
  • the optimal estimated state is used as an initial value for matching between point cloud data, and a cost function is constructed using the constraint relationship between point cloud data and the optimal state and corresponding uncertainty information.
  • the optimal estimated state is used as an initial value for matching between point cloud data
  • a cost function is constructed using the constraint relationship between point cloud data and the optimal state and corresponding uncertainty information
  • the optimal estimated state corresponding to the point cloud data as an initial value, and constructing a first cost function according to the state to be optimized, the optimal estimated state corresponding to the point cloud data and the corresponding uncertainty information;
  • the first cost function and the second cost function are accumulated to obtain a final cost function.
  • the vehicle body sensor data includes a vehicle body position, a vehicle body speed, a vehicle body acceleration, a vehicle body angular velocity, and a vehicle body direction speed;
  • the optimal estimated state includes the optimal estimated position and the optimal Estimated posture;
  • optimal state includes optimal position and optimal posture;
  • state to be optimized includes position to be optimized and posture to be optimized.
  • a device for constructing a point cloud map includes:
  • the data acquisition module is set to acquire point cloud data and corresponding vehicle body sensor data
  • a fusion processing module configured to perform fusion processing on the vehicle body sensor data to obtain an optimal estimation state and corresponding uncertainty information of the vehicle body sensor data
  • the construction module is configured to construct a point cloud map corresponding to the point cloud data according to the optimal estimated state and corresponding uncertainty information.
  • a computer device includes a memory and a processor.
  • the memory stores a computer program.
  • the processor implements the computer program to implement the following steps:
  • the construction method, device, computer equipment and storage medium of the above point cloud map obtain the optimal estimation state and corresponding of the vehicle body sensor data by acquiring the point cloud data and the corresponding vehicle body sensor data; fusing the vehicle body sensor data Uncertainty information; construct a point cloud map corresponding to point cloud data according to the optimal estimated state and the corresponding uncertainty information.
  • the overall state of all point cloud data is optimized according to the uncertainty information to reduce the uncertainty of all states, especially the long-term lack of position observation scenarios (such as tunnel scenes), and improve the overall positioning accuracy; significantly Reduce the error of high-precision map construction caused by lack of location observation information for a long time and insufficient positioning accuracy.
  • FIG. 1 is a schematic flowchart of a method for constructing a point cloud map according to an embodiment
  • FIG. 2 is a schematic flowchart of a step of obtaining uncertainty information according to an embodiment
  • FIG. 3 is a schematic flowchart of a step of constructing a point cloud map according to an embodiment
  • FIG. 4 is a schematic flow chart of a step of obtaining a cost function according to an embodiment
  • FIG. 5 is a structural block diagram of a point cloud map construction device according to an embodiment
  • FIG. 6 is a structural block diagram of a fusion processing module according to an embodiment
  • FIG. 7 is a structural block diagram of a building module according to an embodiment
  • FIG. 8 is a structural block diagram of a function construction submodule according to an embodiment
  • FIG. 9 is an internal structure diagram of a computer device according to an embodiment.
  • a method for constructing a point cloud map including the following steps:
  • Step S201 Acquire point cloud data and corresponding vehicle body sensor data.
  • the terminal may be a personal computer, a notebook computer, a smart phone, a tablet computer, etc.
  • the server may be an independent server or a plurality of servers Server clusters; this method can be applied to the positioning scenarios of autonomous driving or mobile robots.
  • the point cloud data and the corresponding vehicle body sensor data can be obtained first; in a specific example, the point cloud data can be obtained to collect the point cloud data and the corresponding vehicle body sensor data of the vehicle during traveling
  • the point cloud data collection vehicle can collect point cloud data and corresponding vehicle body sensor data in environments where there is no global positioning system (Global Positioning System, GPS) position observation for a long time in tunnels, mines, etc.
  • Global Positioning System GPS
  • the point cloud data collection vehicle may include lidar and various sensors,
  • the sensor may include a GPS unit, an inertial measurement unit (Inertial Measurement Unit, IMU), a wheel speedometer, etc., which is not limited in this embodiment.
  • IMU Inertial Measurement Unit
  • the lidar can be used to collect point cloud data.
  • the point cloud data can include spatial three-dimensional coordinate information and reflection intensity information.
  • the point cloud data collection vehicle collects color information through the color image collection unit
  • the point cloud data also It can include color information
  • point cloud data can be obtained through devices such as lidar and stored in a Point Cloud (PCD) file format.
  • PCD Point Cloud
  • the GPS unit can be used to collect vehicle body position and vehicle body speed.
  • the GPS unit refers to the user equipment part in the global positioning system, that is, the GPS signal receiver, whose main function is to capture The satellite signal of the satellite under test selected by the satellite cutoff angle, and tracking the operation of the satellite under test.
  • the GPS signal receiver captures the tracked satellite signal, it can measure the pseudo distance and the rate of change of the distance from the receiving antenna to the satellite, and demodulate data such as satellite orbit parameters.
  • the microprocessor in the GPS signal receiver can perform positioning calculation according to the positioning solution method, and calculate the location, speed, time and other information of the user's geographic location; the location can include latitude, longitude, and altitude; this implementation
  • the GPS unit in the example can be installed in a point cloud data collection vehicle and used to collect the vehicle body position and the vehicle body speed, and the vehicle body position can include latitude, longitude, and altitude.
  • the IMU can be used to collect vehicle body acceleration and vehicle body angular velocity.
  • the IMU is a device that measures the angular velocity and acceleration of an object.
  • an IMU can contain three single-axis accelerometers and three single-axis gyroscopes.
  • the accelerometer detects the acceleration signal of the object in the independent three-axis of the carrier coordinate system
  • the gyroscope detects the angular velocity signal of the car body relative to the navigation coordinate system.
  • the IMU in this embodiment may be installed in a point cloud data collection vehicle and used to collect vehicle body acceleration and vehicle body angular velocity.
  • the wheel speedometer can be used to collect wheel speed, that is, the speed of the car body in the forward direction;
  • the wheel speedometer is a sensor used to measure the car wheel speed, and its types mainly include magnetoelectric wheel speed sensors and Hall wheels Speed sensor.
  • a point cloud data collection vehicle travels in a long tunnel (such as in a 3km tunnel)
  • multiple frames of point cloud data and multiple points of cloud data are collected through the lidar, GPS unit, inertial measurement unit, and wheel speedometer, respectively.
  • the corresponding sensor data of the vehicle body such as the vehicle body position, the vehicle body speed, the vehicle body acceleration, the vehicle body angular velocity, the vehicle body's forward direction speed, etc.
  • Step S202 Perform fusion processing on the vehicle body sensor data to obtain an optimal estimation state of the vehicle body sensor data and corresponding uncertainty information.
  • fusion processing is performed on the vehicle body sensor data to obtain the optimal estimation state of the vehicle body sensor data and the corresponding uncertainty information.
  • the fusion processing can be performed by a multi-sensor data fusion method, which corresponds to different algorithms due to different levels of information fusion, which can include weighted average fusion method and Kalman
  • the filtering method, the Bayesian estimation method, the probability theory method, the fuzzy logic inference method, the artificial neural network method, the DS evidence theory method, etc. are not limited in this embodiment.
  • the vehicle body sensor data is input into the algorithm model corresponding to the multi-sensor data fusion method to obtain the optimal estimated state of the output vehicle body sensor data; for example, the vehicle body sensor data is used as the Bayesian estimate
  • the input of the algorithm model corresponding to the method obtains the optimal estimated state of the output, that is, the above-mentioned car body position, car body speed, car body acceleration, car body angular speed, car body heading speed, etc. are input to the corresponding Bayesian estimation method.
  • Algorithm model to obtain the optimal estimated state of the output.
  • the vehicle body sensor data as the input of the algorithm model corresponding to the Kalman filter to obtain the optimal estimated state of the output, that is, the above-mentioned vehicle body position, vehicle body speed, vehicle body acceleration, vehicle body angular speed, vehicle body heading speed Wait for the input to the algorithm model corresponding to the Kalman filter to obtain the optimal estimated state of the output.
  • the uncertainty model corresponding to the vehicle body sensor data may be established, and the uncertainty model may be combined with the Bayesian estimation method to calculate the vehicle body sensor data Uncertainty information.
  • the optimal estimated state may include an optimal estimated position, an optimal estimated attitude, an optimal estimated speed, etc., because the vehicle body sensor data has a corresponding relationship with the optimal estimated state, and the vehicle body sensor data and point cloud data have The corresponding relationship, that is, the optimal estimated state may be the optimal estimated state corresponding to the point cloud data.
  • the vehicle body sensor data required for the fusion process may include vehicle body sensor data collected by all sensors at all moments.
  • all moments may refer to the point in time and end of the point cloud data collection vehicle. Collect all the moments in the middle of the time point, for example, the start acquisition time point is time 1, and the end acquisition time point is time T, then collect the vehicle body sensor data at all times from time 1 to time T and fuse it to obtain the best Optimal estimation status and corresponding uncertainty information.
  • Step S203 Construct a point cloud map corresponding to the point cloud data according to the optimal estimated state and the corresponding uncertainty information.
  • the optimal estimated state and corresponding uncertainty information are used to construct a point cloud map corresponding to the point cloud data.
  • the optimal estimated state and uncertainty information are used to construct the cost function; the optimized cost function obtains the optimal state of the point cloud data, adjusts the point cloud data according to the optimal state, and constructs a point cloud map.
  • a nonlinear optimization method can be used to optimize the cost function to obtain the optimal state of the point cloud data; it should be noted that the nonlinear optimization method can include the gradient descent method (Gradient Descent), Newton method ( Newton's method, Quasi-Newton Methods, Gauss-Newton, Conjugate Gradient, etc. This embodiment does not limit this.
  • the process of optimizing the cost function is an iterative process, iterating the cost function until the function converges to obtain the optimal state of the point cloud data (which can include the optimal position and optimal posture), and complete the continuous construction of multi-frame point cloud data
  • the operation of the point cloud map, in the process of iterating against the cost function is actually a process of continuously adjusting the state (including position and attitude) of the point cloud data, and finally obtaining the optimal state of the point cloud data, and adjusting the point cloud according to the optimal state Data to construct a point cloud map.
  • the construction of an accurate high-precision map such as when applied to the construction of a high-precision map of a tunnel, can significantly improve the construction accuracy of the tunnel map.
  • the method for constructing a point cloud map obtain point cloud data and corresponding vehicle body sensor data; perform fusion processing on the vehicle body sensor data to obtain the optimal estimation state and corresponding uncertainty of the vehicle body sensor data Information; construct a point cloud map corresponding to the point cloud data according to the optimal estimated state and the corresponding uncertainty information.
  • the overall state of all point cloud data is optimized according to the uncertainty information to reduce the uncertainty of all states, especially the long-term lack of position observation scenarios (such as tunnel scenes), and improve the overall positioning accuracy; significantly Reduce the error of high-precision map construction caused by lack of location observation information for a long time and insufficient positioning accuracy.
  • FIG. 2 a schematic flowchart of a step of obtaining uncertainty information in this embodiment is shown. Fusion processing is performed on vehicle body sensor data to obtain an optimal estimation state of vehicle body sensor data and The corresponding uncertainty information includes the following sub-steps:
  • the optimal estimation state of the vehicle body sensor data is obtained through the Bayesian estimation method.
  • an uncertainty model is established based on vehicle sensor data.
  • sub-step S13 the Bayesian estimation method is used according to the uncertainty model to obtain the uncertainty information corresponding to the optimal estimated state.
  • the optimal estimation state of the vehicle body sensor data can be first obtained through the Bayesian estimation method, that is, the vehicle body sensor data is fused through the Bayesian estimation method in the multi-sensor data fusion method to obtain the vehicle Body sensor data.
  • the Bayesian estimation method uses all the collected vehicle body sensor data to obtain the optimal estimated state of the vehicle body at each time.
  • the algorithm model corresponding to the Bayesian estimation method is as follows:
  • z k represents the optimal estimated state at time k (including the optimal estimated position, optimal estimated speed, optimal estimated posture, etc.)
  • y 1 T represents all vehicle sensors collected at all times from time 1 to time T
  • the time period from time 1 to time T represents the maximum length of time for collecting data.
  • an uncertainty model can also be established based on the sensor data of the vehicle body.
  • the uncertainty model can be a sensor noise model.
  • the sensor noise model of the IMU and GPS is provided by the manufacturer or technical documents.
  • the data collected by the wheel speedometer To identify the noise model, this embodiment does not limit it.
  • the Bayesian estimation method can also be used according to the uncertainty model to obtain the uncertainty information corresponding to the optimal estimated state.
  • the uncertainty information in the covariance matrix may be extracted and used as the uncertainty information corresponding to the optimal estimation state.
  • FIG. 3 a schematic flowchart of a step of constructing a point cloud map according to this embodiment is shown, and a point cloud map corresponding to point cloud data is constructed according to an optimal estimated state and corresponding uncertainty information , Including the following substeps:
  • a cost function is constructed using the optimal estimated state and corresponding uncertainty information and the constraint relationship between point cloud data.
  • sub-step S22 a nonlinear optimization method is used to optimize the cost function to obtain the optimal state of the point cloud data.
  • the point cloud data is adjusted according to the optimal state of the point cloud data, and a point cloud map is constructed.
  • the cost function is first constructed using the optimal estimated state and the corresponding uncertainty information and the constraint relationship between the point cloud data; then the cost function is optimized by a nonlinear optimization method to obtain the optimal point cloud data State; and then adjust the point cloud data according to the optimal state of the point cloud data to construct a point cloud map.
  • the Gauss-Newton method in the nonlinear optimization method can be used to optimize the cost function to obtain the optimal state of the point cloud data.
  • the cost function is iterated through the Gauss-Newton method until the function converges to obtain the optimal state of the point cloud data.
  • the optimal state may include the optimal position and the optimal posture.
  • the optimal state of the point cloud data is finally obtained, and the point cloud data is adjusted according to the optimal state to construct a point cloud map.
  • building an accurate high-precision map for example, when applied to the construction of a high-precision map that lacks a location observation scene for a long time, can significantly improve the construction accuracy of the map of the corresponding scene.
  • the optimal estimated state and the corresponding uncertainty information and the constraint relationship between the point cloud data are used to construct a cost function, including: using the optimal estimated state as the initial value of matching between the point cloud data, using the point Constraint relationship between cloud data and optimal state and corresponding uncertainty information construct a cost function.
  • an optimal state can be estimated as an initial value of the matching between the point cloud data, using the constraints between the point cloud data and the optimal state and the corresponding cost function constructed uncertainty information; soon as the above-described z k The initial value of matching between point cloud data is to construct the cost function by using the constraint relationship and optimal state and corresponding uncertainty information between the point cloud data.
  • FIG. 4 there is shown a schematic flow chart of the step of obtaining a cost function in this embodiment, using the optimal estimated state as the initial value of matching between point cloud data, using constraints between point cloud data Relation and optimal state and corresponding uncertainty information to construct a cost function, including the following sub-steps:
  • the optimal estimated state corresponding to the point cloud data is used as an initial value, and a first cost function is constructed according to the state to be optimized, the optimal estimated state corresponding to the point cloud data and the corresponding uncertainty information.
  • a second cost function is constructed by the to-be-optimized state of the two adjacent frames of point cloud data and the corresponding uncertainty information.
  • sub-step S33 the first cost function and the second cost function are accumulated to obtain a final cost function.
  • the first cost function can be constructed first, and the optimal estimated state corresponding to the point cloud data is used as the initial value. According to the state to be optimized, the optimal estimated state corresponding to the point cloud data and the corresponding uncertainty information can form the first A cost function.
  • the first cost function may be: e k (x k , z k ) T ⁇ e k (x k , z k ), where z k is the above-mentioned optimal estimation state, and x k is the above The state to be optimized, ⁇ is the weighting coefficient, which is the reciprocal of the uncertainty, the function e k (x k , z k ) represents the constraint relationship between x k and z k (that is, the error function), here Where, k represents the point of collection of the point cloud data or the number of the point cloud; the initial value of the point cloud data of each frame in the ENU (East-North-Up) coordinate system is the above z k , The first cost function can make the position and posture of the two frames of point cloud data after the translation and rotation of the initial value of the two frames of point cloud data roughly aligned.
  • the second cost function may be constructed according to the constraint information of the same area between the point cloud data and the uncertainty information corresponding to the to-be-optimized state of the two adjacent frames of point cloud data.
  • the second cost function may be: e k (x k ,x k-1 ) T ⁇ e k (x k ,x k-1 ), where x k may be a pending frame of one of the two adjacent frames of point cloud data Optimization state, and x k-1 is the state to be optimized for another frame in the two adjacent point cloud data, ⁇ is the weighting coefficient, and the weighting coefficient is the reciprocal of the uncertainty; the second cost function can Constraint relationship of the same area between the frame point cloud data, fine-tuning the relative translation (that is, adjusting the position) and relative rotation (that is, adjusting the posture) of the two adjacent point cloud data to achieve the best match between the two frame point cloud data.
  • the first cost function and the second cost function are accumulated to complete the construction of the cost function to obtain the final cost function; after the construction of the overall final cost function is completed, a nonlinear optimization method (such as Gauss-Newton method) is used to optimize The overall cost function is the smallest to obtain the optimal state of the point cloud data; adjust the point cloud data according to the optimal state of the point cloud data, thereby completing the construction of the point cloud map.
  • a nonlinear optimization method such as Gauss-Newton method
  • F(x) in the following formula 1 represents the final cost function, which means that the first cost function and the second cost function are accumulated to construct the final cost function;
  • x * in the following formula 2 represents the minimum cost function
  • the optimal state is the optimal position and optimal posture of each frame of point cloud data.
  • z k is the above-mentioned optimal estimation state
  • x k is the above-mentioned state to be optimized
  • ⁇ is the weighting coefficient
  • the weighting coefficient is the reciprocal of the uncertainty
  • e k (x k , z k ) Function represents the constraint relationship (ie error) between x k and z k
  • k represents the point of collection of point cloud data or the number of point cloud for each frame
  • F(x) represents the final cost function
  • x * represents the final cost function The best state in the smallest hour.
  • first cost function and second cost function are constructed in steps, in a specific application, they can pass the final cost function through the application according to the first cost function and the second cost function at the same time The construction is completed.
  • step-by-step construction is to better illustrate the construction process of the final cost function in this embodiment.
  • point cloud data collection vehicle collects 3 frames of point cloud data and the corresponding body sensor data.
  • the multi-sensor data fusion method is used to fuse the sensor data of the vehicle body to obtain the optimal estimated state and the corresponding uncertainty information.
  • the first cost function of the three frames of point cloud data is constructed according to the to-be-optimized state, the optimal estimated state corresponding to the point cloud data and the corresponding uncertainty information.
  • the first cost function of point cloud data may include e 1 (x 1 , z 1 ) T ⁇ e 1 (x 1 , z 1 ), e 2 (x 2 , z 2 ) T ⁇ e 2 (x 2 , z 2 ), e 3 (x 3 ,z 3 ) T ⁇ e 3 (x 3 ,z 3 ).
  • the second cost function of the 3 frames of point cloud data may include e 2 ( x 2 ,x 1 ) T ⁇ e 2 (x 2 ,x 1 ), e 3 (x 3 ,x 2 ) T ⁇ e 3 (x 3 ,x 2 ).
  • the vehicle body sensor data includes vehicle body position, vehicle body speed, vehicle body acceleration, vehicle body angular velocity, and vehicle body heading direction speed;
  • the optimal estimated state includes the optimal estimated position and the optimal estimated posture;
  • the optimal state includes the optimal position and the optimal attitude;
  • the optimal state includes the optimal position and the optimal attitude.
  • the obtained optimal estimated state may include the optimal estimated position, the most The optimal estimated posture and optimal estimated speed, etc., but for the construction of a point cloud map, the final cost function can be constructed only by the optimal estimated position, the optimal estimated posture and the corresponding uncertainty information, that is, for the point cloud data
  • the process of constructing a point cloud map it is possible to focus only on position and attitude, not speed, and optimize the final cost function to complete the construction of the point cloud map.
  • steps in the flowcharts of FIGS. 1-4 are displayed in order according to the arrows, the steps are not necessarily executed in the order indicated by the arrows. Unless clearly stated in this article, the execution of these steps is not strictly limited in order, and these steps may be executed in other orders. Moreover, at least some of the steps in FIGS. 1-4 may include multiple sub-steps or multiple stages, and these sub-steps or stages are not necessarily executed and completed at the same time, but may be executed at different times, these sub-steps or stages The execution order of is not necessarily performed sequentially, but may be executed in turn or alternately with at least a part of other steps or sub-steps or stages of other steps.
  • a point cloud map construction device including: a data acquisition module 301, a fusion processing module 302 and a construction module 303.
  • the data acquisition module 301 is configured to acquire point cloud data and corresponding vehicle body sensor data.
  • the fusion processing module 302 is configured to perform fusion processing on the vehicle body sensor data to obtain an optimal estimation state and corresponding uncertainty information of the vehicle body sensor data.
  • the construction module 303 is configured to construct a point cloud map corresponding to the point cloud data according to the optimal estimated state and corresponding uncertainty information.
  • the fusion processing module 302 includes:
  • the state acquisition sub-module 3021 is set to obtain the optimal estimated state of the vehicle body sensor data through a Bayesian estimation method.
  • the model building submodule 3022 is configured to build an uncertainty model based on the vehicle sensor data.
  • the information obtaining submodule 3023 is set to obtain the uncertainty information corresponding to the optimal estimated state by using a Bayesian estimation method according to the uncertainty model.
  • the construction module includes:
  • the function construction submodule 3031 is set to construct a cost function using the optimal estimated state and corresponding uncertainty information and the constraint relationship between point cloud data.
  • the optimal state obtaining submodule 3032 is set to optimize the cost function using a nonlinear optimization method to obtain the optimal state of the point cloud data.
  • the map construction submodule 3033 is configured to adjust the point cloud data according to the optimal state of the point cloud data to construct a point cloud map.
  • the function construction submodule 3031 is set to use the optimal estimated state as an initial value for matching between point cloud data, and to utilize the constraint relationship between point cloud data and the optimal state and corresponding Uncertainty information constructs a cost function.
  • the function construction submodule includes:
  • the first function construction unit 30311 is set to use the optimal estimated state corresponding to the point cloud data as an initial value, and construct a first cost according to the state to be optimized, the optimal estimated state corresponding to the point cloud data and the corresponding uncertainty information function.
  • the second function construction unit 30312 is configured to use the constraint relationship between the point cloud data to construct a second cost function from the to-be-optimized state of two adjacent frames of point cloud data and corresponding uncertainty information.
  • the accumulation unit 30313 is configured to accumulate the first cost function and the second cost function to obtain a final cost function.
  • the vehicle body sensor data includes a vehicle body position, a vehicle body speed, a vehicle body acceleration, a vehicle body angular velocity, and a vehicle body direction speed;
  • the optimal estimated state includes the optimal estimated position and the optimal Estimated posture;
  • optimal state includes optimal position and optimal posture;
  • state to be optimized includes position to be optimized and posture to be optimized.
  • Each module in the above point cloud map construction device may be implemented in whole or in part by software, hardware, and a combination thereof.
  • the above modules may be embedded in the hardware form or independent of the processor in the computer device, or may be stored in the memory in the computer device in the form of software so that the processor can call and execute the operations corresponding to the above modules.
  • the device for constructing a point cloud map provided above may be used to execute the method for constructing a point cloud map provided in any of the foregoing embodiments, and has corresponding functions and beneficial effects.
  • a computer device is provided.
  • the computer device may be a terminal, and an internal structure diagram thereof may be as shown in FIG. 9.
  • the computer equipment includes a processor, a memory, a network interface, a display screen, and an input device connected through a system bus.
  • the processor of the computer device is used to provide computing and control capabilities.
  • the memory of the computer device includes a non-volatile storage medium and an internal memory.
  • the non-volatile storage medium stores an operating system and computer programs.
  • the internal memory provides an environment for the operating system and computer programs in the non-volatile storage medium.
  • the network interface of the computer device is used to communicate with external terminals through a network connection. When the computer program is executed by the processor, a method for constructing a point cloud map is realized.
  • the display screen of the computer device may be a liquid crystal display screen or an electronic ink display screen
  • the input device of the computer device may be a touch layer covered on the display screen, or may be a button, a trackball, or a touchpad provided on the computer device housing , Can also be an external keyboard, touchpad or mouse.
  • FIG. 9 is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer equipment to which the solution of the present application is applied.
  • the specific computer equipment may It includes more or fewer components than shown in the figure, or some components are combined, or have a different component arrangement.
  • a computer device which includes a memory and a processor.
  • a computer program is stored in the memory, and when the processor executes the computer program, the steps in the method for implementing the construction of the point cloud map in the foregoing embodiments .
  • a computer-readable storage medium is provided on which a computer program is stored.
  • the computer program is executed by a processor, the steps in the method for constructing a point cloud map in the foregoing embodiments are implemented.
  • Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory.
  • Volatile memory can include random access memory (RAM) or external cache memory.
  • RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous chain (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.
  • SRAM static RAM
  • DRAM dynamic RAM
  • SDRAM synchronous DRAM
  • DDRSDRAM double data rate SDRAM
  • ESDRAM enhanced SDRAM
  • SLDRAM synchronous chain (Synchlink) DRAM
  • SLDRAM synchronous chain (Synchlink) DRAM
  • Rambus direct RAM
  • DRAM direct memory bus dynamic RAM
  • RDRAM memory bus dynamic RAM

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Computer Graphics (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • Theoretical Computer Science (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)

Abstract

一种点云地图的构建方法、装置、计算机设备和存储介质。该方法包括：获取点云数据及对应的车体传感器数据（S201）；对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息（S202）；根据所述最优估计状态及对应的不确定度信息构建所述点云数据对应的点云地图（S203）。

Description

点云地图的构建方法、装置、计算机设备和存储介质
本申请要求在2018年12月29日提交中国专利局、申请号为201811642897.2的中国专利申请的优先权，该申请的全部内容通过引用结合在本申请中。
技术领域
本申请涉及计算机技术领域，例如涉及一种点云地图的构建方法、装置、计算机设备和存储介质。
背景技术
高精地图技术可以应用于移动机器人、自动驾驶领域中，如配合激光雷达(LiDAR)、相机等传感器可实现移动机器人的厘米级定位。
相关技术中的构建高精地图的方法主要基于非线性优化方法，通过传感器的状态构建代价函数，然后优化该代价函数得到高精地图，但是，该方法在长时间缺乏位置传感器观测及特征不明显的场景，如在较长的隧道，由于初始值不确定度过大，导致非线性优化容易收敛到错误的局部解，从而使得构建的高精地图存在较大误差。
发明内容
基于此，本申请提供一种能够降低地图误差的点云地图的构建方法、装置、计算机设备和存储介质。
一种点云地图的构建方法，所述方法包括：
获取点云数据及对应的车体传感器数据；
对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息；
根据所述最优估计状态及对应的不确定度信息构建所述点云数据对应的点云地图。
在其中一个实施例中，所述对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息，包括：
通过贝叶斯估计方法，得到所述车体传感器数据的最优估计状态；
根据所述车体传感器数据建立不确定度模型；
根据所述不确定度模型，采用贝叶斯估计方法，获得所述最优估计状态对应的不确定度信息。
在其中一个实施例中，所述根据所述最优估计状态及对应的不确定度信息构建所述点云数据对应的点云地图，包括：
采用最优估计状态及对应的不确定度信息以及点云数据间的约束关系构建代价函数；
采用非线性优化方法优化所述代价函数，得到点云数据的最优状态；
根据所述点云数据的最优状态调整点云数据，构建点云地图。
在其中一个实施例中，所述采用最优估计状态及对应的不确定度信息以及点云数据间的约束关系构建代价函数，包括：
将所述最优估计状态作为点云数据间匹配的初始值，利用点云数据间的约束关系和所述最优状态及对应的不确定度信息构建代价函数。
在其中一个实施例中，所述将所述最优估计状态作为点云数据间匹配的初始值，利用点云数据间的约束关系和所述最优状态及对应的不确定度信息构建代价函数，包括：
将点云数据对应的最优估计状态作为初始值，根据待优化状态、所述点云数据对应的最优估计状态和对应的不确定度信息构建第一代价函数；
利用点云数据间的约束关系，通过相邻的两帧点云数据的待优化状态及对应的不确定度信息构建第二代价函数；
将所述第一代价函数及第二代价函数累加，得到最终代价函数。
在其中一个实施例中，所述车体传感器数据包括车体位置、车体速度、车体加速度、车体角速度和车体前进方向速度；所述最优估计状态包括最优估计位置及最优估计姿态；最优状态包括最优位置及最优姿态；待优化状态包括待优化位置及待优化姿态。
一种点云地图的构建装置，所述装置包括：
数据获取模块，设置为获取点云数据及对应的车体传感器数据；
融合处理模块，设置为对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息；
构建模块，设置为根据所述最优估计状态及对应的不确定度信息构建所述点云数据对应的点云地图。
一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现以下步骤：
获取点云数据及对应的车体传感器数据；
对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息；
根据所述最优估计状态及对应的不确定度信息构建所述点云数据对应的点云地图。
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：
获取点云数据及对应的车体传感器数据；
对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息；
根据所述最优估计状态及对应的不确定度信息构建所述点云数据对应的点云地图。
上述点云地图的构建方法、装置、计算机设备和存储介质，通过获取点云数据及对应的车体传感器数据；对车体传感器数据进行融合处理，得到车体传感器数据的最优估计状态及对应的不确定度信息；根据最优估计状态及对应的不确定度信息构建点云数据对应的点云地图。对所有点云数据的状态根据不确定度信息进行整体优化，降低所有状态有不确定度，尤其是长时间缺乏位置观测场景(如隧道场景)的状态的不确定度，提高整体定位精度；显著降低由于长时间缺乏位置观测信息，定位精度不够引起的高精地图构建误差。
附图说明
图1是一个实施例的一种点云地图的构建方法的流程示意图；
图2是一个实施例的一种获得不确定度信息步骤的流程示意图；
图3是一个实施例的一种构建点云地图步骤的流程示意图；
图4是一个实施例的一种获得代价函数步骤的流程示意图；
图5是一个实施例的一种点云地图的构建装置的结构框图；
图6是一个实施例的一种融合处理模块的结构框图；
图7是一个实施例的一种构建模块的结构框图；
图8是一个实施例的一种函数构建子模块的结构框图；
图9是一个实施例的一种计算机设备的内部结构图。
具体实施方式
以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
在一个实施例中，如图1所示，提供了一种点云地图的构建方法，包括以下步骤：
步骤S201，获取点云数据及对应的车体传感器数据。
本实施例可应用于终端或者服务器中，本实施例对此不作限制，其中，终端可以是个人计算机、笔记本电脑、智能手机、平板电脑等，服务器可以用独立的服务器或者是多个服务器组成的服务器集群来实现；该方法可应用于自动驾驶或移动机器人的定位场景中。
本实施例中，可以首先获取到点云数据及对应的车体传感器数据；在一种具体示例中，可以获取到点云数据采集车辆在行进过程中的点云数据及对应的车体传感器数据；一些实施例中，该点云数据采集车辆可以采集在隧道、矿道等长时间无全球定位系统(Global Positioning System，GPS)位置观测的环境中的点云数据及对应的车体传感器数据。
从硬件的角度而言，该点云数据采集车辆可以包括激光雷达及多种传感器，
该传感器可以包括GPS单元、惯性测量单元(Inertial Measurement Unit，IMU)及轮速计等，本实施例对此不作限制。
具体而言，该激光雷达可以用于采集点云数据，点云数据可以包括空间三维坐标信息及反射强度信息，当点云数据采集车辆通过彩色图像采集单元采集颜色信息时，该点云数据还可以包括颜色信息，点云数据可以通过激光雷达等设备获得，以点云(Point Cloud，PCD)文件的格式进行储存。
另一方面，该GPS单元可以用于采集车体位置及车体速度，具体地，该GPS单元是指全球定位系统中的用户设备部分，即GPS信号接收机，其主要功能是捕获到按一定卫星截止角所选择的待测卫星的卫星信号，并跟踪该待测卫星的运行。当GPS信号接收机捕获到跟踪的卫星信号后，即可测量出接收天线至卫星的伪距离和距离的变化率，解调出卫星轨道参数等数据。根据上述的数据，GPS信号接收机中的微处理器就可按定位解算方法进行定位计算，计算出用户所在地理位置的位置、速度、时间等信息；该位置可以包括经纬度、高度；本实施例中的GPS单元可以安装于点云数据采集车辆中，用于采集车体位置及车体速度，该车体位置可以包括经纬度、高度。
一些实施例中，IMU可以用于采集车体加速度和车体角速度，IMU是测量物体的角速度以及加速度的装置。一般的，一个IMU可以包含三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测车体相对于导航坐标系的角速度信号，测量车体在三维空间中的角速度和加速度。本实施例中的IMU可以安装于点云数据采集车辆中，用于采集车体加速度和车体角速度。
具体应用中，该轮速计可以用于采集轮速，即车体前进方向速度；轮速计是用于测量汽车车轮转速的传感器，其种类主要包括磁电式轮速传感器、霍尔式轮速传感器。
举例而言，当点云数据采集车辆在一段较长隧道中行进时(如在3km的隧道中)，通过激光雷达、GPS单元、惯性测量单元及轮速计，分别采集多帧点云数据及其对应的车体传感器数据，如车体位置、车体速度、车体加速度、车 体角速度、车体前进方向速度等。
步骤S202，对车体传感器数据进行融合处理，得到车体传感器数据的最优估计状态及对应的不确定度信息。
进一步应用到本实施例中，针对车体传感器数据进行融合处理，得到该车体传感器数据的最优估计状态及对应的不确定度信息。
针对车体传感器数据的融合处理的具体方式，可以通过多传感器数据融合方法进行融合处理，该多传感器数据融合方法因信息融合的不同层次对应不同的算法，其可以包括加权平均融合法、卡尔曼滤波法、贝叶斯估计方法、概率论方法、模糊逻辑推理法、人工神经网络法、D-S证据理论法等，本实施例对此不作限制。
一些实施例中，将该车体传感器数据输入至多传感器数据融合方法对应的算法模型中，得到输出的车体传感器数据的最优估计状态；举例而言，将车体传感器数据作为贝叶斯估计方法对应的算法模型的输入，得到输出的最优估计状态，即将上述的车体位置、车体速度、车体加速度、车体角速度、车体前进方向速度等输入至贝叶斯估计方法对应的算法模型，获得输出的最优估计状态。
或者，将车体传感器数据作为卡尔曼滤波对应的算法模型的输入，得到输出的最优估计状态，即将上述的车体位置、车体速度、车体加速度、车体角速度、车体前进方向速度等输入至所卡尔曼滤波对应的算法模型，获得输出的最优估计状态。
一些实施例中，针对不确定度信息的获得方式，首先可以根据建立车体传感器数据对应的不确定度模型，将该不确定度模型与贝叶斯估计方法结合，计算出该车体传感器数据的不确定度信息。
需要说明的是，上述的进行融合处理及获得不确定度信息的步骤可以以离线的方式在硬件设备端执行，提高数据处理效率，节省数据处理资源。
该最优估计状态可以包括最优估计位置、最优估计姿态及最优估计速度等，因该车体传感器数据与最优估计状态具有对应的关系，而该车体传感器数据与点云数据具有对应关系，即该最优估计状态可以为点云数据对应的最优估计状 态。
需要说明的是，该融合处理所需要的车体传感器数据可以包括所有时刻的全部传感器采集得到的车体传感器数据，举例而言，所有时刻可以是指点云数据采集车辆的开始采集时间点及结束采集时间点中间的所有时刻，例如，开始采集时间点为时刻1，结束采集时间点为时刻T，则采集时刻1至时刻T中所有时刻的车体传感器数据，将其进行融合处理，得到最优估计状态及对应的不确定度信息。
步骤S203，根据最优估计状态及对应的不确定度信息构建点云数据对应的点云地图。
实际应用到本实施例中，在获得最优估计状态及对应的不确定度信息后，采用该最优估计状态及对应的不确定度信息构建点云数据对应的点云地图。
一些实施例中，采用最优估计状态及不确定度信息构建代价函数；优化代价函数获得点云数据的最优状态，根据最优状态调整点云数据，构建得到点云地图。
在构建代价函数后，可以采用非线性优化方法针对代价函数进行优化，得到点云数据的最优状态；需要说明的是，该非线性优化方法可以包括梯度下降法(Gradient Descent)、牛顿法(Newton's method)、拟牛顿法(Quasi-Newton Methods)、高斯牛顿法(Gauss-Newton)、共轭梯度法(Conjugate Gradient)等，本实施例对此不作限制。
优化代价函数的过程是一个迭代的过程，将代价函数进行迭代，直到函数收敛，得到点云数据的最优状态(可以包括最优位置及最优姿态)，完成连续的多帧点云数据构建点云地图的操作，在针对代价函数进行迭代的过程，实际是不断调整点云数据的状态(包括位置及姿态)的过程，最终得到点云数据的最优状态，根据最优状态调整点云数据，构建得到点云地图。经整体优化后，构建准确的高精地图，如在应用于隧道的高精地图构建时，可以明显提高隧道地图的构建精度。
根据本实施例提供的点云地图的构建方法，获取点云数据及对应的车体传 感器数据；对车体传感器数据进行融合处理，得到车体传感器数据的最优估计状态及对应的不确定度信息；根据最优估计状态及对应的不确定度信息构建点云数据对应的点云地图。对所有点云数据的状态根据不确定度信息进行整体优化，降低所有状态有不确定度，尤其是长时间缺乏位置观测场景(如隧道场景)的状态的不确定度，提高整体定位精度；显著降低由于长时间缺乏位置观测信息，定位精度不够引起的高精地图构建误差。
在另一个实施例中，参照图2，示出了本实施例的一种获得不确定度信息步骤的流程示意图，对车体传感器数据进行融合处理，得到车体传感器数据的最优估计状态及对应的不确定度信息，包括以下子步骤：
子步骤S11，通过贝叶斯估计方法，得到车体传感器数据的最优估计状态。
子步骤S12，根据车体传感器数据建立不确定度模型。
子步骤S13，根据不确定度模型，采用贝叶斯估计方法，获得最优估计状态对应的不确定度信息。
本实施例中，可以首先通过贝叶斯估计方法，得到车体传感器数据的最优估计状态，即通过多传感器数据融合方法中的贝叶斯估计方法将车体传感器数据进行融合处理，得到车体传感器数据。
一些实施例中，通过贝叶斯估计方法，利用全部的采集得到的车体传感器数据，得到车体在每个时刻状态的最优估计状态，贝叶斯估计方法对应的算法模型如下：
p(z k|y 1:T)
其中，z k表示k时刻的最优估计状态(包括最优估计位置、最优估计速度、最优估计姿态等)，y 1:T表示时刻1到时刻T所有时刻采集的所有的车体传感器数据，则时刻1到时刻T所在的时间段表示采集数据的最大时间长度。通过上述的模型可获得车体位置，姿态等的最优估计状态。
一些实施例中，还可以根据车体传感器数据建立不确定度模型，该不确定度模型可以为传感器噪声模型，IMU与GPS的传感器噪声模型由厂商或技术文档提供，轮速计通过采集的数据辨识该噪声模型，本实施例对此不作限制。
实际应用中，还可以根据不确定度模型，采用贝叶斯估计方法，获得最优估计状态对应的不确定度信息。
一些实施例中，可以提取出协方差矩阵中的不确定度信息，将该不确定度信息作为最优估计状态对应的不确定度信息。
在另一个实施例中，参照图3，示出了本实施例的一种构建点云地图步骤的流程示意图，根据最优估计状态及对应的不确定度信息构建点云数据对应的点云地图，包括以下子步骤：
子步骤S21，采用最优估计状态及对应的不确定度信息以及点云数据间的约束关系构建代价函数。
子步骤S22，采用非线性优化方法优化代价函数，得到点云数据的最优状态。
子步骤S23，根据点云数据的最优状态调整点云数据，构建点云地图。
实际应用到本实施例中，首先采用最优估计状态及对应的不确定度信息以及点云数据间的约束关系构建代价函数；再通过非线性优化方法优化代价函数，得到点云数据的最优状态；而后根据点云数据的最优状态调整点云数据，构建点云地图。
在构建代价函数后，可以采用非线性优化方法中的高斯牛顿法针对代价函数进行优化，得到点云数据的最优状态。
即通过高斯牛顿法将代价函数进行迭代，直到函数收敛，得到点云数据的最优状态，该最优状态可以包括最优位置及最优姿态，完成点云数据构建点云地图的操作，在针对代价函数进行迭代的过程后，最终得到点云数据的最优状态，根据最优状态调整点云数据，构建得到点云地图。经整体优化后，构建准确的高精地图，如在应用于长时间缺乏位置观测场景的高精地图的构建时，可以明显提高相应场景的地图的构建精度。
在另一个实施例中，采用最优估计状态及对应的不确定度信息以及点云数据间的约束关系构建代价函数，包括：将最优估计状态作为点云数据间匹配的初始值，利用点云数据间的约束关系和最优状态及对应的不确定度信息构建代价函数。
一些实施例中，可以将最优估计状态作为点云数据间匹配的初始值，利用点云数据间的约束关系和最优状态及对应的不确定度信息构建代价函数；即将上述的z k作为点云数据间匹配的初始值，利用点云数据间的约束关系和最优状态及对应的不确定度信息构建代价函数。
在另一个实施例中，参照图4，示出了本实施例的一种获得代价函数步骤的流程示意图，将最优估计状态作为点云数据间匹配的初始值，利用点云数据间的约束关系和最优状态及对应的不确定度信息构建代价函数，包括以下子步骤：
子步骤S31，将点云数据对应的最优估计状态作为初始值，根据待优化状态、点云数据对应的最优估计状态和对应的不确定度信息构建第一代价函数。
子步骤S32，利用点云数据间的约束关系，通过相邻的两帧点云数据的待优化状态及对应的不确定度信息构建第二代价函数。
子步骤S33，将第一代价函数及第二代价函数累加，得到最终代价函数。
具体应用中，首先可以构建第一代价函数，将点云数据对应的最优估计状态作为初始值，根据待优化状态、点云数据对应的最优估计状态和对应的不确定度信息可以组成第一代价函数。
一些实施例中，该第一代价函数可以为：e k(x k,z k) TΩe k(x k,z k)，其中，z k为上述的最优估计状态，而x k为上述的待优化状态，Ω为加权系数，该加权系数为不确定度的倒数，e k(x k,z k)函数表示待x k与z k之间的约束关系(即误差函数)，在此处，k表示每帧点云数据的采集时刻或点云的编号；每帧的点云数据在ENU(East-North-Up，东-北-天)坐标系中的初始值为上述的z k，第一代价函数可以让每两帧点云数据经过初始值的平移跟旋转后，使该两帧点云数据的位置与姿态大致对齐。
一些实施例中，还可以通过点云数据间相同区域的约束关系，根据相邻的两帧点云数据的待优化状态对应的不确定度信息构建第二代价函数。
该第二代价函数可以为：e k(x k,x k-1) TΩe k(x k,x k-1)，该x k可以为相邻两帧点云数据中其中一帧的待优化状态，而x k-1为相邻两帧点云数据中另外一帧的待优化状态，Ω为加权系数，该加权系数为不确定度的倒数；该第二代价函数可以通 过相邻两帧点云数据之间相同区域的约束关系，微调相邻两帧点云数据的相对平移(即调整位置)和相对旋转(即调整姿态)，以达到两帧点云数据的最佳匹配。
而后，将该第一代价函数及第二代价函数进行累加，完成代价函数的构建，得到最终代价函数；完成整体的最终代价函数构建后，利用非线性优化方法(如高斯牛顿法)优化，使整体代价函数最小，得到点云数据的最优状态；根据点云数据的最优状态调整点云数据，从而完成点云地图构建。
以下的公式一中的F(x)表示最终代价函数，表示将第一代价函数及第二代价函数进行累加，构建得到最终代价函数；以下的公式二中的x *表示最终代价函数最小时的最优状态，即每帧点云数据的最优位置跟最优姿态。
Figure PCTCN2019126328-appb-000001
Figure PCTCN2019126328-appb-000002
在公式一及公式二中，z k为上述的最优估计状态，x k为上述的待优化状态，Ω为加权系数，该加权系数为不确定度的倒数，e k(x k,z k)函数表示待x k与z k之间的约束关系(即误差)，k表示每帧点云数据的采集时刻或点云的编号，F(x)表示最终代价函数，x *表示最终代价函数最小时的最优状态。
需要说明的是，虽然上述的第一代价函数及第二代价函数是分步骤构建，而在具体应用中，其可以通过最终代价函数可以是通过应用程序根据第一代价函数及第二代价函数同时构建完成的，上述的分步骤构建是为了更好说明本实施例的最终代价函数的构建过程。
为了使本领域技术人员更好地理解本实施例，以下用一个具体示例进行说明。
1、若点云数据采集车辆采集到3帧点云数据及对应的车体传感器数据。
2、通过多传感器数据融合方法将车体传感器数据进行融合处理，得到最优估计状态及对应的不确定度信息。
3、根据上述的最优估计状态为初始值，根据待优化状态、点云数据对应的最优估计状态和对应的不确定度信息构建该3帧点云数据的第一代价函数，该3 帧点云数据的第一代价函数可以包括e 1(x 1,z 1) TΩe 1(x 1,z 1)、e 2(x 2,z 2) TΩe 2(x 2,z 2)、e 3(x 3,z 3) TΩe 3(x 3,z 3)。
4、通过相邻的两帧点云数据的待优化状态及对应的不确定度信息构建3帧点云数据的第二代价函数；该3帧点云数据的第二代价函数可以包括e 2(x 2,x 1) TΩe 2(x 2,x 1)、e 3(x 3,x 2) TΩe 3(x 3,x 2)。
5、将第一代价函数及第二代价函数累加，得到最终代价函数，即该F(x)
为e 1(x 1,z 1) TΩe 1(x 1,z 1)、e 2(x 2,z 2) TΩe 2(x 2,z 2)、e 3(x 3,z 3) TΩe 3(x 3,z 3)、e 2(x 2,x 1) TΩe 2(x 2,x 1)、e 3(x 3,x 2) TΩe 3(x 3,x 2)的和。
6、将最终代价函数F(x)根据高斯牛顿法优化，使整体代价函数最小，得到点云数据的最优状态；而后根据点云数据的最优状态调整点云数据，从而完成点云地图构建。
在另一个实施例中，车体传感器数据包括车体位置、车体速度、车体加速度、车体角速度和车体前进方向速度；最优估计状态包括最优估计位置及最优估计姿态；最优状态包括最优位置及最优姿态；待优化状态包括待优化位置及待优化姿态。
需要说明的是，在对车体传感器数据进行融合处理，得到车体传感器数据的最优估计状态及对应的不确定度信息的步骤中，得到的最优估计状态可以包括最优估计位置、最优估计姿态及最优估计速度等，但是，对于点云地图的构建，可以只通过最优估计位置、最优估计姿态及对应的不确定度信息进行最终代价函数的构建，即对于点云数据构建点云地图的过程而言，可以只关注位置与姿态，而不关注速度，优化该最终代价函数，从而完成点云地图的构建。
应该理解的是，虽然图1-4的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1-4中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻 执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
在一个实施例中，如图5所示，提供了一种点云地图的构建装置，包括：数据获取模块301、融合处理模块302和构建模块303。
数据获取模块301，设置为获取点云数据及对应的车体传感器数据。
融合处理模块302，设置为对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息。
构建模块303，设置为根据所述最优估计状态及对应的不确定度信息构建所述点云数据对应的点云地图。
在其中一个实施例中，参照图6，示出了本实施例的一种融合处理模块302的结构框图，所述融合处理模块302包括：
状态获取子模块3021，设置为通过贝叶斯估计方法，得到所述车体传感器数据的最优估计状态。
模型建立子模块3022，设置为根据所述车体传感器数据建立不确定度模型。
信息获取子模块3023，设置为根据所述不确定度模型，采用贝叶斯估计方法，获得所述最优估计状态对应的不确定度信息。
在其中一个实施例中，参照图7，示出了本实施例的一种构建模块303的结构框图，所述构建模块包括：
函数构建子模块3031，设置为采用最优估计状态及对应的不确定度信息以及点云数据间的约束关系构建代价函数。
最优状态获取子模块3032，设置为采用非线性优化方法优化所述代价函数，得到点云数据的最优状态。
地图构建子模块3033，设置为根据所述点云数据的最优状态调整点云数据，构建点云地图。
在其中一个实施例中，所述函数构建子模块3031设置为将所述最优估计状态作为点云数据间匹配的初始值，利用点云数据间的约束关系和所述最优状态 及对应的不确定度信息构建代价函数。
在其中一个实施例中，参照图8，示出了本实施例的一种函数构建子模块3031的结构框图，所述函数构建子模块包括：
第一函数构建单元30311，设置为将点云数据对应的最优估计状态作为初始值，根据待优化状态、所述点云数据对应的最优估计状态和对应的不确定度信息构建第一代价函数。
第二函数构建单元30312，设置为利用点云数据间的约束关系，通过相邻的两帧点云数据的待优化状态及对应的不确定度信息构建第二代价函数。
累加单元30313，设置为将所述第一代价函数及第二代价函数累加，得到最终代价函数。
在其中一个实施例中，所述车体传感器数据包括车体位置、车体速度、车体加速度、车体角速度和车体前进方向速度；所述最优估计状态包括最优估计位置及最优估计姿态；最优状态包括最优位置及最优姿态；待优化状态包括待优化位置及待优化姿态。
关于点云地图的构建装置的具体限定可以参见上文中对于点云地图的构建方法的限定，在此不再赘述。上述点云地图的构建装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
上述提供的点云地图的构建装置可用于执行上述任意实施例提供的点云地图的构建方法，具备相应的功能和有益效果。
在一个实施例中，提供了一种计算机设备，该计算机设备可以是终端，其内部结构图可以如图9所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口、显示屏和输入装置。该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介 质中的操作系统和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种点云地图的构建方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。
本领域技术人员可以理解，图9中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
在一个实施例中，提供了一种计算机设备，包括存储器和处理器，存储器中存储有计算机程序，该处理器执行计算机程序时实现上述各实施例中的点云地图的构建的方法中的步骤。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述各实施例中的点云地图的构建的方法中的步骤。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储 器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对申请专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (10)

  1. 一种点云地图的构建方法，包括：
    获取点云数据及对应的车体传感器数据；
    对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息；
    根据所述最优估计状态及对应的不确定度信息构建所述点云数据对应的点云地图。
  2. 根据权利要求1所述的方法，其中，所述对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息，包括：
    通过贝叶斯估计方法，得到所述车体传感器数据的最优估计状态；
    根据所述车体传感器数据建立不确定度模型；
    根据所述不确定度模型，采用贝叶斯估计方法，获得所述最优估计状态对应的不确定度信息。
  3. 根据权利要求1所述的方法，其中，所述根据所述最优估计状态及对应的不确定度信息构建所述点云数据对应的点云地图，包括：
    采用最优估计状态及对应的不确定度信息以及点云数据间的约束关系构建代价函数；
    采用非线性优化方法优化所述代价函数，得到点云数据的最优状态；
    根据所述点云数据的最优状态调整点云数据，构建点云地图。
  4. 根据权利要求3所述的方法，其中，所述采用最优估计状态及对应的不确定度信息以及点云数据间的约束关系构建代价函数，包括：
    将所述最优估计状态作为点云数据间匹配的初始值，利用点云数据间的约束关系和所述最优状态及对应的不确定度信息构建代价函数。
  5. 根据权利要求4所述的方法，其中，所述将所述最优估计状态作为点云数据间匹配的初始值，利用点云数据间的约束关系和所述最优状态及对应的不确定度信息构建代价函数，包括：
    将点云数据对应的最优估计状态作为初始值，根据待优化状态、所述点云 数据对应的最优估计状态和对应的不确定度信息构建第一代价函数；
    利用点云数据间的约束关系，通过相邻的两帧点云数据的待优化状态及对应的不确定度信息构建第二代价函数；
    将所述第一代价函数及第二代价函数累加，得到最终代价函数。
  6. 根据权利要求1至5任一项所述的方法，其中，所述车体传感器数据包括车体位置、车体速度、车体加速度、车体角速度和车体前进方向速度；所述最优估计状态包括最优估计位置及最优估计姿态；最优状态包括最优位置及最优姿态；待优化状态包括待优化位置及待优化姿态。
  7. 一种点云地图的构建装置，包括：
    数据获取模块，设置为获取点云数据及对应的车体传感器数据；
    融合处理模块，设置为对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息；
    构建模块，设置为根据所述最优估计状态及对应的不确定度信息构建所述点云数据对应的点云地图。
  8. 根据权利要求7所述的装置，其中，所述融合处理模块包括：
    状态获取子模块，设置为通过贝叶斯估计方法，得到所述车体传感器数据的最优估计状态；
    模型建立子模块，设置为根据所述车体传感器数据建立不确定度模型；
    信息获取子模块，设置为根据所述不确定度模型，采用贝叶斯估计方法，获得所述最优估计状态对应的不确定度信息。
  9. 一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其中，所述处理器执行所述计算机程序时实现权利要求1至6中任一项所述的点云地图的构建方法的步骤。
  10. 一种计算机可读存储介质，其上存储有计算机程序，其中，所述计算机程序被处理器执行时实现权利要求1至6中任一项所述的点云地图的构建方法的步骤。
PCT/CN2019/126328 2018-12-29 2019-12-18 点云地图的构建方法、装置、计算机设备和存储介质 WO2020135183A1 (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
US17/419,430 US20220057517A1 (en) 2018-12-29 2019-12-18 Method for constructing point cloud map, computer device, and storage medium

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201811642897.2 2018-12-29
CN201811642897.2A CN111383324B (zh) 2018-12-29 2018-12-29 点云地图的构建方法、装置、计算机设备和存储介质

Publications (1)

Publication Number Publication Date
WO2020135183A1 true WO2020135183A1 (zh) 2020-07-02

Family

ID=71128369

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2019/126328 WO2020135183A1 (zh) 2018-12-29 2019-12-18 点云地图的构建方法、装置、计算机设备和存储介质

Country Status (3)

Country Link
US (1) US20220057517A1 (zh)
CN (1) CN111383324B (zh)
WO (1) WO2020135183A1 (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116664416A (zh) * 2023-04-17 2023-08-29 重庆大学 雷达点云数据处理方法、装置、电子设备及存储介质

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113963027B (zh) * 2021-10-28 2022-09-09 广州文远知行科技有限公司 不确定性检测模型的训练、不确定性的检测方法及装置
CN114549605B (zh) * 2021-12-31 2023-08-04 广州景骐科技有限公司 基于点云匹配的图像优化方法、装置、设备及存储介质
CN116067379B (zh) * 2023-03-07 2023-06-30 青岛慧拓智能机器有限公司 一种基于动态点云的长隧道场景下多传感器融合定位方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104236548A (zh) * 2014-09-12 2014-12-24 清华大学 一种微型无人机室内自主导航方法
CN108445480A (zh) * 2018-02-02 2018-08-24 重庆邮电大学 基于激光雷达的移动平台自适应扩展目标跟踪系统及方法
CN108717712A (zh) * 2018-05-29 2018-10-30 东北大学 一种基于地平面假设的视觉惯导slam方法
CN108921893A (zh) * 2018-04-24 2018-11-30 华南理工大学 一种基于在线深度学习slam的图像云计算方法及系统

Family Cites Families (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9715016B2 (en) * 2015-03-11 2017-07-25 The Boeing Company Real time multi dimensional image fusing
KR101725478B1 (ko) * 2015-05-21 2017-04-11 주식회사 맥스트 3d 점군 인식 기반의 증강 현실 출력 방법과 이를 수행하기 위한 장치 및 시스템
CN108693543B (zh) * 2017-03-31 2022-11-22 法拉第未来公司 用于检测信号欺骗的方法及系统
CN107796397B (zh) * 2017-09-14 2020-05-15 杭州迦智科技有限公司 一种机器人双目视觉定位方法、装置和存储介质
CN107909612B (zh) * 2017-12-01 2021-01-29 驭势科技（北京）有限公司 一种基于3d点云的视觉即时定位与建图的方法与系统
CN108759833B (zh) * 2018-04-25 2021-05-25 中国科学院合肥物质科学研究院 一种基于先验地图的智能车辆定位方法
CN108765481B (zh) * 2018-05-25 2021-06-11 亮风台（上海）信息科技有限公司 一种单目视频的深度估计方法、装置、终端和存储介质
CN108958266A (zh) * 2018-08-09 2018-12-07 北京智行者科技有限公司 一种地图数据获取方法
CN109059942B (zh) * 2018-08-22 2021-12-14 中国矿业大学 一种井下高精度导航地图构建系统及构建方法
CN112930554A (zh) * 2018-09-12 2021-06-08 丰田自动车欧洲公司 用于确定车辆环境的语义网格的电子设备、系统和方法
US11494937B2 (en) * 2018-11-16 2022-11-08 Uatc, Llc Multi-task multi-sensor fusion for three-dimensional object detection
US10991156B2 (en) * 2018-12-05 2021-04-27 Sri International Multi-modal data fusion for enhanced 3D perception for platforms

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104236548A (zh) * 2014-09-12 2014-12-24 清华大学 一种微型无人机室内自主导航方法
CN108445480A (zh) * 2018-02-02 2018-08-24 重庆邮电大学 基于激光雷达的移动平台自适应扩展目标跟踪系统及方法
CN108921893A (zh) * 2018-04-24 2018-11-30 华南理工大学 一种基于在线深度学习slam的图像云计算方法及系统
CN108717712A (zh) * 2018-05-29 2018-10-30 东北大学 一种基于地平面假设的视觉惯导slam方法

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116664416A (zh) * 2023-04-17 2023-08-29 重庆大学 雷达点云数据处理方法、装置、电子设备及存储介质

Also Published As

Publication number Publication date
US20220057517A1 (en) 2022-02-24
CN111383324B (zh) 2023-03-28
CN111383324A (zh) 2020-07-07

Similar Documents

Publication Publication Date Title
US10295365B2 (en) State estimation for aerial vehicles using multi-sensor fusion
WO2020135183A1 (zh) 点云地图的构建方法、装置、计算机设备和存储介质
US20230194265A1 (en) Square-Root Multi-State Constraint Kalman Filter for Vision-Aided Inertial Navigation System
CN109887057B (zh) 生成高精度地图的方法和装置
US9709404B2 (en) Iterative Kalman Smoother for robust 3D localization for vision-aided inertial navigation
US10371529B2 (en) Computational budget estimation for vision-aided inertial navigation systems
CN110084832B (zh) 相机位姿的纠正方法、装置、系统、设备和存储介质
CN107748569B (zh) 用于无人机的运动控制方法、装置及无人机系统
US10907971B2 (en) Square root inverse Schmidt-Kalman filters for vision-aided inertial navigation and mapping
CN108036785A (zh) 一种基于直接法与惯导融合的飞行器位姿估计方法
CN113820735B (zh) 位置信息的确定方法、位置测量设备、终端及存储介质
CN114018274B (zh) 车辆定位方法、装置及电子设备
KR101985344B1 (ko) 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치
CN112985391B (zh) 一种基于惯性和双目视觉的多无人机协同导航方法和装置
CN112577493A (zh) 一种基于遥感地图辅助的无人机自主定位方法及系统
Indelman et al. Incremental light bundle adjustment for robotics navigation
CN114111776B (zh) 定位方法及相关装置
CN112945227A (zh) 定位方法和装置
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN115326084A (zh) 车辆定位方法、装置、计算机设备、存储介质
CN114689047A (zh) 基于深度学习的组合导航方法、装置、系统及存储介质
CN115984463A (zh) 一种适用于狭窄巷道的三维重建方法和系统
Hong et al. Visual inertial odometry using coupled nonlinear optimization
CN113218389B (zh) 一种车辆定位方法、装置、存储介质及计算机程序产品
CN114001730B (zh) 融合定位方法、装置、计算机设备和存储介质

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 19901437

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 19901437

Country of ref document: EP

Kind code of ref document: A1