High-precision low-cost seamless positioning system and method suitable for intelligent motor sweeper

Abstract

The invention discloses a high-precision low-cost seamless positioning system and method suitable for an intelligent sweeper. The invention provides a high-precision low-cost seamless positioning system and method suitable for an intelligent sweeper. The system comprises a three-dimensional laser radar, a GNSS, an IMU and a data processing unit. According to the method, dynamic and static states of a multi-source sensor are classified, GNSS and IMU data are combined in a dynamic state to perform pre-integration processing through extended Kalman filtering, positioning is performed by depending on a three-dimensional laser radar SLAM in a static state, and then the positioning precision is improved through an information fusion technology. And the data processing unit classifies and processes data according to the motion state of the sweeper and monitors and adjusts strategies in real time. The method effectively solves the problem of high-precision low-cost seamless positioning of the intelligent sweeper in different scenes, improves the sweeping efficiency and stability, and is suitable for the field of positioning and navigation of similar equipment such as the intelligent sweeper.

Description

Technical Field

[0001] The present invention relates to an intelligent cleaning vehicle positioning technology, and more particularly to a high-precision, low-cost, seamless positioning system and method suitable for an intelligent cleaning vehicle. Background Art

[0002] In the field of smart street sweepers, accurate positioning is the key to achieving efficient cleaning operations. However, existing positioning technologies have many shortcomings. Some traditional positioning methods rely on a single sensor, such as using only GNSS. In areas where satellite signals are blocked, such as urban streets with tall buildings and tunnels, the positioning accuracy will drop significantly or even fail completely.

[0003] Methods based on inertial navigation, such as using IMU alone, can provide relatively accurate position and attitude information in a short period of time, but since its errors will accumulate over time, it is difficult to guarantee positioning accuracy after long-term operation. Positioning solutions using lidar are often expensive, and when dealing with a wide range of environments, they may be limited by excessive data volume and high computational complexity. In addition, existing multi-sensor fusion solutions are not optimized in data processing and algorithms, resulting in poor fusion effects, unable to give full play to the advantages of each sensor, and difficult to meet the high-precision positioning requirements of smart sweepers in various complex environments.

[0004] At the same time, the existing positioning system lacks adaptability and robustness when dealing with special working scenarios of smart sweepers, such as frequent starting and stopping, low-speed operation, and large environmental changes, and is prone to positioning deviation or loss. Summary of the invention

[0005] In order to solve the above problems, the present invention proposes a high-precision, low-cost, seamless positioning system and method suitable for an intelligent sweeper.

[0006] A high-precision, low-cost, seamless positioning system and method suitable for an intelligent sweeper, the specific steps are as follows, and the characteristics are as follows: Step 1: Integrate the sensor system to monitor the smart sweeper in all directions, including installing sensors such as 3D LiDAR, GNSS, and IMU to ensure accurate capture of environmental information in different working scenarios; Step 2: Adopt the dynamic and static state classification method of multi-source sensors, classify and process the sensor data, combine GNSS and IMU data in dynamic state, use extended Kalman filter for pre-integration processing, and rely on 3D laser radar SLAM for positioning in static state; Step 3: Realize seamless positioning of 3D LiDAR SLAM and GNSS / IMU collaborative sensing. Through information fusion technology, GNSS / IMU data is integrated with 3D LiDAR SLAM positioning results to improve positioning accuracy. Step 4: The data processing unit classifies and processes the sensor data, and adjusts the data processing strategy in real time according to the motion state of the smart sweeper to ensure the accuracy of the positioning results; Step 5: Real-time monitoring and adjustment: the data processing unit monitors the sensor data in real time and dynamically adjusts the positioning and navigation strategy according to the actual working scenario of the sweeper; Step 6: Realize high-precision, low-cost and seamless positioning of the intelligent sweeper, improve cleaning efficiency, and ensure stable operation of the sweeper in different working scenarios.

[0007] As a further improvement of the present invention, the process of integrating the sensor system in step 1 can be expressed as: Step 1.1, 3D LiDAR installation The 3D laser radar is used to collect point cloud data of objects around the vehicle and to avoid obstacles for the sweeper. The 3D laser radar is installed on the top of the smart sweeper, enabling it to perform 360-degree all-round scanning in the horizontal direction and a scanning angle range of -25 degrees to 15 degrees in the vertical direction, ensuring that the 3D laser radar can cover most of the area around the smart sweeper and obtain complete environmental point cloud data. The 3D laser radar emits high-frequency laser pulses, accurately measures the time interval from the laser emission to the reflection back to the radar receiver, and calculates the distance information to the surrounding objects through the following formula :

[0008] in, is the time interval from the laser being emitted to the laser being reflected back to the radar receiver. The speed of light is the speed of light. The laser beam emitted by the laser is distributed in a fan shape or multiple beams, and is scanned in the horizontal and vertical directions to obtain discrete point cloud data. These point cloud data contain the three-dimensional coordinate information of the surface of the surrounding objects, and a detailed three-dimensional model of the environment can be constructed through the point cloud data. When installing the 3D laser radar, accurate horizontal and vertical calibration is required to ensure that the laser radar's scanning reference plane is accurately aligned with the body coordinate system of the smart sweeper, so as to facilitate accurate coordinate conversion and processing of the scanned data later. When the smart sweeper is started, the laser radar's ranging accuracy is calibrated by emitting a specific calibration laser beam and detecting the reflected signal to ensure that the measured distance data is accurate. At the same time, the laser radar's scanning angle is calibrated and compared with a reference standard with a known angle to ensure that the scanning angle matches the set value. Step 1.2, GNSS receiving antenna installation GNSS is used to obtain the location coordinates of the road sweeper and plan the cleaning route. GNSS relies on receiving signals from multiple satellites to determine the three-dimensional location coordinates of the smart road sweeper on the earth. The distance between the satellite and the receiving device is calculated based on the time when the road sweeper GNSS receiving device receives the signal and the signal propagation speed:

[0009] in, It is The distance between the satellite and the receiving device, is the speed of light, It is the time when the GNSS receiving device of the smart sweeper receives the signal. is the time that the satellite continuously transmits signals to the ground; then, by combining the information of multiple satellites and solving the equation group, the position coordinates of the smart sweeper are obtained:

[0010] in, It is The coordinates of the satellites, is the position coordinate of the smart sweeper, The GNSS receiving antenna should be installed in the center of the roof of the smart sweeper. When installing the antenna, ensure that it is vertical to the ground and there are no metal shielding or other interference sources around it. At the same time, the antenna should be grounded to reduce the impact of electromagnetic interference on the signal reception quality, ensuring that the GNSS system can stably and accurately receive satellite signals and calculate the location information of the smart sweeper. Step 1.3, IMU installation IMU is used to measure the angular velocity and acceleration of the smart sweeper, including pitch angle, yaw angle and roll angle. IMU is used to quickly respond to the changes in the motion state of the sweeper in a short time. Even when the satellite signal is blocked or other sensors have a temporary failure, the relative displacement, speed and attitude change of the sweeper can still be calculated through integration operations based on the measured acceleration and angular velocity data, maintaining the continuity of positioning and navigation information, and ensuring the stable operation of the smart sweeper in complex environments. By placing the smart sweeper in a horizontal static state, measuring the acceleration value at this time, and adjusting the zero bias parameter of the IMU acceleration, the acceleration value measured in the static state is close to zero; then, by keeping the smart sweeper in a fixed posture for a period of time, measuring the angle output value, adjusting the angle drift parameter, and reducing the measurement error caused by the sensor's own characteristics.

[0011] As a further improvement of the present invention, the process of adopting the dynamic and static state classification processing of multi-source sensors in step 2 can be expressed as follows: Step 2.1, dynamic and static state judgment According to the motion state of the smart sweeper, the sensor data is divided into dynamic data and static data; the motion state of the smart sweeper is judged by analyzing the acceleration and angular velocity data measured by the IMU; the dynamic judgment formula of the sweeper is:

[0012]

[0013] in, , are the acceleration and angular velocity measured by the IMU, is the acceleration threshold, is the angular velocity threshold; when any of the above judgment conditions are met, the intelligent sweeper is judged to be in a dynamic state; otherwise, it is judged to be in a static state; Step 2.2, Dynamic Data Processing After determining that the smart sweeper is in a dynamic state, the IMU data is first pre-integrated:

[0014] in, Indicates that in the local coordinate system at time To time The speed increment, Represents the rotation matrix from the world coordinate system to the vehicle body coordinate system, represents the time variable, represents the acceleration increment measured by the IMU, represents the position increment in the local coordinate system, express The velocity vector increment of the vehicle in the local coordinate system at the moment; through pre-integration, the IMU measurement data in adjacent time intervals is converted into incremental information of relative position and velocity, reducing the error accumulation caused by integration; The pre-integrated IMU data is fused with the GNSS data through Kalman filtering, and the state transfer equation is:

[0015] in, express The state vector of the system at time , represents the state transition function, express The system noise at the moment, express The control input at time; the observation equation is:

[0016] in, express The observation vector at time, express The state vector of the system at time , express The observation noise at time Represents the observation function; in Kalman filtering, the prediction step is first performed:

[0017] in, Indicates based on Time state estimation and system model, The predicted value of the vehicle state at the moment, represents the time after the Kalman filter update step The state vector estimate, represents the predicted covariance matrix, represents the partial derivative of the state transfer matrix with respect to the state vector, Indicates the updated time The covariance matrix, represents the system noise covariance moment; then the update step is performed:

[0018] in, represents the Kalman gain, represents the partial derivative of the observation equation with respect to the state vector, represents the matrix transpose, represents the observation noise covariance matrix, represents the time after the Kalman filter update step The state vector estimate, Indicates the updated time The covariance matrix, Represents the unit matrix; the observation vector is formed by combining the position information measured by GNSS with the relative position and velocity information obtained by IMU pre-integration , the position, speed and posture of the intelligent sweeper By repeating the prediction and update steps, the position and speed information measured by GNSS are integrated with the relative position and speed information obtained by IMU pre-integration, so as to obtain a more accurate position, speed and attitude estimation of the intelligent sweeper, effectively overcoming the problems of IMU error accumulation and GNSS signal shielding; the final position estimation after the Kalman filter iteration is and its covariance matrix ; Step 2.3, static data processing When the smart sweeper is in a static state, it mainly relies on 3D laser radar SLAM for positioning; the 3D laser radar constructs point cloud data of the surrounding environment by emitting laser beams and receiving reflected light; in the static state, the position of the smart sweeper in the map is determined by the iterative closest point algorithm. First, determine the error function of the iterative closest point algorithm :

[0019] in, Indicates the current point cloud frame The first Points are used to calculate the distance between the corresponding points on the map point cloud in the point cloud matching algorithm; Representing map point cloud The points to determine the position of the sweeper; Then, the optimization algorithm is used to solve the transformation matrix that minimizes the error function, thereby determining the position of the smart sweeper in the map; in each iteration, the point cloud is calculated Relative to point cloud The translation vector and the rotation matrix , so that the point cloud After transformation:

[0020] in, Represents the transformed point cloud; the iteration target is With point cloud The distance error between the corresponding points is the smallest; the iterative process is repeated until the error converges to meet the set accuracy requirements. At this time, the transformation matrix obtained is the positioning result of the intelligent sweeper in the map. , to obtain accurate positioning of the sweeper; the final position estimation of SLAM The covariance matrix is .

[0021] As a further improvement of the present invention, the process of seamless positioning of 3D laser radar SLAM and GNSS / IMU collaborative sensing in step 3 can be expressed as follows: Step 3.1 Calculate the fusion weight matrix The calculation formula of the fusion weight matrix is ​​as follows:

[0022] in, is the fusion weight matrix, if The variance of the diagonal elements of is small, indicating that the position estimation of the GNSS / IMU combination is more accurate. The value of the corresponding element in will be larger, giving the GNSS / IMU position estimate a greater weight during fusion; Step 3.2: Position estimation fusion The calculated fusion weight matrix and , Substitute into the fusion formula:

[0023] Represents the final fused position estimate; this position estimate combines the advantages of GNSS / IMU in macro positioning and motion state estimation and the advantages of 3D lidar SLAM in local environment positioning and map construction, thereby improving the positioning accuracy of the smart sweeper.

[0024] The present invention provides a high-precision, low-cost, seamless positioning system and method suitable for an intelligent sweeper, and the beneficial effects are: 1. This invention greatly improves the positioning accuracy of the intelligent sweeper. Through the fusion of multi-source sensors and dynamic and static state classification processing, accurate and reliable location information can be provided whether in dynamic driving or static parking, ensuring the precise positioning of the sweeper in various complex environments and working scenarios, reducing the situation of cleaning omissions and repeated cleaning.

[0025] 2. The present invention reduces the positioning cost. While ensuring high-precision positioning, it avoids over-reliance on a single high-cost sensor by optimizing the sensor combination and data processing algorithm, effectively controlling the overall system cost.

[0026] 3. The present invention enhances the adaptability and robustness of the system. It can adapt to special working conditions such as frequent start and stop, low-speed operation of the intelligent sweeper, and changes in different environmental conditions, reduce positioning deviation or loss caused by environmental interference or changes in vehicle motion state, and improve the stability and continuity of the cleaning operation. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 It is a flow chart of the present invention.

[0028] Figure 2 This is the extended Kalman filter fusion block diagram of the present invention. DETAILED DESCRIPTION

[0029] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments: The present invention proposes a high-precision, low-cost, seamless positioning system and method suitable for intelligent sweepers. The first step is to install and debug the sensor. For the three-dimensional laser radar, it is accurately installed on the top of the intelligent sweeper to ensure that it can scan 360 degrees in all directions in the horizontal direction and the vertical scanning angle is between -25 degrees and 15 degrees. Strict horizontal and vertical calibration is performed during installation, and the ranging accuracy and scanning angle are calibrated during startup. The GNSS receiving antenna is installed in the center of the roof, keeping it vertical to the ground, without metal shielding and interference sources around it, and grounding is done to reduce electromagnetic interference, ensuring stable and accurate reception of satellite signals and calculation of position coordinates. After the IMU is installed, the acceleration zero bias parameter and angle drift parameter are adjusted respectively by placing the intelligent sweeper in a horizontal static state and a fixed posture for a period of time to reduce the measurement error. In the classification processing of the dynamic and static states of multi-source sensors, the motion state of the intelligent sweeper is judged by a specific formula based on the acceleration and angular velocity data measured by the IMU. When in motion, the IMU data is pre-integrated and then fused with the GNSS data through Kalman filtering. The prediction and update steps are repeated continuously to obtain more accurate position, speed and attitude estimates. When static, three-dimensional lidar SLAM is used for positioning, and the error function is determined by iterative nearest point algorithm. The transformation matrix is ​​continuously optimized and solved until the error converges to meet the set accuracy requirements, and the position of the sweeper in the map is determined. When realizing collaborative seamless positioning of three-dimensional lidar SLAM and GNSS / IMU, the fusion weight matrix is ​​calculated, and it is substituted into the fusion formula with the GNSS / IMU position estimate and the three-dimensional lidar SLAM positioning results to obtain the final fused position estimate. The data processing unit classifies and processes the sensor data according to the motion state of the intelligent sweeper, monitors the sensor data in real time, and dynamically adjusts the positioning and navigation strategy according to the actual working scenario. The invention flow chart is as follows: Figure 1 As shown, the steps of the present invention are described in detail below.

[0030] Step 1: Integrate the sensor system to monitor the smart sweeper in all directions, including installing sensors such as 3D LiDAR, GNSS, and IMU to ensure accurate capture of environmental information in different working scenarios; Step 1.1, 3D LiDAR installation The 3D laser radar is used to collect point cloud data of objects around the vehicle and to avoid obstacles for the sweeper. The 3D laser radar is installed on the top of the smart sweeper, so that it can perform 360-degree all-round scanning in the horizontal direction and a scanning angle range of -25 degrees to 15 degrees in the vertical direction, ensuring that the 3D laser radar can cover most of the area around the smart sweeper and obtain complete environmental point cloud data. The 3D laser radar emits high-frequency laser pulses, accurately measures the time interval from the laser emission to the reflection back to the radar receiver, and calculates the distance information to the surrounding objects through the following formula: :

[0031] in, is the time interval from the laser being emitted to the laser being reflected back to the radar receiver. The laser beam emitted by the laser scanner is fan-shaped or multi-beam distributed, scanning in the horizontal and vertical directions to obtain discrete point cloud data. These point cloud data contain the three-dimensional coordinate information of the surface of surrounding objects, and a detailed three-dimensional model of the environment can be constructed through the point cloud data.

[0032] When installing a 3D laser radar, precise horizontal and vertical calibration is required to ensure that the laser radar's scanning reference plane is accurately aligned with the intelligent sweeper's body coordinate system, so that the scan data can be accurately converted and processed. When the intelligent sweeper is started, the laser radar's ranging accuracy is calibrated by emitting a specific calibration laser beam and detecting the reflected signal to ensure that the measured distance data is accurate. At the same time, the laser radar's scanning angle is calibrated and compared with a reference standard with a known angle to ensure that the scanning angle matches the set value.

[0033] Step 1.2, GNSS receiving antenna installation GNSS is used to obtain the location coordinates of the road sweeper and plan the cleaning route. GNSS relies on receiving signals transmitted by multiple satellites to determine the three-dimensional location coordinates of the smart road sweeper on the earth. The distance between the satellite and the receiving device is calculated based on the time when the road sweeper GNSS receiving device receives the signal and the signal propagation speed:

[0034] in, It is The distance between the satellite and the receiving device, is the speed of light, It is the time when the GNSS receiving device of the smart sweeper receives the signal. is the time that the satellite continuously transmits signals to the ground. Then, by combining the information of multiple satellites and solving the equation group, the position coordinates of the smart sweeper are obtained:

[0035] in, It is The coordinates of the satellites, is the position coordinate of the smart sweeper, The receiver clock error. The GNSS receiving antenna should be installed in the center of the roof of the smart sweeper. When installing the antenna, ensure that it is vertical to the ground and there are no metal shielding or other interference sources around it. At the same time, the antenna should be grounded to reduce the impact of electromagnetic interference on the signal reception quality, ensuring that the GNSS system can stably and accurately receive satellite signals and calculate the location information of the smart sweeper.

[0036] Step 1.3, IMU installation IMU is used to measure the angular velocity and acceleration of the smart sweeper, including pitch angle, yaw angle and roll angle. IMU is used to quickly respond to the changes in the motion state of the sweeper in a short period of time. Even when the satellite signal is blocked or other sensors have a temporary failure, the relative displacement, speed and attitude change of the sweeper can still be calculated through integration operations based on the measured acceleration and angular velocity data, maintaining the continuity of positioning and navigation information, and ensuring the stable operation of the smart sweeper in complex environments.

[0037] By placing the smart sweeper in a horizontal static state, measuring the acceleration value at this time, and adjusting the zero bias parameter of the IMU acceleration, the acceleration value measured in the static state is close to zero. Then, by keeping the smart sweeper in a fixed posture for a period of time, measuring the angle output value, and adjusting the angle drift parameter, the measurement error caused by the sensor's own characteristics can be reduced.

[0038] Step 2: Adopt the dynamic and static state classification method of multi-source sensors, classify and process the sensor data, combine GNSS and IMU data in dynamic state, use extended Kalman filter for pre-integration processing, and rely on 3D laser radar SLAM for positioning in static state; Step 2.1, dynamic and static state judgment The sensor data is divided into dynamic data and static data according to the motion state of the smart sweeper. The motion state of the smart sweeper is determined by analyzing the acceleration and angular velocity data measured by the IMU. The dynamic judgment formula of the sweeper is:

[0039]

[0040] in, , are the acceleration and angular velocity measured by the IMU, is the acceleration threshold, is the angular velocity threshold. When any of the above judgment conditions are met, the intelligent sweeper is judged to be in a dynamic state. Otherwise, it is judged to be in a static state.

[0041] Step 2.2, Dynamic Data Processing After determining that the smart sweeper is in a dynamic state, the IMU data is first pre-integrated:

[0042] in, Indicates that in the local coordinate system at time To time The speed increment, Represents the rotation matrix from the world coordinate system to the vehicle body coordinate system, represents the time variable, represents the acceleration increment measured by the IMU, represents the position increment in the local coordinate system, express The velocity vector increment of the vehicle in the local coordinate system at the moment. Through pre-integration, the IMU measurement data in adjacent time intervals is converted into incremental information of relative position and velocity, reducing the error accumulation caused by integration.

[0043] The pre-integrated IMU data is fused with the GNSS data through Kalman filtering. The extended Kalman filter fusion block diagram is as follows: Figure 2 As shown, the state transfer equation is:

[0044] in, express The state vector of the system at time , represents the state transition function, express The system noise at the moment, express The control input at time. The observation equation is:

[0045] in, express The observation vector at time, express The state vector of the system at time , express The observation noise at time Represents the observation function. In Kalman filtering, the prediction step is first performed:

[0046] in, Indicates based on Time state estimation and system model, The predicted value of the vehicle state at the moment, represents the time after the Kalman filter update step The state vector estimate, represents the predicted covariance matrix, represents the partial derivative of the state transfer matrix with respect to the state vector, Indicates the updated time The covariance matrix, represents the system noise covariance moment. Then the update step is performed:

[0047] in, represents the Kalman gain, represents the partial derivative of the observation equation with respect to the state vector, represents the matrix transpose, represents the observation noise covariance matrix, represents the time after the Kalman filter update step The state vector estimate, Indicates the updated time The covariance matrix, Represents the unit matrix. The observation vector is formed by combining the position information measured by GNSS with the relative position and velocity information obtained by IMU pre-integration. , the position, speed and posture of the intelligent sweeper By repeating the prediction and update steps, the position and speed information measured by GNSS are integrated with the relative position and speed information obtained by IMU pre-integration, so as to obtain a more accurate position, speed and attitude estimation of the intelligent sweeper, effectively overcoming the problems of IMU error accumulation and GNSS signal occlusion. Let the final position estimate after the Kalman filter iteration be and its covariance matrix

[0048] Step 2.3, static data processing When the smart sweeper is in a static state, it mainly relies on 3D laser radar SLAM for positioning. The 3D laser radar constructs point cloud data of the surrounding environment by emitting laser beams and receiving reflected light. In the static state, the position of the smart sweeper in the map is determined by the iterative closest point algorithm. First, determine the error function of the iterative closest point algorithm. :

[0049] in, Indicates the current point cloud frame The first Points are used to calculate the distance between the corresponding points on the map point cloud in the point cloud matching algorithm; Representing map point cloud The points to determine the position of the sweeper.

[0050] Then, the optimization algorithm is used to solve the transformation matrix that minimizes the error function, thereby determining the position of the smart sweeper in the map. In each iteration, the point cloud is calculated. Relative to point cloud The translation vector and the rotation matrix , so that the point cloud After transformation:

[0051] in, Represents the transformed point cloud. The iteration goal is With point cloud The distance error between the corresponding points is the smallest. The iterative process is repeated until the error converges to meet the set accuracy requirements. At this time, the transformation matrix obtained is the positioning result of the smart sweeper in the map. , and obtain accurate positioning of the sweeper. Let the final position estimate of SLAM be The covariance matrix is .

[0052] Step 3: Realize seamless positioning of 3D LiDAR SLAM and GNSS / IMU collaborative sensing. Through information fusion technology, GNSS / IMU data is integrated with 3D LiDAR SLAM positioning results to improve positioning accuracy. Step 3.1 Calculate the fusion weight matrix The calculation formula of the fusion weight matrix is ​​as follows:

[0053] in, is the fusion weight matrix, if The variance of the diagonal elements of is small, indicating that the position estimation of the GNSS / IMU combination is more accurate. The value of the corresponding element in will be larger, giving the GNSS / IMU position estimate a greater weight during fusion.

[0054] Step 3.2: Position estimation fusion The calculated fusion weight matrix and , Substitute into the fusion formula:

[0055] Represents the final fused position estimate. This position estimate combines the advantages of GNSS / IMU in macro positioning and motion state estimation and the advantages of 3D LiDAR SLAM in local environment positioning and map construction, thereby improving the positioning accuracy of the smart sweeper.

[0056] Step 4: The data processing unit classifies and processes the sensor data, and adjusts the data processing strategy in real time according to the motion state of the smart sweeper to ensure the accuracy of the positioning results; Step 5: Real-time monitoring and adjustment: the data processing unit monitors the sensor data in real time and dynamically adjusts the positioning and navigation strategy according to the actual working scenario of the sweeper; Step 6: Realize high-precision, low-cost and seamless positioning of the intelligent sweeper, improve cleaning efficiency, and ensure stable operation of the sweeper in different working scenarios.

[0057] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent change made based on the technical essence of the present invention still falls within the scope of protection required by the present invention.

Claims

1. A high-precision, low-cost, seamless positioning system and method suitable for intelligent road sweepers, the specific steps are as follows, characterized in that: Step 1: Integrate the sensor system to monitor the smart sweeper in all directions, including installing sensors such as 3D LiDAR, GNSS, and IMU to ensure accurate capture of environmental information in different working scenarios; Step 2: Use a multi-source sensor dynamic and static state classification method to classify and process sensor data. In dynamic mode, combine GNSS and IMU data, use extended Kalman filter for pre-integration processing, and rely on 3D laser radar SLAM for positioning in static mode. Step 3: Realize seamless positioning of 3D LiDAR SLAM and GNSS / IMU collaborative sensing. Through information fusion technology, GNSS / IMU data is integrated with 3D LiDAR SLAM positioning results to improve positioning accuracy. Step 4: The data processing unit classifies and processes the sensor data, and adjusts the data processing strategy in real time according to the motion state of the smart sweeper to ensure the accuracy of the positioning results; Step 5: Real-time monitoring and adjustment: the data processing unit monitors the sensor data in real time and dynamically adjusts the positioning and navigation strategy according to the actual working scenario of the sweeper; Step 6: Realize high-precision, low-cost and seamless positioning of the intelligent sweeper, improve cleaning efficiency, and ensure stable operation of the sweeper in different working scenarios.

2. A high-precision, low-cost, seamless positioning system and method suitable for an intelligent road sweeper according to claim 1, characterized in that: The process of integrating the sensor system in step 1 can be expressed as: Step 1.1, 3D LiDAR installation The 3D laser radar is used to collect point cloud data of objects around the vehicle and to avoid obstacles for the sweeper. The 3D laser radar is installed on the top of the smart sweeper, enabling it to perform 360-degree all-round scanning in the horizontal direction and a scanning angle range of -25 degrees to 15 degrees in the vertical direction, ensuring that the 3D laser radar can cover most of the area around the smart sweeper and obtain complete environmental point cloud data. The 3D laser radar emits high-frequency laser pulses, accurately measures the time interval from the laser emission to the reflection back to the radar receiver, and calculates the distance information to the surrounding objects through the following formula : in, is the time interval from the laser being emitted to the laser being reflected back to the radar receiver. The speed of light is the speed of light. The laser beam emitted by the laser is distributed in a fan shape or multiple beams, and is scanned in the horizontal and vertical directions to obtain discrete point cloud data. These point cloud data contain the three-dimensional coordinate information of the surface of the surrounding objects, and a detailed three-dimensional model of the environment can be constructed through the point cloud data. When installing the 3D laser radar, accurate horizontal and vertical calibration is required to ensure that the laser radar's scanning reference plane is accurately aligned with the body coordinate system of the smart sweeper, so as to facilitate accurate coordinate conversion and processing of the scanned data later. When the smart sweeper is started, the laser radar's ranging accuracy is calibrated by emitting a specific calibration laser beam and detecting the reflected signal to ensure that the measured distance data is accurate. At the same time, the laser radar's scanning angle is calibrated and compared with a reference standard with a known angle to ensure that the scanning angle matches the set value. Step 1.2, GNSS receiving antenna installation GNSS is used to obtain the location coordinates of the road sweeper and plan the cleaning route. GNSS relies on receiving signals from multiple satellites to determine the three-dimensional location coordinates of the smart road sweeper on the earth. The distance between the satellite and the receiving device is calculated based on the time when the road sweeper GNSS receiving device receives the signal and the signal propagation speed: in, It is The distance between the satellite and the receiving device, is the speed of light, is the time when the GNSS receiving device of the smart sweeper receives the signal. is the time that the satellite continuously transmits signals to the ground; then, by combining the information of multiple satellites and solving the equation group, the position coordinates of the smart sweeper are obtained: in, It is The coordinates of the satellites, is the position coordinate of the smart sweeper, The GNSS receiving antenna should be installed in the center of the roof of the smart sweeper. When installing the antenna, ensure that it is vertical to the ground and there are no metal shielding or other interference sources around it. At the same time, the antenna should be grounded to reduce the impact of electromagnetic interference on the signal reception quality, ensuring that the GNSS system can stably and accurately receive satellite signals and calculate the location information of the smart sweeper. Step 1.3, IMU installation IMU is used to measure the angular velocity and acceleration of the smart sweeper, including pitch angle, yaw angle and roll angle. IMU is used to quickly respond to the change of the sweeper's motion state in a short time. Even when the satellite signal is blocked or other sensors have a temporary failure, the relative displacement, speed and attitude change of the sweeper can still be calculated through integration operation based on the measured acceleration and angular velocity data, maintaining the continuity of positioning and navigation information, and ensuring the stable operation of the smart sweeper in complex environments. By placing the smart sweeper in a horizontal static state, measuring the acceleration value at this time, and adjusting the zero bias parameter of the IMU acceleration, the acceleration value measured in the static state is close to zero; then, by keeping the smart sweeper in a fixed posture for a period of time, measuring the angle output value, adjusting the angle drift parameter, and reducing the measurement error caused by the sensor's own characteristics.

3. A high-precision, low-cost, seamless positioning system and method suitable for an intelligent road sweeper according to claim 1, characterized in that: In step 2, the process of using multi-source sensor dynamic and static state classification processing can be expressed as follows: Step 2.1, dynamic and static state judgment According to the motion state of the smart sweeper, the sensor data is divided into dynamic data and static data; the motion state of the smart sweeper is judged by analyzing the acceleration and angular velocity data measured by the IMU; the dynamic judgment formula of the sweeper is: in, , are the acceleration and angular velocity measured by the IMU, is the acceleration threshold, is the angular velocity threshold; when any of the above judgment conditions are met, the intelligent sweeper is judged to be in a dynamic state; otherwise, it is judged to be in a static state; Step 2.2, Dynamic Data Processing After determining that the smart sweeper is in a dynamic state, the IMU data is first pre-integrated: in, Indicates that in the local coordinate system at time To time The speed increment, Represents the rotation matrix from the world coordinate system to the vehicle body coordinate system, represents the time variable, represents the acceleration increment measured by the IMU, represents the position increment in the local coordinate system, express The velocity vector increment of the vehicle in the local coordinate system at the moment; through pre-integration, the IMU measurement data in adjacent time intervals is converted into incremental information of relative position and velocity, reducing the error accumulation caused by integration; The pre-integrated IMU data is fused with the GNSS data through Kalman filtering, and the state transfer equation is: in, express The state vector of the system at time , represents the state transition function, express The system noise at the moment, express The control input at time; the observation equation is: in, express The observation vector at time, express The state vector of the system at time , express The observation noise at time Represents the observation function; in Kalman filtering, the prediction step is first performed: in, Indicates based on Time state estimation and system model, The predicted value of the vehicle state at the moment, represents the time after the Kalman filter update step The state vector estimate, represents the predicted covariance matrix, represents the partial derivative of the state transfer matrix with respect to the state vector, Indicates the updated time The covariance matrix, represents the system noise covariance moment; then the update step is performed: in, represents the Kalman gain, represents the partial derivative of the observation equation with respect to the state vector, represents the matrix transpose, represents the observation noise covariance matrix, represents the time after the Kalman filter update step The state vector estimate, Indicates the updated time The covariance matrix, Represents the unit matrix; the observation vector is formed by combining the position information measured by GNSS with the relative position and velocity information obtained by IMU pre-integration , the position, speed and posture of the intelligent sweeper By repeating the prediction and update steps, the position and speed information measured by GNSS are integrated with the relative position and speed information obtained by IMU pre-integration, so as to obtain a more accurate position, speed and attitude estimation of the intelligent sweeper, effectively overcoming the problems of IMU error accumulation and GNSS signal shielding; the final position estimation after the Kalman filter iteration is and its covariance matrix ; Step 2.3, static data processing When the smart sweeper is in a static state, it mainly relies on 3D laser radar SLAM for positioning; the 3D laser radar constructs point cloud data of the surrounding environment by emitting laser beams and receiving reflected light; in the static state, the position of the smart sweeper in the map is determined by the iterative closest point algorithm. First, determine the error function of the iterative closest point algorithm : in, Indicates the current point cloud frame The first Points are used to calculate the distance between the corresponding points on the map point cloud in the point cloud matching algorithm; Representing map point cloud The points to determine the position of the sweeper; Then, the optimization algorithm is used to solve the transformation matrix that minimizes the error function, thereby determining the position of the smart sweeper in the map; in each iteration, the point cloud is calculated Relative to point cloud The translation vector and the rotation matrix , making the point cloud After transformation: in, Represents the transformed point cloud; the iteration target is With point cloud The distance error between the corresponding points is the smallest; the iterative process is repeated until the error converges to meet the set accuracy requirements. At this time, the transformation matrix obtained is the positioning result of the intelligent sweeper in the map. , to obtain accurate positioning of the sweeper; the final position estimation of SLAM The covariance matrix is .

4. The high-precision, low-cost, seamless positioning system and method for an intelligent road sweeper according to claim 1, characterized in that: The process of seamless positioning of 3D LiDAR SLAM and GNSS / IMU collaborative sensing in step 3 can be expressed as follows: Step 3.1 Calculate the fusion weight matrix The calculation formula of the fusion weight matrix is ​​as follows: in, is the fusion weight matrix, if The variance of the diagonal elements of is small, indicating that the position estimation of the GNSS / IMU combination is more accurate. The value of the corresponding element in will be larger, giving the GNSS / IMU position estimate a greater weight during fusion; Step 3.2: Position estimation fusion The calculated fusion weight matrix and , Substitute into the fusion formula: Represents the final fused position estimate; this position estimate combines the advantages of GNSS / IMU in macro positioning and motion state estimation and the advantages of 3D lidar SLAM in local environment positioning and map construction, thereby improving the positioning accuracy of the smart sweeper.

Images