Multi-sensor fusion low-altitude energy complementing robot navigation and obstacle avoidance system

Abstract

The invention relates to the technical field of energy complementing robot navigation and automatic control, and particularly discloses a multi-sensor fusion low-altitude energy complementing robot navigation and obstacle avoidance system which comprises a collaborative sensing and mapping module, a path constraint planning module, a self-adaptive deep reinforcement learning module and an execution module. The collaborative perception and mapping module fuses real-time data of a multi-source heterogeneous sensor and constructs an environment model containing geometric, motion and semantic information of an obstacle; the path constraint planning module generates a path with optimal energy consumption and lowest risk by a fast random exploration tree algorithm based on an energy state and an environment map, and divides the path into staged waypoints; the adaptive deep reinforcement learning module outputs an obstacle avoidance action instruction through a DQN network in combination with a multi-target reward function; the execution module converts the instruction into an execution mechanism control signal, drives the robot to move and monitors the state in a closed-loop mode. The system realizes stable sensing in a complex environment through multi-sensing redundancy and adaptive fusion, and realizes intelligent navigation and accurate obstacle avoidance.

Description

Technical Field

[0001] This invention relates to the field of navigation and automatic control technology for power replenishment robots, specifically a low-altitude power replenishment robot navigation and obstacle avoidance system that integrates multiple sensors. Background Technology

[0002] With the widespread application of drone technology in logistics, inspection, surveying and mapping, its endurance has become a key bottleneck restricting its effectiveness. Low-altitude refueling robots (or "aerial power banks") are receiving increasing attention as an effective means to solve this problem. However, the low-altitude environment is complex and changeable, with both static obstacles (such as buildings and trees) and dynamic obstacles (such as other aircraft), which places extremely high demands on the autonomous navigation and obstacle avoidance capabilities of refueling robots.

[0003] In the existing technology, the navigation and obstacle avoidance system of low-altitude power replenishment robot has many shortcomings: (1) From the perspective of environmental perception, traditional systems mostly rely on a single sensor (such as lidar or visual sensor). Although lidar has advantages in three-dimensional geometric perception, it is easily interfered with in strong light environment; visual sensor can provide rich texture information, but its performance is seriously degraded under fog, dust and low light conditions; millimeter wave radar is highly adaptable to severe weather, but its resolution is low; IMU (inertial measurement unit) can provide motion state information, but it has accumulated errors. The limitations of a single sensor lead to insufficient perception robustness of the system in complex low-altitude environment, making it difficult to continuously and accurately obtain the geometric information, motion state and semantic attributes of obstacles; and a simple multi-sensor data fusion method, such as fixed weight fusion, is used. This method does not consider the performance changes of sensors under different working conditions and cannot dynamically adjust the weight of each sensor data according to the real-time environment. For example, in a specific environment, the performance of some sensors degrades, but fixed weight fusion still gives them a large weight, resulting in inaccurate fusion results and affecting navigation and obstacle avoidance decisions. Moreover, traditional fusion methods have limited ability to handle uncertainties in sensor data, making it difficult to effectively quantify data reliability, which in turn affects the accuracy of the optimal estimation results.

[0004] (2) Regarding path planning, existing solutions often aim for the "shortest path" or "shortest time," without fully considering the energy status of the refueling robot itself. Low-altitude refueling robots have limited energy capacity. If path planning only pursues the optimal distance or time, the robot may be unable to refuel or return safely due to power depletion during the task, seriously affecting the reliability and continuity of the operation. At the same time, existing path planning does not quantify environmental risks precisely enough, making it difficult to effectively distinguish the threat level of static and dynamic obstacles, resulting in insufficient safety of the planned path.

[0005] (3) Traditional navigation and obstacle avoidance systems mostly make decisions based on preset rules. In the face of complex and ever-changing low-altitude environments, the rule base is difficult to cover all possible situations. When encountering unforeseen obstacles or environmental changes, the system cannot flexibly adjust its decisions, resulting in obstacle avoidance failure or unreasonable path selection. Summary of the Invention

[0006] The purpose of this invention is to provide a multi-sensor fusion-based low-altitude power replenishment robot navigation and obstacle avoidance system. This system aims to achieve robust perception in complex environments through multi-source sensor information fusion and semantic understanding; improve the reliability and safety of task execution by introducing energy-constrained path planning; and achieve precise navigation, dynamic obstacle avoidance, and mode switching in three-dimensional space through adaptive deep reinforcement learning control.

[0007] The objective of this invention can be achieved through the following technical solution: a multi-sensor fusion low-altitude power replenishment robot navigation and obstacle avoidance system, comprising: The collaborative perception and mapping module dynamically evaluates and integrates real-time data from multiple heterogeneous sensors to construct a real-time, robust multimodal environment model that includes precise geometric information, motion state, and semantic labels of obstacles. The collaborative perception and mapping module is equipped with a multi-source heterogeneous sensor unit, an adaptive weight fusion unit, and a semantic map construction unit. The multi-source heterogeneous sensor unit integrates solid-state lidar, millimeter-wave radar, vision sensors, and an IMU (inertial measurement unit) to collect real-time obstacle motion data in the aerial flight environment where the low-altitude power replenishment robot operates. This includes the pose and velocity information of each obstacle, where i represents the obstacle number, i=1,2,...,n, and n represents the total number of obstacle numbers; The adaptive weight fusion unit is used to evaluate the working condition of each sensor in real time and assign weights. It outputs robust and accurate optimal estimated pose information and optimal estimated velocity information of each obstacle through weighted averaging. The semantic map construction unit is used to perform semantic segmentation on the fused point cloud and image using a deep learning model to construct a multimodal semantic map containing traversable areas, static obstacles, dynamic obstacles, and energy replenishment target points. The path constraint planning module is used to generate the optimal macroscopic path and phased waypoints based on the real-time energy status and environmental map of the energy replenishment robot. The adaptive deep reinforcement learning module is used to output obstacle avoidance action commands for the power replenishment robot based on the current stage waypoint, real-time environment map and robot state, using the deep reinforcement learning model DQN network. The execution module converts the obstacle avoidance commands of the power replenishment robot into control signals, drives the power replenishment robot to complete the obstacle avoidance actions, and performs the next round of obstacle avoidance analysis based on the execution results of the obstacle avoidance actions.

[0008] The beneficial effects of this invention are: This invention employs a complementary redundancy design of multi-source heterogeneous sensors (solid-state LiDAR, millimeter-wave radar, visual sensors, and IMU) to ensure stable perception even in complex environments such as fog, dust, and strong light. For example, when the confidence level of the visual sensor decreases under strong light, the weights of the LiDAR and millimeter-wave radar automatically increase to maintain perception continuity. IMU confidence level calculation based on Extended Kalman Filter (EKF) quantifies the uncertainty of sensor data in real time, providing a precise basis for adaptive weight fusion and effectively reducing the optimal estimation error of obstacle pose and velocity. A deep learning model is used to perform semantic segmentation on the fused point cloud and image, constructing a real-time environmental map with semantic information. This not only provides a foundation for subsequent intelligent navigation and obstacle avoidance decisions but also ensures that the output environmental model includes obstacle geometry, motion state, and semantic attributes, making it more comprehensive and practical than traditional maps.

[0009] This invention uses a real-time energy status and environmental map based on a refueling robot to generate multiple candidate paths through a path search algorithm. Then, it calculates the comprehensive cost of each candidate path using a path cost function to generate the optimal macroscopic path and stage waypoints. This path planning method that considers energy factors helps improve the robot's endurance and ensures that it can successfully reach the target refueling point.

[0010] This invention employs a deep reinforcement learning model, the DQN network, to output obstacle avoidance commands based on the current stage waypoints, real-time environmental map, and robot state. By defining rich state vectors and action space vectors, the model can comprehensively perceive environmental information and make diverse obstacle avoidance action decisions. A multi-objective reward function is set, including navigation, obstacle avoidance, energy, and semantic reward functions. This multi-objective reward function design comprehensively considers multiple factors, enabling the robot to not only prioritize safety during obstacle avoidance but also consider navigation objectives, energy efficiency, and semantic environmental adaptability, thus improving overall performance. The Q-value is updated using the SARSA formula, and the action space vector with the largest Q-value is found as the output to obtain the obstacle avoidance command for the power replenishment robot. This action selection method based on Q-value updates allows the robot to gradually improve the accuracy and effectiveness of obstacle avoidance decisions through continuous learning and optimization. Attached Figure Description

[0011] The invention will now be further described with reference to the accompanying drawings.

[0012] Figure 1 This is a system block diagram of the present invention.

[0013] Figure 2 This is a logical schematic diagram of the present invention for calculating the confidence level of the IMU at the current acquisition time based on the extended Kalman filter (EKF).

[0014] Figure 3 This is a logical diagram illustrating the autonomous obstacle avoidance decision-making of the power replenishment robot of the present invention. Detailed Implementation

[0015] 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, and 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.

[0016] Please see Figure 1 As shown, the present invention is a navigation and obstacle avoidance system for a low-altitude power replenishment robot with multi-sensor fusion, including a collaborative perception and mapping module, a path constraint planning module, an adaptive deep reinforcement learning module, and an execution module.

[0017] The collaborative perception and mapping module is used to dynamically evaluate and fuse real-time data from multiple heterogeneous sensors to construct a real-time, robust multimodal environment model that includes precise geometric information, motion state, and semantic labels of obstacles. Specifically: The collaborative perception and mapping module is equipped with a multi-source heterogeneous sensor unit, an adaptive weight fusion unit, and a semantic map construction unit.

[0018] The multi-source heterogeneous sensor unit integrates solid-state lidar, millimeter-wave radar, vision sensors, and an IMU (inertial measurement unit) to collect real-time obstacle motion data in the aerial flight environment where the low-altitude power replenishment robot operates. ,in, This represents obstacle motion data collected by a solid-state lidar. This represents obstacle motion data collected by millimeter-wave radar. This represents the obstacle motion data collected by the vision sensor. This represents the obstacle motion data collected by the IMU. The obstacle motion data includes the pose and velocity information of each obstacle, where i represents the obstacle number, i=1,2,...,n, and n represents the total number of obstacle numbers.

[0019] The adaptive weight fusion unit is used to evaluate the working condition of each sensor in real time and assign weights. It outputs robust and accurate optimal estimated pose and optimal estimated velocity information for each obstacle through weighted averaging. Specifically: The point cloud density and average reflection intensity of the lidar at the current acquisition time are obtained, and the confidence level of the lidar at the current acquisition time is generated by weighted calculation. It should be noted that point cloud density refers to the number of effective point clouds per unit solid angle or unit area. The higher the density, the better the detection effect and the higher the confidence level of the lidar. Average reflection intensity refers to the average intensity of the signal returned by the point cloud. If the intensity is within the normal range, it indicates that the propagation medium is good and the lidar confidence level is higher.

[0020] To obtain the signal-to-noise ratio (SNR) of the millimeter-wave radar at the current acquisition moment, map the confidence level of the millimeter-wave radar at the current acquisition moment using the following formula: , This represents an exponential function with base e. This represents the signal-to-noise ratio (SNR) threshold, and k represents a preset slope factor, controlling the steepness of the function around the threshold. It's important to note that SNR refers to the ratio of signal power to noise power. A higher SNR indicates more reliable target detection and a lower false alarm rate. When the SNR is significantly higher than the threshold... When the SNR is close to 1, and far below the threshold, Approaching 0; Acquire the image from the vision sensor at the current acquisition moment, convert it to a grayscale image, and obtain the image brightness at the current acquisition moment. Contrast and clarity Simultaneously, it extracts reasonable brightness ranges, reasonable contrast ranges, and reasonable sharpness thresholds from images acquired by the power replenishment robot in high-quality conditions stored in the database. Extract the upper and lower limits of the reasonable brightness range, calculate the average value, and generate a reasonable reference brightness. The absolute difference between the reasonable reference brightness and the upper limit of the reasonable brightness range is calculated to generate a reasonable deviation brightness threshold. Similarly, a reasonable reference contrast is generated based on a reasonable contrast range. and reasonable deviation contrast threshold A multi-dimensional decision model is used to measure the confidence level of the visual sensor, thus obtaining the confidence level of the visual sensor at the current acquisition time. The multi-dimensional decision-making model is as follows: It should be noted that the image brightness, contrast, and sharpness, as well as their corresponding thresholds, in the model have all been standardized, their dimensions extracted, and their values ​​input into the multi-dimensional decision model to output confidence scores. The higher the confidence score of the visual sensor, the closer the image's brightness, contrast, and sharpness are to a high-quality state, and the higher the reliability of the visual sensor's acquisition results.

[0021] It should be noted that the process of acquiring the image brightness, contrast and sharpness at the current acquisition time by the above visual sensor is as follows: the gradient magnitude and gradient direction of each pixel in the image at the current acquisition time are calculated using the Sobel operator, and the weighted average gradient magnitude of the image at the current acquisition time is calculated according to the preset weights of each gradient direction interval, which is used as the image sharpness at the current acquisition time.

[0022] Obtain the grayscale value of each pixel in the image at the current acquisition time. Based on the preset grayscale value range corresponding to each grayscale level, construct the grayscale histogram of the image at the current acquisition time with grayscale value as the horizontal axis and number of pixels as the vertical axis. Obtain the number of grayscale levels involved in the image at the current acquisition time. Filter the maximum and minimum pixel grayscale values ​​in the image at the current acquisition time. Use the ratio of the difference between the two to the number of grayscale levels as the image contrast at the current acquisition time.

[0023] Calculate the average pixel grayscale value of the image at the current acquisition time and use it as the image brightness at the current acquisition time.

[0024] Acquire the three-axis acceleration data of the IMU at the current acquisition time. and angular velocity data of three axes The data are acquired by the accelerometer and gyroscope in the IMU, respectively, and the state vector of the IMU is defined. ; like Figure 2 As shown, the confidence level of the IMU at the current acquisition time is calculated based on the extended Kalman filter (EKF), including: State-space model construction: A state-space model is constructed to describe the IMU. The state-space model consists of state equations and observation equations. The state equations describe the evolution of the system, and the formula is: ,in For state vectors, To control the input, The process noise (follows a Gaussian distribution with mean 0 and covariance Q) is used; the observation equation describes the relationship between the system's output and state, and is given by: ,in For the observation vector, The observation noise (follows a Gaussian distribution with mean 0 and covariance R); Initialize EKF parameters: Initialize state estimates and initializing the covariance matrix and process noise covariance Observation noise covariance ; Prediction: Using nonlinear state equations, the optimal state is estimated based on the previous time step. Calculate the prior state estimate at the current time. The formula is: Process noise is ignored here. Predicting the state solely through a system model; for nonlinear state equations exist Performing a first-order Taylor expansion at the given location yields the Jacobian matrix. (Linearized form of the state transition matrix), calculate the prior state covariance at the current time using the Jacobian matrix. The formula is: ,in , represents the partial derivative matrix of the state equation with respect to the state vector; it should be noted that the goal of the prediction stage is to infer the current state from the previous state based on the system's motion laws, and to estimate the uncertainty of the prediction. Update: Using nonlinear observation equations, based on the prior state estimation at the current time... Calculate the predicted observations The formula is: Process noise is ignored here. Predicting observations solely through observation models; for nonlinear observation equations exist Performing a first-order Taylor expansion at the given location yields the Jacobian matrix. (Linearized form of the observation matrix), calculate the covariance of the observation prediction error respectively. cross-covariance of observations The formula is ,in , represents the partial derivative matrix of the observation equation with respect to the state vector; calculate the Kalman gain based on the observation covariance and cross-covariance. The formula used to weigh the confidence levels of prior state estimates and observations is: It should be noted that a larger Kalman gain indicates a stronger correction effect of the observed values ​​on the state estimate, and vice versa; the smaller the Kalman gain, the greater the weight of the prior estimate. This should be considered in conjunction with the actual observed values. The error between the predicted observations and the prior state estimate is corrected using Kalman gain to obtain the optimal state estimate at the current time. The formula is: ,in The observation residual reflects the difference between the predicted and actual observations. Based on the Kalman gain and prior covariance, the optimal state covariance matrix at the current time is updated, as shown in the formula: , where I is the identity matrix, ensuring the positive definiteness and symmetry of the covariance matrix; it should be noted that the goal of the update stage is to fuse the predicted state and the actual observed information to obtain a more accurate state estimate and correct for uncertainties; Calculate the confidence level: based on the optimal state covariance matrix at the current time. Quantify the confidence level of each state and extract diagonal elements (This represents the variance of the j-th state, reflecting the uncertainty of that state). The variance is converted to a normalized value using a formula. The confidence level of an interval is given by the formula: It should be noted that, if Approaching 0 (almost no error) Approaching 1 (completely reliable), if Increase (error becomes larger) Reduce (confidence decreases); combine the confidence levels of each IMU state to obtain the overall confidence level. The formula is: .

[0025] It should be noted that the IMU confidence calculation based on EKF involves tracking the uncertainty (covariance) of the IMU data in real time through a "prediction-update" loop, and then converting the uncertainty into a confidence score of [0, 1]. The smaller the uncertainty, the higher the confidence score. This process can accurately reflect the reliability of the IMU under different operating conditions, providing a key basis for subsequent multi-sensor adaptive weight fusion, and ensuring that the low-altitude power replenishment robot can still stably output the optimal motion parameters of obstacles in complex environments.

[0026] Based on the confidence level of the multi-source heterogeneous sensors at the current acquisition time, a weighted algorithm is used. The weight of the lidar at the current acquisition time is calculated. Weight of millimeter-wave radar at the current acquisition time The weight of the visual sensor at the current acquisition moment and the weight of the IMU at the current acquisition time .

[0027] It should be noted that when the performance of any sensor degrades, its impact can be automatically reduced, and the most reliable result can always be output under the current environment. For example, under strong light, the weight of vision is reduced, but the weight of laser and millimeter-wave radar is increased.

[0028] Based on real-time obstacle motion data collected by solid-state lidar, millimeter-wave radar, visual sensors, and IMU from the aerial environment in which the low-altitude power replenishment robot operates, and corresponding real-time weighted data, a weighted fusion model is used. The optimal estimated motion data for each obstacle after fusion were calculated. Including optimal estimated pose information and optimal estimated speed information ,in These represent the x-axis coordinates, y-axis coordinates, Z-axis coordinates, roll angle, pitch angle, and yaw angle of the obstacle collected at time t, respectively. These represent the x-axis velocity component, y-axis velocity component, and Z-axis velocity component of the obstacle collected at time t, respectively.

[0029] The semantic map construction unit utilizes a deep learning model to perform semantic segmentation on the fused point cloud and image, constructing a multimodal semantic map that includes traversable areas, static obstacles, dynamic obstacles, and power replenishment target points. Specifically, it uses the point cloud segmentation model PointNet++ and the image segmentation model SegNet to perform semantic segmentation on the fused point cloud and image from multiple heterogeneous sensors, identifying and labeling semantic tags. These semantic tags include, but are not limited to, traversable open areas, static obstacles, dynamic obstacles, and power replenishment target points. Ultimately, it constructs a real-time environmental map with semantic information, providing a foundation for subsequent intelligent decision-making.

[0030] It should be further explained that this invention uses multi-source heterogeneous sensors to collect obstacle motion data in real time, solving the problem of single sensor failure in harsh environments and enabling stable perception even in complex environments such as fog, dust, and strong light. Simultaneously, it performs real-time confidence assessment and dynamic weight allocation for the multi-source heterogeneous sensors, accurately quantifying the uncertainty of each sensor's data and providing a precise basis for adaptive weight fusion, effectively reducing the optimal estimation error of obstacle pose and velocity. Furthermore, it utilizes a deep learning model to perform semantic segmentation on the fused point cloud and image, constructing a real-time environmental map with semantic information. This not only provides a foundation for subsequent intelligent navigation and obstacle avoidance decisions but also outputs an environmental model that includes obstacle geometry, motion state, and semantic attributes, making it more comprehensive and practical than traditional maps.

[0031] The path constraint planning module is used to generate the optimal macroscopic path and interim waypoints based on the real-time energy status and environmental map of the energy replenishment robot. Specifically: Obtain the coordinates of the power replenishment robot in the world coordinate system at the current time t. Attitude angle data Location speed data And the coordinates of the corresponding target energy replenishment point in the world coordinate system. ; Based on the current semantic map, a path search algorithm (fast random exploration tree algorithm) is used to generate multiple candidate paths from the current position of the power replenishment robot to the target power replenishment point; For each generated candidate path Get path length Path risk value and path-based energy consumption and the remaining power of the real-time replenishment robot The data is then standardized, its dimensions are removed, and its numerical value is obtained through the path cost function. Calculate the comprehensive cost of each candidate path And select the path with the lowest cost as the optimal execution path. These represent the weights corresponding to the set path length, path risk value, and energy consumption ratio, respectively.

[0032] It should be noted that the specific methods for obtaining the path risk value and the estimated path energy consumption are as follows: Each candidate path j is discretized into a series of dense path points at preset intervals. For each waypoint Centered on the point, a three-dimensional buffer zone (such as a sphere with a radius of 5 meters) is generated. The buffer zone represents the safe area that needs to be monitored when the robot passes through that point. For each path point The buffer extracts information on all obstacles falling into the area from the multimodal semantic map. Each obstacle information includes pose information, velocity information, and semantic label, resulting in an obstacle list for each waypoint. Based on the semantic labels of obstacles, they are matched with the basic risk values ​​corresponding to each set semantic label to obtain the basic risk value of the obstacle semantic label, which serves as the obstacle semantic risk factor; Based on the pose information of the obstacle and the position of the path point, obtain the distance from the obstacle to the path point; The obstacle semantic risk factor is multiplied by the distance risk factor to generate the comprehensive obstacle risk value. The maximum value of all obstacle risks in the buffer is taken as the risk value of the path point. Based on this, the risk values ​​of all path points of each candidate path are accumulated to obtain the path risk value of each candidate path.

[0033] It should be further explained that by analyzing the semantic information of the environment along the route, static and dynamic threats are intelligently quantified, and local, discrete risk points are aggregated into a global indicator that can represent the security level of the entire route.

[0034] For each candidate path, the robot is recharged at each path segment (from...). arrive The expected motion state of the robot is calculated, including displacement vector, path segment length, height change, and expected velocity. This is then matched with the reference energy consumption corresponding to each expected motion state to obtain the reference energy consumption of the robot for each path segment. The result of summing the reference energy consumption of all path segments of the robot for each candidate path is used as the path-estimated energy consumption of each candidate path.

[0035] The selected optimal execution path is divided according to a preset distance to obtain a series of dense stage waypoints. ,in This is the next waypoint from the current location. It is the ultimate goal of replenishing energy.

[0036] It should be noted that this invention is based on the real-time energy status and environmental map of the refueling robot. After generating multiple candidate paths through a path search algorithm, the comprehensive cost value of each candidate path is calculated using a path cost function to generate the optimal macroscopic path and stage waypoints. Based on energy factors, the path planning improves the robot's endurance and ensures that it can successfully reach the target refueling point.

[0037] The adaptive deep reinforcement learning module uses a deep reinforcement learning model, the DQN network, to output obstacle avoidance commands for the power replenishment robot based on the current waypoints, real-time environment map, and robot state. Figure 3 As shown, specifically: Obtain the next stage waypoint of the optimal execution path from the current position as the stage target waypoint, and extract the coordinates of the stage target waypoint. , Extract the optimal estimated pose information of each obstacle collected at time t. and optimal estimated speed information The current state vector is generated from the robot's pose and velocity data at time t. ; The current state vector is input into the deep reinforcement learning model DQN network, and the action space vector of the reinforcement learning model is defined as follows. ,in, The velocity increment represents the acceleration or deceleration required by the robot at time t relative to its current speed in order to avoid obstacles. A positive velocity increment indicates acceleration, and a negative velocity increment indicates deceleration. The increment represents the distance the robot needs to move forward or backward relative to the current direction at time t in order to avoid obstacles. A positive increment indicates forward movement, and a negative increment indicates backward movement. The value represents the turning increment, indicating the left or right turning range the power replenishment robot needs to make relative to the current direction in order to avoid obstacles at time t. A positive turning increment indicates that the power replenishment robot turns to the left, and a negative turning increment indicates that the power replenishment robot turns to the right. The height increment represents the range of elevation change required by the replenishing robot at time t to avoid obstacles relative to its current height. A positive height increment indicates an increase in elevation, while a negative height increment indicates a decrease in elevation. This represents the task mode and is a binary variable (0 or 1). This indicates the obstacle avoidance mode, meaning that the power replenishment robot needs to adjust its speed increment, forward / backward increment, turning increment, and height increment to avoid obstacles at the current time t. This indicates a non-obstacle avoidance mode, meaning the power replenishment robot does not need to adjust its speed, attitude angle, or height increments to avoid obstacles at the current time t.

[0038] The reinforcement learning model is configured with a multi-objective reward function, including a navigation reward function, an obstacle avoidance reward function, an energy reward function, and a semantic reward function. The navigation reward function determines whether the robot, under the obstacle avoidance action, moves to the next waypoint. If the robot moves to the next waypoint, a positive reward of r1 = 0.5 is given; if the robot does not move to the next waypoint, a negative reward of r1 = -1.0 is given. Therefore, the navigation reward function is... ; The obstacle avoidance reward function determines the distance between the energy replenishment robot and all obstacles during the current obstacle avoidance action. The formula is: If the distance between the replenishing robot and any obstacle is 0 (i.e., a collision occurs), a negative reward of r2 = -10.0 is given; if the distance between the replenishing robot and any obstacle is less than a preset first distance threshold, a negative reward of r2 = -10.0 is given. (Near-range warning) If the distance between the replenishing robot and any obstacle is greater than the preset second distance threshold, a negative reward of r2=-2.0 will be given. (Safe distance), then a positive reward of r2 = 0.3 is given, where the first distance threshold < the second distance threshold, i.e., the obstacle avoidance reward function is... ; The energy reward function compares the energy consumption of the replenishing robot during the obstacle avoidance maneuver with its historical average energy consumption. If the replenishing robot's energy consumption is lower than the historical average, a positive reward of r3 = 0.2 is given. If the replenishing robot's energy consumption is higher than twice the historical average, a negative reward of r3 = -0.5 is given. If the replenishing robot's energy consumption is between the historical average and twice the historical average, there is no reward or penalty, and r3 = 0. In other words, the energy reward function is... .

[0039] The semantic reward function determines the semantic label of the power replenishment robot under the obstacle avoidance action at the current moment. If the semantic label of the power replenishment robot's current position is "can traverse open ground", a reward of r4 = 0.1 is given; if the semantic label of the power replenishment robot's current position is "dynamic obstacle", a reward of r4 = -0.3 is given; if the semantic label of the power replenishment robot's current position is other, there is no reward or penalty, and r4 = 0. That is, the semantic reward function is... ; That is, the multi-objective reward function is ; The Q value is updated using the SARSA formula, which is: ,in, It's the learning rate, which controls the speed of updates; It is a discount factor that weighs the impact of current rewards against future rewards; It is the new state observed after the action is performed. It is the next action chosen in the new state. It is the maximum Q value of the action in the next state; It is the immediate reward that the energy replenishment robot receives after performing an action in a given state.

[0040] The action space vector with the largest Q value is found by using the Q-value update formula and used as the output, which is the obstacle avoidance action command of the power replenishment robot.

[0041] The execution module converts the obstacle avoidance commands of the power replenishment robot into control signals, drives the robot to complete obstacle avoidance actions, and performs the next round of obstacle avoidance analysis based on the execution results of the obstacle avoidance actions. Specifically: The system receives obstacle avoidance commands from the power replenishment robot and converts the incremental commands into specific control signals (such as PWM waves) for actuators such as motors, ESCs, and servos through the underlying controller of the power replenishment robot (such as a PID controller), thereby driving the power replenishment robot to accurately complete forward, turning, lifting and other actions. After executing the optimal obstacle avoidance action command, the pose information of the power replenishment robot is reacquired, and it is checked whether the power replenishment robot has reached the stage target waypoint. If so, the next stage waypoint is acquired as the stage target waypoint, and the obstacle avoidance action for the next moment is generated through the adaptive deep learning module.

[0042] It should be noted that this invention employs a deep reinforcement learning model, DQN network, which outputs obstacle avoidance action commands based on the current stage waypoints, real-time environmental map, and robot state. By defining rich state vectors and action space vectors, the model can comprehensively perceive environmental information and make diverse obstacle avoidance action decisions. A multi-objective reward function is set, including a navigation reward function, an obstacle avoidance reward function, an energy reward function, and a semantic reward function. This multi-objective reward function integrates multiple factors, enabling the robot to not only prioritize safety during obstacle avoidance but also consider navigation objectives, energy efficiency, and semantic environmental adaptability, thus improving overall performance. The Q-value is updated using the SARSA formula, and the action space vector with the largest Q-value is found as the output to obtain the obstacle avoidance action commands for the power replenishment robot. This action selection method based on Q-value updates allows the robot to gradually improve the accuracy and effectiveness of obstacle avoidance decisions through continuous learning and optimization.

[0043] The above description is merely an example and illustration of the structure of the present invention. Those skilled in the art can make various modifications or additions to the specific embodiments described, or use similar methods to replace them, as long as they do not deviate from the structure of the invention or exceed the scope defined in the claims, all of which should fall within the protection scope of the present invention.

Claims

1. A multi-sensor fusion navigation and obstacle avoidance system for a low-altitude power replenishment robot, characterized in that, include: The collaborative perception and mapping module dynamically evaluates and fuses real-time data from multiple heterogeneous sensors to construct a real-time, robust multimodal environment model that includes precise geometric information, motion state, and semantic labels of obstacles. The path constraint planning module generates the optimal macroscopic path and phased waypoints based on the real-time energy status and environmental map of the energy replenishment robot. The adaptive deep reinforcement learning module is used to output obstacle avoidance action commands for the power replenishment robot based on the current stage waypoint, real-time environment map and robot state, using the deep reinforcement learning model DQN network. The execution module converts the obstacle avoidance commands of the power replenishment robot into control signals, drives the power replenishment robot to complete the obstacle avoidance actions, and performs the next round of obstacle avoidance analysis based on the execution results of the obstacle avoidance actions.

2. The multi-sensor fusion low-altitude power replenishment robot navigation and obstacle avoidance system according to claim 1, characterized in that, The collaborative perception and mapping module is equipped with a multi-source heterogeneous sensor unit, an adaptive weight fusion unit, and a semantic map construction unit. The multi-source heterogeneous sensor unit collects obstacle motion data in the aerial flight environment where the low-altitude power replenishment robot is located in real time, including the pose and velocity information of each obstacle; The adaptive weight fusion unit is used to evaluate the working condition of each sensor in real time and assign weights, and outputs the optimal estimated pose information and optimal estimated velocity information of each obstacle through weighted averaging. The semantic map construction unit is used to perform semantic segmentation on the fused point cloud and image using a deep learning model to construct a multimodal semantic map containing traversable areas, static obstacles, dynamic obstacles, and energy replenishment target points.

3. The low-altitude power replenishment robot navigation and obstacle avoidance system based on multi-sensor fusion according to claim 2, characterized in that, The specific execution process of the adaptive weight fusion unit is as follows: The point cloud density and average reflection intensity of the lidar at the current acquisition time are obtained, and the confidence level of the lidar at the current acquisition time is generated by weighted calculation. Obtain the signal-to-noise ratio of the millimeter-wave radar at the current acquisition time, and map the confidence level of the millimeter-wave radar at the current acquisition time; The system acquires the image from the vision sensor at the current acquisition moment and converts it to grayscale. It then obtains the image's brightness, contrast, and sharpness at that moment. Simultaneously, it extracts reasonable brightness, contrast, and sharpness thresholds from the database of images acquired by the power replenishment robot under high-quality conditions. The upper and lower limits of the reasonable brightness range are averaged to generate a reasonable reference brightness. The absolute difference between the reasonable reference brightness and the upper limit of the reasonable brightness range is calculated to generate a reasonable deviation brightness threshold. Similarly, reasonable reference contrast and reasonable deviation contrast thresholds are generated based on the reasonable contrast range. Finally, a multi-dimensional decision model is used to measure the confidence level of the vision sensor at the current acquisition moment, yielding the confidence level of the vision sensor. Acquire the acceleration data of the three axes and the angular velocity data of the three axes of the IMU at the current acquisition time, and define the state vector of the IMU; The confidence level of the IMU at the current acquisition time is calculated based on the extended Kalman filter. Based on the confidence level of the multi-source heterogeneous sensors at the current acquisition time, a weighted algorithm is used. The weights of the lidar, millimeter-wave radar, visual sensor, and IMU at the current acquisition time are calculated. Based on the real-time acquisition of obstacle motion data and corresponding real-time weights in the aerial flight environment where the low-altitude power replenishment robot is located by solid-state lidar, millimeter-wave radar, vision sensors and IMU, the optimal estimated motion data for each obstacle is calculated by a weighted fusion model, including the optimal estimated pose information and the optimal estimated velocity information.

4. The multi-sensor fusion low-altitude power replenishment robot navigation and obstacle avoidance system according to claim 3, characterized in that, The calculation of the IMU's confidence level at the current acquisition time based on extended Kalman filtering includes: A state-space model is constructed to describe the IMU. The state-space model consists of state equations and observation equations. Initialize the state estimates, initialize the covariance matrix, and initialize the process noise covariance and observation noise covariance; Using the nonlinear state equation, the prior state estimate at the current time is calculated based on the optimal state estimate at the previous time step; a first-order Taylor expansion is performed on the nonlinear state equation at the optimal state estimate at the previous time step to obtain the Jacobian matrix, and the prior state covariance at the current time step is calculated using the Jacobian matrix. Using a nonlinear observation equation, the predicted observation value is calculated based on the prior state estimate at the current time. A first-order Taylor expansion of the nonlinear observation equation at the prior state estimate at the current time is performed to obtain the Jacobian matrix. The covariance of the observation prediction error and the cross-covariance of the observations are then calculated. The Kalman gain is calculated based on the observation covariance and cross-covariance. Combining the error between the actual and predicted observation values, the prior state estimate is corrected using the Kalman gain to obtain the optimal state estimate at the current time. Based on the Kalman gain and the prior covariance, the optimal state covariance matrix at the current time is updated. Based on the optimal state covariance matrix at the current moment, the confidence of each state is quantified, the diagonal elements of the optimal state covariance matrix are extracted, and the variance is converted into a confidence in the interval [0, 1] using a normalization formula; the overall confidence is obtained by combining the confidence of each state of the IMU.

5. The multi-sensor fusion low-altitude power replenishment robot navigation and obstacle avoidance system according to claim 1, characterized in that, The specific execution process of generating the optimal macroscopic path and stage waypoints is as follows: Obtain the current coordinates of the power replenishment robot in the world coordinate system, its attitude angle data, its velocity data, and the coordinates of its corresponding target power replenishment point in the world coordinate system; Based on the current semantic map, a path search algorithm is used to generate multiple candidate paths from the current location of the power replenishment robot to the target power replenishment point. For each generated candidate path, obtain the path length, path risk value, path estimated energy consumption, and real-time remaining power of the replenishing robot. Calculate the comprehensive cost value of each candidate path through the path cost function, and select the path with the lowest cost as the optimal execution path. The selected optimal execution path is divided according to a preset distance to obtain a series of dense stage waypoints.

6. The low-altitude power replenishment robot navigation and obstacle avoidance system based on multi-sensor fusion according to claim 1, characterized in that, The specific execution steps of the obstacle avoidance action command of the output power replenishment robot are as follows: Obtain the next stage waypoint of the optimal execution path at the current position as the stage target waypoint, and extract the coordinates of the stage target waypoint; Extract the optimal estimated pose and optimal estimated velocity information of each obstacle collected at time t, as well as the pose and velocity data of the power replenishment robot at time t, and generate the current state vector; The current state vector is input into the deep reinforcement learning model to define the action space vector of the reinforcement learning model; Set up a multi-objective reward function for the reinforcement learning model, including a navigation reward function, an obstacle avoidance reward function, an energy reward function, and a semantic reward function; Update the Q value using the SARSA formula; The action space vector with the largest Q value is found by using the Q-value update formula and used as the output, which is the obstacle avoidance action command of the power replenishment robot.

7. A multi-sensor fusion low-altitude power replenishment robot navigation and obstacle avoidance system according to claim 6, characterized in that, The navigation reward function determines whether the robot, under the obstacle avoidance action at the current moment, moves to the next stage waypoint. If the robot moves to the next stage waypoint, a positive reward of r1=0.5 is given; if the robot does not move to the next stage waypoint, a negative reward of r1=-1.0 is given. The obstacle avoidance reward function determines the distance between the power replenishment robot and all obstacles during the obstacle avoidance action at the current moment. If the distance between the power replenishment robot and any obstacle is 0, a negative reward r2=-10.0 is given. If the distance between the power replenishment robot and any obstacle is less than a preset first distance threshold, a negative reward r2=-2.0 is given. If the distance between the power replenishment robot and any obstacle is greater than a preset second distance threshold, a positive reward r2=0.3 is given. Wherein, the first distance threshold < the second distance threshold. The energy reward function judges the energy consumption of the replenishing robot under the obstacle avoidance action at the current moment and compares it with the historical average energy consumption. If the energy consumption of the replenishing robot is lower than the historical average energy consumption, a positive reward of r3=0.2 is given. If the energy consumption of the replenishing robot is higher than twice the historical average energy consumption, a negative reward of r3=-0.5 is given. If the energy consumption of the replenishing robot is between the historical average energy consumption and twice the historical average energy consumption, there is no reward or penalty, and r3=0. The semantic reward function determines the semantic label of the power replenishment robot under the obstacle avoidance action at the current moment. If the semantic label of the power replenishment robot's current position is traversable open ground, a reward of r4=0.1 is given. If the semantic label of the power replenishment robot's current position is dynamic obstacle, a reward of r4=-0.3 is given. If the semantic label of the power replenishment robot's current position is other, no reward or penalty is given, and r4=0 is given.

8. The low-altitude power replenishment robot navigation and obstacle avoidance system based on multi-sensor fusion according to claim 1, characterized in that, The specific process of the execution module is as follows: The system receives obstacle avoidance commands from the power replenishment robot and converts the incremental commands into specific control signals for the motor, ESC, and servo actuators through the underlying controller of the power replenishment robot, thereby driving the power replenishment robot to complete forward, turning, and lifting actions. After executing the optimal obstacle avoidance action command, the pose information of the power replenishment robot is reacquired, and it is checked whether the power replenishment robot has reached the stage target waypoint. If so, the next stage waypoint is acquired as the stage target waypoint, and the obstacle avoidance action for the next moment is generated through the adaptive deep learning module.

Images