CN-121720487-B - Unstructured environment-oriented robot dynamic path planning method and system

CN121720487BCN 121720487 BCN121720487 BCN 121720487BCN-121720487-B

Abstract

The invention provides a method and a system for planning a dynamic path of a robot facing an unstructured environment, wherein the method comprises the steps of maintaining a local elevation feature grid map which takes the robot as a center and is in a preset range in real time to determine that a motion mode of the robot in a next control period is a stable mode, a damping mode or an obstacle crossing mode, calling constraint item weight parameter values of a cost function corresponding to the motion mode according to the motion mode, wherein the cost function is calculated according to weighted sum of heading items, distance items, speed items, traffic items and heading vertical constraint items, obtaining a plurality of simulation track sequences of the robot in the next control period based on DWA search, taking a group of speed value sets corresponding to the simulation track sequences with minimum cost calculated based on the cost function as optimal solutions, and generating a speed control instruction of the next control period to issue. The invention realizes a control technology for realizing self-adaptive obstacle avoidance and obstacle surmounting of the robot.

Inventors

LI QIMEI
YU HUAI
Hu Anglong
JIANG MING
XU JINXIANG

Assignees

武汉大学

Dates

Publication Date: 20260508
Application Date: 20260226

Claims (10)

1. The method for planning the dynamic path of the robot facing the unstructured environment is characterized by comprising the following steps of: Maintaining a local elevation feature grid map which takes the robot as a center and is in a preset range in real time, and determining that the motion mode of the robot in the next control period is a stable mode, a damping mode or an obstacle crossing mode according to the local elevation feature grid map and roll angle and pitch angle data of the robot at the current moment; Invoking constraint item weight parameter values of a cost function corresponding to the motion mode according to the motion mode so as to enable the robot to cruise at a high speed in a stable mode, descend in a damping mode, and cut into the terrain in a forward direction in an obstacle crossing mode, wherein the cost function is calculated according to the weighted sum of a heading item, a distance item, a speed item, a traffic item and a heading vertical constraint item; searching to obtain a plurality of simulation track sequences of the robot in the next control period based on a dynamic window method, taking a group of speed value sets corresponding to the simulation track sequences with the minimum cost calculated based on the cost function as an optimal solution, and generating a speed control instruction of the next control period to issue.
2. The method for planning a dynamic path of a robot facing an unstructured environment according to claim 1, wherein the step of determining that the motion mode of the robot in the next control period is a stable mode, a damping mode or an obstacle surmounting mode according to the local elevation feature grid map and roll angle and pitch angle data of the robot at the current moment specifically comprises: Calculating the passing cost of each grid based on the local elevation characteristic grid map, generating a cost map and counting the height distribution characteristics to be unimodal distribution or bimodal distribution based on the grid elevation values; under the condition that the terrain in front of the robot is identified as unimodal distribution, the height variance is higher than a preset threshold value, and the angles of the roll angle and the pitch angle of the robot are both within a preset angle safety range, determining that the motion mode of the current period is a damping mode; when the terrain in front of the robot is identified as bimodal distribution or the angle of the roll angle or the pitch angle of the robot exceeds a preset safety range, determining the motion mode of the current period as an obstacle crossing mode; in other cases, the motion pattern of the current period is determined to be a stationary pattern.
3. The method for planning a dynamic path of a robot facing an unstructured environment according to claim 2, wherein the steps of calculating a passing cost of each grid based on the local elevation feature grid map, generating a cost map and counting the height distribution feature as unimodal distribution or bimodal distribution based on the grid elevation values specifically comprise: Traversing the effective grids in the local elevation feature grid map, acquiring a height set of point clouds in each effective grid, and counting to obtain a height histogram of each grid; Calculating the ground roughness and the terrain gradient of each grid based on the height histogram, and marking the distribution characteristics of the height histogram of the grid as unimodal distribution or bimodal distribution; and generating a cost map corresponding to the local elevation feature grid map based on the ground roughness, the terrain gradient and the distribution features of the effective grid area, so that the grids with high ground roughness and terrain gradient have higher cost, and a preset great cost value is given to the grids with bimodal distribution.
4. The method for planning the dynamic path of the robot facing the unstructured environment according to claim 3, wherein the pass degree term of the cost function is calculated according to the sum of grid cost values of grids corresponding to the track, and the heading vertical constraint term is calculated based on the tangential angle of the tail end of the track and the normal vector angle of the elevation gradient of the terrain in front.
5. The method for dynamic path planning for a robot in an unstructured environment according to claim 1, further comprising: acquiring point cloud data flow acquired by a laser radar on a robot in real time, acquiring the current angular speed and acceleration of the robot by an inertial measurement unit on the robot in real time, and calculating to obtain a real-time pitch angle and a roll angle of the robot; And after performing voxel grid downsampling and high-pass filtering on the acquired point cloud data stream, converting the point cloud data into a preset robot coordinate system.
6. The method for planning a dynamic path of a robot facing an unstructured environment according to claim 5, wherein the step of maintaining a local elevation feature grid map with a preset range centered on the robot in real time specifically comprises: a local rolling grid map which takes the center of the robot as an origin and moves along with the robot is established, the local rolling grid map discretizes the horizontal ground which takes the robot as the center and is in a preset range into a two-dimensional grid with a fixed size, and a point cloud height set in the range is maintained in real time.
7. An unstructured environment-oriented dynamic path planning system for a robot, comprising: the data acquisition module is used for maintaining a local elevation characteristic grid map which takes the robot as a center and is in a preset range in real time, and determining that the motion mode of the robot in the next control period is a stable mode, a damping mode or an obstacle crossing mode according to the local elevation characteristic grid map and roll angle and pitch angle data of the robot at the current moment; The parameter determining module is used for calling the weight parameter value of the constraint item of the cost function corresponding to the motion mode according to the motion mode so that the robot cruises at a high speed in a stable mode, descends in a damping mode, and cuts into the terrain in the forward direction in a obstacle crossing mode, wherein the cost function is calculated according to the weighted sum of the heading item, the distance item, the speed item, the traffic item and the heading vertical constraint item; The path generation module is used for searching and obtaining a plurality of simulation track sequences of the robot in the next control period based on a dynamic window method, taking a group of speed value sets corresponding to the simulation track sequences with the minimum cost calculated based on the cost function as an optimal solution, and generating a speed control instruction of the next control period to issue.
8. An electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, characterized in that the processor implements the method of robot dynamic path planning for unstructured environments according to any of claims 1 to 6 when executing the program.
9. A non-transitory computer readable storage medium, on which a computer program is stored, characterized in that the computer program, when executed by a processor, implements the method of robot dynamic path planning for unstructured environments according to any of claims 1 to 6.
10. A computer program product comprising a computer program, characterized in that the computer program, when executed by a processor, implements a method for dynamic path planning of a robot facing an unstructured environment according to any of claims 1 to 6.

Description

Unstructured environment-oriented robot dynamic path planning method and system Technical Field The invention relates to the technical field of robot navigation, in particular to a method and a system for planning a dynamic path of a robot facing an unstructured environment. Background With the rapid development of robot technology, the wheel-foot type robot (quadruped robot) has wide application in the fields of substation inspection, building security, post-disaster rescue and the like because of the high efficiency of wheel type movement and the obstacle crossing capability of foot type movement. In these practical application scenarios, robots often face complex unstructured environments with multiple floors and obstacle crossing, which presents extremely high challenges for autonomous positioning navigation and path planning capabilities of the robots. The path planning is a core link for realizing autonomous navigation of the robot, and the main task of the path planning is to find a collision-free and optimal motion track from a starting point to a target point in an environment where an obstacle exists. In the local path planning algorithm, the dynamic window method (Dynamic Window Approach, DWA) is widely applied to various mobile robot systems because the dynamic window method has small calculated amount and strong real-time performance and can simultaneously consider the requirements of speed and obstacle avoidance. The traditional DWA algorithm mainly converts the environment into a free space and an obstacle space according to plane distance information acquired by a sensor (such as a 2D laser radar), and selects an optimal control instruction by simulating a plurality of speed tracks and evaluating the distance between the speed tracks and the obstacle, the target course deviation and the speed. However, the existing DWA algorithm has limitations on physical perception of terrain, cross-layer and obstacle crossing gesture control, and is difficult to meet the high-precision motion control requirement of the four-wheel foot robot. Disclosure of Invention The invention provides a method and a system for planning a dynamic path of a robot facing an unstructured environment, which are used for solving the defect that a path planning algorithm in the prior art is difficult to effectively cope with the robot navigation in a complex three-dimensional environment, and realizing the path planning method which can integrate three-dimensional features of terrain and dynamically adjust a track evaluation strategy according to the trafficability of the terrain. The invention provides a robot dynamic path planning method facing unstructured environment, which comprises the following steps: maintaining a local elevation characteristic grid map which takes the robot as a center and is in a preset range in real time, and determining that the motion mode of the robot in the next control period is a stable mode, a damping mode or an obstacle crossing mode according to the local elevation characteristic grid map; Invoking constraint item weight parameter values of a cost function corresponding to the motion mode according to the motion mode so as to enable the robot to cruise at a high speed in a stable mode, descend in a damping mode, and cut into the terrain in a forward direction in an obstacle crossing mode, wherein the cost function is calculated according to the weighted sum of a heading item, a distance item, a speed item, a traffic item and a heading vertical constraint item; Searching to obtain a plurality of simulation track sequences of the robot in the next control period based on a dynamic window method, taking a group of speed value sets corresponding to the simulation track sequences with the minimum cost calculated based on a cost function as an optimal solution, and generating a speed control instruction of the next control period to issue According to the method for planning the dynamic path of the robot facing the unstructured environment, which is provided by the invention, the step of determining that the motion mode of the robot in the next control period is a stable mode, a damping mode or an obstacle surmounting mode according to the local elevation characteristic grid map comprises the following steps: Calculating the passing cost of each grid based on the local elevation characteristic grid map, generating a cost map and counting the height distribution characteristics to be unimodal distribution or bimodal distribution based on the grid elevation values; under the condition that the terrain in front of the robot is identified as unimodal distribution, the height variance is higher than a preset threshold value, and the angles of the roll angle and the pitch angle of the robot are both within a preset angle safety range, determining that the motion mode of the current period is a damping mode; when the terrain in front of the robot is identified as bimodal distribution or the angle of th