A visual navigation system for legged robots in unstructured scenes

Abstract

This application describes a visual navigation system for a legged robot in unstructured scenes. This system addresses the problem of insufficient utilization of ground feature information in existing technologies, which leads to inaccurate navigation. This application establishes a multi-layer local perception map that can describe ground features, adding a description of ground features to the traditional local map. Furthermore, based on the local perception map, the system incorporates ground feature parameters into the trajectory optimization process, thereby addressing the issue of insufficient utilization of ground feature information by legged robots. This improves the accuracy of the robot's visual navigation.

Description

Technical Field

[0001] The present invention relates to the field of robot navigation technology, and in particular to a legged robot visual navigation system for unstructured scenes. Background Art

[0002] Unstructured scenarios, such as disaster relief sites, wilderness areas, and forests, feature complex ground structures, such as slope, height, and inclination, as well as complex ground features, such as stiffness, friction coefficient, and potential hazards. Legged robots, leveraging their discontinuous support, offer superior terrain adaptability in such scenarios. Consequently, many applications of legged robots are geared towards complex outdoor environments. Navigation and planning for legged robots are currently a hot topic of research. Mainstream approaches include coupled navigation planning, which combines center of mass planning (CoM) with foothold planning, and hierarchical navigation planning, which decouples these two tasks. These approaches are implemented using optimization and reinforcement learning. Compared to the high computational effort required by coupled approaches and the large data volumes and training time required by reinforcement learning, optimization-based hierarchical navigation solutions are more suitable for deployment on legged robot platforms. Legged robots face two major challenges during locomotion: First, low friction between the foot and the ground can lead to slippage; second, unknown environmental stiffness can cause a mismatch between the robot's leg stiffness and the surrounding stiffness. Therefore, if only the center of mass motion is considered without considering the ground surface, the two planning components of a hierarchical navigation scheme will conflict. This conflict also arises if the center of mass planning does not satisfy the leg's kinematic constraints. This results in insufficient utilization of ground feature information by the legged robot, leading to inaccurate visual navigation. Summary of the Invention

[0003] The purpose of the present invention is to propose a legged robot visual navigation system for unstructured scenes to address the problem that the existing technology does not fully utilize ground feature information, which leads to inaccurate navigation.

[0004] The technical solution adopted by the present invention to solve the above technical problems is:

[0005] A visual navigation system for a legged robot in unstructured scenes, comprising a map module, a planning module, a state estimation module, and a user command module;

[0006] The state estimation module is used to estimate the position and posture information of the robot in the world coordinate system using a binocular visual inertial odometry;

[0007] The map module is used to obtain the RGB image and depth image of the scene, and combine the position and posture information of the robot in the world coordinate system to obtain a global map and a multi-module local map;

[0008] The multi-module local map includes a dilation information map and a gradient information map;

[0009] The expansion information map is obtained by the following steps:

[0010] First, a fixed three-dimensional (x, y, z) bounded voxel space is set as the container of the local perception map. Then, the pixel positions of the depth image are mapped to the voxel space according to the camera's extrinsic parameter matrix to generate occupied voxels. Then, the sparsity of the robot-sized point cloud and the sparsity of the depth image point cloud are obtained. The container of the local perception map is spatially filled based on the sparsity of the robot-sized point cloud and the sparsity of the depth image point cloud to obtain the expansion information map.

[0011] The gradient information layer map is obtained by the following steps:

[0012] S1: Use the real-time semantic segmentation network ENet to process the RGB image and obtain the ground type label map;

[0013] S2: Use the CSR method to sort the obtained ground type label map by cost level to obtain a ground cost level label map. The CSR method is specifically as follows:

[0014] The physical properties and safety of the ground types were empirically quantified using the Likert scale to obtain the stiffness property level, friction property level, and safety level. A detectability level was established based on whether the depth camera was detectable. All levels were then multiplied to obtain an overall level representing the ground reliability. Finally, the ground reliability levels were ranked according to the overall level.

[0015] S3: Convert the ground cost level label map into a gradient map through the Label2Grad method;

[0016] The user instruction module is used to obtain user command instructions, that is, target coordinate points, and send the user command instructions to the planning module;

[0017] The planning module is used to plan the robot's center of mass trajectory based on the global map, the multi-module local perception map and user command instructions, and send the planning results to the robot.

[0018] Furthermore, the planning module includes a global planning unit, a path smoothing unit, and a local trajectory planning unit;

[0019] The global planning unit performs global path planning based on the target coordinate points and the global map using the A* algorithm;

[0020] The path smoothing unit is used to perform path differential flattening on the path planned by the global planning unit to obtain a local planning initial value;

[0021] The local trajectory planning unit is used to perform trajectory planning using B-spline and soft constraints. The specific steps are as follows:

[0022] The local planning initial values ​​are sampled to obtain the control points of the B-spline. The control points are then used to construct a penalty function that characterizes ground features, obstacles, and robot motion constraints. Based on the penalty function, an unconstrained optimization problem is constructed using the soft constraint method. The unconstrained optimization problem is solved using L-BFGS to obtain the optimal trajectory.

[0023] Furthermore, the local trajectory planning unit includes a map information layer planning subunit and a motion information layer planning subunit;

[0024] The map information layer planning subunit is used to establish a penalty function using a multi-module local perception map, wherein the penalty function includes an obstacle avoidance penalty function, a centroid height penalty function, a pitch angle penalty function, and a gradient information layer penalty function;

[0025] The motion information layer planning subunit uses the relative relationship of the control points to establish a penalty function, and the motion information layer planning subunit includes a motion capability penalty function, a time allocation penalty function, and a smoothing penalty function.

[0026] Furthermore, the motion information layer planning subunit is represented as:

[0027] J locomotion =λ smooth f smooth +λ fitness f fitness +λ fessible f fessible

[0028] Among them, f smooth represents the smooth penalty function, λ smooth represents its penalty factor, f fitness represents the athletic ability penalty function, λ fitness represents its penalty factor, f fessible represents the time allocation penalty function, λ fessible represents its penalty factor.

[0029] Furthermore, the map information layer planning subunit is represented as:

[0030] J map =λ collision fcollision +λ auto (f mch +f pitch )+λ auto f label

[0031] Among them, f collision represents the obstacle avoidance penalty function, f mch represents the centroid height penalty function, f pitch represents the pitch angle penalty function, f label represents the gradient information layer penalty function, λ auto represents the adaptive penalty factor, λ collision Represents the gradient information layer penalty function.

[0032] Furthermore, the process of establishing the centroid height penalty function is as follows:

[0033] Use the control point of the B-spline to calculate the difference between the projection point of the multi-module local perception map on the z-axis of the world coordinate system, obtain the distance between the restricted control point and the ground, and penalize the distance;

[0034] The centroid height penalty function is expressed as:

[0035]

[0036] c m,i =H(Q i )-MCH

[0037] Among them, N C Indicates the total number of control points, i indicates the index of the i-th control point, f i represents the penalty function of the i-th control point, c m,i Indicates the difference between the control point height H and MCH, c m,i If it is greater than the threshold, it will be punished, otherwise it will not be punished. threshold represents the threshold, H(Q i ) represents the height from the control point to the ground, and MCH represents the height of the robot's center of mass;

[0038] Furthermore, the process of establishing the pitch angle penalty function is as follows:

[0039] First, the optimization dimension is restricted to the z-axis of the world coordinate system. Then, two adjacent control points Qi and Qi+1 on the z-axis are obtained, and the difference between the control points Qi and Qi+1 on the z-axis is obtained to approximate the tan value of the pitch angle from Qi to Qi+1. Then, a threshold for the robot's pitch angle is set. Finally, if the tan value of the pitch angle is greater than the threshold, a penalty is imposed, and the penalty is regularized.

[0040] Furthermore, the pitch angle penalty function is expressed as:

[0041]

[0042]

[0043] Among them, k represents the approximate value of the denominator of tan, the subscripts x, y, and z represent the directions of the x, y, and z axes respectively, and Pitch i represents the pitch angle at the i-th control point;

[0044] Furthermore, the process of establishing the gradient information layer penalty function is as follows:

[0045] The control points are projected onto the ground information points on the gradient information map unit. Then, the cost and gradient information of the ground information points are used to establish the ground quality label constraint penalty function on the x-axis and y-axis of the world coordinate system respectively.

[0046] The ground quality label constraint penalty function is expressed as:

[0047]

[0048] Furthermore, the global map is obtained by the following steps:

[0049] Based on the robot's position and posture information in the world coordinate system and the RGB image and depth image of the scene, the scene point cloud is obtained, and the point cloud is spliced ​​to obtain a global map.

[0050] The beneficial effects of the present invention are:

[0051] This application establishes a multi-layered local perception map that describes ground features, adding a new layer of ground feature description to traditional local maps. Furthermore, by integrating ground feature parameters into the trajectory optimization process based on the local perception map, the problem of legged robots' insufficient utilization of ground feature information is addressed, thereby improving the accuracy of the robot's visual navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 This is the overall framework diagram of this application;

[0053] Figure 2 Establish a flow chart for the multi-layer local perception map of this application;

[0054] Figure 3 Schematic diagram of potential hazards considered for the multi-layer local perception map gradient information layer of this application Figure 1 ;

[0055] Figure 4 Schematic diagram of potential hazards considered for the multi-layer local perception map gradient information layer of this application Figure 2 ;

[0056] Figure 5 This is the experimental effect diagram of the multi-layer local perception map gradient information layer in the real scene of this application;

[0057] Figure 6 Schematic diagram of the penalty function for ground quality cost based on the gradient information layer;

[0058] Figure 7 Schematic diagram of the pitch angle constraint to constrain the trajectory tolerance for obstacle crossing tasks based on the robot's passing ability;

[0059] Figure 8 This is a diagram of the pitch angle penalty;

[0060] Figure 9 Schematic diagram of an unstructured scene;

[0061] Figure 10 This is a schematic diagram of the effect of the climbing task;

[0062] Figure 11 This is an illustration of the navigation task effect. Figure 1 ;

[0063] Figure 12 This is a schematic diagram of the navigation effect;

[0064] Figure 13 This is an illustration of the navigation task effect. Figure 2 ;

[0065] Figure 14 Transform schematics for conflict scenarios;

[0066] Figure 15 Schematic diagram of ground quality comparison for trajectory planning. DETAILED DESCRIPTION

[0067] It should be noted that, unless there is any conflict, the various embodiments disclosed in this application can be combined with each other.

[0068] Specific implementation method 1: refer to Figure 1 Specifically describing this embodiment, the legged robot visual navigation system for unstructured scenes described in this embodiment includes a map module, a planning module, a state estimation module, and a user instruction module;

[0069] The state estimation module is used to estimate the position and posture information of the robot in the world coordinate system using a binocular visual inertial odometry;

[0070] The map module is used to obtain the RGB image and depth image of the scene, and combine the position and posture information of the robot in the world coordinate system to obtain a global map and a multi-module local map;

[0071] The multi-module local map includes a dilation information map and a gradient information map;

[0072] The expansion information map is obtained by the following steps:

[0073] First, a fixed three-dimensional (x, y, z) bounded voxel space is set as the container of the local perception map. Then, the pixel positions of the depth image are mapped to the voxel space according to the camera's extrinsic parameter matrix to generate occupied voxels. Then, the sparsity of the robot-sized point cloud and the sparsity of the depth image point cloud are obtained (to simplify the back-end trajectory planning problem). The container of the local perception map is spatially filled based on the sparsity of the robot-sized point cloud and the sparsity of the depth image point cloud to obtain the expansion information map.

[0074] The gradient information layer map is obtained by the following steps:

[0075] S1: Use the real-time semantic segmentation network ENet to process the RGB image and obtain the ground type label map;

[0076] S2: Use the CSR method to sort the obtained ground type label map by cost level to obtain a ground cost level label map. The CSR method is specifically as follows:

[0077] The physical properties and safety of the ground types were empirically quantified using the Likert scale to obtain the stiffness property level, friction property level, and safety level. A detectability level was established based on whether the sensor was detectable. After that, all levels were multiplied to obtain the overall level representing the reliability of the ground. Finally, the ground was ranked according to the overall level representing the reliability of the ground.

[0078] S3: Convert the ground cost level label map into a gradient map through the Label2Grad method.

[0079] The user instruction module is used to obtain user command instructions, that is, target coordinate points, and send the user command instructions to the planning module;

[0080] The planning module is used to plan the robot's center of mass trajectory based on the global map, the multi-module local perception map and user command instructions, and send the planning results to the robot.

[0081] Legged robot layered visual 3D navigation algorithm framework: Figure 1As shown in the figure, the process framework is mainly divided into the map layer, the planning layer, and the state planning layer. The map layer includes a global mapping module and a multi-layer local perception map module, which are responsible for updating and maintaining the global map and local map, respectively. The input is the RGB image and depth image from the depth camera and the robot's position and posture information in the world coordinate system obtained from the state estimation module. The output is a global map and a multi-layer local perception map. The planning layer includes a global planning module, a path smoothing module, and a local trajectory planning module that can utilize ground structure and ground feature information. The global planning module is responsible for global path planning based on the target point and the known global map. The path smoothing module is responsible for differentially flattening the path to provide initial values ​​for local planning. The local trajectory planning module optimizes the trajectory based on the initial values ​​and the multi-layer local perception map to obtain the local trajectory plan. The input is user commands, the global map, and the multi-layer local perception map. The output is the center of mass trajectory plan for the legged robot.

[0082] A method for establishing a multi-layer local map that can describe ground features and structures: Figure 2 As shown in the figure, the perception scheme uses the RGB image and depth map of D435i as input. After the above process, a three-dimensional multi-layer local perception map is finally formed. This three-dimensional local perception map mainly includes two levels: the expansion map layer and the gradient information layer. The expansion map layer fills the space around the occupied voxels based on the occupied voxel space according to the sparsity of the point cloud information converted from the robot size and the depth map. This processing method is a common strategy for robot navigation when processing perception maps. This processing, on the one hand, makes up for the loss of local maps caused by sensors, and on the other hand, approximates the robot's planning to centroid planning as much as possible, reduces the complexity of the problem, and improves the safety of planning. The gradient information layer is the information of whether the visual sensor in the bounded voxel space is visible, such as Figure 3 (a) shows the discrete cost information of the two-dimensional labels obtained based on real-time semantic segmentation reasoning after "cost sorting". Figure 3 As shown in (b), the state transition relationship between different voxels converted into a continuous three-dimensional state can be used for trajectory optimization.

[0083] Gradient information layer map building method based on converting discrete states into continuous states (Label2Grad): In order to incorporate ground quality information into the construction of trajectory optimization problems, it is necessary to obtain the state transfer cost function between each voxel in the sampling space. To address this challenge, this application designs the Label2Grad method to convert the discrete sampling map into a gradient map with gradient direction, where the gradient direction indicates better ground quality. Figure 4As shown, the present application transforms a single multi-state sampling space cost transfer problem into multiple dual-state sampling space transfer cost problems. Finally, based on the Euclidean distance transformation, the present application can effectively model the state transfer cost between voxels in the state space, thereby providing information on gradient direction and cost value for trajectory optimization.

[0084] A hierarchical trajectory optimization method that can utilize ground structure information and ground feature information: The trajectory planning problem of this application is based on B-spline and soft constraints. Since B-spline has a convex hull, that is, the B-spline is within the range of its control points, and moving a control point will not affect other parts of the spline, the trajectory planning problem can be converted into a point planning problem, reducing the amount of calculation. Soft constraints combine all penalty functions into a cost function, and the effect of each penalty function is weighed by the penalty factor. Therefore, this property can be used to introduce ground features, obstacles, and robot motion constraints in combination with control points to construct a penalty function, generate a cost function, and construct an unconstrained nonlinear optimization problem. In order to ensure the convergence of the solution, this application designs the trajectory optimization problem into two layers, namely the map information layer and the motion information layer. The map information layer is a cost function composed of a penalty function constructed by the joint constraints of obstacle avoidance constraints, ground feature constraints, and center of mass height and crossing angle. This layer is responsible for providing the initial value of the safe trajectory. The motion information layer includes a cost function composed of penalty functions constructed by motion ability constraints, smoothness constraints, and trajectory adjustment constraints. This layer is responsible for providing adjusted trajectories and improving the differential flatness of the trajectory.

[0085] Method for establishing trajectory optimization problem based on gradient information layer with available ground feature information: This method is a specific way to construct penalty function using ground feature constraints in ground information layer. Figure 4 As shown, this method aims to establish a penalty term for the ground quality cost based on the local perception map gradient information layer. According to the gradient direction of the state transition relationship, after optimization, the control point will converge to a position with higher ground quality, thereby making the trajectory at a higher quality. Since the trajectory planning is based on the centroid trajectory, there is no ground feature information near the control point, so the control point is projected to the ground information point on the gradient layer, and the cost and gradient information of this point are used for optimization. In order to improve the convergence speed of the solution to the trajectory optimization problem, this application only optimizes on the x-axis and y-axis where the ground quality significantly affects the planning success rate.

[0086] A method for establishing a trajectory optimization problem based on the joint constraint of center of mass height and crossing angle that can determine obstacle avoidance behavior: This method is a specific way for the ground information layer to construct a penalty function using the joint constraint of center of mass height and crossing angle. If the trajectory height is not restricted during trajectory optimization in three-dimensional space, the planned trajectory will be seriously inconsistent with the movement ability of the legged robot. In addition, although the legged robot has excellent passability, it is impossible to cross obstacles with too large an angle, so the planned trajectory must be reasonably planned for obstacle avoidance and obstacle crossing. In response to the above two problems, this application designs a center of mass height constraint to constrain the height of the trajectory relative to the ground. A pitch angle constraint is designed to constrain the trajectory's tolerance to obstacle crossing tasks based on the robot's passability. As Figure 7 As shown in Figure 3, the combined effect of the two constraints ensures the rationality of trajectory planning and enables the trajectory to be reasonably selected to avoid and overcome obstacles.

[0087] The overall framework of this application is as follows Figure 1 As shown, the framework process is mainly divided into map layer, planning layer and state planning layer. The hardware units are as follows: the camera model used by the sensor is D435i and the robot's onboard imu, the execution robot is UnitreeGO1 legged robot, the computing unit is a 1.5kg laptop with low computing power, the graphics card unit is GTX-1050 and the computing unit is i7-8750H. This application is universal and has low requirements for hardware computing power and model reasoning capabilities, so the above hardware can be replaced. This solution is a layered navigation system, which is divided into a global planning layer and a local planning layer. The global planning layer mainly includes the global map in the map layer and the global planning of the planning layer. Since the global map will continue to grow with the exploration process, it involves a large space and is responsible for the global planning of the task, so the update frequency is low, usually 5hz. The global map is obtained by combining the point cloud converted from the depth map with the current robot state estimated posture. The global planning layer uses the A* algorithm for path search. The state estimation layer uses the binocular visual inertial odometer vins-fusion to estimate the robot's posture, velocity and acceleration information in the world coordinate system. As shown Figure 2As shown in the figure, the perception scheme uses the RGB image and depth map of D435i as input. After the above process, a three-dimensional multi-layer local perception map is finally formed. This three-dimensional local perception map mainly consists of two layers: the expansion map layer and the gradient information layer. The depth map will be converted into an occupied voxel space that can describe three-dimensional information. First, a fixed three-dimensional (x, y, z) bounded voxel space is set as the container of the local perception map. Then, according to the camera's external parameter matrix K, the depth map pixel position (u, v, z) is mapped to the corresponding (X, Y, Z) in the voxel space, as shown in formula (1). Occupied voxels are generated, thereby obtaining the three-dimensional information within the perception range of the local perception map in the voxel space. By querying the voxels, this application can determine whether there is an object in the space, and can attach color, semantics and other information to the corresponding object.

[0088]

[0089] The RGB image is converted into a two-dimensional ground cost label segmentation map containing ground cost information. This application first inputs the RGB image into a lightweight, real-time semantic segmentation network, using Enet, to generate a ground type label segmentation map. Finally, based on a cost ranking method, the ground type labels are mapped to corresponding cost labels, resulting in a ground cost label segmentation map. This cost ranking method, referred to in this application as the CostSortRule (CSR), is a rule for subjectively evaluating ground quality corresponding to ground types. Current visual methods can directly determine ground types through semantic segmentation, but cannot determine their physical properties. Furthermore, ground problems that affect robot navigation include more than just physical information; they also include potential hazards. For example, grasslands may contain low shrubs and hidden traps, water surfaces may vary in depth, and ice thickness may vary. Therefore, it is reasonable to scientifically quantify human subjective experience and incorporate it into the evaluation system. In [1], Flamming et al. evaluated the material properties of corresponding substances using a Likert scale, validating the scientific validity of this method in quantifying subjective experience. The following researchers used the same method to achieve semantic segmentation of ground physical property features, so the establishment of CSR will introduce subjective experience through this method. In addition, due to the limitations of the visual sensor's field of view and the presence of negative obstacles (such as potholes), some voxel spaces cannot be detected by the sensor. In this case, the detected voxels are obviously more reliable than the undetected areas. Therefore, this method is based on the classification of ground types and covers three main dimensions: physical properties (friction properties and stiffness properties), safety, and sensor perception range. The comprehensive ranking result of these three dimensions is the final ground quality cost ranking.

[0090] Taking the laboratory's own ground segmentation dataset as an example, cost ranking was performed using CSR. This dataset has the following label formats. It includes six ground types: water, grass, land, ice, stone roads, and cement roads. Ten professionals within the laboratory were invited to subjectively evaluate the impact of the friction and stiffness properties of each terrain under different conditions on robot motion. Friction properties are graded on a scale of 1 to 4, with 1 being the most slippery and 4 being the least slippery. Stiffness properties are graded on a scale of 1 to 5, with 1 being the most deformable and 5 being the least deformable. Based on their subjective judgment and experience, the professionals assigned friction and stiffness coefficients and hazard levels to each terrain type, as shown in Table 1. Higher levels are preferred.

[0091] Table 1 Friction, stiffness and hidden danger level coefficients

[0092]

[0093] To distinguish the cost value of detectable area from undetectable area, such as Figure 3 As shown in (a), this application introduces the detectability level, as shown in Table 2.

[0094] Table 2 Detection level coefficient

[0095]

[0096] This application characterizes the ground quality by multiplying the above grade coefficients to obtain a total reliability grade coefficient. The lower the total coefficient, the lower the reliability of the voxel. Figure 3 (b). This application maps this to a cost level, with lower reliability resulting in higher cost levels, as shown in Table 3. Through the design of CSR, this application can more accurately and comprehensively assess the quality of different ground surfaces, laying the foundation for 3) establishing a gradient direction gradient map that can better represent ground surface quality.

[0097] Table 3 Reliability level coefficient and cost mapping relationship

[0098]

[0099] The dilated map layer fills the space surrounding the occupied voxels based on the robot's size and the sparsity of the point cloud information converted from the depth map. This approach is a common strategy for robot navigation when processing perceptual maps. This approach not only compensates for local map gaps caused by sensors, but also approximates the robot's planning to center-of-mass planning, reducing problem complexity and improving planning safety.

[0100] Generally, gradient-based trajectory planning problems cannot use feature information based on visual reasoning. The reason is that the discrete label state cannot provide a continuous state transition relationship between different voxels. To address this challenge, this method designs a gradient information layer map building method (Label2Grad) based on converting discrete states into continuous states, converting discrete sampling maps into gradient maps with gradient directions, and using gradient directions to represent ground quality is better. Figure 4 As shown in (a), this method transforms a single multi-state sampling space cost transfer problem into multiple dual-state sampling space cost transfer problems. Figure 4 As shown in (c), finally, based on the Euclidean distance transform, this method can effectively model the state transfer cost between voxels in the state space, thereby providing information on gradient direction and cost value for trajectory optimization.

[0101] Euclidean distance is the basis for establishing the state transition cost function between different voxels in this paper. Reference [9] makes it possible to calculate the Euclidean distance in real time. This application refers to the same calculation method, which usually calculates the transition cost between two states in the sampling space. The state transition relationship between voxels with only two states in a certain dimension can be expressed by the following formula. Assume that p is the voxel position of state 1, Dd(p) represents its distance from the nearest voxel position q in another state in the d dimension, and f(q) is the offset at the corresponding position, which is the calculation result of the previous dimension. Therefore, Dd(p) is the smallest parabola about (q, f(q)), as shown in formula (2).

[0102]

[0103] Therefore, for voxel p, the state transition cost of its voxel with respect to other states is the lower envelope of these parabolas. Any two parabolas have exactly one intersection point. The projection position s of this intersection point on the one-dimensional coordinate axis is calculated as shown in Equation (3).

[0104]

[0105] Based on the intersection point, we determine whether the new parabola is the lowest parabola. Finally, we update the lower envelope of the entire dimension from left to right. This lower envelope represents the cost function for state transitions between voxels in different states. This approach establishes the state transition relationship between different voxels in the dual-state space.

[0106] According to CSR in 2), we first construct a single two-state problem. Figure 4As shown in (a), the voxels whose current voxel q is greater than their own cost value are regarded as another state, and those whose current voxel q is less than their own cost value are regarded as the same state. In this way, the voxel at the cost threshold is expressed as the state transfer cost of the voxel with a higher ground cost, that is, a layer of gradient information graph that can represent the state transfer relationship of the dual-state voxel space with the purpose of "staying away from the ground with worse quality than itself" is established. According to the threshold, multiple dual-state sampling space problems are calculated and finally fused to represent the state transfer cost between voxels in the multi-state sampling space, that is, the gradient relationship from good to bad quality between different grounds is obtained. This application uses formula (4) for fusion to represent the gradient direction of ground quality from good to bad. Taking into account the degree of adaptability of different robots to the ground, this application introduces the adaptability coefficient λ. This application can flexibly adjust the algorithm to adapt to the needs of different robots and environmental changes.

[0107] d label =λ1d1+λ2d2+…+λ n d n (4) dlabel represents the total gradient information map, di represents the gradient information map under a certain threshold i, and n represents the total number of threshold i.

[0108] Generally, in the same local perception map, the difference between ground types is not too great. If the transfer relationship between all label categories of voxels is considered every time the map is updated, it will not optimize the performance and will occupy computing resources. Therefore, in the algorithm of this application, the number of pixels of each ground cost label in the current frame is usually counted, and the top three cost labels are selected as significant cost labels, and used as the threshold. The effect is as follows: Figure 4 As shown in (b), it can be concluded from the figure that no matter where the threshold is selected, it can be guaranteed that the voxels with the worst ground quality are regarded as the ground type that should be kept away.

[0109] The Label2Grad method uses three costs as reference gradient maps to create the effect as follows Figure 4 (c) is shown. Among them, λ1 controls the significance of the gradient map formed by the lower cost threshold, and λ2 controls the significance of the gradient map formed by the higher cost threshold. The ratio of the two coefficients is K = λ1 / λ2. As can be seen from the figure, the larger K is, the lower the tolerance of the established gradient value to poor ground quality, and the smaller K is, the higher the tolerance becomes, mainly highlighting the gradient with the worst ground quality. The mapping effect of this method in real scenes is as follows Figure 5 As shown, the gradient information map is a three-dimensional map, but for a more intuitive display effect, its two-dimensional slice at the ground height is displayed.

[0110] The trajectory of the present application is constructed by uniform B-spline curve. The establishment of the local planner of the present application is based on the current more common mobile robot EGO-planner, and is similar to it in the construction method of B-spline. Since B-spline has a convex hull property, that is, the part of the spline curve located at (ti, ti+1) must be in the convex hull composed of control points {Qi-pb, Qi-pb+1, ..., Qi}, where pb is the order. Using this property, the present application transforms the various constraint problems that need to be considered in the trajectory optimization of this article into problems constructed with control points.

[0111] Different from other local planners built based on optimization methods, which focus the planning task on obstacle avoidance, the local planner of this application needs to consider the constraints from ground features and ground structures in trajectory optimization. There are also constraints from the robot's motion capabilities and the time adjustment constraints of the B-spline. Therefore, it is a huge trajectory optimization problem. Therefore, this application chooses a soft constraint method to establish the trajectory optimization problem. However, it is still a challenge to ensure the speed and convergence of the solution to this problem. Therefore, this application adopts a step-by-step optimization method to construct a planner, such as Figure 1 shown.

[0112] The objective cost function optimized in the first step is shown in Equation (5), which is referred to in this application as the perception layer trajectory planning cost function. This part mainly utilizes map information and has the following three goals: obstacle avoidance, determining whether to cross or avoid obstacles based on the map and the vehicle's own capabilities, and based on the above, selecting trajectories with high ground reliability as much as possible.

[0113] J map =λ collision f collision +λ a (f mch +f pitch )+λ auto f label (5)

[0114] where f collision represents the penalty function established based on the distance between the robot and the obstacle, with the goal of achieving obstacle avoidance. mch and f pitch They represent the penalty functions based on the center of mass height constraint and the climbing ability constraint, respectively, to realize the robot’s choice of obstacle crossing or obstacle avoidance. The specific construction method is shown in (5). labelIt represents a penalty function established based on the ground quality, which enables the robot to choose a more reliable passing trajectory according to the ground conditions. The specific construction method is shown in (6). Each penalty function has a corresponding penalty factor λ to control the importance of this factor to the trajectory decision. However, in reality, there may be a scenario where the structure and features conflict, resulting in the non-convexity of the problem. In the experimental part of (7), this application designs such a scenario. In order to ensure that the solution meets the real-time solution, the penalty factor of the ground feature penalty function is designed in this application to be an adaptive penalty factor λ auto The specific construction method is shown in (6).

[0115] The objective cost function optimized in the second step is shown in Equation (6), which is referred to as motion layer planning in this application. This part mainly considers the robot's motion capabilities such as speed and acceleration, the time adjustment of the uniform B-spline, and the improvement of the differential flatness of the planned trajectory.

[0116] J locomotion =λ smooth f smooth +λ fitness f fitness +λ fessible f fessible (6)

[0117] This application establishes a penalty function for ground quality cost based on the gradient information layer. Since the trajectory planning is the center of mass trajectory, there is no ground information near the control point. Figure 6 As shown, the control point is projected onto the ground information point on the gradient layer, and the cost of this point and the gradient information are used for optimization. The ground quality label constraint penalty function of a single control point is shown in Equation (7).

[0118]

[0119] To improve the convergence speed of the optimization problem, this application only optimizes the x-axis and y-axis where the ground quality significantly affects the planning success rate. Therefore, the ground quality label constraint penalty function for trajectory planning is shown in Equation (8).

[0120]

[0121] In fact, in scenarios where there is a conflict between ground structure and ground features, such as Figure 4 Scenarios with obstacles can cause the solution to not converge. Therefore, this application designs an adaptive relaxation penalty factor that automatically adjusts the penalty function's impact based on the planning situation. Because obstacle avoidance has a greater impact on planning success than ground quality, the algorithm reduces the penalty function coefficient to ensure real-time planning when excessive solution attempts are made and other costs, such as obstacle avoidance, are present.

[0122] This application implements the robot's task planning of climbing over or avoiding obstacles by jointly constraining the center of mass and the climbing angle of the planned trajectory. The two-dimensional navigation system can only avoid obstacles, while the three-dimensional navigation system needs to distinguish between avoidance or climbing in unstructured environments. Although the legged robot has excellent passability, it is impossible to cross obstacles with too high angles. Therefore, the planned trajectory must be reasonably planned for obstacle avoidance and obstacle crossing. In addition, if the trajectory height is not restricted during trajectory optimization in three-dimensional space, the planned trajectory will be seriously inconsistent with the movement ability of the legged robot. In response to the above two problems, this application designs a center of mass height constraint to constrain the height of the trajectory relative to the ground. Since the pitch angle can best characterize the climbing task, the pitch angle constraint is designed to constrain the trajectory's tolerance for obstacle crossing tasks based on the robot's passing ability. For example Figure 7 As shown in Figure 3, the combined effect of the two constraints ensures the rationality of trajectory planning and enables the trajectory to be reasonably selected to avoid and overcome obstacles.

[0123] The trajectory planned by the local planner in this application is the trajectory of the center of mass of the legged robot. Therefore, the penalty is set by limiting the distance of the control point to the ground. This distance is obtained by subtracting the projection point of the control point on the map on the z axis. Then, this distance is subtracted from the center of mass height MCH to obtain c m,i , as shown in formula (10), and then quadratized to obtain a single control point penalty function, as shown in formula (9).

[0124]

[0125] c m,i =H(Q i )-MCH(10)

[0126] In this way, the control points are constrained to be near the center of mass height. The rationality of the trajectory height is guaranteed by the convexity of the B-spline. Therefore, the trajectory planning center of mass height penalty function is shown in Equation (11).

[0127]

[0128] Based on the planning of the overtaking ability, this application focuses on the penalty of the pitch angle. If the pitch angle in the trajectory planning is directly calculated, the complexity of the equation will increase, so this application simplifies this problem. Figure 8 As shown. This application first limits the optimization dimension to the z-axis which has the greatest impact on the pitch angle. As shown in formula (12), the difference between the Z-axis of two adjacent control points Qi and Qi+1 is used to approximate the tan value of the pitch angle from Qi to Qi+1. Although the positions of Qi and Qi+1 change during the optimization process, since the distance between the control points of the uniform B-spline is relatively stable, k is generally a relatively stable value. Figure 7 In the experiment, it will not affect the convergence of the solution.

[0129]

[0130] Through the above method, the present application obtains an approximate expression for the pitch angle of Qi. According to the robot's motion capability, a pitch angle threshold is set. It is regularized by squaring to ensure that neither pitch nor elevation exceeds the robot's motion capability during planning. Given the convexity of the B-spline, it can be ensured that the trajectory planning between the control points meets the constraints on the pitch angle. Therefore, the penalty function of the pitch angle of the control point is shown in Equation (14).

[0131]

[0132] Therefore, the penalty function of the trajectory pitch angle of this application is shown in formula (15):

[0133]

[0134] Experimental results:

[0135] The experiments mainly include mapping effect experiments and navigation effect ablation experiments under different ground structures and ground features. This application has designed four scene verification results, namely, scene 1: unstructured scene, such as Figure 9 As shown in the figure, the scenario includes a ramp that is suitable for the robot to pass and a large angle drop that can create a sensor blind spot; Scenario 2: Obstacle scenario, such as Figure 12 As shown in the figure, the obstacle in this scene forms a large angle with the ground, and the height of the obstacle is too high for the robot to cross; Scenario 3: Multiple ground feature scenes, such as Figure 5 As shown, this scenario includes grass and a wooden pavement, and combined with sensor blind spots, it verifies the effectiveness of building a multi-layer local perception map. Scenario 4: Conflict between ground features and ground structures, i.e., obstacles in areas of high ground quality. These four scenarios are representative and represent extreme cases of conventional optimization-based layered navigation. This demonstrates the superiority of our approach.

[0136] The mapping effect experiment was conducted in scenes 1 and 2. Scene 1 is an unstructured scene, which can test the effect of local mapping on structural features. Scenes 1 and 2 are mainly composed of wooden boards and grass. Combined with the sensor-invisible area, there are at least three advantageous terrain features, which are sufficient to verify this application.

[0137] Table 4 shows the attribute information for each layer of the multi-layer local map and the effective range of the visual sensor. Each layer of the multi-layer local map in this application is a three-dimensional map, with consistent dimensions relative to world coordinates, determined by the effective range of the sensor. The resolution is consistent, and the voxel information is stored using a consistent indexing method.

[0138] Table 4 Attribute information of each layer of multi-layer local map

[0139]

[0140] Table 5 shows the average time consumed by each component of the map construction process. The speed of building the entire multi-layer local map ensures that the update rate is consistent with the camera frequency. During the construction process, the main time consumption is the time it takes for the real-time semantic segmentation network to infer ground types, accounting for 80% of the total time. The Label2Grad method in this application accounts for 8%. This time analysis shows the real-time requirements of the local planning in this application.

[0141] Table 5 Average time consumption in each part of the mapping process

[0142]

[0143] Figure 5 and Figure 9 The mapping effect of multi-layer local perception maps can be demonstrated. In the expansion map layer, the expansion is performed according to the size of the robot, and a planning lower limit is set. This design effectively ensures the safety of centroid planning. In the gradient information layer, through the CSR rule, this application sets the ground reliability ranking as wooden boards > grass = ground > invisible scenes. Comparing the 3D semantic segmentation maps of scenes 1 and 2 with the trapezoidal information map slices, it can be seen that the Label2Grad of this method can effectively characterize the voxel state transfer relationship between different ground features and the voxel state transfer relationship within the same ground.

[0144] Navigation Effectiveness Experiment: The core goal of this method's navigation framework is to design a visual navigation method for legged robots in unstructured scenarios, enabling them to fully perceive ground structure and feature information and utilize it for trajectory optimization. Therefore, the verification focuses on the effectiveness of a penalty function that utilizes ground structure information with the combined constraints of center of mass height and overturning angle, a trajectory optimization penalty function that utilizes ground feature information based on a gradient information layer, and scenarios where ground features conflict with ground structure. The verification method primarily utilizes ablation experiments and comparative experiments. In the verification experiment, the center of mass height penalty function is defined as "M," the overturning angle penalty function is defined as "P," the obstacle distance penalty function is defined as "C," and the ground feature penalty function is defined as "L."

[0145] First, we verify the trajectory optimization effect of the combined constraints of center of mass height and overturning angle using ground structure information. The verification scenarios are scenario 1 and scenario 2. Figure 9As shown, in scenario 1, this application sets two target points. Target point 1 corresponds to the climbing task, and target point 2 is in the sensor blind spot, so it corresponds to the avoidance task. The map layer cost functions are "M+P+C", "M+C", "P+C" and "C" to perform the above tasks. In fact, the effect of "C" is similar to the current common three-dimensional navigation planning method. Figure 10 From the robot's running results, when the map layer cost function is "M+P+C", the robot can correctly climb over or avoid according to the target points 1 and 2. For target point 1, the trajectory planning results of "M+C", "P+C" and "C" can make the robot reach the target successfully. However, for target point 2, the trajectory planning results of "M+C", "P+C" and "C" still choose to climb over. Figure 12 As shown in , scenario 2 is a typical obstacle avoidance task scenario. “M+P+C” can successfully complete the navigation task, while “M+C”, “P+C” and “C” completely fail. Figure 11 , Figure 13 As shown in the figure, data analysis shows that the planning results of "M+P+C" fully meet the robot's center of mass height and climbing ability thresholds, while "M+C," "P+C," and "C" do not. This can lead to conflicts between the planning and control layers, causing navigation failures in some scenarios. Analysis of the planning results of "M+C" and "P+C" shows that the penalty function designs of "M" and "P" do indeed guarantee the center of mass height and climbing angle, respectively, but both lead to a one-sided understanding of the environment structure. Only joint constraints can ensure the rationality of the solution.

[0146] Next is to verify the trajectory planning effect of the available ground feature information based on the gradient information layer. The verified scenario is Scenario 3. Scenario 3 is a scene that is as flat as possible but has different ground features. The design purpose is to not allow structural information to affect navigation planning. This application sets a target point and makes the map layer cost function "M+P+C+L" and "M+P+C" to complete the above tasks. Figure 14 From the robot operation results, it can be seen that when the cost function of the "M+P+C" map layer is "M+P+C+L", the robot chooses a solution with higher ground quality when completing the navigation task. In addition, Figure 15 As shown in the figure, the data shows that the planning result of "M+P+C+L" has a higher ground quality than that of "M+P+C". This proves the effectiveness of the "L" planning.

[0147] Finally, for the conflict between ground features and ground structures, the scenario used is scenario 4, which is modified based on scenario 3. Figure 14As shown in Figure 1, obstacles are added at the best position of the ground feature gradient information map to create a conflict scenario. In a conflict scenario, the local planning solver will not converge, but under the action of the adaptive penalty factor, the influence weight of the ground feature will decrease, allowing the robot to give priority to obstacle information. Figure 12 As shown, let the robot perform the same target task in scene 3 and scene 4. If there is no conflict, this application will plan the trajectory with the best ground quality. If there is a conflict, the ground structure will be given priority.

[0148] A hierarchical visual 3D navigation method for legged robots in unstructured scenes is proposed. By building a multi-layer local map that describes ground features and structure, this method not only captures structural information but also complements conventional methods' lack of visual information. It also transforms discrete label information into continuous state transitions required for gradient-based trajectory planning. This method utilizes a hierarchical trajectory optimization approach that leverages both ground structure and feature information. Using the multi-layer local perception map, this method combines structural and feature information with the robot's own properties to construct a penalty function. Using soft constraints, various factors influencing navigation success are organically formulated as a trajectory planning problem. To ensure convergence, a step-by-step optimization approach is implemented, first ensuring planning safety and then smoothness. This allows the robot to select trajectories with optimal ground quality while maintaining its mobility. Finally, based on these two components, a full-stack hierarchical visual navigation method for legged robots is constructed, enabling global map building and global trajectory planning. Ablation and qualitative experimental results demonstrate that the proposed method produces results with superior task performance.

[0149] It should be noted that the specific embodiments are merely explanations and illustrations of the technical solutions of the present invention and cannot be used to limit the scope of protection. Any minor changes made based on the claims and description of the present invention shall still fall within the scope of protection of the present invention.

Claims

1. A visual navigation system for legged robots in unstructured scenes, characterized by The navigation system includes a map module, a planning module, a state estimation module and a user instruction module; The state estimation module is used to estimate the position and posture information of the robot in the world coordinate system using a binocular visual inertial odometry; The map module is used to obtain the RGB image and depth image of the scene, and combine the position and posture information of the robot in the world coordinate system to obtain a global map and a multi-module local map; The multi-module local map includes a dilation information map and a gradient information map; The expansion information map is obtained by the following steps: First, a fixed three-dimensional (x, y, z) bounded voxel space is set as the container of the local perception map. Then, the pixel positions of the depth image are mapped to the voxel space according to the camera's extrinsic parameter matrix to generate occupied voxels. Then, the sparsity of the robot-sized point cloud and the sparsity of the depth image point cloud are obtained. The container of the local perception map is spatially filled based on the sparsity of the robot-sized point cloud and the sparsity of the depth image point cloud to obtain the expansion information map. The gradient information map is obtained by the following steps: S1: Use the real-time semantic segmentation network ENet to process the RGB image and obtain the ground type label map; S2: Use the CSR method to sort the obtained ground type label map by cost level to obtain a ground cost level label map. The CSR method is specifically as follows: The physical properties and safety of the ground types were empirically quantified using the Likert scale to obtain the stiffness property level, friction property level, and safety level. A detectability level was established based on whether the depth camera was detectable. All levels were then multiplied to obtain an overall level representing the ground reliability. Finally, the ground reliability levels were ranked according to the overall level. S3: Convert the ground cost level label map into a gradient map through the Label2Grad method; The user instruction module is used to obtain user command instructions, that is, target coordinate points, and send the user command instructions to the planning module; The planning module is used to plan the robot's center of mass trajectory based on the global map, the multi-module local perception map and user command instructions, and send the planning results to the robot.

2. The legged robot visual navigation method for unstructured scenes according to claim 1 is characterized in that The planning module includes a global planning unit, a path softening unit and a local trajectory planning unit; The global planning unit performs global path planning based on the target coordinate points and the global map using the A* algorithm; The path smoothing unit is used to perform path differential flattening on the path planned by the global planning unit to obtain a local planning initial value; The local trajectory planning unit is used to perform trajectory planning using B-spline and soft constraints. The specific steps are as follows: The local planning initial values ​​are sampled to obtain the control points of the B-spline. The control points are then used to construct a penalty function that characterizes ground features, obstacles, and robot motion constraints. Based on the penalty function, an unconstrained optimization problem is constructed using the soft constraint method. The unconstrained optimization problem is solved using L-BFGS to obtain the optimal trajectory.

3. The legged robot visual navigation method for unstructured scenes according to claim 2 is characterized in that The local trajectory planning unit includes a map information layer planning subunit and a motion information layer planning subunit; The map information layer planning subunit is used to establish a penalty function using a multi-module local perception map, wherein the penalty function includes an obstacle avoidance penalty function, a centroid height penalty function, a pitch angle penalty function, and a gradient information layer penalty function; The motion information layer planning subunit uses the relative relationship of the control points to establish a penalty function, and the motion information layer planning subunit includes a motion capability penalty function, a time allocation penalty function, and a smoothing penalty function.

4. The legged robot visual navigation method for unstructured scenes according to claim 3 is characterized in that The motion information layer planning subunit is represented as: Among them, f smooth represents the smooth penalty function, λ smooth Represents its penalty factor, f fitness represents the athletic ability penalty function, λ fitness Represents its penalty factor, f fessible represents the time allocation penalty function, λ fessible represents its penalty factor.

5. The legged robot visual navigation method for unstructured scenes according to claim 4 is characterized in that The map information layer planning subunit is represented as: in, represents the obstacle avoidance penalty function, represents the centroid height penalty function, represents the pitch angle penalty function, represents the gradient information layer penalty function, represents the adaptive penalty factor Represents the gradient information layer penalty function.

6. The legged robot visual navigation method for unstructured scenes according to claim 3 is characterized in that The process of establishing the centroid height penalty function is as follows: Use the control point of the B-spline to calculate the difference between the projection point of the multi-module local perception map on the z-axis of the world coordinate system, obtain the distance between the restricted control point and the ground, and penalize the distance; The centroid height penalty function is expressed as: in, Represents the total number of control points, i represents the index of the i-th control point, represents the penalty function of the i-th control point, Indicates the difference between the control point height H and MCH, c m,i If the value is greater than the threshold, then the penalty is imposed; otherwise, no penalty is imposed. represents the threshold value, , Indicates the height of the robot's center of mass.

7. The legged robot visual navigation method for unstructured scenes according to claim 6 is characterized in that The process of establishing the pitch angle penalty function is as follows: First, the optimization dimension is restricted to the z-axis of the world coordinate system. Then, two adjacent control points Qi and Qi+1 on the z-axis are obtained, and the difference between the control points Qi and Qi+1 on the z-axis is obtained to approximately represent the tan value of the pitch angle from Qi to Qi+1. Then, a threshold for the robot's pitch angle is set. Finally, the tan value of the pitch angle greater than the threshold is penalized, and the penalty is regularized.

8. The legged robot visual navigation method for unstructured scenes according to claim 7 is characterized in that The pitch angle penalty function is expressed as: Among them, k represents the approximate value of the denominator of tan, the subscripts x, y, and z represent the directions of the x, y, and z axes respectively, and Pitch i represents the pitch angle at the i-th control point.

9. The legged robot visual navigation method for unstructured scenes according to claim 8 is characterized in that The establishment process of the gradient information layer penalty function is: The control points are projected onto the ground information points on the gradient information map unit. Then, the cost and gradient information of the ground information points are used to establish the ground quality label constraint penalty function on the x-axis and y-axis of the world coordinate system respectively. The ground quality label constraint penalty function is expressed as: 。 10. The legged robot visual navigation method for unstructured scenes according to claim 1 is characterized in that The global map is obtained by the following steps: Based on the robot's position and posture information in the world coordinate system and the RGB image and depth image of the scene, the scene point cloud is obtained, and the point cloud is spliced ​​to obtain a global map.

Images