Wheeled robot path planning method considering standing point and robot mass center

Abstract

The invention discloses a method for a mobile robot to autonomously plan a path in a complex non-structural environment, and relates to the field of robot kinematics, the method comprises the following steps: pre-scanning an operating environment, generating an elevation navigation map, and rasterizing the map; the rasterized map is further screened, and grids meeting requirements are reserved; carrying out track preprocessing by utilizing the retained grids; and generating a plurality of groups of four-wheel tracks, and storing the four-wheel tracks in an array. According to the invention, the trafficability of the wheeled robot is considered on the basis of ensuring the running safety of the robot, and the trafficability of the wheeled robot in a non-structural environment is improved.

Description

Technical Field

[0001] The invention relates to the field of wheeled robot path planning in unstructured environments, and in particular to the safety and passability issues of wheeled robots when planning paths on rough roads. Background Art

[0002] Robots are no longer satisfied with operating in simple structured environments, and the demand for operating robots in unstructured environments is also increasing. Under existing technologies, the control of wheeled robots in unstructured environments is mostly achieved by remote control. They cannot fully perceive the dangers in the environment, nor can they plan a reasonable path, which greatly reduces the working efficiency of the robots. Therefore, ensuring that wheeled robots can safely and autonomously plan paths in unstructured environments has become a new demand.

[0003] One of the reasons why rough roads pose a safety threat to robot operation in unstructured environments is that rough roads can cause the center of gravity of wheeled robots to shift. If the center of gravity shift is too large, the wheeled robot will flip over. Secondly, wheeled robots will be limited by the height of their chassis when traveling on rough roads. If the standing points of wheeled robots are not considered in the operating environment, the chassis of the wheeled robot will be damaged, which will affect the safety and passability of the robot. The above two factors will cause the robot to fail in path planning operations in unstructured environments.

[0004] The patent application number 202110801080.0 discloses a method for autonomous navigation of a mobile robot in a complex environment of a nuclear accident, in which the radiation cost map and the slope cost map are superimposed when processing the navigation map to obtain a multi-layer cost map, which is then rasterized, and then a gradient threshold is selected to mark the grids with a slope exceeding the gradient threshold as obstacles to obtain a slope warning cost map. After marking the grids exceeding the threshold as obstacles, the A* algorithm is used to complete the trajectory planning.

[0005] Although the technical solution disclosed in the above patent proposes a navigation control method for a robot in a complex environment, it mainly expands the expansion layer in the navigation map and uses the robot's geometric center or center of mass as several trajectory points in the navigation map. This results in some grids that could have been stood on or path planned being excluded from the available grids. The generated grid map also has a low accuracy due to the need to ensure the safety of the robot, thereby limiting the robot's ability to cross complex terrain.

[0006] Therefore, the researchers of this patent are committed to developing a path planning method that enables a wheeled robot to cross rugged roads in a non-structured complex environment. The path generated by this planning method can ensure the safe operation of the robot while also enabling the robot to cross certain ditches, steep slopes and other rugged roads. Summary of the invention

[0007] In view of the current development status of wheeled robots, the present invention aims to fill the gap in the field of automatic movement of wheeled robots in rugged environments, improve the defects of existing inventions, and add the standing point screening and center of mass screening of the robot's four wheels to the navigation map processing process respectively, and generate a four-wheel path screening point array and screen the path in the trajectory planning stage, so that the robot can generate a reasonable path and operate safely in an unstructured environment, especially when moving on an unstructured road.

[0008] To achieve the above objectives, the present invention has made innovations in the map processing stage and the trajectory planning stage to form a new path planning method for wheeled robots in complex environments. The specific steps of the method are as follows:

[0009] Step 1: Use the three-dimensional radar carried by the wheeled robot to pre-scan the robot's environment, generate an elevation navigation map, and rasterize the generated map.

[0010] Step 2: Map processing: Grid screening is performed on the generated rasterized elevation navigation map.

[0011] Step 3: Grid marking: According to the wheel diameter of the robot, the grids are screened for the directions in which the four wheels will run. The grids that do not meet the safety requirements for the robot's operation are marked as unreachable points, and the screened grids are marked as reachable points.

[0012] Step 4: Use the processed map to perform trajectory preprocessing.

[0013] Step 5: Generate several trajectories, including the trajectories of the four wheels, and store them in a path set. The A* algorithm determines their cost values. Select a reasonable path from the path set to drive the robot to run.

[0014] Furthermore, step 1 mainly includes the following steps:

[0015] Step 1.1, the radar obtains the point cloud of the robot's environment;

[0016] Step 1.2, extracting elevation values ​​from the acquired point cloud;

[0017] Step 1.3, converting the point cloud with extracted elevation values ​​into an elevation navigation map;

[0018] Step 1.4: Rasterize the elevation navigation map. The grid size after rasterization is 50cm×50cm.

[0019] Furthermore, in step 2, the map processing process includes the following steps:

[0020] Step 2.1, filter the grids. First, filter the standing points: select the grids whose real-time altitudes of the left front, right front, left rear, and right rear wheels are higher than the vehicle chassis;

[0021] Step 2.2, then select the centroid offset point, that is, calculate the angle between the vertical normal vector of the grid and the normal vector of the coordinate system, and select the grid whose angle value is less than a certain threshold;

[0022] Step 2.3: Meshes that do not meet the standards will be eliminated.

[0023] Furthermore, in step 3, the grids converted from the points with blurred point cloud information are marked as unreachable points, and the grid screening for the four tires in the present invention replaces the current grid screening for the entire robot, thereby further improving the accuracy of the grid map passability.

[0024] Furthermore, step 4 mainly includes the following steps:

[0025] Step 4.1, filter out the grids that meet several consecutive standing points and put them in the array;

[0026] Step 4.2: Filter out the arrays that meet the centroid shift threshold in the array, and remove the arrays that do not meet the centroid shift threshold.

[0027] Further, step 5 includes the following steps:

[0028] Step 5.1: According to step 4, select the path set that meets the conditions and the trajectories of the four wheels, and put them into the path array;

[0029] Step 5.2: Analyze the cost of each path according to the A* algorithm. The steps of the A* algorithm are as follows:

[0030] Step 5.2.1, set the target node, the starting node is the current position of the robot;

[0031] Step 5.2.2, create two lists: open list and closed list;

[0032] Step 5.2.3, add the starting node to the open list;

[0033] Step 5.2.4, traverse the nodes in the open list. When the open list is empty, it means that no path is found, and the cost value is 0 and the algorithm ends. Otherwise, go to the next step.

[0034] Step 5.2.5, when the open list is not empty, select a node with the smallest path cost function value from the open list and add it to the closed list;

[0035] Step 5.2.6, determine whether the current node is the target node; if the current node is the target node, the search ends and the optimal path is found, otherwise go to step 5.2.7;

[0036] Step 5.2.7: Expand the current node and calculate the path cost function values ​​of all child nodes. This step specifically includes:

[0037] Step 5.2.8, if the child node is not in the open list or the closed list, add it to the open list and add a pointer to the current node to the child node; after the path search is completed, connect the path according to the pointer;

[0038] Step 5.2.9, if the child node is already in the open list, compare the calculated new function value with the old value in the open list, save the node with the smaller function value, and modify the pointer to point to the current child node;

[0039] Step 5.2.10: If the child node is in the closed list, ignore the node and return to step 5.2.7 to continue expanding other nodes;

[0040] Step 5.3: Use the A* algorithm F=G+H to calculate the cost values ​​of all traversable paths.

[0041] Furthermore, step 6 can drive the robot to navigate by selecting any path in the path array.

[0042] The present invention obtains elevation point cloud through three-dimensional radar, constructs elevation navigation map by extracting elevation point cloud, and rasterizes the navigation map; then screens out grids that are not safe for robot operation, collects the remaining grids in an array, and completes the map processing part; the path planning part further refines the grid, and screens out grids that meet the requirements of standing points and centroid threshold range for several consecutive times, then generates several paths by traversing the grid and collects them in the path array, and calculates the cost value of each path by A* algorithm; selects one of the paths to drive the robot to navigate.

[0043] The concept, specific structure and technical effects of the present invention will be further described below in conjunction with the accompanying drawings to fully reflect the purpose, characteristics and effects of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 It is a flow chart of the present invention from data collection to path planning.

[0045] Figure 2 Schematic diagram of a wheeled robot performing the method of the present invention.

[0046] Figure 3 This is a schematic diagram of the map update process.

[0047] Figure 4 This is an example of path and posture selection during path planning for a wheeled robot.

[0048] Figure 5 This is an example of the basis for judging the grid passability of a wheeled robot in a slope environment. DETAILED DESCRIPTION

[0049] The following are specific embodiments of the present invention with reference to the specification and drawings, the purpose is to make the relevant technology more intuitive and easy to understand. The present invention can be easily implemented through simulation or examples, and the protection scope of the present invention is not limited to the embodiments mentioned in the text.

[0050] In the attached drawings, the navigation map represented by the grid map is used as an example and is not limited to the working environment of the wheeled robot. The relevant labels of the grids should be determined according to the actual situation. The present invention does not limit the size of the robot. Because the technical details need to be highlighted, the size of the robot in the drawings may vary to a certain extent.

[0051] like Figure 1 As shown in FIG. 1 , a complete operation process of the present invention is shown, which mainly includes the following steps:

[0052] Step 1: Preprocess the environment in which the robot will operate, obtain environmental information through 3D radar, create an elevation map, and rasterize the map.

[0053] Step 2: Filter the map grids and mark the grids that threaten the static safety of the robot as unreachable areas.

[0054] Step 3: Make an overall assessment of the grids formed by the fuzzy point cloud and the filtered grids, and mark the grids that threaten the robot's safety as unreachable areas.

[0055] Step 4: Preprocess the travel trajectory to form a four-wheel trajectory point array; further screen the trajectory array to remove grids that threaten the robot's travel safety to form a new four-wheel trajectory array.

[0056] Step 5: Based on the updated array information, the A* algorithm is used to evaluate the trajectory cost, calculate the cost value of the trajectory, and perform trajectory planning.

[0057] Specifically, map data collection includes the following steps:

[0058] Step 1.1: Use 3D radar to collect point cloud data, and use Ubuntu 18.04.5 system, ROS system, and Rviz to rasterize the collected data packets.

[0059] Step 1.2: In combination with the size and dynamic characteristics of the robot, the present invention sets the grid size to 50 cm×50 cm.

[0060] Specifically, the map grid screening diagram is as follows Figure 3 As shown in Figure 1, it reflects the update process of the map from M1 to M′5. The specific steps are as follows:

[0061] Step 2.1, first, number the grid points and form a map matrix M1 as follows:

[0062]

[0063] Among them, x i y n Indicates the specific horizontal position of the map grid, h i，n Represents the elevation value at the grid.

[0064] Step 2.2, first filter the grid of the robot's starting position. The filter area is a circular area with the robot's left rear wheel as the center and the robot's turning radius as the radius. The robot's starting position is a flat area by default. i，n The continuous grid area with an average fluctuation value of less than 5cm is screened. This area is usually regarded as a flat area. The purpose of this step is to screen out the grids in the area with elevation values ​​higher than the robot chassis + altitude and remove them.

[0065] Step 2.3, further, remove the area outside the robot's starting position, for h in For areas with a undulation higher than 5cm, the elevation interpolation with the surrounding grids is determined. Areas with an interpolation value higher than 20cm are considered steep hilly areas and marked as unreachable areas, and the grids are removed. The removed grid map is updated to M2, and the removed grid is updated to null. The M2 example is as follows:

[0066]

[0067] Step 2.4, further, for h i，n The area where the absolute value increases linearly to a certain extent is considered as the slope area, and the robot normal vector V is introduced CAR With the grid normal vector V GRID Absolute value of angle ω i，n , update the grid map to M3, the example is as follows:

[0068]

[0069] Step 2.5, further, for ω i，n Grids with absolute values ​​greater than 30° are removed and updated to grid map M4. The example is as follows:

[0070]

[0071] Step 3.1, further, for the neighboring grids with data retained near the deleted grid, the passability of the grid is determined according to the robot's kinematic characteristics and size characteristics, the impassable grid information is eliminated, and the grid map is updated to M5. The example is as follows:

[0072]

[0073] Step 3.2, integrate the data in M5, and the updated M′5 instance is as follows, where m i，n =(x i ,y n ,h i，n ,ω i，n ).

[0074]

[0075] Specifically, trajectory processing includes the following steps:

[0076] Step 4.1, select four rounds of continuous data point areas based on the map data retained in step 3.2.

[0077] Step 4.2: Generate a four-round continuous standing point dataset p based on the continuous area selected in step 4.1 i , (i=1, 2……, n), p i =(r lf , r rf , r lr , r rr ), which includes the left front wheel position r lf , right front wheel position r rf , left rear wheel position r l x, right rear wheel position rx r . Where tire position r = m i，n , the continuous point data set constitutes the path array P i , (i=1, 2……n). Each group P i All represent the four-wheel grid positions of the robot from the starting point to the target point, that is, each group of P i Represents a complete path. The specific examples are as follows:

[0078] P1=[(m 2，1 , m 2，2 , m 1，1 , m 2，1 ), (m 3，1 , m 3，2 , m 2，1 , m2，2 ), (m 3，3 , m 2，3 , m 3，2 , m 2，2 ),……(m 6，5 , m 6，6 , m 5，5 , m 5，6 )]

[0079] Furthermore, P1=(p1,p2,p3……p 10 )

[0080] P2=[(m 2，1 , m 2，2 , m 1，1 , m 2，1 ), (m 3，1 , m 3，2 , m 2，1 , m 2，2 ), (m 4，2 , m 4，3 , m 3，1 , m 3，2 ),……(m 6，5 , m 6，6 , m 5，5 , m 5，6 )]

[0081] Furthermore, P2=(p1,p2,p3,……,p8)

[0082] Step 5.1, calculate P according to the A* algorithm i The cost formula is F=G+H.

[0083] Step 5.2, compare P i The cost value is calculated and one of the paths is selected to enable the robot to complete the trajectory planning task.

[0084] The above describes the example process of the present invention in detail. It should be understood that ordinary technicians in the field can make many modifications and changes based on the concept of the present invention without creative work. Therefore, all technical solutions that can be obtained by technicians in the technical field based on the concept of the present invention through logical analysis, reasoning or limited experiments on the basis of the prior art should be within the scope of protection determined by the claims.

Claims

1. A path planning method for a mobile robot in an unstructured environment, which increases the accuracy of passability. The main steps are as follows: Step 1: Use the three-dimensional radar carried by the wheeled robot to pre-scan the robot's environment, generate an elevation navigation map, and rasterize the generated map. Step 2: Map processing: Grid screening is performed on the generated rasterized elevation navigation map. Step 3: Overall evaluation of the grid. According to the wheel diameter size of the robot, grid screening is performed for the directions in which the four wheels will run. Grids that do not meet the safety requirements for robot operation are marked as unreachable points, and the screened grids are marked as reachable points. Step 4: Use the processed map to perform trajectory preprocessing. Step 5: Generate several trajectories, including the trajectories of the four wheels, and store them in a path set. The A* algorithm determines their cost values. Select a reasonable path from the path set to drive the robot to run.

2. The path planning method for a mobile robot in a complex unstructured environment as claimed in claim 1, characterized in that After extracting environmental values ​​through 3D radar and combining them with the robot's own characteristics, the map is refined, and finally a high-precision elevation grid map is obtained, which includes the elevation value and the absolute value of the angle between the robot's normal vector and the grid's normal vector.

3. The path planning method for a mobile robot in a complex non-structured environment as claimed in claim 2, characterized in that: In step 2, the complex environment map processing process specifically includes the following steps: Step 2.1: Filter the grid Step 2.2: Filter the centroid offset points Step 2.3: Eliminate dangerous grids.

4. The path planning method for a mobile robot in a complex non-structured environment as claimed in claim 3, characterized in that: In step 3, the overall evaluation process of the complex environment grid specifically includes the following steps: Step 3.1, determine the neighboring grids that still have data retained near the deleted grid. Step 3.2, integrate the data in the atlas.

5. The path planning method for a mobile robot in a complex non-structured environment as claimed in claim 4, characterized in that: In step 4, the complex environment trajectory preprocessing generation process specifically includes the following steps: Step 4.1, select four consecutive data point regions. Step 4.2: Generate a four-round continuous standing point dataset.

6. The path planning method for a mobile robot in a complex non-structured environment as claimed in claim 5, characterized in that: In step 5, the complex environment trajectory generation will use the A* algorithm to calculate the cost value and then generate several trajectories for selection to drive the robot to complete the task target.

Images