A robot navigation method based on three-dimensional semantic map and large language model

Abstract

A robot navigation method based on a three-dimensional semantic map and a large language model comprises the following steps: decomposing a navigation task into three levels: a high-level planning goal, a middle-level planning goal, and a low-level execution goal; defining the high-level planning goal as determining a target area; selecting the target area through collaborative analysis of the three-dimensional semantic map and the navigation task; defining the middle-level planning goal as planning a travel path; obtaining the travel path through an SPFA algorithm based on a two-dimensional map of the target area; defining the low-level execution goal as controlling the robot to complete a travel action based on the travel path; acquiring point cloud data of an image of the robot's surrounding environment, extracting scene-level global features from the point cloud data through a 3D convolutional neural network, and extracting object-level fine-grained features through a feature pyramid network; fusing these features to form a unified multi-scale feature; and dynamically generating specific navigation instructions for the three-level navigation task through semantic understanding and analysis of the multi-scale features through a large language model.

Description

Technical Field

[0001] The present invention relates to the field of robot navigation, and in particular to a robot navigation method based on a three-dimensional semantic map and a large language model. Background Art

[0002] With the rapid development of intelligent robotics, hierarchical navigation systems have become a core technology for enabling robots to efficiently perform complex tasks. Existing mainstream approaches typically employ a classic three-layered structure: a high-level planning layer performs path planning based on static environmental information, a mid-level layer adjusts the path in real time, and a local execution layer handles dynamic obstacle avoidance based on real-time sensor data.

[0003] While this type of architecture improves navigation efficiency to a certain extent, it still faces significant technical bottlenecks in complex dynamic scenarios, specifically in the following two aspects: First, there is a lack of kinematic constraint optimization. The local planning layer typically focuses only on dynamic obstacle avoidance and real-time path adjustment, without deeply optimizing path smoothness and kinematic constraints. The path is directly passed to the underlying control module, resulting in trajectory jitter or unstable steering during robot movement. Especially in confined spaces or highly dynamic scenarios, the robot is prone to motion jitter or even deviation from the intended path due to sudden changes in trajectory curvature or delayed obstacle avoidance response. Second, there is a lack of dynamic feedback mechanisms between layers. If the path generated by the global planning layer becomes invalid due to sudden environmental changes (such as insurmountable obstacles), the local execution layer cannot trigger replanning at the global layer, causing the robot to become trapped in a local optimum or repeat invalid actions, ultimately leading to mission failure. Summary of the Invention

[0004] In view of the above-mentioned defects of the prior art, the present invention proposes a robot navigation method based on a three-dimensional semantic map and a large language model. Through a three-layer collaborative architecture of high-level planning goals, middle-level planning goals and low-level execution goals, the method solves the problems of motion jitter and trajectory deviation caused by the lack of kinematic constraint optimization in traditional methods, and significantly improves the success rate of passing through narrow channels; by establishing a dynamic feedback mechanism between levels, the second-level response capability to sudden obstacles is improved; further, the robot's autonomous navigation and task execution capabilities in complex dynamic scenes are improved.

[0005] In order to achieve the above object, the technical solution adopted by the present invention is:

[0006] A robot navigation method based on a three-dimensional semantic map and a large language model comprises the following steps:

[0007] S1. Mission Planning:

[0008] Encode the navigation task input by the user to generate a semantic vector; decompose the navigation task into three levels: high-level planning goal, middle-level planning goal, and low-level execution goal. After encoding each level into a one-hot vector, each level is mapped to the same dimension as the semantic vector through a fully connected layer to obtain the corresponding task level encoding;

[0009] The high-level planning goal is defined as determining a target area; the target area is selected through collaborative analysis of a 3D semantic map and the navigation task; a 3D semantic map is an advanced map representation that not only contains 3D spatial information but also incorporates semantic understanding of each object in the environment;

[0010] The middle-level planning goal is defined as a planned travel path; and the travel path is obtained by using an SPFA algorithm based on a two-dimensional map of the target area;

[0011] The underlying execution goal is defined as controlling the robot to complete a moving action according to the moving path;

[0012] S2. Obtain environmental characteristics:

[0013] Obtaining point cloud data of the robot's surrounding environment image, extracting scene-level global features from the point cloud data using a 3D convolutional neural network, and extracting object-level fine-grained features using a feature pyramid network;

[0014] S3. Multi-scale feature association:

[0015] fusing the task-level encoding, the semantic vector, the scene-level global features, and the object-level fine-grained features to form a unified multi-scale feature;

[0016] S4. Semantic understanding and navigation instruction generation:

[0017] The multi-scale features are semantically understood and parsed through a large language model to dynamically generate specific navigation instructions for the navigation tasks at three levels.

[0018] Preferably, in step S1, defining the high-level planning goal as determining the target area includes:

[0019] Divide the environment into multiple candidate areas according to the semantic label set of the three-dimensional semantic map; perform similarity matching between the semantic vector and the semantic label set of each candidate area, and calculate a generated area semantic score;

[0020] Based on the two-dimensional map, the SPFA algorithm is used to calculate the shortest path from the robot's current position to the center of the candidate area; according to the length of the shortest path and the obstacle density of the shortest path, the path accessibility score is calculated using an exponential decay function;

[0021] The target area is selected by linearly weighted fusion of the area semantic score and the path accessibility score.

[0022] Preferably, in step S1, defining the underlying execution target includes:

[0023] Establishing a robot dynamics model: The robot dynamics model establishes safety constraints based on the perceived obstacle position information; the robot dynamics model dynamically generates motion instructions based on the travel path and the safety constraints; if the travel path is detected to be blocked during the control of the robot to complete the travel action, the mid-level planning goal is replanned.

[0024] Preferably, in step S3, after forming the multi-scale features, an attention mechanism is used to dynamically assign weights to the scene-level global features and the object-level fine-grained features.

[0025] Compared with the prior art, the beneficial effects of the present invention are embodied in:

[0026] 1. Hierarchical planning of navigation tasks using the three levels of high-level planning objectives, mid-level planning objectives, and low-level execution objectives allows for decoupling and collaborative optimization of tasks at each level. The high-level planning objective dynamically selects a global target area based on a three-dimensional semantic map; the mid-level planning objective generates a smooth trajectory by combining path reachability with kinematic constraints; and the low-level execution objective utilizes model predictive control for dynamic obstacle avoidance. This hierarchical division effectively addresses the motion jitter and trajectory deviation issues associated with traditional end-to-end models due to the lack of kinematic constraints, significantly improving navigation success rates in confined spaces and highly dynamic scenarios.

[0027] 2. By establishing a dynamic inter-layer feedback mechanism, the system can respond to sudden environmental changes in real time and trigger replanning. This cross-layer dynamic feedback mechanism significantly improves the robot's ability to respond to sudden obstacles within seconds, ensuring its robustness in complex dynamic environments.

[0028] 3. During the multi-scale feature fusion stage, by fusing task-level encoding, semantic vectors, scene-level global features, and object-level fine-grained features, and dynamically assigning feature weights based on an attention mechanism, the system can autonomously adjust its environmental perception focus based on different task requirements, making the weight allocation more tailored to actual needs and thus improving the navigation system's scenario adaptability.

[0029] 4. In addition, the large language model can accurately understand the user's instruction intention by deeply analyzing and reasoning the semantic and contextual semantic information of the navigation task, thereby improving the accuracy of task completion. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] Figure 1 This is a flow chart of the method of embodiment 1 of the present invention. DETAILED DESCRIPTION

[0031] In order to make the technical means, creative features, objectives and effects of the invention easier to understand, the present invention is further described with reference to specific figures. However, the present invention is not limited to the following implementation cases.

[0032] It should be noted that the structures, proportions, sizes, etc. illustrated in the drawings in this specification are only used to match the contents disclosed in the specification so that people familiar with this technology can understand and read them. They are not used to limit the conditions under which the present invention can be implemented. Therefore, they have no substantive technical significance. Any structural modification, change in proportional relationship or adjustment of size should still fall within the scope of the technical content disclosed in the present invention without affecting the efficacy and purpose that can be achieved by the present invention.

[0033] Example 1:

[0034] like Figure 1 A robot navigation method based on a three-dimensional semantic map and a large language model is shown, comprising the following steps:

[0035] S1: Encode the navigation task input by the user into a semantic vector; decompose the navigation task into three levels: high-level planning goals, mid-level planning goals, and low-level execution goals, and encode them into one-hot vectors respectively. Then, map them to the same dimension as the semantic vector through a fully connected layer to obtain the task level encoding. The main methods are:

[0036] Using BERT text encoder to transform navigation tasks Encoded into semantic vectors , the calculation formula is as follows:

[0037] (1)

[0038] The purpose of this is to place the task encoding and semantic vector in the same dimensional space, which facilitates subsequent feature fusion and decision-making.

[0039] Furthermore, a dynamic inter-layer feedback mechanism is established, enabling the system to respond to sudden environmental changes in real time and trigger replanning. This cross-layer dynamic feedback mechanism significantly improves the robot's ability to respond to sudden obstacles within seconds, ensuring its robustness in complex dynamic environments.

[0040] S1-1: Define the high-level planning goal as determining the target area; select the target area through collaborative analysis of the 3D semantic map and navigation tasks; the main approach is:

[0041] First, the environment is divided into several candidate regions according to the semantic label set of the 3D semantic map. The 3D semantic map is a digital model that contains the geometric structure of 3D space (such as position, shape, size), semantic attributes of objects (such as "kitchen", "corridor") and topological relationships. With each candidate region A set of semantic tags Perform similarity matching and calculate regional semantic scores ( ), the calculation formula is as follows:

[0042] (2)

[0043] in is the cosine similarity function, For label The higher the semantic score, the stronger the relevance between the region and the navigation task.

[0044] Then, based on the two-dimensional grid map , using SPFA algorithm to calculate the robot's current position to the candidate area Shortest path length to the center , and combined with the obstacle density on the path , the path accessibility score is calculated using an exponential decay function :The calculation formula is as follows:

[0045] (3)

[0046] in is the maximum length of all possible paths in the map, 、 Used to balance the effects of path length and obstacle density (default 、 ).

[0047] Finally, the target region D is selected by linearly weighted fusion of the region semantic score and the path accessibility score. The objective function of the target region D is as follows:

[0048] (4)

[0049] in 、 is the weight coefficient used to balance semantic relevance and path accessibility. (The default value is =0.7, )

[0050] Encapsulate the coordinate range and semantic label of the target area D as high-level planning output parameters Input to mid-level planning objectives.

[0051] S1-2: Define the middle-level planning goal as planning the travel path; based on the two-dimensional map of the target area, use the SPFA algorithm to obtain the travel path; the main approach is:

[0052] According to the output of high-level planning goals Plan a feasible path. Based on the two-dimensional map , based on the SPFA algorithm, output a set of feasible paths from the starting point to the target area D The SPFA algorithm is a shortest path algorithm based on dynamic relaxation. It optimizes the relaxation process through queue management to improve the path search efficiency, and combines curvature optimization to ensure that the path meets the robot's kinematic constraints. Its cost function is:

[0053] (5)

[0054] in is the actual path length from the starting point to the center of the target area D, is the path curvature, is the curvature weight coefficient (default = 0.2). Then, B-spline interpolation is performed on the path set P to generate a smooth curve , reduce the robot's steering jitter and improve movement stability.

[0055] Will The coordinate sequence is encapsulated as the output parameter of the middle-level planning Input to the system's underlying execution module.

[0056] S1-3: Define the underlying execution goal as controlling the robot to complete the movement according to the travel path; the main approach is:

[0057] First, establish the dynamic model of the robot:

[0058] (6)

[0059] in represents the state of the robot at time k (including position and orientation), is the linear velocity, is the angular velocity, Indicates the control period.

[0060] Then, based on the real-time perceived obstacle position information, safety constraints are established to ensure that the robot does not collide with obstacles:

[0061] (7)

[0062] in is the predicted position of the i-th step in the prediction time domain, is the position of the j-th obstacle, is a safety margin (default =0.2m).

[0063] According to the output of mid-level planning objectives Generate reference trajectory , i paths At the reference point of step k, the control sequence is optimized within the prediction time domain N steps :

[0064] (8)

[0065] in, It is a cost term imposed on the control input, used to limit drastic changes or excessive values of the controlled variable, thereby ensuring the smoothness and stability of the system.

[0066] The above model predictive control module receives the output of the middle-level planning target , dynamically generate motion instructions that take into account both obstacle avoidance safety and global target reachability , ensuring that tasks can be performed safely even in the face of sudden obstacles.

[0067] If a path is detected during the underlying execution If it is permanently blocked, it triggers the middle-level re-planning and updates .

[0068] S2: Obtain point cloud data of the robot's surrounding environment image and extract scene-level global features from the point cloud data through a 3D convolutional neural network The feature pyramid network is used to extract object-level fine-grained features. The main approach is:

[0069] The navigation system extracts scene-level global features from point cloud data through 3D convolutional neural networks , construct a three-dimensional semantic map containing spatial topological relationships. Contains 5 layers of 3D convolution, each layer has a channel number of [64, 128, 256, 512, 1024], a step size of [2, 2, 1, 1, 1], and an output feature dimension of .

[0070] The navigation system uses the feature pyramid network to obtain the low-level feature maps Extract the geometric features of the object from the high-level feature map Extract the semantic information of the object and output object-level features .

[0071] S3: Fusion of task-level encoding, navigation task encoding, scene-level global features, and object-level fine-grained features to form a unified multi-scale feature. The main approach is:

[0072] First, the task level encoding obtained in S1 is integrated and navigation task semantic encoding , generating joint context , enhance the task-oriented context expression ability, the calculation formula is:

[0073] (9)

[0074] in , are learnable parameters.

[0075] scene-level features and object-level features Project them into the query space to generate corresponding query vectors and :

[0076] (10)

[0077] Joint Task Context The matching degree between the two types of features and the task context is calculated by dot product. The calculation formula is as follows:

[0078] (11)

[0079] (12)

[0080] Among them, the ReLU activation function is used to filter negative correlation and highlight positive correlation.

[0081] Then, the scene-level feature weights are generated through Softmax normalization and object-level feature weights , the calculation formula is as follows:

[0082] (13)

[0083] The normalized features are weighted and summed to generate a unified multi-scale feature representation.

[0084] (14)

[0085] Through the dual perception of language instructions and task level, weight and It can automatically adjust according to different tasks to make the weight distribution more in line with actual needs.

[0086] S4: Use a large language model to semantically understand and parse multi-scale features, and dynamically generate specific navigation instructions for different layered tasks. The main approach is:

[0087] The multi-scale features obtained in S3 are obtained through a large language model (such as DeepSeek-R1) Perform semantic understanding and parsing, and leverage the reasoning and decision-making capabilities of large language models to dynamically generate specific navigation instructions for different hierarchical tasks.

[0088] S4-1: High-Level Planning:

[0089] Task encoding T=[1,0,0], scene-level feature weight =0.85 and object-level feature weights =0.15.

[0090] Input: 3D semantic map : Contains point cloud data of the office environment; language instructions : "Please go to the kitchen and get a glass of water."

[0091] Decision: Calculate the semantic similarity of candidate areas (conference room, corridor, kitchen) , the kitchen has the highest score (0.92). The shortest path length from the current robot position to the kitchen =4.5m, obstacle density =0.1, calculated according to the formula .78, total score =0.88, and the kitchen is selected as the target area.

[0092] Output: Target area D: Coordinate range of the kitchen area ( [5.0,8.0],y [3.0,6.0],z [0,2.5]).

[0093] Navigation Command: Global Target: Move southeast to the kitchen area, coordinate range: , ,

[0094] z , be careful to avoid obstacles such as tables and chairs in the path.

[0095] S4-2: Mid-level path planning:

[0096] Task encoding T=[0,1,0], scene-level feature weight =0.55 and object-level feature weights =0.45.

[0097] Input: 2D map : Grid map of target area D (including obstacle distribution); current position of the robot: x=1.0, y=1.0).

[0098] Decision: First, the initial path is generated by the SPFA algorithm. The length is 5.8m and the curvature =0.12; path The length is 6.1m, the curvature =0.07. Cost function =5.94, =6.24, so choose ,right Perform B-spline interpolation to eliminate sharp angles at path turning points and generate smooth curves .

[0099] Output: : [(1.0,1.0) (3.2,2.6) (5.1,4.2) (6.5,5.0)]

[0100] Navigation instructions: From your current location, move northeast to (3.2, 2.6), then move northeast to (5.1, 4.2), and finally to the center of the kitchen (6.5, 5.0).

[0101] S4-3: Bottom-level execution actions:

[0102] Task encoding T=[0,0,1], scene-level feature weight =0.25 and object-level feature weights =0.75.

[0103] Input: The path output by the middle-level path planning ;Obstacle information: The pedestrian is detected at (4.0,3.5).

[0104] Decision: Through model predictive control optimization, within the prediction time domain N=5 steps, the obstacle is detected to be 0.08m away from the robot's predicted position, which is less than the safety margin. An obstacle penalty term is added to the optimization objective function to generate instructions to slow down and turn left to ensure that safety constraints are met.

[0105] Output: Control instructions :

[0106] =0.5m / s, =0.1rad / s] (lasts 2 seconds)

[0107] [ =0.3m / s, =-0.2rad / s] (obstacle avoidance adjustment)

[0108] Navigation command: Execute action: Drive straight ahead at 0.5 m / s for 2 seconds, then decelerate to 0.3 m / s and turn left (angular velocity -0.2 rad / s) to avoid the pedestrian ahead.

Claims

1. A robot navigation method based on a three-dimensional semantic map and a large language model, characterized in that: The following steps are involved: S1. Mission Planning: Encode the navigation task input by the user to generate a semantic vector; decompose the navigation task into three levels: high-level planning goal, middle-level planning goal, and low-level execution goal. After encoding each level into a one-hot vector, each level is mapped to the same dimension as the semantic vector through a fully connected layer to obtain the corresponding task level encoding; Defining the high-level planning goal as determining a target area; Selecting the target area through collaborative analysis of the three-dimensional semantic map and the navigation task; The middle-level planning goal is defined as a planned travel path; and the travel path is obtained by using an SPFA algorithm based on a two-dimensional map of the target area; The underlying execution goal is defined as controlling the robot to complete a moving action according to the moving path; S2. Obtain environmental characteristics: Obtaining point cloud data of the robot's surrounding environment image, extracting scene-level global features from the point cloud data using a 3D convolutional neural network, and extracting object-level fine-grained features using a feature pyramid network; S3. Multi-scale feature association: fusing the task-level encoding, the semantic vector, the scene-level global features, and the object-level fine-grained features to form a unified multi-scale feature; S4. Semantic understanding and navigation instruction generation: The multi-scale features are semantically understood and parsed through a large language model to dynamically generate specific navigation instructions for the navigation tasks at three levels.

2. The robot navigation method based on three-dimensional semantic map and large language model according to claim 1 is characterized in that: In step S1, defining the high-level planning goal to determine the target area includes: Divide the environment into multiple candidate areas according to the semantic label set of the three-dimensional semantic map; perform similarity matching between the semantic vector and the semantic label set of each candidate area, and calculate a generated area semantic score; Based on the two-dimensional map, the SPFA algorithm is used to calculate the shortest path from the robot's current position to the center of the candidate area; according to the length of the shortest path and the obstacle density of the shortest path, the path accessibility score is calculated using an exponential decay function; The target area is selected by linearly weighted fusion of the area semantic score and the path accessibility score.

3. The robot navigation method based on three-dimensional semantic map and large language model according to claim 2 is characterized in that: In step S1, defining the underlying execution target includes: Establishing a robot dynamics model: The robot dynamics model establishes safety constraints based on the perceived obstacle position information; the robot dynamics model dynamically generates motion instructions based on the travel path and the safety constraints; if the travel path is detected to be blocked during the control of the robot to complete the travel action, the mid-level planning goal is replanned.

4. The robot navigation method based on three-dimensional semantic map and large language model according to claim 1 is characterized in that: In step S3, after the multi-scale features are formed, an attention mechanism is used to dynamically assign weights to the scene-level global features and the object-level fine-grained features.

Images