Multi-source fusion robot navigation method and system based on adaptive disaster recovery processing

Abstract

The application discloses a multi-source fusion robot navigation method and system based on adaptive disaster recovery processing, relates to the technical field of robot navigation, and if the repositioning fails, switches to a disaster recovery mode, mainly controls the robot to return to the home position by using Beidou navigation, and continuously detects a laser positioning state, judges whether the laser positioning is recovered, in the returning process, if the laser positioning is not recovered when reaching a charging room area, controls the robot to stop in place and waits until the laser positioning is recovered to normal or other instructions are received, if the laser positioning is recovered in the returning process, re-performs dynamic weight distribution according to related parameters of the laser and Beidou, recovers multi-source fusion navigation, and continues to advance to a target position or executes a subsequent task. The navigation system realizes reliable navigation and abnormal self-recovery in a complex environment through detection of matching degrees of the laser and Beidou, an error ellipse, hierarchical triggering of coordinate repositioning / returning to the home position disaster recovery / laser positioning waiting and other strategies.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot navigation, in particular to a multi-source fusion robot navigation method and system based on adaptive disaster recovery processing. BACKGROUND

[0002] When robot navigation is performed in a complex environment (such as dynamic obstacle shielding and weak satellite signal area), problems such as insufficient laser matching degree and Beidou positioning error exceeding the limit may occur, resulting in navigation failure, wandering or deviation. In view of the requirements for real-time monitoring of laser matching degree, controllability verification of Beidou error, rapid repositioning in the case of low matching degree, and self-recovery of laser positioning during the return process in a complex scenario, the traditional fixed weight fusion or single disaster recovery mode cannot meet the requirements.

[0003] Based on this requirement, the present application provides a multi-source fusion robot navigation method based on adaptive disaster recovery processing, which realizes reliable navigation and abnormal self-recovery in a complex environment by detecting the matching degree of laser and Beidou and the error ellipse, and triggering coordinate repositioning / return disaster recovery / laser positioning waiting strategies in stages. SUMMARY

[0004] The purpose of the present application is to provide a multi-source fusion robot navigation method and system based on adaptive disaster recovery processing to solve the problems in the background art.

[0005] To achieve the above-mentioned purpose, the present application provides the following technical scheme: a multi-source fusion robot navigation method based on adaptive disaster recovery processing, the navigation method comprising the following steps:

[0006] S1: After the robot starts running, the matching degree of laser and Beidou is detected, and the Beidou positioning error ellipse is calculated, if the matching degree is higher than the matching degree threshold and the difference is within the standard range, then the dynamic weight distribution of the double-layer filter architecture of improved extended Kalman filter + factor graph optimization is adopted, and the navigation continues;

[0007] S2: When the matching degree of laser and Beidou is lower than the matching degree threshold, the coordinate repositioning function of the Beidou system is started, if the repositioning is successful, it is switched to the auxiliary mode, and the Beidou navigation is continued to run; if the repositioning fails, it is switched to the disaster recovery mode, the Beidou navigation is used to control the robot to return, and the laser positioning state is continuously detected to determine whether the laser positioning is found back;

[0008] S3: During the return process, if the laser positioning has not been restored when the charging room area is reached, the robot is controlled to stop and wait until the laser positioning is restored to normal or other instructions are received, if the laser positioning is found back during the return, then the dynamic weight distribution is re-performed according to the related parameters of laser and Beidou, the multi-source fusion navigation is restored, and the robot continues to move towards the target location or performs subsequent tasks.

[0009] Preferably, when the matching degree of the laser and the Beidou is lower than the matching degree threshold, the coordinate relocation function of the Beidou system is started, and if the relocation is successful, the system is switched to an auxiliary mode, and the Beidou navigation is mainly continued to run, including the following steps:

[0010] The coordinate relocation function of the Beidou satellite navigation is started, the current robot position is verified through positioning means, and the navigation capability is restored, the positioning interface of the Beidou module is called, and the absolute coordinates at the current time are obtained;

[0011] If the output Beidou coordinates after the relocation meet the preset reliability condition, it is determined that the coordinate relocation is successful, and the system is switched to the auxiliary mode;

[0012] In the auxiliary mode, the navigation strategy takes the Beidou satellite navigation data as the core, and is supplemented by the dynamic information provided by the inertial measurement unit to compensate the motion, the navigation continuity is maintained in the laser failure scene, and the robot continues to run along the original planned path.

[0013] Preferably, if the relocation fails, the system is switched to a disaster recovery mode, the Beidou navigation is mainly used to control the robot to return, and the laser positioning state is continuously detected to determine whether the laser positioning is recovered, including the following steps:

[0014] If the Beidou coordinate relocation function fails to meet the reliability condition, it is determined that the relocation fails, the system is switched to the disaster recovery mode, and the inertial measurement unit and the odometer are combined to navigate, and the absolute position reference provided by the Beidou navigation is combined.

[0015] Preferably, the system is switched to the disaster recovery mode, the inertial measurement unit and the odometer are combined to navigate, and the absolute position reference provided by the Beidou navigation is combined, including the following steps:

[0016] Based on the angular velocity and acceleration raw data output by the inertial measurement unit, the relative motion increment of the robot is calculated, and the travel distance and direction of the robot on the ground contact surface are calculated in combination with the wheel rotation measured by the odometer;

[0017] The absolute position coordinates of the Beidou navigation are introduced as a global constraint, the return path planning preferentially selects a known safe route, the robot moves along the safe route to the target return point, and the laser positioning state detection thread is continuously run in the background.

[0018] Preferably, the laser positioning state detection thread is continuously run in the background, including the following steps:

[0019] The current frame data of the laser radar is periodically collected, and is matched with the environment features in the pre-stored map, the number of matched feature points and the spatial consistency index are counted;

[0020] If the number of successfully matched laser feature points detected at a certain moment during the return process reaches the number threshold, and the spatial error of the matching result is within the acceptable range, then it is determined that the laser positioning status has been restored.

[0021] If no laser matching feature is detected until the charging room area is reached, the disaster recovery mode will be maintained.

[0022] Preferably, during the return journey, if laser positioning is not restored upon reaching the charging station area, the robot will stop and wait until laser positioning is restored or other instructions are received. If laser positioning is restored during the return journey, dynamic weight allocation will be performed based on the relevant parameters of laser and BeiDou navigation to restore multi-source fusion navigation, allowing the robot to continue moving towards the target location or performing subsequent tasks. This includes the following steps:

[0023] When the robot continues to move towards the charging room area according to the return path planned in the disaster recovery mode, the laser positioning status detection logic is triggered. If the laser radar still fails to detect environmental matching features at the location point, it is determined that the laser positioning has not been recovered. At this time, the control command is output to control the robot to perform the on-site stop action and enter the waiting state.

[0024] If at some point in the future the number of laser matching points reaches the threshold and the spatial error of the matching result is within the allowable range, then the laser positioning is determined to have returned to normal, the waiting state is exited and the navigation recovery process is initiated.

[0025] If laser positioning recovery is not detected within the preset maximum waiting time, or if other instructions are received from the external control terminal, then the corresponding operation will be performed according to the instructions.

[0026] Preferably, during the return journey, if laser positioning is not restored upon reaching the charging station area, the robot is controlled to stop and wait until laser positioning is restored or other instructions are received. If laser positioning is restored during the return journey, dynamic weight allocation is performed based on the relevant parameters of laser and BeiDou navigation to restore multi-source fusion navigation, allowing the robot to continue moving towards the target location or performing subsequent tasks. The process also includes the following steps:

[0027] If, at some point during the robot's return path, it detects that the current frame point cloud data of the LiDAR successfully matches the environmental features of the pre-stored map, and the matching result meets the recovery conditions, then it is determined that the laser positioning status has been restored. At this time, the disaster recovery mode is terminated, the multi-source fusion navigation mechanism is reactivated, and the weight allocation strategy of the dual-layer filtering architecture is dynamically adjusted based on the real-time parameters of the current laser and BeiDou.

[0028] Preferably, the formula for calculating the matching degree is: ,in, Indicates laser matching degree, used to measure the degree of matching between LiDAR data. This refers to the number of successfully matched points, specifically the number of points in the point cloud data obtained from LiDAR scanning that can be successfully matched with points in a map or other reference data. This represents the total number of points, that is, the total number of point cloud data points acquired by the LiDAR in a single scan.

[0029] Preferably, the formula for calculating the BeiDou positioning error ellipse is:

[0030] ,in, The ellipse representing the BeiDou positioning error describes the error in BeiDou positioning in the planar direction. The standard deviation represents the eastward positioning error and is used to reflect the dispersion of BeiDou positioning measurements in the eastward direction. The standard deviation represents the northward positioning error and is used to measure the dispersion of BeiDou positioning measurements in the northward direction.

[0031] This application also provides a multi-source fusion robot navigation system based on adaptive disaster recovery processing, including a matching analysis module, a control module, and a positioning judgment module;

[0032] Matching Analysis Module: After the robot starts running, it detects the matching degree between the laser and Beidou, and calculates the Beidou positioning error ellipse. If the matching degree is higher than the matching degree threshold and the difference is within the standard range, it adopts a dynamic weight allocation of a two-layer filtering architecture of improved extended Kalman filter + factor graph optimization to continue navigation.

[0033] Control module: When the matching degree between laser and Beidou is lower than the matching degree threshold, the coordinate repositioning function of Beidou system is activated. If the repositioning is successful, it switches to auxiliary mode and continues to operate with Beidou navigation as the main method. If the repositioning fails, it switches to disaster recovery mode and uses Beidou navigation as the main method to control the robot to return to home and continuously detects the laser positioning status to determine whether the laser positioning has been recovered.

[0034] Positioning and Judgment Module: During the return journey, if the laser positioning is not restored upon reaching the charging room area, the robot will stop on the spot and wait until the laser positioning is restored or other instructions are received. If the laser positioning is restored during the return journey, the robot will dynamically re-allocate the weights based on the relevant parameters of the laser and Beidou navigation, restore multi-source fusion navigation, and continue to move towards the target location or perform subsequent tasks.

[0035] The technical effects and advantages provided by the present invention in the above technical solution are as follows:

[0036] 1. This application detects the matching degree between laser and BeiDou and the BeiDou positioning error ellipse in real time, and uses a two-layer filtering architecture with dynamic weight allocation (improved extended Kalman filter + factor graph optimization) to prioritize the use of multi-source fusion navigation when the matching degree and positioning accuracy meet the standards, so as to ensure high-precision positioning and stable operation in normal environments.

[0037] 2. This application enables rapid verification of BeiDou's independent positioning capability by activating the BeiDou coordinate repositioning function. Upon successful repositioning, the system can switch to auxiliary mode to continue navigation with BeiDou as the primary system, avoiding navigation interruption caused by temporary laser failure. Furthermore, when repositioning fails, the system automatically switches to disaster recovery mode, using BeiDou navigation as the primary system to control the robot's safe return. During the return process, the system continuously monitors the laser positioning status to achieve proactive risk avoidance and path recovery in abnormal situations.

[0038] 3. This application addresses the issue of traditional navigation schemes easily losing control when lost, veering off course, or experiencing low confidence levels by waiting in the charging area or continuously retrieving laser signals. Through a hierarchical switching mechanism of main mode – auxiliary mode – disaster recovery mode and adaptive disaster recovery logic, it effectively handles complex operating conditions of single / dual-source anomalies in laser / BeiDou navigation, significantly improving the robot's fault tolerance, positioning continuity, and task completion reliability. Attached Figure Description

[0039] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in this invention. For those skilled in the art, other drawings can be obtained based on these drawings.

[0040] Figure 1 This is a flowchart of the navigation method of the present invention. Detailed Implementation

[0041] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0042] Example: This example provides a multi-source fusion robot navigation method based on adaptive disaster recovery processing. Please refer to [link / reference]. Figure 1 As shown, the navigation method includes the following steps:

[0043] S1: After startup, the matching degree between the laser and BeiDou is first detected. According to the formula... (With a matching threshold set at 30%), the laser matching degree is calculated, where... This represents the laser matching degree, used to measure the degree of matching between LiDAR data and existing maps or other reference data. When this value is higher than the set matching degree threshold (30% in this case), it indicates that the laser matching is good, and navigation can be performed reliably based on the laser data. This refers to the number of successfully matched points, which is the number of points in the point cloud data obtained from LiDAR scanning that can be successfully matched with points in a map or other reference data. These matched points indicate that the environmental features detected by the LiDAR at that location are consistent with known information. The total number of points indicates the total number of point cloud data points acquired by the LiDAR in a single scan. It represents the total number of environmental points covered by this scan, and is also determined according to the formula... (RTK mode) Calculate the BeiDou positioning error ellipse. The error ellipse representing BeiDou positioning comprehensively describes the error of BeiDou positioning in a planar direction (usually divided into east and north directions). It is a quantitative representation of BeiDou positioning accuracy in a two-dimensional plane. When this error value is within the standard range, it indicates that BeiDou positioning accuracy is high and can be reliably used for navigation. The standard deviation, representing the eastward positioning error, reflects the dispersion of BeiDou positioning measurements in the eastward direction, indicating the magnitude of uncertainty in eastward positioning. A larger standard deviation suggests a potentially larger error in eastward positioning and lower positioning accuracy. The standard deviation of the northward positioning error measures the dispersion of BeiDou positioning measurements in the northward direction, i.e., the uncertainty of northward positioning. Similarly, the larger this value, the lower the accuracy of northward positioning may be. If the matching degree is higher than the matching degree threshold and the difference is within the standard range, a dynamic weight allocation using an improved extended Kalman filter (EKF) + factor graph optimization two-layer filtering architecture is employed to continue normal navigation; if the matching degree is lower than the matching degree threshold, the process proceeds to the next step.

[0044] S2: When the laser and BeiDou matching degree is lower than the matching degree threshold, the coordinate repositioning function of the BeiDou system is activated. If the repositioning is successful, it switches to auxiliary mode (BeiDou) and continues to operate with BeiDou navigation as the primary method; if the repositioning fails, it directly switches to disaster recovery mode (IMU+ODO), in which case BeiDou navigation is the primary method to control the robot to return to its starting point. The laser positioning status is continuously monitored to determine whether the laser positioning has been recovered.

[0045] S3: During the return journey, if the laser positioning has not been restored when the robot reaches the charging room area, the robot will stop on the spot and wait until the laser positioning is restored or other valid instructions are received. If the laser positioning is successfully restored during the return journey, the robot will dynamically re-allocate the weights based on the relevant parameters of the laser and Beidou navigation, restore the multi-source fusion navigation, and continue to move towards the target location or perform subsequent tasks.

[0046] This embodiment also provides a multi-source fusion robot navigation system based on adaptive disaster recovery processing. Please refer to [link / reference]. Figure 1 As shown, it includes a matching analysis module, a control module, and a location judgment module;

[0047] Matching analysis module: After the robot starts running, it detects the matching degree between the laser and Beidou, and calculates the Beidou positioning error ellipse. If the matching degree is higher than the matching degree threshold and the difference is within the standard range, it adopts a dynamic weight allocation of a two-layer filtering architecture of improved extended Kalman filter + factor graph optimization, and continues navigation. The matching degree analysis result is sent to the control module, and the positioning result is sent to the positioning judgment module.

[0048] Control module: When the matching degree between laser and Beidou is lower than the matching degree threshold, the coordinate repositioning function of Beidou system is activated. If the repositioning is successful, it switches to auxiliary mode and continues to operate with Beidou navigation as the main method. If the repositioning fails, it switches to disaster recovery mode and controls the robot to return to home with Beidou navigation as the main method. It also continuously monitors the laser positioning status to determine whether the laser positioning has been recovered. The judgment result is sent to the positioning judgment module.

[0049] Positioning and Judgment Module: During the return journey, if the laser positioning is not restored upon reaching the charging room area, the robot will stop on the spot and wait until the laser positioning is restored or other instructions are received. If the laser positioning is restored during the return journey, the robot will dynamically re-allocate the weights based on the relevant parameters of the laser and Beidou navigation, restore multi-source fusion navigation, and continue to move towards the target location or perform subsequent tasks.

[0050] Furthermore, the various steps of this application are described in detail below:

[0051] S1: After startup, the matching degree between the laser and BeiDou is first detected. Simultaneously, the BeiDou positioning error ellipse is calculated. If the matching degree is higher than the matching degree threshold and the difference is within the standard range, a dynamic weight allocation using a two-layer filtering architecture with improved extended Kalman filter (EKF) and factor graph optimization is employed to continue normal navigation.

[0052] After the robot starts up and enters operation, it first synchronously collects real-time data from lidar and BeiDou satellite navigation, initiating the environmental perception and positioning accuracy verification process. The core of this step is to ensure that navigation has a reliable positioning foundation from the initial stage through preprocessing and reliability assessment before multi-source data fusion.

[0053] Based on the point cloud data (a discrete set of 3D spatial features such as obstacles, ground, and structures in the environment) acquired by the current frame scan of the LiDAR, feature matching is performed with a pre-stored high-precision map (or a constructed local feature map) to calculate the degree of matching between the LiDAR data and known environmental features, i.e., the matching degree between LiDAR and BeiDou. This matching process extracts key features (such as geometric features like planes, edges, and corners) from the LiDAR point cloud and compares them with reference features at corresponding locations on the map in terms of spatial position, normal vector, and density distribution. The number of successfully matched feature points (i.e., points that meet preset geometric consistency, distance thresholds, and normal vector angle ranges) is counted to assess the reliability of the association between the current LiDAR scan data and the map environment. Simultaneously, positioning data output by the BeiDou satellite navigation module (including parameters such as longitude, latitude, altitude, and positioning accuracy factor PDOP) is called in parallel. Based on the BeiDou positioning results, a positioning error ellipse is calculated (this ellipse characterizes the error distribution range of the current positioning result in the planar coordinate system by analyzing the standard deviation parameters (eastward error standard deviation) and (northward error standard deviation) of the BeiDou positioning output, combined with the error propagation model, and is used to quantify the positioning accuracy).

[0054] After calculating the laser matching degree and the BeiDou positioning error ellipse, the matching degree value is compared with a preset matching degree threshold (e.g., 30%, which can be adjusted according to actual scenario requirements). Simultaneously, the comprehensive error corresponding to the BeiDou positioning error ellipse (e.g., calculating the planar positioning error by taking the square root of the sum of the squares of the standard deviations of the eastward and northward errors, or directly referencing the accuracy index ≤3cm in BeiDou RTK mode) is checked to see if it is within the standard allowable range (e.g., error less than or equal to 3 cm, or meeting the preset threshold for high-precision navigation tasks). If the determination result is that the laser matching degree is higher than the matching degree threshold (indicating strong correlation between the current LiDAR scanning data and map environment features, and reliable environment identification), and the comprehensive error corresponding to the BeiDou positioning error ellipse is within the standard range (indicating that the BeiDou independent positioning accuracy meets navigation requirements), then the improved extended Kalman filter (EKF) and factor graph optimization two-layer filtering architecture are activated, and a dynamic weight allocation strategy is executed to achieve efficient fusion of multi-source data and navigation decision-making.

[0055] Among them, the improved extended Kalman filter (EKF) serves as the primary fusion layer, and its processing logic is implemented based on a state-space model:

[0056] The robot's state vector is defined (including key parameters such as position x, y, z, velocity v, attitude angles (e.g., roll, pitch, yaw) and sensor biases (e.g., gyroscope bias, accelerometer bias). A state prediction equation is used to calculate the current state estimate based on the previous state and the current control input u (e.g., motor speed, steering command), combined with a kinematic model. For example, position updates are based on velocity and time integration, and attitude angle updates consider the cumulative effect of angular velocity. Process noise w (characterizing motion model uncertainties, such as wheel slippage and motion errors caused by terrain undulations) is introduced. Subsequently, observation data from LiDAR and BeiDou (e.g., position correction obtained from LiDAR point cloud matching, absolute position coordinates output by BeiDou) are used as observations. An observation equation is used to map the state vector to the observable space (e.g., the deviation between the LiDAR-matched position and the predicted position, the difference between the BeiDou coordinates and the predicted coordinates) to calculate the observation residual. The state prediction value is then corrected using the observation noise v (characterizing sensor measurement errors, such as LiDAR point cloud noise and BeiDou signal multipath interference), resulting in a more accurate state estimate for the current moment.

[0057] The factor graph optimization layer, as a secondary fusion layer, achieves its function by constructing a globally consistent optimization problem:

[0058] The robot's core state variables (such as position x / y / z, velocity v, and sensor bias) are defined as nodes in the factor graph. The constraints between different sensor data (such as absolute position constraints from BeiDou and pose constraints obtained from LiDAR matching) are defined as edges (i.e., constraints) in the factor graph. Each factor (constraint) is accompanied by a corresponding error function (such as BeiDou position bias error) and a covariance matrix (characterizing the reliability of the constraint). At each time step, the nodes and factors from the current and historical time steps are integrated into a complete factor graph. This graph is then iteratively solved using a nonlinear least squares optimization algorithm (such as the Levenberg-Marquardt method) to minimize the cumulative error of all factors, thereby obtaining the globally optimal state estimation result.

[0059] Through the synergistic effect of a dual-layer filtering architecture, the improved extended Kalman filter is responsible for high-frequency, short-cycle state prediction and initial correction (responding to rapid dynamic changes, such as robot emergency stops and turns), while the factor graph optimization is responsible for low-frequency, long-cycle global consistency correction (eliminating accumulated errors, such as drift problems during long-term operation). This ultimately achieves high-precision dynamic weight allocation between LiDAR and BeiDou data—for example, when the LiDAR matching degree is high and the BeiDou error is low, the fusion weight of the LiDAR data is dynamically increased (enhancing positioning accuracy driven by environmental features), while reasonably suppressing potential biases in the BeiDou data; conversely, the weight ratio is adjusted to balance the contributions of both. Based on this fusion result, the robot navigation generates reliable motion control commands (such as speed adjustment and path tracking), thus continuously executing high-precision normal navigation tasks in the main mode where both matching degree and positioning accuracy meet the standards, ensuring the accuracy of path planning and the reliability of obstacle avoidance.

[0060] Based on the raw angular velocity and acceleration data output by the inertial measurement unit (IMU), the relative motion increment of the robot in a short period of time is calculated. The calculation logic is as follows:

[0061] The raw angular velocity data output by the IMU is acquired at a fixed sampling frequency (e.g., 100Hz-200Hz). The units are radians per second, corresponding to the rotational speeds around the X, Y, and Z axes in the body coordinate system, respectively, and the raw acceleration data (usually expressed as...). The units are meters per second², corresponding to the specific forces along the X, Y, and Z axes in the body coordinate system (i.e., the acceleration components after deducting gravity). To reduce high-frequency noise interference, the raw data needs to be preprocessed. High-frequency vibration noise (such as motor vibration or instantaneous impacts caused by wheel slippage) is filtered out using a digital low-pass filter (e.g., a second-order Butterworth filter with a cutoff frequency set to 1 / 101 / 5 of the sampling frequency), while retaining effective low-frequency motion information to obtain the smoothed angular velocity. and acceleration ;

[0062] Using the quaternion of the previous period at the current moment Based on (representing the rotational relationship of the robot from the navigation coordinate system to the body coordinate system at the previous moment), It is the real part of the quaternion. It is the imaginary part of the quaternion, using the pre-processed angular velocity. Computer volume coordinate system in an extremely short time interval The attitude rotation increment within a period of 0.01 seconds (i.e., a 100Hz sampling period). The specific processing logic is as follows: The angular velocity vector... Converted to rotational vector form, using quaternion differential equations (in Numerical integration is performed using an antisymmetric matrix constructed from angular velocities (e.g., employing the fourth-order Runge-Kutta method or the first-order Euler method). The derivative of a quaternion. Given quaternions, find the quaternion increment for the current period. and compare it with the quaternion from the previous time step. Multiplication ( After normalization, we obtain the quaternion at the current time step. This quaternion can be further converted to Euler angles (e.g., roll angle). Pitch angle Yaw angle A rotation matrix, or a navigation coordinate system, is used to describe the real-time attitude relationship between the robot and the body coordinate system.

[0063] Preprocessed raw acceleration data This is the specific force measurement in the body coordinate system (including the net acceleration after the robot's own acceleration and the gravitational component cancel each other out). To obtain the robot's actual acceleration relative to the navigation coordinate system (such as an inertial frame or a north-northeast coordinate system), the specific force vector in the body coordinate system needs to be... Using the quaternion at the current moment (or the corresponding rotation matrix) Transform to the navigation coordinate system to obtain the relative force components in the navigation coordinate system. The essence of this transformation logic is to align the acceleration direction of the body coordinate system to the navigation coordinate system using a quaternion rotation formula (or rotation matrix multiplication), thereby eliminating the influence of changes in body attitude on the acceleration direction.

[0064] In the navigation coordinate system, assuming the robot operates within a short time interval... The velocity change within the time frame is primarily determined by the specific force integral (ignoring higher-order effects such as Earth's rotation), and the initial velocity at the current moment is the velocity calculated in the previous cycle. (Initial velocity can be initialized via BeiDou or external sensors). Comparison of force components in the transformed navigation coordinate system. Perform first-order integral ( , This allows us to obtain the velocity increment and then update the velocity at the current moment. Subsequently, the updated velocity components were integrated again using a first-order integral. The displacement increment of the robot in the navigation coordinate system is obtained. (This represents the relative distance the robot moves in the east, north, and vertical directions during the current cycle).

[0065] Based on the combined attitude calculation and force integration results, the system outputs the robot's position within a short time interval. The relative motion increment within, including displacement increment ( (used to describe position changes) and attitude changes ( It can be achieved through quaternion differences Convert to Euler angle changes, or directly use quaternion increments. The attitude rotation is used as input for subsequent inertial navigation algorithms (such as fusion with BeiDou / laser data) to correct short-term motion accumulation errors or construct motion constraint factors. This calculation process ensures the numerical stability of attitude updates through quaternion attitude solution and achieves relative motion estimation in a short time by combining specific force integral, providing a basic relative position rate estimation capability for early flight in disaster recovery mode.

[0066] S2: When the laser and BeiDou matching degree is lower than the matching degree threshold, the BeiDou coordinate repositioning function is activated. If the repositioning is successful, it switches to auxiliary mode (BeiDou) and continues to operate with BeiDou navigation as the primary method; if the repositioning fails, it directly switches to disaster recovery mode (IMU+ODO), in which case BeiDou navigation is the primary method to control the robot to return to home. The laser positioning status is continuously monitored to determine whether the laser positioning has been recovered.

[0067] When the laser-BeiDou matching degree calculated in real time in step S1 is lower than the preset matching degree threshold (e.g., 30%, which is set based on the reliability boundary of environmental feature matching; a value lower than this indicates a significant decrease in the correlation between the current laser radar scanning data and the environmental features of the pre-stored map, which may lead to positioning failure), the anomaly handling process is triggered:

[0068] First, activate the coordinate repositioning function of the BeiDou satellite navigation system to verify the reliability of the robot's current position and attempt to restore navigation capabilities. Then, call the BeiDou module's positioning interface (e.g., differential positioning mode) to obtain the absolute coordinates at the current moment (including longitude, latitude, altitude, and positioning accuracy factor PDOP), and analyze the positioning stability by combining this with historical trajectory data (e.g., the BeiDou position sequence of the most recent N seconds).

[0069] If the BeiDou coordinates output after repositioning meet the preset reliability conditions (e.g., positioning accuracy factor ≤ 3, standard deviation of errors in the east and north directions both less than or equal to 3 cm, or spatial deviation of multiple consecutive positioning results less than a threshold), then the coordinate repositioning is considered successful, and the system switches to auxiliary mode (BeiDou dominant). In this mode, the navigation strategy uses BeiDou satellite navigation data as the core, supplemented by short-term dynamic information (such as acceleration and angular velocity) provided by the inertial measurement unit (IMU) for short-term motion compensation, but the decision weight is tilted towards BeiDou (e.g., BeiDou data accounts for 70% to 90% of the fusion result), thereby maintaining basic navigation continuity in laser failure scenarios, and the robot continues to run along the original planned path or a temporary safe path (e.g., a conservative path that avoids known obstacles).

[0070] If the BeiDou coordinate repositioning function fails to meet reliability conditions (e.g., positioning accuracy factor PDOP > 3, or eastward / northward error standard deviation exceeding 3 cm, manifested as large fluctuations in positioning results and significant deviations from known map reference points), repositioning is deemed a failure, and the system immediately switches to disaster recovery mode. Relying on the combined navigation capabilities of the inertial measurement unit (IMU) and odometry (ODO), combined with the absolute position reference provided by BeiDou navigation (as a global directional constraint), the robot is controlled to perform a safe return-to-home procedure. Specifically:

[0071] Based on the raw angular velocity and acceleration data output by the inertial measurement unit, the robot's relative motion increment (including displacement) over a short period of time is calculated using inertial navigation algorithms (such as quaternion attitude calculation + specific force integration). With attitude change Simultaneously, by combining the wheel rotation measured by an odometer (such as the encoder pulse count of a wheeled robot), the robot's travel distance and direction on the ground contact surface are calculated. Based on this, the absolute position coordinates of BeiDou navigation are introduced as a global constraint (e.g., aligning the currently calculated position with the absolute position output by BeiDou every T seconds to correct accumulated errors), avoiding the drift problem of pure inertial navigation. The return path planning prioritizes known safe routes (e.g., a stored sequence of return point coordinates or a straight path to the nearest charging station). The robot moves along this path towards the target return point (such as the charging station or the initial starting position), while a laser positioning status detection thread continuously runs in the background.

[0072] Raw angular velocity and acceleration data output from the inertial measurement unit (IMU):

[0073] After filtering out high-frequency noise using a digital low-pass filter, the preprocessed angular velocity is numerically integrated using quaternion differential equations (e.g., using the first-order Euler method) to solve for the quaternion increment of the current cycle and update the quaternion of the previous moment, thereby obtaining the robot's attitude in the navigation coordinate system (converted to a rotation matrix or Euler angles via quaternions). The raw acceleration data in the body coordinate system is then converted to the navigation coordinate system to obtain the specific force component. Subsequently, the specific force component is integrated first-order to calculate the velocity increment and update the current velocity. Then, the velocity is integrated first-order to obtain the robot's displacement increment in the navigation coordinate system (i.e., the displacement in the relative motion increment). ) and calculating attitude changes through quaternion differences (i.e. Simultaneously, based on the wheel rotation measured by the odometer (such as encoder pulse count), the pulse count is converted into the wheel rolling angle or linear displacement through the wheel radius and transmission ratio. Combined with the robot's wheelbase and wheelbase geometric parameters, the ground travel distance (propulsion along the longitudinal axis of the vehicle) and direction (angular offset determined by the speed difference between the left and right wheels or the steering angle) corresponding to wheel rolling are calculated using a differential drive model or Ackerman steering model. Finally, the relative displacement and attitude calculated by the IMU are initially fused with the travel distance and direction calculated by the odometer (such as through weighted averaging or simple complementation) to obtain a more reliable travel distance (considering both wheel rolling and inertial displacement) and direction (integrating the IMU attitude and odometer steering information) on the ground contact surface. This serves as a short-term motion increment estimate for subsequent pose correction and path tracking in the navigation system.

[0074] The laser positioning status detection logic periodically (e.g., every 0.5 seconds or every scan of a point cloud frame) collects the current frame data of the LiDAR and attempts to match it with environmental features in the pre-stored map (the matching method is consistent with the laser matching degree calculation logic in S1, extracting key geometric features and comparing spatial position, normal vector, and density distribution), and counts the number of successfully matched feature points and spatial consistency index. If, at a certain moment during the return process, the number of successfully matched laser feature points reaches the threshold (e.g., the number of matched points in two consecutive frames exceeds M, or the matching degree in a single frame exceeds 15%), and the spatial error of the matching result is within an acceptable range (e.g., the deviation from the map reference feature is less than 0.5 meters), then the laser positioning status is determined to have been restored (i.e., laser positioning has been recovered); conversely, if no effective laser matching feature is detected until reaching the charging room area (e.g., less than D meters from the charging room entrance, where D is a preset safe waiting radius, such as 2 meters), then the disaster recovery mode is maintained until subsequent processing (entering step S3).

[0075] S3: During the return journey, if the laser positioning has not been restored when the robot reaches the charging room area, the robot will stop on the spot and wait until the laser positioning is restored or other valid instructions are received. If the laser positioning is successfully restored during the return journey, the robot will dynamically re-allocate the weights based on the relevant parameters of the laser and Beidou navigation, restore the multi-source fusion navigation, and continue to move towards the target location or perform subsequent tasks.

[0076] During the robot's return journey in disaster recovery mode, the recovery status of its laser positioning is continuously monitored. Based on whether the laser positioning is successfully regained before reaching the charging station area, differentiated control strategies are implemented to ensure the robot eventually enters a safe state or restores efficient navigation capabilities. This step's processing logic is specifically divided into two branches:

[0077] Branch 1 (Laser Positioning Not Recovered): When the robot continues to move towards the charging room area according to the return path planned in the disaster recovery mode (e.g., the path corrected by the estimated position and the absolute constraint of Beidou), (e.g., the distance between the current position and the entrance of the charging room is less than the preset threshold D, usually 2 to 3 meters, or the robot is confirmed to be within the preset geofence area around the charging room through map matching), the final laser positioning status detection logic is triggered. If, up to this position, the LiDAR still fails to detect effective environmental matching features (i.e., the number of key feature matching points between the laser point cloud and the pre-stored map is less than the quantity threshold M in multiple consecutive scanning cycles, for example, the number of matching points is less than 5 in 3 consecutive frames, or the matching degree of a single frame is less than 10%, indicating that the reliability of environmental feature association is insufficient), it is determined that the laser positioning has not been recovered. At this time, a control command is immediately output to control the robot to perform an on-site stop action (e.g., send a zero-speed command through the motor drive module, or gradually reduce the speed to zero based on the motion control algorithm), and enter a low-power waiting state. During the waiting period, low-frequency data acquisition from the lidar, BeiDou, and IMU sensors is maintained (e.g., laser scan once per second, BeiDou position updated every 10 seconds). The laser positioning status is periodically (e.g., every 5 seconds) re-detected. If the number of laser matching points reaches a threshold at some point (e.g., more than M matching points in a single frame, or a matching degree exceeding 15%), and the spatial error of the matching result is within the allowable range (e.g., deviation from map reference features less than 0.3 meters), then the laser positioning is determined to have returned to normal, the waiting state is exited, and the navigation recovery process is initiated. If no laser positioning recovery is detected within the preset maximum waiting time T (e.g., 5 minutes), or if other valid instructions are received from an external control terminal (e.g., operator's handheld device) (e.g., forced continued waiting, manual takeover of navigation, modification of target position, etc.), then the corresponding operation is executed according to the instruction (e.g., maintaining the waiting state or responding to manual instructions).

[0078] Branch 2 (Laser Positioning Recovery): If, at a certain moment on the robot's return path (e.g., the estimated location has not yet reached the charging room area, possibly in the middle area 5 to 10 meters away from the charging room, or within any scanning cycle), the robot detects that the current frame point cloud data of the LiDAR successfully matches the environmental features of the pre-stored map (the matching logic is the same as in S1 / S2: extract key geometric features of the laser point cloud, such as planes, edges, or corners, compare the spatial position with the reference features of the corresponding location in the map, verify the angle between the normal vectors, and analyze the consistency of the density distribution, and count the number of successfully matched feature points and the spatial error), and the matching result meets the recovery conditions (e.g., the number of matched points in two consecutive frames exceeds M, the matching degree of a single frame exceeds 15%, and the deviation from the map reference features is less than 0.5 meters), then it is determined that the laser positioning status has been recovered. At this point, the disaster recovery mode is immediately terminated, the multi-source fusion navigation mechanism is reactivated, and the weight allocation strategy of the dual-layer filtering architecture (improved extended Kalman filter EKF + factor graph optimization) is dynamically adjusted based on the real-time parameters of the current laser and BeiDou (including laser matching degree, standard deviation of eastward / northward error corresponding to BeiDou positioning error ellipse, sensor noise characteristics, etc.).

[0079] Specifically, by evaluating the reliability of laser matching degree (e.g., the more matching points and the smaller the spatial error, the higher the credibility of laser data) and the accuracy of BeiDou positioning error (e.g., the smaller the error, the lower the weight of BeiDou data can be), the dynamic fusion weight of laser and BeiDou data is calculated (e.g., laser weight = matching degree normalized value × error compensation coefficient, BeiDou weight = 1 - laser weight), and this weight is applied to the input layer of the two-layer filter. The contribution ratio of laser observation residual and BeiDou observation residual is adjusted according to the weight. When constructing constraint factors, the factor graph optimization layer assigns differentiated information matrices to laser matching constraints (e.g., laser pose edges between pose nodes) and BeiDou absolute constraints (e.g., BeiDou absolute position edges of position nodes). The inverse of the information matrix corresponds to the covariance matrix. The higher the weight, the larger the value of the information matrix and the stronger the constraint. By dynamically allocating weights, the main mode of multi-source fusion navigation (laser-led or laser-BeiDou collaborative-led) is restored, generating high-precision state estimation results (position, velocity, attitude), which in turn drives the robot to continue moving toward the original target location (if the target does not change during the return journey) or to perform subsequent tasks (such as switching to charging mode after arriving at the charging room and then returning to the work area).

[0080] In the description of this specification, references to terms such as "an embodiment," "example," "specific example," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0081] The preferred embodiments of the present invention disclosed above are merely illustrative of the invention. These preferred embodiments do not exhaustively describe all details, nor do they limit the invention to specific implementations. Clearly, many modifications and variations can be made based on the content of this specification. This specification selects and specifically describes these embodiments to better explain the principles and practical applications of the invention, thereby enabling those skilled in the art to better understand and utilize the invention. The invention is limited only by the claims and their full scope and equivalents.

Claims

1. A multi-source fusion robot navigation method based on adaptive disaster recovery processing, characterized in that: The navigation method includes the following steps: S1: After the robot starts running, it detects the matching degree between the laser and Beidou, and calculates the Beidou positioning error ellipse. If the matching degree is higher than the matching degree threshold and the difference is within the standard range, it adopts a dynamic weight allocation of a two-layer filtering architecture of improved extended Kalman filter + factor graph optimization to continue navigation. S2: When the matching degree between the laser and Beidou is lower than the matching degree threshold, the coordinate repositioning function of the Beidou system is activated. If the repositioning is successful, the system switches to auxiliary mode and continues to operate with Beidou navigation as the main system. If the repositioning fails, the system switches to disaster recovery mode and uses Beidou navigation as the main system to control the robot to return to home. The system also continuously monitors the laser positioning status to determine whether the laser positioning has been recovered. S3: During the return journey, if the laser positioning is not restored when the robot reaches the charging room area, the robot will stop and wait until the laser positioning is restored or other instructions are received. If the laser positioning is restored during the return journey, the robot will dynamically re-allocate the weights based on the relevant parameters of the laser and Beidou, restore the multi-source fusion navigation, and continue to move towards the target location or perform subsequent tasks. When the matching degree between the laser and BeiDou is lower than the matching degree threshold, the coordinate repositioning function of the BeiDou system is activated. If the repositioning is successful, it switches to auxiliary mode and continues to operate with BeiDou navigation as the primary method, including the following steps: Activate the coordinate repositioning function of Beidou satellite navigation, verify the current position of the robot and restore navigation capability through positioning means, and call the positioning interface of Beidou module to obtain the absolute coordinates at the current moment; If the BeiDou coordinates output after repositioning meet the preset reliability conditions, the coordinate repositioning is determined to be successful, and the system switches to auxiliary mode. In auxiliary mode, the navigation strategy is based on BeiDou satellite navigation data, supplemented by dynamic information provided by the inertial measurement unit for motion compensation. In the case of laser failure, the navigation continuity is maintained and the robot continues to run along the original planned path. If relocation fails, switch to disaster recovery mode, using BeiDou navigation as the primary control for the robot to return to home, and continuously monitor the laser positioning status to determine if the laser positioning has been recovered, including the following steps: If the BeiDou coordinate repositioning function fails to meet the reliability conditions, the repositioning is deemed to have failed and the system switches to disaster recovery mode, relying on the combined navigation capabilities of the inertial measurement unit and the odometer, combined with the absolute position reference provided by BeiDou navigation. Switching to disaster recovery mode, relying on the combined navigation capabilities of the inertial measurement unit and odometry, and combining the absolute position reference provided by BeiDou navigation, includes the following steps: Based on the raw data of angular velocity and acceleration output by the inertial measurement unit, the relative motion increment of the robot is calculated. At the same time, combined with the wheel rotation measured by the odometer, the travel distance and direction of the robot on the ground contact surface are estimated. The absolute position coordinates of Beidou navigation are introduced as a global constraint. The return path planning prioritizes known safe routes. The robot moves along the safe route to the target return point, while the laser positioning status detection thread runs continuously in the background.

2. The multi-source fusion robot navigation method based on adaptive disaster recovery processing according to claim 1, characterized in that: The laser positioning status detection thread runs continuously in the background, including the following steps: The current frame data of the lidar is periodically collected and matched with environmental features in the pre-stored map. The number of successfully matched feature points and spatial consistency index are counted. If the number of successfully matched laser feature points detected at a certain moment during the return process reaches the number threshold, and the spatial error of the matching result is within the acceptable range, then it is determined that the laser positioning status has been restored. If no laser matching feature is detected until the charging room area is reached, the disaster recovery mode will be maintained.

3. The multi-source fusion robot navigation method based on adaptive disaster recovery processing according to claim 1, characterized in that: During the return journey, if laser positioning is not restored upon reaching the charging station area, the robot will stop and wait until laser positioning is restored or other instructions are received. If laser positioning is restored during the return journey, dynamic weight allocation will be performed based on the relevant parameters of laser and BeiDou navigation to restore multi-source fusion navigation, allowing the robot to continue moving towards the target location or performing subsequent tasks, including the following steps: When the robot continues to move towards the charging room area according to the return path planned in the disaster recovery mode, the laser positioning status detection logic is triggered. If the laser radar still fails to detect environmental matching features at the location point, it is determined that the laser positioning has not been recovered. At this time, the control command is output to control the robot to perform the on-site stop action and enter the waiting state. If at some point in the future the number of laser matching points reaches the threshold and the spatial error of the matching result is within the allowable range, then the laser positioning is determined to have returned to normal, the waiting state is exited and the navigation recovery process is initiated. If laser positioning recovery is not detected within the preset maximum waiting time, or if other instructions are received from the external control terminal, then the corresponding operation will be performed according to the instructions.

4. The multi-source fusion robot navigation method based on adaptive disaster recovery processing according to claim 1, characterized in that: During the return journey, if laser positioning is not restored upon reaching the charging station area, the robot will stop and wait until laser positioning is restored or other instructions are received. If laser positioning is restored during the return journey, dynamic weight allocation will be performed based on the relevant parameters of laser and BeiDou navigation to restore multi-source fusion navigation, allowing the robot to continue moving towards the target location or performing subsequent tasks. This also includes the following steps: If, at some point during the robot's return path, it detects that the current frame point cloud data of the LiDAR successfully matches the environmental features of the pre-stored map, and the matching result meets the recovery conditions, then it is determined that the laser positioning status has been restored. At this time, the disaster recovery mode is terminated, the multi-source fusion navigation mechanism is reactivated, and the weight allocation strategy of the dual-layer filtering architecture is dynamically adjusted based on the real-time parameters of the current laser and BeiDou.

5. The multi-source fusion robot navigation method based on adaptive disaster recovery processing according to claim 1, characterized in that: The formula for calculating the matching degree is: ,in, Indicates laser matching degree, used to measure the degree of matching between LiDAR data. This refers to the number of successfully matched points, specifically the number of points in the point cloud data obtained from LiDAR scanning that can be successfully matched with points in a map or other reference data. This represents the total number of points, that is, the total number of point cloud data points acquired by the LiDAR in a single scan.

6. The multi-source fusion robot navigation method based on adaptive disaster recovery processing according to claim 5, characterized in that: The formula for calculating the BeiDou positioning error ellipse is as follows: ,in, The ellipse representing the BeiDou positioning error describes the error in BeiDou positioning in the planar direction. The standard deviation represents the eastward positioning error and is used to reflect the dispersion of BeiDou positioning measurements in the eastward direction. The standard deviation represents the northward positioning error and is used to measure the dispersion of BeiDou positioning measurements in the northward direction.

7. A multi-source fusion robot navigation system based on adaptive disaster recovery processing, used to implement the navigation method according to any one of claims 1-6, characterized in that: It includes a matching analysis module, a control module, and a location judgment module; Matching Analysis Module: After the robot starts running, it detects the matching degree between the laser and Beidou, and calculates the Beidou positioning error ellipse. If the matching degree is higher than the matching degree threshold and the difference is within the standard range, it adopts a dynamic weight allocation of a two-layer filtering architecture of improved extended Kalman filter + factor graph optimization to continue navigation. Control module: When the matching degree between laser and Beidou is lower than the matching degree threshold, the coordinate repositioning function of Beidou system is activated. If the repositioning is successful, it switches to auxiliary mode and continues to operate with Beidou navigation as the main method. If the repositioning fails, it switches to disaster recovery mode and uses Beidou navigation as the main method to control the robot to return to home and continuously detects the laser positioning status to determine whether the laser positioning has been recovered. Positioning and Judgment Module: During the return journey, if the laser positioning is not restored upon reaching the charging room area, the robot will stop on the spot and wait until the laser positioning is restored or other instructions are received. If the laser positioning is restored during the return journey, the robot will dynamically re-allocate the weights based on the relevant parameters of the laser and Beidou navigation, restore multi-source fusion navigation, and continue to move towards the target location or perform subsequent tasks.

Images