Robot dynamic obstacle trajectory prediction navigation method based on visual model
By using multimodal data fusion and visual model prediction to predict obstacle trajectories, the problems of unstable perception and unbalanced path planning in traditional robot navigation are solved, enabling efficient and safe navigation in complex environments.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-11-06
- Publication Date
- 2026-03-31
AI Technical Summary
Traditional robot dynamic obstacle navigation technology is susceptible to environmental interference in the perception stage, the fusion strategy cannot be dynamically adjusted, trajectory prediction and risk assessment are incomplete, and path planning is difficult to achieve a dynamic balance between efficiency and safety.
By collecting multimodal perception data, extracting thermal infrared and visual dynamic feature vectors, calculating fusion weights, and using a mutual information enhancement fusion algorithm, combined with a visual model, obstacle trajectory prediction is performed to quantify uncertainty and risk and generate obstacle avoidance paths.
It improves the robot's perception accuracy and stability in complex and dynamic scenarios, ensures that path planning is both safe and efficient, can quickly respond to scene changes, reduce collision risks, and improve navigation reliability.
Smart Images
Figure CN121761888A_ABST
Abstract
Description
Technical Field
[0001] This application relates to the field of power equipment maintenance, and in particular to a robot dynamic obstacle trajectory prediction and navigation method based on a vision model. Background Technology
[0002] With the widespread application of robotics in industrial production, logistics, service interaction, and outdoor inspection, autonomous navigation in dynamic scenarios has become one of the core indicators for measuring the intelligence level of robots. In dynamic environments, robots need to perceive the movement status of surrounding obstacles in real time, accurately predict their future trajectories, and plan safe and efficient obstacle avoidance paths accordingly. This process places extremely high demands on the coordination of multimodal perception, feature fusion, trajectory prediction, and risk decision-making. Currently, multi-sensor fusion technology is gradually becoming the mainstream way for robots to perceive dynamic environments. By combining the advantages of different sensors, the perception limitations of a single sensor in complex environments can be compensated for. At the same time, the development of trajectory prediction and risk assessment technologies has also provided key support for robots to cope with dynamic obstacles. However, existing technologies still need further breakthroughs in adapting to dynamic scene changes, improving perception robustness, and optimizing decision-making safety to meet the needs of practical applications.
[0003] However, traditional robot dynamic obstacle navigation technology has many shortcomings in practical applications. In the perception stage, some technologies rely on single-modal sensors, which are easily affected by environmental factors. For example, visual perception becomes less accurate in low-light or blurry scenes, and thermal infrared perception is prone to misjudgment in complex temperature fields. Even when multimodal fusion is used, fixed weight allocation is often adopted, which cannot adjust the fusion strategy in real time according to the dynamic complexity of the scene and image quality. As a result, the fused features cannot accurately reflect the real state of the scene. In the trajectory prediction and risk assessment stages, traditional methods mostly use deterministic models, which fail to effectively quantify the uncertainty in the prediction process. Moreover, risk assessment often only considers single factors such as obstacle distance, ignoring key indicators such as obstacle threat level and motion mutability, resulting in an incomplete risk assessment. In the path planning stage, some algorithms do not fully incorporate real-time risk coefficients, making it difficult to achieve a dynamic balance between efficiency and safety, and easily leading to problems such as path planning lag or insufficient safety redundancy. Summary of the Invention
[0004] The purpose of this invention is to provide a robot dynamic obstacle trajectory prediction and navigation method based on a visual model, in order to solve the problem that traditional robot dynamic obstacle navigation technology is difficult to achieve a dynamic balance between efficiency and safety, and is prone to path planning lag or insufficient safety redundancy.
[0005] The above-mentioned objective of this application is achieved through the following technical solution: S1: Collect raw data of multimodal perception of dynamic scenes and perform spatiotemporal alignment, extract thermal infrared dynamic feature vectors and visual dynamic feature vectors, and calculate visual image blur and scene dynamic complexity at the same time. S2: Based on thermal infrared feature vectors, visual feature vectors, visual image blurring, and scene dynamic complexity, the fusion weights of thermal infrared features and visual features are calculated using a dynamic modal weighting algorithm; the thermal infrared features and visual features are fused using a mutual information enhancement fusion algorithm, and the fused feature vector is output. S3: Input the fused feature vector into the visual model to calculate the trajectory probability distribution of dynamic obstacles; based on the trajectory probability distribution, calculate the comprehensive uncertainty through a composite uncertainty quantification algorithm, and then calculate the risk coefficient through a dynamic risk coupling algorithm; S4: Based on the fusion of feature vectors and trajectory probability distributions, combined with uncertainty and risk coefficients, key feature heatmaps, causal relationship rules and counterfactual explanations are generated. The obstacle avoidance path is planned through the A algorithm, and a decision report containing risk coefficients, heatmaps and explanatory text is output to realize the robot's dynamic navigation.
[0006] Optionally, step S1 includes: The robot synchronously collects raw data for multimodal perception of dynamic scenes using a multi-sensor kit. The multi-sensor kit includes: a thermal infrared camera, a vision camera, a light sensor, a lidar, and a wheel speed odometer; The raw data includes: thermal infrared image sequences acquired by thermal infrared cameras, RGB image sequences acquired by visual cameras, light intensity data acquired by light sensors, obstacle point cloud data acquired by lidar, and robot motion distance data acquired by wheel speed odometers.
[0007] Optionally, step S1 may further include: The method for extracting the dynamic feature vector of thermal infrared radiation is as follows: The thermal infrared image sequence is input into a spatiotemporal convolutional network with 3 to 5 3D convolutional layers. The kernel size of each convolutional layer is 3×3×3 to 5×5×5, and the stride is 1 to 2. After batch normalization and ReLU activation, the output features are subjected to global average pooling to obtain a 128 to 256-dimensional thermal infrared dynamic feature vector. The method for extracting visual dynamic feature vectors is as follows: RGB image sequences are input into an optical flow network to obtain inter-frame optical flow fields. Simultaneously, they are input into a target detection model to obtain obstacle bounding boxes and velocity and acceleration parameters. The motion vectors of obstacle regions in the optical flow field are subjected to mean pooling, concatenated with obstacle motion parameters, and mapped through a fully connected layer to obtain visual dynamic feature vectors of 128 to 256 dimensions.
[0008] Optionally, step S1 includes: The formula for calculating visual image blur is:
[0009] in, For visual image blur, The gradient matrix is calculated using the Sobel operator after converting an RGB image to grayscale, retaining only the gradient values. elements, For gradient threshold, for Information entropy For image resolution, The gradient matrix represents the theoretical maximum information entropy. The formula for calculating the dynamic complexity of a scene is:
[0010] in, For scene dynamic complexity, This represents the number of dynamic obstacles in the current frame. This represents the total number of obstacles in the current frame. The coefficient of variation of the velocity of the dynamic obstacle. These are the weighting coefficients; The obstacle point cloud data includes: the number of dynamic obstacles and the total number of obstacles.
[0011] Optionally, step S2 includes: The expression for the dynamic modal weighting algorithm is:
[0012]
[0013] in, For thermal infrared feature weights, For visual feature weights, For thermal infrared mode reliability, For visual modal reliability, Light intensity, For visual image blur, For scene dynamic complexity, To adjust the parameters.
[0014] Optionally, step S2 may further include: In dynamic multimodal feature fusion, the expression for the mutual information-enhanced fusion algorithm is:
[0015] in, To fuse feature vectors, For thermal infrared feature vectors, For visual feature vectors, For the mutual information gain of thermal infrared on vision, For visual mutual information gain to thermal infrared, For thermal infrared feature weights, For visual feature weights.
[0016] Optionally, step S3 includes: A visual model is constructed based on a Long Short-Term Memory (LSTM) network, which consists of one input layer, two to three LSTM hidden layers, and one output layer. The LSTM hidden layer introduces the Monte Carlo dropout mechanism; The network weights of the visual model follow a Gaussian prior distribution; The output layer is a fully connected layer that outputs the probability distribution parameters of the trajectory coordinates of the dynamic obstacle at a preset number of future time points, including the mean vector of the (x, y) coordinates of the trajectory point at each time point and a 2×2 covariance matrix. During model training, the optimization objective is to maximize the lower bound of evidence (ELBO), and the Adam optimizer is used. During the inference phase, the LSTM hidden layer is sampled a preset number of times using Monte Carlo sampling. Based on the sampling results, the final trajectory probability distribution is obtained by fitting. The expression for the composite uncertainty quantization algorithm is:
[0017] in, To account for uncertainties, To predict the trajectory distribution entropy, For the sudden change in the motion of obstacles, These are weight parameters; The expression for the dynamic risk coupling algorithm is:
[0018] in, For risk coefficient, To account for uncertainties, For obstacle threat level, The real-time distance between the robot and the obstacle. This serves as the baseline safety distance.
[0019] Optionally, step S4 includes: The specific steps for planning an obstacle avoidance path using Algorithm A are as follows: Initialization: Construct open and closed lists, add the starting point to the open list, and calculate the cumulative movement cost of the starting point. Heuristic Costs and total cost The formula is: ; Node selection: Select from the open list for total cost The smallest node is set as the current node, and it is moved to the closed list. Target determination: If the current node is the target point, backtrack along the parent node to generate the initial path and end the search; Neighborhood expansion: Generate neighboring nodes of the current node, filtering out nodes that are outside the environment boundary, located in obstacle areas, or already in the closed list; Cost update: Calculate the cumulative movement cost of adjacent nodes. Heuristic Costs and total cost If the node is not in the open list or is new If the value is smaller, update its cost and parent node and add it to the open list; Loop Search: Return to the node selection step until the target point is selected from the open list, and output the obstacle avoidance path.
[0020] An electronic device includes a processor, a memory, a user interface, and a network interface. The memory is used to store instructions, the user interface and the network interface are used to communicate with other devices, and the processor is used to execute the instructions stored in the memory to enable the electronic device to perform a vision model-based robot dynamic obstacle trajectory prediction and navigation method.
[0021] A computer-readable storage medium storing instructions that, when executed, perform a robot dynamic obstacle trajectory prediction and navigation method based on a vision model.
[0022] The beneficial effects of the technical solution provided in this application are: By synchronously acquiring raw multimodal perception data of dynamic scenes and completing spatiotemporal alignment, thermal infrared and visual dynamic feature vectors are extracted. Simultaneously considering visual image ambiguity and scene dynamic complexity, a dynamic modal weighting algorithm is used to calculate the dual-modal fusion weights. Then, a mutual information enhancement fusion algorithm is employed to achieve deep feature fusion. This process overcomes the limitations of single-modal perception, adaptively adjusting modal weights based on scene dynamics and image quality. It reduces the interference of environmental factors on the perception results, making the output fused feature vectors more comprehensively reflect scene dynamic information. This provides high-quality data support for subsequent obstacle trajectory prediction, improves the robot's perception accuracy and stability in complex dynamic scenes, avoids perception bias caused by single-modal failure, and enhances adaptability to diverse dynamic environments.
[0023] By inputting fused feature vectors into a visual model to obtain the probability distribution of future obstacle trajectories, combining a composite uncertainty quantification algorithm to calculate the comprehensive uncertainty, and then using a dynamic risk coupling algorithm to obtain the risk coefficient, the obstacle avoidance path planning is optimized based on the A algorithm and a decision report containing key information is generated. Combined with dynamic closed-loop navigation control to periodically update navigation parameters, this design achieves coordinated optimization of trajectory prediction, risk assessment, and path planning. It can accurately quantify the uncertainty and obstacle risk in trajectory prediction, ensuring that path planning is both efficient and safe. Furthermore, by dynamically adjusting the navigation strategy through real-time data updates, the robot can quickly respond to scene changes, effectively reduce collision risks, improve the reliability and intelligence level of the navigation process, and meet the navigation needs in complex dynamic environments. Attached Figure Description
[0024] The present application will be further described below with reference to the accompanying drawings and embodiments. In the accompanying drawings: Figure 1 This is a step diagram of an embodiment of this application; Figure 2 This is a schematic diagram of the electronic device structure in the embodiments of this application. Detailed Implementation
[0025] To provide a clearer understanding of the technical features, objectives, and effects of this application, the specific embodiments of this application will now be described in detail with reference to the accompanying drawings.
[0026] The embodiments of this application provide a robot dynamic obstacle trajectory prediction and navigation method based on a visual model.
[0027] Please refer to Figure 1 , Figure 1 This is a flowchart illustrating the steps of a robot dynamic obstacle trajectory prediction and navigation method based on a vision model, as described in an embodiment of this application, including: S1: Collect raw data of multimodal perception of dynamic scenes and perform spatiotemporal alignment, extract thermal infrared dynamic feature vectors and visual dynamic feature vectors, and calculate visual image blur and scene dynamic complexity at the same time. S2: Based on thermal infrared feature vectors, visual feature vectors, visual image blurring, and scene dynamic complexity, the fusion weights of thermal infrared features and visual features are calculated using a dynamic modal weighting algorithm; the thermal infrared features and visual features are fused using a mutual information enhancement fusion algorithm, and the fused feature vector is output. S3: Input the fused feature vector into the visual model to calculate the trajectory probability distribution of dynamic obstacles; based on the trajectory probability distribution, calculate the comprehensive uncertainty through a composite uncertainty quantification algorithm, and then calculate the risk coefficient through a dynamic risk coupling algorithm; S4: Based on the fusion of feature vectors and trajectory probability distributions, combined with uncertainty and risk coefficients, key feature heatmaps, causal relationship rules and counterfactual explanations are generated. The obstacle avoidance path is planned through the A algorithm, and a decision report containing risk coefficients, heatmaps and explanatory text is output to realize the robot's dynamic navigation.
[0028] This application provides an embodiment as follows: dynamic closed-loop navigation control: the robot executes the output obstacle avoidance path, and the path command is converted into motion control signal through the actuator; S100 is repeated according to a preset cycle, dynamic multi-mode data is collected to S400, dynamic risk decision is generated, and dynamic navigation is realized by updating features, weights, uncertainties, risk coefficients and paths based on real-time data.
[0029] As one embodiment, dynamic scene data is collected through a multi-sensor suite, thermal infrared and visual dynamic feature vectors are extracted, and image blur and scene complexity are calculated; the features are processed using a dynamic modal weight and mutual information enhancement fusion algorithm, and input into an LSTM model to predict the probability distribution of obstacle trajectories; the risk is assessed through a comprehensive uncertainty and risk coupling algorithm, and obstacle avoidance paths and decision reports are generated to achieve dynamic closed-loop navigation control of the robot. Step S1 includes: The robot synchronously collects raw data for multimodal perception of dynamic scenes using a multi-sensor kit. The multi-sensor kit includes: a thermal infrared camera, a vision camera, a light sensor, a lidar, and a wheel speed odometer; The raw data includes: thermal infrared image sequences acquired by thermal infrared cameras, RGB image sequences acquired by visual cameras, light intensity data acquired by light sensors, obstacle point cloud data acquired by lidar, and robot motion distance data acquired by wheel speed odometers.
[0030] Step S1 also includes: The method for extracting the dynamic feature vector of thermal infrared radiation is as follows: The thermal infrared image sequence is input into a spatiotemporal convolutional network with 3 to 5 3D convolutional layers. The kernel size of each convolutional layer is 3×3×3 to 5×5×5, and the stride is 1 to 2. After batch normalization and ReLU activation, the output features are subjected to global average pooling to obtain a 128 to 256-dimensional thermal infrared dynamic feature vector. The method for extracting visual dynamic feature vectors is as follows: RGB image sequences are input into an optical flow network to obtain inter-frame optical flow fields. Simultaneously, they are input into a target detection model to obtain obstacle bounding boxes and velocity and acceleration parameters. The motion vectors of obstacle regions in the optical flow field are subjected to mean pooling, concatenated with obstacle motion parameters, and mapped through a fully connected layer to obtain visual dynamic feature vectors of 128 to 256 dimensions.
[0031] Step S1 includes: The formula for calculating visual image blur is:
[0032] in, For visual image blur, The gradient matrix is calculated using the Sobel operator after converting an RGB image to grayscale, retaining only the gradient values. elements, For gradient threshold, for Information entropy For image resolution, The gradient matrix represents the theoretical maximum information entropy. The formula for calculating the dynamic complexity of a scene is:
[0033] in, For scene dynamic complexity, This represents the number of dynamic obstacles in the current frame. This represents the total number of obstacles in the current frame. The coefficient of variation of the velocity of the dynamic obstacle. These are the weighting coefficients; The obstacle point cloud data includes: the number of dynamic obstacles and the total number of obstacles.
[0034] Step S2 includes: The expression for the dynamic modal weighting algorithm is:
[0035]
[0036] in, For thermal infrared feature weights, For visual feature weights, For thermal infrared mode reliability, For visual modal reliability, Light intensity, For visual image blur, For scene dynamic complexity, To adjust the parameters.
[0037] Step S2 also includes: In dynamic multimodal feature fusion, the expression for the mutual information-enhanced fusion algorithm is:
[0038] in, To fuse feature vectors, For thermal infrared feature vectors, For visual feature vectors, For the mutual information gain of thermal infrared on vision, For visual mutual information gain to thermal infrared, For thermal infrared feature weights, For visual feature weights.
[0039] Step S3 includes: A visual model is constructed based on a Long Short-Term Memory (LSTM) network, which consists of one input layer, two to three LSTM hidden layers, and one output layer. This application provides an embodiment as follows: the input layer receives a sequence of fused feature vectors from 10 to 20 consecutive frames, with the dimension of a single frame's fused feature vector being 128 to 256 dimensions. The hidden state dimension of the LSTM hidden layer is 256 to 512 dimensions.
[0040] The LSTM hidden layer introduces the Monte Carlo dropout mechanism; The network weights of the visual model follow a Gaussian prior distribution; The output layer is a fully connected layer that outputs the probability distribution parameters of the trajectory coordinates of the dynamic obstacle at a preset number of future time points, including the mean vector of the (x, y) coordinates of the trajectory point at each time point and a 2×2 covariance matrix. During model training, the optimization objective is to maximize the lower bound of evidence (ELBO), and the Adam optimizer is used. During the inference phase, the LSTM hidden layer is sampled a preset number of times using Monte Carlo sampling. Based on the sampling results, the final trajectory probability distribution is obtained by fitting. The expression for the composite uncertainty quantization algorithm is:
[0041] in, To account for uncertainties, To predict the trajectory distribution entropy, For the sudden change in the motion of obstacles, These are weight parameters; The expression for the dynamic risk coupling algorithm is:
[0042] in, For risk coefficient, To account for uncertainties, For obstacle threat level, The real-time distance between the robot and the obstacle. This serves as the baseline safety distance.
[0043] Step S4 includes: The specific steps for planning an obstacle avoidance path using Algorithm A are as follows: Initialization: Construct open and closed lists, add the starting point to the open list, and calculate the cumulative movement cost of the starting point. Heuristic Costs and total cost The formula is: ; Node selection: Select from the open list for total cost The smallest node is set as the current node, and it is moved to the closed list. Target determination: If the current node is the target point, backtrack along the parent node to generate the initial path and end the search; Neighborhood expansion: Generate neighboring nodes of the current node, filtering out nodes that are outside the environment boundary, located in obstacle areas, or already in the closed list; Cost update: Calculate the cumulative movement cost of adjacent nodes. Heuristic Costs and total cost If the node is not in the open list or is new If the value is smaller, update its cost and parent node and add it to the open list; Loop Search: Return to the node selection step until the target point is selected from the open list, and output the obstacle avoidance path.
[0044] Example 1: Dynamic navigation scenario of indoor intelligent warehousing robot.
[0045] Scenario Background: This embodiment is designed for a large-scale e-commerce indoor warehouse environment. The warehouse space contains multiple AGV handling robots, warehouse staff, and fixed shelves. Ambient lighting is provided by overhead LED lights. Some areas have uneven lighting due to the obstruction of shelves. The robots need to complete the task of transferring goods from "inbound shelf A" to "outbound sorting station B". Throughout the process, they need to avoid dynamic obstacles in real time to ensure transportation efficiency and safety.
[0046] S100, Dynamic Multi-Mode Data Acquisition: The robot is equipped with a multi-sensor suite consisting of a thermal infrared camera, a vision camera, a light sensor, a lidar, and a wheel speed odometer. It simultaneously collects multimodal perception raw data from the warehouse scene and performs spatiotemporal alignment, providing comprehensive and synchronous raw data support for subsequent feature extraction and risk assessment. Multi-source data acquisition: Thermal infrared cameras continuously acquire thermal infrared image sequences of shelves, AGVs, and workers, clearly distinguishing different types of dynamic obstacles through differences in thermal radiation, unaffected by localized lighting obstruction; visual cameras acquire RGB image sequences of the scene, capturing the appearance details and color features of obstacles to assist in accurate obstacle type identification; illumination sensors collect ambient light intensity in real time, providing a basis for subsequent judgment of visual data reliability and adjustment of modal weights; LiDAR scanning acquires 3D point cloud data of AGVs, workers, and shelves, accurately obtaining the spatial position and contour information of obstacles, compensating for the shortcomings of vision and thermal infrared in distance measurement; wheel speed odometers record the robot's own motion mileage data, enabling real-time calibration of the robot's position to ensure that subsequent path planning matches actual movement.
[0047] Feature vector extraction: The thermal infrared image sequence is input into a spatiotemporal convolutional network with four 3D convolutional layers. After batch normalization and ReLU activation, global average pooling is performed on the output features to obtain a 200-dimensional thermal infrared dynamic feature vector. This vector can effectively preserve the thermal radiation variation law of dynamic obstacles and can stably reflect the movement trend of obstacles even in low-light areas. The RGB image sequence is input into an optical flow network to obtain the inter-frame optical flow field. At the same time, it is input into the target detection model to obtain the bounding boxes of AGV and workers and the corresponding velocity and acceleration parameters. The motion vectors of AGV and worker regions in the optical flow field are average pooled and concatenated with the above motion parameters. After mapping through a fully connected layer, a 200-dimensional visual dynamic feature vector is obtained. This vector can combine the appearance motion and physical motion parameters of obstacles to accurately depict the real-time motion state of obstacles.
[0048] S200, Dynamic Multimodal Feature Fusion: By dynamically adjusting modal weights and enhancing modal complementarity, more comprehensive and reliable fusion features are generated, providing high-quality input data for subsequent trajectory prediction. For example, in low-light areas obscured by shelves, the low light intensity leads to increased visual image blur and decreased visual modality reliability. The thermal infrared modality, being unaffected by light, has significantly improved reliability. The algorithm adjusts parameters to make the weight of thermal infrared features higher than that of visual features, ensuring that the more reliable thermal infrared features are used preferentially. In high-light areas without obstruction, the visual image is clear and its reliability is improved. The algorithm automatically increases the weight of visual features, making full use of the advantages of visual features in detail depiction.
[0049] Dual-modal feature fusion: By using a mutual information enhancement fusion algorithm, the mutual information gain between thermal infrared and visual modes is calculated, and the mutual information gain between visual and thermal infrared is calculated, thus strengthening the complementary information between the two modes. Thermal infrared features supplement the motion trend information of visual mode in low light, and visual features supplement the detailed information of thermal infrared mode in obstacle type differentiation. Finally, a 200-dimensional fusion feature vector is output. This vector integrates the advantages of the two modes, effectively reduces the limitations of single-modal data, and improves the accuracy of subsequent trajectory prediction.
[0050] S300, Dynamic Trajectory Quantization Prediction: By quantifying the future trajectory and risk of obstacles, a clear risk orientation is provided for subsequent path planning, avoiding untimely obstacle avoidance due to fuzzy trajectory prediction. Trajectory probability distribution calculation: A sequence of fused feature vectors from 15 consecutive frames is input into a Long Short-Term Memory (LSTM) network. The hidden state dimension of the LSTM hidden layer is 384-dimensional, and a Monte Carlo dropout mechanism is introduced in each layer. The network weights follow a Gaussian prior distribution, which can effectively handle the temporal correlation of the sequence data and quantify the prediction uncertainty. The output layer is a fully connected layer, which outputs the trajectory coordinate probability distribution parameters of the AGV and the worker at the next 8 time steps. It can clearly show the possible range of the future position of the obstacle. The trajectory probability distribution of the AGV is more concentrated due to its more stable motion pattern, while that of the worker is more dispersed due to its higher randomness. The network training phase aims to maximize the lower bound of evidence (ELBO) using the Adam optimizer, with a training batch size of 48 and 100 iterations to ensure that the network has accurate trajectory prediction capabilities. In the inference phase, the LSTM hidden layer is sampled 30 times using Monte Carlo sampling. The final trajectory probability distribution is fitted based on the sampling results to further reduce the prediction error.
[0051] Comprehensive Uncertainty Calculation: Based on the above trajectory probability distribution and the movement data of AGV and staff, a composite uncertainty quantification algorithm is used to calculate the comprehensive uncertainty. The algorithm combines the predicted trajectory distribution entropy and the obstacle movement abrupt change, and balances the two through weight parameters. The predicted trajectory distribution entropy reflects the dispersion of trajectory prediction, while the obstacle movement abrupt change reflects the possibility of the obstacle suddenly changing its movement state. Because staff are more flexible in their movement, both values are higher than those of AGV. The final comprehensive uncertainty can fully reflect the unreliability of the trajectory prediction results and provide uncertainty basis for subsequent risk assessment.
[0052] Risk coefficient calculation: The dynamic risk coupling algorithm is used to calculate the risk coefficient. The algorithm is based on comprehensive uncertainty, combined with the obstacle threat level, the real-time distance between the robot and the obstacle and the baseline safety distance. AGVs have a higher threat level than human workers due to their large weight and high speed. The closer the robot is to the obstacle, the higher the collision risk. Finally, the risk coefficient corresponding to each dynamic obstacle is output. The risk coefficient of a close AGV may reach 0.8, while the risk coefficient of a distant human worker may be 0.3. This coefficient can intuitively quantify the collision risk of different obstacles, which is convenient for prioritizing the avoidance of high-risk targets in the future.
[0053] S400, Dynamic Risk Decision Generation: It generates intuitive auxiliary information and safe paths, providing robots with clear navigation decision-making basis and ensuring that path planning is both efficient and risk-avoiding. Decision Support Information Generation: Based on fused feature vectors, trajectory probability distribution, comprehensive uncertainty, and risk coefficients, three types of auxiliary information are generated. A key feature heatmap uses varying shades of color to mark high-risk areas within the warehouse space; for example, frequently traversed AGV lanes are marked in dark red, helping robots quickly locate dangerous areas. Causal association rules clarify the conditions for risk generation, such as "AGV speed > 1 m / s and distance to robot < 2 m → risk coefficient > 0.6," facilitating understanding of the risk source. Counterfactual explanations provide feasible solutions to reduce risk, such as "If the AGV decelerates to 0.8 m / s, its risk coefficient will drop below 0.5," providing a reference direction for subsequent path adjustments.
[0054] Obstacle avoidance path planning: Algorithm A is used to plan the obstacle avoidance path from "shelf A in the inbound area" to "sorting station B in the outbound area". The specific execution process includes: initializing the open and closed lists, adding the starting point "robot's initial position next to shelf A" to the open list, calculating the cumulative movement cost g, heuristic cost h, and total cost f = f + g of the starting point. The cumulative movement cost reflects the actual distance from the starting point to the current node, the heuristic cost reflects the estimated distance from the current node to the target point, and the total cost ensures that the path balances distance and efficiency; selecting the node with the smallest total cost f from the open list as the current node and moving it to the closed list, prioritizing nodes that are closer to the target and have lower costs. The path direction is determined; it is determined whether the current node is the target point "sorting station B". If not, neighboring nodes of the current node are generated, and nodes that exceed the storage boundary, are located in the shelf area, or are already in the closed list are filtered to avoid the path crossing obstacles or exceeding the working range; the g, h, and f values of neighboring nodes are calculated. If the node is not in the open list or the new g value is smaller, its cost and parent node are updated and added to the open list to ensure that a better path can be found; the above steps of node selection, target determination, neighborhood expansion, and cost update are repeated until the target point is selected. After generating the initial path, the obstacle avoidance path is output. This path can avoid high-risk obstacles while maintaining a shorter transportation distance.
[0055] Decision report output: The final output includes the risk coefficient of each dynamic obstacle, heatmap of key features, and counterfactual explanation text. It integrates all key decision information to provide an intuitive and comprehensive reference for the robot to perform navigation tasks, and avoids decision-making errors caused by fragmented information.
[0056] S500, Dynamic Closed-Loop Navigation Control: A closed loop of path execution and real-time adjustment is achieved to ensure the robot's continuous and safe navigation in dynamic environments and avoid collisions caused by environmental changes. Based on the output obstacle avoidance path, the robot converts the path instructions into motion control signals through its own actuators, ensuring that the robot can move accurately along the planned path. At the same time, it repeats the process from S100 to S400 according to a preset cycle. Based on the real-time collected data on the position changes of AGVs and workers, it updates the multimodal features, fusion weights, comprehensive uncertainty, risk coefficients, and obstacle avoidance paths in a timely manner. For example, when an AGV suddenly changes its direction of movement, the system can recalculate its risk coefficient and adjust its path within 1 second to avoid collisions caused by information lag, ensuring that the robot can continuously and safely complete cargo transfer tasks in a dynamically changing warehouse environment.
[0057] Example 2: Outdoor park patrol robot dynamic navigation scenario Scenario Background: This embodiment is for an outdoor campus of a university. The campus contains pedestrians, bicycles, small motor vehicles, trees, streetlights, and benches. The ambient light changes with the weather. The robot needs to perform patrol tasks according to a preset patrol route and avoid dynamic obstacles in real time, adapting to changes in outdoor light and obstacle movement.
[0058] S100, Dynamic Multi-Mode Data Acquisition: By simultaneously collecting outdoor scene data through multiple sensors, the effects of varying outdoor lighting and diverse obstacle types are overcome, providing comprehensive and reliable raw data for subsequent processing. Multi-source data acquisition: Thermal infrared cameras continuously acquire thermal infrared image sequences of pedestrians, bicycles, and motor vehicles; visual cameras acquire RGB image sequences of the park; light sensors acquire outdoor light intensity in real time, providing key basis for judging the reliability of visual data and adjusting modal weights; LiDAR scanning acquires 3D point cloud data of pedestrians, bicycles, motor vehicles, trees, and streetlights; wheel speed odometers record the patrol robot's movement mileage data, calibrate the robot's own position in real time, and ensure that subsequent path planning matches the actual patrol route.
[0059] Feature vector extraction: The thermal infrared image sequence is input into a spatiotemporal convolutional network with three 3D convolutional layers. After batch normalization and ReLU activation, global average pooling is performed on the output features to obtain a 128-dimensional thermal infrared dynamic feature vector. This vector can preserve the thermal radiation motion law of dynamic obstacles and can still stably reflect the movement trend of obstacles in low light conditions at dusk, unaffected by changes in light. The RGB image sequence is input into an optical flow network to obtain the inter-frame optical flow field. At the same time, it is input into the target detection model to obtain the bounding boxes of pedestrians, bicycles, and motor vehicles and their corresponding velocity and acceleration parameters. The motion vectors of the above-mentioned dynamic obstacle regions in the optical flow field are average pooled and concatenated with the motion parameters. After mapping through a fully connected layer, a 128-dimensional visual dynamic feature vector is obtained. This vector, combined with the appearance motion and physical motion parameters of the obstacle, can accurately distinguish the motion differences of different types of obstacles, providing categorized motion features for subsequent trajectory prediction.
[0060] S200, Dynamic Multimodal Feature Fusion: By dynamically adjusting weights and enhancing complementary information, fused features adapted to changes in outdoor lighting are generated, improving the stability and accuracy of subsequent trajectory predictions. Fusion Weight Calculation: Based on thermal infrared feature vectors, visual feature vectors, visual image blurriness, and scene dynamic complexity, a dynamic modal weighting algorithm is used to calculate the fusion weights. For example, in the evening, the light intensity is low, the visual image is blurry, and the reliability of the visual modality decreases. The reliability of the thermal infrared modality is significantly improved because it is not affected by light. At this time, the algorithm adjusts the parameters to make the thermal infrared feature weights significantly higher than the visual feature weights. On a sunny day at noon, the light is sufficient, the visual image is clear, and the visual modality can accurately distinguish the type of obstacle. The algorithm automatically increases the visual feature weights to make them close to or even higher than the thermal infrared feature weights, ensuring that the more reliable modal features are preferentially used under different lighting conditions.
[0061] Dual-modal feature fusion: The mutual information enhancement fusion algorithm is used to fuse thermal infrared and visual feature vectors. The algorithm strengthens the complementary advantages of the two modalities by calculating the mutual information gain of thermal infrared to vision and the mutual information gain of vision to thermal infrared. Thermal infrared features supplement the motion trend information of vision in low light and hazy weather, avoiding motion judgment errors caused by blurred visual data. Visual features supplement the shortcomings of thermal infrared in obstacle type differentiation, and can accurately distinguish the motion differences between bicycles and motor vehicles. Finally, a 128-dimensional fused feature vector is output. This vector integrates the advantages of the two modalities, effectively copes with the problems of outdoor light changes and diverse obstacle types, and provides high-quality input for subsequent trajectory prediction.
[0062] S300, Dynamic Trajectory Quantization Prediction: Quantifying the future trajectories and risks of different types of outdoor obstacles provides a clear risk basis for adjusting patrol routes, ensuring the avoidance of high-risk targets in complex outdoor environments: Trajectory probability distribution calculation: A sequence of fused feature vectors from 10 consecutive frames is input into an LSTM network. The hidden state dimension of the LSTM hidden layer is 256-dimensional, and a Monte Carlo dropout mechanism is introduced in each layer. The network weights follow a Gaussian prior distribution, which can effectively handle the temporal correlation of outdoor sequence data and quantify prediction uncertainty. The output layer is a fully connected layer, which outputs the trajectory coordinate probability distribution parameters of pedestrians, bicycles, and motor vehicles at the next 5 time points. Motor vehicles have more stable motion patterns due to road constraints, and their trajectory probability distribution is more concentrated. Pedestrians have a more dispersed distribution due to their higher randomness in motion. This parameter can clearly show the probability range of the future positions of different obstacles. During the network training phase, the goal is to maximize ELBO. The Adam optimizer is used, with a training batch size of 32 and 80 iterations to ensure that the network can accurately predict the trajectories of different types of obstacles outdoors. During the inference phase, the LSTM hidden layer is sampled 10 times using Monte Carlo sampling to fit the final trajectory probability distribution, further reducing the prediction error in complex outdoor environments.
[0063] Comprehensive Uncertainty Calculation: A composite uncertainty quantification algorithm is used to calculate the comprehensive uncertainty, combining the predicted trajectory distribution entropy and the abrupt change in obstacle movement. The two are balanced by weighting parameters. Pedestrian movement has high randomness, and the predicted trajectory distribution entropy is higher than that of motor vehicles. Bicycles may suddenly change lanes, and their movement abrupt change is higher than that of motor vehicles. During peak school hours, due to the density of obstacles and chaotic movement, both values increase. The final comprehensive uncertainty can fully reflect the unreliability of trajectory prediction results in outdoor environments and provide uncertainty reference for subsequent risk assessment.
[0064] Risk coefficient calculation: The risk coefficient is calculated using a dynamic risk coupling algorithm. Based on comprehensive uncertainty, it combines the obstacle threat level, the real-time distance between the robot and the obstacle, and the baseline safe distance. Motor vehicles pose the highest threat due to their high speed and large size, followed by bicycles, and pedestrians pose the lowest threat. The closer the robot is to the obstacle, the higher the collision risk. For example, the risk is higher when a motor vehicle is 5m away from the robot than when it is 10m away. The final output is the risk coefficient of each dynamic obstacle. The risk coefficient of a close motor vehicle can reach 0.9, while the risk coefficient of a distant pedestrian may be 0.1. This coefficient can intuitively quantify the collision risk of different obstacles outdoors, making it easier to prioritize the avoidance of high-risk targets in the future.
[0065] S400, Dynamic Risk Decision Generation: Generate auxiliary information and obstacle avoidance paths adapted to outdoor patrol needs, ensuring that the robot patrols along a preset route while avoiding dynamic obstacles: Decision-Making Aid Information Generation: Based on fused feature vectors, trajectory probability distribution, comprehensive uncertainty, and risk coefficients, three types of information are generated. Key feature heatmaps mark high-risk areas within the park, such as the parking area for motor vehicles at the entrance of the teaching building and the bicycle lane next to the playground, which are marked in dark red. This helps the robot quickly identify dangerous areas during patrols and adjust its movement direction in advance. Causal association rules clarify the conditions for the occurrence of risks, such as "motor vehicle speed > 15km / h and distance from the robot < 4m → risk coefficient > 0.7", which facilitates understanding the constituent factors of high-risk outdoor scenarios. Counterfactual explanations provide feasible solutions to reduce risks, such as "if the robot avoids to the west by 1m, the real-time distance from the motor vehicle increases to 6m, and the risk coefficient will drop below 0.4", providing specific directions for patrol path adjustments.
[0066] Obstacle avoidance path planning: Algorithm A is used to plan the obstacle avoidance path along the preset patrol route. The process includes: initializing the open list and the closed list; adding the starting point "the robot's initial position next to the school gate" to the open list; calculating the cumulative movement cost g, the heuristic cost h, and the total cost f. The cumulative movement cost reflects the actual patrol distance from the starting point to the current node, the heuristic cost reflects the estimated distance from the current node to the next patrol point, and the total cost ensures that the path balances patrol efficiency and safety; selecting the node with the smallest f in the open list as the current node and moving it to the closed list; prioritizing the path that is closer to the next patrol point and has a lower cost. The process involves several steps: First, determining if the current node is the next patrol point. If not, generating adjacent nodes and filtering out nodes that are outside the park boundary, located in tree / streetlight areas, or already in the closed list to prevent the path from crossing static obstacles or exceeding the patrol range. Second, calculating the g, h, and f values of adjacent nodes. If a node is not in the open list or its new g value is smaller, updating its cost and its parent node and adding it to the open list ensures that a better obstacle avoidance path can be found. Third, repeating the above steps until the target patrol point is reached, generating an obstacle avoidance path, such as avoiding dense pedestrians or fast-moving bicycles at the entrance of the teaching building, to ensure that the patrol path is safe and does not deviate from the preset route.
[0067] Decision report output: Outputs a decision report containing risk coefficients for each dynamic obstacle, heatmaps of key features, and explanatory text. It integrates key decision information required for outdoor patrols, providing a clear basis for the robot to perform patrol tasks and avoiding decision confusion caused by complex outdoor environments.
[0068] S500, Dynamic Closed-Loop Navigation Control: To enable dynamic adjustments to outdoor patrols, ensuring the robot continues to patrol safely amidst environmental changes and adapting to the randomness of outdoor obstacle movement: The robot converts obstacle avoidance path instructions into motion control signals through its actuators, and moves stably along a preset patrol route. At the same time, it repeats the process from S100 to S400 according to a preset cycle. Based on real-time data on the position changes of pedestrians, bicycles, and motor vehicles, it updates multimodal features, fusion weights, comprehensive uncertainty, risk coefficients, and obstacle avoidance paths in a timely manner. For example, when a student suddenly crosses the patrol route from the entrance of the teaching building, the system can recalculate the student's risk coefficient and adjust the path within 2 seconds to ensure that the continuity of the patrol mission is not affected while avoiding collisions. This allows the robot to continuously complete safety patrols in complex and ever-changing outdoor park environments.
[0069] In summary, in the navigation scenario of the outdoor campus patrol robot, the robot, based on the "sovereignty.docx" method, collects data through multiple sensors, extracts thermal infrared and visual feature vectors, and calculates visual image blur and scene dynamic complexity using corresponding formulas. The robot then adapts to changes in illumination using a dynamic modal weighting algorithm to adjust weights, and obtains reliable fusion features using a mutual information enhancement fusion algorithm. An LSTM network is input to predict the trajectories of pedestrians, bicycles, etc., and risks are quantified using composite uncertainty quantification and dynamic risk coupling algorithms. The patrol path is planned using the A algorithm, with closed-loop updates every 2 seconds. This approach can address issues such as fluctuating outdoor lighting and diverse obstacles, ensuring the robot patrols along a preset route, avoids unexpected obstacles in a timely manner, and stably completes campus patrol tasks.
[0070] This application also discloses an electronic device. (See reference...) Figure 2 , Figure 2 This is a schematic diagram of the structure of an electronic device disclosed in an embodiment of this application. The electronic device 500 may include: at least one processor 501, at least one network interface 504, a user interface 503, a memory 505, and at least one communication bus 502.
[0071] The communication bus 502 is used to enable communication between these components.
[0072] The user interface 503 may include a display screen, and optionally, the user interface 503 may also include a standard wired interface or a wireless interface.
[0073] The network interface 504 may optionally include a standard wired interface or a wireless interface (such as a Wi-Fi interface).
[0074] This application also discloses a computer-readable storage medium storing multiple instructions adapted for loading by a processor to execute the above-described method for predicting and navigating dynamic obstacle trajectories of a robot based on a vision model.
[0075] The above are merely exemplary embodiments of this disclosure and should not be construed as limiting the scope of this disclosure. Any equivalent changes and modifications made in accordance with the teachings of this disclosure shall still fall within the scope of this disclosure.
[0076] This application is intended to cover any variations, uses, or adaptations of this disclosure that follow the general principles of this disclosure and include common knowledge or customary techniques in the art not described in this disclosure. The specification and embodiments are to be considered exemplary only, and the scope and spirit of this disclosure are defined by the claims.
Claims
1. A robot dynamic obstacle trajectory prediction navigation method based on a visual model, characterized in that, The method comprises the following steps: S1: Collecting raw data of multi-modal perception of dynamic scenes and performing space-time alignment, extracting thermal infrared dynamic feature vectors and visual dynamic feature vectors, and simultaneously calculating visual image blurriness and scene dynamic complexity; S2: Based on the thermal infrared feature vectors, visual feature vectors, visual image blurriness and scene dynamic complexity, the fusion weight of the thermal infrared feature and the visual feature is calculated through a dynamic modal weight algorithm; The mutual information enhanced fusion algorithm is used to fuse the thermal infrared feature and the visual feature, and output the fused feature vector; S3: The fused feature vector is input into a visual model to calculate the trajectory probability distribution of the dynamic obstacle; based on the trajectory probability distribution, the comprehensive uncertainty is calculated through a composite uncertainty quantification algorithm, and the risk coefficient is calculated through a dynamic risk coupling algorithm; S4: Based on the fused feature vector, the trajectory probability distribution, the comprehensive uncertainty and the risk coefficient, a key feature heat map, a causal association rule and a counterfactual explanation are generated, an obstacle avoidance path is planned through an A algorithm, and a decision report containing the risk coefficient, the heat map and the explanation text is output, realizing dynamic navigation of the robot.
2. The robot dynamic obstacle trajectory prediction and navigation method based on visual model of claim 1, wherein, Step S1 comprises: Synchronously collecting raw data of multi-modal perception of dynamic scenes through a multi-sensor suite carried by the robot; The multi-sensor suite comprises a thermal infrared camera, a visual camera, an illumination sensor, a laser radar and a wheel speed odometer; The raw data comprises a thermal infrared image sequence collected by the thermal infrared camera, an RGB image sequence collected by the visual camera, illumination intensity data collected by the illumination sensor, obstacle point cloud data collected by the laser radar and robot motion mileage data collected by the wheel speed odometer.
3. The vision model based robot dynamic obstacle trajectory prediction navigation method of claim 2, wherein, Step S1 further comprises: The extraction method of the thermal infrared dynamic feature vector is: The thermal infrared image sequence is input into a space-time convolution network comprising 3 to 5 layers of 3D convolution layers, each layer of convolution kernel size is 3×3×3 to 5×5×5, the step is 1 to 2, after batch normalization and ReLU activation, the output feature is globally averaged pooled to obtain a thermal infrared dynamic feature vector of 128 to 256 dimensions; The extraction method of the visual dynamic feature vector is: The RGB image sequence is input into an optical flow network to obtain an inter-frame optical flow field, and a target detection model is input to obtain an obstacle bounding box and a speed and acceleration parameter, the motion vector of the obstacle region in the optical flow field is mean-pooled, and the obstacle motion parameter is spliced after the mapping through a fully connected layer to obtain a visual dynamic feature vector of 128 to 256 dimensions.
4. The vision model based robot dynamic obstacle trajectory prediction navigation method of claim 1, wherein, Step S1 comprises: The calculation formula of the visual image blurriness is: wherein, is the visual image blur, is the gradient matrix obtained by Sobel operator after the RGB image is grayed, and only the elements of the gradient value are retained, is the gradient threshold value, is the information entropy of , wherein, is the image resolution, is the theoretical maximum information entropy of the gradient matrix; The calculation formula of the scene dynamic complexity is: wherein, is the scene dynamic complexity, is the number of dynamic obstacles in the current frame, is the total number of obstacles in the current frame, is the coefficient of variation of the dynamic obstacle motion speed, is the weight coefficient; The obstacle point cloud data comprises the number of dynamic obstacles and the total number of obstacles.
5. The vision model based robot dynamic obstacle trajectory prediction navigation method of claim 1, wherein, Step S2 comprises: The expression of the dynamic modal weight algorithm is: wherein, is a thermal infrared feature weight, is a visual feature weight, is a thermal infrared modality reliability, is a visual modality reliability, is an illumination intensity, is a visual image blur, is a scene dynamic complexity, is a tuning parameter.
6. The vision model based robot dynamic obstacle trajectory prediction navigation method of claim 1, wherein, Step S2 further comprises: In the dynamic multi-modal feature fusion, the expression of the mutual information enhanced fusion algorithm is: wherein, is a fusion feature vector, is a thermal infrared feature vector, is a visual feature vector, is a mutual information gain of thermal infrared over visual, is a mutual information gain of visual over thermal infrared, is a thermal infrared feature weight, is a visual feature weight.
7. The vision model based robot dynamic obstacle trajectory prediction navigation method of claim 1, wherein, Step S3 comprises: A visual model is constructed based on a long short-term memory (LSTM) network, which comprises 1 input layer, 2 to 3 LSTM hidden layers and 1 output layer; The LSTM hidden layer introduces a Monte Carlo dropout mechanism; The network weight of the visual model obeys a Gaussian prior distribution; The output layer is a full connection layer, and outputs a trajectory coordinate probability distribution parameter of the dynamic obstacle at a preset number of future time points, including a mean vector and a 2*2 dimensional covariance matrix of a coordinate (x, y) of each time point; In the model training process, the evidence lower bound (ELBO) is maximized as an optimization objective, and an Adam optimizer is used; In the reasoning stage, a preset number of Monte Carlo samplings are performed on the LSTM hidden layer, and based on the sampling results, a final trajectory probability distribution is fitted; An expression of the composite uncertainty quantification algorithm is: wherein, is a comprehensive uncertainty, is a predicted trajectory distribution entropy, is an obstacle motion abruptness, is a weight parameter; An expression of the dynamic risk coupling algorithm is: wherein, is a risk coefficient, is a comprehensive uncertainty, is an obstacle threat degree, is a real-time distance between the robot and the obstacle, is a reference safety distance.
8. The vision model based robot dynamic obstacle trajectory prediction navigation method of claim 1, wherein, Step S4 includes: The specific steps of planning an obstacle avoidance path through the A algorithm are as follows: Initialization: build open list and closed list, add start node to open list, and calculate the cumulative movement cost of start node , heuristic cost , and total cost , the formula is: ; Node selection: select the node with the lowest total cost from the open list The node with the lowest cost is selected as the current node and moved to the closed list. Target judgment: if the current node is the target point, an initial path is generated by backtracking along the parent node, and the search is ended; Neighborhood expansion: adjacent nodes of the current node are generated, and nodes beyond the environment boundary, located in the obstacle region, and already in the closed list are filtered; Cost update: compute cumulative movement cost of neighbor nodes , heuristic cost , and total cost , if node is not on open list or new value is smaller, update its cost with parent node and add to open list; Loop search: the node selection step is returned to be executed until the target point is selected from the open list, and an obstacle avoidance path is output.
9. An electronic device, comprising: The electronic device includes a processor, a memory, a user interface, and a network interface. The memory is configured to store instructions. The user interface and the network interface are configured to communicate with other devices. The processor is configured to execute the instructions stored in the memory, so that the electronic device performs the robot dynamic obstacle trajectory prediction navigation method based on the visual model according to any one of claims 1-8.
10. A computer-readable storage medium, characterized in that, The computer readable storage medium stores instructions, and when the instructions are executed by a computer, the robot dynamic obstacle trajectory prediction navigation method based on the visual model according to any one of claims 1-8 is executed.
Citation Information
Cited By
A vision-based obstacle recognition and ranging method
CN122135341A