Autonomous navigation method and system of quadruped robot for special environments

Abstract

The present invention relates to the field of navigation technology, and specifically provides a method and system for autonomous navigation of a quadruped robot for special environments, including: using the recognition result of the depth image by the target detection network to generate semantic tags for the depth point cloud data corresponding to the depth image; using the laser radar mileage calculation method to calculate local odometer information according to the laser point cloud data and the inertial sensor data; fusing the depth point cloud data with semantic tags, the laser point cloud data and the local odometer information to obtain the positioning data in the pre-built point cloud map; using the perception method of the regional planner to build a multi-layer cost map according to the pre-built point cloud map, the latest acquired depth image, the laser point cloud data and the inertial sensor data; using the global planning path algorithm and the local path planning algorithm to generate navigation path information according to the positioning data and the multi-layer cost map. The present invention improves the navigation accuracy.

Description

Technical Field

[0001] The present invention belongs to the field of navigation technology, and in particular relates to a quadruped robot autonomous navigation method and system for special environments. Background Art

[0002] In special environments such as underground coal mines and coal preparation plants, equipment inspection is a labor-intensive and dangerous task. The normal operation of equipment is crucial to production safety, but due to the complex operating environment, wheeled platforms cannot operate, so quadruped robots can be used for autonomous navigation inspection.

[0003] However, traditional autonomous navigation methods usually only provide a global path, which is difficult to meet the precise positioning and navigation requirements of quadruped robots in complex terrain such as narrow alleys and ups and downs. Summary of the invention

[0004] In view of the above-mentioned deficiencies in the prior art, the present invention provides a method and system for autonomous navigation of a quadruped robot for special environments to solve the above-mentioned technical problems.

[0005] In a first aspect, the present invention provides a quadruped robot autonomous navigation method for a special environment, comprising:

[0006] Acquire a depth image, and generate a semantic label for the depth point cloud data corresponding to the depth image using a recognition result of the depth image by a target detection network;

[0007] Obtain laser point cloud data and inertial sensor data, and use the laser radar mileage calculation method to calculate local odometer information based on the laser point cloud data and inertial sensor data;

[0008] Fuse the semantically labeled depth point cloud data, laser point cloud data, and local odometer information to obtain positioning data in the pre-built point cloud map;

[0009] Using the perception method of the region planner, a multi-layer cost map is constructed based on the pre-built point cloud map, the latest acquired depth image, laser point cloud data and inertial sensor data;

[0010] Using the global planning path algorithm and the local path planning algorithm, the navigation path information is generated according to the positioning data and the multi-layer cost map.

[0011] In an optional embodiment, a depth image is obtained, and a recognition result of the depth image by a target detection network is used to generate a semantic label for the depth point cloud data corresponding to the depth image, including:

[0012] Convert the depth image in the two-dimensional coordinate system into depth point cloud data in the three-dimensional coordinate system;

[0013] Using a target detection network to identify the depth image, obtain a semantic label of the target area type, wherein the semantic label includes the target type;

[0014] Generate the same semantic labels for the points in the depth point cloud corresponding to the pixels in the target area.

[0015] In an optional embodiment, the depth point cloud data with semantic labels, the laser point cloud data and the local odometer information are fused to obtain the positioning data in the pre-built point cloud map, including:

[0016] Transform the depth point cloud data with semantic labels into the coordinate system of the quadruped robot to obtain semantic point cloud data;

[0017] Use KD tree to build spatial index for semantic point cloud data and assign semantic labels to the nearest neighbor laser point cloud data;

[0018] The dynamic targets in the fused laser point cloud data are removed and smoothed to obtain the fused point cloud at time step t. ;

[0019] Fine Pose Estimation of Quadruped Robots Described as:

[0020]

[0021] in, To pre-build point cloud maps, is the defined rough guess matrix, REG() is the point cloud registration algorithm;

[0022] After the first frame of point cloud matching is completed, obtain the pose transformation from the point cloud to the starting point , calculation formula:

[0023]

[0024] in, is the quadruped robot pose estimate at each time step t obtained from the FAST-LIO radar odometry; is the initial transformation matrix between the map and the quadruped robot body obtained from the manually pointed initial pose; It represents the transformation relationship between the starting point of the quadruped robot body and the map coordinate system, which is a constant;

[0025] Time step The rough guess matrix It can be calculated by the following formula:

[0026]

[0027] in, A local transformation from an initial pose to a quadruped robot maintained for lidar odometry;

[0028] At time step When , the normal distribution transformation algorithm is used to obtain a fine pose estimate, and the point cloud is fused. A grid The point cloud distribution in can be expressed by the following formula:

[0029] ;

[0030] In the above formula represents the mean, Represents the covariance matrix, and the specific calculation method is:

[0031] ;

[0032] ;

[0033] When receiving the point cloud from data fusion When using Transform each point in the square to the reference point cloud space. The transformation formula is: , and calculate the objective function Score:

[0034] ;

[0035] Confirm that the objective function score converges and obtain the optimal registration estimate from the results of the normal distribution transformation algorithm , the quadruped robot in time step Location It can be obtained by the following formula:

[0036] ;

[0037] in yes The inverse matrix of is at the time step A rough guess matrix.

[0038] In an optional implementation, a KD tree is used to construct a spatial index for the semantic point cloud data, and a semantic label is assigned to the nearest neighbor laser point cloud data, including:

[0039] Extracting coordinates and semantic labels of sample points from semantic point cloud data, where the semantic labels include mobile devices, static obstacles, and potholes on the road surface;

[0040] Construct a KD tree using the coordinates of the sample points;

[0041] Taking a point in the laser point cloud data as a target point, searching for the nearest neighbor point of the target point from the KD tree;

[0042] Setting the semantic label of the nearest neighbor point as the semantic label of the target point;

[0043] The laser point cloud data is traversed, and the semantic label obtained for each point is stored as a point attribute.

[0044] In an optional embodiment, a multi-layer cost map is constructed based on a pre-built point cloud map, a newly acquired depth image, laser point cloud data, and inertial sensor data using a perception method of a region planner, including:

[0045] Using the perception method of the region planner, the maximum feasible depth set is generated based on the latest acquired laser point cloud data ;

[0046] For each direction the passable depth ,use The method calculates its coordinates in the Cartesian coordinate system ;

[0047] By transforming the matrix T Transform the coordinates to the odometer coordinate system and get the coordinates ;

[0048] According to the resolution of the grid And the offset of the cost grid map origin in the odometer coordinate system and Calculate the index value in the corresponding grid map ,Will The traversable area grid at is marked as occupied;

[0049] The semantic point cloud data generated by the target detection network based on the latest acquired depth image is used for polar coordinate registration to generate a traversable area with semantic information;

[0050] The maximum feasible depth set is updated using the traversable area with semantic information, and the traversable area in the Cartesian coordinate system is updated synchronously;

[0051] Each time after traversing the passable data generated by a frame of point cloud, only the cost map is updated The cost of the moving part and the part where data changes occur;

[0052] The pre-built point cloud map is used as the static layer, and the traversable area in the Cartesian coordinate system is used as the obstacle layer to construct a multi-layer cost map.

[0053] In an optional embodiment, the multi-layer cost map further includes:

[0054] The dilation layer is used to dilate the obstacles to calculate the cost of each 2D costmap cell;

[0055] Add a static map roughness sublayer and a static map slope sublayer to the static layer. The static map roughness sublayer is used to reflect the roughness of the environment surface in the pre-built point cloud map, and the static map slope sublayer is used to represent the slope information of the environment in the pre-built point cloud map.

[0056] A real-time map roughness sublayer and a real-time map slope sublayer are added to the obstacle layer. The real-time map roughness sublayer is used to reflect the roughness of the environmental surface in the passable area, and the real-time map slope sublayer is used to represent the slope information of the environment in the passable area.

[0057] In an optional implementation, a global planning path algorithm and a local path planning algorithm are used to generate navigation path information according to positioning data and a multi-layer cost map, including:

[0058] Using the Hybrid A* algorithm, taking the positioning data as the current node, generating a global path from the current node to the target node, wherein the global path conforms to the kinematic model of the quadruped robot;

[0059] The time elastic band algorithm is used to take the global path pose points as the initial pose sequence state nodes, and the local trajectory is generated according to the constraints and the distribution of obstacles near each state node.

[0060] The constraint conditions include a slope constraint and a roughness constraint.

[0061] In an optional implementation, the global path pose point is used as the initial pose sequence state node using the time elastic band algorithm, and a local trajectory is generated according to the constraint conditions and the distribution of obstacles near each state node, including:

[0062] The constraint function is expressed as a piecewise continuous and differentiable cost function :

[0063]

[0064] in, Indicates the boundary, Indicates zoom, represents the polynomial order, A small translation representing an approximation;

[0065] The objective function of the time elastic band algorithm is to minimize the distance between the robot's posture and the obstacle. , and ensure that the distance is not less than , the penalty constraint for obstacles is set as:

[0066] ;

[0067] Set the slope threshold of the maximum traversable area of ​​the quadruped robot and roughness constraint threshold , and the grid points greater than the threshold are used as terrain constraint areas, and the maximum passable terrain slope cost constraint is set and roughness constraint cost constraint Add to optimized hypergraph:

[0068] ;

[0069] ;

[0070] in, and Represents the terrain slope threshold s t and the roughness constraint threshold r t The converted cost function boundary is obtained through the following mapping relationship:

[0071] ;

[0072] ;

[0073] The motion trajectory of the quadruped robot is regarded as a sequence of multiple postures, each posture is regarded as a node, constraints are imposed on the nodes, and the constraints are regarded as edges in the graph structure to construct a hypergraph.

[0074] The cost of each constraint is summed up to obtain the total cost of the hypergraph in the current state. The g2o framework is used to iteratively optimize the posture parameters contained in the nodes until the optimal posture sequence corresponding to the minimum total cost is selected, and the optimal posture sequence is integrated into a local trajectory.

[0075] In an optional implementation, after generating the navigation path information, the method further includes:

[0076] Calculate the robot motion control variables based on the adjacent posture differences of the local trajectory and the time difference between adjacent postures;

[0077] The motion control variables are input into a bottom-level control module, which includes a reinforcement learning strategy generator and a proportional-derivative controller.

[0078] In a second aspect, the present invention provides a quadruped robot autonomous navigation system for special environments, comprising:

[0079] A first processing module is used to obtain a depth image, and generate a semantic label for the depth point cloud data corresponding to the depth image using a recognition result of the depth image by a target detection network;

[0080] The second processing module is used to obtain laser point cloud data and inertial sensor data, and calculate local odometer information based on the laser point cloud data and inertial sensor data using a laser radar odometer calculation method;

[0081] Feature fusion module, used to fuse the depth point cloud data with semantic labels, laser point cloud data and local odometer information to obtain the positioning data in the pre-built point cloud map;

[0082] A map building module that uses the perception method of the region planner to build a multi-layer cost map based on pre-built point cloud maps, newly acquired depth images, laser point cloud data, and inertial sensor data;

[0083] The path planning module is used to generate navigation path information based on positioning data and multi-layer cost maps using global planning path algorithm and local path planning algorithm.

[0084] The beneficial effect of the present invention lies in that the autonomous navigation method and system of a quadruped robot for special environments provided by the present invention, aimed at the high-precision positioning requirements of the quadruped robot navigation in special environments, integrates the information of three modes of laser radar, inertial sensor IMU, and depth camera, extracts semantic information in the camera data, and integrates the semantic information into the point cloud information, so that the point cloud information has semantic characteristics, improves the positioning accuracy, and has robustness to dynamic targets during the positioning process.

[0085] In addition, the invention has a reliable design principle, a simple structure and a very broad application prospect. BRIEF DESCRIPTION OF THE DRAWINGS

[0086] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0087] Figure 1 The figure is a schematic diagram of an application scenario of an embodiment of the present invention.

[0088] Figure 2 The figure is a schematic diagram of another application scenario of an embodiment of the present invention.

[0089] Figure 3 is a schematic flow chart of a method according to an embodiment of the present invention.

[0090] Figure 4 It is a schematic diagram of a coordinate transformation TF tree of a method according to an embodiment of the present invention.

[0091] Figure 5 It is a flowchart of the method for detecting the annular traversable area according to an embodiment of the present invention.

[0092] Figure 6 It is a schematic diagram of the effect of updating the annular traversable area according to a method of an embodiment of the present invention.

[0093] Figure 7 It is a schematic diagram of a multi-layer cost map of a method according to an embodiment of the present invention.

[0094] Figure 8 It is a flowchart of a local path planning method according to an embodiment of the present invention.

[0095] Fig. 9 The figure is a schematic diagram of terrain course learning of a method according to an embodiment of the present invention.

[0096] Fig.10 is a schematic block diagram of a system according to an embodiment of the present invention. DETAILED DESCRIPTION

[0097] In order to enable those skilled in the art to better understand the technical solutions in the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

[0098] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art of the present invention. The terms used in the specification of the present invention herein are only for the purpose of describing specific embodiments and are not intended to limit the present invention.

[0099] The key terms appearing in the present invention are explained below.

[0100] The target detection network is a neural network model used in the field of computer vision to identify the location and category of specific target objects in images or videos. Representative models: The YOLO (You Only Look Once) series is a typical representative of the one-stage target detection network. YOLO divides the image into multiple grids, each of which is responsible for predicting the target that falls into the grid. Its detection speed is very fast and can achieve real-time detection, which is suitable for scenes with high real-time requirements. SSD (Single Shot MultiBox Detector) is also a one-stage target detection algorithm. It performs target detection on feature maps of different scales and can detect target objects of different sizes.

[0101] FAST-LIO (FastLiDAR-Inertial Odometry) is an efficient LiDAR-Inertial Odometry method. FAST-LIO combines data from LiDAR and Inertial Measurement Unit (IMU) to achieve high-precision pose estimation through a tightly coupled iterative extended Kalman filter (EKF). The algorithm performs well in fast motion, noisy or cluttered environments, and has the characteristics of high computational efficiency and strong robustness.

[0102] KD tree (K-Dimension tree) is a data algorithm widely used in computer science and computational geometry. It is mainly used to efficiently organize and search points in multidimensional space.

[0103] The NDT algorithm, whose full name is the Normal Distributions Transform algorithm, is a classic point cloud registration algorithm.

[0104] Circular Accessible Depth (CAD) is a robust accessibility representation method for unmanned ground vehicle (UGV) navigation. The CAD method enables UGVs to learn accessibility in a variety of scenarios containing irregular obstacles through a novel representation. This method not only provides richer and more diverse road accessibility information than most 3D detection techniques, thereby improving the safety of autonomous navigation, but also uniformly describes accessibility in a 2D bird's-eye view (BEV), which facilitates subsequent planning and control.

[0105] The Hybrid A* algorithm is a graph search algorithm that is based on the A* algorithm and has been improved to meet the path planning needs of robots such as vehicles with kinematic constraints.

[0106] The TEB algorithm, full name Time Elastic Band algorithm, is an algorithm used for robot path planning and trajectory optimization, and is particularly suitable for real-time navigation in dynamic environments.

[0107] g2o (General Graph Optimization) is an open source C++ library for graph optimization. It is widely used in robotics, computer vision and other fields. It is mainly used to deal with large-scale nonlinear optimization problems. The functions of the g2o framework are as follows:

[0108] Graph optimization basics: G2O expresses optimization problems in the form of a graph. In this graph, nodes (vertices) represent optimization variables, such as the robot's position (position and posture); edges (edges) represent the constraint relationship between nodes, such as the relative transformation relationship between the robot's position at two adjacent moments, or the relationship between the robot's position and the observed map points.

[0109] Hypergraph optimization: A hypergraph is a special graph structure in which one edge can connect multiple nodes. In practical applications, hypergraphs can express complex constraint relationships more flexibly. The g2o framework provides a wealth of tools and algorithms to optimize hypergraphs, minimizing the sum of constraint errors represented by the entire hypergraph by adjusting the states of nodes (i.e., optimizing the values ​​of variables).

[0110] Local trajectory generation: During the robot's motion, the data obtained by sensors (such as lidar, camera, etc.) is used to construct a hypergraph containing pose nodes and constraint edges. After optimizing the hypergraph using the g2o framework, a more accurate pose estimation of the robot at different times can be obtained. These optimized pose sequences constitute the robot's local trajectory, providing key basic information for the robot's navigation, mapping and other tasks.

[0111] RL policy generator, also known as reinforcement learning (RL) policy generator, is a key component in the reinforcement learning system, which is used to generate strategies for intelligent agents to take actions in the environment.

[0112] PD controller, or Proportional - Derivative Controller, is a commonly used feedback control device that is widely used in many fields such as industrial automation, robotic control, aerospace, etc.

[0113] In the mine application scenario, due to the complex terrain, the common method of generating navigation paths based on pre-built maps cannot adapt to the complex terrain. The present invention deploys sensors including laser radar, depth camera, inertial sensor IMU, etc. on the quadruped robot, uses the fusion of laser radar and IMU to perform SLAM (Simultaneous Localization and Mapping) to complete mapping and positioning, completes dynamic and static target recognition through vision, aligns laser and visual information and performs repositioning to improve positioning accuracy; the navigation process completes obstacle recognition through the CAD circular traversable network model to fuse visual target information and plan a reasonable path.

[0114] Of course, in other scenes with complex road environments, such as mountainous terrain, the quadruped robot autonomous navigation for special environments provided by the present invention is also applicable.

[0115] like Figure 1 As shown, the method provided in this embodiment is implemented based on six parts: a sensor module, a positioning and mapping module, a visual perception module, a multimodal data fusion positioning module, a circular passable area detection module, a planning module and a control module.

[0116] First, please refer to Figure 2 The positioning and mapping module uses the FAST-LIO lidar mileage calculation method to fuse the lidar feature points with the IMU data to provide a more accurate odometer data. The information and target detection results are then input into the multimodal data fusion positioning module. This module performs laser vision data registration and uses the semantic information output by the target detection network to semantically classify the point cloud information. The static point cloud information and the previously constructed point cloud map are used for NDT precise positioning to provide accurate positioning information for the robot.

[0117] The obtained semantic information and point cloud information are sent to the annular traversable area detection module, which gives the obstacle cost map in the current state through multi-frame point cloud data.

[0118] Then, the planning module performs global path planning through the map information built in advance and the current positioning information of the robot. Then, the local path planner accepts the path information of the global planner and the cost map given by the traversable area network to perform local path planning. Finally, the planning signal given by the path planning module is input into the control module. First, the RL strategy planner is used to give the joint target position, and then the PD controller outputs the motor control signal according to the joint target position to complete the movement.

[0119] The quadruped robot autonomous navigation method for special environments provided in the embodiment of the present invention is executed by a computer device, and accordingly, the quadruped robot autonomous navigation system for special environments runs in the computer device.

[0120] Figure 3 is a schematic flow chart of a method according to an embodiment of the present invention. Figure 3 The execution subject can be a quadruped robot autonomous navigation system for special environments. According to different requirements, the order of the steps in the flow chart can be changed, and some can be omitted.

[0121] like Figure 3 As shown, the method includes:

[0122] S310, acquiring a depth image, and using a recognition result of the depth image by a target detection network to generate a semantic label for depth point cloud data corresponding to the depth image.

[0123] Get the depth image of the current environment through the depth camera driver. In Python, you can use the OpenCV library to read and process image data.

[0124] Load the pre-trained target detection network model and input the depth image into the model for target recognition. Based on the recognition results, generate semantic labels for the depth point cloud data corresponding to the depth image. You can use the PCL library to convert the depth image into depth point cloud data and add corresponding semantic labels to each point cloud data point based on the target detection results.

[0125] S320, obtaining laser point cloud data and inertial sensor data, and calculating local odometer information based on the laser point cloud data and inertial sensor data using a laser radar odometer calculation method.

[0126] Obtain the laser point cloud data and inertial sensor data at the current moment from the laser radar and inertial sensor.

[0127] Use a lidar odometry method (such as LOAM, LeGO-LOAM, etc.) to calculate local odometry information based on the laser point cloud data and inertial sensor data.

[0128] S330, fusing the depth point cloud data with semantic labels, the laser point cloud data and the local odometer information to obtain the positioning data in the pre-built point cloud map.

[0129] The depth point cloud data with semantic labels, laser point cloud data and local odometer information are fused. Point cloud data can be fused using methods such as feature matching and ICP (Iterative Closest Point) algorithm to align point cloud data from different sources to the same coordinate system.

[0130] Based on the fused point cloud data, positioning is performed in the pre-built point cloud map to generate positioning data. A map matching-based method can be used to match the current point cloud data with the pre-built point cloud map to determine the position and posture of the mobile device in the map.

[0131] S340, using the perception method of the region planner, a multi-layer cost map is constructed based on the pre-built point cloud map, the latest acquired depth image, the laser point cloud data and the inertial sensor data.

[0132] Acquire the latest depth images, laser point cloud data, and inertial sensor data again.

[0133] Using the perception method of the region planner, a multi-layer cost map is constructed based on the pre-built point cloud map, the latest acquired depth image, laser point cloud data, and inertial sensor data. Specifically, the pre-built point cloud map can be used as the static layer, the traversable area in the Cartesian coordinate system can be used as the obstacle layer, and the expansion layer, static map roughness sublayer, static map slope sublayer, real-time map roughness sublayer, and real-time map slope sublayer can be added at the same time.

[0134] S350, using the global planning path algorithm and the local path planning algorithm, generate navigation path information according to the positioning data and the multi-layer cost map.

[0135] Use a global planning path algorithm (such as A* algorithm, Dijkstra algorithm, etc.) to plan a global path from the current position to the target position based on the positioning data and multi-layer cost map.

[0136] Use local path planning algorithms (such as DWA (Dynamic Window Approach), T-RRT (Time-Elapsed Rapidly-Exploring Random Trees), etc.) to refine and adjust the global path and generate navigation path information suitable for real-time execution on mobile devices.

[0137] In an embodiment of the present invention, based on step S310, a possible embodiment is given below to illustrate its specific implementation scheme in a non-limiting manner.

[0138] S3101. Convert the depth image in the two-dimensional coordinate system into depth point cloud data in the three-dimensional coordinate system.

[0139] The depth camera first captures a depth image of the scene, which contains the distance information of each pixel from the camera.

[0140] Using the distance information in the depth image and combining it with the camera's intrinsic parameters (such as focal length, optical center position, etc.), the two-dimensional coordinates (u, v) and distance information (d) of each pixel can be converted into coordinates (X, Y, Z) in three-dimensional space.

[0141] The converted three-dimensional coordinate points are combined to form point cloud data. If the camera also captures a color image, the color information in the color image can be assigned to each point in the point cloud, thereby generating point cloud data containing color information.

[0142] S3102. Use a target detection network to identify the depth image to obtain a semantic label of the target area type, where the semantic label includes the target type.

[0143] According to application requirements and performance requirements, the YOLOv8 model is selected as the target detection network.

[0144] Download and load the pre-trained weight files of the corresponding network. These weight files are trained on large-scale datasets and contain the feature parameters learned by the network.

[0145] Preprocess the depth image to make it meet the input requirements of the target detection network. Common preprocessing operations include image scaling, normalization, etc.

[0146] The preprocessed depth image is input into the object detection network for inference to obtain the detection result. The detection result usually contains information such as the bounding box coordinates, category label, and confidence score of the target object.

[0147] The category label of the target area is extracted from the detection results as a semantic label.

[0148] S3103. Generate the same semantic label for the points in the depth point cloud corresponding to the pixel points in the target area.

[0149] The bounding box of the target area obtained by target detection can determine the pixels within the target area in the depth image. Then, based on the correspondence between the depth image and the depth point cloud, the points in the depth point cloud corresponding to these pixels are found and the same semantic labels are assigned to them.

[0150] In an embodiment of the present invention, based on step S320, a possible embodiment is given below to illustrate its specific implementation scheme in a non-limiting manner.

[0151] Maintaining the local transformation from the initial pose to the quadruped robot using FAST-LIO lidar odometry FAST-LIO uses a tightly coupled iterative Kalman filter algorithm to fuse lidar feature points with IMU data to provide a relatively accurate local odometer information. We use this virtual odometer to provide an initial rough estimate for subsequent point cloud registration.

[0152] S3201.FAST-LIO defines the system state vector, which contains the robot's position, attitude (expressed as quaternions), velocity, IMU's gyroscope bias, and accelerometer bias.

[0153] S3202. By integrating the IMU measurement, a predicted value of the state can be obtained, and the system state is predicted based on the pre-integration result of the IMU measurement data.

[0154] S3203. Extract corner and plane point features from LiDAR point clouds. Common methods include curvature-based feature extraction, which calculates the curvature of each point to determine whether it is a corner or a plane point. Align the current LiDAR point cloud with the previous point cloud to find the transformation relationship between the point clouds. FAST-LIO uses a variant of the Iterative Closest Point (ICP) algorithm for point cloud alignment.

[0155] S3204. Define the observation model to link the observation of the lidar feature points with the system state. And use Kalman filtering to update the system state and covariance matrix.

[0156] S3205. Through the above prediction and update steps, the local transformation from the initial posture to the quadruped robot is obtained by continuous iterative calculation. This local transformation can be expressed as a homogeneous transformation matrix .

[0157] In an embodiment of the present invention, based on step S330, a possible embodiment is given below to illustrate its specific implementation scheme in a non-limiting manner.

[0158] S3301. Transform the depth point cloud data with semantic labels into the quadruped robot coordinate system to obtain semantic point cloud data.

[0159] S3302. Use the KD tree to build a spatial index for the semantic point cloud data and assign semantic labels to the nearest neighbor laser point cloud data.

[0160] After aligning the semantic point cloud data B and the laser point cloud data A, the two point cloud data are crossed together, and the KD tree is used to build a spatial index for the semantic point cloud, and then the nearest neighbor laser radar point cloud is assigned. The specific process is as follows:

[0161] (1) Build a spatial index.

[0162] Extract the coordinates and semantic information of the points from point cloud B; use the coordinate data of point cloud B to build a KD-Tree or Octree index to quickly query the nearest neighbor points.

[0163] (2) Assign semantic information to each point in point cloud A.

[0164] Traverse each point in point cloud A; select the semantic information matching strategy according to the needs:

[0165] Nearest neighbor matching: Search point cloud B for the nearest neighbor of each point in point cloud A; assign the semantic information of the nearest neighbor to the current point in point cloud A;

[0166] Weighted average (applicable to dense point clouds): Search for the K nearest neighbor points of each point in point cloud B; calculate the weight according to the inverse of the distance; use the weighted average method to fuse the semantic information of the K nearest neighbor points and assign it to the current point of point cloud A.

[0167] (3) Update point cloud A.

[0168] Store the semantic information of point cloud A as point attributes (such as color, category label, etc.); merge the updated semantic information with the original point cloud data.

[0169] S3303. Remove dynamic targets from the fused laser point cloud data and perform smoothing to obtain a fused point cloud at time step t. .

[0170] S3304. Fine Pose Estimation of Quadruped Robots Described as:

[0171]

[0172] in, To pre-build point cloud maps, is the defined rough guess matrix, and REG() is the point cloud registration algorithm.

[0173] S3305. Calculation , please refer to Figure 4 .

[0174] After the first frame of point cloud matching is completed, obtain the pose transformation from the point cloud to the starting point , calculation formula:

[0175]

[0176] in, is the quadruped robot pose estimate at each time step t obtained from the FAST-LIO radar odometry; is the initial transformation matrix between the map and the quadruped robot body obtained from the manually pointed initial pose; It represents the transformation relationship between the starting point of the quadruped robot body and the map coordinate system, which is a constant. Since the map coordinate system and the starting point are fixed, this transformation relationship is also fixed throughout the navigation task.

[0177] Therefore, the time step The rough guess matrix It can be calculated by the following formula:

[0178]

[0179] in, The local transformation from the initial pose to the quadruped robot maintained for lidar odometry.

[0180] S3306. Calculation .

[0181] At time step When , the normal distribution transformation algorithm is used to obtain a fine pose estimate, and the point cloud is fused. A grid The point cloud distribution in can be expressed by the following formula:

[0182] ;

[0183] In the above formula represents the mean, Represents the covariance matrix, and the specific calculation method is:

[0184] ;

[0185] ;

[0186] When receiving the point cloud from data fusion When using Transform each point in the square to the reference point cloud space. The transformation formula is: , and calculate the objective function Score:

[0187] ;

[0188] Confirm that the objective function score converges and obtain the optimal registration estimate from the results of the normal distribution transformation algorithm .

[0189] S3307. Quadruped robot in time step Location It can be obtained by the following formula:

[0190] ;

[0191] in yes The inverse matrix of is at the time step A rough guess matrix.

[0192] In an embodiment of the present invention, based on step S340, a possible embodiment is given below to illustrate its specific implementation scheme in a non-limiting manner. Figure 5 .

[0193] In order to obtain information about various obstacles in a large scale in special environments such as underground coal mines and coal preparation plants, and to ensure that the quadruped robot does not cause harm to mobile working equipment and workers, Circular Accessible Depth (CAD for short) is used as the perception method of the regional planner. It is a robust accessibility representation method. CAD can use multi-line lidar point cloud, visual sensor information and map information to predict the boundary of the accessible area and obtain the accessible depth probability distribution map based on the polar coordinate grid.

[0194] Original point cloud construction region planner cost map algorithm example:

[0195] 1. Input: point cloud sequence

[0196] 2.Output: Cost map

[0197] 3.for in do

[0198] 4. ⟵

[0199] 5.for in do

[0200] 6. ⟵

[0201] 7. ⟵

[0202] 8. ⟵

[0203] 9.

[0204] 10. end for

[0205] 11.

[0206] 12. end for

[0207] 13.return

[0208] The map generation algorithm is explained as follows:

[0209] : Given a point in polar coordinates , returns a point in Cartesian coordinates ,in:

[0210] ;

[0211] : given a point in the robot base coordinate system , which will return the point in the odometry coordinate system The calculation formula is:

[0212] ;

[0213] in, represents the transformation matrix from the odometer coordinate system to the base coordinate system, .

[0214] :The origin of the odometer coordinate system is The cost grid coordinate system takes the center of the first cell in the lower left corner of the grid map as its origin, and uses Indicates that we set the grid coordinate system to always be consistent with the direction of the odometer coordinate system. When the cost map grid moves with the robot, it is assumed that its origin The offset in the odometer coordinate system is and , then the point in the grid coordinate system The coordinates can be calculated:

[0215] ;

[0216] Due to the resolution of the grid Knowing that, we can calculate the point Index in the grid :

[0217] ;

[0218] : According to the index , the cost map The cost value corresponding to the grid is marked as occupied.

[0219] : Update costmap .

[0220] Specifically, the costmap construction process includes the following steps:

[0221] S3401. Generate the maximum feasible depth set based on the latest laser point cloud data using CAD algorithm .

[0222] (1) Filter the latest laser point cloud data to remove noise points and outliers to improve the quality and accuracy of the data. Methods such as statistical filtering and radius filtering can be used.

[0223] (2) The processed laser point cloud data is used to construct a KD-Tree data structure to quickly perform nearest neighbor search. KD-Tree is a tree-like data structure used for storing and retrieving high-dimensional spatial data points, which can greatly improve search efficiency.

[0224] (3) Divide the space around the robot into multiple directions. For example, divide the 360° space into multiple sectors at certain angle intervals (such as 5°) to form multiple sectors.

[0225] (4) For each sector, start from the laser radar position and search along the ray in that direction. During the search, when the first point cloud point is encountered, the distance to that point is recorded as the maximum feasible depth in that direction. At the same time, the depth is evaluated according to the pre-set cost model. If the environmental cost around the point is high (for example, close to an obstacle), the maximum feasible depth is appropriately adjusted to ensure safety.

[0226] (5) Summarize the maximum feasible depths calculated in all directions to generate a maximum feasible depth set.

[0227] S3402. For the passable depth in each direction ,use The method calculates its coordinates in the Cartesian coordinate system .

[0228] S3403. Transform the matrix T Transform the coordinates to the odometer coordinate system and get the coordinates .

[0229] S3404. According to the resolution of the grid And the offset of the cost grid map origin in the odometer coordinate system and Calculate the index value in the corresponding grid map ,Will The passable area grid at is marked as occupied, indicating that the location is a passable area.

[0230] S3405. Use the target detection network to perform polar coordinate alignment on the semantic point cloud data generated based on the latest acquired depth image to generate a traversable area with semantic information.

[0231] The latest acquired depth image is input into the object detection network, which identifies different target objects in the image and assigns a semantic label to each target object (such as "equipment", "wall", "puddle", etc.). Then, combined with the depth image information, each pixel is converted into three-dimensional point cloud data, and each point cloud data point is assigned a corresponding semantic label to generate semantic point cloud data.

[0232] Convert semantic point cloud data from Cartesian coordinate system to polar coordinate system. Use polar coordinate registration algorithm based on feature matching or iterative closest point (ICP). By finding the corresponding feature points of semantic point cloud data in polar coordinate system, calculating rotation and translation transformation, the semantic point cloud data is registered to a unified coordinate system. In the registration process, semantic information is used to improve the accuracy and robustness of registration, such as more accurate matching of point clouds of the same semantic category.

[0233] According to the registered semantic point cloud data, analyze which areas are passable. For example, if an area is not occupied by the semantic point cloud marked as an obstacle, it is divided into a passable area and the corresponding semantic information is given to the area.

[0234] S3406. Update the maximum feasible depth set using the traversable area with semantic information, and synchronously update the traversable area in the Cartesian coordinate system.

[0235] For the passable area with semantic information, check its boundaries in all directions. If the passable boundary distance in a certain direction is greater than the depth value of the corresponding direction in the current maximum feasible depth set, update the depth value of that direction in the maximum feasible depth set.

[0236] According to the updated maximum feasible depth set, the coordinates of the passable points in each direction in the Cartesian coordinate system are recalculated. For the new passable points, according to the methods of S3402 and S3403, they are converted to the odometer coordinate system and the passable area representation in the Cartesian coordinate system is updated.

[0237] S3407. Each time after traversing the passable data generated by a frame of point cloud, only the cost map is updated The cost of the moving part and the part where data changes occur.

[0238] By comparing the position information of the current frame point cloud data with the previous frame point cloud data (such as the change in the robot's posture), determine which parts of the cost map need to be updated due to the movement of the robot. This can be achieved by analyzing the odometry information or the relative transformation of the point cloud.

[0239] For each grid cell, compare whether the passable data corresponding to the grid cell in the current frame and the previous frame has changed. If the passable state (occupied or unoccupied) or semantic information has changed, mark the grid cell as a data change part.

[0240] For the detected moving parts and data change parts of the grid cells, their costs are updated according to the pre-defined cost update rules. For example, if an originally unoccupied grid becomes occupied and the semantic information of the area is represented as an obstacle, the cost of the grid is increased; conversely, if an occupied grid becomes unoccupied, its cost is reduced accordingly.

[0241] The update effect of the traversable area is as follows Figure 6 shown.

[0242] S3408. Use the pre-built point cloud map as the static layer and the traversable area in the Cartesian coordinate system as the obstacle layer to construct a multi-layer cost map.

[0243] First, the pre-built point cloud map is used as a static layer. The pre-built point cloud map is constructed by using advanced LiDAR and other sensor technologies to scan a specific area in all directions and obtain a large amount of spatial point data. These point cloud data accurately depict the spatial position and shape information of various objects in the area. Using them as a static layer provides a stable basic framework for the entire cost map, covering relatively fixed environmental elements such as buildings and road infrastructure.

[0244] Secondly, the passable area in the Cartesian coordinate system is used as the obstacle layer. The Cartesian coordinate system is a coordinate system widely used in mathematics and physics. It provides an accurate way to describe spatial positions. In this scenario, the data obtained by the sensor is analyzed and processed to determine which areas are passable in the Cartesian coordinate system. The parts outside these passable areas are identified as obstacles, thus forming an obstacle layer. This layer is crucial for mobile devices to accurately identify and avoid obstacles and plan safe driving paths.

[0245] Based on the above static layer and obstacle layer, a multi-layer cost map is further constructed. In addition to these two basic layers, the multi-layer cost map also has other important components. The specific structure is as follows Figure 7 As shown, it includes the main cost map (including static layer and obstacle layer), expansion layer, real-time map roughness layer, real-time map slope layer, static map roughness layer, and static map slope layer.

[0246] First, the expansion layer. The main function of the expansion layer is to reasonably expand the obstacles so as to calculate the cost of each 2D cost map unit. In practical applications, since mobile devices themselves have a certain size, simply judging whether they are passable by the precise obstacle boundary is not enough to ensure safety. Therefore, by expanding the range of obstacles, the safety margin of mobile devices during driving can be more comprehensively considered. When calculating the cost of each 2D cost map unit, the closer the unit is to the expanded obstacle, the higher its cost; conversely, the farther the unit is, the lower its cost. This cost calculation method can guide mobile devices to avoid obstacles as much as possible and choose safer paths.

[0247] Second, add the static map roughness sublayer and the static map slope sublayer to the static layer. The static map roughness sublayer is used to reflect the roughness of the surface of the static environment. Different surface roughness will have different degrees of impact on the driving of the mobile device. For example, a surface that is too rough may increase the wear of the device, reduce the driving speed, and even affect the stability of the device. By adding this sublayer, the mobile device can comprehensively consider the surface roughness factor when planning the path and choose a more suitable route. The static map slope sublayer is used to represent the slope information of the static environment. The size of the slope is directly related to the power demand and driving safety of the mobile device. A larger slope may require the device to consume more energy and may even cause the device to lose control. Therefore, this sublayer can help mobile devices plan paths in advance, avoid overly steep areas, and ensure smooth and safe driving.

[0248] Third, add the real-time map roughness sublayer and real-time map slope sublayer to the obstacle layer. The real-time map roughness sublayer and real-time map slope sublayer are similar to the corresponding sublayers in the static layer, but they focus more on reflecting the surface characteristics of the obstacle area that changes in real time. In actual scenarios, the situation of obstacles may change at any time, such as the emergence of new temporary obstacles or the movement of obstacles. Through these two real-time sublayers, mobile devices can obtain these change information in real time and adjust the path planning strategy in time to adapt to dynamic environmental changes.

[0249] In an embodiment of the present invention, based on step S350, a possible embodiment is given below to illustrate its specific implementation scheme in a non-limiting manner.

[0250] S3501. Using the Hybrid A* algorithm, with the positioning data as the current node, a global path from the current node to the target node is generated, and the global path conforms to the kinematic model of the quadruped robot.

[0251] The Hybrid A* algorithm combines the heuristic search advantages of the A algorithm with the robot kinematic model, aiming to solve the path planning problem with kinematic constraints. It uses the evaluation function of the A algorithm as = As the basic framework, From the starting point to the current node The actual cost, It is a heuristic estimated cost from the current node to the target node. However, unlike the A algorithm, Hybrid A* fully considers the robot's kinematic characteristics during the search process, such as the minimum turning radius, speed limit, etc. By discretizing the search space into grid cells and generating feasible motion trajectories (action sets) in each cell based on the kinematic model, it converts between the continuous configuration space and the discrete grid space to find a path that meets the kinematic constraints and can approach the optimal path. For example, in an autonomous driving scenario, it can plan a smooth path that the car can actually drive, rather than an ideal path based only on geometric space.

[0252] The algorithm flow is as follows:

[0253] 1. Initialization phase

[0254] Determine the starting node and the target node.

[0255] Initialize the open list and the closed list, put the starting node into the open list, and calculate its initial value.

[0256] 2. Main search loop

[0257] Select a value from an open list The smallest node as the current node.

[0258] If the current node is the target node, the initial path is constructed by backtracking the parent node and entering the path smoothing phase.

[0259] Remove the current node from the open list and add it to the closed list.

[0260] According to the kinematic model, the current node Generate adjacent feasible nodes (by applying a set of actions).

[0261] For each adjacent node Do the following:

[0262] like In the closed list and the new path to If the cost of the next node is not better than the previous path, the node is skipped.

[0263] like Not in the open list, calculate its Value, set the parent node to , and add it to the open list.

[0264] like If the cost of the new path to the open list is better, update it. The value and parent node are .

[0265] 3. Path smoothing stage

[0266] Since the searched path may be discrete and jagged, the initial path obtained by backtracking is smoothed using methods such as spline curve fitting to obtain a path that is more suitable for actual motion execution, and finally output a path planning result that complies with kinematic constraints and is relatively smooth.

[0267] S3502. Use the time elastic band algorithm to take the global path pose points as the initial pose sequence state nodes, and generate local trajectories based on the constraints and the distribution of obstacles near each state node; the constraints include slope constraints and roughness constraints.

[0268] The core idea of ​​TEB is to construct the posture sequence and time interval sequence as the basic nodes of the graph. Different constraints constitute the constraint edges between each node. Since an edge may connect more than two nodes, these basic nodes and constraint edges constitute a hypergraph. A sub-objective function is calculated for each constraint item, and their weighted sum is used to form a main objective function. Then, a real-time weighted multi-objective optimization framework is used to adjust the posture sequence and time difference sequence to make the objective function optimal.

[0269] like Figure 7The figure shows the overall control flow of the TEB algorithm. In the initialization phase of the TEB algorithm, the global path pose point is used as the initial pose sequence state node, and the initial time difference variable is calculated based on the dynamic and kinematic constraints of the quadruped robot. Then, according to the distribution of obstacles near each state node, the information of the closest obstacle is maintained. After that, a hypergraph is constructed with the pose nodes, time difference nodes, and each constraint edge, and sent to the G2O framework for graph optimization calculation. The algorithm will dynamically adjust the spatial resolution and time resolution of the trajectory according to the actual situation. After obtaining the optimized pose nodes and time difference nodes, the actual required linear velocity, angular velocity and other control variables are calculated. The above control process stops the loop after the robot reaches the vicinity of the target point.

[0270] The purpose of local path planning is to plan a safe path that guides the robot to travel along the global planned path without colliding with obstacles. TEB is a path planning method based on graph optimization. It regards the robot's motion trajectory as a sequence of multiple postures, each posture as a node, and imposes various constraints on the nodes, such as obstacle constraints, speed constraints, etc., and the constraints can be regarded as edges in the graph structure. The costs calculated for each constraint are summed to obtain the total cost of the graph in the current state, and the parameters contained in the nodes are continuously adjusted to minimize the total cost, so as to plan a path that meets the requirements.

[0271] The original TEB algorithm relies on the traditional cost map and sets obstacle constraints to make the robot avoid these areas. The obstacle judgment indicator is the terrain height, which is not suitable for quadruped robots with terrain traversal capabilities. Based on the terrain slope and terrain roughness cost map, we proposed an improved TEB algorithm, adding slope constraints and roughness constraints to TEB to give full play to the flexibility of quadruped robots in complex terrain.

[0272] In the TEB algorithm, the constraint function is expressed as a piecewise continuous and differentiable cost function :

[0273] ;

[0274] in, Indicates the boundary, Indicates zoom, represents the polynomial order, Represents a small shift of the approximation.

[0275] In the TEB algorithm, the objective function aims to minimize the distance between the robot's posture and the obstacle. , and ensure that the distance is not less than , the penalty constraint for obstacles is set as:

[0276] ;

[0277] Set the maximum passable terrain slope threshold for quadruped robots and roughness constraint threshold , and take the grid points greater than the threshold as the terrain constraint area, refer to the obstacle constraint function, design the maximum passable terrain slope cost constraint and roughness constraint cost constraint and add them to the TEB optimization hypergraph:

[0278] ;

[0279] ;

[0280] in, and They represent the cost function boundaries after the conversion of the terrain slope threshold st and the roughness constraint threshold rt, which are obtained through the following mapping relationship:

[0281] ;

[0282] ;

[0283] In this embodiment, direct mapping is adopted, that is, , In some other implementations, a suitable mapping relationship f can also be flexibly designed according to actual needs.

[0284] When constructing the hypergraph, the terrain constraint area is added as a new node in the hypergraph, and then the hypergraph is optimized through the g2o framework to generate local trajectories. After obtaining the local trajectory, the difference between the adjacent poses of the local trajectory and the time difference between the two poses can be used to calculate the local trajectory. Calculate the robot motion control variables and send them to the underlying control module to control the robot motion. When the local elevation grid map is updated in real time, a new local trajectory planning will be performed.

[0285] The purpose of this improvement is to take into account that quadruped robots are more dependent on the slope and friction of the ground than wheeled and other ground robots. In other words, they are more sensitive to changes in these two characteristics. Therefore, we design such path planning to ensure that the quadruped robot always maintains its own state of motion during navigation, avoids slipping, rollover and other dangerous situations as much as possible, and ensures the safety of the entire navigation process.

[0286] On the basis of the above embodiments, in order to further improve the motion adaptability of the quadruped robot, as an implementable method, a reinforcement learning strategy generator and a proportional differential controller are used to control the motion of the quadruped robot.

[0287] The control module is composed of a strategy network that can face various terrains through course reinforcement learning. For a quadruped robot, after sensing the terrain information, it adjusts different strategies to adapt to different scenarios (such as stairs, uphill, etc.). After outputting the strategy, the joint motor position is output to the PD controller, and the PD controller outputs the motor signal to complete the action. In this process, if the RL strategy fails and the output joint position does not conform to the model of the quadruped robot, the PD control acts as a buffer to ensure stable control of the strategy signal.

[0288] The terrain course learning process of RL policy generator is as follows Fig. 9 shown.

[0289] Since quadruped robots need to take into account multiple aspects when learning motion control strategies, such as maintaining stability, command following, gait generation, and terrain crossing, it is difficult for robots to learn all the knowledge directly from scratch. Curriculum learning is a progressive training strategy that imitates the human learning process. It first allows the model to learn simple samples and gradually advance to complex samples and knowledge learning. Curriculum learning has been applied in scenarios such as computer vision and natural language processing, and has been proven to be of great significance for accelerating model training and improving model generalization capabilities. In this paper, in order to let the robot learn the main goal first (complete posture balance and speed command following on a relatively flat ground), curriculum learning methods are used in terrain learning and reward function setting.

[0290] In terrain course learning, n map scenes are generated for each type of terrain, corresponding to n difficulty levels. The higher the difficulty level, the higher the terrain undulation and the more difficult it is to pass. The robot course level update rules are as follows: Figure 8 As shown: First, the robot is randomly initialized to maps of different course levels; when the robot's movement distance in the current level map exceeds half of the current map size or exceeds 80% of the speed command multiplied by the round time, it is considered that the current robot can achieve motion control under the current terrain difficulty. At this time, the robot course difficulty is increased and it is placed in a more difficult terrain to continue learning; conversely, when the robot's movement distance in the current level map is less than 30% of the current map size or less than 50% of the speed command multiplied by the round time, it is reset to a low-difficulty level terrain. In the reward function course, in order to prioritize learning the main target reward, set the secondary reward weight factor During the learning process, the proportion of the secondary reward function in the total reward function is gradually increased, so that the robot can gradually learn the secondary goals (gait, smooth landing point) on the basis of first learning the basic skills.

[0291] The control principle of the PD controller is as follows:

[0292] The quadruped robot controls the motion of the body by controlling the position or torque of the 12 joint motors. This patent defines the output action of the reinforcement learning agent as , which contains 12-dimensional joint position increment instructions 4-dimensional gait rhythm control signal . Joint position instructions Can be based on position increment instructions The calculation results are:

[0293] ;

[0294] After obtaining the joint position command, the corresponding joint torque is calculated by the PD controller To control the motor, the joint torque calculation formula is as follows:

[0295] ;

[0296] in, and They are the proportional adjustment coefficient and the differential adjustment coefficient, which are set to fixed values ​​in simulation and deployment. The joint velocity command Set to 0.

[0297] In some embodiments, the quadruped robot autonomous navigation system for special environments may include multiple functional modules composed of computer program segments. The computer program of each program segment in the quadruped robot autonomous navigation system for special environments may be stored in the memory of a computer device and executed by at least one processor to execute (see Figure 1 Description) The function of autonomous navigation of a quadruped robot for special environments.

[0298] In this embodiment, the quadruped robot autonomous navigation system for special environments can be divided into multiple functional modules according to the functions it performs, such as Fig.10 As shown. The functional modules of the system 1000 may include: a first processing module 1010, a second processing module 1020, a feature fusion module 1030, a map construction module 1040, and a path planning module 1050. The module referred to in the present invention refers to a series of computer program segments that can be executed by at least one processor and can complete fixed functions, which are stored in a memory. In this embodiment, the functions of each module will be described in detail in subsequent embodiments.

[0299] Although the present invention has been described in detail with reference to the accompanying drawings and in combination with the preferred embodiments, the present invention is not limited thereto. Without departing from the spirit and essence of the present invention, a person of ordinary skill in the art may make various equivalent modifications or substitutions to the embodiments of the present invention, and these modifications or substitutions shall be within the scope of the present invention. Any person of ordinary skill in the art may easily think of changes or substitutions within the technical scope disclosed by the present invention, and these shall be within the scope of protection of the present invention.

Claims

1. A quadruped robot autonomous navigation method for special environments, characterized in that: include: Acquire a depth image, and generate a semantic label for the depth point cloud data corresponding to the depth image using a recognition result of the depth image by a target detection network; Obtain laser point cloud data and inertial sensor data, and use the laser radar mileage calculation method to calculate local odometer information based on the laser point cloud data and inertial sensor data; Fuse the semantically labeled depth point cloud data, laser point cloud data, and local odometer information to obtain positioning data in the pre-built point cloud map; Using the perception method of the region planner, a multi-layer cost map is constructed based on the pre-built point cloud map, the latest acquired depth image, laser point cloud data and inertial sensor data; Generate navigation path information based on positioning data and multi-layer cost maps using global planning path algorithm and local path planning algorithm; Using the perception method of the region planner, a multi-layer cost map is constructed based on the pre-built point cloud map, the latest acquired depth image, laser point cloud data and inertial sensor data, including: Using the perception method of the region planner, the maximum feasible depth set is generated based on the latest acquired laser point cloud data ; For each direction the passable depth ,use The method calculates its coordinates in the Cartesian coordinate system ; By transforming the matrix T Transform the coordinates to the odometer coordinate system and get the coordinates ; According to the resolution of the grid And the offset of the cost grid map origin in the odometer coordinate system and Calculate the index value in the corresponding grid map ,Will The traversable area grid at is marked as occupied; The semantic point cloud data generated by the target detection network based on the latest acquired depth image is used for polar coordinate registration to generate a traversable area with semantic information; The maximum feasible depth set is updated using the traversable area with semantic information, and the traversable area in the Cartesian coordinate system is updated synchronously; Each time after traversing the passable data generated by a frame of point cloud, only the cost map is updated The cost of the moving part and the part where data changes occur; The pre-built point cloud map is used as the static layer, and the traversable area in the Cartesian coordinate system is used as the obstacle layer to construct a multi-layer cost map.

2. The method according to claim 1, characterized in that Acquire a depth image, and generate a semantic label for depth point cloud data corresponding to the depth image using a recognition result of the depth image by a target detection network, including: Convert the depth image in the two-dimensional coordinate system into depth point cloud data in the three-dimensional coordinate system; Using a target detection network to identify the depth image, obtain a semantic label of the target area type, wherein the semantic label includes the target type; Generate the same semantic labels for the points in the depth point cloud corresponding to the pixels in the target area.

3. The method according to claim 1, characterized in that: The depth point cloud data with semantic labels, laser point cloud data and local odometer information are fused to obtain the positioning data in the pre-built point cloud map, including: Transform the depth point cloud data with semantic labels into the coordinate system of the quadruped robot to obtain semantic point cloud data; Use KD tree to build spatial index for semantic point cloud data and assign semantic labels to the nearest neighbor laser point cloud data; The dynamic targets in the fused laser point cloud data are removed and smoothed to obtain the fused point cloud at time step t. ; Fine Pose Estimation of Quadruped Robots Described as: in, To pre-build point cloud maps, is the defined rough guess matrix, REG() is the point cloud registration algorithm; After the first frame of point cloud matching is completed, obtain the pose transformation from the point cloud to the starting point , calculation formula: in, is the quadruped robot pose estimate at each time step t obtained from the FAST-LIO radar odometry; is the initial transformation matrix between the map and the quadruped robot body obtained from the manually pointed initial pose; It represents the transformation relationship between the starting point of the quadruped robot body and the map coordinate system, which is a constant; Time step The rough guess matrix It can be calculated by the following formula: in, A local transformation from an initial pose to a quadruped robot maintained for lidar odometry; At time step When , the normal distribution transformation algorithm is used to obtain a fine pose estimate, and the point cloud is fused. A grid The point cloud distribution in can be expressed by the following formula: ； In the above formula represents the mean, Represents the covariance matrix, and the specific calculation method is: ； ； When receiving the point cloud from data fusion When using Transform each point in the square to the reference point cloud space. The transformation formula is: , and calculate the objective function Score: ； Confirm that the objective function score converges and obtain the optimal registration estimate from the results of the normal distribution transformation algorithm , the quadruped robot in time step Location It can be obtained by the following formula: ； in yes The inverse matrix of is at the time step A rough guess matrix.

4. The method according to claim 3, characterized in that: Use KD tree to build spatial index for semantic point cloud data and assign semantic labels to the nearest neighbor laser point cloud data, including: Extracting coordinates and semantic labels of sample points from semantic point cloud data, where the semantic labels include mobile devices, static obstacles, and potholes on the road surface; Construct a KD tree using the coordinates of the sample points; Taking a point in the laser point cloud data as a target point, searching for the nearest neighbor point of the target point from the KD tree; Setting the semantic label of the nearest neighbor point as the semantic label of the target point; The laser point cloud data is traversed, and the semantic label obtained for each point is stored as a point attribute.

5. The method according to claim 4, characterized in that The multi-layer cost map also includes: The dilation layer is used to dilate the obstacles to calculate the cost of each 2D costmap cell; Add a static map roughness sublayer and a static map slope sublayer to the static layer. The static map roughness sublayer is used to reflect the roughness of the environment surface in the pre-built point cloud map, and the static map slope sublayer is used to represent the slope information of the environment in the pre-built point cloud map. A real-time map roughness sublayer and a real-time map slope sublayer are added to the obstacle layer. The real-time map roughness sublayer is used to reflect the roughness of the environmental surface in the passable area, and the real-time map slope sublayer is used to represent the slope information of the environment in the passable area.

6. The method according to claim 1, characterized in that Using the global planning path algorithm and the local path planning algorithm, the navigation path information is generated according to the positioning data and the multi-layer cost map, including: Using the Hybrid A* algorithm, taking the positioning data as the current node, generating a global path from the current node to the target node, wherein the global path conforms to the kinematic model of the quadruped robot; The time elastic band algorithm is used to take the global path pose points as the initial pose sequence state nodes, and the local trajectory is generated according to the constraints and the distribution of obstacles near each state node. The constraint conditions include a slope constraint and a roughness constraint.

7. The method according to claim 6, characterized in that The time elastic band algorithm is used to take the global path pose points as the initial pose sequence state nodes. According to the constraints and the distribution of obstacles near each state node, the local trajectory is generated, including: The constraint function is expressed as a piecewise continuous and differentiable cost function : in, Indicates the boundary, Indicates zoom, represents the polynomial order, A small translation representing an approximation; The objective function of the time elastic band algorithm is to minimize the distance between the robot's posture and the obstacle. , and ensure that the distance is not less than , the penalty constraint for obstacles is set as: ； Set the slope threshold of the maximum traversable area of ​​the quadruped robot and roughness constraint threshold , and the grid points greater than the threshold are used as terrain constraint areas, and the maximum passable terrain slope cost constraint is set and roughness constraint cost constraint Add to optimized hypergraph: ； ； in, and Represents the terrain slope threshold s t and the roughness constraint threshold r t The converted cost function boundary is obtained through the following mapping relationship: ； ； The motion trajectory of the quadruped robot is regarded as a sequence of multiple postures, each posture is regarded as a node, constraints are imposed on the nodes, and the constraints are regarded as edges in the graph structure to construct a hypergraph. The cost of each constraint is summed up to obtain the total cost of the hypergraph in the current state. The g2o framework is used to iteratively optimize the posture parameters contained in the nodes until the optimal posture sequence corresponding to the minimum total cost is selected, and the optimal posture sequence is integrated into a local trajectory.

8. The method according to claim 1, characterized in that: After generating the navigation path information, the method further includes: Calculate the robot motion control variables based on the adjacent posture differences of the local trajectory and the time difference between adjacent postures; The motion control variables are input into a bottom-level control module, which includes a reinforcement learning strategy generator and a proportional-derivative controller.

9. A quadruped robot autonomous navigation system for special environments, characterized in that: include: A first processing module is used to obtain a depth image, and generate a semantic label for the depth point cloud data corresponding to the depth image using a recognition result of the depth image by a target detection network; The second processing module is used to obtain laser point cloud data and inertial sensor data, and calculate local odometer information based on the laser point cloud data and inertial sensor data using a laser radar odometer calculation method; Feature fusion module, used to fuse the depth point cloud data with semantic labels, laser point cloud data and local odometer information to obtain the positioning data in the pre-built point cloud map; A map building module that uses the perception method of the region planner to build a multi-layer cost map based on pre-built point cloud maps, newly acquired depth images, laser point cloud data, and inertial sensor data; The path planning module is used to generate navigation path information based on positioning data and multi-layer cost maps using global planning path algorithms and local path planning algorithms; Using the perception method of the region planner, a multi-layer cost map is constructed based on the pre-built point cloud map, the latest acquired depth image, laser point cloud data and inertial sensor data, including: Using the perception method of the region planner, the maximum feasible depth set is generated based on the latest acquired laser point cloud data ; For each direction the passable depth ,use The method calculates its coordinates in the Cartesian coordinate system ; By transforming the matrix T Transform the coordinates to the odometer coordinate system and get the coordinates ; According to the resolution of the grid And the offset of the cost grid map origin in the odometer coordinate system and Calculate the index value in the corresponding grid map ,Will The traversable area grid at is marked as occupied; The semantic point cloud data generated by the target detection network based on the latest acquired depth image is used for polar coordinate registration to generate a traversable area with semantic information; The maximum feasible depth set is updated using the traversable area with semantic information, and the traversable area in the Cartesian coordinate system is updated synchronously; Each time after traversing the passable data generated by a frame of point cloud, only the cost map is updated The cost of the moving part and the part where data changes occur; The pre-built point cloud map is used as the static layer, and the traversable area in the Cartesian coordinate system is used as the obstacle layer to construct a multi-layer cost map.

Images