CN113819914B

CN113819914B - Map construction method and device

Info

Publication number: CN113819914B
Application number: CN202010566851.8A
Authority: CN
Inventors: 刘光伟; 赵季
Original assignee: Beijing Tusen Weilai Technology Co Ltd
Current assignee: Beijing Tusen Weilai Technology Co Ltd
Priority date: 2020-06-19
Filing date: 2020-06-19
Publication date: 2024-06-07
Anticipated expiration: 2040-06-19
Also published as: CN113819914A

Abstract

The application provides a map construction method and device, and relates to the technical field of high-precision maps. The method comprises the following steps: acquiring sensor data acquired by various sensors; constructing a plurality of local point cloud maps of a preset road section based on an odometer method of multi-sensor fusion, and establishing frame pose constraints of two adjacent frames in the same local point cloud map; calculating pose relation between adjacent local point cloud maps, and establishing sub-map pose constraint of the adjacent local point cloud maps; obtaining pose observation data of a movable object in a preset road range before and after a preset road section, and establishing a local point cloud map pose observation constraint and a frame pose observation constraint; performing global optimization based on the sub-map, determining the optimizing pose of each local point cloud map, and determining the pose of each frame in each local point cloud map after global optimization based on the sub-map; performing global optimization based on frames, and determining the optimization pose of each frame; and optimizing the pose and the sensor data according to each frame, and fusing map data to form a global map.

Description

Map construction method and device

Technical Field

The application relates to the technical field of high-precision maps, in particular to a map construction method and device.

Background

At present, with the development of automatic driving technology and intelligent robot technology, how to ensure accurate driving of an automatic driving vehicle and an intelligent robot becomes a hot spot problem. In the automatic driving technology, a high-precision map is generally applied, which is different from a traditional navigation map, wherein the high-precision map contains a large amount of driving assistance information, and the most important information is an accurate three-dimensional representation based on a road network, such as intersection layout, road sign position and the like. In addition, the high-precision map contains a lot of semantic information, and the map may report meaning of different colors on the communication traffic light, which may indicate speed limitation of the road, a position where the left turn lane starts, and the like. One of the most important features of the high-precision map is precision, which enables an automatic driving vehicle or the like to reach centimeter-level precision, which is important to ensure the safety of the automatic driving vehicle.

In the field of automatic driving, data is generally acquired through a laser radar in the construction of a high-precision map, so that when the high-precision map is constructed, a high-precision point cloud map is formed first. The formation of the high-precision point cloud map is required to depend on a good positioning environment and a mileage calculation method, and the construction result of the high-precision point cloud map is poor in some road sections (long tunnels and remote mountain areas) with poor satellite signals. The application aims to provide a construction scheme of a high-precision point cloud map aiming at a road section with poor satellite signals.

Disclosure of Invention

The embodiment of the application provides a map construction method and a map construction device, which can realize the construction of a high-precision point cloud map of a road section with poor satellite signals, thereby ensuring the normal running of an automatic driving vehicle, an intelligent robot and the like.

In order to achieve the above purpose, the embodiment of the present application adopts the following technical scheme:

in a first aspect of an embodiment of the present application, a map construction method is provided, including:

acquiring sensor data acquired by various sensors carried on a movable object when the movable object runs on a preset road section;

an odometer method based on multi-sensor fusion is used for incrementally constructing a plurality of local point cloud maps of a preset road section and establishing frame pose constraints of two adjacent frames in the same local point cloud map;

calculating pose relation between adjacent local point cloud maps through point cloud registration, and establishing sub-map pose constraint of the adjacent local point cloud maps;

obtaining pose observation data of a movable object in a preset road range before and after a preset road section, and establishing a local point cloud map pose observation constraint and a frame pose observation constraint;

Performing global optimization based on the sub map according to the sub map pose constraint and the local point cloud map pose observation constraint, determining the optimized pose of each local point cloud map, and determining the pose of each frame in each local point cloud map after global optimization based on the sub map;

Performing global optimization based on frames on the global frame pose according to the frame pose constraint and the frame pose observation constraint, and determining the optimized pose of each frame;

And optimizing the pose and the sensor data according to each frame, and fusing map data to form a global map.

In a second aspect of an embodiment of the present application, there is provided a map construction apparatus including:

The data acquisition unit is used for acquiring sensor data acquired by various sensors carried on the movable object when the movable object runs on a preset road section;

The multi-sensor fusion unit is used for incrementally constructing a plurality of local point cloud maps of a preset road section based on a multi-sensor fusion odometer method and establishing frame pose constraints of two adjacent frames in the same local point cloud map;

The sub-map pose constraint building unit is used for calculating pose relation between adjacent local point cloud maps through point cloud registration and building sub-map pose constraint of the adjacent local point cloud maps;

The pose observation constraint establishing unit is used for obtaining pose observation data of the movable object in a preset road range before and after a preset road section, and establishing a local point cloud map pose observation constraint and a frame pose observation constraint;

The global optimization unit based on the sub map is used for carrying out global optimization based on the sub map according to the sub map pose constraint and the local point cloud map pose observation constraint, determining the optimizing pose of each local point cloud map, and determining the pose of each frame in each local point cloud map after global optimization based on the sub map;

The global optimization unit based on the frames is used for carrying out global optimization based on the frames on the global positions of all frames according to the position constraints of the frames and the position observation constraints of the frames, and determining the optimized positions of all frames;

And the global map forming unit is used for carrying out map data fusion according to the optimized pose of each frame and the sensor data to form a global map.

In a third aspect of embodiments of the present application, a computer readable storage medium is provided, including a program or instructions, which when run on a computer, implement the method according to the first aspect.

In a fourth aspect of embodiments of the application, there is provided a computer program product comprising instructions which, when run on a computer, cause the computer to perform the method as described in the first aspect above.

In a fifth aspect of embodiments of the present application, a computer server is provided, comprising a memory, and one or more processors communicatively coupled to the memory;

the memory stores instructions executable by the one or more processors to cause the one or more processors to implement the method of the first aspect.

The map construction method and device provided by the embodiment of the application are characterized in that firstly, sensor data acquired by various sensors carried on a movable object when the movable object runs on a preset road section are obtained; then, based on a multisensor fusion odometer method, a plurality of local point cloud maps of a preset road section are constructed in an incremental mode, and frame pose constraints of two adjacent frames in the same local point cloud map are built; then calculating the pose relation between adjacent local point cloud maps through point cloud registration, and establishing sub-map pose constraint of the adjacent local point cloud maps; secondly, pose observation data of the movable object in a preset road range before and after a preset road section are obtained, and a local point cloud map pose observation constraint and a frame pose observation constraint are established; performing global optimization based on the sub map according to the sub map pose constraint and the local point cloud map pose observation constraint, determining the optimized pose of each local point cloud map, and determining the pose of each frame in each local point cloud map after global optimization based on the sub map; then, global optimization based on frames is carried out on the global frame pose according to the frame pose constraint and the frame pose observation constraint, and the optimized pose of each frame is determined; and further, optimizing the pose and the sensor data according to each frame, and fusing map data to form a global map. Therefore, the method effectively utilizes the characteristics of high accuracy in a short time and high positioning accuracy of the preset road range before and after the preset road section by constructing the independent local point cloud map, global optimization based on the sub-map and global optimization based on the frame, simultaneously avoids the problem that the optimization algorithm falls into local optimization, can realize the construction of the high-accuracy point cloud map of the road section with poor satellite signals, and further ensures the normal running of automatic driving vehicles, intelligent robots and the like.

Drawings

In order to more clearly illustrate the embodiments of the application or the technical solutions of the prior art, the drawings which are used in the description of the embodiments or the prior art will be briefly described, it being obvious that the drawings in the description below are only some embodiments of the application, and that other drawings can be obtained according to these drawings without inventive faculty for a person skilled in the art.

Fig. 1 is a flowchart of a map construction method according to an embodiment of the present application;

FIG. 2 is a second flowchart of a map construction method according to an embodiment of the present application;

FIG. 3 is a schematic view of a tunnel scenario in an embodiment of the present application;

FIG. 4 is a schematic diagram showing comparison of the results of the odometer method based on multi-sensor fusion provided by the embodiment of the application and the prior art method in a tunnel scene;

FIG. 5 is a schematic diagram of a comparison result of a map generated by a map construction method according to an embodiment of the present application;

fig. 6 is a schematic structural diagram of a map building device according to an embodiment of the present application.

Detailed Description

The following description of the embodiments of the present application will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present application, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the application without making any inventive effort, are intended to be within the scope of the application.

It should be noted that the terms "first," "second," and the like in the description and the claims of the present application and the above figures are used for distinguishing between similar objects and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used may be interchanged where appropriate in order to describe the embodiments of the application herein. Furthermore, the terms "comprises," "comprising," and "having," and any variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, system, article, or apparatus that comprises a list of steps or elements is not necessarily limited to those steps or elements expressly listed but may include other steps or elements not expressly listed or inherent to such process, method, article, or apparatus.

In order to enable those skilled in the art to better understand the present application, some technical terms appearing in the embodiments of the present application are explained below:

Movable object: the system is an object which can be used for map acquisition, such as a vehicle, a mobile robot, an aircraft and the like, and various types of sensors, such as a laser radar, a camera and the like, can be carried on the mobile object.

And (3) GNSS: global Navigation SATELLITE SYSTEM, global navigation satellite system.

GPS: global Positioning System, global positioning system.

IMU: inertial Measurement Unit an inertial measurement unit is a device for measuring three-axis attitude angle (or angular velocity) and acceleration of an object.

High precision map: unlike conventional navigation maps, high-precision maps contain a large amount of driving assistance information, the most important of which is based on accurate three-dimensional characterization of the road network, such as intersection layout and road sign position. In addition, the high-precision map contains a lot of semantic information, and the map may report meaning of different colors on the communication traffic light, which may indicate speed limitation of the road, a position where the left turn lane starts, and the like. One of the most important features of high-precision maps is precision, which enables the vehicle to reach centimeter-level precision, which is important to ensure the safety of an automatically driven automobile.

Mapping (Mapping): and constructing a high-precision map describing the current scene according to the estimated real-time pose of the vehicle or the mobile robot and the acquired data of vision sensors such as a laser radar.

Pose (Pose): the sum of the positions and orientations includes 6 degrees of freedom, including 3 degrees of freedom in position and 3 degrees of freedom in orientation. The 3 degrees of freedom of orientation are typically expressed in terms of Pitch angle (Pitch), roll angle (Roll), yaw angle (Yaw).

Frame (Frame): the sensor completes one-time observation of the received measurement data, for example, one frame of data of the camera is a picture, and one frame of data of the laser radar is a group of laser point clouds.

Sub map (Submap): the global map is composed of several sub-maps, each sub-map containing observations of successive multiframes.

Registration (Registration): and matching the observation results of the same area at different moments and different positions to obtain the relative pose relationship between the two observation moments.

NDT: normal Distributions Transform, a normal distribution transformation algorithm, is a registration algorithm that is applied to a statistical model of three-dimensional points, using standard optimization techniques to determine the optimal match between two point clouds.

NovAtel: in the field of precision Global Navigation Satellite Systems (GNSS) and their subsystems, there are leading suppliers of products and technologies. The embodiment of the application shows a NovAtel integrated navigation system.

In some embodiments of the application, the term "vehicle" is to be construed broadly to include any moving object, including for example aircraft, watercraft, spacecraft, automobiles, trucks, vans, semi-trailers, motorcycles, golf carts, off-road vehicles, warehouse transportation vehicles or agricultural vehicles, and vehicles traveling on rails, such as electric cars or trains, and other rail vehicles. The "vehicle" in the present application may generally include: power systems, sensor systems, control systems, peripherals, and computer systems. In other embodiments, the vehicle may include more, fewer, or different systems.

Wherein the power system is a system for providing power motion to a vehicle, comprising: engine/motor, transmission and wheels/tires, and energy unit.

The control system may include a combination of devices that control the vehicle and its components, such as steering units, throttle valves, braking units.

The peripheral device may be a device that allows the vehicle to interact with external sensors, other vehicles, external computing devices, and/or users, such as a wireless communication system, touch screen, microphone, and/or speaker.

Based on the above-described vehicle, a sensor system and an automatic driving control device are also provided in the automatic driving vehicle.

The sensor system may include a plurality of sensors for sensing information of the environment in which the vehicle is located, and one or more actuators that change the position and/or orientation of the sensors. The sensor system may include any combination of sensors such as global positioning system sensors, inertial measurement units, radio detection and ranging (RADAR) units, cameras, laser rangefinders, light detection and ranging (LIDAR) units, and/or acoustic sensors; the sensor system may also include sensors (e.g., O ₂ monitors, fuel gauges, engine thermometers, etc.) that monitor the vehicle interior system.

The autopilot control arrangement may include a processor and memory having at least one machine executable instruction stored therein, the processor executing the at least one machine executable instruction to perform functions including a map engine, a positioning module, a perception module, a navigation or routing module, an autopilot module, and the like. The map engine and the positioning module are used for providing map information and positioning information. The sensing module is used for sensing things in the environment of the vehicle according to the information acquired by the sensor system and map information provided by the map engine. The navigation or path module is used for planning a driving path for the vehicle according to the processing results of the map engine, the positioning module and the sensing module. The automatic control module analyzes and converts decision information input of modules such as a navigation or path module and the like into control command output of a vehicle control system, and sends the control command to corresponding components in the vehicle control system through a vehicle-mounted network (such as a vehicle internal electronic network system realized by a CAN bus, a local area interconnection network, a multimedia orientation system transmission mode and the like) to realize automatic control of the vehicle; the automatic control module can also acquire information of each component in the vehicle through the vehicle-mounted network.

At present, in the field of automatic driving, there are two main types of commonly used high-precision point cloud map construction algorithms: the method comprises the steps of obtaining the pose of a vehicle by using a high-precision integrated navigation system, simultaneously combining multi-line laser radar observation data to construct a global high-precision point cloud map, using a plurality of laser radar devices to scan three-dimensional data of surrounding scenes in real time as shown in a document [1]([1]Haala,Norbert,et al."Mobile LiDAR mapping for 3D point cloud collection in urban areas—A performance test."Int.Arch.Photogramm.Remote Sens.Spat.Inf.Sci 37(2008):1119-1127.), simultaneously measuring the real-time pose of the map vehicle by using the integrated navigation system consisting of a high-precision GPS and an IMU, calculating the real-time pose of the laser radar according to external parameters between the integrated navigation system and the laser radar, and then superposing the laser point cloud according to the pose to obtain a global map; another is to use the mileage calculation method described in document [2]([2]Zhang,Ji,and Sanjiv Singh."Low-drift and real-time lidar odometry and mapping."Autonomous Robots 41.2(2017):401-416.), that is, continuously register the laser radar point cloud observed by the current frame with the laser point cloud at the previous time or several times, so as to incrementally estimate the real-time pose of the current vehicle and construct a global map.

Under the scenes such as tunnels, the integrated navigation system may not receive satellite signals due to poor satellite signals, and pose estimation errors are extremely large, so that the graph construction result is inaccurate in the area with poor satellite signals depending on the graph construction mode of the high-precision integrated navigation system;

in addition, the scheme relying on the mileage calculation method inevitably has accumulated errors, under the scenes of long-distance tunnels and the like, the accumulated errors of the mileage calculation method at the tunnel outlet are overlarge, usually more than hundreds of meters, and smooth and high-consistency point cloud maps cannot be built inside and outside the tunnels, so that the actual requirements of algorithm modules such as high-precision positioning and path planning in unmanned driving cannot be met.

The embodiment of the application aims to provide a map construction method to solve the problems that in the prior art, the map construction of a high-precision point cloud map is difficult and the accuracy is poor under the environment with poor satellite signals and long distance such as a long tunnel.

As shown in fig. 1, an embodiment of the present application provides a map construction method, including:

and 101, acquiring sensor data acquired by various sensors carried on the movable object when the movable object runs on a preset road section.

Step 102, an odometer method based on multi-sensor fusion is adopted, a plurality of local point cloud maps of a preset road section are built in an incremental mode, and frame pose constraints of two adjacent frames in the same local point cloud map are built.

And 103, calculating the pose relation between the adjacent local point cloud maps through point cloud registration, and establishing sub-map pose constraint of the adjacent local point cloud maps.

And 104, obtaining pose observation data of the movable object in a preset road range before and after a preset road section, and establishing a local point cloud map pose observation constraint and a frame pose observation constraint.

And 105, carrying out global optimization based on the sub-map according to the sub-map pose constraint and the local point cloud map pose observation constraint, determining the optimized pose of each local point cloud map, and determining the pose of each frame in each local point cloud map after global optimization based on the sub-map.

And 106, performing global optimization on the global frame pose according to the frame pose constraint and the frame pose observation constraint, and determining the optimized pose of each frame.

And 107, optimizing the pose and the sensor data according to each frame, and fusing map data to form a global map.

In order that those skilled in the art can more clearly understand the embodiments of the present application, the embodiments of the present application will be further described with reference to specific steps, examples and drawings. As shown in fig. 2, the embodiment of the present application provides a map construction method, which is applied to a movable object carrying various sensors, and the movable object may refer to an autonomous vehicle, an intelligent robot, an unmanned plane, etc., but is not limited thereto. The preset road section may refer to a road section to be mapped, such as a tunnel, a highway with poor satellite signals, a national road, etc., but is not limited thereto. In addition, various sensors carried on the movable object may include an inertial measurement unit IMU, a wheel speed meter, a laser radar, and a barometer; wherein the IMU includes an accelerometer and a gyroscope.

The method comprises the following steps:

step 201, three-axis acceleration data measured by an accelerometer, three-axis angular velocity data measured by a gyroscope, wheel speed data of a movable object measured by a wheel speed meter, point cloud data measured by a laser radar and altitude observation data measured by a barometer are obtained when the movable object runs on a preset road section.

After step 201, steps 202 to 205 are continued.

Step 202, modeling is carried out according to triaxial acceleration data measured by an accelerometer, and roll angle constraint and pitch angle constraint of the movable object are established.

The accelerometer in the IMU can measure triaxial acceleration data under the IMU coordinate system in real time, and the measured triaxial acceleration data generally consists of two parts of gravity acceleration and acceleration of the movable object, but because the acceleration of the movable object is usually far smaller than the gravity acceleration, the influence of the acceleration of the movable object can be ignored.

Specifically, step 202 may be implemented as follows:

modeling is performed based on triaxial acceleration data measured by the accelerometer.

The mathematical model established has the following relation:

in the above mathematical model, a _x、a_y、a_z represents the triaxial acceleration data measured by the accelerometer; a rotation matrix from an IMU coordinate system to a world coordinate system; g represents the normalized gravitational acceleration; a _r represents a vehicle body acceleration.

By simplifying the mathematical model, the estimated roll angle value theta _roll and the estimated pitch angle value theta _pitch of the IMU under the world coordinate system can be determined; wherein,A _x、a_y、a_z represents the triaxial acceleration data measured by the accelerometer.

In order to reduce the degree of freedom of joint optimization in the subsequent step and avoid rapid degradation of the odometer method due to sparse features in the scenes of tunnels, cross-sea bridges and the like, the application provides that the roll angle estimated value theta _roll and the pitch angle estimated value theta _pitch are added into the subsequent joint optimization process as fixed constraints. In addition, as the state variable of the gesture in the combined optimization is required to be represented by a quaternion, the quaternion is required to be converted into a rotation matrix, and then the rotation matrix is converted into an Euler angle form, so that the roll angle constraint r _Roll (X) and the pitch angle constraint r _Pitch (X) of the movable object can be established according to the roll angle estimated value theta _roll and the pitch angle estimated value theta _pitch; wherein ,r_Roll(X)＝θ_roll-arcsin(-R₁₃);r_Pitch(X)＝θ_pitch-arctan2(R₂₃,R₃₃);X represents the pose of the IMU in the world coordinate system, X is a state variable to be optimized, and X comprises a position p and a pose q; r is the form of a rotation matrix of the gesture q in the state variable X to be optimized, and R ₂₃、R₃₃、R₁₃ is the elements of the corresponding rows and columns in the rotation matrix R respectively.

And 203, performing kinematic modeling by adopting an Ackerman model according to triaxial angular velocity data measured by a gyroscope and movable object wheel speed data measured by a wheel speed meter, and establishing an Ackerman model constraint of the horizontal position and the yaw angle of the movable object.

Specifically, step 203 may be implemented in the following manner:

the application can perform the kinematic modeling of the movable object based on the ackermann model. For ease of calculation, in the ackerman kinematic model, a vehicle body coordinate system is generally established with the center of a rear axle (e.g., a vehicle rear axle) of a movable object as an origin.

In general, default inputs of the ackerman kinematic model are the speed of a movable object and the steering wheel angle, but in practical application, the inventor finds that the accuracy of the steering wheel angle is generally difficult to guarantee, and in order to improve the accuracy and the robustness of the whole odometer method, the application uses an angle integral value of an included angle between the advancing direction of the movable object and the y axis in a world coordinate system to replace the steering wheel angle. Therefore, it is necessary to determine the integral value of the angle between the advancing direction of the movable object and the y-axis in the world coordinate system based on the three-axis angular velocity data measured by the gyroscope: Wherein, theta _i represents the integral value of the angle between the advancing direction of the movable object and the y axis at the i-th moment; t represents the t time; /(I) The rotation transformation relation from the vehicle body coordinate system to the IMU coordinate system is obtained in advance; /(I)Yaw angle in the triaxial angular velocity data measured for the gyro at time t.

Then, in the Ackerman kinematics model, the speed of the left rear wheel of the movable object at the ith moment measured by the wheel speed meter under the vehicle body coordinate system can be measuredAnd speed of right rear wheel in vehicle body coordinate systemDetermining the speed v _i of the center of the rear axle of the movable object under the vehicle body coordinate system; whereinIs a velocity noise known in advance.

Then, by adopting the kinematic modeling of the ackerman model, the pose transfer equation of the movable object under the world coordinate system can be determined:

x_i+1＝x_i+v_i·Δt·sinθ_i

y_i+1＝y_i+v_i·Δt·cosθ_i

Wherein deltat is the time difference between two adjacent measurement moments of the wheel speed meter; x _i、y_i represents the horizontal position of the movable object in the world coordinate system.

Since the measurement frequency of the IMU and the wheel speed meter is generally higher than the laser radar frequency, the x _i、y_i、θ_i between the kth time and the k+1 time of two adjacent laser radars can be integrated according to the measurement frequency of the laser radars, so as to determine the respective change delta x _k(k+1)、δ_yk(k+1)、δθ_k(k+1) of x _i、y_i、θ_i under the world coordinate system.

Then, the pose transformation relation from the IMU coordinate system to the vehicle body coordinate system can be determined according to the external parameters between the vehicle body coordinate system and the IMU coordinate systemAnd determining pose transformation relation/>, under world coordinate system, of IMU between the kth moment and the (k+1) th momentWherein:

thus, ackermann model constraints r _Akerman (X) for movable objects can be established; wherein:

X represents the pose of the IMU in the world coordinate system, and X is a state variable to be optimized. For example, in the formula, X _k、X_k+1 is the pose of the IMU at the kth and the kth+1 time in the world coordinate system, respectively.

And 204, modeling according to the point cloud data measured by the laser radar, and establishing the laser radar pose constraint of the movable object.

Here, this step 204 may be implemented in the following manner, for example, including the following steps:

step 2041, performing motion compensation on each frame of point cloud data measured by the laser radar, and determining the position of each frame of point cloud data after the motion compensation.

The reason for the need for motion compensation is: the lidar is generally in a mechanical structure, and a certain period of time (usually 0.1s or 0.05 s) is required for completing one frame of scanning, and during the period of time, due to the high-speed movement of a movable object (such as a vehicle), the acquired original data of the lidar can be affected by the movement, so that a deviation exists between a measured value and a true value. In order to reduce the influence of movable motion, the application can estimate the pose transformation relation of the IMU under the world coordinate system according to the Ackerman modelAnd performing motion compensation on the original data measured by the laser radar. Because the time interval between the two scans is very short, the motion between the two frames can be assumed to be linear motion, and the pose of the point acquired by the laser radar in one frame relative to the starting moment of the frame can be obtained through timestamp interpolation, so that all the points acquired by the laser radar in one frame are converted to the starting moment of the frame, and the positions of the points after motion compensation are determined.

And 2042, performing feature extraction on the motion-compensated point cloud data of each frame, and dividing the points in the point cloud data of each frame into line feature points and plane feature points according to curvature information of the points in the point cloud data of each frame.

This step 2042 may be implemented specifically as follows:

And obtaining any point on a wire harness and a plurality of points within a preset range of any point on the wire harness from one frame of point cloud data after motion compensation. Here, since the laser points measured by the laser radar are arranged according to the beam, a plurality of points within a preset range of each laser point can be found according to the beam, for example, a plurality of laser points on the left and right sides of the beam (for example, 5 laser points on the left and right sides of the beam, but not limited thereto).

And determining the curvature of any point according to the coordinates of the any point under the laser radar coordinate system and the coordinates of a plurality of points in a preset range of any point on the wire harness under the laser radar coordinate system. For example, the curvature at any point can be determined using the following curvature calculation formula: Wherein c represents/> Curvature at the point; /(I)Respectively representing coordinates of the ith and jth points on the kth line in the current frame under a laser radar coordinate system, wherein S represents a point set formed by a plurality of points on the left side and the right side of the ith point, and S represents the number of the points contained in the point set.

According to a preset curvature threshold value, when the curvature of a point is larger than the curvature threshold value, the point is taken as a line characteristic point, and when the curvature of the point is smaller than the curvature threshold value, the point is taken as a plane characteristic point.

And 2043, superposing preset frame point cloud data before the current frame point cloud data according to the pose obtained by pose estimation, and determining a local line characteristic map and a local facial characteristic map corresponding to the current frame point cloud data.

Specifically, pose estimation is performed incrementally, so that line feature points, surface feature points and corresponding poses of each frame point cloud before the current frame are known, and therefore preset frame point cloud data (such as 15 frame point cloud data) before the current frame point cloud data can be overlapped according to the poses obtained by pose estimation, and a corresponding local line feature map (composed of line feature points) and a local surface feature map (composed of plane feature points) can be obtained.

Step 2044, obtaining the initial pose of the current frame laser radar under the world coordinate system according to external parameters between the laser radar and the IMU:

Wherein, p _LiDAR is the initial position of the laser radar at the current moment in the world coordinate system, R _LiDAR is the initial gesture of the laser radar at the current moment in the world coordinate system, R _IMU、t_IMU is the gesture and the position of the IMU at the current moment in the world coordinate system respectively, AndAnd respectively obtaining an attitude transformation relation and a position transformation relation in advance through external parameter calibration between the laser radar and the IMU.

Step 2045, searching in the local line feature map to obtain a plurality of adjacent points corresponding to each line feature point in the current frame point cloud data according to the data index established for each point by adopting the KD-Tree algorithm in advance, and searching in the local surface feature map to obtain a plurality of adjacent points corresponding to each plane feature point in the current frame point cloud data.

Step 2046, fitting a line according to a plurality of adjacent points (for example, 5) corresponding to the line characteristic point x ^l in the current frame point cloud data, and taking a distance function between the line characteristic point x ^l and the line as a line characteristic point error function;

The line characteristic point error function is: Wherein/> AndIs any two points on the straight line.

In step 2047, a plane ax+by+cz+d=0 is obtained By fitting (e.g. By SVD decomposition) a plurality of neighboring points (e.g. 5) corresponding to the plane feature point x ^p in the current frame point cloud data, and the distance function between the plane feature point x ^p and the plane is used as the plane feature point error function.

Wherein A, B, C and D represent parameters of the plane obtained by fitting.

The surface characteristic point error function is as follows: Wherein n represents a matrix: n= (a, B, C).

And 2048, establishing a laser radar pose constraint r _LiDAR (X) of the movable object according to the line characteristic point error function and the plane characteristic point error function.

Wherein:

X represents the pose of the IMU under the world coordinate system, and X is a state variable to be optimized; n _line represents the number of line feature points in the current frame point cloud data, and n _plane represents the number of plane feature points in the current frame point cloud data.

Step 205, modeling according to the height observation data measured by the barometer, and establishing barometer constraint of the height position of the movable object.

Specifically, barometers may obtain the current altitude by measuring the barometric pressure. Although factors such as abrupt temperature changes, airflow shocks, etc. affect the absolute accuracy of barometer altitude measurements, the relative accuracy of barometer observations is typically high. The low accuracy of the height estimation is always a prominent problem of the current main current mileage calculation method, so in order to improve the accuracy of the height estimation of the odometer and reduce the accumulated error of the system, the following method can be adopted in the embodiment of the application:

According to the current time altitude observation data Z _k+1 measured by the barometer, the initial time altitude observation data Z ₀ measured by the barometer in advance and the altitude estimation value of the current time measured by the IMU under the world coordinate system Altitude estimation/>, under world coordinate system, of initial moment measured in advance by IMUModeling, establishing barometer constraints r _Altimeter (X) of the height position of the movable object; wherein:

X represents the pose of the IMU under the world coordinate system, and X is a state variable to be optimized; Rotation data and translation data of the barometer coordinate system to the world coordinate system at the current moment are obtained in advance respectively.

And 206, carrying out joint optimization solution on the roll angle constraint, the pitch angle constraint, the Ackerman model constraint, the laser radar pose constraint and the barometer constraint by adopting a nonlinear optimization method, and determining a pose result of the movable object.

Specifically, the nonlinear least square problem can be solved on the joint optimization cost function by adopting an optimization algorithm for the roll angle constraint r _Roll (X), the pitch angle constraint r _Pitch (X), the Ackerman model constraint r _Akerman (X), the laser radar pose constraint r _LiDAR (X) and the barometer constraint r _Altimeter (X), and the pose result (namely the maximum posterior probability estimation of the current state variable X to be optimized) of the IMU of the movable object under the world coordinate system can be determined. The optimization algorithm may be a gaussian newton algorithm or a Levenberg-Marquardt algorithm (L-M algorithm, levenberg-Marquardt method), but is not limited thereto.

Wherein, the joint optimization cost function is:

Wherein, And respectively corresponding preset information matrixes of the constraint items.

In this way, by the multi-sensor (including the laser radar, the IMU, the wheel speed meter and the barometer) fusion-based odometer method implemented in the steps 201 to 206, the accurate relative pose between frames acquired by the laser radar can be obtained, and the real-time pose estimation under the conditions of sparse characteristics, such as tunnels, cross-sea bridges and the like, poor GPS signals, and the obtained pose result has good accuracy and robustness.

In an embodiment of the present application, the inventor performs experimental verification on the above-mentioned odometer method based on multi-sensor fusion, and the process is as follows:

In order to verify the accuracy and the robustness of the multi-sensor fusion-based odometer method, in the embodiment of the application, a data acquisition vehicle provided with sensors such as a laser radar, an IMU, a wheel speed meter, a barometer and the like is used, a section of extra-long tunnel data is acquired for experimental verification, the total length of the tunnel is about 9.2Km, as shown in fig. 3, the scene characteristics in the tunnel are sparse, and the walls on two sides are smooth planes.

After the multi-sensor fusion-based odometer method in the scenario shown in fig. 3 is adopted, a comparison experiment is performed on the same data by combining a laser odometer method LOAM and a laser inertial navigation odometer method LIO-Mapping which are the most representative in the prior art. The experimental results are shown in fig. 4, wherein the abscissa of fig. 4 is used for representing the position information of the IMU in the pose in the world coordinate system, group-Truth represents the true value of the pose, and Sensor-Fusion-Odometry represents the multi-Sensor Fusion-based odometer method according to the embodiment of the application. In the experimental scene, the LOAM algorithm and the LIO-Mapping algorithm are seriously degraded and cannot run the complete process, so that the pose of the IMU carried by the data acquisition vehicle is lost under the world coordinate system, and the requirement of tunnel map building is not met; under the same conditions, the odometer method based on multi-sensor fusion provided by the embodiment of the application can run completely, and the final pose estimation result inevitably has accumulated errors, but the accurate relative pose among frames in the tunnel is obtained, thus laying a foundation for the subsequent map construction.

Step 207, incrementally constructing a plurality of local point cloud maps of the preset road section according to the pose result of the movable object.

For example, after the pose result of the movable object is obtained in real time, the pose result of the movable object in each frame may be determined, so that a plurality of local point cloud maps of the preset road section may be built in an incremental manner, for example, the preset road section is 10000m, one local point cloud map may be built every 100m, and a total of 100 local point cloud maps may be built, but not limited thereto.

And step 208, establishing frame pose constraints of two adjacent frames in the same local point cloud map.

Because in the mileage calculation method, the pose estimation precision of the initial frames is usually not high, in order to ensure the robustness of the system, the pose results of the movable objects corresponding to the frames in the same local point cloud map are sequenced according to the frame acquisition sequence; discarding the first n frames (such as the first 5 frames, but not limited to) in the pose result of the movable object corresponding to each frame after sequencing; where n is a preset frame threshold, for example, n=5, but is not limited thereto.

In this way, the frame pose constraint of two adjacent frames in the same local point cloud map can be established according to the pose results of the movable objects corresponding to the sequenced frames after the previous n frames are discarded

Wherein: x _i,x_i+1 is the pose of the movable object corresponding to the ith frame and the (i+1) th frame in the local point cloud map respectively; /(I) For the pre-calculated relative pose relationship of two adjacent frames, the relative pose relationship/>, of the two adjacent frames can be determined by the multi-sensor fusion-based odometer method

And 209, calculating the pose relation between the adjacent local point cloud maps through point cloud registration, and establishing sub-map pose constraints of the adjacent local point cloud maps.

Here, this step 209 may be implemented in the following manner, for example:

Registering adjacent local point cloud maps through an NDT algorithm, and determining a pose transformation relation T _NDT between the adjacent local point cloud maps; here, the manner of determining the pose transformation relationship T _NDT between adjacent partial point cloud maps by the NDT algorithm is: firstly, selecting a point cloud of a local point cloud map to perform three-dimensional grid division, then calculating a normal distribution for each grid, then downsampling laser points in the other local point cloud map, projecting all the downsampled points into the three-dimensional grid according to initial pose transformation provided by the integrated navigation system, simultaneously calculating probability density functions of each point, and calculating the maximum likelihood of products of all probability density functions, thereby obtaining a pose transformation relation T _NDT between adjacent local point cloud maps.

Sub-map pose constraint for establishing adjacent local point cloud map

Wherein: X _i,X_i+1 respectively represents the pose of the ith local point cloud map and the pose of the (i+1) th local point cloud map; /(I) And representing the pose transformation relationship between the ith local point cloud map and the (i+1) th local point cloud map.

Step 210, obtaining pose observation data of a movable object in a preset road range before and after a preset road section, and establishing a local point cloud map pose observation constraint and a frame pose observation constraint.

In step 210, pose observation data of a preset road range of a movable object before and after a preset road section is obtained, and a local point cloud map pose observation constraint is established, which may be as follows:

obtaining the observation pose corresponding to the local point cloud map of the movable object in the preset road range before and after the preset road section (I.e., the combined navigation system can be used to measure the observation pose corresponding to each local point cloud map, for example, the pose of the first frame of each local point cloud map can be used as the observation pose corresponding to the local point cloud map, but the method is not limited to this); wherein i represents an i-th local point cloud map;

establishing local point cloud map pose observation constraint

Wherein,X _i is used for representing the pose of the ith local point cloud map.

In step 210, pose observation data of a preset road range of a movable object before and after a preset road section is obtained, and a frame pose observation constraint is established, which may be as follows:

obtaining the observation pose corresponding to the frame of the preset road range of the movable object before and after the preset road section (I.e., a combined navigation system can be used to measure the observed pose corresponding to each frame, but is not limited thereto); where i represents the global ith frame.

Establishing frame pose observation constraint

Wherein,X _i is used to represent the pose of the global ith frame.

Step 211, carrying out global optimization based on the sub map according to the sub map pose constraint and the local point cloud map pose observation constraint, determining the optimized pose of each local point cloud map, and determining the pose of each frame in each local point cloud map after global optimization based on the sub map.

Here, this step 211 may be as follows:

position and pose constraint according to map sub-map And local point cloud map pose observation constraintsDetermining sub-map joint optimization constraints:

Wherein n _Submap represents the number of global sub-maps, for example, if the preset road section is 10000m, a local point cloud map can be built every 100m, and if 100 local point cloud maps are built in total, the number of global sub-maps is 100, but the method is not limited thereto; n _GPS-Submap represents the number of observation pose corresponding to the local point cloud map of the preset road range of the movable object before and after the preset road section, for example, among the 100 local point cloud maps, the observation pose corresponding to the first 2 local point cloud maps and the last 2 local point cloud maps is reliable and accurate, and then the observation pose of the 4 local point cloud maps can be applied, n _GPS-Submap is equal to 4, but is not limited thereto; x _i,X_i+1 respectively represents the pose of the ith local point cloud map and the pose of the (i+1) th local point cloud map; c _Submap、C_GPS-Submap is a preset information matrix, and is a diagonal matrix with diagonal elements being fixed in value, and the diagonal matrix is used for giving weights to two constraints.

And calculating the sub-map joint optimization constraint, and solving to obtain the optimization pose X _i of each local point cloud map. The method is a nonlinear least square problem, and can be solved by using a Gauss Newton algorithm and the like, so that the maximum posterior probability estimation of the current state quantity X _i can be obtained and used as the optimization pose of each local point cloud map.

And determining the pose of each frame in the local point cloud map according to the frame pose constraint of two adjacent frames in the same local point cloud map. After obtaining the optimized pose of each local point cloud map, the optimized pose of the first frame of each local point cloud map is generally obtained, so that the pose of each frame can be obtained as an initial value according to the frame pose constraint of two adjacent frames, and the initial value needs to be optimized continuously. The method is characterized in that after the optimization pose of each local point cloud map is obtained, the influence of accumulated errors of the odometer on the map construction precision is eliminated to a large extent, but certain accumulated errors still exist in each local point cloud map, and the errors are not negligible for a high-precision map. In order to reduce the influence of accumulated errors in the local point cloud map on the map construction precision, the application performs global optimization for all frames on the basis of the local point cloud map optimization.

And 212, performing global optimization on the global frame pose according to the frame pose constraint and the frame pose observation constraint, and determining the optimized pose of each frame.

Here, this step 212 may be performed as follows:

Constrained according to frame pose And frame pose observation constraintDetermining a frame global optimization constraint of each frame pose:

wherein n _Frame represents the number of global frames, for example, in the above 100 local point cloud maps, each local point cloud map is composed of 100 frames, and the number of global frames is 10000 frames, but not limited thereto; n _GPS-Frame represents the number of observation pose corresponding to the frame of the preset road range of the movable object before and after the preset road section, for example, in the 100 local point cloud maps, the observation pose corresponding to the first 2 local point cloud maps and the last 2 local point cloud maps are reliable and accurate, and each local point cloud map is composed of 100 frames, then n _GPS-Frame is 400, but not limited to this; x _i,x_i+1 represents the pose of the global ith frame and the pose of the (i+1) th frame, respectively; c _Frame、C_GPS-Frame is a preset information matrix, which is a diagonal matrix with diagonal elements as fixed values and is used for giving weight to two constraints;

And calculating the global optimization constraint of the frames, and solving to obtain the optimization pose x _i of each frame. The nonlinear least square problem can be solved by using a Gaussian Newton algorithm, and the maximum posterior probability estimation of the current state quantity x _i can be obtained and used as the optimal pose of each frame.

And 213, optimizing the pose and the sensor data according to each frame, and fusing map data to form a global map.

The sensor data mainly refer to coordinates of points in each frame of laser point cloud measured by the laser radar under a laser radar coordinate system. This step 213 may be performed as follows:

And mapping the points in each frame of laser point cloud to a world coordinate system according to the frame optimization pose and the coordinates of the points in each frame of laser point cloud measured by the laser radar in the laser radar coordinate system.

And superposing the points in the laser point cloud mapped to the world coordinate system to form a global map.

In order to fully verify the accuracy and robustness of the steps in the embodiment of the application, the embodiment of the application uses a data acquisition vehicle equipped with sensors such as LiDAR, IMU, wheel speed meter, barometer and the like, acquires data of a certain high-speed special tunnel to perform experimental verification, the total length of the tunnel is about 9.2Km, and the experimental result is shown in fig. 5, wherein the abscissa and ordinate of fig. 5 are used for representing the position information of the IMU in the pose of the world coordinate system. As can be seen from the experimental results, the combined Navigation can not receive satellite signals when running in the tunnel, so that the pose estimation error is extremely large (see curve Integrated-Navigation); although the mileage calculation method based on the multi-Sensor Fusion can accurately estimate the relative pose between adjacent frames, the accumulated error at the tunnel exit is larger and cannot be directly used for drawing (see curve Sensor-Fusion-Odometry). On the basis of the mileage calculation method based on multi-sensor fusion, the embodiment of the application continues to perform global optimization based on frames, and determines the optimization pose of each frame, thereby forming a global map (see curve Tunnel-Mapping) very similar to a true value.

In addition, as shown in fig. 6, an embodiment of the present application further provides a map building apparatus, including:

and a data obtaining unit 31 for obtaining sensor data collected by various sensors mounted on the movable object when the movable object travels on a preset road section.

The multisensor fusion unit 32 is configured to incrementally construct a plurality of local point cloud maps of a preset road section based on a multisensor fusion method, and to construct a frame pose constraint of two adjacent frames in the same local point cloud map.

The sub-map pose constraint establishing unit 33 is configured to calculate pose relationships between adjacent partial point cloud maps through point cloud registration, and establish sub-map pose constraints of the adjacent partial point cloud maps.

The pose observation constraint establishing unit 34 is configured to obtain pose observation data of a preset road range of the movable object before and after a preset road section, and establish a local point cloud map pose observation constraint and a frame pose observation constraint.

The global optimization unit 35 based on the sub map is configured to perform global optimization based on the sub map according to the sub map pose constraint and the local point cloud map pose observation constraint, determine the optimized pose of each local point cloud map, and determine the pose of each frame in each local point cloud map after global optimization based on the sub map.

And a global frame-based optimization unit 36, configured to perform global frame-based optimization on global frame poses according to the frame pose constraints and the frame pose observation constraints, and determine each frame optimization pose.

The global map forming unit 37 is configured to perform map data fusion according to the pose and sensor data optimized by each frame, so as to form a global map.

It should be noted that, the specific implementation manner of the map construction device provided by the embodiment of the present application may refer to the method embodiments corresponding to fig. 1 to 5, and are not described herein again.

In addition, the embodiment of the present application further provides a computer readable storage medium, which includes a program or instructions, where the program or instructions implement the method described in fig. 1 to 5.

In addition, the embodiment of the application also provides a computer program product containing instructions, which when run on a computer, cause the computer to perform the method as described in the above figures 1 to 5.

In addition, the embodiment of the application also provides a computer server, which comprises a memory and one or more processors which are in communication connection with the memory;

The memory has stored therein instructions executable by the one or more processors to cause the one or more processors to implement the methods described above with respect to fig. 1-5.

It will be appreciated by those skilled in the art that embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.

The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems) and computer program products according to embodiments of the application. It will be understood that each flow and/or block of the flowchart illustrations and/or block diagrams, and combinations of flows and/or blocks in the flowchart illustrations and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.

These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.

These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.

The principles and embodiments of the present application have been described in detail with reference to specific examples, which are provided to facilitate understanding of the method and core ideas of the present application; meanwhile, as those skilled in the art will have variations in the specific embodiments and application scope in accordance with the ideas of the present application, the present description should not be construed as limiting the present application in view of the above.

Claims

1. A map construction method, comprising:

calculating the pose relation between adjacent local point cloud maps through point cloud registration, and establishing sub-map pose constraints of the adjacent local point cloud maps, comprising:

Registering adjacent local point cloud maps through a normal distribution transformation algorithm, and determining pose transformation relations between the adjacent local point cloud maps; and

Establishing sub-map pose constraints of adjacent local point cloud maps through the pose transformation relationship;

Obtaining pose observation data of a movable object in a preset road range before and after a preset road section, and establishing a local point cloud map pose observation constraint and a frame pose observation constraint, wherein the method comprises the following steps:

obtaining observation pose corresponding to a local point cloud map of a movable object in a preset road range before and after a preset road section;

Establishing a local point cloud map pose observation constraint according to the observation pose corresponding to the local point cloud map;

obtaining the observation pose corresponding to the frame of the movable object in the preset road range before and after the preset road section; and

Establishing frame pose observation constraint through the observation pose corresponding to the frame;

Performing global optimization based on frames on the global frame pose according to the frame pose constraint and the frame pose observation constraint, and determining the optimized pose of each frame; and

2. The method of claim 1, wherein the various sensors include inertial measurement units IMU, wheel speed meters, lidar, and barometer; wherein the IMU includes an accelerometer and a gyroscope.

3. The method according to claim 2, wherein the obtaining sensor data collected by various sensors mounted on the movable object while the movable object is traveling on the preset road section includes:

And acquiring triaxial acceleration data measured by an accelerometer, triaxial angular velocity data measured by a gyroscope, wheel speed data of the movable object measured by a wheel speed meter, point cloud data measured by a laser radar and altitude observation data measured by a barometer when the movable object runs on a preset road section.

4. The method of claim 3, wherein the multisensor fusion-based odometer method incrementally constructs a plurality of local point cloud maps for a preset road segment, comprising:

modeling the sensor data acquired by various sensors respectively, and establishing a constraint relation of the pose of the movable object;

carrying out joint optimization solving on the constraint relation of the pose of the movable object, and determining the pose result of the movable object;

and incrementally constructing a plurality of local point cloud maps of the preset road section according to the pose result of the movable object.

5. The method of claim 4, wherein modeling the sensor data collected by the various sensors, respectively, establishes a constraint relationship of the pose of the movable object, and comprises:

Modeling is carried out according to triaxial acceleration data measured by an accelerometer, and roll angle constraint and pitch angle constraint of a movable object are established;

according to triaxial angular velocity data measured by a gyroscope and movable object wheel speed data measured by a wheel speed meter, performing kinematic modeling by adopting an Ackerman model, and establishing Ackerman model constraint of the horizontal position and yaw angle of the movable object;

modeling according to the point cloud data measured by the laser radar, and establishing laser radar pose constraint of the movable object;

Modeling is performed according to the height observation data measured by the barometer, and barometer constraint of the height position of the movable object is established.

6. The method of claim 5, wherein the performing joint optimization solution on the constraint relation of the pose of the movable object to determine the pose result of the movable object comprises:

And carrying out joint optimization solution on the roll angle constraint, the pitch angle constraint, the Ackerman model constraint, the laser radar pose constraint and the barometer constraint by adopting a nonlinear optimization method, and determining a pose result of the movable object.

7. The method of claim 6, wherein modeling from the accelerometer-measured triaxial acceleration data to establish roll angle constraints and pitch angle constraints for the movable object comprises:

modeling is carried out according to triaxial acceleration data measured by an accelerometer, and a roll angle estimated value theta _roll and a pitch angle estimated value theta _pitch of the IMU under a world coordinate system are determined; wherein, A _x、a_y、a_z represents triaxial acceleration data measured by the accelerometer;

Establishing roll angle constraint r _Roll (X) and pitch angle constraint r _Pitch (X) of the movable object according to the roll angle estimated value theta _roll and the pitch angle estimated value theta _pitch; wherein ,r_Roll(X)＝θ_roll-arcsin(-R₁₃);r_Pitch(X)＝θ_pitch-arctan2(R₂₃,R₃₃);X represents the pose of the IMU in the world coordinate system, X is a state variable to be optimized, and X comprises a position p and a pose q; r is the form of a rotation matrix of the gesture q in the state variable X to be optimized, and R ₂₃、R₃₃、R₁₃ is the elements of the corresponding rows and columns in the rotation matrix R respectively.

8. The method of claim 6, wherein the step of using the ackerman model for kinematic modeling based on the three-axis angular velocity data measured by the gyroscope and the movable object wheel speed data measured by the wheel speed meter to establish the ackerman model constraints of the horizontal position and the yaw angle of the movable object comprises:

according to triaxial angular velocity data measured by a gyroscope, determining an angle integral value of an included angle between the advancing direction of a movable object and a y-axis under a world coordinate system: Wherein, theta _i represents the integral value of the angle between the advancing direction of the movable object and the y axis at the i-th moment; t represents the t time; /(I) The rotation transformation relation from the vehicle body coordinate system to the IMU coordinate system is obtained in advance; /(I)Yaw angle in the triaxial angular velocity data measured for the gyroscope at the t-th moment;

Speed of the left rear wheel of the movable object in the vehicle body coordinate system according to the ith moment measured by the wheel speed meter And speed of right rear wheel in vehicle body coordinate systemDetermining the speed v _i of the center of the rear axle of the movable object under the vehicle body coordinate system; wherein, Is a velocity noise known in advance;

And adopting an Ackerman model to carry out kinematic modeling, and determining a pose transfer equation of the movable object under the world coordinate system:

x_i+1＝x_i+v_i·Δt·sinθ_i

y_i+1＝y_i+v_i·Δt·cosθ_i

Wherein deltat is the time difference between two adjacent measurement moments of the wheel speed meter; x _i、y_i represents the horizontal position of the movable object in the world coordinate system;

Integrating x _i、y_i、θ_i between the kth time and the (k+1) th time of two adjacent laser radars according to the measuring frequency of the laser radars, and determining respective change amounts delta x _k(k+1)、δy_k(k+1)、δθ_k(k+1) of x _i、y_i、θ_i under a world coordinate system;

Determining pose transformation relation from an IMU coordinate system to a vehicle body coordinate system according to external parameters between the vehicle body coordinate system and the IMU coordinate system And determining the pose transformation relation of the IMU from the kth moment to the (k+1) th moment in the world coordinate systemWherein:

Establishing an Ackerman model constraint r _Akerman (X) of the movable object; wherein:

x represents the pose of the IMU in the world coordinate system, and X is a state variable to be optimized.

9. The method of claim 6, wherein modeling from point cloud data of lidar measurements to establish lidar pose constraints for the movable object comprises:

performing motion compensation on each frame of point cloud data measured by the laser radar, and determining the position of each frame of point cloud data after the motion compensation;

Feature extraction is carried out on each frame of point cloud data subjected to motion compensation, and points in each frame of point cloud data are divided into line feature points and plane feature points according to curvature information of the points in each frame of point cloud data;

overlapping preset frame point cloud data before the current frame point cloud data according to the pose obtained by pose estimation, and determining a local line characteristic map and a local face characteristic map corresponding to the current frame point cloud data;

According to the external parameters between the laser radar and the IMU, obtaining the initial pose of the laser radar of the current frame under the world coordinate system:

Wherein, p _LiDAR is the initial position of the laser radar at the current moment in the world coordinate system, R _LiDAR is the initial gesture of the laser radar at the current moment in the world coordinate system, R _IMU、t_IMU is the gesture and the position of the IMU at the current moment in the world coordinate system respectively, AndRespectively obtaining an attitude transformation relation and a position transformation relation in advance through external parameter calibration between a laser radar and an IMU;

searching a local line characteristic map to obtain a plurality of adjacent points corresponding to each line characteristic point in the current frame point cloud data according to a data index established for each point by adopting a KD-Tree algorithm in advance, and searching a local surface characteristic map to obtain a plurality of adjacent points corresponding to each plane characteristic point in the current frame point cloud data;

Fitting a plurality of adjacent points corresponding to the line characteristic points x ^l in the point cloud data of the current frame to obtain a straight line, and taking a distance function between the line characteristic points x ^l and the straight line as a line characteristic point error function;

The line characteristic point error function is: Wherein/> AndAny two points on the straight line;

Fitting a plurality of adjacent points corresponding to the plane characteristic points x ^p in the point cloud data of the current frame to obtain a plane ax+by+Cz+D=0, and taking a distance function between the plane characteristic points x ^p and the plane as a plane characteristic point error function; wherein A, B, C and D represent parameters of the plane obtained by fitting;

the surface characteristic point error function is as follows: wherein n represents a matrix: n= (a, B, C);

establishing a laser radar pose constraint r _LiDAR (X) of the movable object according to the line characteristic point error function and the plane characteristic point error function; wherein:

10. The method of claim 6, wherein modeling based on barometer-measured altitude observations establishes barometer constraints for an altitude position of the movable object, comprising:

11. The method of claim 6, wherein the joint optimization solution is performed on the roll angle constraint, pitch angle constraint, ackerman model constraint, laser radar pose constraint and barometer constraint by a nonlinear optimization method, and determining the pose result of the movable object comprises:

Solving a nonlinear least square problem for a joint optimization cost function by adopting an optimization algorithm to determine pose results of an IMU of a movable object under a world coordinate system;

wherein, the joint optimization cost function is:

Wherein, Respectively corresponding preset information matrixes of the constraint items; x represents the pose of the IMU in the world coordinate system, and X is a state variable to be optimized.

12. The method of claim 6, wherein the pose transformation relationship is T _NDT, and the establishing the sub-map pose constraint of the neighboring local point cloud map by the pose transformation relationship comprises:

Sub-map pose constraint for establishing adjacent local point cloud map

13. The method of claim 6, wherein the observation pose corresponding to the local point cloud map isAnd the establishing of the local point cloud map pose observation constraint through the observation pose corresponding to the local point cloud map comprises the following steps:

establishing a local point cloud map pose observation constraint through the observation pose corresponding to the local point cloud map

Wherein,X _i represents the pose of the i-th local point cloud map.

14. The method of claim 6, wherein the observed pose corresponding to the frame isAnd the frame pose observing constraint is established through the observing poses corresponding to the frames, comprising:

establishing frame pose observation constraint

Wherein,J represents the global jth frame and x _j is used to represent the pose of the global jth frame.

15. The method of claim 6, wherein establishing the frame pose constraints for two adjacent frames in the same local point cloud map comprises:

Sequencing pose results of movable objects corresponding to frames in the same local point cloud map according to a frame acquisition sequence;

Discarding the first n frames in the pose results of the movable object corresponding to the sequenced frames; wherein n is a preset frame threshold;

According to the pose results of the movable objects corresponding to the sequenced frames after the previous n frames are discarded, establishing the frame pose constraints of two adjacent frames in the same local point cloud map

Wherein: x _j,x_j+1 is the pose of the movable object corresponding to the j frame and the j+1 frame in the local point cloud map,/>, respectively And the relative pose relation of two adjacent frames is obtained through pre-calculation.

16. The method of claim 15, wherein performing a frame-based global optimization of global frame poses based on the frame pose constraints and frame pose observation constraints, determining frame-optimized poses comprises:

Where n _Frame represents the number of global frames; n _GPS-Frame represents the number of observation pose corresponding to the frame of the preset road range of the movable object before and after the preset road section; x _j,x_j+1 represents the pose of the global jth frame and the pose of the j+1th frame, respectively; c _Frame、C_GPS-Frame is a preset information matrix;

And calculating the global optimization constraint of the frames, and solving to obtain the optimization pose x _j of each frame.

17. The method of claim 6, wherein performing sub-map-based global optimization based on the sub-map pose constraints and the local point cloud map pose observation constraints, determining each local point cloud map optimization pose, and determining each frame pose within each local point cloud map after sub-map-based global optimization, comprises:

position and pose constraint according to map sub-map And local point cloud map pose observation constraintDetermining sub-map joint optimization constraints:

；

Where n _Submap represents the number of global sub-maps; n _GPS-Submap represents the number of observation pose corresponding to a local point cloud map of a preset road range of a movable object before and after a preset road section; x _i,X_i+1 represents the pose of the ith local point cloud map and the pose of the (i+1) th local point cloud map respectively, and C _Submap、C_GPS-Submap is a preset information matrix;

Calculating the sub-map joint optimization constraint, and solving to obtain an optimization pose X _i of each local point cloud map;

and determining the pose of each frame in the local point cloud map according to the frame pose constraint of two adjacent frames in the same local point cloud map.

18. The method of claim 6, wherein performing map data fusion to form a global map based on the frame-optimized pose and sensor data, comprises:

according to the frame optimization pose and the coordinates of the points in each frame of laser point cloud measured by the laser radar under the laser radar coordinate system, mapping the points in each frame of laser point cloud under the world coordinate system;

19. A map construction apparatus, characterized by comprising:

The sub-map pose constraint establishing unit is used for calculating pose relation between adjacent local point cloud maps through point cloud registration and establishing sub-map pose constraint of the adjacent local point cloud maps, and comprises the following steps:

The pose observation constraint establishing unit is used for obtaining pose observation data of a movable object in a preset road range before and after a preset road section and establishing a local point cloud map pose observation constraint and a frame pose observation constraint, and comprises the following steps:

The global optimization unit based on the frames is used for carrying out global optimization based on the frames on the global positions of all frames according to the position constraints of the frames and the position observation constraints of the frames, and determining the optimized positions of all frames; and

20. A computer readable storage medium comprising a program or instructions which, when run on a computer, implement the method of any one of claims 1 to 18.

21. A computer program product comprising instructions which, when run on a computer, cause the computer to perform the method of any of claims 1 to 18.

22. A computer server comprising a memory, and one or more processors communicatively coupled to the memory;

Stored in the memory are instructions executable by the one or more processors to cause the one or more processors to implement the method of any one of claims 1 to 18.