Multi-source fusion following obstacle avoidance system with target signal loss prediction compensation

Abstract

The invention discloses a multi-source fusion following obstacle avoidance system with target signal loss prediction compensation, and relates to the technical field of robot tracking and positioning, and when a UWB signal is blocked, weakened or lost for a short time, the system automatically starts target state prediction based on a constant speed model (CV) to realize short-time continuous extrapolation of a target position. According to the mechanism, the problems of following interruption, path hopping or robot stagnation caused by measurement interruption of a traditional system are effectively avoided, the continuity and stability of an overall following task are improved, and the continuity of a positioning and following link is remarkably enhanced.

Description

Technical Field

[0001] This invention relates to the field of robot tracking and positioning technology, and in particular to a multi-source fusion following obstacle avoidance system with target signal loss prediction compensation. Background Technology

[0002] With the rapid development of intelligence and robotics, quadruped robots have been gradually applied to scenarios such as patrol and prevention, disaster relief, and urban security. When performing tasks in complex environments, these robots need to have capabilities such as high-precision positioning, autonomous human following, dynamic obstacle avoidance, and path optimization.

[0003] Traditional indoor positioning technologies (such as visual SLAM, inertial positioning, and radio ranging) each rely on different sensing mechanisms and are affected by various factors in complex environments. For example, visual SLAM is sensitive to changes in lighting and occlusion; inertial positioning is susceptible to accumulated errors; and radio ranging can lead to positioning deviations due to multipath reflections. In complex or dynamic environments, relying on a single sensor often fails to provide consistently stable and reliable positioning results. To improve the robustness of positioning systems in practical applications, positioning technologies that integrate multimodal sensors such as UWB (Ultra-Wideband), IMU (Inertial Measurement Unit), and LiDAR are gradually becoming mainstream. By combining the advantages of different sensors, positioning estimates can be obtained more stably under conditions of weak texture, occlusion, or changes in lighting.

[0004] Liu et al. proposed a localization method integrating UWB and IMU, employing extended Kalman filtering (EKF) to tightly couple UWB ranging data with inertial measurement information, achieving joint estimation of the target position. The multi-sensor fusion localization method based on UWB and IMU proposed by Liu et al. is a representative solution in the field of indoor mobile robot localization. This scheme establishes prediction and observation equations for the system and utilizes EKF to achieve real-time estimation of position and attitude, thereby reducing errors caused by single-sensor drift to a certain extent. Its core idea is to use the angular velocity and acceleration provided by the IMU as system inputs, combine them with UWB ranging information, and utilize the prediction-update mechanism of EKF to achieve state estimation and error correction. This method can achieve high localization accuracy in short-distance, low-speed movement scenarios, verifying the feasibility of UWB+IMU fusion in indoor environments. However, this method still has limitations in complex dynamic environments. When the UWB signal is blocked or there is multipath interference, the system relies on IMU integration to generate a large cumulative drift. It lacks a prediction and compensation mechanism for the future motion state of the target. Furthermore, this study is not linked with dynamic local path planning algorithms, so it cannot maintain continuous following in complex environments (such as temporary loss of positioning).

[0005] Ji et al. proposed an improved CKF method to adapt to state estimation problems in nonlinear environments; Eang et al. used deep neural networks to assist EKF in modeling observation noise and achieving adaptive adjustment of ranging errors. Kwon et al.'s UWB-based autonomous following system is a typical example of UWB application in dynamic target following. This system uses UWB to measure the relative position of personnel and robots, and combines it with an obstacle detection module to achieve obstacle avoidance and path adjustment through rule-based logic. This scheme is mainly aimed at polar and complex terrain environments, verifying the reliability and robustness of UWB positioning in occluded scenarios, and providing basic support for human-robot collaborative walking. However, the obstacle avoidance strategy used in this method cannot perform real-time path optimization in dynamically changing environments, and it is not combined with local path planning algorithms, making it difficult to balance target orientation, obstacle avoidance safety, and path smoothness. Furthermore, this system uses a wheeled robot as the experimental platform and does not consider the kinematic constraints and posture control problems of quadrupedal robot platforms, thus limiting its application to quadrupedal robots with complex gait characteristics.

[0006] However, the above technical solutions are mostly focused on improving the accuracy at the positioning level, and have not yet formed a complete closed-loop system of "fusion positioning → target prediction → dynamic local path planning → control execution".

[0007] Especially when UWB signals are obstructed or briefly lost, the lack of an effective target state prediction and compensation mechanism can lead to discontinuities in the system's positioning results, thus affecting autonomous following and real-time obstacle avoidance planning. Therefore, it is necessary to construct a prediction and planning closed-loop system that can maintain continuity even when perception information is incomplete.

[0008] It is evident that the existing technology suffers from the following key technical problems: (1) The problem of UWB signal being easily blocked, causing the target location to be interrupted: When the UWB signal is blocked or briefly lost, the robot cannot obtain continuous position input from the target due to the lack of an effective state prediction mechanism. This leads to problems such as interruption of the following path, incorrect robot orientation, or sudden stop in traditional following systems. The lack of an effective short-term prediction and compensation mechanism makes it impossible to maintain the continuity of following.

[0009] (2) Local Path Planning (DWA) cannot adapt to dynamic target following scenarios: Traditional DWA is only suitable for static or weakly dynamic obstacle environments, cannot handle real-time tracking errors of dynamically moving targets, and has fixed planning weights that cannot adaptively adjust with changes in obstacle density or target speed.

[0010] (3) Lack of a unified fusion framework among multi-source sensor information Existing technologies often achieve this through "modular segmentation": independent ranging for UWB, independent integration for IMU, independent matching for LiDAR, and independent planning for DWA. The lack of a unified state model to link these data leads to problems such as insufficient information utilization, low module coupling, and inability to effectively propagate and correct errors within the system. Summary of the Invention

[0011] In view of the above problems, the present invention provides a multi-source fusion following obstacle avoidance system with target signal loss prediction compensation to overcome or at least partially solve the above problems.

[0012] This invention provides the following solution: A multi-source fusion following obstacle avoidance system with target signal loss prediction compensation includes: The perception layer includes a UWB positioning module, a lidar module, and an IMU module. The UWB positioning module, the lidar module, and the IMU module are respectively used to collect target relative position information, surrounding environment point cloud data, and robot posture data. The decision layer receives target relative position information, surrounding environment point cloud data, and robot posture data sent by the perception layer. It employs extended Kalman filtering to achieve multimodal fusion and combines a personnel state prediction model with a local path planning module to complete dynamic local path planning. The personnel state prediction model includes global position estimation of the following personnel and short-time extrapolation prediction using a constant velocity model when UWB signal occlusion is determined. The local path planning module generates optimal speed commands based on an improved dynamic window algorithm. The control layer is used to convert the optimal path speed command into a quadruped motion control signal through inverse kinematics and PID algorithm, and realize the robot's autonomous motion through torque feedforward and PID hybrid control. An execution layer is used to drive the robot to perform forward and / or turning and / or stopping and / or obstacle avoidance actions, and to provide feedback on the execution results. The improved dynamic window algorithm includes a dynamic weight adjustment mechanism and a scoring model that integrates prediction information. The dynamic weight adjustment mechanism is used to adjust the weight of the scoring function in real time according to the predicted position of the person and the density of obstacles, so that the system can automatically balance the weights of target orientation, obstacle avoidance and speed efficiency in different scenarios. The scoring model that integrates prediction information is used to introduce the predicted output position of the constant speed model into the target item of the dynamic window algorithm to improve the path continuity in the case of short-term target loss.

[0013] Preferably, the UWB positioning module includes four UWB anchor points mounted on the robot body and a UWB tag worn by the target personnel; the lidar module is used to provide 360° two-dimensional planar scanning point cloud data and output 360° scanning point cloud data to establish an obstacle set; the IMU module is used to measure the three-axis angular velocity and linear acceleration of the robot body, and obtain the attitude and velocity changes through integration.

[0014] Preferably, multimodal fusion is achieved using extended Kalman filtering, including: Construct a global coordinate system from the robot's starting point The robot's online velocity and heading angle change rate are obtained by integrating the angular velocity and linear acceleration of the IMU, and used as the driving input for EKF state prediction; the lidar obtains the robot's absolute position and heading angle in the global coordinate system through a point cloud matching algorithm, which serves as the observation input for EKF. By incorporating the robot's linear velocity components, heading angle, and heading angle change rate into the state vector, an EKF fusion structure suitable for dynamic following scenarios is constructed, enabling joint estimation of the robot's global position, velocity, and attitude change rate.

[0015] Preferably, the global position estimation of the follower includes: The robot's pose estimation by fusing EKF output Transform into a homogeneous transformation matrix ;

[0016] The coordinate vector of the target in the global coordinate system is then:

[0017] In the formula: Indicates the heading angle. and This represents the robot's position in the global coordinate system.

[0018] Preferably, the short-time extrapolation prediction using a constant-rate model includes: After obtaining the continuous position estimate of the target in the global coordinate system, the target's motion direction and instantaneous velocity are constructed by the position difference between two adjacent frames; After obtaining the target's current direction of motion and instantaneous velocity, a uniform motion model is used to predict its future position. The predicted position of the target at the next moment is represented as follows:

[0019] In the formula: Indicates instantaneous velocity. Indicates the direction of movement. It represents the time difference between time t and time t-1.

[0020] Preferably, the scoring function is expressed by the following formula:

[0021] In the formula: For target-oriented terms, it indicates how close the trajectory is to the predicted location of the person; For obstacle avoidance, it indicates the safety of the minimum distance from the obstacle; This is the speed term, reflecting the efficiency of the robot's movement speed; This is a weighting coefficient used to balance the three indicators.

[0022] Preferably: all candidate velocity pairs are scored within the velocity sample space, and the optimal velocity command pair with the highest comprehensive score is selected as the control output using the following formula:

[0023] In the formula, Indicates the first Group velocity samples, The overall score representing the trajectory. Indicates taking such that The highest speed group, This indicates the optimal speed command.

[0024] Preferably, the quadrupedal motion control signal includes the relative speed of each leg's foot and the angle of each joint on the machine body.

[0025] Preferably, the relative velocity of the foot of each leg on the fuselage should be calculated by the following formula:

[0026] In the formula: For the first The velocity vector of one leg point, Let be the position vector of the foot relative to the fuselage. This indicates the optimal speed command.

[0027] Preferably, the angles of each joint are calculated using the following formula:

[0028]

[0029]

[0030] In the formula: Indicates the first The spatial distance from the foot to the hip of a leg. Indicates the length of the thigh and calf. These represent the rotation angles of the hip, knee, and ankle joints, respectively. They represent the first The positional components of the foot of a leg relative to the hip joint in the front-to-back and vertical directions in the fuselage coordinate system.

[0031] Preferably, the control rate of the control layer is expressed by the following formula:

[0032] In the formula: These represent the proportional coefficient, integral coefficient, and differential coefficient, respectively. This represents the error between the expected joint angle and the actual joint angle. Represented as a time infinitesimal element used for integration and differentiation calculations. It represents the amount of error that changes over time.

[0033] According to specific embodiments provided by the present invention, the present invention discloses the following technical effects: This invention provides a multi-source fusion following obstacle avoidance system with target signal loss prediction compensation. When the UWB signal is occluded, weakened, or briefly lost, the system automatically activates target state prediction based on a constant velocity (CV) model to achieve short-term continuous extrapolation of the target position. This mechanism effectively avoids the problems of following interruption, path jump, or robot stagnation caused by measurement interruption in traditional systems, improving the continuity and stability of the overall following task, and significantly enhancing the continuity of the localization and following link.

[0034] Based on the traditional DWA algorithm, target prediction position is added and a dynamic weight adjustment mechanism is adopted, which enables the planner to automatically adjust the scoring function according to the target change trend and environmental density, making the output speed more stable and the trajectory more natural. It can adapt to the scenario of dynamic target moving quickly, significantly improving the jitter phenomenon of traditional DWA in dynamic scenarios; the planned trajectory is smoother, more stable and has a higher response speed.

[0035] The global pose and obstacle point cloud provided by the lidar are simultaneously used in EKF fusion and DWA obstacle avoidance calculation, enabling real-time detection, judgment, and avoidance of obstacles in dynamic environments. Compared to traditional single-sensor-based solutions, this invention maintains higher safety in complex environments with dynamic obstacles; obstacle avoidance safety is improved, and environmental adaptability is stronger.

[0036] By integrating multimodal data from UWB, IMU, and LiDAR, employing a target state prediction and compensation mechanism, and implementing a closed-loop collaboration of perception, decision-making, control, and execution, this invention effectively mitigates the impact of single-sensor failure, measurement noise, and target signal loss, enabling the system to maintain high precision and reliability in complex indoor and outdoor scenarios; the overall robustness of the system is significantly enhanced.

[0037] Of course, any product implementing this invention does not necessarily need to achieve all of the advantages described above at the same time. Attached Figure Description

[0038] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly described below. Obviously, the drawings described below are merely some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without any creative effort.

[0039] Figure 1 This is a flowchart illustrating the workflow of a multi-source fusion following obstacle avoidance system with target signal loss prediction compensation provided in an embodiment of the present invention. Figure 2 This is a schematic diagram illustrating the effect of the local path planning algorithm provided in an embodiment of the present invention. Detailed Implementation

[0040] 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 a part of the embodiments of the present invention, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention are within the scope of protection of the present invention.

[0041] See Figure 1 This invention provides a multi-source fusion following obstacle avoidance system with target signal loss prediction compensation, as shown in the embodiments of the present invention. Figure 1 As shown, the system may include: The perception layer includes a UWB positioning module, a LiDAR module, and an IMU module. These modules are used to collect target relative position information, surrounding environment point cloud data, and robot posture data, respectively. Specifically, in this embodiment, the UWB positioning module may include four UWB anchor points mounted on the robot body and a UWB tag worn by the target person. The LiDAR module provides 360° two-dimensional planar scanning point cloud data and outputs this data to establish an obstacle set. The IMU module measures the robot's three-axis angular velocity and linear acceleration, and obtains posture and velocity changes through integration.

[0042] The decision layer receives target relative position information, surrounding environment point cloud data, and robot posture data sent by the perception layer. It employs extended Kalman filtering to achieve multimodal fusion and combines a personnel state prediction model with a local path planning module to complete dynamic local path planning. The personnel state prediction model includes global position estimation of the following personnel and short-time extrapolation prediction using a constant velocity model when UWB signal occlusion is determined. The local path planning module generates optimal speed commands based on an improved dynamic window algorithm. Specifically, embodiments of the present invention can provide multimodal fusion using extended Kalman filtering, including: Construct a global coordinate system from the robot's starting point The robot's online velocity and heading angle change rate are obtained by integrating the angular velocity and linear acceleration of the IMU, and used as the driving input for EKF state prediction; the lidar obtains the robot's absolute position and heading angle in the global coordinate system through a point cloud matching algorithm, which serves as the observation input for EKF. By incorporating the robot's linear velocity components, heading angle, and heading angle change rate into the state vector, an EKF fusion structure suitable for dynamic following scenarios is constructed, enabling joint estimation of the robot's global position, velocity, and attitude change rate.

[0043] The global position estimation of the follower includes: The robot's pose estimation by fusing EKF output Transform into a homogeneous transformation matrix ;

[0044] The coordinate vector of the target in the global coordinate system is then:

[0045] In the formula: Indicates the heading angle. and This represents the robot's position in the global coordinate system.

[0046] The short-time extrapolation prediction using a constant-rate model includes: After obtaining the continuous position estimate of the target in the global coordinate system, the target's motion direction and instantaneous velocity are constructed by the position difference between two adjacent frames; After obtaining the target's current direction of motion and instantaneous velocity, a uniform motion model is used to predict its future position. The predicted position of the target at the next moment is represented as follows:

[0047] In the formula: Indicates instantaneous velocity. Indicates the direction of movement. It represents the time difference between time t and time t-1.

[0048] The scoring function is expressed by the following formula:

[0049] In the formula: For target-oriented terms, it indicates how close the trajectory is to the predicted location of the person; For obstacle avoidance, it indicates the safety of the minimum distance from the obstacle; This is the speed term, reflecting the efficiency of the robot's movement speed; This is a weighting coefficient used to balance the three indicators.

[0050] All candidate velocity pairs are scored within the velocity sample space, and the optimal velocity command pair with the highest comprehensive score is selected as the control output using the following formula:

[0051] In the formula, Indicates the first Group velocity samples, The overall score representing the trajectory. Indicates taking such that The highest speed group, This indicates the optimal speed command.

[0052] The control layer is used to convert the optimal path speed command into a quadruped motion control signal through inverse kinematics and PID algorithm, and realize the robot's autonomous motion through torque feedforward and PID hybrid control. In specific implementation, the embodiments of the present invention can provide the quadruped motion control signal including the relative velocity of the foot end of each leg on the robot body and the angle of each joint.

[0053] The relative velocity that the foot of each leg on the fuselage should possess is calculated by the following formula:

[0054] In the formula: For the first The velocity vector of one leg point, Let be the position vector of the foot relative to the fuselage. This indicates the optimal speed command.

[0055] The angles of each joint are calculated using the following formula:

[0056]

[0057]

[0058] In the formula: Indicates the first The spatial distance from the foot to the hip of a leg. Indicates the length of the thigh and calf. These represent the rotation angles of the hip, knee, and ankle joints, respectively. They represent the first The positional components of the foot of a leg relative to the hip joint in the front-to-back and vertical directions in the fuselage coordinate system.

[0059] The control rate of the control layer is expressed by the following formula:

[0060] In the formula: These represent the proportional coefficient, integral coefficient, and differential coefficient, respectively. This represents the error between the expected joint angle and the actual joint angle. Represented as a time infinitesimal element used for integration and differentiation calculations. It represents the amount of error that changes over time.

[0061] An execution layer is used to drive the robot to perform forward and / or turning and / or stopping and / or obstacle avoidance actions, and to provide feedback on the execution results. The improved dynamic window algorithm includes a dynamic weight adjustment mechanism and a scoring model that integrates prediction information. The dynamic weight adjustment mechanism is used to adjust the weight of the scoring function in real time according to the predicted position of the person and the density of obstacles, so that the system can automatically balance the weights of target orientation, obstacle avoidance and speed efficiency in different scenarios. The scoring model that integrates prediction information is used to introduce the predicted output position of the constant speed model into the target item of the dynamic window algorithm to improve the path continuity in the case of short-term target loss.

[0062] The multi-source fusion following obstacle avoidance system with target signal loss prediction compensation provided in this invention constructs a multi-modal fusion framework of UWB, LiDAR, and IMU, introduces a target state prediction mechanism, and combines it with an improved dynamic local path planning method. This enables the robot to achieve continuous, stable, and reliable target following capabilities even in scenarios with intermittent perception information and dynamic environmental changes. Furthermore, through the integrated design of multi-source fusion localization, prediction compensation, and planning control, real-time following and obstacle avoidance control of the robot in complex environments is achieved, significantly improving the system's robustness and continuity.

[0063] The system employs a continuous state estimation method that adaptively switches between the EKF prediction model, which integrates UWB, IMU, and LiDAR, and the target constant velocity (CV) prediction model. This method is used to maintain the continuity of the target position and robot state when UWB signals are blocked or lost for a short period of time, thus solving the problem that existing solutions are prone to path jumps or follow-up interruptions when observation is interrupted.

[0064] An improved DWA comprehensive scoring mechanism that integrates target prediction information introduces target prediction position as a guiding term on the basis of traditional DWA, and dynamically adjusts the weights of target, obstacle avoidance and speed terms in combination with environmental obstacle density. This scoring mechanism generates the optimal speed pair that adapts to dynamic target movement and environmental changes, achieving a smoother and more stable following trajectory.

[0065] The integrated closed-loop system of multimodal fusion, prediction compensation, path planning, and control execution constructs a complete closed-loop architecture from sensing (UWB / IMU / LiDAR) → fusion localization (EKF) → prediction compensation (CV) → dynamic path planning (improved DWA) → motion control (inverse kinematics + PID) → execution feedback. This enables multi-sensor data, prediction results, and control commands to work collaboratively in the same link, improving the long-term stability and environmental adaptability of the system in complex indoor and outdoor environments.

[0066] The robust following mechanism under UWB signal loss and abnormal scenarios adopts a state prediction compensation method based on target motion trend analysis. This enables the system to maintain continuous estimation of the target's global position even when UWB ranging is lost due to occlusion or weak signals, and ensures uninterrupted output of robot following commands, thereby significantly improving the system's robustness in complex environments.

[0067] The following is a detailed description of the multi-source fusion following obstacle avoidance system with target signal loss prediction compensation provided in the embodiments of the present invention.

[0068] This invention proposes a multi-source fusion following obstacle avoidance system with target signal loss prediction compensation. It uses UWB, IMU, and LiDAR as core sensing units and employs extended Kalman filtering (EKF) for multimodal data fusion. When the UWB signal is blocked, a constant velocity model (CV) is used to predict the future position of the person, ensuring continuous state estimation. The decision layer generates optimal speed commands based on the improved dynamic window method (DWA), the control layer converts the speed commands into joint angles using inverse kinematics and PID algorithms, and the execution layer drives a quadrupedal servo system to complete the action, thus forming a closed-loop system of "perception-decision-control-execution" to achieve continuous and smooth autonomous following and dynamic obstacle avoidance.

[0069] The overall system architecture of this invention consists of a perception layer, a decision-making layer, and a control execution layer, and its workflow is as follows: ① The perception layer collects raw data from UWB, IMU, and LiDAR, and performs time synchronization and smoothing processing to form structured input. The relative position of the target person in the robot's local coordinate system is obtained through the four-base station UWB ranging equation, which is used for subsequent global coordinate transformation and target state prediction; ② The decision-making layer first uses EKF to fuse high-frequency motion prediction information from IMU with global pose observation data from LiDAR to achieve continuous and stable global state estimation of the robot. This state is then used to convert the UWB output into the target's position in the global coordinate system. When UWB ranging loss, anomalies, or signal quality degradation occur, a constant velocity (CV) model is automatically activated. Based on the target's global position in the two most recent frames, the target's direction and velocity are calculated. The target's position at the next moment is predicted using the uniform velocity model, generating temporary target coordinates to ensure continuous tracking even when the target is temporarily unobservable. Then, the improved DWA path planning module receives target position information from the EKF or CV model, and constructs an obstacle set by combining the LiDAR point cloud. It generates velocity combinations in the velocity sampling space, calculates a comprehensive score by introducing a target prediction term and a dynamic weight adjustment mechanism, and selects the velocity pair with the highest score as the optimal motion command for the next moment. ③ The control layer calculates the expected speed of each foot based on the optimal speed, then uses inverse kinematics to obtain the corresponding joint angle, and uses PID control law to perform closed-loop adjustment of the joint angle to generate smooth and reliable robot leg execution commands. ④ The execution layer drives the servo actuator to complete forward, backward, turning and obstacle avoidance actions according to the joint drive commands output by the control layer, and sends the IMU attitude and joint status feedback information back to the control layer in real time, realizing the closed-loop collaborative operation of perception-fusion-planning-control-execution.

[0070] System workflow as follows Figure 1 As shown.

[0071] 1. Perception Layer: Composed of UWB positioning module, LiDAR module and IMU module, used to collect target relative position information, surrounding environment point cloud data and robot posture data.

[0072] UWB positioning module: Consists of four UWB anchor points mounted on the robot body and a UWB tag worn by the target personnel. The UWB system measures the distance between the tag and the robot using two-way time-of-flight (TOF). Its positioning principle is based on measuring the time it takes for the radio frequency signal to travel from the tag to the anchor point and back. The distance calculation formula for this method is as follows:

[0073] in, This is expressed as the distance between the tag and the base station. The speed of signal propagation is approximated by the speed of light in a vacuum. Indicates the time when the tag receives the signal. Indicates the time when the tag sends a signal. Indicates the moment when the anchor point replies with a signal. The time when the anchor point receives the signal is indicated by the time of the signal. Subtracting these values ​​gives the time taken for the tag to travel to and from the anchor point, and thus the distance.

[0074] Radar module: Provides 360° two-dimensional planar scanning point cloud data and outputs 360° scanning point cloud data for building an obstacle set. The point cloud data is represented as follows:

[0075] in, Indicates the first The point in the radar coordinate system coordinates This indicates the radar scanning distance threshold.

[0076] IMU module: measures the three-axis angular velocity and linear acceleration of the robot body, and obtains the attitude and velocity changes through integration.

[0077] 2. Decision-making level: Multimodal fusion is achieved by using extended Kalman filter (EKF), and dynamic local path planning is completed by combining personnel state prediction (CV model) and local path planning module (improved DWA).

[0078] ① Multi-sensor fusion algorithm.

[0079] UWB multi-anchor point positioning to follow the target: The positioning system uses airborne anchor points, which are fixedly installed on the robot body to establish a local coordinate system with the robot's center. The distance between the label position and the anchor point position is expressed as:

[0080] in, Indicates the number of the plotted point. Indicates anchor point The distance from the label, the anchor point coordinates are The location of the tag to be located is .

[0081] To ensure the solvability and robustness of the positioning, this system employs four-base station UWB ranging. The four base stations provide sufficient geometric constraints, ensuring a unique positional solution and maintaining high positioning stability even when a single base station is obstructed or the ranging is abnormal. Therefore, the relative position of the tag in the robot coordinate system can be obtained by solving the polygonal positioning equations. .

[0082] EKF fusion model: Construct a global coordinate system from the robot's starting point The robot's linear velocity and heading angle rate of change are obtained by integrating the angular velocity and linear acceleration from the IMU, and used as the driving input for EKF state prediction. The LiDAR obtains the robot's absolute position and heading angle in the global coordinate system through a point cloud matching algorithm, which serves as the observation input for EKF. By incorporating the robot's linear velocity components, heading angle, and heading angle rate of change into the state vector, an EKF fusion structure suitable for dynamic following scenarios is constructed to achieve joint estimation of the robot's global position, velocity, and attitude rate of change. The system state vector is defined as follows:

[0083] in, and This represents the robot's position in the global coordinate system. and For velocity components, Indicates the heading angle. Angular velocity represents the heading angle.

[0084] Assuming the robot is in the sampling period The internal motion is smooth, and according to the kinematic model, it changes from the state at the previous time step. Calculate the current predicted state The system prediction equation is then:

[0085] in, It is process noise, used to describe the uncertainty caused by model approximation or external disturbances.

[0086] LiDAR can directly obtain the robot's position and heading angle in the global coordinate system through point cloud matching algorithms. Its observation vector is:

[0087] in, , For robot position observation, This is for heading angle observation. The observation function is:

[0088] Therefore, the observation equation is:

[0089] in, To observe noise, lidar can directly measure position and heading angle, so velocity and angular velocity are not included in the observation.

[0090] The Kalman gain is:

[0091] Status updated to:

[0092] The final state estimate after fusion is obtained:

[0093] EKF employs a "prediction-update" mechanism to fuse high-frequency motion information from the IMU with global pose observations from the LiDAR, achieving continuous and accurate robot pose estimation. It also converts the relative coordinates provided by UWB into the target's position in the global coordinate system, providing reliable input for subsequent planning and control. In practical applications, EKF can be replaced with CKFUKF to improve nonlinear estimation accuracy; for higher real-time performance, a simplified complementary filtering scheme can be used.

[0094] ②Personnel status prediction algorithm (constant velocity model CV).

[0095] Global position estimation of follower personnel: In UWB multi-anchor positioning, the obtained label position is located in the robot's local coordinate system. The following only represents the target's relative position with respect to the robot's center. To use this information for global coordinate calculations, the relative coordinates need to be transformed to the global coordinate system. .

[0096] The robot's pose estimation by fusing EKF output Transform into a homogeneous transformation matrix .

[0097]

[0098] The coordinate vector of the target in the global coordinate system is then:

[0099] Constant velocity model: To ensure continuous tracking of the system when the UWB signal is blocked or lost, a constant velocity model is introduced for short-time extrapolation prediction.

[0100] After obtaining continuous position estimates of the target in the global coordinate system, the target's motion direction is constructed using the position difference between two adjacent frames. The global positions of the target at times t and t-1 are respectively... and The displacement vector is then defined as:

[0101] Normalize the displacement vector to obtain the direction of movement:

[0102] The instantaneous velocity of the target is calculated by the position difference between two adjacent frames. :

[0103] Obtain the target's current direction of motion. With speed Then, assuming the target's speed remains constant over a short time interval, a uniform motion model is used to predict its future position. The predicted position of the target at the next moment can then be expressed as:

[0104] When the UWB signal is restored, the system automatically switches back to UWB positioning output, achieving a seamless transition. This mechanism effectively avoids path interruptions caused by signal blockage, allowing the robot to maintain its following direction towards the target person.

[0105] ③ Local path planning algorithm (improved DWA), such as Figure 2 As shown.

[0106] To achieve dynamic obstacle avoidance and smooth following of robots in complex environments, this invention improves upon the traditional Dynamic Window Algorithm (DWA). The main modifications are as follows: Dynamic weight adjustment mechanism: The system adjusts the weight of the scoring function in real time based on the predicted location of the personnel and the density of obstacles, so that the system can automatically balance the weights of "goal orientation - obstacle avoidance - speed efficiency" in different scenarios; A scoring model that integrates predictive information: The position of the CV prediction output. Introducing DWA's target items improves path continuity in cases of short-term target loss.

[0107] The specific calculations are as follows: The system uses the robot's linear velocity... With angular velocity To define the planning variables, a feasible velocity sampling space is defined:

[0108] Each pair of velocity samples The short-time trajectory is as follows:

[0109] in, This is the current position of the robot. The current facing angle. To control the cycle.

[0110] Comprehensive scoring function:

[0111] in, For target-oriented terms, it indicates how close the trajectory is to the predicted location of the person; For obstacle avoidance, it indicates the safety of the minimum distance from the obstacle; This is the speed term, reflecting the efficiency of the robot's movement speed; This is a weighting coefficient used to balance the three indicators.

[0112] The system scores all candidate velocity pairs within the velocity sample space and ultimately selects the velocity pair with the highest comprehensive score as the control output using the following formula:

[0113] in, To indicate the first Group velocity samples, The overall score for the trajectory. Indicates taking such that The highest speed group, This determines the optimal speed command. Through dynamic weight adjustment and personnel prediction fusion, stable following and real-time obstacle avoidance are achieved in complex scenarios.

[0114] Control and execution layers: The optimal path speed command is converted into a quadruped motion control signal, and smooth autonomous motion is achieved through torque feedforward and PID hybrid control.

[0115] Perform actions such as forward movement, turning, stopping, and obstacle avoidance, and provide feedback on the results.

[0116] ① Kinematic relationship: The decision-making level provides the robot with the optimal speed pair in the ground coordinate system. At that time, the relative speed that the feet of each leg on the control layer computer should possess. Its basic relation is:

[0117] in, For the first The velocity vector of the foot point. Let be the position vector of the foot relative to the fuselage.

[0118] ② Solving for joint angles: The robot's single leg consists of two connecting rods, with lengths of respectively (Thigh segment) and (Lower leg segment), then the angles of each joint can be obtained by the following inverse kinematic formulas:

[0119]

[0120]

[0121] in, This represents the spatial distance from the foot to the hip (i.e., the vector magnitude). It indicates the length of the thigh and calf. These refer to the rotation angles of the hip, knee, and ankle joints, respectively.

[0122] ③ Control Law: To ensure that the actual joint angle accurately tracks the desired angle, the control layer employs a proportional-integral-derivative (PID) controller, with the following control law:

[0123] Closed-loop adjustment of joint angle errors enables smooth movement and rapid gait control. The execution layer receives angle commands from the integrated control board to drive the servo actuators to complete the movements and sends sensor feedback back to the control layer to form a closed-loop correction, thereby enabling the robot to walk stably and follow autonomously in complex terrain.

[0124] In practical applications, the CV model can be replaced with CA (constant acceleration) or LSTM neural network prediction models to enhance adaptability to complex human movements. Improved DWA can also be replaced with TEB (Timed Elastic Band) or MPC (Model Predictive Control) methods for local path planning. If the target platform is a wheeled robot, a differential motion control model can be directly used, eliminating the need for quadruped inverse kinematics calculations.

[0125] As can be seen, existing tracking systems typically interrupt target localization directly when UWB ranging signals are obstructed or lost, leading to path jumps and control discontinuities. This invention introduces a constant velocity (CV) prediction model into the state link. Upon detecting UWB data anomalies, it automatically switches to prediction mode, calculating the target's direction of movement and velocity based on the target's position in the two most recent frames. This generates a short-term predicted position to replace the actual ranging input, ensuring continuous output of the target's state even when it is unobservable. This guarantees uninterrupted tracking and significantly improves the system's robustness to signal interruptions.

[0126] Traditional Direct Weighing and Control (DWA) struggles to handle dynamically moving targets, and its fixed scoring weights cannot adaptively adjust to changes in obstacle density or target state. This invention proposes a planning mechanism of "prediction fusion + dynamic weight adjustment." It introduces the predicted target position as a planning objective into the scoring model and dynamically adjusts the weights of the objective, obstacle avoidance, and speed components based on environmental density, motion trends, and other state variables. This achieves an adaptive balance between path smoothness, obstacle avoidance safety, and follow-response speed, effectively improving the continuity and stability of the planning process.

[0127] This invention constructs a unified decision-making link consisting of UWB-IMU-LiDAR fusion localization (EKF), CV prediction compensation, improved DWA planning, and inverse kinematics control, enabling target localization, state prediction, path planning, and motion control to operate collaboratively within a single framework. This structure solves the problems of information fragmentation between multiple sensor modules and the difficulty in achieving real-time coupling between the planning and localization layers in existing technologies, enabling the system to achieve higher following stability, environmental adaptability, and control continuity in complex dynamic scenarios.

[0128] In summary, the multi-source fusion following obstacle avoidance system with target signal loss prediction compensation provided by this invention automatically activates target state prediction based on a constant velocity (CV) model when the UWB signal is occluded, weakened, or briefly lost, achieving short-term continuous extrapolation of the target position. This mechanism effectively avoids the problems of following interruption, path jump, or robot stagnation caused by measurement interruption in traditional systems, improving the continuity and stability of the overall following task, and significantly enhancing the continuity of the localization and following link.

[0129] Based on the traditional DWA algorithm, target prediction position is added and a dynamic weight adjustment mechanism is adopted, which enables the planner to automatically adjust the scoring function according to the target change trend and environmental density, making the output speed more stable and the trajectory more natural. It can adapt to the scenario of dynamic target moving quickly, significantly improving the jitter phenomenon of traditional DWA in dynamic scenarios; the planned trajectory is smoother, more stable and has a higher response speed.

[0130] The global pose and obstacle point cloud provided by the lidar are simultaneously used in EKF fusion and DWA obstacle avoidance calculation, enabling real-time detection, judgment, and avoidance of obstacles in dynamic environments. Compared to traditional single-sensor-based solutions, this invention maintains higher safety in complex environments with dynamic obstacles; obstacle avoidance safety is improved, and environmental adaptability is stronger.

[0131] By integrating multimodal data from UWB, IMU, and LiDAR, employing a target state prediction and compensation mechanism, and implementing a closed-loop collaboration of perception, decision-making, control, and execution, this invention effectively mitigates the impact of single-sensor failure, measurement noise, and target signal loss, enabling the system to maintain high precision and reliability in complex indoor and outdoor scenarios; the overall robustness of the system is significantly enhanced.

[0132] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0133] As can be seen from the above description of the embodiments, those skilled in the art can clearly understand that the present invention can be implemented by means of software plus necessary general-purpose hardware platforms. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in various embodiments or some parts of the embodiments of the present invention.

[0134] The various embodiments in this specification are described in a progressive manner. Similar or identical parts between embodiments can be referred to mutually. Each embodiment focuses on describing the differences from other embodiments. In particular, for system or system embodiments, since they are basically similar to method embodiments, the description is relatively simple, and relevant parts can be referred to the descriptions in the method embodiments. The systems and system embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without creative effort.

[0135] The above description is merely a preferred embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention are included within the scope of protection of the present invention.

Claims

1. A multi-source fusion following obstacle avoidance system with target signal loss prediction compensation, characterized in that, include: The perception layer includes a UWB positioning module, a lidar module, and an IMU module. The UWB positioning module, the lidar module, and the IMU module are respectively used to collect target relative position information, surrounding environment point cloud data, and robot posture data. The decision layer receives the target relative position information, the surrounding environment point cloud data, and the robot posture data sent by the perception layer. It uses extended Kalman filtering to achieve multimodal fusion and combines a personnel state prediction model with a local path planning module to complete dynamic local path planning. The personnel state prediction model includes global position estimation of the following personnel and short-time extrapolation prediction using a constant velocity model when UWB signal occlusion is determined. The local path planning module includes generating optimal speed instructions based on an improved dynamic window algorithm; The control layer is used to convert the optimal path speed command into a quadruped motion control signal through inverse kinematics and PID algorithm, and realize the robot's autonomous motion through torque feedforward and PID hybrid control. An execution layer is used to drive the robot to perform forward and / or turning and / or stopping and / or obstacle avoidance actions, and to provide feedback on the execution results. The improved dynamic window algorithm includes a dynamic weight adjustment mechanism and a scoring model that integrates prediction information. The dynamic weight adjustment mechanism is used to adjust the weight of the scoring function in real time according to the predicted position of the person and the density of obstacles, so that the system can automatically balance the weights of target orientation, obstacle avoidance and speed efficiency in different scenarios. The scoring model that integrates prediction information is used to introduce the predicted output position of the constant speed model into the target item of the dynamic window algorithm to improve the path continuity in the case of short-term target loss.

2. The multi-source fusion following obstacle avoidance system with target signal loss prediction compensation according to claim 1, characterized in that, The UWB positioning module includes four UWB anchor points mounted on the robot body and a UWB tag worn by the target personnel; the lidar module is used to provide 360° two-dimensional planar scanning point cloud data and output 360° scanning point cloud data to establish an obstacle set; the IMU module is used to measure the three-axis angular velocity and linear acceleration of the robot body, and obtain the attitude and velocity changes through integration.

3. The multi-source fusion following obstacle avoidance system with target signal loss prediction compensation according to claim 1, characterized in that, Multimodal fusion using extended Kalman filtering includes: Construct a global coordinate system from the robot's starting point The robot's online velocity and heading angle change rate are obtained by integrating the angular velocity and linear acceleration of the IMU, and are used as the driving input for EKF state prediction; the lidar obtains the robot's absolute position and heading angle in the global coordinate system through a point cloud matching algorithm, and is used as the observation input for EKF. By incorporating the robot's linear velocity components, heading angle, and heading angle change rate into the state vector, an EKF fusion structure suitable for dynamic following scenarios is constructed, enabling joint estimation of the robot's global position, velocity, and attitude change rate.

4. The multi-source fusion following obstacle avoidance system with target signal loss prediction compensation according to claim 1, characterized in that, The global position estimation of the follower includes: The robot's pose estimation by fusing EKF output Transform into a homogeneous transformation matrix ; The coordinate vector of the target in the global coordinate system is then: In the formula: Indicates the heading angle. and This represents the robot's position in the global coordinate system.

5. The multi-source fusion following obstacle avoidance system with target signal loss prediction compensation according to claim 1, characterized in that, The short-time extrapolation prediction using a constant-rate model includes: After obtaining the continuous position estimate of the target in the global coordinate system, the target's motion direction and instantaneous velocity are constructed by the position difference between two adjacent frames; After obtaining the target's current direction of motion and instantaneous velocity, a uniform motion model is used to predict its future position. The predicted position of the target at the next moment is represented as follows: In the formula: Indicates instantaneous velocity. Indicates the direction of movement. It represents the time difference between time t and time t-1.

6. The multi-source fusion following obstacle avoidance system with target signal loss prediction compensation according to claim 1, characterized in that, The scoring function is expressed by the following formula: In the formula: For target-oriented terms, it indicates how close the trajectory is to the predicted location of the person; For obstacle avoidance, it indicates the safety of the minimum distance from the obstacle; This is the speed term, reflecting the efficiency of the robot's movement speed; This is a weighting coefficient used to balance the three indicators.

7. The multi-source fusion following obstacle avoidance system with target signal loss prediction compensation according to claim 1, characterized in that, All candidate velocity pairs are scored within the velocity sample space, and the optimal velocity command pair with the highest comprehensive score is selected as the control output using the following formula: In the formula, Indicates the first Group velocity samples, The overall score representing the trajectory. Indicates taking such that The highest speed group, This indicates the optimal speed command.

8. The multi-source fusion following obstacle avoidance system with target signal loss prediction compensation according to claim 7, characterized in that, The quadrupedal motion control signals include the relative speed of each leg's foot and the angles of each joint on the fuselage.

9. The multi-source fusion following obstacle avoidance system with target signal loss prediction compensation according to claim 8, characterized in that, The relative velocity that the foot of each leg on the fuselage should possess is calculated by the following formula: In the formula: For the first The velocity vector of one leg point, Let be the position vector of the foot relative to the fuselage. Indicates the optimal speed command; The angles of each joint are calculated using the following formula: In the formula: Indicates the first The spatial distance from the foot to the hip of a leg. Indicates the length of the thigh and calf. These represent the rotation angles of the hip, knee, and ankle joints, respectively. They represent the first The positional components of the foot of a leg relative to the hip joint in the front-to-back and vertical directions in the fuselage coordinate system.

10. The multi-source fusion following obstacle avoidance system with target signal loss prediction compensation according to claim 1, characterized in that, The control rate of the control layer is expressed by the following formula: In the formula: These represent the proportional coefficient, integral coefficient, and differential coefficient, respectively. This represents the error between the expected joint angle and the actual joint angle. Represented as a time infinitesimal element used for integration and differentiation calculations. It represents the amount of error that changes over time.

Images