Medical rescue unmanned aerial vehicle and robot dog cooperative linkage method

Abstract

The invention belongs to the technical field of path planning of unmanned equipment, and relates to a medical rescue unmanned aerial vehicle and robot dog cooperative linkage method, which comprises the following steps: integrating LiDAR point cloud, visual images, IMU inertial data and compensated UWB positioning data into a multi-modal data stream subjected to space-time alignment through time alignment and space alignment technologies; respectively extracting LiDAR and visual feature poses through parallel SLAM (Simultaneous Localization and Mapping) calculation, and carrying out graph optimization and joint optimization by utilizing a GTSAM library, so as to construct a globally optimized pose and a global semantic map; the path planning algorithm provides dynamic channel routing for the unmanned aerial vehicle and the robot dog based on a global semantic map, the feature fusion weight is adjusted by considering the environment illumination intensity and the point cloud density, the reverse iris control mechanism detects whether the robot dog enters a shielding area through a UWB signal intensity attenuation value, the unmanned aerial vehicle is switched to a UWB signal follower, and the UWB signal follower is switched to the unmanned aerial vehicle. And through combination with a Kalman filter, penetration positioning in a shielding area is realized, so that stable execution of a rescue task in a complex environment is ensured.

Description

Technical Field

[0001] This application belongs to the technical field of path planning for unmanned devices. More specifically, it relates to a collaborative linkage method for medical rescue drones and robotic dogs. Background Art

[0002] In emergency rescue scenarios, such as natural disasters like earthquakes, fires, floods, and man-made accidents, quickly and accurately locating and rescuing the injured is of utmost importance. Traditional manual rescue often faces complex, dangerous, and dynamically changing environments, which pose extremely high requirements for the safety of rescue personnel and rescue efficiency. For this reason, drones and robotic dogs, as advanced rescue tools, have been widely used in modern medical rescue due to their high mobility and environmental adaptability. Drones can quickly fly over obstacles for aerial reconnaissance and material delivery, while robotic dogs can flexibly move through complex terrains to perform ground search and transportation tasks.

[0003] Traditional single-modal SLAM (Simultaneous Localization and Mapping) technology has been widely used in drone navigation and positioning. Although single-modal SLAM performs excellently under specific conditions, its limitations in extreme environments are also very obvious. LiDAR SLAM has poor robustness in bad weather, while visual SLAM has weak performance in environments with drastic light changes or low light. Existing single-modal SLAM solutions are difficult to cope with complex and changing rescue environments, resulting in reduced positioning accuracy and map construction failure, thus affecting the execution of rescue tasks. Summary of the Invention

[0004] The present invention provides a collaborative linkage method for medical rescue drones and robotic dogs, aiming at the technical problem that the prior art is difficult to cope with complex and changing rescue environments, resulting in reduced positioning accuracy and map construction failure, thus affecting the execution of rescue tasks.

[0005] A collaborative linkage method for medical rescue drones and robotic dogs includes the following steps: Time alignment: Use the Precision Time Protocol to align the timestamps of LiDAR (Light Detection and Ranging) point clouds and visual images; use linear interpolation to perform time compensation on UWB (Ultra-Wideband) signals to match them with the visual frame rate; Spatial alignment: Use a calibration board joint calibration method to determine the external parameter matrix between LiDAR and multi-view vision, and register the LiDAR point cloud and the visual structured light recovery point cloud through the ICP (Iterative Closest Point) algorithm to eliminate the spatial offset between multiple sensors. Obtain the spatio-temporally aligned multi-modal data stream based on time alignment and space alignment, including LiDAR point cloud, visual image, IMU inertial data, and compensated UWB positioning data; Parallel SLAM calculation: Based on the LOAM algorithm (Lidar Odometry and Mapping), extract the LiDAR point cloud features to obtain the LiDAR pose; at the same time, use the ORB algorithm (Oriented FAST and Rotated BRIEF) to extract visual feature points and combine IMU pre-integration to obtain the visual pose; Graph optimization joint optimization: Construct a factor graph through the GTSAM (Georgia Tech Smoothing and Mapping) library, use the LiDAR and visual poses as nodes for joint optimization to obtain the globally optimized pose, and then obtain the global semantic map. During the joint optimization process, dynamically adjust the fusion weights of visual and LiDAR point cloud features according to the environmental light intensity and point cloud density; Dynamic channel routing: Based on the global semantic map, use a path planning algorithm to provide guidance for the coordinated actions of drones and robotic dogs, and introduce risk areas for dynamic adjustment during the guidance process; Reverse iris control: Based on the intensity attenuation value of the UWB signal, determine whether the robotic dog enters the shielding area. If it enters, the drone switches to a UWB signal follower, receives the UWB positioning packet transmitted backward by the robotic dog, and combines the Kalman filter to fuse UWB and IMU data to achieve penetration positioning within the shielding area.

[0006] Through time alignment and space alignment technologies, the present invention integrates LiDAR point cloud, visual image, IMU inertial data, and compensated UWB positioning data into a spatio-temporally aligned multi-modal data stream, solving the problems of sensor data asynchrony and spatial offset; through parallel SLAM calculation, extract LiDAR and visual feature poses respectively, and use the GTSAM library for graph optimization joint optimization, thereby constructing the globally optimized pose and global semantic map to cope with complex and changing rescue environments; the path planning algorithm provides dynamic channel routing for drones and robotic dogs based on the global semantic map, adjusts the feature fusion weight considering environmental light intensity and point cloud density to ensure positioning accuracy and map construction reliability; in addition, the reverse iris control mechanism detects whether the robotic dog enters the shielding area through the UWB signal intensity attenuation value, switches the drone to a UWB signal follower, and combines the Kalman filter to achieve penetration positioning within the shielding area, thus ensuring the stable execution of the rescue mission in complex environments.

[0007] Preferably, the LiDAR pose is obtained based on the following steps: Extract edge points and plane points from the LiDAR point cloud, where the edge points are the points in the point cloud with a curvature greater than the threshold ; the plane points are the points in the point cloud with a surface greater than the threshold ; the point cloud curvature is calculated as follows: ; In the formula: is the curvature of point i; represents the neighborhood point set of point i; represents the coordinates of point i in the LiDAR coordinate system; represents the coordinates of point j in the LiDAR coordinate system; Based on the edge points and plane points of the current frame and the previous frame, a non-linear optimization method is used to minimize the matching error of the feature points, and then the LiDAR pose is estimated. The optimization objective is: ; In the formula: represents the LiDAR pose transformation matrix; represents the edge points or plane points of the current frame; represents the edge points or plane points of the previous frame; represents the optimal pose transformation matrix obtained through optimization calculation; Multiply the global position of the previous frame by the transformation matrix of the current frame to obtain the LiDAR pose of the current frame in the global coordinate system.

[0008] Preferably, the specific steps to obtain the visual pose are as follows: Use the internal parameter matrix K and distortion coefficient D of the camera to perform distortion processing on each frame of the image; Based on the distorted image, use the ORB algorithm to extract the key feature points in the image, including FAST corner detection and generating binary descriptors based on the detected corners; According to the response value R of each key feature point, filter out the key feature points with a response value greater than the set threshold, where the response value is calculated through the pixel gray value difference in the corner neighborhood; Use the KNN algorithm to match the key feature points of the current frame and the previous frame, retain the matching points with the smallest Hamming distance, and use the random sample consensus algorithm to remove the wrong matching points; Calculate the fundamental matrix F through the matching points, and then use singular value decomposition to solve the initial visual pose; then use the IMU data to pre-integrate the initial visual pose to improve the accuracy and robustness of the pose estimation; Calculate the reprojection error of the feature points, and optimize the visual pose based on minimizing the reprojection error, and obtain the visual pose based on this.

[0009] Preferably, the graph optimization joint optimization includes the following steps: Factor graph construction: The LiDAR pose and the visual pose are used as nodes in the factor graph, and constraint factors are added. The constraint factors include pose constraint factors, IMU pre-integration factors, and loop closure detection factors. Among them, the pose constraint factor is the relative pose constraint between the LiDAR pose and the visual pose; the IMU pre-integration factor represents the motion constraint provided by the IMU data; the loop closure detection factor represents the global consistency constraint obtained through loop closure detection. Joint optimization and optimization objective: Use the GTSAM library to optimize the factor graph. The optimization objective function is as follows: ; In the formula: represents the optimization variables, including the LiDAR pose and the visual pose; represents the error function of the i-th constraint factor, which is the error between a certain constraint factor and the optimization variables; By minimizing the error function, the optimized LiDAR pose and visual pose are finally obtained; Dynamic weight allocation: Calculate the visual pose confidence based on the illumination intensity of the image and the number of feature points; calculate the LiDAR pose confidence based on the point cloud density and the number of LiDAR feature points; according to the calculated visual and LiDAR confidences, allocate weights to obtain the visual weight and the LiDAR weight, where the sum of the visual weight and the LiDAR weight is 1; Based on the obtained visual weight sum and LiDAR weight, weighted fusion of the LiDAR and visual poses is performed to obtain the fused pose, and based on this, the global semantic map is obtained.

[0010] Preferably, the dynamic channel routing includes the following steps: Use the ResNet-50 model to detect cracks, output the crack confidence of each detection area, and retain the crack detection results with a confidence greater than 0.95; Use an infrared thermal imaging camera to detect the temperature difference distribution, output the temperature information of each area, and obtain the distribution of high-temperature areas by calculating the temperature gradient field; Fuse the crack detection confidence and the temperature difference information through the Bayesian probability model to construct a dynamic risk field: ; In the formula: represents the crack confidence; represents at the point the dynamic risk probability value; represents the weight coefficient; represents the temperature difference, which is the difference between the temperature at area and the reference temperature ; k represents the adjustment coefficient of the Sigmoid function; represents area The temperature gradient at represents the maximum value of the temperature gradient; According to values, the area is divided into different risk levels. The higher the value, the higher the risk, and the lower the value, the lower the risk. An output rasterized dynamic risk probability map is generated; Based on the global semantic map, key nodes are extracted to form a set of nodes in the graph, and a topological graph is constructed. The weight of the node is related to the risk area, and the dynamic risk probability is used to represent the risk of the node. The weight of the edge is composed of the Euclidean distance and the risk coefficient; In the air layer, the adaptive RRT*-Connect path planning algorithm is used to search for paths on the topological graph. The sampling strategy preferentially expands the nodes in the low-risk area. The cost function of the path combines the path length and the risk value, and the weight is dynamically adjusted according to the remaining power. Finally, a set of safe paths for the air layer is obtained; The path planning on the ground layer integrates the dynamic window method and the risk field gradient descent method, and adjusts the speed space sampling through the risk gradient to obtain a set of local safe paths for the ground layer; Based on the safe paths of the air layer and the local safe paths of the ground layer, reinforcement learning is used for dynamic adjustment, the optimal mode is selected from the candidate paths, and the path tracking parameters are dynamically adjusted. Preferably, the specific steps of using the adaptive RRT*-Connect path planning algorithm to search for paths on the topological graph are as follows: Tree structure definition: The root node of the starting tree is the current position of the UAV; the target tree is the position of the target point; Parameter initialization: Set the maximum number of iterations, step size, and adaptive weight and ; Adaptive sampling strategy: Based on the dynamic risk probability map, the area is divided into low-risk areas and medium-high-risk areas; uniform sampling is used for low-risk areas; Gaussian sampling is used for high-risk areas, and the target point is used as the mean, and the standard deviation is 3.0 to generate normal distribution sampling points, guiding the search to extend in the direction of the target and reducing invalid branches; Nearest neighbor node selection: Calculate the improved cost-sensitive distance between nodes: ; In the formula: and represent two different nodes in the path; represents the path length weight; represents the path cost weight; represents the Euclidean distance; represents from to the path cost integral; Feasible path generation: Expand new nodes from the nearest neighbor node to the randomly sampled point and satisfy the kinematic constraints: ; In the formula: represents the newly expanded node; represents the nearest neighbor node; represents the randomly sampled point; represents the step size; If the path crosses the dangerous area, then discard this node; Weight adjustment: When the remaining power is less than the preset threshold, increase the path length weight; if the average risk entropy of the current path is greater than the preset threshold, then increase the risk weight; Path smoothing: Optimize the original path by gradient descent, and perform redundant node construction, delete collinear nodes, where deleting collinear nodes means that when the turning angle of three consecutive nodes is less than the preset threshold, then delete the middle node; Through continuous iteration, the optimized aerial layer path is obtained, and the length of the optimized path does not exceed 1.1 times the theoretical path.

[0011] Preferably, the specific steps of the ground layer path planning are as follows: Risk gradient field calculation: Calculate the gradient of the risk probability field through the discrete difference method, that is, the change rate of the risk field in the x and y directions, where the changes in the x and y directions are both calculated by the central difference method; Expand the dangerous area: Define the area exceeding the danger threshold as the red dangerous area, and use morphological dilation operation with the dilation radius set to 1.5m; Dynamic window generation: Generate the dynamic speed window of the robot dog according to the current speed and speed limit; Velocity discretization: Discretize the linear velocity and angular velocity; Risk gradient direction velocity adjustment: For each candidate velocity pair, calculate the average risk gradient at the trajectory point and correct the velocity based on the average risk gradient; Velocity constraint reprojection: Limit the corrected velocity within the range of the dynamic window to ensure that the velocity of the robot always meets the kinematic constraints; Trajectory prediction: For each corrected velocity pair, predict the trajectory within the next 0.5 seconds; and calculate the cost function for each velocity pair: ; In the formula: represents the overall cost of the given velocity pair ; represents the distance cost between the current position and the target position; represents the total risk existing on the path; represents the heading angle between the current position and the local target position; , and Represents the weight parameter, and the sum is 1; Static obstacle detection: If the predicted trajectory point enters the expanded red danger area, the velocity pair is removed; Dynamic obstacle avoidance: For moving obstacles, calculate the collision time with the obstacle. If the collision time is less than the preset threshold, the velocity pair is considered dangerous and the velocity pair is removed; Cost sorting: For the candidate velocity pairs that meet the constraints, sort them in ascending order according to the total cost, and select the top 5% with the lowest cost as the candidate velocity set, and add Gaussian noise to the optimal velocity pair. Based on this, the local safe path of the ground layer is obtained.

[0012] Preferably, the specific structure of the reinforcement learning is as follows: State vector: Establish a seven-dimensional state vector including path length, safety score, remaining power, risk entropy, communication delay, UAV altitude, and robot dog angular velocity; Action definition: Discrete actions: Include air dominance, ground dominance, and hybrid mode. The logic of the hybrid mode is that the air path is used for global navigation, and the ground path is adjusted locally, and the control weights of the air path and the ground path in the hybrid mode are adjusted based on the priority weight, where the priority weight is dynamically calculated based on the continuous action; Continuous action: Used to adjust the path tracking aggressiveness coefficient; Composite reward function: ; In the formula: Represents the current path length; Represents the risk entropy; Represents the coefficient; Represents the base of the natural logarithm; Represents the remaining power; Represents the initial power; Represents the weight coefficient of the energy consumption term; Represents the weight coefficient of the communication penalty term; Represents the communication delay.

[0013] Preferably, the reinforcement learning adopts the PPO training framework, where the network structure includes a high-level policy network and a low-level policy network. The input of the high-level policy network is a 7-dimensional state vector, which is processed through 3 fully connected layers, and the activation function adopts the ReLU activation function, and outputs the discrete action probability distribution; The input of the low-level policy network is the state vector and the one-hot encoding of the discrete action. Based on 4 fully connected layers, the activation function adopts Tanh, and outputs the mean and variance of the continuous action.

[0014] The beneficial effects of the present invention include: Through time alignment and space alignment technologies, the present invention integrates LiDAR point clouds, visual images, IMU inertial data, and compensated UWB positioning data into a multi-modal data stream that has been spatio-temporally aligned, solving the problems of sensor data asynchronization and spatial offset; through parallel SLAM calculations, LiDAR and visual feature poses are respectively extracted, and the GTSAM library is used for joint optimization of graph optimization to construct globally optimized poses and a global semantic map to cope with complex and changing rescue environments; the path planning algorithm provides dynamic channel routing for drones and legged robots based on the global semantic map, adjusts the feature fusion weight considering the environmental light intensity and point cloud density, and ensures the positioning accuracy and the reliability of map construction; in addition, the reverse iris control mechanism detects whether the legged robot enters the shielding area through the UWB signal strength attenuation value, switches the drone to be a UWB signal follower, and combines with a Kalman filter to achieve penetration positioning in the shielding area, thus ensuring the stable execution of the rescue mission in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following will briefly introduce the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings in the following description are only some embodiments of the present application. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0016] Figure 1 It is the overall step block diagram provided by the embodiment of the present invention.

[0017] Figure 2 It is the specific step block diagram of the parallel SLAM calculation provided by the embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0018] In order to make the technical problems, technical solutions, and beneficial effects to be solved by the present application more clearly understood, the following further details the present application in conjunction with the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0019] See Figure 1 As shown, a method for collaborative linkage between a medical rescue drone and a legged robot includes the following steps: Time alignment: Align the timestamps of the LiDAR point cloud and the visual image using the Precision Time Protocol; perform time compensation on the UWB signal using linear interpolation to match it with the visual frame rate; Exemplarily, select the hardware of the visual processing unit (VPU) as the clock reference, that is, the master clock; the LiDAR, UWB, and IMU modules are aligned with the master clock through PTP packets; Sensor data tagging alignment: For each LiDAR frame Match the nearest visual frame , satisfying ; High-frequency IMU data (200Hz) is resampled according to the main clock timestamp to generate a 30Hz data stream aligned with the video frame; UWB signal linear interpolation compensation: For the time window and between adjacent UWB signals , perform linear interpolation according to the visual frame time : ; In the formula: represents the estimated value of the UWB signal at the visual frame time obtained by linear interpolation calculation.

[0020] Spatial alignment: Adopt the calibration board joint calibration method to determine the external parameter matrix between LiDAR and multi-view vision. Through the ICP algorithm, register the LiDAR point cloud and the visual structured light restored point cloud to eliminate the spatial offset between multiple sensors. Exemplarily, a 3D stereo calibration board is used) checkerboard + reflective markers; Place the calibration board in the collaborative working space of the drone and the robot dog, and synchronously collect LiDAR point clouds, multi-view vision images, and UWB positioning data (a total of 20 groups of different pose data are collected); LiDAR feature extraction: Extract the 3D corner coordinates of the reflective marker points on the calibration board; Visual features: Extract the 2D pixel coordinates of the checkerboard through findChessboardCorners of OpenCV; UWB reference: Calculate the installation offset of the UWB tag by inversely calculating the known position of the calibration board; External parameter matrix calculation: Solve the rotation matrix and the translation vector through the PnP algorithm, and the optimization objective is to minimize the reprojection error: ; In the formula: represents the camera projection model, and the Levenberg-Marquardt algorithm is used for iterative optimization; represents the 3D angular coordinates; represents the 2D pixel coordinates; represents the rotation matrix; represents the translation vector; Convert the UWB positioning data to the LiDAR coordinate system: ; In the formula: Represents the original UWB positioning data; Represents the UWB positioning data converted to the LiDAR coordinate system; Represents the rotation matrix from the LiDAR coordinate system to the visual coordinate system; Represents the translation vector from the LiDAR coordinate system to the visual coordinate system; ICP point cloud registration: Use VoxelGrid filtering for the LiARD point cloud; and apply statistical outlier removal to the visual point cloud; Extract the fast point feature histogram of the point cloud, and estimate the initial transformation matrix through the RANSAC algorithm; After the initial alignment, the average distance error of the point cloud is less than 15 cm; Furthermore, in this embodiment, ICP iterative optimization can be adopted. By minimizing the mean square error as the loss function, the transformation matrix is iteratively optimized; How to perform iterative optimization is a conventional technical means in this field, so it will not be elaborated here until the iteration times or the preset change rate threshold is reached.

[0021] Based on time alignment and space alignment, a multi-modal data stream after spatio-temporal alignment is obtained, including LiDAR point cloud, visual image, IMU inertial data, and compensated UWB positioning data; See Figure 2 As shown, parallel SLAM calculation: Based on the LOAM algorithm, extract the LiDAR point cloud features to obtain the LiDAR pose; At the same time, use the ORB-SLAM3 algorithm to extract visual feature points and combine IMU pre-integration to obtain the visual pose; The LiDAR pose is obtained based on the following steps: Extract edge points and plane points from the LiDAR point cloud, where the edge points are points with a point cloud curvature greater than the threshold ; The plane points are points with a point cloud surface greater than the threshold ; The point cloud curvature is calculated as follows: ; In the formula: is the curvature of point i; represents the neighborhood point set of point i; represents the coordinate of point i in the LiDAR coordinate system; represents the coordinate of point j in the LiDAR coordinate system; Based on the edge points and plane points of the current frame and the previous frame, a non-linear optimization method is used to minimize the matching error of the feature points, and then the LiDAR pose is estimated. The optimization objective is: ; In the formula: represents the LiDAR pose transformation matrix; represents the edge point or plane point of the current frame; Represents the edge points or planar points of the previous frame; Represents the optimal pose transformation matrix obtained through optimization calculation; In this embodiment, a non - linear optimization method is used to minimize the matching error of LiDAR feature points to estimate the pose, which can better handle the noise and uncertainty in the point cloud data and improve the accuracy of pose estimation.

[0022] Multiply the global position of the previous frame by the transformation matrix of the current frame to obtain the LiDAR pose of the current frame in the global coordinate system.

[0023] The specific steps to obtain the visual pose are as follows: Use the internal parameter matrix K and distortion coefficient D of the camera to perform distortion processing on each frame of the image; ; In the formula: represents the pixel coordinates of the distorted image; represents the pixel coordinates after undistortion; Based on the distorted image, use the ORB algorithm to extract the key feature points in the image, including FAST corner detection and generating binary descriptors based on the detected corners; According to the response value R of each key feature point, filter out the key feature points with response values greater than the set threshold, where the response value is calculated through the pixel gray - value difference in the corner neighborhood; ; In the formula: represents the gray - value of the pixel point; represents the center coordinates of the corner; represents the pixel area around the corner; represents the response value; Use the KNN algorithm to match the key feature points of the current frame and the previous frame, retain the matching points with the smallest Hamming distance, and use the random sample consensus algorithm to remove the wrong matching points; Calculate the fundamental matrix F through the matching points, then use singular value decomposition to solve the visual initial pose; and then use the IMU data to pre - integrate the visual initial pose to improve the accuracy and robustness of pose estimation; ; In the formula: represents the essential matrix; represents the camera internal parameter matrix; represents the transpose inverse matrix of the camera internal parameter matrix; represents the inverse matrix of the camera internal parameter matrix; the steps to solve the visual initial pose by singular value decomposition of the essential matrix are as follows: First, use singular value decomposition to decompose the essential matrix E: ; In the formula: and represent orthogonal matrices; represents a diagonal matrix; Adjust the non - zero singular values in to (1, 1, 0) to ensure that the rank of E is 2; Recover the visual pose from the essential matrix E: The essential matrix E is decomposed into a rotation matrix R and a translation vector t: ; In the formula: is the skew - symmetric matrix of t; There are four possible solutions above (combinations of two rotation matrices R and two translation vectors t). Select the optimal solution by verifying the four solutions (such as triangulation and reprojection error), specifically as follows: Calculate the reprojection error of the feature points, and optimize the visual pose based on minimizing the reprojection error. Based on this, obtain the visual pose: ; In the formula: represents the camera pose of the i - th frame; represents the coordinates of the j - th map point in the visual coordinate system; represents the pixel coordinates of the j - th feature point in the i - th frame; represents the optimized visual pose; According to the matching points and the camera pose, generate 3D map points through the triangulation algorithm. The formula is: ; In the formula: represents the pixel coordinates of the j - th feature point in the i - th frame; represents the triangulation algorithm function; After selecting the optimal visual pose through triangulation and reprojection error verification, perform pre - integration using the IMU, specifically as follows: For discrete IMU data, the pre - integration formula is as follows: ; ; ; In the formula: represents the angular velocity at the k - th moment; represents the acceleration at the k - th moment; represents the rotation matrix at the k - th moment; represents the time interval at the k - th moment; represents the gravitational acceleration; represents the velocity increment from moment k to moment ; Obtain the accurate visual pose by optimizing and fusing the visual initial pose and the IMU integration result, where the optimization objective is: ; In the formula: Represents the set of poses in the optimization variables; Represents the set of feature points in the optimization variables; Represents the observation value of the j-th feature point in the j-th frame; Represents the pose of the j-th frame; Represents projecting the feature point onto the image plane of the i-th frame; Represents the three-dimensional polar coordinates of the j-th feature point, where V represents the visual information related to the feature point, i.e., the descriptor of the feature point; Represents the position increment from time i to time j; and respectively represent the position parts in the poses of the j-th frame and the i-th frame; Represents the velocity increment from time i to time j; and respectively represent the velocities at time j and time i; Represents the rotation increment from time i to time j; Represents the rotation matrix of the j-th frame; Represents the transpose of the rotation matrix of the i-th frame; By optimizing the above objective function, minimizing the visual reprojection error and the IMU pre-integration error, more accurate pose estimation can be obtained.

[0024] In this embodiment, the visual initial pose is pre-integrated using IMU data, and the IMU pre-integration results are fused during the optimization process to improve the accuracy and robustness of pose estimation.

[0025] Graph optimization joint optimization: A factor graph is constructed using the GTSAM library, with the LiDAR and visual poses as nodes for joint optimization to obtain the globally optimized pose, and then the global semantic map is obtained. During the joint optimization process, the fusion weights of the visual and LiDAR point cloud features are dynamically adjusted according to the environmental light intensity and point cloud density; The graph optimization joint optimization includes the following steps: Factor graph construction: The LiDAR pose and the visual pose are used as nodes in the factor graph, and constraint factors are added, where the constraint factors include pose constraint factors, IMU pre-integration factors, and loop closure detection factors; among them, the pose constraint factor is the relative pose constraint between the LiDAR pose and the visual pose; the IMU pre-integration factor represents the motion constraint provided by the IMU data; the loop closure detection factor represents the global consistency constraint obtained through loop closure detection; Joint optimization and optimization objective: Use the GTSAM library to optimize the factor graph, where the objective function to be optimized is as follows: ; In the formula: Represents the optimization variables, including LiDAR pose and visual pose; Represents the error function of the i-th constraint factor, which is the error between a certain constraint factor and the optimization variables; Exemplarily, the pose constraint factor is expressed as: ; ; In the formula: Represents Pose; Represents the visual pose; Represents the relative pose; Represents the measured relative pose; The IMU pre-integration factor is expressed as: ; In the formula: Represents the pose measured by the IMU; Represents the pose predicted through and the visual pose; The loop closure detection factor is expressed as: ; In the formula: Represents the pose at time ; Represents the pose at time ; Represents the relative pose obtained through loop closure detection; By minimizing the error function, the optimized LiDAR pose and visual pose are finally obtained; Dynamic weight allocation: Calculate the visual pose confidence based on the illumination intensity of the image and the number of feature points : ; In the formula: Represents the maximum illumination intensity; Represents the maximum number of feature points (obtained through ORB feature extraction); Represents the weight coefficient; Represents the number of feature points in the current image; Represents the illumination intensity of the current image; Calculate the LiDAR pose confidence based on the point cloud density and the number of LiDAR feature points: ; In the formula: Represents the maximum point cloud density; Represents the weight coefficient; Represents the density of the current LiDAR point cloud; and respectively represent the number of edge points and surface points extracted from the LiDAR point cloud; According to the calculated visual and LiDAR confidence levels, weights are assigned to obtain visual weight and LiDAR weight, where the sum of the visual weight and the LiDAR weight is 1; ; In the formula: represents the visual weight; represents the LiDAR weight; Based on the obtained visual weight and LiDAR weight, the LiDAR and visual poses are weighted and fused to obtain the fused pose, and based on this, a global semantic map is obtained: ; In the formula: represents the optimized visual pose; represents the optimized LiDAR pose; represents the finally fused pose; In this embodiment, the LiDAR pose and the visual pose are used as nodes in the factor graph, and various constraint factors (pose constraint factor, IMU pre-integration factor, loop detection factor) are introduced to achieve efficient fusion of multi-sensor data; in particular, the pose constraint factor enhances the robustness and accuracy of the system by calculating the relative pose between the LiDAR pose and the visual pose.

[0026] Calculate the visual pose confidence based on the illumination intensity and the number of feature points of the image, and calculate the LiDAR pose confidence based on the point cloud density and the number of LiDAR feature points. Dynamically adjust the visual weight and the LiDAR weight through these confidence levels to ensure the rationality of the two sensor data during the fusion process; allow the system to adaptively adjust the importance of the sensor data according to the actual environmental conditions, improving the accuracy and reliability of the pose estimation. Therefore, introducing various constraint factors and a dynamic weight allocation mechanism effectively fuses LiDAR and visual data, significantly improving the accuracy of pose estimation and the robustness of the system, and having great application potential and practical value.

[0027] Dynamic path routing: Based on the global semantic map, a path planning algorithm is used to provide guidance for the coordinated actions of the unmanned aerial vehicle and the robotic dog, and risk areas are introduced for dynamic adjustment during the guidance process; The dynamic path routing includes the following steps: Use the ResNet-50 model to detect cracks, output the crack confidence of each detection area, and retain the crack detection results with a confidence greater than 0.95; Use an infrared thermal imaging camera to detect the temperature difference distribution, output the temperature information of each area, and obtain the distribution of high-temperature areas by calculating the temperature gradient field; Fuse the crack detection confidence and the temperature difference information through a Bayesian probability model to construct a dynamic risk field: ; In the formula: represents the crack confidence; Represents the dynamic risk probability value at the point ; Represents the weight coefficient; Represents the temperature difference, which is the difference between the temperature at the area and the reference temperature ; Represents the adjustment coefficient of the Sigmoid function; Represents the temperature gradient at the area ; Represents the maximum value of the temperature gradient; According to the value, the area is divided into different risk levels. The higher the value, the higher the risk; the lower the value, the lower the risk. Output the gridded dynamic risk probability map; For example: The output resolution is 5 cm. According to the value, the area is divided into the following levels: Safe area: Green, ; Warning area: Yellow, ; Dangerous area: Red, ; In this embodiment, in order to ensure real-time performance, a sliding window mechanism can also be used to perform Kalman filtering on the detection results of 5 consecutive frames to suppress false detection interference. In this way, the sensor output can be smoothed, the noise influence can be reduced, and the accuracy of the risk field detection can be improved; Extract key nodes (such as corridor intersections, door frames, voids in ruins, etc.) based on the global semantic map to form a node set in the graph and construct a topological graph. The weight of the node is related to the risk area, and the dynamic risk probability is used to represent the risk of the node. The weight of the edge is composed of the Euclidean distance and the risk coefficient; Among them, the weight of the edge is calculated based on the following steps: Probability field normalization: Convert the dynamic risk probability into the cost weight for path search: ; In the formula: Represents the cost weight for path search; the coefficient 10 is used to amplify the risk difference; Calculate the weight of the edge based on the cost weight for path search: ; In the formula: , Represents the key node in the topological graph; Represents the node and the weight of the edge between; Represents the Euclidean distance; Represents the node and The maximum path cost at The air layer adopts an adaptive The path planning algorithm searches for paths on the topological graph. The sampling strategy preferentially expands the nodes in the low-risk area, and the cost function of the path combines the path length and the risk value, and dynamically adjusts the weight according to the remaining power, and finally obtains the set of safe paths in the air layer; Adopt an adaptive The specific steps of the path planning algorithm to search for paths on the topological graph are as follows: Tree structure definition: The starting tree The root node of is the current position of the UAV ; The target tree Is the target point position ; Parameter initialization: Set the maximum number of iterations, step size, and adaptive weight And ; Exemplarily, the maximum number of iterations is 1000, and the step size is 1.0m; And The initial values of and are 0.6 and 0.4 respectively; Adaptive sampling strategy: Based on the dynamic risk probability map, the area is divided into a low-risk area And medium-high risk areas ; Uniform sampling is used for the low-risk area; Gaussian sampling is used for the high-risk area, and the target point is used as the mean, and the standard deviation is 3.0 to generate normal distribution sampling points, guiding the search to extend in the direction of the target and reducing the invalid branches, specifically as follows: ; Where: Represents the low-risk set; Represents the standard deviation of the Gaussian distribution; Represents uniform sampling, and the action area is the low-risk area ; Represents Gaussian sampling, and the action area is the medium-high risk area, with the target point As the mean, and the standard deviation Is 3.0 to generate normal distribution sampling points; , Indicates that 70% of the resources are used for exploring the safe area, and 30% of the resources are used for approaching the target in the risk area; Nearest neighbor node selection: Calculate the improved cost-sensitive distance between nodes: ; Where: And Represent two different nodes in the path; Represents the path length weight; Represents the path cost weight; Represents the Euclidean distance; Represents from To Path cost integration; Feasible path generation: Expand new nodes from the nearest neighbor node to the randomly sampled point and satisfy the kinematic constraints: ; Where: Represents the newly expanded node; Represents the nearest neighbor node; Represents the randomly sampled point; Represents the step size; If the path crosses the dangerous area, then discard this node; Weight adjustment: When the remaining power is less than the preset threshold (less than 30%), increase the path length weight. The exemplary calculation formula is as follows: ; Based on the calculated updated path length weight and the updated path cost weight ; After normalization, substitute it into the nearest neighbor node selection for calculation; If the average risk entropy of the current path is greater than the preset threshold (greater than 1.2), then increase the risk weight; Exemplary: ; Where: Represents the path cost weight before update; Represents the path cost weight after update; Path smoothing: Optimize the original path by gradient descent, and prune redundant nodes, delete collinear nodes. Among them, when deleting collinear nodes, if the turning angle of three consecutive nodes is less than the preset threshold (5°), then delete the middle node; Exemplary: ; Where: Represents the optimized path node; Represents the node on the original path; Represents the learning rate; Represents the path node at the risk gradient; Optimize the smoothness of the path by the gradient descent method, reduce the path curvature, so as to avoid sharp turns or unnecessary complex paths. The optimization process is iteratively executed 10 times to ensure the path quality; Obtain the optimized aerial layer path through continuous iteration, and ensure that the length of the optimized path does not exceed 1.1 times the theoretical path.

[0028] In this embodiment, by fusing crack detection and temperature difference information and using a Bayesian probability model to construct a dynamic risk field, the environmental risk can be more comprehensively reflected, and the detection accuracy and reliability can be improved; by introducing a sliding window mechanism and Kalman filtering, the false detection interference can be effectively reduced in an environment with high real-time requirements, and the robustness of the system can be enhanced; by calculating the path planning cost weight based on the dynamic risk probability value and combining with the adaptive RRT*-Connect algorithm, the nodes can be preferentially expanded in the low-risk area, the search in the high-risk area can be reduced, and the accuracy and safety of path planning can be improved; by dynamically adjusting the path planning weight according to the remaining battery power and path risk entropy, the path planning can be optimized under different conditions, and the adaptability and flexibility of the system can be enhanced.

[0029] The path planning on the ground layer integrates the dynamic window method and the risk field gradient descent method, and adjusts the speed space sampling through the risk gradient to obtain a set of local safe paths on the ground layer; The specific steps of the path planning on the ground layer are as follows: Risk gradient field calculation: Calculate the gradient of the risk probability field through the discrete difference method, that is, the change rate of the risk field in the x and y directions, where the changes in the x and y directions are both calculated by the central difference method, as follows: ; In the formula: represents the gradient of the risk probability field; represents the change rate of the risk probability field in the x direction; represents the change rate of the risk probability field in the y direction; Central difference method: In the formula: represents the risk probability value at the position ; represents the risk probability value at the position ; represents the discrete interval in the x direction; Inflated dangerous area: Define the area exceeding the danger threshold as the red dangerous area ( ), and adopt morphological dilation operation with the dilation radius set to 1.5 m; Dynamic window generation: Generate the dynamic speed window of the robot dog according to the current speed and speed limit; Linear velocity range: ; where is the time window; represents the current linear velocity of the robot dog; represents the maximum acceleration of the robot dog; represents the maximum linear velocity of the robot dog; Angular velocity range: ; where represents the current angular velocity of the robot dog; Represents the maximum angular velocity of the robotic dog; Velocity discretization: Discretize the linear velocity and angular velocity, where the sampling interval is: ; Generate a set of candidate velocity pairs based on this ; Risk gradient direction velocity adjustment: For each candidate velocity pair , calculate the mean value of the risk gradient at the trajectory point , and correct the velocity based on the mean value of the risk gradient; ; In the formula: Represents the gradient descent step size; Represents the L2 norm of the risk gradient; Based on the above correction, the velocity direction is shifted towards the low-risk area, preventing the robotic dog from entering the high-risk area; Velocity constraint reprojection: Limit the corrected velocity within the range of the dynamic window , ensuring that the velocity of the robotic dog always satisfies the kinematic constraints; Trajectory prediction: For each corrected velocity pair , predict the trajectory within the next 0.5 seconds , where N = 10, which is the number of points for trajectory prediction; and calculate the cost function for each velocity pair: ; In the formula: Represents the overall cost of the given velocity pair ; Represents the distance cost between the current position and the target position; Represents the total risk existing on the path; Represents the heading angle between the current position and the local target position; , and Represents the weight parameters, and their sum is 1; ; In the formula: Represents the coordinate vector of the target position; Represents the coordinate vector of the local target position; Represents the Euclidean norm of the vector; ; In the formula: Represents the risk value at the path point ; Represents the coordinates on the path; N represents the total number of points on the path; ; In the formula: Represents the current velocity vector of the robotic dog; Represents the coordinate vector of the local target position; Represents the current position coordinate vector of the robotic dog; Represents the magnitude of the velocity vector; Represents the distance between the current position and the local target position; Represents the arccosine function; Static obstacle detection: If the predicted trajectory point enters the inflated red danger area Reject this velocity pair; Dynamic obstacle avoidance: For moving obstacles, calculate the collision time with the obstacle. If the collision time is less than the preset threshold (2.0), the velocity pair is considered dangerous and the velocity pair is rejected; ; where: Represents the collision time; Represents the Euclidean distance between the robotic dog and the dynamic obstacle; Represents the position vector of the robotic dog; Represents the position vector of the dynamic obstacle; Represents the Euclidean distance calculation; Cost sorting: For candidate velocity pairs that meet the constraints, sort them in ascending order according to the total cost, and select the top 5% with the lowest cost as the candidate velocity set, and add Gaussian noise to the optimal velocity pair. Based on this, the local safe path of the ground layer is obtained. ; ; where: Represents the generated optimal linear velocity; Represents the generated optimal angular velocity; Represents the execution velocity; Represents the execution angular velocity; Represents a normal distribution with a mean of 0 and a standard deviation of 0.05, used to generate Gaussian noise for the linear velocity; Represents a normal distribution with a mean of 0 and a standard deviation of 0.02, used to generate Gaussian noise for the angular velocity; In this embodiment, the gradient of the risk probability field is calculated using the discrete difference method, and the area exceeding the danger threshold is defined as the red danger area. The danger area is expanded through morphological dilation operation; high-risk areas can be more accurately identified and avoided. At the same time, the dilation operation can take into account a certain safety margin, further improving the safety of path planning; for each candidate velocity pair, it is corrected according to the mean value of the risk gradient at the trajectory point, so that the velocity direction deviates towards the low-risk area; enabling the robotic dog to dynamically avoid risk areas during movement, ensuring the safety and feasibility of the path; during the trajectory prediction process, the distance cost, risk cost, and heading cost are comprehensively considered and optimized through weight parameters, and the top 5% velocity pairs with the lowest cost are selected as the candidate velocity set; through comprehensive consideration and optimization of multiple factors, the effect of path planning is improved, making the planned path more efficient and safe.

[0030] Based on the air-layer safety path and the ground-layer local safety path, dynamic adjustment is carried out using reinforcement learning to select the optimal mode from the candidate paths and dynamically adjust the path tracking parameters.

[0031] Among them, the input data sources for reinforcement learning include: the candidate path set and the real-time state information; Among them, the candidate path set includes the air-layer path set and the ground-layer path set; The real-time state information includes the remaining battery power, communication delay, risk entropy, UAV altitude, and robot dog angular velocity; In the reinforcement learning framework, the above input data is encoded into a seven-dimensional state vector; The specific structure of the reinforcement learning is as follows: State vector: A seven-dimensional state vector including path length, safety score, remaining battery power, risk entropy, communication delay, UAV altitude, and robot dog angular velocity is established ; Current path length; Represents the path safety score; Represents the maximum risk entropy; Represents the maximum flight altitude of the UAV, such as 10m; Represents the maximum angular velocity of the robot dog; Represents the remaining battery power; Represents the communication delay; Represents the current altitude of the UAV; Represents the current angular velocity of the robot dog; Represents the maximum path length; Represents the risk entropy; Action definition: Discrete actions: Include air dominance, ground dominance, and hybrid mode. Among them, the logic of the hybrid mode is that the air path is used for global navigation, and the ground path is then locally adjusted, and the control weights of the air path and the ground path in the hybrid mode are adjusted based on the priority weight, where the priority weight is dynamically calculated based on the continuous action , where Represents the continuous action value; Continuous action: Used to adjust the path tracking aggressiveness coefficient, , control mapping, aggressiveness coefficient ; Safety distance: ; Composite reward function: ; In the formula: Represents the current path length; Represents the risk entropy; Represents the coefficient; Represents the base of the natural logarithm; Represents the remaining battery power; Represents the initial battery level; Represents the weight coefficient of the energy consumption item; Represents the weight coefficient of the communication penalty item; Represents the communication delay.

[0032] The reinforcement learning adopts the PPO training framework, where the network structure includes a high-level policy network and a low-level policy network. The input of the high-level policy network is a 7-dimensional state vector, which is processed through 3 fully connected layers. The activation function uses the ReLU activation function, and the output is a discrete action probability distribution; The input of the low-level policy network is the state vector and the one-hot encoding of the discrete action. Based on 4 fully connected layers, the activation function uses Tanh, and the output is the mean and variance of the continuous action.

[0033] How to perform iterative sampling training based on the above technical solution of reinforcement learning is a conventional technical means in the art, so it will not be elaborated in this embodiment.

[0034] Reverse iris control: Based on the intensity attenuation value of the UWB signal, determine whether the legged robot enters the shielding area. If it enters, the UAV switches to a UWB signal follower, receives the UWB positioning packet transmitted backward by the legged robot, and combines the Kalman filter to fuse the UWB and IMU data to achieve penetration positioning in the shielding area; The specific steps are as follows: Signal intensity monitoring: Real-time monitor the UWB signal intensity , calculate the attenuation rate: ; In the formula: Represents the UWB signal intensity at the current moment t; Represents the UWB signal intensity at the previous moment; Represents the time interval; Mode switching trigger: When any of the following conditions is met, trigger the reverse iris control mode: 1. The UWB signal intensity attenuates ; 2. The SLAM positioning confidence ; Exemplarily, the SLAM positioning confidence can be obtained by weighted summation based on the successful matching ratio of feature points, reprojection error, and IMU pre-integration consistency; 3. The building model matching result is the shielding area; determined by querying whether the real-time pose of the legged robot is within the bounding box of the shielding area; Signal reception: The UAV receives the UWB positioning packet transmitted backward by the legged robot; Positioning solution: , where c represents the speed of light; Represents the reception time; Indicates the transmission time; Coordinate transformation: Convert the relative distance d to the UAV coordinate system: ; where represents the UAV pose vector; u represents the unit vector of the signal direction; Kalman filter fusion: State vector: The pose of the quadruped robot , including the position and the velocity ; Observation model: UWB positioning data , including the position observed by UWB; Prediction update: Prediction: Predict the pose of the quadruped robot based on IMU data: ; In the formula: represents the state transition matrix; represents the IMU input, including acceleration and angular velocity; represents the pose of the quadruped robot predicted by IMU data; represents the state vector at the previous moment; represents the control matrix; Update: Fuse the UWB observation value and update the state estimate: ; In the formula: represents the updated state vector of the quadruped robot; K represents the Kalman gain matrix; represents the residual between the observation value and the predicted value; H represents the observation matrix; UAV trajectory planning: The UAV flies along the outer contour of the building, maintaining the optimal signal path with the quadruped robot: Trajectory optimization: The objective function is the minimum signal propagation path loss: ; In the formula: d represents the distance between the UAV and the quadruped robot; f represents the signal frequency; represents the attenuation factor; t represents the time variable.

[0035] Constraint conditions: The distance between the UAV and the boundary of the shielding area is greater than 1 meter; the signal strength is greater than -90 dBm; Based on the above steps, the shielding area penetration positioning result, the UAV navigation instruction, and the mode switching flag (the flag bit for entering / exit the reverse iris control mode) can be obtained.

[0036] Through time alignment and space alignment technologies, the present invention integrates LiDAR point clouds, visual images, IMU inertial data, and compensated UWB positioning data into a multi-modal data stream that has been spatio-temporally aligned, solving the problems of asynchronous sensor data and spatial offset; through parallel SLAM calculations, LiDAR and visual feature poses are respectively extracted, and the GTSAM library is used for joint optimization of graph optimization to construct a globally optimized pose and a global semantic map to cope with complex and changing rescue environments; the path planning algorithm provides dynamic channel routing for drones and legged robots based on the global semantic map, adjusts the feature fusion weight considering the environmental light intensity and point cloud density, and ensures the positioning accuracy and the reliability of map construction; in addition, the reverse iris control mechanism detects whether the legged robot enters a sheltered area through the UWB signal strength attenuation value, switches the drone to be a UWB signal follower, and combines a Kalman filter to achieve penetration positioning in the sheltered area, thus ensuring the stable execution of rescue tasks in complex environments.

[0037] The above are only the preferred embodiments of the present application and are not intended to limit the present application. Any modifications, equivalent replacements, and improvements made within the spirit and principle of the present application shall be included within the protection scope of the present application.

Claims

1. A collaborative linkage method for a medical rescue drone and a robot dog, characterized in that, Including the following steps: Time alignment: Adopt the Precision Time Protocol to align the timestamps of LiDAR point clouds and visual images; Use linear interpolation to perform time compensation on UWB signals to match them with the visual frame rate; Spatial alignment: Adopt the calibration board joint calibration method to determine the external parameter matrix between LiDAR and multi-view vision, and register the LiDAR point cloud and the visual structured light recovered point cloud through the ICP algorithm to eliminate the spatial offset between multi-sensors; Based on time alignment and spatial alignment, obtain the spatio-temporally aligned multi-modal data stream, including LiDAR point clouds, visual images, IMU inertial data, and compensated UWB positioning data; Parallel SLAM calculation: Based on the LOAM algorithm, extract LiDAR point cloud features to obtain the LiDAR pose; At the same time, use the ORB algorithm to extract visual feature points and combine IMU pre-integration to obtain the visual pose; Graph optimization joint optimization: Construct a factor graph through the GTSAM library, use the LiDAR and visual poses as nodes for joint optimization to obtain the globally optimized pose, and then obtain the global semantic map. And during the joint optimization process, dynamically adjust the fusion weights of visual and LiDAR point cloud features according to the environmental light intensity and point cloud density; Dynamic channel routing: Based on the global semantic map, use the path planning algorithm to provide guidance for the coordinated actions of drones and robot dogs, and introduce risk areas for dynamic adjustment during the guidance process; Reverse iris control: Based on the intensity attenuation value of the UWB signal, determine whether the robot dog enters the shielding area. If it enters, the drone switches to a UWB signal follower, receives the UWB positioning packet transmitted backward by the robot dog, and combines the Kalman filter to fuse UWB and IMU data to achieve penetration positioning within the shielding area.

2. The method for collaborative linkage of a medical rescue drone and a robotic dog according to claim 1, wherein, The LiDAR pose is obtained based on the following steps: Extract edge points and plane points from the LiDAR point cloud, where the edge points are the points with the point cloud curvature greater than the threshold ; the plane points are the points with the point cloud surface greater than the threshold ; the point cloud curvature is calculated as follows: ; in the formula: is the curvature of point i; represents the neighborhood point set of point i; represents the coordinates of point i in the LiDAR coordinate system; represents the coordinates of point j in the LiDAR coordinate system; Based on the edge points and plane points of the current frame and the previous frame, a non-linear optimization method is used to minimize the matching error of the feature points, and then the LiDAR pose is estimated. The optimization objective is as follows: where: represents the LiDAR pose transformation matrix; represents the edge points or plane points of the current frame; represents the edge points or plane points of the previous frame; represents the optimal pose transformation matrix obtained through optimization calculations; Multiply with the transformation matrix of the current frame based on the global position of the previous frame to obtain the LiDAR pose of the current frame in the global coordinate system.

3. A method for collaborative linkage between a medical rescue drone and a robotic dog according to claim 1, characterized in that, The specific steps to obtain the visual pose are as follows: Use the internal parameter matrix K and distortion coefficient D of the camera to perform distortion processing on each frame of the image; Based on the distorted image, use the ORB algorithm to extract key feature points in the image, including FAST corner detection and generating binary descriptors based on the detected corners; According to the response value R of each key feature point, perform screening to retain the key feature points with response values greater than the set threshold, where the response value is calculated through the pixel gray value difference in the corner neighborhood; Adopt the KNN algorithm to match the key feature points of the current frame and the previous frame, retain the matching points with the smallest Hamming distance, and use the random sample consensus algorithm to remove the wrong matching points; Calculate the fundamental matrix F through the matching points, and then use singular value decomposition to solve the visual initial pose; Then use the IMU data to perform pre-integration on the visual initial pose to improve the accuracy and robustness of pose estimation; Calculate the reprojection error of the feature points, and optimize the visual pose based on minimizing the reprojection error, and obtain the visual pose based on this.

4. A method for collaborative linkage of a medical rescue drone and a robot dog according to claim 1, characterized in that, The graph optimization joint optimization includes the following steps: Factor graph construction: The LiDAR pose and the visual pose are used as nodes in the factor graph, and constraint factors are added. The constraint factors include pose constraint factors, IMU pre-integration factors, and loop closure detection factors. Among them, the pose constraint factor is the relative pose constraint between the LiDAR pose and the visual pose; the IMU pre-integration factor represents the motion constraint provided by the IMU data; the loop closure detection factor represents the global consistency constraint obtained through loop closure detection. Joint optimization and optimization objective: Use the GTSAM library for factor graph optimization, and the objective function for optimization is as follows: ; where: represents the optimization variables, including LiDAR poses and visual poses; represents the error function of the i-th constraint factor, which is the error between a certain constraint factor and the optimization variables; By minimizing the error function, the optimized LiDAR pose and visual pose are finally obtained. Dynamic weight assignment: Calculate the visual pose confidence based on the illumination intensity of the image and the number of feature points; calculate the LiDAR pose confidence based on the point cloud density and the number of LiDAR feature points; according to the calculated visual and LiDAR confidences, assign weights to obtain the visual weight and the LiDAR weight, where the sum of the visual weight and the LiDAR weight is 1. Based on the obtained visual weight and LiDAR weight, fuse the LiDAR and visual poses with weights to obtain the fused pose, and based on this, obtain the global semantic map.

5. A method for collaborative linkage between a medical rescue drone and a robot dog according to claim 1, characterized in that, The dynamic channel routing includes the following steps: Use the ResNet-50 model to detect cracks, output the crack confidence of each detection area, and retain the crack detection results with a confidence greater than 0.

95. Use an infrared thermal imaging camera to detect the temperature difference distribution, output the temperature information of each area, and obtain the distribution of high-temperature areas by calculating the temperature gradient field. Fuse the crack detection confidence and temperature difference information through a Bayesian probability model to construct a dynamic risk field: ; In the formula: represents the crack confidence level; represents the dynamic risk probability value at point ; represents the weight coefficient; represents the temperature difference, which is the difference between the temperature at area and the reference temperature ; k represents the adjustment coefficient of the Sigmoid function; represents the temperature gradient at area ; represents the maximum value of the temperature gradient; According to the value divides the area into different risk levels, where the higher the value, the higher the risk, and the lower the value, the lower the risk, and outputs a gridded dynamic risk probability map; Extract key nodes based on the global semantic map to form a set of nodes in the graph, and construct a topological graph. The weight of the node is related to the risk area, and the dynamic risk probability is used to represent the risk of the node. The weight of the edge is composed of the Euclidean distance and the risk coefficient. The aerial layer uses the adaptive RRT*-Connect path planning algorithm to search for paths on the topological graph. The sampling strategy preferentially expands the nodes in the low-risk area. The cost function of the path combines the path length and the risk value, and dynamically adjusts the weight according to the remaining power. Finally, a set of safe paths for the aerial layer is obtained. The ground layer path planning fuses the dynamic window method and the risk field gradient descent method, and adjusts the speed space sampling through the risk gradient to obtain a set of local safe paths for the ground layer. Based on the safe paths of the aerial layer and the local safe paths of the ground layer, use reinforcement learning for dynamic adjustment, select the optimal mode from the candidate paths, and dynamically adjust the path tracking parameters.

6. A method for collaborative linkage between a medical rescue drone and a robotic dog according to claim 5, characterized in that, The specific steps of using the adaptive RRT*-Connect path planning algorithm to search for paths on the topological graph are as follows: Tree structure definition: The root node of the start tree is the current position of the UAV; the target tree is the position of the target point. Parameter initialization: Set the maximum number of iterations, step size, and adaptive weight and ; Adaptive sampling strategy: Divide the area into low-risk areas and medium-high-risk areas based on the dynamic risk probability map; use uniform sampling for low-risk areas. For high-risk areas, use Gaussian sampling, and generate normal distribution sampling points with the target point as the mean and a standard deviation of 3.0, guiding the search to extend in the direction of the target and reducing invalid branches. Nearest neighbor node selection: Calculate the improved cost-sensitive distance between nodes: ; Wherein: and represent two different nodes in the path; represents the path length weight; represents the path cost weight; represents the Euclidean distance; represents the path cost integral from and ; Feasible path generation: Expand new nodes from the nearest neighbor node to the randomly sampled point, and satisfy the kinematic constraints: ; In the formula: represents the newly expanded node; represents the nearest neighbor node; represents the randomly sampled point; represents the step size; If the path passes through a dangerous area, discard the node; Weight adjustment: When the remaining battery power is less than the preset threshold, increase the weight of the path length; if the average risk entropy of the current path is greater than the preset threshold, increase the risk weight; Path smoothing: Optimize the original path using gradient descent, and perform redundant node pruning and collinear node deletion. The collinear node deletion means that when the turning angle of three consecutive nodes is less than the preset threshold, the middle node is deleted; Through continuous iteration, an optimized aerial layer path is obtained, ensuring that the length of the optimized path does not exceed 1.1 times the theoretical path.

7. A method for collaborative linkage between a medical rescue drone and a robotic dog according to claim 5, characterized in that, The specific steps of the ground layer path planning are as follows: Risk gradient field calculation: Calculate the gradient of the risk probability field through the discrete difference method, that is, the rate of change of the risk field in the x and y directions, where the changes in both the x and y directions are calculated using the central difference method; Inflate the dangerous area: Define the area exceeding the danger threshold as the red dangerous area, and use morphological dilation operation with a dilation radius set to 1.5m; Dynamic window generation: Generate a dynamic speed window for the robot dog based on the current speed and speed limit; Velocity discretization: Discretize the linear velocity and angular velocity; Risk gradient direction velocity adjustment: For each candidate velocity pair, calculate the average risk gradient at the trajectory point and correct the velocity based on the average risk gradient; Velocity constraint reprojection: Limit the corrected velocity within the range of the dynamic window to ensure that the velocity of the robot dog always satisfies the kinematic constraints; Trajectory prediction: For each corrected velocity pair, predict the trajectory within the next 0.5 seconds; and calculate the cost function for each velocity pair: ; Wherein: represents the overall cost of the given speed to ; represents the distance cost between the current position and the target position; represents the total risk existing on the path; represents the heading angle between the current position and the local target position; , and represent weight parameters, and their sum is 1; Static obstacle detection: If the predicted trajectory point enters the inflated red dangerous area, eliminate this velocity pair; Dynamic obstacle avoidance: For moving obstacles, calculate the time to collision with the obstacle. If the time to collision is less than the preset threshold, consider the velocity pair dangerous and eliminate the velocity pair; Cost sorting: Sort the candidate velocity pairs that meet the constraints in ascending order according to the total cost, and select the top 5% with the lowest cost as the candidate velocity set. Add Gaussian noise to the optimal velocity pair to obtain the local safe path of the ground layer.

8. A method for collaborative linkage of a medical rescue drone and a robotic dog according to claim 5, characterized in that, The specific structure of the reinforcement learning is as follows: State vector: Establish a seven-dimensional state vector including path length, safety score, remaining battery power, risk entropy, communication delay, UAV altitude, and robot dog angular velocity; Action definition: Discrete actions: Include aerial dominance, ground dominance, and hybrid mode. The logic of the hybrid mode is that the aerial path is used for global navigation, and the ground path is adjusted locally. Based on the priority weight, adjust the control weights of the aerial path and the ground path in the hybrid mode, where the priority weight is dynamically calculated based on continuous actions; Continuous actions: Used to adjust the path tracking aggressiveness coefficient; Composite reward function: ; In the formula: represents the current path length; represents the risk entropy; represents the coefficient; represents the base of the natural logarithm; represents the remaining battery power; represents the initial battery power; represents the weight coefficient of the energy consumption term; represents the weight coefficient of the communication penalty term; represents the communication delay.

9. A method for collaborative linkage between a medical rescue drone and a robotic dog according to claim 8, characterized in that, The reinforcement learning adopts the PPO training framework, where the network structure includes a high-level policy network and a low-level policy network. The input of the high-level policy network is a 7-dimensional state vector, which is processed through 3 fully connected layers. The activation function uses the ReLU activation function, and the output is the discrete action probability distribution; The input of the low-level policy network is the state vector and the one-hot encoding of the discrete action. Based on 4 fully connected layers, the activation function uses Tanh, and the output is the mean and variance of the continuous action.

Images