Rescue robot path planning method and system under industrial vision assistance

Abstract

The application discloses a rescue robot path planning method and system under industrial vision assistance, and relates to the field of industrial vision, and the method comprises the following steps: collecting three-dimensional space data of a rescue environment in real time, generating dynamic environment point cloud data set, and constructing a three-dimensional semantic map of a rescue area; calling real-time updated thermal imaging data, combining the three-dimensional semantic map for path analysis, and obtaining a path planning strategy set; predicting the movement trajectory of a dynamic obstacle based on a local dynamic obstacle avoidance strategy, optimizing the global path planning strategy according to the obstacle prediction trajectory data, and generating a movement control instruction of the rescue robot. The technical problem that the existing rescue robot path planning has poor real-time performance and adaptability due to insufficient dynamic environment information perception is solved, the strong perception ability of industrial vision is relied on, dynamic environment information is captured in real time and accurately, and the real-time response speed of path planning and the adaptability to dynamic environment are improved.

Description

Technical Field

[0001] This application relates to the field of industrial vision, and in particular to a path planning method and system for rescue robots assisted by industrial vision. Background Technology

[0002] In complex and dynamically changing rescue scenarios, the rapid and safe planning of rescue robot paths is crucial for efficiently completing rescue missions, impacting personnel safety and the effectiveness of rescue operations. Currently, the main method for solving the path planning problem for rescue robots is to construct a static environmental map based on traditional environmental perception technology and then plan the path accordingly. However, current methods struggle to acquire comprehensive dynamic information about the environment in real time, leading to an inability to respond promptly to emergencies in complex rescue scenarios. The planned paths often become invalid due to the appearance of dynamic obstacles, causing the robot to easily become stuck or at risk of collisions, thus reducing rescue efficiency and safety.

[0003] At present, the path planning of rescue robots suffers from poor real-time performance and adaptability due to insufficient perception of dynamic environmental information. Summary of the Invention

[0004] This application provides a path planning method and system for rescue robots assisted by industrial vision. It utilizes industrial vision to collect real-time 3D spatial data of the rescue environment to generate a dynamic point cloud dataset and construct a 3D semantic map. Then, it retrieves real-time thermal imaging data and analyzes the 3D semantic map to derive a path planning strategy set containing global and local strategies. Based on local dynamic obstacle avoidance strategies, it predicts the trajectory of dynamic obstacles, optimizes the global path planning strategy accordingly, and generates robot motion control commands. These technical means solve the technical problem of poor real-time performance and adaptability in existing rescue robot path planning due to insufficient perception of dynamic environmental information. It achieves the technical effect of relying on the strong perception capability of industrial vision to capture dynamic environmental information in real time and accurately, thereby improving the real-time response speed and adaptability of path planning to dynamic environments.

[0005] This application provides a path planning method for rescue robots under industrial vision assistance, comprising: real-time acquisition of three-dimensional spatial data of the rescue environment to generate a dynamic environmental point cloud dataset; constructing a three-dimensional semantic map of the rescue area based on the dynamic environmental point cloud dataset; retrieving real-time updated thermal imaging data and performing path analysis in conjunction with the three-dimensional semantic map to obtain a path planning strategy set, the path planning strategy set including a global path planning strategy and a local dynamic obstacle avoidance strategy; predicting the motion trajectory of dynamic obstacles based on the local dynamic obstacle avoidance strategy; optimizing the global path planning strategy based on the obstacle prediction trajectory data; and generating motion control commands for the rescue robot.

[0006] In a possible implementation, the real-time acquisition of three-dimensional spatial data of the rescue environment to generate a dynamic environmental point cloud dataset, and the construction of a three-dimensional semantic map of the rescue area based on the dynamic environmental point cloud dataset, involves the following processing: real-time acquisition of multi-dimensional perception data of the rescue environment through a multi-modal sensor array to generate a dynamic environmental point cloud dataset; motion distortion correction of the dynamic environmental point cloud dataset to generate a corrected point cloud dataset; multi-source data fusion of the dynamic environmental point cloud dataset according to the corrected point cloud dataset to generate a semantically enhanced point cloud dataset; three-dimensional scene analysis based on the semantically enhanced point cloud dataset; dynamic weight allocation of the semantically enhanced point cloud dataset according to the analysis results to determine multiple dynamic weight coefficients; and matching the multiple dynamic weight coefficients with the semantically enhanced point cloud dataset to construct the three-dimensional semantic map.

[0007] In a possible implementation, motion distortion correction is performed on the dynamic environment point cloud dataset to generate a corrected point cloud dataset, and the following processing is performed: the pose change data of the rescue robot is recorded in real time by an inertial measurement unit; the instantaneous pose data of the rescue robot at the acquisition time of the dynamic environment point cloud dataset is calculated based on the pose change data; pose compensation is performed on the dynamic environment point cloud dataset according to the instantaneous pose data to generate the corrected point cloud dataset.

[0008] In a possible implementation, 3D scene parsing is performed based on the semantically enhanced point cloud dataset. Dynamic weight allocation is then performed on the semantically enhanced point cloud dataset according to the parsing results, determining multiple dynamic weight coefficients. The following processing is then performed: A multi-layered 3D convolutional neural network is constructed to perform 3D scene parsing on the semantically enhanced point cloud dataset: the input layer receives the semantically enhanced point cloud dataset; the first hidden layer extracts local geometric features, and the second hidden layer fuses temperature and material features; the output layer analyzes the data using a classifier, combining local geometric features with fused temperature and material features to generate point-level semantic labels, which are then added to the parsing results; the point-level semantic labels are converted into risk level parameters, and the predicted trajectories of moving obstacles are used to dynamically update the weights of the semantically enhanced point cloud dataset based on the risk level parameters, thus determining the multiple dynamic weight coefficients.

[0009] In a possible implementation, real-time updated thermal imaging data is retrieved and combined with the 3D semantic map for path analysis to obtain a path planning strategy set. The path planning strategy set includes a global path planning strategy and a local dynamic obstacle avoidance strategy. The following processing is performed: the thermal imaging data and the 3D semantic map are spatiotemporally aligned to generate a thermal radiation risk layer; the 3D semantic map is annotated based on the thermal radiation risk layer to delineate the boundaries of the heat-sensitive area; kinematic constraints of the rescue robot are introduced, and multi-objective optimization is performed according to the kinematic constraints and the boundaries of the heat-sensitive area; hierarchical planning is performed based on the multi-objective optimization results to generate the global path planning strategy and the local dynamic obstacle avoidance strategy.

[0010] In a possible implementation, kinematic constraints of the rescue robot are introduced, and multi-objective optimization is performed based on these kinematic constraints and the boundary of the heat-sensitive area. Based on the multi-objective optimization results, hierarchical planning is performed to generate the global path planning strategy and the local dynamic obstacle avoidance strategy. The following processes are then performed: a heat exposure risk field is constructed based on the kinematic constraints of the rescue robot and the boundary of the heat-sensitive area; the heat exposure risk field is traversed to search for multiple candidate paths, which are then evaluated to generate a global path baseline; a dynamic planning window is constructed based on the global path baseline for local detection; when a moving obstacle intrusion is detected in the dynamic planning window, a local emergency control command is constructed; the global path planning strategy is constructed based on the global path baseline, and the local dynamic obstacle avoidance strategy is constructed based on the local emergency control command.

[0011] In a possible implementation, the motion trajectory of dynamic obstacles is predicted based on the local dynamic obstacle avoidance strategy. The global path planning strategy is optimized based on the obstacle prediction trajectory data to generate motion control commands for the rescue robot. The following processes are performed: dynamic obstacle perception data is extracted in real time based on the local dynamic obstacle avoidance strategy, and the motion state vector of the obstacle is constructed based on the dynamic obstacle perception data; the motion state vector is predicted using a spatiotemporal attention mechanism to obtain obstacle prediction trajectory data, which includes a probabilistic trajectory distribution map; the global path is reconstructed according to the probabilistic trajectory distribution map to generate a multimodal optimized path set; the multimodal optimized path set is used for inverse calculation to obtain the joint space control commands for the rescue robot; the joint space control commands are executed to perform trajectory synchronization verification, and the motion control commands for the rescue robot are generated based on the verification results.

[0012] In a possible implementation, the global path is reconstructed according to the probabilistic trajectory distribution map to generate a multimodal optimized path set. Inverse calculation is then performed based on the multimodal optimized path set to obtain the joint space control commands for the rescue robot. The following processes are then executed: multimodal sampling search is performed according to the probabilistic trajectory distribution map to obtain multiple optimized path sets; reachability analysis is performed based on the multiple optimized path sets to construct an executable path cluster; the global path is reconstructed according to the executable path cluster to obtain the multimodal optimized path set; inverse motion analysis is performed on the multimodal optimized path set to construct a path point sequence; and the path point sequence is converted into the joint space control commands.

[0013] In a possible implementation, the joint space control command is executed to perform trajectory synchronization verification. Based on the verification result, the motion control command for the rescue robot is generated, and the following processing is performed: the joint space control command is executed to perform feature point matching to obtain a motion trajectory deviation value; based on the motion trajectory deviation value, verification analysis is performed; when the motion trajectory deviation value exceeds a preset deviation threshold, the visual servo control module is triggered to perform pose compensation for the rescue robot and generate pose compensation data; based on the pose compensation data and the verification result, the motion control command for the rescue robot is constructed.

[0014] This application also provides a path planning system for rescue robots under industrial vision assistance, including: a 3D semantic map construction module, used to collect 3D spatial data of the rescue environment in real time, generate a dynamic environmental point cloud dataset, and construct a 3D semantic map of the rescue area based on the dynamic environmental point cloud dataset; a path planning strategy set acquisition module, used to retrieve real-time updated thermal imaging data, combine it with the 3D semantic map to perform path analysis, and obtain a path planning strategy set, the path planning strategy set including a global path planning strategy and a local dynamic obstacle avoidance strategy; and a path planning strategy optimization module, used to predict the motion trajectory of dynamic obstacles based on the local dynamic obstacle avoidance strategy, optimize the global path planning strategy based on the obstacle prediction trajectory data, and generate motion control commands for the rescue robot.

[0015] The proposed industrial vision-assisted path planning method and system for rescue robots first acquires real-time 3D spatial data of the rescue environment to generate a dynamic environmental point cloud dataset. Based on this dataset, a 3D semantic map of the rescue area is constructed. Then, real-time updated thermal imaging data is retrieved and combined with the 3D semantic map for path analysis to obtain a path planning strategy set. This strategy set includes a global path planning strategy and a local dynamic obstacle avoidance strategy. Finally, the trajectory of dynamic obstacles is predicted based on the local dynamic obstacle avoidance strategy, and the global path planning strategy is optimized based on the predicted obstacle trajectory data to generate motion control commands for the rescue robot. This achieves the technical effect of leveraging the strong perception capabilities of industrial vision to capture dynamic environmental information in real-time and accurately, thereby improving the real-time response speed and adaptability of path planning to dynamic environments. Attached Figure Description

[0016] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings of the embodiments of the present invention will be briefly described below. Flowcharts are used in this application to illustrate the operations performed by the system according to the embodiments of the present application. It should be understood that the preceding or following operations are not necessarily performed precisely in sequence. Instead, various steps can be processed in reverse order or simultaneously as needed. Furthermore, other operations can be added to these processes, or one or more steps can be removed from these processes.

[0017] Figure 1 A flowchart illustrating the path planning method for rescue robots with industrial vision assistance provided in this application embodiment.

[0018] Figure 2 A schematic diagram of the structure of the industrial vision-assisted rescue robot path planning system provided in the embodiments of this application.

[0019] Figure labeling: 3D semantic map construction module 10, path planning strategy set acquisition module 20, path planning strategy optimization module 30. Detailed Implementation

[0020] The above description is merely an overview of the technical solution of this application. In order to better understand the technical means of this application and to implement it in accordance with the contents of the specification, and to make the above and other objects, features and advantages of this application more obvious and understandable, specific embodiments of this application are given below.

[0021] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description of this application will be provided in conjunction with the accompanying drawings. The described embodiments should not be considered as limitations on this application. All other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0022] In the following description, references to "some embodiments" describe a subset of all possible embodiments. However, it is understood that "some embodiments" may be the same or different subsets of all possible embodiments and may be combined with each other without conflict. The terms "first" and "second" are used merely to distinguish similar objects and do not represent a specific ordering of objects. The terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or server that includes a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or modules not explicitly listed or inherent to these processes, methods, products, or devices. Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. The terminology used herein is for the purpose of describing embodiments of this application only.

[0023] This application provides a path planning method for rescue robots under industrial vision assistance, such as... Figure 1 As shown, the method includes:

[0024] Step S100: Real-time acquisition of three-dimensional spatial data of the rescue environment, generation of dynamic environmental point cloud dataset, and construction of a three-dimensional semantic map of the rescue area based on the dynamic environmental point cloud dataset.

[0025] Specifically, LiDAR (Light Detection and Ranging) or depth cameras (such as Kinect and RealSense) are used as 3D data acquisition devices. LiDAR acquires the 3D coordinates of each point in the environment by emitting a laser beam and measuring the time difference (TOF) or phase difference of the reflected light; depth cameras calculate depth information using structured light or binocular stereo vision principles. The acquired raw point cloud data undergoes noise reduction and filtering to remove invalid or abnormal data points. For example, a voxel grid filter method is used, dividing the space into fixed-size voxels, retaining only one point within each voxel, thereby reducing the amount of point cloud data and removing noise.

[0026] By using real-time data streaming, newly acquired point cloud data is fused with previous data to construct a dynamic environment point cloud dataset. For example, incremental point cloud fusion algorithms, such as ICP (Iterative Closest Point), are used to register new point clouds with existing point clouds, enabling dynamic updates. A timestamp is added to each frame of point cloud data to distinguish data from different times during subsequent processing, supporting the analysis of dynamic environments. The dynamic environment point cloud dataset refers to a collection of point cloud data containing three-dimensional spatial information of the rescue environment, collected and updated in real time during the rescue robot's movement. It reflects dynamic changes in the environment, such as the movement of obstacles and the collapse of objects.

[0027] Semantic segmentation networks in deep learning (such as Mask R-CNN and DeepLab) are used to semantically annotate point cloud data. For example, each point in the point cloud or a segmented region of the point cloud is labeled with a semantic category such as "wall," "ground," "obstacle," or "trapped person." Point cloud data with semantic information is then fused into a map to construct a 3D semantic map. For example, an octree data structure is used to organize and store point cloud data, while semantic information is stored as an additional attribute in each node for fast querying and updating. A 3D semantic map is a map representation that combines 3D spatial information with semantic information. It not only includes the geometric shape and location information of objects in the environment but also labels the semantic categories of objects (such as walls, ground, obstacles, trapped people), providing robots with a richer understanding of their environment.

[0028] For example, the Velodyne VLP-16 LiDAR can acquire 3D point cloud data of the surrounding environment at a rate of approximately 300,000 points per second. Mounted on top of a rescue robot, the LiDAR scans the surrounding environment in real time as the robot moves through the rubble, generating dynamic point cloud data. Voxel grid filtering reduces the resolution of the point cloud data to 0.1 meters, effectively reducing the data volume and removing noise. Then, a Mask R-CNN network is used to perform semantic segmentation on the filtered point cloud, labeling regions in the point cloud as categories such as "walls," "rubble fragments," and "trapped personnel," and this semantically informative point cloud data is fused into a 3D semantic map.

[0029] In one possible implementation, the real-time acquisition of three-dimensional spatial data of the rescue environment generates a dynamic environmental point cloud dataset. Based on the dynamic environmental point cloud dataset, a three-dimensional semantic map of the rescue area is constructed. Step S100 further includes step S110, which involves real-time acquisition of multi-dimensional perception data of the rescue environment using a multi-modal sensor array to generate a dynamic environmental point cloud dataset. Specifically, a lidar scans the environmental geometry at a frequency of 10Hz to generate initial point cloud data. An RGB-D camera is simultaneously triggered to acquire color depth images and extract texture features and object surface reflectivity. An infrared thermal imager acquires temperature distribution data at a sampling rate of 5Hz and marks the boundaries of heat source areas. The lidar point cloud, RGB-D image, and thermal imaging data are aligned by timestamp to ensure data synchronization. The multi-modal data are fused to generate a multi-modal dynamic environmental point cloud dataset containing geometric, texture, reflectivity, and temperature information.

[0030] For example, a Velodyne VLP-16 LiDAR, a Microsoft Kinect RGB-D camera, and a FLIR E5 thermal imager were used. The LiDAR scanned the environment at a frequency of 10 Hz to generate initial point cloud data; the RGB-D camera simultaneously acquired color depth images to extract texture features and object surface reflectivity; the thermal imager acquired temperature distribution data at a frequency of 5 Hz and marked the boundaries of heat source regions. These data were then fused into a multimodal dynamic environmental point cloud dataset through timestamp alignment.

[0031] Step S120: Motion distortion correction is performed on the dynamic environment point cloud dataset to generate a corrected point cloud dataset. Multi-source data fusion is then performed on the dynamic environment point cloud dataset according to the corrected point cloud dataset to generate a semantically enhanced point cloud dataset. Specifically, motion distortion correction is performed on the point cloud data using a motion estimation algorithm (such as the ICP algorithm) to eliminate point cloud data distortion caused by robot motion, generating a corrected point cloud dataset. The corrected point cloud data is then fused with RGB-D images and thermal imaging data to generate a semantically enhanced point cloud dataset. The semantically enhanced point cloud dataset contains geometric, texture, reflectivity, and temperature information, providing richer data for subsequent 3D scene analysis.

[0032] For example, during robot movement, the point cloud data collected by LiDAR exhibits motion distortion. The ICP algorithm is used to register the point cloud data, eliminating motion distortion and generating a corrected point cloud dataset. Each point in the corrected point cloud data is then matched with its corresponding pixel in an RGB-D image and its corresponding temperature value in thermal imaging data to generate point cloud data containing geometric, texture, reflectivity, and temperature information.

[0033] Step S130: Perform 3D scene parsing based on the semantically enhanced point cloud dataset. Based on the parsing results, dynamically assign weights to the semantically enhanced point cloud dataset to determine multiple dynamic weight coefficients. Specifically, use a 3D scene parsing network in deep learning (such as PointNet, VoxelNet, etc.) to parse the semantically enhanced point cloud dataset, identifying objects, obstacles, trapped individuals, etc. in the environment. The parsing results include information such as the object's category, location, and pose. Based on the parsing results, dynamically assign weights to each point or region in the semantically enhanced point cloud dataset. The dynamic weight coefficients reflect the importance of the point or region in path planning; for example, obstacles have higher weights, while trapped individuals have lower weights.

[0034] For example, the PointNet network is used to parse a semantically enhanced point cloud dataset. The network outputs the semantic category of each point, such as "wall," "ground," "obstacle," and "trapped person." Based on the parsing results, the weight of the obstacle is set to 0.8, and the weight of the trapped person is set to 0.2, indicating that obstacles should be avoided first in path planning.

[0035] Step S140: Match the multiple dynamic weight coefficients with the semantically enhanced point cloud dataset to construct the 3D semantic map. Specifically, match the dynamic weight coefficients with the semantically enhanced point cloud dataset to generate a 3D semantic map. The 3D semantic map includes geometric, texture, reflectivity, temperature, and semantic information of the environment, as well as the dynamic weight coefficients for each point or region.

[0036] In one possible implementation, motion distortion correction is performed on the dynamic environment point cloud dataset to generate a corrected point cloud dataset. Step S120 further includes step S121, which involves recording the pose change data of the rescue robot in real time using an inertial measurement unit (IMU). Specifically, an IMU is installed on the rescue robot to measure the robot's acceleration, angular velocity, and attitude information in real time. The IMU includes a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer, capable of providing high-frequency pose change data.

[0037] For example, the rescue robot is equipped with an Xsens MTi series IMU, which can record acceleration, angular velocity, and attitude angle data in real time at a frequency of 100Hz. The IMU is mounted on the robot chassis and synchronized with the timestamps of the LiDAR, RGB-D camera, and thermal imager. The acceleration and angular velocity data recorded by the IMU during robot movement are shown in Table 1.

[0038] Table 1: Examples of Pose Change Data

[0039]

[0040] Step S122: Calculate the instantaneous pose data of the rescue robot at the acquisition time of the dynamic environment point cloud dataset based on the pose change data. Specifically, using the acceleration and angular velocity data recorded by the IMU, the instantaneous pose data of the robot at each point cloud acquisition time is calculated through integration. An extended Kalman filter (EKF) or an unscented Kalman filter (UKF) is used to filter the IMU data to improve the accuracy of pose estimation.

[0041] Step S123: Perform pose compensation on the dynamic environment point cloud dataset according to the instantaneous pose data to generate the corrected point cloud dataset. Specifically, pose compensation is performed on the point cloud data based on the instantaneous pose data. The specific method is to transform each point cloud data point from the robot's current coordinate system to the global coordinate system. Using rigid body transformation formulas, the coordinates of the point cloud data points are adjusted to eliminate stretching distortion caused by robot movement. The corrected point cloud dataset is generated from the pose-compensated point cloud data to ensure the geometric consistency of the point cloud data.

[0042] For example, suppose that at 0.01 seconds of point cloud acquisition, the robot's instantaneous pose data is position [0.01, 0.02, 0.03] m and attitude angle [0.1, 0.2, 0.3] °. The pose compensation for the point cloud data points is performed as follows: Assume that the coordinates of point cloud data point P in the robot's current coordinate system are [1.0, 2.0, 3.0] m. Using the rigid body transformation formula, P is transformed to the global coordinate system: P global =R·P robot +T, where R is the rotation matrix and T is the translation vector.

[0043] R= ;

[0044] T= ;

[0045] After calculation, the coordinates of point cloud data point P in the global coordinate system are: P global = Repeat the above steps to perform pose compensation on all point cloud data points and generate a corrected point cloud dataset.

[0046] This implementation method records the robot's pose change data in real time using an IMU and calculates the instantaneous pose based on this data. It then compensates for the pose of the point cloud data, effectively eliminating point cloud stretching distortion caused by robot movement. The corrected point cloud data has higher geometric consistency, improves the accuracy of environmental perception, provides more accurate environmental information for subsequent path planning, reduces path planning errors caused by motion distortion, enhances the rescue robot's navigation and obstacle avoidance capabilities in complex dynamic environments, and strengthens the reliability and safety of mission execution.

[0047] In one possible implementation, 3D scene parsing is performed based on the semantically enhanced point cloud dataset. Dynamic weight allocation is then performed on the semantically enhanced point cloud dataset according to the parsing results, determining multiple dynamic weight coefficients. Step S130 further includes step S131, constructing a multi-layered 3D convolutional neural network to perform 3D scene parsing on the semantically enhanced point cloud dataset: the input layer receives the semantically enhanced point cloud dataset; the first hidden layer extracts local geometric features; the second hidden layer fuses temperature and material features; the output layer analyzes the local geometric features and the fused temperature and material features using a classifier, generating point-level semantic labels, which are then added to the parsing results. Specifically, a multi-layered 3D convolutional neural network (3D CNN) is constructed to parse the semantically enhanced point cloud dataset. The network includes an input layer, multiple hidden layers, and an output layer, each layer responsible for extracting features at different levels. The semantically enhanced point cloud dataset is converted into a voxel representation, that is, the 3D space is divided into a fixed-size voxel grid, where each voxel contains geometric, texture, reflectivity, and temperature information of the point cloud data. For example, semantically enhanced point cloud data can be voxelized into a 1 cm³ voxel grid, where each voxel contains the geometric coordinates, RGB color values, reflectance, and temperature values ​​of the point cloud data.

[0048] The input layer receives a voxelized semantically enhanced point cloud dataset, where each voxel contains multi-dimensional information (such as geometric coordinates, RGB color, reflectance, and temperature). The dimension of the input data can be represented as [V, C], where V is the number of voxels and C is the feature dimension of each voxel. For example, assuming the voxelized dataset contains 1000 voxels, and each voxel contains 4 features (geometric coordinates, RGB color, reflectance, and temperature), the dimension of the input data is [1000, 4].

[0049] The first hidden layer uses 3D convolution operations to extract local geometric features and identify basic structures such as the ground and walls. The second hidden layer further integrates temperature and material features to detect key areas such as flame zones (high-temperature areas), electrical equipment (specific material reflectivity), and biological heat sources (such as the body temperature of trapped personnel). Activation functions (such as ReLU) are used to perform nonlinear transformations on the features to enhance their expressive power.

[0050] The output layer uses a softmax classifier to map the extracted features to predefined semantic categories. The softmax classifier outputs the probability distribution of each point belonging to different semantic categories, and selects the category with the highest probability as the point-level semantic label. The generated point-level semantic labels are added to the parsing results to form a complete semantic parsing dataset. For example, for a point in point cloud data, after its features are processed by the network, the probability distribution output by the softmax classifier is: ground: 0.1, wall: 0.2, flame zone: 0.6, electrical equipment: 0.05, trapped person: 0.05. The final semantic label for this point is "flame zone".

[0051] Step S132: The point-level semantic labels are converted into risk level parameters. The predicted trajectories of moving obstacles are combined with the risk level parameters to dynamically update the weights of the semantically enhanced point cloud, determining the multiple dynamic weight coefficients. Specifically, the point-level semantic labels are converted into risk level parameters, for example: flame zone weight: 0.9, moving obstacle weight: 0.7, stable structure (such as wall, ground) weight: 0.3. The risk level parameters reflect the importance and danger of different semantic categories in path planning. Combined with the trajectory prediction of moving obstacles, the weight coefficients in the semantically enhanced point cloud dataset are dynamically updated, that is, the trajectory of moving obstacles is predicted using a Kalman filter or LSTM network, and the weight coefficients are adjusted according to the prediction results to determine the dynamic weight coefficient of each point.

[0052] This implementation utilizes a multi-layered 3D convolutional neural network to accurately parse semantically enhanced point cloud datasets, identifying key areas such as the ground, walls, fire zones, electrical equipment, and biological heat sources. By converting semantic labels into risk level parameters and combining them with trajectory predictions of moving obstacles, the weight coefficients are dynamically updated, reflecting the danger and importance of the environment in real time, thus improving the flexibility and adaptability of path planning. Through dynamic weight coefficients, the path planning algorithm can prioritize avoiding high-risk areas (such as fire zones and moving obstacles), selecting safer paths, improving the rescue robot's navigation and obstacle avoidance capabilities in complex and dynamic environments, and enhancing the safety and reliability of mission execution.

[0053] Step S200: Retrieve real-time updated thermal imaging data and perform path analysis in conjunction with the three-dimensional semantic map to obtain a path planning strategy set, which includes a global path planning strategy and a local dynamic obstacle avoidance strategy.

[0054] Specifically, thermal imaging cameras (such as the FLIR series) are used to collect thermal imaging data of the rescue environment. Thermal imaging data reflects the distribution of ambient temperature. In rescue scenarios, thermal imaging data can help robots identify heat sources, such as the body temperature of trapped personnel or the high temperature of a fire area.

[0055] Spatial alignment and fusion of thermal imaging data with 3D semantic maps is performed. For example, by calibrating the relative position and orientation between the thermal imaging camera and the 3D data acquisition device (such as LiDAR), pixels in the thermal imaging image are mapped to their corresponding positions in the 3D semantic map. Alternatively, a feature point matching method can be used to first extract feature points (such as corner points and edge points) from both the thermal imaging image and the 3D semantic map, and then calculate the spatial transformation relationship between the two by matching these feature points, thus achieving data fusion.

[0056] In the fused map, temperature information from thermal imaging data is used to identify heat sources (such as trapped people, fire sources, etc.). For example, a temperature threshold is set, and areas above the threshold are marked as "heat sources" and used as priority targets or danger zones in path planning.

[0057] For global path planning, the A* algorithm, Dijkstra's algorithm, or improved sampled-basis path planning algorithms (such as PRM, RRT*) are used. For example, the 3D semantic map is discretized into a grid map, and impassable areas such as "walls" and "ruins" are marked as obstacles, while passable areas such as "ground" are marked as free space. Then, using the rescue robot's starting position and target position (such as the location of the trapped person or the safety exit) as the start and end points, the A* algorithm is used to search for the optimal path in the grid map.

[0058] During path planning, kinematic constraints of the rescue robot, such as minimum turning radius and maximum speed, should be considered. For example, when calculating the path, the robot's kinematic model (such as a differential drive model or a four-wheel independent drive model) should be used as a constraint to ensure that the planned path is actually executable by the robot. For differential drive robots, the path should meet the minimum turning radius limit to avoid collisions or inability to turn during path execution.

[0059] For local dynamic obstacle avoidance, potential field-based methods (such as artificial potential field methods) or sampling-based local obstacle avoidance algorithms (such as DWA - Dynamic Window Approach) are used. For example, dynamic obstacles are represented on a map as regions with repulsive potential fields. The robot adjusts its movement direction according to the gradient direction of the potential field within the local region to avoid the obstacles.

[0060] By analyzing changes in thermal imaging and point cloud data, dynamic obstacles can be detected in real time. For example, if a significant change is detected in the point cloud data of a certain area within a short period of time (such as the appearance of a new obstacle or the movement of an obstacle), it is identified as a dynamic obstacle, and its position and motion information are transmitted to the local dynamic obstacle avoidance algorithm so that the obstacle avoidance strategy can be adjusted in a timely manner.

[0061] For example, suppose a rescue robot is equipped with a FLIR E5 thermal imaging camera and a Velodyne VLP-16 LiDAR. At a fire scene, images captured by the thermal imaging camera show high-temperature areas (such as flames and trapped personnel), while the 3D point cloud data generated by the LiDAR provides geometric information about the environment. By calibrating the relative positions and attitudes of the two, pixels in the thermal imaging image are mapped to corresponding positions in the point cloud data, achieving data fusion. In the fused map, high-temperature areas are marked as "heat sources" and used as priority targets in path planning. Using the location of trapped personnel as the target, the A* algorithm is used for global path planning on a discretized 3D semantic map. During the planning process, the differential-driven kinematics model of the rescue robot is considered to ensure that the path meets the minimum turning radius constraint. Simultaneously, the DWA algorithm is used for local dynamic obstacle avoidance. When a dynamic obstacle (such as a moving firefighter or a falling object) is detected, the DWA algorithm adjusts the robot's speed and direction in real time based on the obstacle's position and motion information to avoid the obstacle.

[0062] The path planning strategy set includes a global path planning strategy and a local dynamic obstacle avoidance strategy. The global path planning strategy is used to plan the optimal path from the robot's current position to the target position; the local dynamic obstacle avoidance strategy is used to avoid dynamic obstacles in real time during the robot's movement to ensure movement safety.

[0063] In one possible implementation, real-time updated thermal imaging data is retrieved and combined with the 3D semantic map for path analysis to obtain a path planning strategy set. This path planning strategy set includes a global path planning strategy and a local dynamic obstacle avoidance strategy. Step S200 further includes step S210, which involves spatiotemporally aligning the thermal imaging data with the 3D semantic map to generate a thermal radiation risk layer. Specifically, the timestamps of the thermal imaging data and the 3D semantic map are ensured to be consistent. The thermal imaging data is updated at a lower frequency (e.g., 5Hz), while the 3D semantic map is updated at a higher frequency (e.g., 10Hz). The thermal imaging data is aligned with the most recent 3D semantic map data through interpolation or timestamp matching.

[0064] By calibrating the relative position and orientation between the thermal imaging camera and the 3D data acquisition device (such as LiDAR), thermal imaging data is mapped onto a 3D semantic map. For example, spatial alignment can be performed using feature point matching (such as SIFT, SURF) or ICP algorithms.

[0065] Temperature information from thermal imaging data is converted into thermal radiation risk levels. For example, risk levels are classified according to temperature range (e.g., 0-30°C is low risk, 30-60°C is medium risk, and above 60°C is high risk). The processed thermal radiation risk level data is then fused with a 3D semantic map to generate a thermal radiation risk layer. This layer contains the thermal radiation risk level for each spatial location.

[0066] For example, the thermal imaging camera updates data at a frequency of 5 Hz, and the LiDAR updates data at a frequency of 10 Hz. The thermal imaging data is aligned with the most recent 3D semantic map data through timestamp matching. The ICP algorithm is used to map pixels in the thermal imaging data to their corresponding locations in the 3D semantic map. The temperature values ​​in the thermal imaging data are converted into thermal radiation risk levels, generating a thermal radiation risk layer. Areas with temperatures between 0-30°C are marked as low risk (green), areas between 30-60°C as medium risk (yellow), and areas above 60°C as high risk (red).

[0067] Step S220: The 3D semantic map is annotated based on the thermal radiation risk layer to delineate the boundaries of heat-sensitive areas. Specifically, the boundaries of heat-sensitive areas are delineated according to the risk levels in the thermal radiation risk layer. Heat-sensitive areas refer to high-risk areas (such as flame zones or high-temperature zones) and their surrounding safety buffer zones. Image segmentation algorithms (such as threshold-based segmentation or region growing algorithms) are used to process the thermal radiation risk layer to extract the boundaries of high-risk areas. The delineated boundaries of heat-sensitive areas are then annotated in the 3D semantic map, updating the map's semantic information. For example, the boundaries between high-risk areas and safety buffer zones are marked in the 3D semantic map.

[0068] For example, suppose that areas above 60°C are considered high-risk areas in the thermal radiation risk layer. Using a threshold-based segmentation algorithm, the boundaries of high-risk areas are extracted, and a 1-meter-wide safety buffer zone is drawn around them. The boundaries of high-risk areas and the safety buffer zone are marked on the 3D semantic map, these areas are labeled as "thermal sensitive areas," and assigned corresponding risk level parameters (e.g., a weight of 0.9 for high-risk areas and a weight of 0.7 for the safety buffer zone).

[0069] Step S230 involves introducing kinematic constraints of the rescue robot and performing multi-objective optimization based on these constraints and the boundary of the heat-sensitive area. Hierarchical planning is then performed based on the multi-objective optimization results to generate the global path planning strategy and the local dynamic obstacle avoidance strategy. Specifically, considering the kinematic model of the rescue robot (such as a differential drive model or a four-wheel independent drive model), its minimum turning radius, maximum speed, acceleration, and other parameters are determined. In path planning, it is ensured that the planned path satisfies the robot's kinematic constraints.

[0070] Multi-objective optimization is performed by combining the boundary of the heat-sensitive area and kinematic constraints. Optimization objectives include: shortest path length (reducing rescue time), minimum thermal risk (avoiding high-risk areas), and motion feasibility (satisfying the robot's kinematic constraints). Optimization results are generated using multi-objective optimization algorithms (such as Pareto optimization and genetic algorithms).

[0071] Based on the multi-objective optimization results, hierarchical planning is performed. Specifically, the A algorithm or RRT algorithm is used to plan the optimal path from the starting point to the ending point in a 3D semantic map for global path planning, considering the boundaries of heat-sensitive areas and kinematic constraints. The DWA algorithm or artificial potential field method is used to adjust the path in real time within local areas to avoid dynamic obstacles and heat-sensitive regions, performing local dynamic obstacle avoidance.

[0072] For example, suppose a rescue robot uses a differential drive model with a minimum turning radius of 1 meter and a maximum speed of 0.5 m / s. Path planning ensures the planned path satisfies these kinematic constraints. A genetic algorithm is used for multi-objective optimization, with objectives including shortest path length, minimum thermal risk, and motion feasibility. The generated optimization results are a set of candidate path solutions, each with a corresponding path length, thermal risk value, and motion feasibility score. Based on the optimization results, the A* algorithm is used for global path planning to generate the optimal path from the starting point to the destination. In local dynamic obstacle avoidance, the DWA algorithm is used to adjust the path in real time to avoid dynamic obstacles and thermally sensitive areas.

[0073] This approach, through spatiotemporal alignment and the generation of thermal radiation risk layers, can accurately assess the thermal radiation risk in the environment, providing important reference information for path planning and improving the robot's navigation capabilities in complex environments such as high temperatures and fires.

[0074] In one possible implementation, kinematic constraints of the rescue robot are introduced. Multi-objective optimization is performed based on these kinematic constraints and the boundary of the heat-sensitive area. Hierarchical planning is then performed based on the multi-objective optimization results to generate the global path planning strategy and the local dynamic obstacle avoidance strategy. Step S230 further includes step S231, constructing a heat exposure risk field based on the rescue robot's kinematic constraints and the boundary of the heat-sensitive area. Specifically, the heat exposure risk field is a scalar field representing the heat exposure risk level at each location in the environment. The heat exposure risk at each location is calculated by combining the boundary of the heat-sensitive area and the robot's kinematic constraints. For example, high-risk areas (such as flame zones) have higher risk values, while locations far from high-risk areas have lower risk values. Robot kinematic constraints (such as minimum turning radius and maximum speed) are considered to ensure that path planning avoids high-risk areas while satisfying the robot's movement capabilities.

[0075] For example, assuming the boundaries of the heat-sensitive area have been defined, the risk value for a high-risk area (such as a flame zone) is 0.9, for a medium-risk area (such as a high-temperature area) it is 0.7, and for a low-risk area (such as a safety buffer zone) it is 0.3. Based on the robot's kinematic constraints (such as a minimum turning radius of 1 meter), a larger safety buffer zone is set around the high-risk area, with the risk value gradually decreasing. The heat exposure risk field is represented as a three-dimensional grid, and the risk value of each grid point is calculated based on its distance from the heat-sensitive area and the robot's kinematic constraints. For example, the risk value of a grid point closer to the high-risk area is 0.8, and the risk value of a grid point farther away from the high-risk area is 0.2.

[0076] Step S232: Traverse the thermal exposure risk field to search for multiple candidate paths, evaluate these paths, and generate a global path baseline. Specifically, use path planning algorithms (such as A algorithm or RRT algorithm) to search for multiple candidate paths in the thermal exposure risk field. During the search, path length, thermal risk value, and kinematic constraints are considered. The generated candidate paths should avoid high-risk areas and satisfy the robot's kinematic constraints. Each candidate path is evaluated, considering the total path length, average thermal risk value, and motion feasibility. A multi-objective optimization algorithm (such as Pareto optimization) is used to rank the candidate paths, and the optimal path is selected as the global path baseline.

[0077] For example, suppose we use the A* algorithm to search for candidate paths in a thermal exposure risk field. From the starting point to the ending point, five candidate paths are found, each avoiding high-risk areas and satisfying the robot's kinematic constraints. The five candidate paths are evaluated: path 1 has a total length of 10 meters and an average thermal risk value of 0.3; path 2 has a total length of 12 meters and an average thermal risk value of 0.2; and path 3 has a total length of 11 meters and an average thermal risk value of 0.25. Through multi-objective optimization, path 2 is selected as the global path baseline.

[0078] Step S233: Construct a dynamic planning window based on the global path baseline for local detection. When a moving obstacle is detected intruding into the dynamic planning window, a local emergency control command is generated. Specifically, a dynamic planning window is constructed around the global path baseline for local detection. The size of the window can be adjusted according to the robot's kinematic constraints and the boundary of the heat-sensitive area. The dynamic planning window is used to detect dynamic obstacles on the path in real time.

[0079] Within the dynamic programming window, sensors (such as LiDAR and RGB-D cameras) are used to detect moving obstacles in real time. When an intrusion of a moving obstacle is detected, local emergency control commands are generated to adjust the robot's direction and speed to avoid the obstacle.

[0080] For example, suppose the global path baseline is a straight line from the starting point to the ending point, and the dynamic programming window is 2 meters wide and 5 meters long. The window moves along the path baseline, detecting dynamic obstacles on the path in real time. Within the dynamic programming window, the LiDAR detects a moving obstacle intruding into the path. Based on the obstacle's position and speed, local emergency control commands are constructed to adjust the robot's direction and speed to avoid the obstacle. For example, the robot decelerates and turns to bypass the obstacle.

[0081] Step S234: Construct the global path planning strategy based on the global path baseline, and construct the local dynamic obstacle avoidance strategy based on the local emergency control commands. Specifically, a global path planning strategy is generated based on the global path baseline. This strategy includes a path point sequence, speed commands, and turning commands. The global path planning strategy ensures that the robot's path from the starting point to the ending point is optimal, while avoiding high-risk areas.

[0082] Based on local emergency control commands, a local dynamic obstacle avoidance strategy is generated. This strategy includes real-time adjustments to the movement direction and speed commands. The local dynamic obstacle avoidance strategy ensures that the robot can avoid obstacles in a dynamic environment, guaranteeing safe movement.

[0083] Step S300: Based on the local dynamic obstacle avoidance strategy, predict the motion trajectory of the dynamic obstacle, optimize the global path planning strategy according to the obstacle prediction trajectory data, and generate motion control commands for the rescue robot.

[0084] Specifically, machine learning-based prediction algorithms, such as Long Short-Term Memory (LSTM) networks or Kalman filters, are employed. For example, an LSTM network can be used to learn from historical position and velocity data of dynamic obstacles to predict their future trajectories. LSTM networks are capable of processing time-series data, capturing patterns and trends in obstacle movement, and thus accurately predicting their future trajectories.

[0085] The position coordinates and velocity information of dynamic obstacles in a 3D semantic map are used as input data. After data preprocessing (such as normalization), the data is fed into the prediction model. For example, the position coordinates of the obstacles are converted to a local coordinate system relative to the robot to better capture relative motion relationships. The prediction model outputs a sequence of the dynamic obstacles' positions over a future period, serving as the trajectory prediction result. For example, predicting the position coordinates of the obstacles every 0.1 seconds within the next 5 seconds forms a predicted trajectory.

[0086] An obstacle avoidance priority weight matrix is ​​constructed based on the predicted trajectories and importance of dynamic obstacles. This matrix represents the obstacle avoidance priority at different locations. During path planning optimization, the weight values ​​in the matrix are dynamically adjusted based on the predicted trajectories and importance of dynamic obstacles to guide the robot to prioritize avoiding obstacles with higher priority weights. For example, dynamic obstacles close to the robot's path are assigned higher priority weights, while obstacles far from the path are assigned lower weights. Simultaneously, the weight matrix is ​​dynamically adjusted considering the hazard of the obstacles (e.g., whether they are fire sources or collapsing structures) and the urgency of the trapped personnel. For instance, if a dynamic obstacle is a burning object and is close to a trapped person, its priority weight is set to the highest, prioritizing the avoidance of this obstacle to ensure the safety of the trapped person.

[0087] Obstacle avoidance priority weight matrices are applied to the global path planning strategy to optimize paths. For example, by adjusting the cost function in the path planning algorithm, obstacle avoidance priority weights can be added as penalty terms to the path cost calculation. When a path passes near an obstacle with a high priority weight, the cost function value increases significantly, prompting the path planning algorithm to choose a path that avoids that obstacle. For instance, when using the A* algorithm, in calculating the cost of each node, in addition to considering the path length and obstacle distance, the penalty value corresponding to the obstacle avoidance priority weight is added, and a new optimized path is searched.

[0088] Based on the optimized global path planning strategy and local dynamic obstacle avoidance strategy, combined with the kinematic model of the rescue robot, motion control commands are generated. For example, for a differential drive robot, the target speed and steering angle of the robot are calculated based on the path planning results and converted into rotational speed commands for the left and right wheels. Specifically, points on the path are mapped to the robot's motion parameters using kinematic equations to generate corresponding speed and steering commands. To ensure the smoothness of the robot's motion, the generated motion control commands are smoothed. For example, a PID controller is used to adjust the speed and steering commands to avoid sudden changes in commands that could lead to robot instability. Simultaneously, trajectory tracking algorithms (such as the Pure Pursuit algorithm) can be introduced to enable the robot to smoothly track the planned path.

[0089] For example, suppose a rescue robot detects a dynamic obstacle (such as a moving firefighter) at a fire scene. An LSTM network is used to analyze the firefighter's historical position and velocity data to predict its trajectory within the next 5 seconds. The prediction shows the firefighter will move along a curved path, close to the robot's planned path. Based on the predicted trajectory, an obstacle avoidance priority weight matrix is ​​constructed, setting a higher weight for the area where the firefighter is located. This obstacle avoidance priority weight matrix is ​​applied to the global path planning strategy to re-optimize the path. The optimized path avoids the firefighter's predicted trajectory area, ensuring the robot does not collide with the firefighter during movement. Based on the optimized path, combined with the robot's differential drive kinematics model, motion control commands are generated. For example, if the target speed of the robot at each point on the path is calculated to be 0.5 m / s and the turning angle to be 30 degrees, this is converted into left and right wheel speed commands of 100 rpm and 120 rpm, respectively. A PID controller is used to adjust the commands, allowing the robot to move smoothly along the optimized path.

[0090] In one possible implementation, the motion trajectory of the dynamic obstacle is predicted based on the local dynamic obstacle avoidance strategy, and the global path planning strategy is optimized based on the obstacle prediction trajectory data to generate motion control commands for the rescue robot. Step S300 further includes step S310, extracting dynamic obstacle perception data in real time based on the local dynamic obstacle avoidance strategy, and constructing the obstacle's motion state vector based on the dynamic obstacle perception data. Specifically, sensors (such as LiDAR, RGB-D camera) are used to perceive the position, velocity, and acceleration information of the dynamic obstacle in real time. The perception data of the dynamic obstacle is extracted through the local dynamic obstacle avoidance strategy, including position coordinates, velocity vector, and acceleration vector. The extracted dynamic obstacle perception data is integrated into a motion state vector, represented as S=[x,y,z,v]. x ,v y ,v z ,a x ,a y ,a z ], where (x, y, z) are the position coordinates, (v x ,v y ,v z ) is the velocity vector, (a x ,a y ,a z ) is the acceleration vector.

[0091] Step S320: A spatiotemporal attention mechanism is used to predict the motion state vector to obtain obstacle prediction trajectory data, which includes a probabilistic trajectory distribution map. Specifically, a spatiotemporal attention mechanism (such as the Transformer architecture) is used to process the motion state vector to capture the spatiotemporal features of dynamic obstacles. The spatiotemporal attention mechanism can dynamically adjust the degree of attention given to different time steps and spatial positions, improving prediction accuracy. Through the spatiotemporal attention mechanism, predicted trajectory data of the obstacle is generated, including a probabilistic trajectory distribution map, where the probabilistic trajectory distribution map represents the probability distribution of the obstacle's position at different future time steps. Examples of probabilistic trajectory distribution maps are shown in Table 2.

[0092] Table 2: Examples of Probabilistic Trajectory Distribution Plots

[0093]

[0094] Step S330: Reconstruct the global path according to the probabilistic trajectory distribution map to generate a multimodal optimized path set. Perform inverse calculation based on the multimodal optimized path set to obtain the joint space control commands for the rescue robot. Specifically, reconstruct the global path according to the probabilistic trajectory distribution map, avoiding high-probability obstacle locations. Use a multi-objective optimization algorithm (such as Pareto optimization) to generate the multimodal optimized path set, considering path length, thermal risk, and obstacle avoidance probability. Perform inverse calculation based on the multimodal optimized path set to generate the joint space control commands for the rescue robot. The inverse calculation converts path points into joint angle commands for the robot, ensuring that the robot can move according to the planned path.

[0095] For example, suppose the probabilistic trajectory distribution map shows that an obstacle will move to a certain area on the path within the next 3 seconds. Based on this information, the global path is reconstructed to avoid high-probability obstacle locations. A multi-objective optimization algorithm generates a multimodal optimized path set, where each path considers path length, thermal risk, and obstacle avoidance probability. For example, path 1 has a total length of 10 meters, a thermal risk of 0.3, and an obstacle avoidance probability of 0.9; path 2 has a total length of 12 meters, a thermal risk of 0.2, and an obstacle avoidance probability of 0.95. Assuming path 2 is selected as the optimal path, the joint space control commands generated by the reverse calculation are shown in Table 3.

[0096] Table 3: Examples of Joint Space Control Commands

[0097]

[0098] Step S340: Execute the joint space control commands to verify trajectory synchronization, and generate motion control commands for the rescue robot based on the verification results. Specifically, execute the joint space control commands and monitor the robot's motion state in real time using sensors (such as encoders and IMUs). Compare the actual motion trajectory with the planned trajectory to verify trajectory synchronization. Based on the verification results, adjust the joint space control commands to generate the final motion control commands. Use a PID controller or other control algorithms to ensure the robot moves according to the planned trajectory.

[0099] For example, suppose that after the robot executes joint space control commands, the actual motion trajectory monitored by the encoder deviates from the planned trajectory. For instance, the actual position is (1.02, 2.03, 0.51), while the planned position is (1.0, 2.0, 0.5). Based on the verification results, the joint space control commands are adjusted to generate the final motion control commands. For example, the adjusted joint angle commands are (0.11, 0.22, 0.33, 0.44), ensuring that the robot moves according to the planned trajectory.

[0100] In one possible implementation, the global path is reconstructed according to the probabilistic trajectory distribution map to generate a multimodal optimized path set. Inverse calculation is then performed based on the multimodal optimized path set to obtain the joint space control commands for the rescue robot. Step S330 further includes step S331, where multimodal sampling search is performed according to the probabilistic trajectory distribution map to obtain multiple optimized path sets. Reachability analysis is then performed based on these multiple optimized path sets to construct an executable path cluster. Specifically, multiple candidate paths are generated using multimodal sampling methods (such as Monte Carlo sampling and Latin hypercube sampling) based on the probabilistic trajectory distribution map. Each candidate path considers the high-probability location and movement trend of dynamic obstacles. The candidate paths are evaluated by combining multimodal information such as thermal radiation risk, path length, and obstacle avoidance probability. Kinematic constraint checks are performed on each candidate path to ensure that the path satisfies the kinematic model of the rescue robot (such as minimum turning radius, maximum speed, and acceleration). Collision detection algorithms (such as BVH trees and GJK algorithms) are used to check whether each point on the path collides with obstacles in the environment. Based on kinematic constraints and collision detection results, each candidate path is scored for feasibility, and executable paths are selected. The selected executable paths are then clustered to form executable path clusters. Each path cluster contains a set of similar paths that demonstrate high feasibility in terms of kinematics and obstacle avoidance. Each path cluster is further optimized, and the optimal path is selected as the representative path of the cluster.

[0101] For example, suppose a probabilistic trajectory distribution map shows that a dynamic obstacle will move to a certain area on the path within the next 3 seconds. Monte Carlo sampling is used to generate 100 candidate paths, each considering the high-probability location and movement trend of the dynamic obstacle. Kinematic constraint checks and collision detection are performed on the 100 candidate paths, and 30 executable paths are selected. The feasibility score for each path is shown in Table 4.

[0102] Table 4: Examples of Reachability Analysis

[0103]

[0104] The 30 executable paths were clustered into 3 path clusters. The optimal path of each path cluster was selected as the representative path, as shown in Table 5.

[0105] Table 5: Example of Executable Path Cluster Construction

[0106]

[0107] Step S332: Reconstruct the global path according to the executable path cluster to obtain a multimodal optimized path set. Specifically, the representative path in the executable path cluster is fused with the global path baseline to generate the multimodal optimized path set. A multi-objective optimization algorithm (such as Pareto optimization) is used to further optimize the path, considering multimodal information such as path length, thermal risk, and obstacle avoidance probability. The optimal path is selected from the multimodal optimized path set as the final global path. The final path is smoothed to ensure its continuity and executability.

[0108] For example, representative paths from the three path clusters are fused with the global path baseline to generate a multimodal optimized path set. The optimization results for each path are shown in Table 6.

[0109] Table 6: Examples of Global Path Restructuring

[0110]

[0111] Using a multi-objective optimization algorithm, path number 15 is selected as the final global path. The path is then smoothed to generate the final global path.

[0112] Step S333: Perform inverse kinematic analysis on the multimodal optimized path set to construct a path point sequence, and convert the path point sequence into the joint space control commands. Specifically, convert the path points in the multimodal optimized path set into a path point sequence, where each path point contains position and orientation information. Use inverse kinematics algorithms (such as the Jacobi pseudo-inverse method or DLS method) to convert the path point sequence into joint space control commands, including joint angle, velocity, and acceleration commands. Inverse kinematics calculation ensures that the robot can move according to the planned path. Smooth the joint space control commands to ensure smooth robot movement.

[0113] For example, suppose the final global path contains 10 path points, and the position and orientation information of each path point is shown in Table 7.

[0114] Table 7: Examples of Reverse Motion Analysis

[0115]

[0116] The joint space control commands generated through inverse kinematics calculation are shown in Table 8.

[0117] Table 8: Examples of Joint Space Control Command Generation

[0118]

[0119] In one possible implementation, the joint space control command is executed to perform trajectory synchronization verification. Based on the verification result, the motion control command for the rescue robot is generated. Step S340 further includes step S341, executing the joint space control command to perform feature point matching and obtain the motion trajectory deviation value. Specifically, during the execution of the joint space control command, visual sensors (such as RGB-D cameras or LiDAR) are used to collect feature points in the environment in real time. Feature points can be fixed markers, object edges, or significant geometric features in the environment. The real-time collected feature points are matched with feature points on the pre-planned path. Feature matching algorithms (such as SIFT, SURF, ORB) are used to calculate the deviation value between the real-time feature points and the planned feature points. The deviation value can be expressed as position deviation and attitude deviation.

[0120] Step S342: Based on the motion trajectory deviation value, a verification analysis is performed. When the motion trajectory deviation value exceeds a preset deviation threshold, the visual servo control module is triggered to perform pose compensation for the rescue robot, generating pose compensation data. Specifically, the calculated motion trajectory deviation value is compared with the preset deviation threshold. The preset deviation threshold can be adjusted according to task requirements and robot performance. When the motion trajectory deviation value exceeds the preset deviation threshold, the visual servo control module is triggered. The visual servo control module adjusts the robot's pose based on real-time visual feedback and calculates pose compensation data, including position compensation and posture compensation. The pose compensation data is used to adjust the robot's motion trajectory, causing it to return to the planned path.

[0121] Step S343: Construct the motion control commands for the rescue robot based on the pose compensation data and the verification results. Specifically, combine the pose compensation data with the verification results to adjust the joint space control commands. Using a PID controller or other control algorithms, convert the pose compensation data into joint angle, velocity, and acceleration commands to generate the final motion control commands, ensuring the robot moves along the adjusted trajectory.

[0122] This application's embodiments utilize industrial vision to collect real-time 3D spatial data of the rescue environment to generate a dynamic point cloud dataset and construct a 3D semantic map. Then, real-time thermal imaging data is retrieved, and combined with the 3D semantic map analysis, a path planning strategy set containing global and local strategies is derived. Based on the local dynamic obstacle avoidance strategy, the trajectory of dynamic obstacles is predicted, and the global path planning strategy is optimized accordingly. These techniques, including generating robot motion control commands, solve the technical problem of poor real-time performance and adaptability in existing rescue robot path planning due to insufficient perception of dynamic environmental information. This achieves the technical effect of relying on the strong perception capabilities of industrial vision to capture dynamic environmental information in real-time and accurately, thereby improving the real-time response speed and adaptability of path planning to dynamic environments.

[0123] In the above text, refer to Figure 1 A path planning method for rescue robots under industrial vision assistance according to embodiments of the present invention is described in detail. Next, reference will be made to... Figure 2 A path planning system for rescue robots with industrial vision assistance according to an embodiment of the present invention is described.

[0124] The industrial vision-assisted path planning system for rescue robots according to embodiments of the present invention addresses the technical problem of poor real-time performance and adaptability in existing rescue robot path planning due to insufficient perception of dynamic environmental information. It achieves the technical effect of leveraging the strong perception capabilities of industrial vision to capture dynamic environmental information in real time and accurately, thereby improving the real-time response speed and adaptability of path planning to dynamic environments. The industrial vision-assisted path planning system for rescue robots includes: a 3D semantic map construction module 10, a path planning strategy set acquisition module 20, and a path planning strategy optimization module 30.

[0125] The three-dimensional semantic map construction module 10 is used to collect three-dimensional spatial data of the rescue environment in real time, generate a dynamic environmental point cloud dataset, and construct a three-dimensional semantic map of the rescue area based on the dynamic environmental point cloud dataset; the path planning strategy set acquisition module 20 is used to retrieve real-time updated thermal imaging data, perform path analysis in combination with the three-dimensional semantic map, and obtain a path planning strategy set, which includes a global path planning strategy and a local dynamic obstacle avoidance strategy; the path planning strategy optimization module 30 is used to predict the motion trajectory of dynamic obstacles based on the local dynamic obstacle avoidance strategy, optimize the global path planning strategy based on the obstacle prediction trajectory data, and generate motion control commands for the rescue robot.

[0126] The specific configuration of the 3D semantic map construction module 10 will be described in detail below. As mentioned above, the 3D spatial data of the rescue environment is collected in real time to generate a dynamic environmental point cloud dataset. Based on the dynamic environmental point cloud dataset, a 3D semantic map of the rescue area is constructed. The 3D semantic map construction module 10 may further include: a multi-dimensional perception data acquisition unit for collecting multi-dimensional perception data of the rescue environment in real time through a multi-modal sensor array to generate a dynamic environmental point cloud dataset; a multi-source data fusion unit for performing motion distortion correction on the dynamic environmental point cloud dataset to generate a corrected point cloud dataset, and performing multi-source data fusion on the dynamic environmental point cloud dataset according to the corrected point cloud dataset to generate a semantically enhanced point cloud dataset; a dynamic weight allocation unit for performing 3D scene analysis based on the semantically enhanced point cloud dataset, and performing dynamic weight allocation on the semantically enhanced point cloud dataset according to the analysis results to determine multiple dynamic weight coefficients; and a matching unit for matching the multiple dynamic weight coefficients with the semantically enhanced point cloud dataset to construct the 3D semantic map.

[0127] The dynamic environment point cloud dataset is subjected to motion distortion correction to generate a corrected point cloud dataset. The multi-source data fusion unit may further include: a pose change data recording subunit for recording the pose change data of the rescue robot in real time through an inertial measurement unit; an instantaneous pose data calculation subunit for calculating the instantaneous pose data of the rescue robot at the acquisition time of the dynamic environment point cloud dataset based on the pose change data; and a pose compensation subunit for performing pose compensation on the dynamic environment point cloud dataset according to the instantaneous pose data to generate the corrected point cloud dataset.

[0128] Specifically, a 3D scene parsing is performed based on the semantically enhanced point cloud dataset. Dynamic weight allocation is then performed on the semantically enhanced point cloud dataset according to the parsing results to determine multiple dynamic weight coefficients. The dynamic weight allocation unit may further include: a 3D scene parsing subunit for constructing a multi-layered 3D convolutional neural network to perform 3D scene parsing on the semantically enhanced point cloud dataset: an input layer receives the semantically enhanced point cloud dataset; a first hidden layer extracts local geometric features; a second hidden layer fuses temperature and material features; the output layer analyzes the local geometric features and the fused temperature and material features using a classifier to generate point-level semantic labels, which are then added to the parsing results; and a dynamic weight update subunit for converting the point-level semantic labels into risk level parameters, predicting the trajectory of moving obstacles, and dynamically updating the weights of the semantically enhanced point cloud dataset using the risk level parameters to determine the multiple dynamic weight coefficients.

[0129] The specific configuration of the path planning strategy set acquisition module 20 will be described in detail below. As mentioned above, real-time updated thermal imaging data is retrieved and combined with the three-dimensional semantic map for path analysis to obtain a path planning strategy set. The path planning strategy set includes a global path planning strategy and a local dynamic obstacle avoidance strategy. The path planning strategy set acquisition module 20 may further include: a spatiotemporal alignment unit for spatiotemporally aligning the thermal imaging data with the three-dimensional semantic map to generate a thermal radiation risk layer; a labeling unit for labeling the three-dimensional semantic map based on the thermal radiation risk layer to delineate the boundaries of the heat-sensitive area; and a multi-objective optimization unit for introducing the kinematic constraints of the rescue robot, performing multi-objective optimization according to the kinematic constraints and the boundaries of the heat-sensitive area, and performing hierarchical planning based on the multi-objective optimization results to generate the global path planning strategy and the local dynamic obstacle avoidance strategy.

[0130] The process involves introducing kinematic constraints on the rescue robot, performing multi-objective optimization based on these constraints and the boundary of the heat-sensitive area, and then performing hierarchical planning based on the multi-objective optimization results to generate the global path planning strategy and the local dynamic obstacle avoidance strategy. The multi-objective optimization unit may further include: a heat exposure risk field construction subunit for constructing a heat exposure risk field based on the kinematic constraints of the rescue robot and the boundary of the heat-sensitive area; a global path baseline generation subunit for traversing the heat exposure risk field to search for multiple candidate paths, evaluating these candidate paths, and generating a global path baseline; a local emergency control command construction subunit for constructing a dynamic planning window based on the global path baseline for local detection, and constructing a local emergency control command when a moving obstacle intrusion is detected in the dynamic planning window; and a global path planning strategy construction subunit for constructing the global path planning strategy based on the global path baseline and the local dynamic obstacle avoidance strategy based on the local emergency control command.

[0131] The specific configuration of the path planning strategy optimization module 30 will be described in detail below. As mentioned above, the path planning strategy optimization module 30 predicts the motion trajectory of dynamic obstacles based on the local dynamic obstacle avoidance strategy, optimizes the global path planning strategy based on the obstacle prediction trajectory data, and generates motion control commands for the rescue robot. The path planning strategy optimization module 30 may further include: a motion state vector construction unit for extracting dynamic obstacle perception data in real time based on the local dynamic obstacle avoidance strategy, and constructing the motion state vector of the obstacle based on the dynamic obstacle perception data; a prediction unit for predicting the motion state vector using a spatiotemporal attention mechanism to obtain obstacle prediction trajectory data, the obstacle prediction trajectory data including a probabilistic trajectory distribution map; a reverse calculation unit for reconstructing the global path according to the probabilistic trajectory distribution map, generating a multimodal optimized path set, performing reverse calculation based on the multimodal optimized path set, and obtaining joint space control commands for the rescue robot; and a motion control command generation unit for executing the joint space control commands to perform trajectory synchronization verification, and generating the motion control commands for the rescue robot based on the verification results.

[0132] Specifically, the global path is reconstructed according to the probabilistic trajectory distribution map to generate a multimodal optimized path set. Inverse calculation is then performed based on the multimodal optimized path set to obtain the joint space control commands for the rescue robot. The inverse calculation unit may further include: an reachability analysis subunit for performing multimodal sampling search based on the probabilistic trajectory distribution map to obtain multiple optimized path sets, and performing reachability analysis based on the multiple optimized path sets to construct an executable path cluster; a path reconstruction subunit for reconstructing the global path according to the executable path cluster to obtain the multimodal optimized path set; and a path point sequence construction subunit for performing inverse motion analysis on the multimodal optimized path set to construct a path point sequence, and converting the path point sequence into the joint space control commands.

[0133] The process includes executing the joint space control commands to perform trajectory synchronization verification, and generating motion control commands for the rescue robot based on the verification results. The motion control command generation unit may further include: a feature point matching subunit for executing the joint space control commands to perform feature point matching and obtain a motion trajectory deviation value; a pose compensation subunit for performing verification analysis based on the motion trajectory deviation value, and triggering a visual servo control module to perform pose compensation for the rescue robot and generate pose compensation data when the motion trajectory deviation value exceeds a preset deviation threshold; and a motion control command construction subunit for constructing the motion control commands for the rescue robot based on the pose compensation data and the verification results.

[0134] The industrial vision-assisted rescue robot path planning system provided in this embodiment of the invention can execute the industrial vision-assisted rescue robot path planning method provided in any embodiment of the invention, and has the corresponding functional modules and beneficial effects of the execution method.

[0135] Although this application makes various references to certain modules in the system according to the embodiments of this application, any number of different modules can be used and run on user terminals and / or servers. The various units and modules included are only divided according to functional logic, but are not limited to the above division, as long as the corresponding functions can be achieved; in addition, the specific names of each functional unit are only for easy distinction between each other and are not used to limit the scope of protection of this invention.

[0136] The specific embodiments described above do not constitute a limitation on the scope of protection of this application. Those skilled in the art should understand that various modifications, combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this application should be included within the scope of protection of this application. In some cases, the actions or steps described in this application can be performed in a different order than that shown in the embodiments and still achieve the desired results. Furthermore, the processes depicted in the accompanying drawings do not necessarily require a specific or sequential order to achieve the desired results. In some embodiments, multitasking and parallel processing are also possible or may be advantageous.

Claims

1. A path planning method for rescue robots under industrial vision assistance, characterized in that, The method includes: Real-time acquisition of three-dimensional spatial data of the rescue environment, generation of dynamic environmental point cloud dataset, and construction of a three-dimensional semantic map of the rescue area based on the dynamic environmental point cloud dataset; Real-time updated thermal imaging data is retrieved and combined with the three-dimensional semantic map to perform path analysis and obtain a path planning strategy set, which includes a global path planning strategy and a local dynamic obstacle avoidance strategy. Based on the local dynamic obstacle avoidance strategy, the motion trajectory of dynamic obstacles is predicted, and the global path planning strategy is optimized based on the obstacle prediction trajectory data to generate motion control commands for the rescue robot. The process of acquiring real-time 3D spatial data of the rescue environment, generating a dynamic environmental point cloud dataset, and constructing a 3D semantic map of the rescue area based on the dynamic environmental point cloud dataset includes: A multi-modal sensor array is used to collect multi-dimensional perception data of the rescue environment in real time, generating a dynamic environmental point cloud dataset. Motion distortion correction is performed on the dynamic environment point cloud dataset to generate a corrected point cloud dataset. Multi-source data fusion is then performed on the dynamic environment point cloud dataset according to the corrected point cloud dataset to generate a semantically enhanced point cloud dataset. Based on the semantically enhanced point cloud dataset, 3D scene parsing is performed, and dynamic weight allocation is performed on the semantically enhanced point cloud dataset according to the parsing results to determine multiple dynamic weight coefficients; The multiple dynamic weight coefficients are matched with the semantically enhanced point cloud dataset to construct the three-dimensional semantic map; Specifically, 3D scene parsing is performed based on the semantically enhanced point cloud dataset, and dynamic weight allocation is performed on the semantically enhanced point cloud dataset according to the parsing results to determine multiple dynamic weight coefficients, including: Construct a multi-layered 3D convolutional neural network to perform 3D scene parsing on the semantically enhanced point cloud dataset: The input layer receives the semantically enhanced point cloud dataset; The first hidden layer extracts local geometric features, and the second hidden layer fuses temperature and material features. The output layer analyzes the data by combining local geometric features with temperature and material features through a classifier, generates point-level semantic labels, and adds the point-level semantic labels to the parsing results. The point-level semantic labels are converted into risk level parameters. The trajectory of the predicted moving obstacle is combined with the risk level parameters to dynamically update the weights of the semantically enhanced point cloud, and the multiple dynamic weight coefficients are determined.

2. The path planning method for rescue robots under industrial vision assistance as described in claim 1, characterized in that, The method for performing motion distortion correction on the dynamic environment point cloud dataset to generate a corrected point cloud dataset includes: The inertial measurement unit records the pose change data of the rescue robot in real time; The instantaneous pose data of the rescue robot at the time of acquisition of the dynamic environment point cloud dataset is calculated based on the pose change data. The dynamic environment point cloud dataset is subjected to pose compensation based on the instantaneous pose data to generate the corrected point cloud dataset.

3. The path planning method for rescue robots under industrial vision assistance as described in claim 1, characterized in that, Retrieve real-time updated thermal imaging data and combine it with the three-dimensional semantic map to perform path analysis, thereby obtaining a path planning strategy set. The path planning strategy set includes global path planning strategies and local dynamic obstacle avoidance strategies. The method includes: The thermal imaging data is spatiotemporally aligned with the three-dimensional semantic map to generate a thermal radiation risk layer. The three-dimensional semantic map is annotated based on the thermal radiation risk layer to delineate the boundaries of thermally sensitive areas. The kinematic constraints of the rescue robot are introduced, and multi-objective optimization is performed according to the kinematic constraints and the boundary of the heat-sensitive area. Based on the multi-objective optimization results, hierarchical planning is performed to generate the global path planning strategy and the local dynamic obstacle avoidance strategy.

4. The path planning method for rescue robots under industrial vision assistance as described in claim 3, characterized in that, Introducing kinematic constraints for the rescue robot, performing multi-objective optimization based on these kinematic constraints and the boundary of the heat-sensitive region, and performing hierarchical planning based on the multi-objective optimization results to generate the global path planning strategy and the local dynamic obstacle avoidance strategy, the method includes: A thermal exposure risk field is constructed based on the kinematic constraints of the rescue robot and the boundary of the thermally sensitive area. The heat exposure risk field is traversed to search for multiple candidate paths, and an evaluation is conducted based on these multiple candidate paths to generate a global path baseline. A dynamic planning window is constructed based on the global path baseline to perform local detection. When a moving obstacle is detected to have intruded into the dynamic planning window, a local emergency control command is constructed. The global path planning strategy is constructed based on the global path baseline, and the local dynamic obstacle avoidance strategy is constructed based on the local emergency control command.

5. The path planning method for rescue robots under industrial vision assistance as described in claim 1, characterized in that, The method includes predicting the trajectory of dynamic obstacles based on the local dynamic obstacle avoidance strategy, optimizing the global path planning strategy based on the obstacle prediction trajectory data, and generating motion control commands for the rescue robot. Based on the local dynamic obstacle avoidance strategy, dynamic obstacle perception data is extracted in real time, and the motion state vector of the obstacle is constructed based on the dynamic obstacle perception data. The motion state vector is predicted using a spatiotemporal attention mechanism to obtain obstacle prediction trajectory data, which includes a probabilistic trajectory distribution map. The global path is reconstructed according to the probabilistic trajectory distribution map to generate a multimodal optimized path set. Inverse calculation is performed based on the multimodal optimized path set to obtain the joint space control commands of the rescue robot. The joint space control commands are executed to perform trajectory synchronization verification, and the motion control commands for the rescue robot are generated based on the verification results.

6. The path planning method for rescue robots under industrial vision assistance as described in claim 5, characterized in that, The global path is reconstructed according to the probabilistic trajectory distribution map to generate a multimodal optimized path set. Inverse calculation is then performed based on the multimodal optimized path set to obtain the joint space control commands for the rescue robot. The method includes: Multimodal sampling search is performed based on the probabilistic trajectory distribution map to obtain multiple optimized path sets. Based on the multiple optimized path sets, reachability analysis is performed to construct an executable path cluster. The global path is reconstructed according to the executable path cluster to obtain a multimodal optimized path set; The multimodal optimized path set is subjected to inverse motion analysis to construct a path point sequence, and the path point sequence is converted into the joint space control command.

7. The path planning method for rescue robots under industrial vision assistance as described in claim 5, characterized in that, The method includes executing the joint space control commands to perform trajectory synchronization verification, and generating the motion control commands for the rescue robot based on the verification results. The joint space control command is executed to perform feature point matching and obtain the motion trajectory deviation value. Based on the motion trajectory deviation value, a verification analysis is performed. When the motion trajectory deviation value exceeds the preset deviation threshold, the visual servo control module is triggered to perform pose compensation for the rescue robot and generate pose compensation data. The motion control commands for the rescue robot are constructed based on the pose compensation data and the verification results.

8. A path planning system for rescue robots assisted by industrial vision, characterized in that, The system is used to implement the industrial vision-assisted path planning method for rescue robots according to any one of claims 1-7, the system comprising: The three-dimensional semantic map construction module is used to collect three-dimensional spatial data of the rescue environment in real time, generate a dynamic environmental point cloud dataset, and construct a three-dimensional semantic map of the rescue area based on the dynamic environmental point cloud dataset. The path planning strategy set acquisition module is used to retrieve real-time updated thermal imaging data, combine it with the three-dimensional semantic map to perform path analysis, and obtain a path planning strategy set, which includes a global path planning strategy and a local dynamic obstacle avoidance strategy. The path planning strategy optimization module is used to predict the motion trajectory of dynamic obstacles based on the local dynamic obstacle avoidance strategy, optimize the global path planning strategy based on the obstacle prediction trajectory data, and generate motion control commands for the rescue robot. The 3D semantic map construction module includes: The multi-dimensional perception data acquisition module is used to collect multi-dimensional perception data of the rescue environment in real time through a multi-modal sensor array and generate a dynamic environmental point cloud dataset. The motion distortion correction module is used to perform motion distortion correction on the dynamic environment point cloud dataset, generate a corrected point cloud dataset, and perform multi-source data fusion on the dynamic environment point cloud dataset according to the corrected point cloud dataset to generate a semantically enhanced point cloud dataset. The dynamic weight allocation module is used to perform 3D scene parsing based on the semantically enhanced point cloud dataset, and to dynamically allocate weights to the semantically enhanced point cloud dataset according to the parsing results, thereby determining multiple dynamic weight coefficients. The matching module is used to match the multiple dynamic weight coefficients with the semantically enhanced point cloud dataset to construct the three-dimensional semantic map; The dynamic weight allocation module includes: The 3D scene parsing module is used to construct a multi-layer 3D convolutional neural network to perform 3D scene parsing on the semantically enhanced point cloud dataset: the input layer receives the semantically enhanced point cloud dataset; the first hidden layer extracts local geometric features, and the second hidden layer fuses temperature and material features; the output layer analyzes the data by combining local geometric features and fused temperature and material features through a classifier, generates point-level semantic labels, and adds the point-level semantic labels to the parsing result; The weight dynamic update module is used to convert the point-level semantic labels into risk level parameters, predict the trajectory of moving obstacles, combine the risk level parameters to dynamically update the weights of the semantically enhanced point cloud, and determine the multiple dynamic weight coefficients.

Images