Method, system, device and equipment for enhancing laser point cloud positioning stability

Abstract

The invention discloses a method, system, device and equipment for enhancing laser point cloud positioning stability, and the method comprises the steps: collecting single-frame laser point cloud data of mobile equipment in real time, carrying out the point cloud matching positioning of the single-frame laser point cloud data and a pre-made environment point cloud map, and generating an initial pose of the mobile equipment; synchronously acquiring measurement data of the inertial measurement unit and data of the wheel speed meter, and generating a track plotting pose through a track plotting algorithm based on the synchronously acquired data; inputting the initial pose matched with the point cloud and the track plotting pose into an extended Kalman filtering system, and performing pose fusion estimation to obtain an optimal pose; and monitoring the local feature stability of the point cloud matching in real time, judging the stability, and updating the optimal pose when the point cloud matching is unstable until the point cloud matching is recovered to be stable. According to the invention, the success rate of point cloud matching is enhanced, the positioning stability is improved, and the problem of long-time positioning loss caused by object shielding in the moving process of the mobile equipment can be avoided.

Description

Technical Field

[0001] The present invention relates to the field of positioning technology, and in particular to a method, system, device and equipment for enhancing the stability of laser point cloud positioning. Background Art

[0002] In AGV systems, using multi-line laser radar to achieve high-precision positioning is a good solution. The point cloud matching algorithm can provide AGV vehicles with highly accurate position and posture information. However, due to factors such as dynamic changes in the environment, for example, when a large vehicle passes by, it blocks most of the laser point cloud on one side. At this time, there is a large difference between the real-time frame data of the point cloud scan and the prior map, resulting in unsuccessful matching and loss of positioning results. However, when the large vehicle leaves the area, the environmental features can be matched with the map normally. Therefore, it is impossible to guarantee a stable matching result at any time when only considering the characteristics of the point cloud itself. Especially when the point cloud is not matched accurately in a short period of time (within seconds), if the lost point cloud posture can be quickly recovered, the robustness of the point cloud positioning will be greatly improved.

[0003] Currently, the most common method is to optimize the point cloud matching algorithm itself to improve the accuracy of point cloud matching. Similar methods include point cloud matching models such as ICP and NDT. However, such methods pose great challenges to a single point cloud matching model. Once an error occurs in the intermediate matching process, it is difficult to retrieve the accurate or similar matching pose, and the positioning function cannot be continued, resulting in positioning loss. Summary of the Invention

[0004] In order to solve some or all of the technical problems existing in the above-mentioned prior art, the present invention provides a method, system, device and equipment for enhancing the stability of laser point cloud positioning, which enhances the success rate of point cloud matching, improves the stability of positioning, and can avoid the problem of long-term positioning loss caused by object obstruction during the movement of mobile devices.

[0005] The technical solutions of the present invention are as follows:

[0006] In a first aspect, a method for enhancing the stability of laser point cloud positioning is provided, comprising:

[0007] Collect single-frame laser point cloud data of mobile devices in real time, match and locate the point cloud with the pre-made environmental point cloud map, and generate the initial pose of the mobile device;

[0008] Synchronously collect the measurement data of the inertial measurement unit and the data of the wheel speed meter, and generate the dead reckoning posture through the dead reckoning algorithm based on the measurement data of the inertial measurement unit and the data of the wheel speed meter;

[0009] The initial pose of point cloud matching and the dead reckoning pose are input into the extended Kalman filter system for pose fusion estimation to obtain the optimal pose.

[0010] The stability of local features of point cloud matching is monitored in real time. If the local features are stable, the extended Kalman filter system is updated with the point cloud matching results as observations. If the local features are unstable and the point cloud matching error exceeds the preset threshold, the extended Kalman filter system switches to dead reckoning mode, uses the dead reckoning pose as the estimated value, sets the local search range based on the estimated value, re-executes the point cloud matching operation, and generates a corrected pose.

[0011] The corrected pose is fed back to the extended Kalman filter system to update the optimal pose until the point cloud matching becomes stable.

[0012] Furthermore, in the above method for enhancing the stability of laser point cloud positioning, the point cloud is obtained by sensor scanning, and the local features of the point cloud matching include the part where the point cloud scanned by the current sensor matches the environmental point cloud map.

[0013] Furthermore, in the above method for enhancing the stability of laser point cloud positioning, the mean square error result of the point cloud matching is used as the matching score, and the matching score is used as the preset threshold.

[0014] Furthermore, in the above-mentioned method for enhancing the stability of laser point cloud positioning, the estimated pose value calculated by trajectory calculation is continuously provided after the point cloud matching fails, and the duration ranges from 1 to 10 seconds or the mobile device travels no more than 50 meters.

[0015] In a second aspect, a system for enhancing the stability of laser point cloud positioning is provided, comprising:

[0016] An initial pose generation module is used to collect single-frame laser point cloud data of a mobile device in real time, and perform point cloud matching and positioning with a pre-made environmental point cloud map to generate an initial pose of the mobile device;

[0017] A dead reckoning pose generation module, configured to synchronously collect measurement data from an inertial measurement unit and data from a wheel speedometer, and generate a dead reckoning pose based on the measurement data from the inertial measurement unit and the data from the wheel speedometer using a dead reckoning algorithm;

[0018] An optimal pose generation module is used to input the initial pose of point cloud matching and the dead reckoning pose into an extended Kalman filter system to perform pose fusion estimation to obtain the optimal pose;

[0019] A local feature determination and corrected pose generation module is used to monitor the stability of local features of point cloud matching in real time. If the local features are stable, the extended Kalman filter system is updated using the point cloud matching results as observations. If the local features are unstable, causing the point cloud matching error to exceed a preset threshold, the extended Kalman filter system switches to dead reckoning mode, uses the dead reckoning pose as an estimated value, sets a local search range based on the estimated value, re-executes the point cloud matching operation, and generates a corrected pose.

[0020] The point cloud matching recovery module is used to feed back the corrected pose to the extended Kalman filter system and update the optimal pose until the point cloud matching is restored to stability.

[0021] In a third aspect, a device for applying the above-mentioned method for enhancing the stability of laser point cloud positioning is provided, comprising:

[0022] An acquisition module is provided on the mobile device and is used to acquire single-frame laser point cloud data of the mobile device in real time;

[0023] a first detection device, the first detection device being provided on the mobile device and being used for synchronously collecting posture data and acceleration data of the mobile device;

[0024] a second detection device, the second detection device being provided on the mobile device and being used to detect mobile data of the mobile device;

[0025] A control module is provided, wherein the control module is connected to the acquisition module, the first detection device and the second detection device respectively, and a pre-made environmental point cloud map is provided inside the control module. The control module is used to perform point cloud matching and positioning on the single-frame laser point cloud data of the mobile device collected in real time and the pre-made environmental point cloud map to generate an initial posture of the mobile device; a track-reckoning algorithm is also provided in the control module, and the control module is used to generate a track-reckoning posture according to the posture data, acceleration data and movement data of the mobile device through the set track-reckoning algorithm; the control module is also used to receive the initial posture and the track-reckoning posture of the point cloud matching, and to receive the received point cloud matching The initial pose of the cloud matching and the dead reckoning pose are input into the extended Kalman filter system for pose fusion estimation to obtain the optimal pose; at the same time, the control module is also used to judge the stability of the local features of the monitoring point cloud matching. If the local features are stable, the extended Kalman filter system is updated with the point cloud matching results as the observation values; if the local features are unstable so that the point cloud matching error exceeds the preset threshold, the extended Kalman filter system switches to the dead reckoning mode, uses the dead reckoning pose as the estimated value, sets the local search range based on the estimated value, re-executes the point cloud matching operation, generates a corrected pose, and feeds the corrected pose back to the extended Kalman filter system to update the optimal pose until the point cloud matching becomes stable.

[0026] Furthermore, in the above-mentioned device for enhancing the stability of laser point cloud positioning, the acquisition module includes: a laser radar or an ultrasonic sensor.

[0027] Furthermore, in the above-mentioned apparatus for enhancing the stability of laser point cloud positioning, the first detection device includes an inertial measurement unit.

[0028] Furthermore, in the above-mentioned device for enhancing the stability of laser point cloud positioning, the second detection unit includes a wheel speed meter.

[0029] In a fourth aspect, a device is provided, on which the device for enhancing the stability of laser point cloud positioning as described above is provided.

[0030] The main advantages of the technical solution of the present invention are as follows:

[0031] The method, system and device for enhancing the stability of laser point cloud positioning of the present invention perform point cloud matching and positioning by real-time acquisition of single-frame laser point cloud data and a pre-made environmental point cloud map. At the same time, the collected inertial measurement unit data and wheel speed meter data are tracked and calculated at the back end of the system, and the posture is estimated with the help of an extended Kalman filter system. When the local point cloud features are stable, the extended Kalman filter system is updated according to the effective results of the point cloud matching as observation data. When the local point cloud features are unstable, that is, when positioning is lost due to point cloud matching errors, the extended Kalman filter system will give the point cloud matching an estimated value through the posture estimation data generated by the track calculation, and the point cloud registration function will re-search for matching results near this estimated value. Within the effective accuracy range of the track calculation, the failure rate of the point cloud matching is greatly reduced, and the stability of positioning is improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] The drawings described herein are used to provide a further understanding of the embodiments of the present invention and constitute a part of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

[0033] Figure 1 A schematic flow chart of a method for enhancing laser point cloud positioning stability provided by one embodiment of the present invention;

[0034] Figure 2 A schematic diagram of the principle of a method for enhancing the stability of laser point cloud positioning provided by one embodiment of the present invention;

[0035] Figure 3 A schematic diagram of a system for enhancing laser point cloud positioning stability provided by an embodiment of the present invention;

[0036] Figure 4 A schematic diagram of a device for enhancing laser point cloud positioning stability provided by an embodiment of the present invention;

[0037] Description of reference numerals:

[0038] 10. Initial pose generation module; 20. Dead reckoning pose generation module; 30. Optimal pose generation module; 40. Local feature determination and corrected pose generation module; 50. Point cloud matching and recovery module;

[0039] 100, acquisition module; 200, first detection device; 300, second detection device; 400, control module. DETAILED DESCRIPTION

[0040] To make the objectives, technical solutions, and advantages of the present invention more clear, the technical solutions of the present invention will be clearly and completely described below in conjunction with specific embodiments of the present invention and corresponding drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0041] To facilitate a clearer understanding and explanation of the technical solutions provided by the embodiments of the present invention, the technical terms involved in the embodiments of the present invention are further explained below. Specifically, the technical terms involved in the present invention include:

[0042] A wheel speedometer is a common device that directly measures vehicle speed and displacement from sensors mounted on the wheels. It is generally categorized into three types: purely mechanical, mechanical-electronic, and purely electronic. For example, a purely electronic rotary encoder uses the principle of electromagnetic induction to convert tire rotations into a voltage signal, thereby measuring vehicle speed and angle. Positioning is then achieved by integrating the speed over time. The information provided by the wheel speedometer is in the vehicle coordinate system. It generally reports the speeds of the left front, right front, left rear, and right rear wheels. The vehicle's forward speed is equal to the average of the left and right wheel speeds. It also provides speed indicators for vehicle status, such as forward, reverse, rollover, and parked.

[0043] An inertial measurement unit (IMU) is a device that measures an object's three-axis attitude angle (or angular rate) and acceleration. Typically, an IMU consists of three single-axis accelerometers and three single-axis gyroscopes. The accelerometers detect the object's acceleration signals along three independent axes in the carrier's coordinate system, while the gyroscopes detect the carrier's angular velocity signals relative to the navigation coordinate system. Together, they measure the object's angular velocity and acceleration in three-dimensional space and use this to calculate the object's attitude.

[0044] The following is combined with Figure 1-4 , describes in detail the technical solution provided by the embodiments of the present invention.

[0045] First, as attached Figure 1-2 As shown, an embodiment of the present invention provides a method for enhancing the stability of laser point cloud positioning, which includes the following steps S1 to S5:

[0046] Step S1: Collect single-frame laser point cloud data of the mobile device in real time, and perform point cloud matching and positioning with the pre-made environmental point cloud map to generate the initial pose of the mobile device;

[0047] Specifically, there are a variety of general methods for producing an environmental point cloud map and generating an initial pose. In the embodiments of the present invention, any one or more methods can be selected to achieve the production of an environmental point cloud map and the generation of a corresponding initial pose of a mobile device.

[0048] For example, in this embodiment of the present invention, point cloud maps can be created using common open source projects. For example, the liosam open source project can be used to create point cloud maps using collected 3D point cloud data. This section describes a general open method. Once the map is generated, it is loaded using ROS middleware and then loaded into Rvize. The initial pose can then be set manually using a mouse click.

[0049] Step S2: synchronously collecting the measurement data of the inertial measurement unit and the data of the wheel speed meter, and generating a dead reckoning posture through a dead reckoning algorithm based on the measurement data of the inertial measurement unit and the data of the wheel speed meter;

[0050] In some optional implementations of this embodiment, dead reckoning uses a wheel speed encoder to record wheel pulses. The total distance of the total pulses is calculated based on the pre-calibrated distance between each pulse. Each time a discrete observation is obtained, the odometer is reset and recording begins again until the next observation is received. This eliminates accumulated odometer errors while also providing a relatively accurate pose estimate when the next observation is not received.

[0051] Step S3: Input the initial pose of point cloud matching and the dead reckoning pose into the extended Kalman filter system to perform pose fusion estimation and obtain the optimal pose;

[0052] In this embodiment of the present invention, the EKF has two main inference processes. One is the prediction process. Before accurate observation feedback is obtained, the pose integrated from high-frequency sensors with large cumulative errors is used as the estimated state. The covariance matrix P in the EKF is used for extrapolation to infer a more accurate noise value. Here, the dead reckoning value is used as the prediction value. The other is the update process. After accurate observation results are obtained, the observation value is used to update and modify the error caused by the EKF prediction. This will simultaneously correct the X state value and the corresponding value in the P covariance matrix. Here, the point cloud matching result is used as the observation value, and this process is called the fusion process.

[0053] The Kalman filter (KF) is a publicly available mathematical algorithm that uses a linear system's state equation and observational data from the system's input and output to optimally estimate the system's state. Its core concept is to reduce errors by fusing observational data with predicted data, thereby obtaining a more accurate state estimate. This embodiment of the present invention utilizes this algorithmic framework, so its details and principles will not be elaborated upon.

[0054] Step S4: Monitor the stability of local features of point cloud matching in real time. If the local features are stable, update the extended Kalman filter system using the point cloud matching results as observations. If the local features are unstable, causing the point cloud matching error to exceed a preset threshold, switch the extended Kalman filter system to dead reckoning mode, use the dead reckoning pose as an estimated value, set a local search range based on the estimated value, re-execute the point cloud matching operation, and generate a corrected pose.

[0055] Specifically, in the embodiment of the present invention, the point cloud can be obtained by sensor scanning, and the local feature refers to the portion of the point cloud scanned by the current sensor that matches the map point cloud.

[0056] In the embodiment of the present invention, the mean square error result of the point cloud matching is used as the matching score, and 1.0 is used as the preset threshold. A mean square error less than 1.0 is stable, and a mean square error greater than 1.0 is unstable.

[0057] In an embodiment of the present invention, a value in any range of 0.8-1.5 can be used as a preset threshold, for example, 0.8, 0.9, 1.1, 1.2, 1.3, 1.4 and 1.5 can be used as the preset threshold according to actual conditions and needs.

[0058] In the embodiment of the present invention, dead reckoning mode refers to a situation where, when there are no observations for more than one second, the positioning system can only make pose predictions using high-frequency sensors with accumulated errors, such as wheel speedometers and IMUs. In the embodiment of the present invention, dead reckoning uses a wheel speed encoder to record the number of wheel pulses, and the total mileage of the total pulses can be calculated based on the pre-calibrated distance between each two pulses. Each time a discrete observation is obtained, the odometer is reset and recording begins again until the next observation is received. This can eliminate the accumulated errors of the odometer and, at the same time, obtain a relatively accurate estimated pose when the next observation does not appear.

[0059] Step S5: Feedback the corrected pose to the extended Kalman filter system to update the optimal pose until the point cloud matching becomes stable.

[0060] In an embodiment of the present invention, the estimated pose value calculated by dead reckoning is continuously provided after the point cloud matching fails, and the duration ranges from 1 to 10 seconds or the mobile device travels no more than 50 meters.

[0061] First, it should be noted that in some optional implementations of this embodiment, the mobile device includes any movable object. When performing laser point cloud positioning stability enhancement, the corresponding detection equipment is mounted on the movable object to acquire corresponding data and parameters. For example, the mobile device includes a mobile vehicle, aircraft, and other equipment. In this embodiment, an AGV is used for illustration.

[0062] Combine Figures 1 to 2 The principles of the method for enhancing the stability of laser point cloud positioning according to the embodiment of the present invention include:

[0063] The AGV vehicle performs point cloud matching and positioning by real-time collection of single-frame laser point cloud data and pre-made environmental point cloud maps. At the same time, the collected IMU data and wheel speed meter data are used for track calculation on the back end of the system, and the extended Kalman filter system is used for pose estimation. When the local point cloud features are stable, the extended Kalman filter system is updated according to the effective results of the point cloud matching as observation data. When the local point cloud features are unstable, that is, when the positioning is lost due to point cloud matching errors, the extended Kalman filter system will use the pose estimation data generated by track calculation to give the point cloud matching an estimated value. The point cloud registration function will re-search for matching results near this estimated value. Within the effective accuracy range of track calculation, the failure rate of point cloud matching is greatly reduced and the stability of positioning is improved.

[0064] In combination with practical applications, taking an AGV vehicle as an example, the following steps are used to illustrate the method for enhancing the stability of laser point cloud positioning according to an embodiment of the present invention:

[0065] Create a multi-line laser point cloud map, start the laser point cloud, IMU and wheel speed sensor equipment on the AGV vehicle, start the point cloud positioning function module and the pose estimation module. When the local features of the laser point cloud change significantly, the track calculation process will continuously use the estimated value as a reference for point cloud matching until the matching is stable, thereby avoiding long-term positioning loss.

[0066] It should be noted that the production of laser point cloud maps belongs to a specialized field and there are many ways to produce them. In the embodiment of the present invention, any mapping tool can be used to produce point cloud maps.

[0067] Therefore, the present invention provides a method for enhancing the stability of laser point cloud positioning. By combining an extended Kalman filter system and a dead reckoning algorithm, the independent point cloud matching model is separated to establish an architecture for closed-loop feedback of the inferred posture. When a single point cloud matching model fails, the initial posture of the point cloud matching can be continuously provided within a period of time (several seconds) or a driving distance (tens of meters).

[0068] Second, as Figure 3 As shown, an embodiment of the present invention further provides a system for enhancing the stability of laser point cloud positioning, the system comprising: an initial pose generation module 10, a dead reckoning pose generation module 20, an optimal pose generation module 30, a local feature determination and correction pose generation module 40, and a point cloud matching recovery module 50, wherein:

[0069] The initial pose generation module 10 is used to collect single-frame laser point cloud data of the mobile device in real time, and perform point cloud matching and positioning with the pre-made environmental point cloud map to generate the initial pose of the mobile device; the track-reckoning pose generation module 20 is used to synchronously collect the measurement data of the inertial measurement unit and the data of the wheel speed meter, and generate the track-reckoning pose based on the measurement data of the inertial measurement unit and the data of the wheel speed meter through the track-reckoning algorithm; the optimal pose generation module 30 is used to input the initial pose of the point cloud matching and the track-reckoning pose into the extended Kalman filter system to perform pose fusion estimation to obtain the optimal pose; the local feature map is used to generate the optimal pose through the track-reckoning pose generation module 20; the optimal pose generation module 30 ... The feature determination and corrected pose generation module 40 is used to monitor the stability of local features of point cloud matching in real time. If the local features are stable, the extended Kalman filter system is updated with the point cloud matching results as observation values; if the local features are unstable and the point cloud matching error exceeds the preset threshold, the extended Kalman filter system switches to the track calculation mode, uses the track calculation pose as the estimated value, sets the local search range based on the estimated value, re-executes the point cloud matching operation, and generates a corrected pose; the point cloud matching recovery module 50 is used to feed back the corrected pose to the extended Kalman filter system, updates the optimal pose, and returns the point cloud matching to stability.

[0070] Thirdly, as Figure 4 As shown, the present invention also provides a device for applying the above-mentioned method for enhancing the stability of laser point cloud positioning, the device comprising: an acquisition module 100, a first detection device 200, a second detection device 300 and a control module 400, wherein:

[0071] The acquisition module 100 is arranged on the mobile device for collecting single-frame laser point cloud data of the mobile device in real time; the first detection device 200 is arranged on the mobile device for synchronously collecting the posture data and acceleration data of the mobile device; the second detection device 300 is arranged on the mobile device for detecting the movement data of the mobile device; the control module 400 is connected to the acquisition module 100, the first detection device 200 and the second detection device 300 respectively, and a pre-made environmental point cloud map is arranged inside the control module 400. The control module 400 is used to match and locate the single-frame laser point cloud data of the mobile device collected in real time with the pre-made environmental point cloud map to generate the initial posture of the mobile device; the control module 400 is also provided with a track calculation algorithm, and the control module 400 is used to calculate the initial posture of the mobile device according to the posture data, acceleration data and movement data of the mobile device. The dead reckoning algorithm is used to generate a dead reckoning pose. The control module 400 is also used to receive the initial pose and the dead reckoning pose of the point cloud matching, and input the received initial pose and the dead reckoning pose of the point cloud matching into the extended Kalman filter system to perform pose fusion estimation to obtain the optimal pose. At the same time, the control module 400 is also used to judge the stability of the local features of the monitoring point cloud matching. If the local features are stable, the extended Kalman filter system is updated with the point cloud matching results as the observation values. If the local features are unstable and the point cloud matching error exceeds the preset threshold, the extended Kalman filter system is switched to the dead reckoning mode, with the dead reckoning pose as the estimated value, and the local search range is set based on the estimated value. The point cloud matching operation is re-executed to generate a corrected pose, and the corrected pose is fed back to the extended Kalman filter system to update the optimal pose until the point cloud matching becomes stable.

[0072] In some optional implementations of this embodiment, the pre-made environmental point cloud map and track calculation algorithm in the control device are stored therein by downloading, pre-storing or transmitting, and the control module 400 uses them in a calling manner during use.

[0073] In some optional implementations of this embodiment, the acquisition module 100 includes: a lidar or an ultrasonic sensor; the first detection device 200 includes an inertial measurement unit; the second detection unit includes a wheel speed meter; and the control device includes an industrial computer capable of data processing and data storage, such as an industrial personal computer.

[0074] Therefore, the device for enhancing the stability of laser point cloud positioning through the acquisition module 100, the first detection device 200, the second detection device 300 and the control module 400 can enable the acquisition module 100, the first detection device 200 and the second detection device 300 on the device to collect data corresponding to the mobile device when the device is blocked by an obstruction for a long time during its movement. Through the control device and the track calculation algorithm set therein, the device for enhancing the stability of laser point cloud positioning in coordination with the extended Kalman filter system can enhance the success rate of point cloud matching, thereby improving its positioning stability. As a result, the problem of long-term positioning loss caused by object obstruction can be avoided during the movement of the mobile device.

[0075] In a fourth aspect, an embodiment of the present invention further provides a device, on which the above-mentioned device for enhancing the stability of laser point cloud positioning is provided.

[0076] Therefore, by setting the device for enhancing the stability of laser point cloud positioning of the above-mentioned embodiment of the present invention on the device, when the device is blocked by an obstruction for a long time during movement, the device for enhancing the stability of laser point cloud positioning carried on the device can enhance the success rate of point cloud matching, thereby improving its positioning stability. As a result, the problem of long-term positioning loss caused by object obstruction can be avoided during the movement of the mobile device.

[0077] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device that includes a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In addition, "front", "back", "left", "right", "upper" and "lower" in this document are all referenced to the placement states shown in the accompanying drawings.

[0078] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A method for enhancing the stability of laser point cloud positioning, characterized in that: include: Collect single-frame laser point cloud data of mobile devices in real time, match and locate the point cloud with the pre-made environmental point cloud map, and generate the initial pose of the mobile device; Synchronously collect the measurement data of the inertial measurement unit and the data of the wheel speed meter, and generate the dead reckoning posture through the dead reckoning algorithm based on the measurement data of the inertial measurement unit and the data of the wheel speed meter; The initial pose of point cloud matching and the dead reckoning pose are input into the extended Kalman filter system for pose fusion estimation to obtain the optimal pose. The stability of local features of point cloud matching is monitored in real time. If the local features are stable, the extended Kalman filter system is updated with the point cloud matching results as observations. If the local features are unstable and the point cloud matching error exceeds the preset threshold, the extended Kalman filter system switches to dead reckoning mode, uses the dead reckoning pose as the estimated value, sets the local search range based on the estimated value, re-executes the point cloud matching operation, and generates a corrected pose. The corrected pose is fed back to the extended Kalman filter system to update the optimal pose until the point cloud matching becomes stable.

2. The method for enhancing laser point cloud positioning stability according to claim 1, characterized in that: The point cloud is obtained by sensor scanning, and the local features of the point cloud matching include the parts of the point cloud scanned by the current sensor that match the environmental point cloud map.

3. The method for enhancing laser point cloud positioning stability according to claim 1, characterized in that: The mean square error result of point cloud matching is used as the matching score, and the matching score is used as the preset threshold.

4. The method for enhancing laser point cloud positioning stability according to claim 1, characterized in that: The estimated pose value calculated by the trajectory is continuously provided after the point cloud matching fails, and the duration ranges from 1 to 10 seconds or the mobile device travels no more than 50 meters.

5. A system for enhancing the stability of laser point cloud positioning, characterized in that: include: An initial pose generation module is used to collect single-frame laser point cloud data of a mobile device in real time, and perform point cloud matching and positioning with a pre-made environmental point cloud map to generate an initial pose of the mobile device; A dead reckoning pose generation module, configured to synchronously collect measurement data from an inertial measurement unit and data from a wheel speedometer, and generate a dead reckoning pose based on the measurement data from the inertial measurement unit and the data from the wheel speedometer using a dead reckoning algorithm; An optimal pose generation module is used to input the initial pose of point cloud matching and the dead reckoning pose into an extended Kalman filter system to perform pose fusion estimation to obtain the optimal pose; A local feature determination and corrected pose generation module is used to monitor the stability of local features of point cloud matching in real time. If the local features are stable, the extended Kalman filter system is updated using the point cloud matching results as observations. If the local features are unstable, causing the point cloud matching error to exceed a preset threshold, the extended Kalman filter system switches to dead reckoning mode, uses the dead reckoning pose as an estimated value, sets a local search range based on the estimated value, re-executes the point cloud matching operation, and generates a corrected pose. The point cloud matching recovery module is used to feed back the corrected pose to the extended Kalman filter system and update the optimal pose until the point cloud matching is restored to stability.

6. A device using the method for enhancing laser point cloud positioning stability according to any one of claims 1 to 4, characterized in that: include: An acquisition module is provided on the mobile device and is used to acquire single-frame laser point cloud data of the mobile device in real time; a first detection device, the first detection device being provided on the mobile device and being used for synchronously collecting posture data and acceleration data of the mobile device; a second detection device, the second detection device being provided on the mobile device and being used to detect mobile data of the mobile device; A control module is provided, wherein the control module is connected to the acquisition module, the first detection device and the second detection device respectively, and a pre-made environmental point cloud map is provided inside the control module. The control module is used to perform point cloud matching and positioning on the single-frame laser point cloud data of the mobile device collected in real time and the pre-made environmental point cloud map to generate an initial posture of the mobile device; a track-reckoning algorithm is also provided in the control module, and the control module is used to generate a track-reckoning posture according to the posture data, acceleration data and movement data of the mobile device through the set track-reckoning algorithm; the control module is also used to receive the initial posture and the track-reckoning posture of the point cloud matching, and to receive the received point cloud matching The initial pose of the cloud matching and the dead reckoning pose are input into the extended Kalman filter system for pose fusion estimation to obtain the optimal pose; at the same time, the control module is also used to judge the stability of the local features of the monitoring point cloud matching. If the local features are stable, the extended Kalman filter system is updated with the point cloud matching results as the observation values; if the local features are unstable so that the point cloud matching error exceeds the preset threshold, the extended Kalman filter system switches to the dead reckoning mode, uses the dead reckoning pose as the estimated value, sets the local search range based on the estimated value, re-executes the point cloud matching operation, generates a corrected pose, and feeds the corrected pose back to the extended Kalman filter system to update the optimal pose until the point cloud matching becomes stable.

7. The device for enhancing laser point cloud positioning stability according to claim 6, characterized in that: The acquisition module includes: a laser radar or an ultrasonic sensor.

8. The device for enhancing laser point cloud positioning stability according to claim 6, characterized in that: The first detection device includes an inertial measurement unit.

9. The device for enhancing laser point cloud positioning stability according to claim 6, characterized in that: The second detection unit includes a wheel speedometer.

10. A device, characterized in that The device is provided with a device for enhancing the stability of laser point cloud positioning as described in any one of claims 6 to 9.

Images