A semantic navigation method for legged robots

Abstract

The application discloses a semantic navigation method for a leg-foot robot, and comprises the following steps: collecting semantic images, depth images and corresponding camera six-degree-of-freedom poses, and reconstructing a semantic map; taking the semantic map as a cost map, and sending the images into an initial network after pretreatment, obtaining a semantic navigation planning network model through error backpropagation optimization model; obtaining original point cloud information of an environment, obtaining inertial measurement information through an inertial measurement unit, obtaining depth images and color images through a camera, and setting a destination pose artificially; globally optimizing the pretreated original point cloud information and the inertial measurement information, so as to obtain a robot pose state, a global point cloud map and a local map of a current position of the robot; sending the image information into a planning network to obtain a planning path; and combining the robot pose state, outputting a desired linear velocity and angular velocity of the robot. The application lays a foundation for intelligent navigation planning of the leg-foot robot in a complex environment.

Description

Technical Field

[0001] This invention relates to the field of robotics, and in particular to a semantic navigation method for legged robots. Background Technology

[0002] Legged robots, with their excellent terrain adaptability, demonstrate great application potential in complex environments. However, to fully realize this advantage, legged robots also need effective autonomous navigation capabilities. This capability enables robots to perform path planning and autonomous decision-making in complex environments, and select the optimal route for efficient and safe movement.

[0003] The development of autonomous navigation technology has undergone significant evolution, gradually moving from geometric path planning to semantic navigation, which has become a research hotspot for intelligent navigation in complex scenarios.

[0004] In the early stages of legged robot navigation technology development, geometric navigation was the mainstream method. This type of technology mainly relies on the geometric information of the environment to plan paths. By collecting geometric data of the environment through sensors (such as LiDAR, depth cameras, etc.) and combining it with classic path planning algorithms, such as the A* algorithm and Dijkstra's algorithm, the robot can find the geometrically shortest path. This method performs well in static, regular environments, and can quickly generate the optimal path to the target. However, as robot application scenarios become more complex, the limitations of geometric navigation have become increasingly apparent. Especially in dynamic or unknown environments, relying solely on geometric information for path planning may lead to high robot movement costs and risks, and often cannot cope with complex terrain and obstacles.

[0005] In recent years, semantic navigation technology has gradually emerged, relying not only on geometric information but also incorporating semantic understanding of the environment. Through deep learning, semantic navigation identifies and understands objects, terrain, and scenes in the environment, helping robots make more intelligent decisions in path planning. This technological advancement enables robots to navigate more accurately and flexibly in complex environments.

[0006] Currently, most related research both domestically and internationally focuses on semantic target navigation. The core of semantic target navigation is to plan a corresponding path and execute the navigation task based on a given semantic target point, such as "navigate to the front door" or "navigate to the desk." A typical characteristic of this type of research is that given the semantic information of the navigation target point, the robot navigates to a specific location according to the instructions. For example, Huang et al. from the University of Freiburg proposed a navigation method based on visual language maps (VisualLanguage Maps for Robot Navigation), which enables robots to navigate autonomously based on human language commands by combining visual and natural language information. Similarly, Yokoyama et al. from Georgia Institute of Technology developed a zero-shot semantic navigation method (Vision-Language Frontier Maps for Zero-Shot Semantic Navigation), enabling robots to navigate to unseen objects in unknown environments using visual information. These studies provide theoretical support for intelligent navigation of robots in unknown and dynamic environments and significantly improve the autonomous navigation capabilities of robots in semantically rich environments. These technologies are only suitable for navigation of specific targets, and their path planning flexibility and intelligence are relatively limited, making it impossible for robots to fully utilize semantic information to select the optimal path in complex environments.

[0007] Domestic research also mainly focuses on this aspect. Wen Huanyu et al. (CN114739408A) proposed a semantic navigation method based on a two-dimensional semantic grid map. This method achieves semantic navigation by dividing the map into several grids and assigning different semantic information to each grid. Furthermore, Lin Mengxiang et al. (CN118896617A) applied object detection models and large-scale natural language models to add region and location attributes to objects, creating a semantic topological map on a two-dimensional grid map, and then querying navigation targets based on the semantic topological map to achieve the navigation task.

[0008] Despite significant advancements in existing semantic navigation technologies, domestic research largely focuses on semantic target navigation. While these methods utilize semantic information to some extent, they are ineffective in unfamiliar environments lacking prior maps. Furthermore, in complex environments such as bushes, mountains, and ditches, semantic target navigation fails to adequately consider the legged robot's mobility and does not calculate the traversal costs for different terrains to achieve efficient navigation. In the semantic navigation method for legged robots proposed in this invention, the robot not only combines geometric information for path planning but also evaluates the semantic features and traversal costs of different paths. For example, when selecting a path, the robot prioritizes paths with lower traversal costs (such as flat ground compared to rugged mountains or areas with obstacles), thereby improving the intelligence and efficiency of navigation. This type of method enables the robot to perform dynamic planning in more complex environments, effectively avoiding the shortcomings of navigation based solely on geometric distance. Furthermore, addressing the shortcomings of existing solutions that require prior map information, this method only requires pre-training a planner model for various terrains before it can be directly deployed in unfamiliar environments without map construction, thus improving the robot's mobility in unfamiliar environments. Summary of the Invention

[0009] The purpose of this invention is to address the shortcomings of existing technologies by providing a semantic navigation method for legged robots. To achieve the above objective, this invention provides a semantic navigation method for legged robots, comprising the following steps:

[0010] (1) Collect semantic images, depth images and corresponding camera six-degree-of-freedom poses under different environments, and reconstruct a complete semantic map in three dimensions;

[0011] (2) The semantic map obtained in step (1) is used as the cost map in the training process, and the input depth image and semantic image are preprocessed and sent into the initial network. The semantic navigation planning network model is obtained by optimizing the model through error backpropagation.

[0012] (3) Obtain the original point cloud information of the environment through lidar, obtain inertial measurement information through inertial measurement unit, obtain depth image and color image through RGBD camera, and obtain the target position and pose set by the user;

[0013] (4) The original point cloud information, inertial measurement information and image information obtained in step (3) are preprocessed, wherein the preprocessing includes point cloud information preprocessing, IMU pre-integration processing, semantic segmentation and image encoding;

[0014] (5) Perform global optimization on the preprocessed raw point cloud information and inertial measurement information in step (4) to obtain the robot pose state, global point cloud map, and local map of the robot's current position; send the preprocessed image information in step (4) into the planning network to obtain the planned path;

[0015] (6) Based on the planned path obtained in step (5), and combined with the robot's pose state, output the robot's expected linear velocity and angular velocity.

[0016] Further, step (1) specifically involves: using simulation software and an RGBD camera to collect image information and poses in different scenarios in the simulation and real environment, and aligning the point clouds of adjacent frames through an iterative nearest-point algorithm to gradually construct a complete semantic map.

[0017] Furthermore, the iterative nearest point algorithm continuously adjusts the source point cloud to minimize the overlap area with the target point cloud, thereby aligning two adjacent sets of point clouds and ultimately achieving 3D reconstruction.

[0018] Further, step (2) specifically involves: encoding the depth map and semantic map using an improved ResNet18 encoding method and the target navigation points after linear layer encoding into a vector, which is then fed into the initial planning network. The network predicts a set of path points leading to the target location and the probability of collision on the path. Spline interpolation is used to fit the path points into a continuous curve that the robot can execute, and the curve is projected onto the semantic map to obtain the corresponding loss. The error is then backpropagated to optimize the network parameters.

[0019] Further, step (4) specifically involves: performing IMU pre-integration processing on the inertial measurement information obtained in step (3) to obtain the robot pose changes in adjacent time intervals; performing point cloud information preprocessing on the robot pose changes in adjacent time intervals and the original point cloud information obtained in step (3) to obtain feature point clouds.

[0020] Furthermore, in step (4), the IMU pre-integration process specifically involves: converting the relative motion of the IMU between two frames into a fixed quantity to obtain the robot pose change in adjacent time intervals.

[0021] Further, in step (4), the point cloud information preprocessing specifically involves: combining the robot pose changes in adjacent time periods with the original point cloud information to perform point cloud noise removal processing; and extracting the features of the point cloud from the noise-removed point cloud information based on the local curvature of the point cloud to obtain the feature point cloud.

[0022] Further, in step (5), the global optimization of the preprocessed original point cloud information and inertial measurement information includes: for the state variables to be estimated, constructing a joint cost function that includes the lidar point cloud registration error function and the IMU pre-integration error function, and iteratively optimizing it using the Levenberg-Marquardt algorithm to obtain the optimized robot state and realize the front-end odometry estimation; by constructing the cost function of the entire system, obtaining the robot's posterior pose estimation under the sliding window graph optimization framework; this process transforms the system state estimation into a nonlinear least squares optimization problem, and uses the LM optimization algorithm to solve it to obtain the posterior pose estimation.

[0023] Further, step (6) includes: subscribing to robot pose information in the path following algorithm and calculating the distance from the last path point to the current position; traversing path points and selecting a target point based on a preset forward look distance; calculating the orientation angle between the current position and the selected target point and standardizing the deviation to the range of [-π, π]; determining whether to control the robot to move forward or backward based on the magnitude of the orientation difference and avoiding frequent switching through a time threshold; simultaneously, adding momentum correction to smooth the changes in orientation difference and avoid robot motion instability due to short-term fluctuations; then, using the orientation difference to output angular velocity to adjust the robot's orientation toward the target point, and dynamically adjusting the angular velocity gain based on the robot's speed; limiting the maximum value of the angular velocity to prevent the robot from rotating too violently; in terms of linear speed, if the robot's orientation is already toward the target point and the distance to the target point is greater than the stopping distance, then the robot accelerates linearly with maximum acceleration; otherwise, the robot decelerates to prioritize ensuring orientation.

[0024] The beneficial effects of this invention are that it fully leverages the role of semantic information in the planning process by integrating geometric and semantic information. Furthermore, considering the superior mobility of legged robots in various complex terrains, it utilizes depth information to consider the ESDF map while simultaneously segmenting the passage costs of different regions in three-dimensional space using semantic information. Based on a trained planning model, it implements an end-to-end local semantic navigation planner that outputs trajectories from image input. In addition, this invention proposes a method for automatically generating large amounts of simulation data using simulation software, greatly accelerating the acquisition of datasets and the training speed of the model. The method of this invention can be used on arbitrary blocks of semantic images, thus allowing for real-time trajectory updates as the robot moves. In summary, the local semantic navigation planner and data generation scheme in this invention provide a solid foundation for more intelligent navigation planning for legged robots in complex environments. Attached Figure Description

[0025] Figure 1 This is an overall flowchart of the present invention;

[0026] Figure 2This invention improves the ResNet-18 network graph;

[0027] Figure 3 The network structure diagram for this invention is shown below;

[0028] Figure 4 This is a path diagram of the training process of this invention;

[0029] Figure 5 This is a diagram defining the coordinate system for the physical deployment of this invention.

[0030] Figure 6 The diagram shows the effect of verifying the invention in a simulation environment. Detailed Implementation

[0031] The present invention will now be described in detail with reference to the accompanying drawings. Unless otherwise specified, the features of the following embodiments and implementations can be combined with each other.

[0032] Legged robots are widely used in various scenarios due to their superior locomotion capabilities. However, in complex environments, most current navigation solutions do not fully leverage the locomotion advantages of legged robots. They only consider the shortest path and collision-free movement at the geometric level, without taking into account the locomotion capabilities and costs of legged robots in different types of terrain. Their path planning flexibility and intelligence are relatively limited, and they cannot help robots fully utilize semantic information to select the optimal path in complex environments.

[0033] To address the aforementioned problems, this invention proposes a data-driven semantic cost navigation method for legged robots, such as... Figure 1 As shown, this method can be divided into two parts: model training and model deployment. The following section details the steps involved in its implementation:

[0034] S1. Collect semantic images, depth images, and corresponding camera six-degree-of-freedom poses under different environments, and reconstruct a complete semantic map in three dimensions.

[0035] Specifically, depth images, semantic images, and six-degree-of-freedom pose information in different scenes in the simulation and real-world environments were acquired using Isaac Sim and Carla simulation software and a Realsense D435i physical camera. The point clouds of adjacent frames were then aligned using an iterative nearest-point algorithm to gradually construct a complete semantic map for model training.

[0036] The iterative nearest point algorithm finds the optimal match between point clouds through continuous iteration, estimating their rigid body transformation. Its basic idea is: given a source point cloud and a target point cloud, the transformation matrix is ​​obtained by minimizing the error by finding the nearest point in the target point cloud for each point in the source point cloud. Let the source point cloud be P = {p i The target point cloud is Q = {q} j}, for each point p i ∈P, find the nearest point q in the target point cloud Q. j Then, a rigid body transformation (rotation matrix R and translation vector t) is calculated to make the source point cloud as close as possible to the target point cloud after transformation. The found rigid body transformation is applied to the source point cloud, and the above steps are repeated until convergence, i.e., the transformation matrix no longer changes significantly or the error no longer decreases significantly. The core of the algorithm is to minimize the mean square error between the source and target point clouds. The objective function can be defined as:

[0037]

[0038] Where R represents the rotation matrix from the source point cloud to the target point cloud, t represents the translation vector, and N represents the number of points to be processed.

[0039] S2. The semantic map obtained in step (1) is used as the cost map in the training process. The input depth image and semantic image are preprocessed and sent into the initial network. The model is optimized by backpropagation of error, and finally the semantic navigation planning network model is obtained.

[0040] Specifically, the depth map and semantic map are encoded using an improved ResNet18 encoding method, and then jointly encoded with the target navigation points after linear layer encoding into a vector, which is then fed into the initial planning network. The network predicts a set of path points leading to the target location and the probability of collision on that path. Spline interpolation is used to fit the path points into a continuous curve that the robot can execute, and then the curve is projected onto the semantic cost map to obtain the corresponding loss. The error is then backpropagated to optimize the network parameters.

[0041] After generating multiple environmental semantic maps using step S1, the image information used during reconstruction is encoded as a dataset and sent into the planning network. Based on the pose information of the images, reachable target location points are generated with distances set to 1, 3, 5, 7, and 10 meters, and the ratio is 0.2:0.35:0.25:0.15:0.05.

[0042] S21. During training, semantic information, as privileged information, can be directly obtained from the simulator. The image information encoding process specifically involves: semantic maps and depth maps obtaining feature encodings through an improved ResNet-18 backbone network; and the third part, the relative positions of target points, being mapped into a high-dimensional space through a linear layer. The improvements to the ResNet-18 network in this scheme are as follows: Figure 2 As shown.

[0043] Specifically as follows:

[0044] ① In the first residual block of each stage, the convolutional layer is replaced with a separable convolutional layer; this reduces the number of convolutional kernel calculations, separates the processing of depth and channel number by the convolutional kernel, and enables feature extraction in the channel domain and spatial domain respectively. Using a 1x1 convolutional kernel to uniformly process the channel number reduces the number of parameters, lowers the model complexity, makes the model more generalizable, mitigates the risk of overfitting, and improves computational efficiency.

[0045] ② A compression-activation module (SE) is added after each residual block to adaptively adjust the importance of each channel and improve the network's expressive power. The SE module can be divided into three parts: information compression, information activation, and recalibration. The information compression part performs global average pooling on the feature maps of all channels in the spatial dimension of the input feature map X(H*W*C), compressing the feature map of each channel into a scalar Z = [z1, z2, z3, ..., z...]. n The scalar (1*1*C) represents the global information for each channel. The information activation part processes the compressed vector through a two-layer fully connected network to learn the weights for each channel. The first fully connected layer has C / r neurons for dimensionality reduction, and the second fully connected layer restores the information to C dimensions and constrains the output to the range [0,1] using a sigmoid activation function, resulting in the weight matrix S for each channel.

[0046] S=σ(W2*ReLU(W1*Z))#(2)

[0047] Where W1 and SW2 are the weight matrices of the fully connected layer, and σ represents the Sigmoid activation function.

[0048] The recalibration process multiplies the obtained weights S element-wise with each channel of the input feature map X, recalibrating the importance of each channel. After recalibration, the weight matrix of the feature map adjusts the response strength of each channel. The output feature map... for:

[0049]

[0050] S22, the planner network mainly consists of a CNN and a multilayer perceptron, built using structured basic planning convolutional blocks, such as... Figure 3 As shown, after the pre-convolutional layer Head_Conv1, features are extracted through four basic planning convolutional blocks with different numbers of channels. Finally, a Flatten layer and a dense layer are used to obtain the two-dimensional coordinates of five planning points and a collision probability.

[0051] S23. For training the planning network, its output includes a set of sparse keypoints K as the core input for trajectory optimization, and the probability ε of a collision that may occur on that trajectory. Only trajectories with a collision probability less than 0.5 are optimized. This operation is to prevent the network from learning too many colliding trajectories and obtaining an extremely conservative strategy. During training, the network loss consists of path loss. and collision loss The composition is denoted by τ, which represents the planned path. The collision loss function is as follows:

[0052]

[0053] BCELoss is the binary cross-entropy loss, p i K To predict path points, Q tra This represents a passable area. The first case of the function above assumes that any keypoint in the trajectory is within a passable area, indicating that a collision will not occur. If this condition is not met, it means that there are impassable points in the trajectory, and the loss is then backpropagated to the network for correction.

[0054] For path loss It consists of four parts:

[0055]

[0056] Where α, β, γ, and δ are loss scaling factors.

[0057] This is the feasibility loss. The robot's physical dimensions are considered in this function, not only at each path point p. i K The cost is calculated at the center position, and also at a distance ω corresponding to the robot's width. R The cost of points perpendicular to the path is calculated, which takes into account the loss after the robot's lateral volume expansion.

[0058]

[0059] This is the bilinear interpolation at the corresponding point in the semantic map.

[0060] For destination loss items, For the last point in the path, the function penalizes deviations from the destination by calculating the distance between the last trajectory point and the destination and introducing a logarithmic scale to reduce the influence of long-distance target points.

[0061]

[0062] This is the motion trajectory smoothness loss, used to check the smoothness of the trajectory. It assumes that the motion time between any two path points on the path is equal and performs isochronous interpolation during processing. The goal is to reduce the difference in trajectory interval lengths, thereby minimizing the total acceleration. The Euclidean distance from the start to the finish line is used to adjust the track length and reward shorter movements. Returns the path interval between two given points on the trajectory, thus rewarding trajectories where the distance between two adjacent path points is close to the average value, making the trajectory smoother.

[0063]

[0064] The height loss term is introduced to make the path more regular in order to keep the robot at its base height h. R Above, avoid any operation that is higher or lower than the height of the obstacle.

[0065]

[0066] S24. After generating several target points, the pose, depth map input, and semantic map input at the starting point are implicitly obtained. The planner is trained to obtain five future local waypoints based on current observations and target point positions. A relatively smooth curve is obtained through interpolation, such as... Figure 4 As shown, the combined path loss and collision probability loss are used as network errors and propagated in reverse to ultimately train the semantic navigation planning network model.

[0067] The model training has been completed through steps S1 and S2 above. The following steps will explain in detail the implementation and deployment process:

[0068] S3. Obtain raw point cloud information of the environment through LiDAR, obtain inertial measurement information through inertial measurement unit, obtain raw depth and color images through RGBD camera, and obtain the target pose set by the user.

[0069] For example, the coordinate relationships between the acquired data are summarized as follows: Figure 5 As shown, the system specifically includes the robot body coordinate system B, the IMU center coordinate system I, the lidar coordinate system L, the camera coordinate system C, and the robot odometry coordinate system O. In this embodiment, since the IMU is installed at the robot's center of mass, the IMU coordinate system I coincides with the robot body coordinate system B, and the lidar coordinate system L is fixedly connected to the IMU coordinate system I. The external parameter transformation relationship is as follows: I T L The camera coordinate system C is fixedly connected to the lidar coordinate system L, and the external parameter transformation relationship is as follows: L TC Let the coordinates at the time the IMU receives the first frame of data be the world coordinate system W. After completing the above coordinate system alignment, set the IMU's coordinate system at time t as follows: Then the IMU in the world coordinate system F W The state to be estimated It can be written as:

[0070]

[0071] in, These represent the position, velocity, and attitude of the IMU in the world coordinate system W at time t, respectively. and These represent the biases of the IMU accelerometer and gyroscope, respectively, with the superscript T indicating the transpose operation.

[0072] S4. Preprocess the inertial measurement information, raw point cloud information, and camera image information obtained in step S3, wherein the preprocessing includes IMU pre-integration processing, point cloud information preprocessing, and image information preprocessing.

[0073] Specifically, the inertial measurement information obtained in step S3 is subjected to IMU pre-integration processing to obtain the robot pose changes in adjacent time intervals under IMU estimation; the robot pose changes in adjacent time intervals under IMU estimation are used; and the camera image information obtained in step S3 is used to obtain a semantic image using a visual semantic segmentation algorithm.

[0074] S41, IMU pre-integration processing specifically involves: based on the angular velocity and acceleration information of the IMU in two frames, the relative motion of the IMU between the two frames is calculated into a fixed quantity to obtain the robot pose change in adjacent time intervals.

[0075] Point cloud information preprocessing involves: combining robot pose changes within adjacent time intervals with the original point cloud information to remove point cloud noise; and extracting point cloud features based on the local curvature of the point cloud after noise removal to obtain feature point clouds. Specifically, the point cloud data is transformed based on the relative pose changes of the robot's motion within the LiDAR scanning cycle, and at the end of a LiDAR frame scan, all point clouds are projected back to the LiDAR coordinate system to obtain a frame of point cloud information with distortion removed.

[0076] Since this invention only utilizes lidar-inertial odometry for robot pose estimation to achieve path following, which is not strongly related to the core content of this invention, only the principle is briefly explained without further elaboration.

[0077] S42. Image information preprocessing involves: collecting image information of various terrain objects, pre-segmenting them using a pre-trained model and assigning different colors to distinguish them, such as grassland, gravel, snow, stairs, etc., and training different terrain features using semantic segmentation models such as Mask2Former to achieve rapid semantic segmentation of different terrain environments, providing semantic images for subsequent semantic planning and navigation.

[0078] S5. Jointly optimize the preprocessed raw point cloud information and inertial measurement information from step S4 to obtain the robot pose state, global point cloud map, and local map of the robot's current position. Input the preprocessed image information from step (4) into the planning network to obtain the planned path. Verify the planning effect in a simulation environment. Figure 6 As shown, the algorithm plans a local trajectory from the green square to the red square. The local planned trajectory (green curve) does not simply take the shortest straight path, but considers semantic factors during the planning process and selects the zebra crossing section with lower traffic cost throughout. This shows that the optimal path generation algorithm is effective in the simulation.

[0079] S51. The core of the above joint optimization lies in: constructing a joint cost function that includes the LiDAR point cloud registration error function and the IMU pre-integration error function for the state variables to be estimated, and iteratively optimizing it using the Levenberg-Marquardt (LM) algorithm to finally obtain the optimized robot state and achieve front-end odometry estimation. By constructing the cost function of the entire system, the posterior pose estimation of the robot is obtained under the sliding window graph optimization framework. This process transforms the system state estimation into a nonlinear least squares optimization problem, which is solved using the LM optimization algorithm to obtain the posterior pose estimation. The design of the overall cost function integrates the complementary information of LiDAR and IMU data to ensure the robustness and accuracy of the estimation, as shown in the following equation:

[0080]

[0081] In the formula, σ i Let σ be the IMU pre-integration error function. line The line residual function obtained by registering the line feature point cloud of lidar, σ plane The surface residual function obtained by registering the surface feature point cloud of lidar.

[0082] S52. The image preprocessing operation is the same as the training process. The original depth image and the semantic image generated after the semantic segmentation model are encoded using the improved ResNet-18 network. Then, they are jointly encoded with the target position after linear layer encoding into a 64*64*3 vector and input into the planning network. The network directly predicts a set of trajectory points and generates a path that the robot can execute based on spline interpolation.

[0083] S6. After obtaining the planned path in step (5), the robot pose state output by the lidar-inertial odometry is combined with the path following algorithm to output the robot's expected linear velocity and angular velocity.

[0084] Specifically, in the path following algorithm, robot pose information is subscribed to, and then the last path point (x) is calculated. end ,y end ) to the current position (x rel ,y rel The distance d) end It is used to determine whether the robot is close to the target point.

[0085]

[0086] Next, the waypoints are traversed, and the target point is selected based on the preset forward distance. This ensures that the target point is always a certain distance ahead, maintaining smooth navigation.

[0087]

[0088] Where, d i d represents the straight-line distance between the selected path point and the current position. lookAhead X represents the forward sight distance threshold. i The x-coordinate represents the coordinate of the selected path point in the x-direction, and the y-coordinate represents the coordinate of the selected path point in the x-direction. i This indicates the Y-coordinate of the selected path point.

[0089] Next, the orientation angle between the current position and the selected target point is calculated, and the deviation is standardized to the range [-π, π]. Based on the magnitude of the orientation difference, the robot is controlled to move forward or backward, and a time threshold is used to avoid frequent switching.

[0090] Path direction angle θ path The directional deviation Δθ is expressed as follows:

[0091]

[0092] Δθ=θ rel -θ path Δθ∈[-π,π]#(15)

[0093] Where, θ relThis indicates the current orientation angle of the robot.

[0094] Simultaneously, momentum correction is added to smooth out changes in orientation difference and prevent robot instability caused by short-term fluctuations. α is the momentum factor, and Δθ is the inflection point. last This indicates the directional deviation angle of the previous output.

[0095] Δθ=(1-α)·Δθ+α·Δθ last #(16)

[0096] Next, the algorithm first uses the direction difference to output angular velocity to adjust the robot's orientation towards the target point. Based on the above, the robot's angular velocity output can be obtained as follows, and the angular velocity gain is dynamically adjusted according to the robot's speed. The maximum value of the angular velocity is limited to prevent the robot from rotating too violently.

[0097] ω=-k yaw ·Δθ#(17)

[0098] Regarding online speed, if the robot is currently facing the target point and the distance to the target point is greater than the stopping distance d... stop Then let the robot accelerate to its maximum speed a max For linear acceleration, Δt = 0.01s. In all other cases, the robot is controlled to decelerate to prioritize maintaining its orientation.

[0099]

[0100] Where, θ thre This indicates the acceptable directional deviation angle threshold.

[0101] In addition, a safety mechanism is in place throughout the process. When the robot's roll angle or pitch angle exceeds the threshold, it is considered that the robot may be tipping over, and an emergency stop mechanism will be triggered.

[0102] This invention proposes a semantic navigation method for legged robots, offering the following advantages: The method comprehensively considers both geometric and semantic information, fully leveraging the potential of semantic information in path planning. Addressing the superior mobility of legged robots in complex terrain, this invention constructs an ESDF (Directed Distance Field) map based on depth information while simultaneously segmenting different regions in 3D space using semantic information to generate semantically distinguishable passage cost distributions. Based on this, by training a planning model, an end-to-end local semantic navigation planner is achieved, from image input to trajectory output, effectively improving the intelligence level of navigation. Furthermore, this invention proposes a method for automatically generating large-scale simulation data using simulation software. This method improves the efficiency of dataset acquisition and model training speed, providing efficient data support for navigation system optimization. The method of this invention can flexibly adapt to arbitrary block operations of semantic images and update the planned trajectory in real time combined with robot motion, ensuring the dynamic adaptability and real-time performance of the navigation system.

[0103] In summary, the local semantic navigation planner and automated data generation scheme proposed in this invention lay the foundation for intelligent navigation planning of legged robots in complex environments. It not only improves planning efficiency but also provides an important reference for navigation research that integrates semantic and geometric information.

Claims

1. A semantic navigation method for legged robots, characterized in that, Includes the following steps: (1) Collect semantic images, depth images and corresponding camera six-degree-of-freedom poses under different environments, and reconstruct a complete semantic map in three dimensions; (2) The semantic map obtained in step (1) is used as the cost map in the training process, and the input depth image and semantic image are preprocessed and sent into the initial network. The model is optimized by backpropagation of error to obtain the semantic navigation planning network model. The model includes: encoding the depth map and semantic map with improved ResNet18 and the target navigation points after linear layer encoding into a vector and sending it into the initial planning network. The network predicts a set of path points leading to the target location and the probability of collision on the path. The path points are fitted into a continuous curve that can be executed by the robot using spline interpolation and projected onto the semantic map to obtain the corresponding loss. The error is backpropagated to optimize the network parameters. (3) Obtain the original point cloud information of the environment through lidar, obtain inertial measurement information through inertial measurement unit, obtain depth image and color image through RGBD camera, and obtain the target position and pose set by the user; (4) Preprocess the raw point cloud information, inertial measurement information and image information obtained in step (3), wherein the preprocessing includes point cloud information preprocessing, IMU pre-integration processing, semantic segmentation and image encoding; (5) Perform global optimization on the preprocessed raw point cloud information and inertial measurement information in step (4) to obtain the robot pose state, global point cloud map and local map of the robot's current position; send the preprocessed image information in step (4) into the planning network to obtain the planned path; (6) Based on the planned path obtained in step (5), and combined with the robot's pose state, output the robot's expected linear velocity and angular velocity; including: subscribing to the robot's pose information in the path following algorithm, calculating the distance from the last path point to the current position; traversing the path points, selecting the target point according to the preset forward look distance; calculating the orientation angle between the current position and the selected target point, and standardizing the deviation to The system controls the robot to move forward or backward based on the magnitude of the orientation difference, and avoids frequent switching by using a time threshold. Simultaneously, momentum correction is added to smooth changes in the orientation difference, preventing instability caused by short-term fluctuations. Then, the angular velocity output from the orientation difference is used to adjust the robot's orientation towards the target point, and the angular velocity gain is dynamically adjusted based on the robot's speed. The maximum value of the angular velocity is limited to prevent the robot from rotating too violently. Regarding linear speed, if the robot is already facing the target point and the distance to the target point is greater than the stopping distance, the robot accelerates linearly with maximum acceleration; otherwise, the robot decelerates to prioritize maintaining its orientation.

2. The semantic navigation method for legged robots according to claim 1, characterized in that, The specific steps (1) are as follows: using simulation software and RGBD camera to collect image information and poses in different scenarios in the simulation and real environment, and aligning the point clouds of adjacent frames through the iterative nearest point algorithm to gradually build a complete semantic map.

3. The semantic navigation method for legged robots according to claim 2, characterized in that, The iterative nearest point algorithm continuously adjusts the source point cloud to minimize the overlap area with the target point cloud, thereby aligning two adjacent sets of point clouds and ultimately achieving 3D reconstruction.

4. The semantic navigation method for legged robots according to claim 1, characterized in that, The specific steps (4) are as follows: IMU pre-integration processing is performed on the inertial measurement information obtained in step (3) to obtain the robot pose changes in adjacent time periods; point cloud information preprocessing is performed on the robot pose changes in adjacent time periods and the original point cloud information obtained in step (3) to obtain feature point clouds.

5. The semantic navigation method for legged robots according to claim 1, characterized in that, In step (4), the IMU pre-integration process specifically involves: converting the relative motion of the IMU between two frames into a fixed quantity to obtain the robot pose change in adjacent time intervals.

6. The semantic navigation method for legged robots according to claim 1, characterized in that, In step (4), the point cloud information preprocessing specifically involves: combining the robot pose changes in adjacent time periods with the original point cloud information to perform point cloud noise removal processing; and extracting the features of the point cloud from the noise-removed point cloud information based on the local curvature of the point cloud to obtain the feature point cloud.

7. The semantic navigation method for legged robots according to claim 1, characterized in that, In step (5), the global optimization of the preprocessed raw point cloud information and inertial measurement information includes: for the state variables to be estimated, constructing a joint cost function that includes the lidar point cloud registration error function and the IMU pre-integration error function, and iteratively optimizing it using the Levenberg-Marquardt algorithm to obtain the optimized robot state and realize the front-end odometry estimation; by constructing the cost function of the entire system, obtaining the robot's posterior pose estimation under the sliding window graph optimization framework; this process transforms the system state estimation into a nonlinear least squares optimization problem, and uses the LM optimization algorithm to solve it to obtain the posterior pose estimation.

Images