A robot control method and system based on terrain passability
By constructing a 2.5-dimensional digital elevation map and passability cost function, dividing the terrain areas and planning the path, the navigation difficulties of mobile robots in unstructured environments, especially on soft terrain, are solved, and navigation capabilities and motion stability are improved.
Patent Information
- Application Number
- CN202210668685.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-06-14
- Publication Date
- 2024-11-12
- Estimated Expiration
- 2042-06-14
AI Technical Summary
Mobile robots have difficulty in autonomous navigation in unstructured and complex environments, especially when moving on soft terrain, which will cause subsidence and slippage, resulting in reduced mobility and affecting autonomous navigation capabilities.
Using a control method based on terrain passability, depth images and color images are obtained through external perception modules, a 2.5-dimensional digital elevation map is constructed, and the passability cost function is used to determine the passability of each grid, and multiple passability areas are divided. and plan the path of the robot according to these areas.
It improves the rationality of the robot's path planning in a structured environment, and enhances its autonomous navigation capabilities in an unstructured environment, especially its motion stability and efficiency on soft terrain.
Smart Images

Figure CN115185266B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of two-dimensional position or waterway control technology, and in particular to a robot control method and system based on terrain accessibility. Background Technology
[0002] With the rapid development of robotics and computer technology, mobile robots with autonomous navigation capabilities have received widespread attention and application in national production and daily life. From home services and disaster relief to military operations and planetary exploration, mobile robots are playing an increasingly important role. For mobile robots, the biggest technological challenge is autonomous navigation, which involves perceiving the surrounding environment and using that information for path planning and motion control. Compared to structured environments, mobile robots face even greater challenges in autonomous navigation in unstructured, complex environments.
[0003] In structured environments, with relatively abundant geographic information, the perception system can convert the perceived information about the ground into simple boundary structures to guide the formulation of perception behavior strategies. However, in unstructured environments, the lack of artificial beacons, or even the absence of artificial roads, makes it difficult to describe the perceived information about the ground in a structured and regularized form, significantly increasing the difficulty of autonomous navigation for mobile robots. Furthermore, structured environments are predominantly characterized by rigid terrain such as concrete and asphalt surfaces, while unstructured environments include soft terrain such as sand, grass, and mud. Because the environmental medium in soft terrain typically possesses a certain degree of fluidity or deformability, mobile robots will sink and slip when moving on soft terrain, and the resistance they experience is often greater than that encountered when traversing rigid terrain. This significantly reduces the robot's mobility, thereby affecting its autonomous navigation capabilities. Summary of the Invention
[0004] The purpose of this invention is to provide a robot control method and system based on terrain accessibility, which can reasonably plan the path and control the motion of the robot in a structured environment, thereby improving the rationality of the robot path planning.
[0005] To achieve the above objectives, the present invention provides the following solution:
[0006] A robot control method based on terrain accessibility, the method being applied to a robot equipped with an internal sensing module and an external sensing module;
[0007] The internal sensing module is used to acquire the robot's posture parameters;
[0008] The external sensing module is used to acquire depth and color images of the robot's external environment;
[0009] The method includes:
[0010] Acquire depth and color images of the robot's external environment, as well as the robot's pose parameters;
[0011] Based on the depth and color images of the robot's external environment and the robot's posture parameters, a 2.5-dimensional digital elevation map is constructed.
[0012] The accessibility of each grid cell in the 2.5D digital elevation map is determined using the accessibility cost function to obtain the accessibility map;
[0013] Based on the accessibility map, the robot's external environment is divided into multiple accessibility zones;
[0014] The robot's path is planned based on multiple areas of varying accessibility.
[0015] Optionally, the step of constructing a 2.5D digital elevation map based on the depth and color images of the robot's external environment and the robot's posture parameters specifically includes:
[0016] The color image is input into the terrain category recognition model to obtain the terrain category l for each pixel of the color image. type The terrain category recognition model is obtained by training a neural network using a labeled color image of the robot's external environment.
[0017] The coordinates (x, y) of each pixel in the color image in the color image coordinate system. C ,y C Add terrain category l type Obtain the semantic coordinates (x) in the color image coordinate system. C ,y C ,l type );
[0018] Based on the first rotation matrix, the coordinates (x, y) of each pixel in the depth image in the depth image coordinate system are... D ,y D ,z D ), transformed into coordinates (x) in the color image coordinate system. C ,y C ,z C );
[0019] The semantic coordinates (x, y) of each pixel in the color image coordinate system C ,y C ,l type Add depth information z) C The depth semantic coordinates (x, y) in the color image coordinate system are obtained. C ,yC ,z C ,l type );
[0020] Based on the second rotation matrix, the depth semantic coordinates (x, y) in the color image coordinate system are... C ,y C ,z C ,l type This is transformed into the depth semantic coordinates (x, y) of each pixel in the world coordinate system. I ,y I ,z I ,l type );
[0021] Add the robot's pose parameters to the depth semantic coordinates (x, y) of each pixel in the world coordinate system. I ,y I ,z I ,l type This yields a 2.5-dimensional digital elevation map.
[0022] Optionally, the step of determining the accessibility of each grid cell in the 2.5D digital elevation map using a accessibility cost function to obtain a accessibility map specifically includes:
[0023] The roughness of each grid cell in the 2.5D digital elevation map is determined by using a sliding window and the formula r = 1 / v.
[0024] The least squares method is used to construct the fitting plane equation Ax + By + Cz + 1 = 0 for each sliding window;
[0025] Based on the multiple fitted plane equations, the formula s = cos -1 (C) Determine the tilt of the grid within each sliding window;
[0026] The step gradient of the raster within each sliding window is determined using the sliding window and the maximum step gradient value.
[0027] Based on the amount of foot depression or foot deformation of the robot, using the formula... Determine the normal softness at each point;
[0028] Determine the average normal softness of all points within the grid as the normal softness of the grid;
[0029] Based on the normal force acting on the robot's feet, using the formula... Determine the degree of tangential friction at each point;
[0030] Determine the average tangential friction level of all points within the grid, which is the tangential friction level of the grid;
[0031] Based on roughness, tilt, step gradient, normal softness, and tangential friction, the accessibility of each grid is determined, resulting in a accessibility map.
[0032] Where r is the roughness of the grid; v is the divergence of the vector; v = (N-1) / (NR); N is the number of vectors within the window; and R is the magnitude of the vector. It is the average of the x-coordinates of the depth semantic coordinates of all points in the raster; It is the average value of the y-coordinate in the depth semantic coordinates of all points in the raster; F' is the average z-coordinate of the depth semantic coordinates of all points in the grid; A, B, and C are coefficients of the fitted plane equation; s is the tilt; δ is the normal softness; k is the equivalent stiffness coefficient of the terrain; n is the exponential coefficient of foot settlement or foot deformation; F' N F represents the robot's rated contact force; f is the degree of tangential friction; α and β are both weights, α + β = 1; N denoted as σ, where μ is the normal force acting on the foot; μ is the coefficient of sliding friction of the ground; and K is the shear modulus of the soft ground.
[0033] Optionally, the accessibility cost function is:
[0034]
[0035] Where τ represents the passability of the terrain, τ∈[0,1]; ω1 is the terrain slope weight, ω2 is the roughness weight, ω3 is the step gradient weight, ω4 is the normal softness weight, and ω5 is the tangential friction weight, ω1+ω2+ω3+ω4+ω5=1; s is the terrain slope; h is the step gradient; δ is the normal softness; f is the tangential friction; s crit The critical value for terrain slope; r crit h is the critical roughness value. crit The critical value of the step gradient; δ crit f is the critical value for the degree of normal softness. crit This is the critical value for the degree of tangential friction.
[0036] Optionally, dividing the robot's external environment into multiple accessibility zones based on the accessibility map specifically includes:
[0037] Determine any grid cell in the accessibility map as the current grid cell;
[0038] Determine whether the passability of the current grid is less than a first threshold to obtain a first determination result;
[0039] If the first determination result is yes, then the current grid is divided into a low-passability region;
[0040] If the first judgment result is negative, then it is determined whether the passability of the current grid is less than the second threshold, and a second judgment result is obtained; the first threshold is less than the second threshold.
[0041] If the second determination result is yes, then the current grid is divided into a medium passability region;
[0042] If the second determination result is negative, then the current grid is divided into a high-passability region.
[0043] Optionally, the step of planning the robot's path based on multiple traversal areas specifically includes:
[0044] Obtain the starting and ending points of the path;
[0045] Based on the starting point and the target point, the path of the robot is planned in the accessibility area; the accessibility area includes a medium accessibility area and a high accessibility area;
[0046] Control the robot to travel along the planned path.
[0047] Optionally, controlling the robot to travel along the planned path specifically includes:
[0048] When the robot passes through a passable area along the planned path, control the robot to move at the first speed;
[0049] As the robot traverses a highly accessible area along a planned path, it is controlled to travel at a second speed; the first speed is less than the second speed.
[0050] A terrain-accessible robot control system includes:
[0051] The data acquisition module is used to acquire depth and color images of the robot's external environment, as well as the robot's posture parameters;
[0052] A 2.5D digital elevation map construction module is used to construct a 2.5D digital elevation map based on the depth and color images of the robot's external environment and the robot's posture parameters.
[0053] The accessibility map module is used to determine the accessibility of each grid in the 2.5D digital elevation map using the accessibility cost function, thereby obtaining the accessibility map;
[0054] The degree zone division module is used to divide the robot's external environment into multiple degree zones based on the accessibility map;
[0055] The path planning module is used to plan the robot's path based on various traversability areas.
[0056] Optionally, the 2.5D digital elevation map construction module specifically includes:
[0057] The terrain category recognition unit is used to input the color image into the terrain category recognition model to obtain the terrain category l of each pixel in the color image. type The terrain category recognition model is obtained by training a neural network using a labeled color image of the robot's external environment.
[0058] Semantic coordinate determination unit, used to determine the coordinates (x, y) of each pixel in a color image in the color image coordinate system. C ,y C Add terrain category l type Obtain the semantic coordinates (x, y) in the color image coordinate system. C ,y C ,l type );
[0059] A coordinate determination unit in the color image coordinate system is used to determine the coordinates (x, y) of each pixel in the depth image in the depth image coordinate system based on a first rotation matrix. D ,y D ,z D ), transformed into coordinates (x) in the color image coordinate system. C ,y C ,z C );
[0060] Depth semantic coordinate determination unit, used to determine the semantic coordinates (x, y) of each pixel in the color image coordinate system. C ,y C ,l type Add depth information z) C The depth semantic coordinates (x, y) in the color image coordinate system are obtained. C ,y C ,z C ,l type );
[0061] The depth semantic coordinate transformation unit is used to transform the depth semantic coordinates (x, y) in the color image coordinate system based on the second rotation matrix. C ,y C ,z C ,l type This is transformed into the depth semantic coordinates (x, y) of each pixel in the world coordinate system. I ,y I ,z I ,l type );
[0062] A 2.5D digital elevation map determination unit is used to add the robot's pose parameters to the depth semantic coordinates (x, y) of each pixel in the world coordinate system. I ,y I ,z I ,l type This yields a 2.5-dimensional digital elevation map.
[0063] Optionally, the accessibility map may specifically include, depending on the module:
[0064] A roughness determination unit is used to determine the roughness of each grid in the 2.5D digital elevation map by using the formula r = 1 / v through a sliding window;
[0065] The fitting plane equation construction unit is used to construct the fitting plane equation Ax+By+Cz+1=0 for each sliding window using the least squares method;
[0066] The tilt determination unit is used to determine the tilt angle based on multiple fitted plane equations using the formula s = cos -1 (C) Determine the tilt of the grid within each sliding window;
[0067] The step gradient determination unit is used to determine the step gradient of the grid within each sliding window using a sliding window and the maximum step gradient value.
[0068] The normal softness determination unit is used to determine the degree of softness of the robot's foot using a formula based on the amount of foot indentation or foot deformation. Determine the normal softness at each point;
[0069] The grid normal softness determination unit is used to determine the average normal softness of all points within the grid, which is the normal softness of the grid.
[0070] The tangential friction determination unit is used to determine the degree of friction based on the normal force acting on the robot's foot using the formula... Determine the degree of tangential friction at each point;
[0071] The grid tangential friction degree determination unit is used to determine the average tangential friction degree of all points within the grid, which is the tangential friction degree of the grid;
[0072] The accessibility map determines the cell, which is used to determine the accessibility of each grid cell based on roughness, tilt, step gradient, normal softness and tangential friction, and thus obtains the accessibility map;
[0073] Where r is the roughness of the grid; v is the divergence of the vector; v = (N-1) / (NR); N is the number of vectors within the window; and R is the magnitude of the vector. It is the average of the x-coordinates of the depth semantic coordinates of all points in the raster; It is the average value of the y-coordinate in the depth semantic coordinates of all points in the raster; F' is the average z-coordinate of the depth semantic coordinates of all points in the grid; A, B, and C are coefficients of the fitted plane equation; s is the tilt; δ is the normal softness; k is the equivalent stiffness coefficient of the terrain; n is the exponential coefficient of foot settlement or foot deformation; F' N F represents the robot's rated contact force; f is the degree of tangential friction; α and β are both weights, α + β = 1; N denoted as σ, where μ is the normal force acting on the foot; μ is the coefficient of sliding friction of the ground; and K is the shear modulus of the soft ground.
[0074] According to specific embodiments provided by the present invention, the present invention discloses the following technical effects:
[0075] The purpose of this invention is to provide a robot control method and system based on terrain accessibility. This method constructs a 2.5D digital elevation map based on depth and color images of the robot's external environment and the robot's posture parameters. It then uses a accessibility cost function to determine the accessibility of each grid cell in the 2.5D digital elevation map, obtaining a accessibility map. Based on the accessibility map, the robot's external environment is divided into multiple accessibility zones. Finally, the robot's path is planned according to these zones. By constructing the 2.5D digital elevation map, the method enables reasonable path planning and motion control of the robot in a structured environment, improving the rationality of robot path planning. Attached Figure Description
[0076] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.
[0077] Figure 1 This is a flowchart of a robot control method based on terrain accessibility in an embodiment of the present invention;
[0078] Figure 2 This is a schematic diagram of coordinate system establishment in an embodiment of the present invention;
[0079] Figure 3 This is a schematic diagram of the semantic-elevation information fusion map model construction framework in an embodiment of the present invention;
[0080] Figure 4 This is a schematic diagram of depth image and semantic image fusion in an embodiment of the present invention.
[0081] Figure 5 This is a schematic diagram of Bekker's wheel-ground interaction model in an embodiment of the present invention;
[0082] Figure 6 This is a schematic diagram of the hierarchical feature map construction framework in an embodiment of the present invention;
[0083] Figure 7 This is a schematic diagram of roughness vector divergence method measurement in an embodiment of the present invention;
[0084] Figure 8 This is a schematic diagram of a sliding window in an embodiment of the present invention. Detailed Implementation
[0085] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0086] The purpose of this invention is to provide a robot control method and system based on terrain accessibility, which can reasonably plan the path and control the motion of the robot in a structured environment, thereby improving the rationality of the robot path planning.
[0087] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.
[0088] like Figure 1 This invention provides a robot control method based on terrain accessibility. The method is applied to a robot equipped with an internal sensing module and an external sensing module. The internal sensing module is used to acquire the robot's posture parameters, and the external sensing module is used to acquire depth images and color images of the robot's external environment.
[0089] The methods include:
[0090] Acquire depth and color images of the robot's external environment, as well as the robot's pose parameters.
[0091] Based on depth and color images of the robot's external environment, and the robot's posture parameters, construct a 2.5D digital elevation map.
[0092] The accessibility of each grid cell in a 2.5D digital elevation map is determined using the accessibility cost function, resulting in an accessibility map.
[0093] Based on the accessibility map, the robot's external environment is divided into zones with varying degrees of accessibility.
[0094] The robot's path is planned based on various levels of accessibility.
[0095] Specifically, a 2.5D digital elevation map is constructed based on depth and color images of the robot's external environment and the robot's posture parameters. This includes inputting the color image into a terrain category recognition model to obtain the terrain category for each pixel in the color image. type The terrain category recognition model is obtained by training a neural network using labeled color images of the robot's external environment; the coordinates (x, y) of each pixel in the color image in the color image coordinate system are... C ,y C Add terrain category l type Obtain the semantic coordinates (x) in the color image coordinate system. C ,y C ,l type Based on the first rotation matrix, the coordinates (x, y) of each pixel in the depth image are determined in the depth image coordinate system. D ,y D ,z D ), transformed into coordinates (x) in the color image coordinate system. C ,y C ,z C ); the semantic coordinates (x, y) of each pixel in the color image coordinate system. C ,y C ,l type Add depth information z) C The depth semantic coordinates (x, y) in the color image coordinate system are obtained. C ,y C ,z C ,l type Based on the second rotation matrix, the depth semantic coordinates (x, y) in the color image coordinate system are... C ,y C ,z C ,l type This is transformed into the depth semantic coordinates (x, y) of each pixel in the world coordinate system. I ,y I ,z I ,l type Add the robot's pose parameters to the depth semantic coordinates (x, y) of each pixel in the world coordinate system. I ,y I ,z I ,l type This yields a 2.5-dimensional digital elevation map.
[0096] The accessibility of each grid cell in a 2.5D digital elevation map is determined using a accessibility cost function, resulting in a accessibility map. Specifically, this involves: determining the roughness of each grid cell using a sliding window and the formula r = 1 / v; constructing the fitting plane equation Ax + By + Cz + 1 = 0 for each sliding window using the least squares method; and based on multiple fitting plane equations, using the formula s = cos... -1 (C) Determine the tilt of the grid within each sliding window; using the sliding window and the maximum step gradient value, determine the step gradient of the grid within each sliding window; based on the robot's foot indentation or foot deformation, use the formula... Determine the normal softness at each point; determine the average normal softness of all points within the grid, which is the normal softness of the grid; based on the normal force acting on the robot's foot, use the formula... Determine the tangential friction degree at each point; determine the average tangential friction degree of all points within the grid, which is the tangential friction degree of the grid; based on roughness, tilt, step gradient, normal softness, and tangential friction degree, determine the passability of each grid, thus obtaining a passability map; where r is the roughness of the grid; v is the divergence of the vector; v = (N-1) / (NR); N is the number of vectors within the window; R is the magnitude of the vector; It is the average of the x-coordinates of the depth semantic coordinates of all points in the raster; It is the average value of the y-coordinate in the depth semantic coordinates of all points in the raster; F' is the average z-coordinate of the depth semantic coordinates of all points in the grid; A, B, and C are coefficients of the fitted plane equation; s is the tilt; δ is the normal softness; k is the equivalent stiffness coefficient of the terrain; n is the exponential coefficient of foot settlement or foot deformation; F' N F represents the robot's rated contact force; f is the degree of tangential friction; α and β are both weights, α + β = 1; N denoted as σ, where μ is the normal force acting on the foot; μ is the coefficient of sliding friction of the ground; and K is the shear modulus of the soft ground.
[0097] Specifically, the accessibility cost function is:
[0098]
[0099] Where τ represents the passability of the terrain, τ∈[0,1]; ω1 is the terrain slope weight, ω2 is the roughness weight, ω3 is the step gradient weight, ω4 is the normal softness weight, and ω5 is the tangential friction weight, ω1+ω2+ω3+ω4+ω5=1; s is the terrain slope; h is the step gradient; δ is the normal softness; f is the tangential friction; s critThe critical value for terrain slope; r crit h is the critical roughness value. crit The critical value of the step gradient; δ crit f is the critical value for the degree of normal softness. crit This is the critical value for the degree of tangential friction.
[0100] Furthermore, based on the accessibility map, the robot's external environment is divided into multiple accessibility zones, specifically including: identifying any grid cell in the accessibility map as the current grid cell; determining whether the accessibility of the current grid cell is less than a first threshold to obtain a first determination result; if the first determination result is yes, then the current grid cell is classified as a low accessibility zone; the accessibility zones include: low accessibility zone, medium accessibility zone, and high accessibility zone; if the first determination result is no, then determining whether the accessibility of the current grid cell is less than a second threshold to obtain a second determination result; the first threshold is less than the second threshold; if the second determination result is yes, then the current grid cell is classified as a medium accessibility zone; if the second determination result is no, then the current grid cell is classified as a high accessibility zone.
[0101] The robot's path is planned based on various accessibility zones, specifically including: obtaining the starting point and target point of the path; planning the robot's path in the accessibility zone based on the starting point and target point; the accessibility zone includes medium accessibility zone and high accessibility zone; and controlling the robot to travel along the planned path.
[0102] Controlling the robot to travel along the planned path specifically includes: controlling the robot to travel at a first speed when it passes through a medium-accessible area along the planned path; controlling the robot to travel at a second speed when it passes through a high-accessible area along the planned path; the first speed is less than the second speed.
[0103] Specifically, the terrain accessibility based on soil mechanics proposed in this invention is defined as a function of terrain geometric features, terrain category, terrain physical properties, and robot state:
[0104]
[0105] in, Represents coordinates in a 3D environment The terrain accessibility at the location; It is the set of terrain geometric features at coordinate x at time t; It is the set of terrain categories at coordinate x at time t; It is the set of topographic physical features at coordinate x at time t, i.e., the soil mechanics model; Let f(·) be the state of a robot with n degrees of freedom at time t. The function f(·) is a model that maps all considered elements to terrain accessibility, and its identification is the ultimate goal of the terrain accessibility estimation problem.
[0106] The flowchart of the topographic accessibility estimation method based on soil mechanics proposed in this invention is as follows: Figure 1 As shown in the diagram, the external perception module mainly consists of sensors such as depth cameras or LiDAR, used to acquire color image information and point cloud information of the environment. The internal perception module consists of an odometry and an inertial measurement unit, used for robot state estimation. Since the units of different factors are different, each factor is first normalized, and then a accessibility function is constructed. Finally, a accessibility cost map is drawn based on the accessibility cost function. Based on quantified terrain accessibility, a reference is provided for mobile robot passage control strategies, path planning, and motion control.
[0107] Remote modeling of unknown environments is a prerequisite for robot motion planning (path planning, gait planning). Vision, with its rich information and wide perception range, enables remote environmental modeling that is difficult to achieve with other short-range sensors. However, relying solely on visual information is insufficient. During the movement of a mobile robot, the interaction between its mobile devices (wheels, legs, tracks, etc.) and the ground affects its ground traversal performance. The interaction between the mobile devices and the terrain is closely related to the geometry of the surface and the physical properties of the terrain. Simply establishing a geometric model of the environment cannot meet the requirements for avoiding non-geometric hazards such as soft sand in complex, unstructured environments. Therefore, in addition to geometric modeling of the environment, it is necessary to incorporate the physical characteristics of the environment, that is, to consider the impact of soil mechanics on the mobile robot's motion performance.
[0108] To conduct a more reliable analysis of terrain accessibility, firstly, in terms of visual environmental modeling, a 2.5D environmental elevation map incorporating uncertainty information was established, considering sensor noise and positioning errors. Secondly, for common outdoor terrain categories, corresponding terrain segmentation models were trained, and a semantic map construction framework was built to classify unknown environments. Regarding environmental physical feature extraction, by simplifying and unifying the interaction model between the robot and the terrain under both soft and hard ground, parameterized indices characterizing the normal softness and tangential friction properties of the ground were extracted. Finally, by integrating terrain geometric and physical properties, a terrain accessibility model based on soil mechanics was constructed.
[0109] To further illustrate the terrain accessibility estimation method based on a soil mechanics model proposed in this invention, the specific implementation of the method is explained using the movement of a legged mobile robot in a complex unstructured environment as an example. This includes the acquisition of various influencing factors in the accessibility cost function and the construction of the cost function itself. First, an external perception system for the robot is established using external sensors such as depth cameras or LiDAR to acquire terrain point cloud information, construct a digital elevation map, and extract features such as color and texture of the terrain for terrain classification. Then, based on the digital elevation map, the geometric features of the terrain are extracted; based on the terrain classification information, the physical properties of the terrain are obtained; and the robot's state is estimated using internal perception sensors. Finally, the above factors are normalized to construct the terrain accessibility cost function, and a accessibility cost map is drawn.
[0110] 1.2.5D Digital Elevation Map Construction
[0111] When mobile robots navigate in unknown environments, environmental perception is essential for their autonomous operation, and terrain geometry is the most fundamental environmental information. Extracting geometric information requires establishing a geometric model of the environment. This invention uses a 2.5D grid map for environmental geometry modeling. Because sensors have noise and positioning errors are caused by robot movement, maximum height and probability values are added to each grid cell on top of the 2D grid map to create a 2.5D grid map model with uncertainty. This 2.5D grid map, constructed using an extended elevation map approach, not only achieves the visual effect of a 3D grid but also significantly reduces the storage and computational burden of grid maps.
[0112] The 2.5D raster map used in this invention adopts the approach of a two-dimensional raster map. First, the point cloud data acquired by the depth camera or LiDAR is rasterized using small square grids. The information of each small grid consists of its row and column information in the map and its elevation value. Based on the analysis needs, a raster area of the required size is defined for analysis. This invention establishes a 61×61 raster map area centered on the origin of the robot's coordinate system, extending 5m forward, 1m backward, and 3m to the left and right. Subsequent terrain accessibility analysis only considers point cloud data whose horizontal coordinates fall within this range in the world coordinate system.
[0113] First, the height values of the 3D point cloud in the world coordinate system must be obtained. Establish, for example... Figure 2The reference coordinate system is shown in the figure. In the figure, I is the world coordinate system, which is fixed; B is the robot body coordinate system, which moves with the robot; and S is the coordinate system of the external sensing sensor (depth camera or LiDAR, etc.). This invention acquires 3D point cloud data in the robot body coordinate system using a depth camera, with each projection point corresponding to a 3D coordinate. By calibrating the extrinsic parameters of the sensor, the translation and rotation matrices (transformation matrices) from the sensor coordinate system to the robot body coordinate system can be obtained, thus transforming the point cloud data into the robot body coordinate system. Then, considering the robot's attitude information relative to the ground, the 3D point cloud data is further transformed into the world coordinate system, where the vertical coordinate of the point is the corresponding height value.
[0114] When stationary, the mobile robot body maintains a constant height, and can be considered to have no attitude change relative to the horizontal plane. It is only necessary to perform a translation transformation from the robot body coordinate system to the world coordinate system, measure the offset of the robot center relative to the center of the world coordinate system in each direction, project the point cloud data onto the world coordinate system, and obtain stable height value information.
[0115] When in motion, the robot's attitude information must be considered. The robot's attitude information is measured using inertial measurement units, resulting in the pose transformation relationship between the robot's body coordinate system and the world coordinate system. Thus, the point cloud data is projected from the robot's body coordinate system to the world coordinate system through pose transformation, and the resulting three-dimensional coordinate z-axis coordinate is the height value. The height value of each small grid cell is represented by the average height value of all points actually falling into a single square grid cell area. As the robot moves relative to world coordinate system I, the data in the elevation map needs to be continuously updated.
[0116] 2. Terrain Classification
[0117] (1) Training of semantic segmentation model
[0118] Terrain classification based on visual information can be viewed as a semantic segmentation problem, where a class label is assigned to each pixel in the input image. This invention adopts the Deeplabv2 semantic segmentation framework published by Liang-Chieh Chen et al. in 2017, which represents a high level of current deep learning-based semantic segmentation. The training dataset uses the Microsoft COCO-Stuff dataset, which is composed of pixel-level annotations from the COCO dataset. These annotations can be used for semantic segmentation, object detection, and scene understanding tasks such as image captioning. The dataset contains 164k images, including 80 object classes, 91 material categories, and one unlabeled category. The following categories were extracted as experimental data: hard ground, sand, grass, vegetation, gravel ground, mud, swamp, rock, stone, and other ground surfaces—a total of 10 categories. All other categories were treated as background.
[0119] (2) Semantic map construction
[0120] Two-dimensional images that have only undergone semantic segmentation cannot be directly used for navigation. Therefore, a method was designed to integrate semantic information with the previously established elevation map. Figure 3 The fusion framework is illustrated below. First, color images obtained from a depth camera are processed into semantic images using a semantic segmentation model. Then, based on the estimated robot pose and camera pose, the two-dimensional semantic images are calculated into semantic point clouds. On the other hand, an environmental elevation map is generated by constructing a framework based on the environmental elevation map. Finally, the semantic point clouds are projected onto the corresponding elevation map grids to generate a multi-information fusion environmental map model containing both environmental elevation and semantic information.
[0121] A depth camera is used to simultaneously acquire color and depth images. The color camera coordinate system is set to the C coordinate system. The color image contains the coordinates of its x-axis within its coordinate plane. C ,y C (r,g,b) data stream. Where (x C ,y C (r, g, b) represents the position of a point on the coordinate plane of the color image, and (r, g, b) represents the brightness values of the three colors (red, green, and blue) at that point. To directly represent the semantic image data, the color data stream is directly replaced with the semantic data stream (x... C ,y C ,l type ), l type The terrain category can be obtained through the aforementioned semantic segmentation training model. Setting the depth camera coordinate system as the D coordinate system, the depth camera image can be interpreted as (x, y) on the depth image coordinate plane. D ,y D ,z D Data flow.
[0122] like Figure 4 As shown, the plane coordinates (x, y) of the color camera coordinate system are... C ,y C As a bridge connecting depth images and semantic images, it uses a rotation matrix between the depth camera and the color camera. The depth image data stream can be converted to the coordinate system of a color image (semantic image), and corresponding z-coordinates can be added to the semantic information. C Coordinate values. At this point, the rotation matrix between the estimated color camera coordinate system and the world coordinate system is used. This allows us to construct a semantic point cloud in the corresponding world coordinate system. Finally, the semantic point cloud is projected onto the raster map to complete the construction of the semantic map.
[0123] 3. Methods for characterizing ground physical properties based on soil mechanics
[0124] Legged mobile robots rely on the interaction between their feet and the ground to propel themselves forward in outdoor environments. The foot-ground contact varies depending on the physical properties of the terrain. They may slip on smooth tiles or sink in sand or swamps. Foot-ground interaction is primarily manifested in normal and tangential forces. The normal force represents the supporting force provided by the ground to the robot, indirectly reflecting the softness of the ground. When the ground's bearing capacity is insufficient, the feet will sink. The tangential force, on the other hand, represents the driving force provided by the ground to the robot, indirectly reflecting the degree of tangential friction. If the tangential force provided by the ground is too small, the robot is prone to slipping, affecting its stable movement. Therefore, the interaction between the feet and the ground can be used to visually characterize the physical properties of the ground and to avoid slippery and sinking terrain, thus planning safe paths.
[0125] (1) Characterization of ground normal softness
[0126] 1) Normal contact model
[0127] Depending on the foot type and ground characteristics, foot-ground contact can generally be categorized into four types: rigid foot contact with soft ground; flexible foot contact with soft ground; flexible foot contact with hard ground; and rigid foot contact with hard ground. Since the interaction between a rigid foot and a hard ground can cause destructive damage to the robot, common mobile robots typically incorporate springs or soft materials at the foottips to mitigate this damage. Therefore, this invention does not consider collision contact between a rigid foot and a hard ground. The normal contact model between the foot and ground considered in this invention can be standardized as follows:
[0128] F N =kδ n
[0129] Among them, F N δ represents the normal contact force on the robot's foot; k is the equivalent stiffness coefficient for different terrains; and n is the exponential coefficient of foot indentation / deformation. On hard surfaces, δ represents foot deformation; on soft terrain, δ represents foot indentation. The values of k and n depend on the ground type and the foot material. For a given legged mobile robot, the foot design and materials are fixed, so the variation of these parameters depends only on the ground type, and therefore can be used as key parameters to measure the normal softness of the ground.
[0130] It is important to note that this invention assumes that once the ground type is determined, the corresponding equivalent stiffness coefficient and settlement / deformation index coefficient, among other topographic physical property parameters, are known quantities. This assumption is reasonable because for common terrains (hard surfaces, dry sand, etc.), there is already a wealth of research data available for soil property parameters, and relevant parameter databases have been established. For complex terrains (wet sand, silt), soil property parameters can be quickly identified through online learning, and related research results have been published. Furthermore, the innovation of this invention lies in considering the interaction between soil and the robot on the robot's motion performance, and in constructing a terrain accessibility estimation function in unstructured complex environments, rather than in innovations in specific technical methods such as terrain classification or soil parameter identification. Therefore, when using the accessibility estimation method proposed in this invention to estimate the accessibility of unstructured complex environments, it is reasonable to use a mature technical means to complete soil parameter identification, and this does not affect the innovation of the terrain accessibility estimation method proposed in this invention.
[0131] 2) Characterization of normal softness
[0132] The normal softness of the terrain is determined by the combined effect of the equivalent stiffness coefficient and the subsidence / deformation exponent. This invention estimates the foot subsidence / deformation under this working condition based on the robot's weight and a normal foot-ground contact model, thereby measuring the normal softness of the terrain. Let F' be the robot's rated contact force. N This rating is calculated based on the robot's weight, state estimates, and dynamic model. The subsidence / deformation δ under the corresponding working condition is used to measure the normal softness of the terrain, as shown in the following formula:
[0133]
[0134] It should be noted that the calculation formulas differ for different locomotion devices (wheels, legs, tracks, etc.) due to variations in the contact model, but the characterization of normal softness is not constrained by the type of locomotion device. Here, we will again use a wheeled mobile robot as an example for illustration. Figure 5 This is a wheel-land interaction model based on the Bekker soil mechanics model.
[0135] Figure 5 Explanation of parameters: r is the wheel radius, W is the vertical load on the wheel, F' RC The driving force acting on the axle is θ1, the entry angle of the wheel when it moves on the sand is σ, the normal stress acting on the radial direction of the wheel is z, and the wheel's sinkage is Z. w (θ) represents the amount of wheel sinking at the turning angle θ.
[0136] When a wheel is stationary on sand, there is no relative movement between the wheel surface and the sand, so the wheel is not subjected to tangential forces. The normal force F that balances the vertical load is... N It is generated solely by the normal stress σ:
[0137]
[0138] In the formula, b is the width of the wheel.
[0139] Motion resistance F RC Driven by the driving force F' acting on the wheel axle RC overcome:
[0140]
[0141] like Figure 5 As shown, from geometric relationships, we can obtain:
[0142] x = rsinθ
[0143] z w (θ)=r(cosθ-cosθ1)
[0144] dx=rcosθdθ
[0145] dz=-rsinθdθ
[0146] When θ = 0, x(0) = 0, z w (0)=z; when θ=θ1, x(θ1)=x1,z w (θ1) = 0. (The rest of the text appears to be incomplete and requires further context.) Substituting into the aforementioned formula, we can obtain the vertical load W and the motion resistance F. RC The amount of wheel sinking z:
[0147]
[0148]
[0149]
[0150] Among them, K s The equivalent subsidence modulus of the terrain. k c For terrain cohesive modulus, Let n be the terrain friction modulus, and n be the terrain settlement index.
[0151] In summary, the normal softness characterization is suitable for analyzing the passability of different walking devices on complex terrain.
[0152] (2) Characterization of the degree of tangential friction on the ground
[0153] 1) Tangential foot-ground interaction mechanical model
[0154] The tangential interactions between different types of feet and the ground can be uniformly represented as
[0155] F T = -μ·th(k,1.5K)F N
[0156]
[0157] Among them, F T F is the tangential force acting on the foot. N denoted by denoted as σ, where μ is the normal force acting on the foot; μ is the coefficient of sliding friction of the ground; k is the shear displacement of the soft ground; and K is the shear modulus of the soft ground. The values of these two parameters depend on the ground type and the material of the foot. Similarly, for a given legged mobile robot, where the foot design and material are fixed, the variation of these parameters depends only on the ground type, and therefore can be used as key parameters to measure the degree of normal softness of the ground.
[0158] 2) Characterization of tangential friction
[0159] The degree of tangential friction on the ground can be measured by two parameters: the maximum tangential force that the ground can provide during contact and the tangential displacement at which the maximum tangential force is reached. A slip coefficient is set to measure the degree of tangential friction of the terrain, as specifically expressed below:
[0160]
[0161] Here, α and β are weights, and α + β = 1. By changing their values, the importance of the maximum tangential force and the saturation displacement (tangential displacement at which the maximum tangential force is reached) to the slip coefficient can be adjusted; a larger slip coefficient f indicates that the foot is less likely to slip on the ground. For a given robot, the slip coefficient is only related to the ground parameters, so this parameter can be used as a single-valued quantity to characterize the degree of tangential friction on the ground. Similarly, the characterization of tangential friction is also applicable to wheeled mobile robots.
[0162] 4. Construction of a accessibility cost map that integrates the geometric and physical features of the ground environment
[0163] Environmental information is typically stored in cost maps for path planning. This invention constructs... Figure 6 The layered feature map shown is composed of a base layer and a derivation layer. The base layer contains basic elements of terrain geometry and physical properties, including the height layer, height variance layer, normal softness layer, and tangential friction degree layer. The derivation layer is derived directly or indirectly from the environmental elevation information in the base layer, including but not limited to the roughness layer, tilt layer, and step gradient layer.
[0164] (1) Construction of the base layer
[0165] 1) Height layer
[0166] The method for constructing height layers in this invention involves acquiring environmental point clouds using a depth camera, then projecting the environmental point clouds onto a grid plane in an established world coordinate system to construct a 2.5D elevation map. The height value of each small grid cell is represented by the average height value of all points that actually fall within a single square grid cell area.
[0167] 2) Height variance layer
[0168] Because sensor data contains noise and the robot's position estimation has errors, the constructed terrain elevation map has a certain degree of deviation. This invention calculates the uncertainty using Kalman filtering when constructing the digital elevation map, taking into account both height measurement errors and positioning errors generated during robot movement.
[0169] 3) Normal softness layer
[0170] By determining the rated normal force and terrain-related parameters based on the robot's mass and other body information, the corresponding normal softness layer of the terrain can be calculated.
[0171] 4) Tangential friction layer
[0172] Based on the robot's mass and other body information, the rated normal force and terrain-related parameters are determined, and the corresponding tangential friction layer of the terrain can be calculated.
[0173] (2) Construction of the deductive layer
[0174] 1) Roughness layer
[0175] Roughness measures the texture of a terrain surface by quantifying the perpendicular deviation of the real surface from its ideal shape. A rough surface has a larger perpendicular deviation, while a smaller deviation indicates a relatively smooth surface. This invention uses the vector divergence method to measure surface roughness.
[0176] Input a digital elevation map in matrix form, perform rapid 3D reconstruction using a triangular mesh method, and then calculate the unit normal vector (x) for each triangle. i ,y i ,z i ),like Figure 7-8 As shown in the figure, the roughness of each grid can be calculated sequentially using a sliding window method. The size d of the sliding window can be adjusted according to the terrain and the robot's dimensions.
[0177] The roughness of each grid cell can be calculated using the following formula:
[0178]
[0179]
[0180] v = (N-1) / (NR)
[0181] r = 1 / v
[0182] Where N is the number of vectors within the window; R represents the magnitude of the vector; v represents the divergence of the vector; r represents the roughness of the grid; and r is denoted as the maximum allowable roughness value to satisfy the stability margin requirements during robot motion. crit This value can be determined by the robot's kinematic parameters.
[0183] 2) Inclined layer
[0184] The slope of the terrain is another important indicator for evaluating terrain passability. The greater the slope, the greater the component of gravity downwards along the slope, and the easier it is for the robot to slip. Therefore, the greater the slope, the more difficult it is to meet the stability margin requirements of the robot, and the greater the possibility of the robot tipping over. The slope of each grid can also be calculated using a sliding window. Different sizes of sliding windows can be selected for different planning purposes. If the area of the sliding window is smaller than the area of the robot's foot, the calculated slope layer is suitable for selecting the robot's footing point; if the area of the sliding window is set close to the robot's body area, the calculated slope layer is more suitable for the robot's motion planning. The height values of the grid points contained in the sliding window are estimated using the least squares method to obtain the corresponding plane equation. The general plane equation obtained by fitting is expressed as:
[0185] Ax + By + Cz + 1 = 0
[0186] Where A, B, and C are the corresponding plane coefficients. The inclination of the fitted plane can be expressed as:
[0187] s = cos -1 (C)
[0188] Let s be the maximum allowable tilt angle for the robot to move without tipping over. crit This value can be determined by performing a force analysis on the robot.
[0189] 3) Gradient layer
[0190] The terrain step value represents the height difference of the terrain. Although the slope can also represent the height change of the terrain to some extent, it is still not an accurate measure of whether a robot can traverse such a height change. For example, when the height difference of a local terrain is small and the corresponding slope is close to 90 degrees, the robot can still traverse the corresponding terrain; however, 90 degrees is an extremely dangerous angle for slope. The step value can be calculated by moving the pane, and the whole process is divided into two steps: the first step is to set the maximum acceptable step height value h. crit This value is determined by the motion space of the robot's feet. The absolute value of the difference between each grid cell in each pane and the center grid cell is calculated, and the maximum value is selected as the temporary step gradient value h. max If h max Greater than h crit Then the tilt angle s needs to be greater than the maximum tilt angle value s crit The second step is to update the final step value using the following formula:
[0191]
[0192] Where h is the first gradient value; n st This indicates that the height difference in the sliding pane is greater than h. crit The number of grid cells; n scrit This indicates that the height difference is greater than h. crit The number of valid grid cells in the sliding pane.
[0193] Finally, based on terrain geometry and physical properties, a accessibility cost function is established:
[0194]
[0195] In the formula, τ represents the passability of the terrain, τ∈[0,1]. The larger the value, the stronger the passability of the terrain; τ=0 indicates that the terrain is impassable. ω1,ω2,ω3,ω4,ω5 represent weights, and the weight values can be set according to the robot's own performance and motion requirements, ω1+ω2+ω3+ω4+ω5=1.
[0196] s, r, h, δ, and f represent the terrain slope, roughness, step gradient, normal softness, and tangential friction, respectively. crit ,r crit ,h crit ,δ crit ,f crit This represents the critical value of the corresponding factor, that is, the theoretical maximum value limited by the robot's structural parameters, motion parameters, and terrain physical characteristics. Where s crit This is the maximum allowable tilt angle to prevent the robot from tipping over, determined by the robot's structural and motion parameters. critIt is the maximum roughness value allowed to ensure the stability margin of the robot during movement, and is determined by the robot's structural and motion parameters. crit This is the maximum number of steps the robot can traverse, determined by the robot's workspace. δ crit This is the maximum allowable amount of sinking / deformation to ensure the robot does not lose its mobility. For rigid / flexible feet moving on soft terrain, δ... crit This is the maximum allowable foot depression during robot movement. When the depression exceeds this value, the robot will sink into the terrain and lose its ability to move. For flexible feet moving on hard terrain, δ... crit This is the maximum allowable deformation of the robot's foot during movement. When the deformation exceeds this value, the robot's foot will be damaged and lose its ability to move.
[0197] When any of the values of s, r, h, δ, f exceeds the corresponding critical value, τ = 0, indicating that the terrain is impassable.
[0198] Based on the terrain accessibility cost function constructed in this invention, a terrain accessibility cost map can be further drawn. Based on the cost map, the terrain can be divided into four levels: impassable (τ=0), low accessibility (0<τ<<0.3), medium accessibility (0.3<τ<<0.7), and high accessibility (0.7<τ<<1).
[0199] For impassable or low-accessibility terrain, a detour control strategy is adopted; for medium-accessibility terrain, the robot is slowed down to pass through; for high-accessibility terrain, the robot can drive normally at a given speed without changing motion parameters.
[0200] Furthermore, the present invention also provides a robot control system based on terrain accessibility, comprising:
[0201] The data acquisition module is used to acquire depth and color images of the robot's external environment, as well as the robot's posture parameters.
[0202] The 2.5D digital elevation map construction module is used to construct a 2.5D digital elevation map based on the depth and color images of the robot's external environment and the robot's posture parameters.
[0203] The accessibility map module is used to determine the accessibility of each grid cell in the 2.5D digital elevation map using the accessibility cost function, thus obtaining the accessibility map.
[0204] The accessibility zone division module is used to divide the robot's external environment into multiple accessibility zones based on the accessibility map.
[0205] The path planning module is used to plan the robot's path based on various traversability areas.
[0206] The 2.5D digital elevation map construction module specifically includes: a terrain category recognition unit, used to input color images into the terrain category recognition model to obtain the terrain category of each pixel in the color image. type The terrain category recognition model is obtained by training a neural network using labeled color images of the robot's external environment; the semantic coordinate determination unit is used to determine the coordinates (x, y) of each pixel in the color image in the color image coordinate system. C ,y C Add terrain category l type Obtain the semantic coordinates (x, y) in the color image coordinate system. C ,y C ,l type ); Coordinate determination unit in color image coordinate system, used to determine the coordinates (x, y, x) of each pixel in the depth image in the depth image coordinate system based on the first rotation matrix. D ,y D ,z D ), transformed into coordinates (x) in the color image coordinate system. C ,y C ,z C ); Depth semantic coordinate determination unit, used to determine the semantic coordinates (x, y, y) of each pixel in the color image coordinate system. C ,y C ,l type Add depth information z) C The depth semantic coordinates (x, y) in the color image coordinate system are obtained. C ,y C ,z C ,l type The depth semantic coordinate transformation unit is used to transform the depth semantic coordinates (x, y) in the color image coordinate system based on the second rotation matrix. C ,y C ,z C ,l type This is transformed into the depth semantic coordinates (x, y) of each pixel in the world coordinate system. I ,y I ,z I ,l type ); 2.5D digital elevation map determination unit, used to add robot pose parameters to the depth semantic coordinates (x, y) of each pixel in the world coordinate system. I ,y I ,z I ,l type This yields a 2.5-dimensional digital elevation map.
[0207] The accessibility map module specifically includes: a roughness determination unit, used to determine the roughness of each grid cell in the 2.5D digital elevation map using the formula r = 1 / v through a sliding window; a fitted plane equation construction unit, used to construct the fitted plane equation Ax + By + Cz + 1 = 0 for each sliding window using the least squares method; and a tilt determination unit, used to determine the tilt based on multiple fitted plane equations using the formula s = cos -1 (C) Determine the tilt of the grid within each sliding window; a step gradient determination unit is used to determine the step gradient of the grid within each sliding window using the sliding window and the maximum step gradient value; a normal softness determination unit is used to determine the normal softness based on the robot's foot indentation or foot deformation using the formula... The normal softness degree of each point is determined; the grid normal softness degree determination unit is used to determine the average normal softness degree of all points within the grid, which is the normal softness degree of the grid; the tangential friction degree determination unit is used to determine the normal force on the robot's foot using the formula... The tangential friction degree of each point is determined; the grid tangential friction degree determination unit is used to determine the average tangential friction degree of all points in the grid, which is the tangential friction degree of the grid; the accessibility map determination unit is used to determine the accessibility of each grid based on roughness, tilt, step gradient, normal softness and tangential friction degree, and obtain the accessibility map.
[0208] Where r is the roughness of the grid; v is the divergence of the vector; v = (N-1) / (NR); N is the number of vectors within the window; and R is the magnitude of the vector. It is the average of the x-coordinates of the depth semantic coordinates of all points in the raster; It is the average value of the y-coordinate in the depth semantic coordinates of all points in the raster; F' is the average z-coordinate of the depth semantic coordinates of all points in the grid; A, B, and C are coefficients of the fitted plane equation; s is the tilt; δ is the normal softness; k is the equivalent stiffness coefficient of the terrain; n is the exponential coefficient of foot settlement or foot deformation; F' N F represents the robot's rated contact force; f is the degree of tangential friction; α and β are both weights, α + β = 1; N denoted as σ, where μ is the normal force acting on the foot; μ is the coefficient of sliding friction of the ground; and K is the shear modulus of the soft ground.
[0209] The various embodiments in this invention are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the systems disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the descriptions are relatively simple; relevant parts can be found in the method section.
[0210] Specific examples have been used to illustrate the principles and implementation methods of this invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of this invention. Furthermore, those skilled in the art will recognize that, based on the ideas of this invention, there will be changes in the specific implementation methods and application scope. Therefore, the content of this invention should not be construed as a limitation of this invention.
Claims
1. A robot control method based on terrain accessibility, characterized in that: The method is applied to a robot, wherein the robot is provided with an internal perception module and an external perception module; The internal perception module is used to obtain the posture parameters of the robot; The external perception module is used to obtain a depth image and a color image of the robot's external environment; The method comprises: Acquire a depth image and a color image of the robot's external environment, as well as posture parameters of the robot; Constructing a 2.5-dimensional digital elevation map based on the depth image and color image of the robot's external environment and the posture parameters of the robot; Determining the passability of each grid in the 2.5-dimensional digital elevation map using a passability cost function to obtain a passability map; According to the passability map, the robot's external environment is divided into multiple passability areas; Planning the path of the robot according to various passing areas; Determining the passability of each grid in the 2.5-dimensional digital elevation map by using the passability cost function to obtain a passability map specifically includes: Determine the roughness of each grid in the 2.5-dimensional digital elevation map by sliding the window using the formula r=1 / v; The fitting plane equation Ax+By+Cz+1=0 of each sliding window is constructed using the least squares method; According to the plurality of fitting plane equations, using the formula s=cos -1 (C) determining the inclination of the grid within each sliding window; Using the sliding window and the maximum order gradient value, determine the order gradient of the grid in each sliding window; According to the foot end sinking amount or foot end deformation amount of the robot, use the formula Determine the normal softness of each point; Determine the average normal softness of all points in the grid, which is the normal softness of the grid; According to the normal force on the robot's foot, use the formula Determine the degree of tangential friction at each point; Determine the average value of the tangential friction degree of all points in the grid, which is the tangential friction degree of the grid; According to the roughness, slope, step gradient, normal softness and tangential friction, the passability of each grid is determined to obtain a passability map; Where r is the roughness of the grid; v is the divergence of the vector; v = (N-1) / (NR); N is the number of vectors in the window; R is the modulus of the vector; is the average value of the x-coordinates of all the points in the grid in depth semantic coordinates; is the average value of the y coordinates of all the points in the grid in depth semantic coordinates; is the average value of the z coordinate in the depth semantic coordinates of all points in the grid; A, B and C are the coefficients of the fitting plane equation; s is the inclination; δ is the normal softness; k is the equivalent stiffness coefficient of the terrain; n is the exponential coefficient of the foot end settlement or foot end deformation; F' N is the rated contact force of the robot; f is the degree of tangential friction, α and β are weights, α+β=1; F N is the normal force on the foot end; μ is the sliding friction coefficient of the ground; K is the shear deformation modulus coefficient of the soft ground; The passability cost function is: Among them, τ is the permeability of the terrain, τ∈[0,1]; ω1 is the terrain slope weight, ω2 is the roughness weight, ω3 is the order gradient weight, ω4 is the normal softness weight, ω5 is the tangential friction weight, ω1+ω2+ω3+ω4+ω5=1; s is the terrain slope; h is the order gradient; δ is the normal softness; f is the tangential friction; s crit is the critical value of terrain slope; r crit is the critical value of roughness; h crit is the step gradient critical value; δ crit is the critical value of normal softness; f crit is the critical value of tangential friction.
2. A robot control method based on terrain accessibility according to claim 1, characterized in that: The step of constructing a 2.5-dimensional digital elevation map according to the depth image and the color image of the robot's external environment and the posture parameters of the robot specifically includes: The color image is input into the terrain category recognition model to obtain the terrain category l of each pixel point in the color image type ; The terrain category recognition model is obtained by training a neural network using a labeled color image of the robot's external environment; The coordinates of each pixel in the color image in the color image coordinate system (x C ,y C ) add terrain category l type , get the semantic coordinates (x C ,y C ,l type ); Based on the first rotation matrix, the coordinates (x D ,y D ,z D ), converted to the coordinates (x C ,y C ,z C ); The semantic coordinates (x C ,y C ,l type ) to add depth information z C , get the depth semantic coordinates (x C ,y C ,z C ,l type ); Based on the second rotation matrix, the depth semantic coordinates (x C ,y C ,z C ,l type ), which is converted into the depth semantic coordinates (x I ,y I ,z I ,l type ); Add the robot's posture parameters to the depth semantic coordinates (x I ,y I ,z I ,l type ) to obtain a 2.5-dimensional digital elevation map.
3. A robot control method based on terrain accessibility according to claim 1, characterized in that: The method of dividing the robot's external environment into multiple passability areas according to the passability map specifically includes: Determine any grid in the passability map as the current grid; Determine whether the passability of the current grid is less than a first threshold, and obtain a first determination result; If the first judgment result is yes, dividing the current grid into a low passability area; If the first judgment result is no, then judging whether the passability of the current grid is less than a second threshold value, and obtaining a second judgment result; the first threshold value is less than the second threshold value; If the second judgment result is yes, dividing the current grid into a medium passability area; If the second judgment result is no, the current grid is divided into a high passability area.
4. The robot control method based on terrain accessibility according to claim 3, characterized in that: The path planning of the robot according to the multiple passing degree areas specifically includes: Get the starting point and target point of the path; Planning a path of the robot in a passability region according to the starting point and the target point; the passability region includes a medium passability region and a high passability region; Control the robot to move along the planned path.
5. The robot control method based on terrain accessibility according to claim 4, characterized in that: The controlling the robot to travel along the planned path specifically includes: When the robot passes through the medium passability area along the planned path, controlling the robot to travel at a first speed; When the robot passes through the high-passability area along the planned path, the robot is controlled to travel at a second speed; the first speed is less than the second speed.
6. A robot control system based on terrain accessibility, characterized in that: include: A data acquisition module, used to acquire a depth image and a color image of the robot's external environment, as well as posture parameters of the robot; A 2.5-dimensional digital elevation map construction module, used to construct a 2.5-dimensional digital elevation map according to the depth image and color image of the robot's external environment and the posture parameters of the robot; A passability map according module is used to determine the passability of each grid in the 2.5-dimensional digital elevation map using a passability cost function to obtain a passability map; A passability region division module is used to divide the robot's external environment into multiple passability regions according to the passability map; A path planning module, used for planning the path of the robot according to various passing areas; The accessibility map specifically includes, according to the module: A roughness determination unit, used to determine the roughness of each grid in the 2.5-dimensional digital elevation map by using a sliding window and a formula r=1 / v; A fitting plane equation construction unit, used to construct a fitting plane equation Ax+By+Cz+1=0 for each sliding window using a least squares method; The inclination determination unit is used to determine the inclination according to the plurality of fitting plane equations using the formula s=cos -1 (C) determining the inclination of the grid within each sliding window; A step gradient determination unit, used to determine the step gradient of the grid in each sliding window by using the sliding window and the maximum step gradient value; The normal softness determination unit is used to determine the softness of the robot according to the foot end sinking amount or foot end deformation amount using the formula Determine the normal softness of each point; A grid normal softness determination unit is used to determine the average normal softness of all points in the grid, which is the normal softness of the grid; The tangential friction degree determination unit is used to determine the normal force on the foot end of the robot using the formula Determine the degree of tangential friction at each point; A grid tangential friction degree determination unit is used to determine the average value of the tangential friction degree of all points in the grid, which is the tangential friction degree of the grid; A passability map determination unit is used to determine the passability of each grid according to the roughness, inclination, step gradient, normal softness and tangential friction to obtain a passability map; Where r is the roughness of the grid; v is the divergence of the vector; v = (N-1) / (NR); N is the number of vectors in the window; R is the modulus of the vector; is the average value of the x-coordinates of all the points in the grid in depth semantic coordinates; is the average value of the y coordinates of all the points in the grid in depth semantic coordinates; is the average value of the z coordinate in the depth semantic coordinates of all points in the grid; A, B and C are the coefficients of the fitting plane equation; s is the inclination; δ is the normal softness; k is the equivalent stiffness coefficient of the terrain; n is the exponential coefficient of the foot end settlement or foot end deformation; F' N is the rated contact force of the robot; f is the degree of tangential friction, α and β are weights, α+β=1; F N is the normal force on the foot end; μ is the sliding friction coefficient of the ground; K is the shear deformation modulus coefficient of the soft ground; The passability cost function is: Among them, τ is the permeability of the terrain, τ∈[0,1]; ω1 is the terrain slope weight, ω2 is the roughness weight, ω3 is the order gradient weight, ω4 is the normal softness weight, ω5 is the tangential friction weight, ω1+ω2+ω3+ω4+ω5=1; s is the terrain slope; h is the order gradient; δ is the normal softness; f is the tangential friction; s crit is the critical value of terrain slope; r crit is the critical value of roughness; h crit is the step gradient critical value; δ crit is the critical value of normal softness; f crit is the critical value of tangential friction.
7. A robot control system based on terrain accessibility according to claim 6, characterized in that: The 2.5-dimensional digital elevation map construction module specifically includes: A terrain category recognition unit is used to input the color image into a terrain category recognition model to obtain the terrain category l of each pixel point of the color image. type ; The terrain category recognition model is obtained by training a neural network using a labeled color image of the robot's external environment; The semantic coordinate determination unit is used to determine the coordinates (x C ,y C ) add terrain category l type , get the semantic coordinates (x C ,y C ,l type ); A coordinate determination unit in the color image coordinate system is used to transform the coordinates (x D ,y D ,z D ), converted to the coordinates (x C ,y C ,z C ); The depth semantic coordinate determination unit is used to determine the semantic coordinates (x C ,y C ,l type ) to add depth information z C , get the depth semantic coordinates (x C ,y C ,z C ,l type ); A depth semantic coordinate conversion unit is used to convert the depth semantic coordinate (x C ,y C ,z C ,l type ), which is converted into the depth semantic coordinates (x I ,y I ,z I ,l type ); 2.5D digital elevation map determination unit, used to add the robot's posture parameters to the depth semantic coordinates (x I ,y I ,z I ,l type ) to obtain a 2.5-dimensional digital elevation map.
Citation Information
Patent Citations
Sea target size detection method based on vision and laser sensor data fusion
CN109283538A
Dynamic cost map navigation method based on line laser and binocular vision
CN109765901A
Method for judging passing ability of obstacles in front of robot
CN111046846A
Unmanned aerial vehicle control method and device
CN111984026A
Improved lazy theta method for smoothing operation path of wheeled robot under three-dimensional terrain
CN112917476A
Cited By
Terrain and force fused quadruped robot reachability map construction method and system
CN116147642A
