CN114577214B - Wheeled robot path planning method applied to cross-heterogeneous multi-layer space - Google Patents

Wheeled robot path planning method applied to cross-heterogeneous multi-layer space Download PDF

Info

Publication number
CN114577214B
CN114577214B CN202210203947.7A CN202210203947A CN114577214B CN 114577214 B CN114577214 B CN 114577214B CN 202210203947 A CN202210203947 A CN 202210203947A CN 114577214 B CN114577214 B CN 114577214B
Authority
CN
China
Prior art keywords
map
grid
node
path
point
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202210203947.7A
Other languages
Chinese (zh)
Other versions
CN114577214A (en
Inventor
吴立刚
田昊宇
高亚斌
姚蔚然
齐乃明
吴承伟
刘健行
孙光辉
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Institute of Technology
Original Assignee
Harbin Institute of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Harbin Institute of Technology filed Critical Harbin Institute of Technology
Priority to CN202210203947.7A priority Critical patent/CN114577214B/en
Publication of CN114577214A publication Critical patent/CN114577214A/en
Application granted granted Critical
Publication of CN114577214B publication Critical patent/CN114577214B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention discloses a wheel type robot path planning method applied to a multi-layer space, and relates to a mobile robot path planning method of a multi-layer space. The invention aims to solve the problem that a path planning algorithm cannot obtain accurate robot path planning in a multilayer space. A wheel type robot path planning method applied to a cross-heterogeneous multi-layer space comprises the following specific processes: step 1, constructing a map network of a multilayer space; and 2, searching a path in the map network based on the map network of the multilayer space. The invention is used for the technical field of robot mapping and navigation algorithms.

Description

Wheeled robot path planning method applied to cross-heterogeneous multi-layer space
Technical Field
The invention relates to a mobile robot path planning method in a multilayer space, and belongs to the technical field of robot mapping and navigation algorithms.
Background
The wheel type mobile robot is a common mobile robot, has the characteristics of simple structure, low cost and good stability, and can play an important role in various scenes. The mobile robot adopts various sensors such as a laser radar, a camera, an inertia measurement unit, a speedometer and the like to construct an environment map and perform self-positioning, and a motion path and a motion speed are planned in the map so as to reach a preset target. At present, the methods of positioning and path planning and motion planning of a robot in a two-dimensional plane are mature.
With the development of science and technology, the application field of the wheeled mobile robot is increasingly wide, and the moving range is not limited to a two-dimensional plane any more and is gradually expanded to a three-dimensional multilayer space. The cross-heterogeneous multi-layer space is composed of a plurality of two-dimensional plane areas with different heights and connecting areas between the planes, each two-dimensional plane area has different shapes, and common cross-heterogeneous multi-layer spaces comprise subway stations, airports, underground parking lots and the like. In the process that the robot executes tasks such as cleaning and patrol in a multi-layer heterogeneous complex space, the movement path planning in the multi-layer space is needed. Compared with a two-dimensional plane, the environment complexity of the path planning problem of the multi-layer space is increased, and the calculation amount of the path planning is increased. The problem of area overlapping can be caused by the mapping of a heterogeneous multi-layer complex space on a two-dimensional plane, and the effectiveness of a two-dimensional plane path planning method is influenced. In addition, the problems of robot height change, wheeled mobile robot posture constraint and the like which cannot occur in a two-dimensional plane exist in path planning of a multi-layer complex space. These problems make the traditional path planning algorithm for two-dimensional maps difficult to apply directly across heterogeneous multi-layered complex spaces.
Disclosure of Invention
The invention aims to solve the problems that the height change of a robot, the posture constraint of a wheeled mobile robot and the like cannot occur in a two-dimensional plane in the existing path planning of the cross-heterogeneous multi-layer complex space, and the like, and the problems cause that the path planning algorithm of the traditional two-dimensional map is difficult to be directly applied to the cross-heterogeneous multi-layer complex space, so that the accurate multi-layer space robot path planning cannot be obtained, and provides a path planning method of the wheeled robot applied to the cross-heterogeneous multi-layer space.
A wheel type robot path planning method applied to a cross-heterogeneous multi-layer space comprises the following specific processes:
step 1, constructing a map network of a multilayer space;
and 2, searching a path in the map network based on the map network of the multilayer space.
The invention has the beneficial effects that:
1. the invention provides a method for forming a map network description by a plurality of two-dimensional grid maps to cross heterogeneous multi-layer complex spaces. Compared with the conventional method for describing the space environment by using a three-dimensional point cloud map, the method can effectively reduce the data volume of map storage;
2. the invention provides a hierarchical A-star search path planning method based on a map network, which solves the problem of path planning across heterogeneous multi-layer spaces by a hierarchical two-dimensional space planning method;
3. the invention provides a two-dimensional grid map path planning method considering terrain factors by considering motion constraint of a wheeled robot, and solves the problem of motion constraint of the robot in a complex terrain environment.
Drawings
FIG. 1 is a schematic diagram of a map network according to the present invention;
FIG. 2a is a schematic view of a spatial zone map according to the present invention;
FIG. 2b is a schematic view of a multi-level two-dimensional grid map according to the present invention;
FIG. 3 is a schematic diagram of path planning in a map network according to the present invention;
FIG. 4 is a schematic diagram of a two-dimensional grid map path planning of the present invention;
FIG. 5 is a flow chart of a map network construction of the present invention;
FIG. 6 is a flow chart of path planning within a map network according to the present invention;
fig. 7 is a flow chart of path planning in a two-dimensional grid map according to the present invention.
Detailed Description
The first embodiment is as follows: the method for planning the path of the wheeled robot applied to the cross-heterogeneous multi-layer space comprises the following specific processes:
as shown in fig. 1, the map network is composed of a plurality of path nodes and a plurality of maps, and a single map is used as an edge connecting the path nodes in the map network.
As shown in fig. 2a and 2b, the partial region crossing the heterogeneous space shown in the left drawing includes a floor plane region and a stair slope region, and the plane region includes an obstacle. And (4) as shown in the right graph corresponding to the two-dimensional map, wherein the static map layer comprises the boundary and the obstacle position which are measured by SLAM mapping, the cost map layer expands the obstacle boundary part with a certain expansion radius, the rest area is a passable area, and the information of the stair slope area is marked in the topographic map layer.
As shown in fig. 3Point in the drawing 0 To point 8 For original path nodes of map networks, points start And point dest The starting point and the end point for path planning are respectively positioned on the map start And map dest The above. Creating a two-dimensional grid map between a starting point and an end point and between the starting point and the end point of the original map and the original map start1 、map start2 、map dest1 、map dest2 Using the original two-dimensional grid map start And map dest The content is used as the content of the newly-built two-dimensional grid map, and the position information of the starting point and the ending point of the map is replaced. Forming a temporary map network by using an original map network, a newly-built path node and a newly-built two-dimensional raster map, and starting from a starting point in the temporary map network start A search is started.
As shown in fig. 4, a path from a start grid point to a target grid point is planned in a two-dimensional grid map. If the point 1 in the graph is not on the stair slope area of the topographic map layer of the two-dimensional grid map, searching and expanding 8 grid points around the grid point according to a conventional mode, and performing feasibility judgment, cost estimation and other processing on 8 adjacent grid points; in the figure, the 2 points are located in a stair slope area, generalized grid points are generated along the gradient direction of a slope and the opposite direction of the slope, and feasibility judgment, cost estimation and other processing are performed on the 2 newly-built generalized grid points.
The cross-heterogeneous multi-layer space refers to a heterogeneous multi-layer space, such as a building multi-layer space of an airport railway station and the like, wherein the configuration, the boundary and the shape of an obstacle and the connection among layers in the space are different. The method can realize the cross-layer movement of the robot, so the method is a wheel type robot path planning method crossing heterogeneous multi-layer space.
The invention is mainly used for solving the problem of path planning of the wheel type mobile robot across heterogeneous multi-layer spaces, and mainly comprises a map network construction method of the multi-layer spaces and a hierarchical A-search path planning method based on the map network.
Step 1, constructing a map network of a multilayer space (the multilayer space refers to multilayer buildings such as subway stations, airports, underground parking lots and the like, and the number of the multilayer buildings is more than or equal to 2);
and 2, searching a path in the map network based on the map network of the multilayer space.
The second embodiment is as follows: the present embodiment is different from the first embodiment in that a map network of a multi-story space (the multi-story space refers to multi-story buildings such as subway stations, airports, underground parking lots, and the like, and the number of the multi-story is 2 or more) is constructed in step 1; the specific process is as follows:
step 11, dividing the multilayer space at the position of a connection part between a stair and a plane, a stair turntable and the like into a plurality of areas, wherein the projections of the areas on a two-dimensional plane are not overlapped;
the multilayer space map network is formed by connecting path nodes (area division positions) in space with two-dimensional grid maps of all areas;
in the multilayer space map network, path nodes in space are used as nodes of the network, and a two-dimensional grid map is used as an edge of the network;
the path node set of the multilayer space map network is marked as Points ═ point i |i＝0,1,2,…,N-1}；
Wherein N is the number of path nodes in the map network; point i Is the ith path node;
the two-dimensional grid map set of the multi-layer space map network is marked as Maps ═ map j |j＝0,1,2,…,M-1}；
Wherein M is the number of two-dimensional grid maps in the map network; map j Is the jth two-dimensional grid map;
the multi-layer space map network information consists of a path node set and a two-dimensional raster map set, and is marked as GraphNet (Points, Maps);
the multi-layer space map network is divided into static map layers j Cost map layer costmap j And a topographic map layer terrainmap j ；
Two ends of the two-dimensional grid map are path points, wherein one point is used as a map building origin (coordinate system origin) of the two-dimensional grid map;
a two-dimensional grid map is formed by projecting a partial area of a multi-layer space in a two-dimensional planeIs obtained by rasterization, and records the map building starting point and the map building end point start of the two-dimensional grid map of the area j ,dest j ∈Points；
Position and posture of map building starting point and map building end point of two-dimensional grid map in world coordinate system F 0 The inner transform is a homogeneous transform matrix, denoted as
Figure BDA0003528396510000041
Map coordinate system F of map-building end point of two-dimensional grid map j The position and posture in (1) is converted into a homogeneous transformation matrix represented as
Figure BDA0003528396510000042
The multilayer space map is divided into static map layers j Cost map layer costmap j And the terrain map layer terrainmap j ；
The static map layer divides a map area into a passable area, a barrier occupation area and an unknown area;
the cost map layer is obtained by expanding the occupied area of the obstacle through a static map and is used for avoiding the obstacle in the path planning;
the terrain map layer comprises position and gradient direction information of a slope and a stair area of a map area, and is used for planning the path planning of the robot in a special area so as to meet the motion constraint of the robot;
two-dimensional grid map with n j ×m j Storing in matrix form, wherein the row direction is the y direction of the coordinate system of the two-dimensional grid map, the column direction is the x direction of the coordinate system of the two-dimensional grid map, and recording the x coordinate o of the origin of the matrix of the two-dimensional grid map in the coordinate system of the map xj And y coordinate o yj Recording the position and the posture of the two-dimensional grid map coordinate system relative to a world coordinate system;
the two-dimensional grid map is represented as:
Figure BDA0003528396510000051
wherein, static map j For static map layers, costmap j For cost map layers, terrainmap j Is a topographic map layer; staticmap j ,costmap j ,
Figure BDA0003528396510000052
Figure BDA0003528396510000053
Representing a map coordinate system F j In the world coordinate system F 0 Position and attitude in (expressed by homogeneous transformation matrix), start j 、dest j Respectively a map building starting point and a map building end point of the two-dimensional grid map;
in the map construction process, the robot establishes a multi-layer spatial map network by using sensors such as a laser radar sensor, an Inertial Measurement Unit (IMU) sensor, a speedometer and an altimeter which are carried by the robot, and the path planning of the robot is realized through the map network;
step 12: establishing a two-dimensional raster map list and a path node list, wherein the two-dimensional raster map list and the path node list are empty lists in an initial state;
establishing a map coordinate system F by taking the starting point of the first two-dimensional grid map as the origin 0 (the first map coordinate system is simultaneously used as a world coordinate system), the map coordinate system is simultaneously used as the world coordinate system, and the current position point in the map coordinate system is used as the current position point in the map coordinate system 0 (origin of coordinates) is added to the path node list and the node (current position point) is recorded 0 ) Is represented by a homogeneous transformation matrix as
Figure BDA0003528396510000054
Wherein,
Figure BDA0003528396510000055
representing the current position point in the map coordinate system 0 The position posture of (2); i is 3 And I 4 Respectively representing 3-order and 4-order identity matrixes; o is 3×1 ＝(0 0 0) T ，O 1×3 ＝(0 0 0)；
Step 13: the operator remotely controls the robot to move to the node of the existing path of the map network, and the current position is taken as the starting point of the two-dimensional grid map building j Establishing a map coordinate system F j Collecting data of a laser radar, an IMU and a milemeter, and constructing a two-dimensional grid map (the construction method is SLAM);
the remote control robot moves to a proper position in the environment, two-dimensional grid map construction (the construction method is SLAM) is carried out, so that the corresponding areas of the currently constructed two-dimensional grid map do not overlap in a two-dimensional space, and the current position of the robot is taken as the end point of the two-dimensional grid map construction, namely a path node in a map network and is marked as dest j Completing the current map coordinate system F j Constructing a static map layer part of a two-dimensional grid map of the lower map;
step 14: recording two-dimensional grid map construction end dest j Position posture in a current map coordinate system, wherein an x coordinate and a y coordinate of the map coordinate system are obtained by a two-dimensional grid map construction process, a z coordinate is measured by an altimeter, a posture angle is measured by an IMU (inertial measurement Unit), and a two-dimensional grid map construction end point dest is obtained j The position posture in the current map coordinate system is converted into a homogeneous transformation matrix:
Figure BDA0003528396510000061
wherein,
Figure BDA0003528396510000062
representing the end of a two-dimensional grid map construction dest j Attitude rotation matrix within the current map coordinate system, (x) j ,y j ,z j ) Representing the end of a two-dimensional grid map construction dest j In the position coordinate vector in the current map coordinate system, T represents the transposition;
map building end dest of two-dimensional grid map j The position posture in the current map coordinate system is converted into a position posture in the world coordinate system:
Figure BDA0003528396510000063
wherein,
Figure BDA0003528396510000064
representing the position and the posture of the starting point of the two-dimensional grid map in a world coordinate system F 0 The inner conversion is a homogeneous conversion matrix;
Figure BDA0003528396510000065
representing the position and the attitude of the two-dimensional grid map end point in a world coordinate system F 0 The inner conversion is a homogeneous conversion matrix;
step 15: searching a current two-dimensional grid mapping end point in an existing path node list, and judging whether the current two-dimensional grid mapping end point exists in the path node list according to the position posture in the world coordinate system: if the destination does not exist, adding the destination into a path node list, and adding the map into a two-dimensional grid map list; if the two-dimensional grid map exists, adding the map into a two-dimensional grid map list, and marking a map building end point of the two-dimensional grid map as an existing end point node in a path node list;
step 16: if the two-dimensional grid map network already comprises all passable areas of the multilayer space, judging that the construction of the static map layer of the multilayer space map network is finished, and executing the step 17; if the construction of the multilayer space map is not finished, returning to the step 13 at the current position, and starting the construction of a new two-dimensional grid map;
and step 17: expanding the static map layer to obtain cost map layer costmap j ；
Step 18: terrain map layer terrainmap constructed based on static map layer j 。
Other steps and parameters are the same as those in the first embodiment.
The third concrete implementation mode: in this embodiment, the difference from the first or second embodiment is that the step 17 is performed staticallyExpanding the graph to obtain cost map layer costmap j (in costmap) j (c x1 ,c y1 ) Represents costmap j C of the matrix x1 Line c y1 Column element); the specific process is as follows:
expanding the barriers and the map boundary contained in the static map layer of the map network, wherein the expansion radius is r in a map coordinate system inflate If the resolution of the map grid is f, the number of expansion grids in the two-dimensional grid map is:
Figure BDA0003528396510000071
in the static map layer, the corresponding value of the grid occupied by the barrier is 1, the corresponding value of the grid in the passable area is 0, and the unknown area is-1;
for static map layer obstacle occupying grids, taking the neighborhood taking the number of expansion grids as the radius:
Figure BDA0003528396510000072
wherein (c) x ,c y ) Representing coordinates of grid points in the neighborhood center, (c) x1 ,c y1 ) Expressing grid point coordinates in the neighborhood;
setting the corresponding value of the grid of the cost map in the neighborhood to be 1, and indicating that grid points in the area are not accessible:
costmap j (c x1 ,c y1 )＝1if(c x1 ,c y1 )∈U(c x ,c y ),staticmap j (c x ,c y )＝1。
other steps and parameters are the same as those in the first or second embodiment.
The fourth concrete implementation mode: the difference between this embodiment and the first to the third embodiment is that, in the step 18, the topographic map layer terrainmap is constructed based on the static map layer j (with terrainmap) j (c x1 ,c y1 ) Express terrainmap j C of the matrix x1 Line c y1 Column elements); the specific process is as follows:
in a multi-layer space, special terrain areas such as a single stair and a slope are generally polygonal, and the height gradient directions in the polygonal areas are generally the same. In the process of generating the topographic map layer, an operator selects a polygonal area on the two-dimensional grid map as a slope and stair area, and the polygonal area is marked as a Polygon area terrain And setting the gradient direction angle theta of the slope and the stair area grad ；
In the topographic map layer, a program selection operator selects grid points in a polygonal area, and assigns grid corresponding numerical values as gradient direction angles:
terrainmap j (c x1 ,c y1 )＝θ grad if(c x1 ,c y1 )∈Polygon terrain 。
other steps and parameters are the same as those in one of the first to third embodiments.
The fifth concrete implementation mode: the difference between this embodiment and one of the first to the fourth embodiments is that, in step 2, a path in a map network is searched based on the map network of a multi-layer space; the specific process is as follows:
step 21: determining a temporary map network;
step 22: initializing a search of a temporary map network A; the specific process is as follows:
establishing an open node list and a closed node list, wherein the initial open node list and the closed node list are empty lists;
calculating the starting point path node of the robot path planning start An overall estimation cost;
setting a path node of a starting point of robot path planning as a root node, namely, a father node is empty, and writing the node into an open node list;
the computing robot path planning starting point path node start The specific process of the overall estimation cost is as follows:
path node dissipation function value g (point) of robot path planning starting point start ) Is 0, elicitation letterValue h (point) start ) Planning Euclidean distance from the starting point to the target point for the path, thereby obtaining the path node point of the starting point of the path planning start Overall estimation cost f (point) start )＝g(point start )+h(point start )；
Step 23: expanding the nodes; the specific process is as follows:
selecting the node with the minimum total estimation cost from the open node list as the current optimal node, and recording the node as the point * Finding points in a map network * Neighboring nodes, note
Figure BDA0003528396510000081
Wherein n is a group with point * The number of adjacent nodes;
in a two-dimensional grid map (the two-dimensional grid map is connected with the current optimal node point) * Two-dimensional grid map to neighboring nodes) from the current optimal node point * To adjacent nodes
Figure BDA0003528396510000082
Path of (1), path is noted
Figure BDA0003528396510000083
The path length is noted
Figure BDA0003528396510000084
The height difference between the two nodes is recorded as
Figure BDA0003528396510000085
Estimating neighboring nodes
Figure BDA0003528396510000086
Total estimated cost, wherein neighboring nodes
Figure BDA0003528396510000087
The dissipation function value is the dissipation function value of the current optimal node and the current adjacent node (the current adjacent node) from the current optimal node
Figure BDA0003528396510000088
) Cost value of, neighboring node
Figure BDA0003528396510000089
Heuristic function values as neighboring nodes
Figure BDA00035283965100000810
Euclidean distance to the target point, expressed as:
Figure BDA00035283965100000811
Figure BDA00035283965100000812
Figure BDA00035283965100000813
wherein,
Figure BDA00035283965100000814
for adjacent nodes
Figure BDA00035283965100000815
Dissipation function value, g (point) * ) Dissipation function value, K, for the current optimal node h For a set height cost factor, K h >0；，
Figure BDA0003528396510000091
For adjacent nodes
Figure BDA0003528396510000092
The function value is inspired, and the function value,
Figure BDA0003528396510000093
is composed of
Figure BDA0003528396510000094
Position coordinates in the world coordinate system (comprising three coordinate components of x, y and z)
Figure BDA0003528396510000095
Figure BDA0003528396510000096
Planning the position coordinates (comprising three coordinate components of x, y and z) of the destination in the world coordinate system for the path
Figure BDA0003528396510000097
Figure BDA0003528396510000098
For adjacent nodes
Figure BDA0003528396510000099
An overall estimation cost;
step 24: updating the node list; the specific process is as follows:
step 241: judging neighboring nodes
Figure BDA00035283965100000910
Whether the position is in the open node list or not;
1) if the adjacent node already exists in the open node list
Figure BDA00035283965100000911
Same-position node point old If yes, comparing the existing node points in the open node list old With adjacent node
Figure BDA00035283965100000912
Overall estimation cost of (2): if the adjacent node
Figure BDA00035283965100000913
The total estimated cost does not exceed (is less than or equal to) point old If the total estimated cost is less than the total estimated cost, the existing node point in the open node list is determined old Deleting, and connecting adjacent nodes
Figure BDA00035283965100000914
Adding the data into an open node list; if the adjacent node
Figure BDA00035283965100000915
The total estimated cost is greater than point old The total estimated cost of (2) is then retained old ；
2) If the adjacent node does not exist in the open node list
Figure BDA00035283965100000916
If the nodes with the same position are located, the adjacent nodes are connected
Figure BDA00035283965100000917
Adding the data into an open node list;
step 242: the current optimal node point * And deleting the data from the open node list and adding the data to the closed node list.
Step 25: target (robot path planning end point of path node) dest ) Judging the arrival state; the specific process is as follows:
if the current optimal node (point in step 23) * ) Having reached the target node (the current optimal node is the target node, then the process goes to step 26; if the target node is not reached, returning to the step 23 to continue searching;
step 26: generating a path; the specific process is as follows:
after the path search is finished, a path searching process is traced back, a path point list is created, and a target node (robot path planning end point) is searched dest ) Starting, recording the position of the current node and writing the position into a path point list, searching a father node of the current node, and executing the loop iteration until the path node point of the robot path planning starting point is searched start And arranging the route point lists in a reverse order to obtain the planned route in the map network.
Other steps and parameters are the same as in one of the first to fourth embodiments.
The sixth specific implementation mode is as follows: the difference between this embodiment and one of the first to fifth embodiments is that, in step 21, a temporary map network is determined; the specific process is as follows:
expanding the existing map network to generate a temporary map network;
determining the current position and the target position of the robot and a two-dimensional grid map where the two points are located, and setting the two points of the current position and the target position as the starting point path node point of the path planning of the robot start And robot path planning end point dest Adding the route node list into a route node list of a map network as a temporary route node list;
establishing a Path node Point start Map of two-dimensional grid map to the area start Two-dimensional grid map of map construction starting point and map construction end point start1 、map start2 ；
Establishing a Path node Point dest Map of two-dimensional grid map to the area dest Two-dimensional grid map of map construction starting point and map construction end point dest1 、map dest2 ；
Map two-dimensional grid map start 、map dest 、map start1 、map start2 、map dest1 、map dest2 Adding the grid map list into a two-dimensional grid map list of a map network to serve as a temporary two-dimensional grid map list; as shown in FIG. 3;
the temporary map network consists of a temporary path node list and a temporary two-dimensional raster map list and is set to be a multidirectional network (every two adjacent path nodes can pass in two directions);
Points temp ＝{Points,point start ,point dest }
map start1 ＝map start ,map start2 ＝map start
map dest1 ＝map dest ,map dest2 ＝map dest
Maps temp ＝{Maps,map start1 ,map start2 ,map dest1 ,map dest2 }
GraphNet temp ＝(Points temp ,Maps temp )
in the formula, Points temp For temporary path node list, Points for map network path node list, Maps for two-dimensional raster map list of map network, Maps for map network temp For temporary two-dimensional grid map lists, GraphNet temp Is a temporary map network.
Other steps and parameters are the same as those in one of the first to fifth embodiments.
The seventh concrete implementation mode: the difference between this embodiment and one of the first to sixth embodiments is that, in the step 23, a two-dimensional grid map (the two-dimensional grid map is used to connect the current optimal node points) * Two-dimensional grid map to neighboring nodes) from the current optimal node point * To adjacent nodes
Figure BDA0003528396510000111
A path of (a); the specific process is as follows:
step 231: transforming the path node position postures of the starting point and the end point of the two-dimensional grid map from the world coordinate system to the current map coordinate system F j The preparation method comprises the following steps of (1) performing; the specific process is as follows:
setting the starting point and the end point of the path planning in the two-dimensional grid map as point respectively i1 And point i2 The position and posture homogeneous transformation matrix in the world coordinate system is
Figure BDA0003528396510000112
And
Figure BDA0003528396510000113
setting a position posture homogeneous transformation matrix of a map coordinate system in a world coordinate system in a two-dimensional grid map as
Figure BDA0003528396510000114
Then the position and posture homogeneous transformation matrix of the path planning starting point and the ending point in the current map coordinate system is:
Figure BDA0003528396510000115
Figure BDA0003528396510000116
extracting the current map coordinate system F of the starting point and the end point j The x coordinate and the y coordinate part in the position and posture homogeneous transformation matrix are marked as x sj ，y sj ，x dj ，y dj ：
Figure BDA0003528396510000117
In the formula m ij The ith row and the jth column of elements of the homogeneous transformation matrix M;
and mapping the position coordinates into a grid coordinate system to obtain grid coordinates, wherein the grid point coordinates of the starting point and the end point are respectively as follows:
Figure BDA0003528396510000118
Figure BDA0003528396510000119
wherein the cell start As the starting point grid node coordinates, (c) xs ,c ys ) Is the x coordinate and the y coordinate of the grid node of the starting point, (o) xj ,o yj ) The coordinate of the origin of the two-dimensional grid map matrix in a map coordinate system is shown, f is the resolution of the two-dimensional grid map, cell dest As the coordinates of the end grid node, (c) xd ,c yd ) The x coordinate and the y coordinate of the end point grid node are obtained;
step 232: path search is carried out in a two-dimensional grid map coordinate system by adopting an A-method and taking grids as basic units, and the search is initialized: the specific process is as follows:
establishing an open grid node list and a closed grid node list, wherein the initial open grid node list and the closed grid node list are empty lists;
calculating robot path planning starting point grid node cell start An overall estimation cost;
setting a grid node of a robot path planning starting point as a root node, namely, writing a father node into an open grid node list, wherein the father node is empty;
the grid node cell of the starting point of the path planning of the computing robot start The specific process of the overall estimation cost is as follows:
the dissipation function value of the grid node of the robot path planning starting point is 0, the heuristic function value is the Euclidean distance from the grid node of the path planning starting point to the grid node of the terminal point, and the grid node dissipation function value and the heuristic function value are added to obtain the total estimated cost of the grid node of the robot path planning starting point;
step 233: expanding grid nodes, namely expanding the current optimal grid node cell in the open grid node list * Obtaining neighboring nodes
Figure BDA0003528396510000121
Step 234: updating a grid node list; the specific process is as follows:
judging whether the adjacent nodes exist in the open grid node list or not
Figure BDA0003528396510000122
Grid node cell with same position old ；
If the adjacent node does not exist in the open grid node list
Figure BDA0003528396510000123
If the grid nodes are in the same position, the adjacent grid nodes are connected
Figure BDA0003528396510000124
Adding the list into an open grid node list;
if there is already a grid node adjacent to the open grid node list
Figure BDA0003528396510000125
Grid node cell with same position old Then, the existing grid node cell is compared old With adjacent grid nodes
Figure BDA0003528396510000126
If neighboring grid nodes
Figure BDA0003528396510000127
The total estimation cost is less than or equal to the existing grid node cell old Estimating the cost totally, then the existing grid node cell in the open grid node list is used old Deleting, connecting adjacent grid nodes
Figure BDA0003528396510000128
Adding the list into an open grid node list; if adjacent grid nodes
Figure BDA0003528396510000129
The total estimation cost is larger than the existing grid node cell old The total estimated cost is reserved for the existing grid node cell old ；
Step 235: judging the arrival state of the end point grid node;
if the current optimal grid node cell * Having reached the destination grid node (euclidean distance to the target point less than one grid length), then proceed to step 236; if the destination grid node is not reached, returning to the step 233 to continue searching;
step 236: generating a path; the specific process is as follows:
and after the path search is finished, a path backtracking search process is carried out, a path grid point list is created, a current grid node is recorded from a destination grid node, a father node of the current grid node is searched, the loop iteration execution is carried out until a starting point grid node is searched, the path grid point list is arranged in a reverse order to obtain a path grid node list, and grid coordinates in the path grid node list are converted into map coordinate system coordinates to obtain a motion path in a two-dimensional grid map.
Other steps and parameters are the same as those in one of the first to sixth embodiments.
The specific implementation mode is eight: the difference between this embodiment and one of the first to seventh embodiments is that the expansion of the grid node in step 233 is performed by using the current optimal grid node cell in the open grid node list * Obtaining neighboring nodes
Figure BDA0003528396510000131
The specific process is as follows:
in the two-dimensional grid map, grid points of which the coordinate values are not integers are called generalized grid nodes;
selecting the grid node with the minimum total estimation cost from the open grid node list as the current optimal grid node, and recording the grid node as the current optimal grid node
Figure BDA0003528396510000132
Judging the state of the grid state in each map layer:
Figure BDA0003528396510000133
the coordinate point of the current optimal grid node is obtained;
1) if the current optimal grid node is not in the slope and stair area marked in the topographic map layer, 8 grid points adjacent to the current optimal grid node are selected (if the grid node is a generalized grid node, the adjacent grid node is selected by rounding the coordinate), and the grid points are marked as
Figure BDA0003528396510000134
The grid coordinates are noted as:
Figure BDA0003528396510000135
wherein,
Figure BDA0003528396510000136
to be adjacent to the current optimum grid nodeCoordinates of grid points (Δ x) k ,Δy k ) Moving distance for the current optimal grid node coordinate;
judging adjacent grid points
Figure BDA0003528396510000137
State if adjacent grid points
Figure BDA0003528396510000138
The corresponding value in the cost map layer is 1, which represents the adjacent grid point
Figure BDA0003528396510000139
Occupied by obstacles, and connecting the adjacent grid points
Figure BDA00035283965100001310
Judging as an invalid grid point and deleting; if adjacent grid points
Figure BDA00035283965100001311
If the corresponding value of the cost map layer is 0, estimating the adjacent grid points
Figure BDA00035283965100001312
The overall estimated cost value of;
the adjacent grid points
Figure BDA00035283965100001313
The overall estimated cost value solving process is as follows:
adjacent grid points
Figure BDA00035283965100001314
The dissipation function value is the dissipation function value of the father node and the dissipation function value from the father node to the current adjacent grid point
Figure BDA00035283965100001315
Cost value of, adjacent grid points
Figure BDA0003528396510000141
Heuristic function value of current adjacencyGrid point
Figure BDA0003528396510000142
The euclidean distance to the end grid node, expressed as:
Figure BDA0003528396510000143
Figure BDA0003528396510000144
Figure BDA0003528396510000145
wherein, g (cell) * ) For the current optimal grid node dissipation function value,
Figure BDA0003528396510000146
for adjacent grid points
Figure BDA0003528396510000147
The value of the dissipation function is,
Figure BDA0003528396510000148
for adjacent grid points
Figure BDA0003528396510000149
The function value is inspired, and the function value,
Figure BDA00035283965100001410
for adjacent grid points
Figure BDA00035283965100001411
The overall estimated cost value of;
2) if the current optimal grid node is located on the slope and stair area marked in the topographic map layer, the current optimal grid node is expanded along the gradient direction and the opposite direction of the gradient direction of the slope and stair area marked, and the adjacent grid nodes of the current optimal grid node are marked as:
Figure BDA00035283965100001412
Figure BDA00035283965100001413
wherein,
Figure BDA00035283965100001414
f is the coordinate of the adjacent grid node of the current optimal grid node, and the resolution of the two-dimensional grid map;
and judging the states of the adjacent grid nodes, wherein the gradient direction of the slope stair area is possibly not parallel to the coordinate axis of the x axis or the coordinate axis of the y axis, and the x coordinate and the y coordinate of the adjacent grid nodes are possibly not integers, so that corresponding points cannot be searched in the grid map. The coordinates of adjacent grid nodes are rounded and then corresponding values are inquired in a grid map;
judging the state of the adjacent grid nodes, if the corresponding value of the adjacent grid nodes in the cost map layer is 1, indicating that the adjacent grid nodes are occupied by the obstacles, and judging the adjacent grid nodes as invalid grid points and deleting the invalid grid points; if the corresponding value of the adjacent grid node in the cost map layer is 0, estimating the total estimated cost value of the adjacent grid node;
the solving process for estimating the total estimated cost value of the adjacent grid nodes comprises the following steps:
the adjacent grid node dissipation function value is the current optimal grid node dissipation function value and the cost value from the current optimal grid node to the current adjacent grid node, and the adjacent grid node heuristic function value is the Euclidean distance from the current adjacent grid node to the terminal grid node, and is expressed as:
Figure BDA0003528396510000151
Figure BDA0003528396510000152
Figure BDA0003528396510000153
wherein,
Figure BDA0003528396510000154
are the nodes of the adjacent grid, and are,
Figure BDA0003528396510000155
is composed of
Figure BDA0003528396510000156
And
Figure BDA0003528396510000157
Figure BDA0003528396510000158
the dissipation function values are for the adjacent grid nodes,
Figure BDA0003528396510000159
are the coordinates of the nodes of the adjacent grid,
Figure BDA00035283965100001510
is composed of
Figure BDA00035283965100001511
And
Figure BDA00035283965100001512
Figure BDA00035283965100001513
a function value is inspired for the adjacent grid nodes,
Figure BDA00035283965100001514
the cost value is estimated for the total of the neighboring grid nodes.
Other steps and parameters are the same as those in one of the first to seventh embodiments.
The specific implementation method nine: the difference between this embodiment and the first to eighth embodiments is that, in the step 236, the grid coordinates in the path grid node list are transformed into the coordinates of the map coordinate system, so as to obtain the motion path in the two-dimensional grid map; the specific process is as follows:
the length of N obtained after A is searched j The list of path raster nodes is:
cellpath(point i1 ,point i2 )＝{(c xk ,c yk )|k＝1,2,…,N j }
wherein, c xk And c yk Is a path point grid coordinate;
transforming grid coordinates into map coordinate system coordinates
(x k ,y k )＝(c xk f+o xj ,c yk f+o yj )
Wherein (x) k ,y k ) Is a path point coordinate;
the motion path in the two-dimensional grid map can be obtained:
path(point i1 ,point i2 )＝{(x k ,y k )|k＝1,2,…,N j }。
other steps and parameters are the same as those in one to eight of the embodiments.
The specific implementation mode is ten: the difference between this embodiment and one of the first to ninth embodiments is that the planned route in the map network in the step 26 is defined by N point A path node and N point -1 two-dimensional grid map planned path component, denoted as:
Figure BDA00035283965100001515
Figure BDA0003528396510000161
wherein,
Figure BDA0003528396510000162
is as follows
Figure BDA0003528396510000163
Is first and second
Figure BDA0003528396510000164
Individual path nodes belonging to temporary path node set Points in temporary map network temp ，
Figure BDA0003528396510000165
To be within a two-dimensional grid map
Figure BDA0003528396510000166
To
Figure BDA0003528396510000167
(ii) a planned path of (x) s ,y s ) For the grid coordinates of the planned path points within the two-dimensional grid map,
Figure BDA0003528396510000168
the number of grid points on the planned path.
Other steps and parameters are the same as those in one of the first to ninth embodiments.
The present invention is capable of other embodiments and its several details are capable of modifications in various obvious respects, all without departing from the spirit and scope of the present invention.

Claims (9)

1. A wheel type robot path planning method applied to a cross-heterogeneous multi-layer space is characterized by comprising the following steps: the method comprises the following specific processes:
step 1, constructing a map network of a multilayer space;
step 2, searching a path in a map network based on the map network of the multilayer space;
constructing a map network of a multilayer space in the step 1; the specific process is as follows:
step 11, dividing the multilayer space at the joint of the stairs and the plane and the position of a stair turntable into a plurality of areas, wherein the projections of the areas on the two-dimensional plane do not overlap;
the multilayer space map network is formed by connecting path nodes in space with a two-dimensional grid map of each area;
in the multilayer space map network, path nodes in a space are used as nodes of the network, and a two-dimensional grid map is used as an edge of the network;
the path node set of the multilayer space map network is marked as Points ═ point i |i＝0,1,2,…,N-1}；
Wherein N is the number of path nodes in the map network; point i Is the ith path node;
the two-dimensional grid map set of the multi-layer space map network is marked as Maps ═ map j |j＝0,1,2,…,M-1}；
Wherein M is the number of two-dimensional grid maps in the map network; map j Is the jth two-dimensional grid map;
the multi-layer space map network information consists of a path node set and a two-dimensional raster map set, and is marked as GraphNet (Points, Maps);
the multi-layer space map network is divided into static map layers j Cost map layer costmap j And the terrain map layer terrainmap j ；
Step 12: establishing a two-dimensional raster map list and a path node list, wherein the two-dimensional raster map list and the path node list are empty lists in an initial state;
establishing a map coordinate system F by taking the starting point of the first two-dimensional grid map as the origin 0 The map coordinate system is simultaneously used as a world coordinate system, and the current position point in the map coordinate system is used as the current position point 0 Adding the position information into a path node list, and recording the position posture of the node, wherein the position posture is represented by a homogeneous transformation matrix
Figure FDA0003793098820000011
Wherein,
Figure FDA0003793098820000012
representing the current position point in the map coordinate system 0 The position posture of (2); i is 3 And I 4 Respectively representing 3-order and 4-order identity matrixes; o is 3×1 ＝(0 0 0) T ，O 1×3 ＝(0 0 0)；
Step 13: the remote-controlled robot moves to the node of the existing path of the map network, and the current position is taken as the starting point of the two-dimensional grid map building j Establishing a map coordinate system F j Collecting data of a laser radar, an IMU and a milemeter, and constructing a two-dimensional grid map;
the remote control robot moves to construct a two-dimensional grid map, so that corresponding areas of the currently constructed two-dimensional grid map do not overlap in a two-dimensional space, and the current position of the robot is taken as a two-dimensional grid map construction end point, namely a path node in a map network and recorded as dest j Completing the current map coordinate system F j Constructing a static map layer part of a two-dimensional grid map of the lower map;
step 14: recording a two-dimensional grid map construction end point dest j Position posture in a current map coordinate system, wherein an x coordinate and a y coordinate of the map coordinate system are obtained by a two-dimensional grid map construction process, a z coordinate is measured by an altimeter, a posture angle is measured by an IMU (inertial measurement Unit), and a two-dimensional grid map construction end point dest is obtained j The position posture in the current map coordinate system is converted into a homogeneous transformation matrix:
Figure FDA0003793098820000021
wherein,
Figure FDA0003793098820000022
representing the end of a two-dimensional grid map construction dest j Attitude rotation matrix within the current map coordinate system, (x) j ,y j ,z j ) Representing two-dimensional grid map constructionGraph end dest j In the position coordinate vector in the current map coordinate system, T represents the transposition;
map building end dest of two-dimensional grid map j The position posture in the current map coordinate system is converted into a position posture in the world coordinate system:
Figure FDA0003793098820000023
wherein,
Figure FDA0003793098820000024
representing the position and the attitude of the starting point of the two-dimensional grid map in a world coordinate system F 0 The inner conversion is a homogeneous conversion matrix;
Figure FDA0003793098820000025
representing the position and the attitude of the two-dimensional grid map end point in a world coordinate system F 0 The inner conversion is a homogeneous conversion matrix;
step 15: searching a current two-dimensional grid mapping end point in an existing path node list, and judging whether the current two-dimensional grid mapping end point exists in the path node list according to the position posture in the world coordinate system: if the destination does not exist, adding the destination into a path node list, and adding the map into a two-dimensional grid map list; if the two-dimensional grid map exists, adding the map into a two-dimensional grid map list, and marking a map building end point of the two-dimensional grid map as an existing end point node in a path node list;
step 16: if the two-dimensional grid map network already comprises all passable areas of the multilayer space, judging that the construction of the static map layer of the multilayer space map network is finished, and executing the step 17; if the construction of the multilayer space map is not finished, returning to the step 13 at the current position, and starting the construction of a new two-dimensional grid map;
and step 17: performing expansion processing on the static map layer to obtain costmap of the cost map layer j ；
Step 18: terrain map layer terrainmap constructed based on static map layer j 。
2. The wheeled robot path planning method applied to the cross-heterogeneous multi-layer space according to claim 1, wherein the method comprises the following steps: and in the step 17, the static map is expanded to obtain a costmap of the cost map layer j (ii) a The specific process is as follows:
expanding the barriers and the map boundary contained in the static map layer of the map network, wherein the expansion radius is r in a map coordinate system inflate If the resolution of the map grid is f, the number of expansion grids in the two-dimensional grid map is:
Figure FDA0003793098820000031
in the static map layer, the corresponding value of the grid occupied by the barrier is 1, the corresponding value of the grid in the passable area is 0, and the unknown area is-1;
for static map layer obstacle occupying grids, taking the neighborhood taking the number of expansion grids as the radius:
Figure FDA0003793098820000032
wherein (c) x ,c y ) Representing coordinates of grid points in the center of the neighborhood, (c) x1 ,c y1 ) Representing grid point coordinates in the neighborhood;
setting the corresponding value of the grid of the cost map in the neighborhood to be 1, and indicating that grid points in the area are not accessible:
costmap j (c x1 ,c y1 )＝1 if(c x1 ,c y1 )∈U(c x ,c y ),staticmap j (c x ,c y )＝1。
3. the path planning method for wheeled robot spanning heterogeneous multi-layer space according to claim 2The method is characterized in that: in the step 18, the terra map layer terrainmap is constructed based on the static map layer j (ii) a The specific process is as follows:
selecting a Polygon area on a two-dimensional grid map as a slope and stair area, and marking as Polygon area terrain And setting the gradient direction angle theta of the slope and the stair area grad ；
In the topographic map layer, selecting grid points in a polygonal area, and assigning grid corresponding numerical values as gradient direction angles:
terrainmap j (c x1 ,c y1 )＝θ grad if(c x1 ,c y1 )∈Polygon terrain 。
4. the wheeled robot path planning method applied to the cross-heterogeneous multi-layer space according to claim 3, wherein the method comprises the following steps: searching a path in the map network based on the map network of the multilayer space in the step 2; the specific process is as follows:
step 21: determining a temporary map network;
step 22: initializing searching; the specific process is as follows:
establishing an open node list and a closed node list, wherein the initial open node list and the closed node list are empty lists;
calculating the starting point path node of the robot path planning start An overall estimation cost;
setting a path node of a starting point of robot path planning as a root node, namely, a father node is empty, and writing the node into an open node list;
the computing robot path planning starting point path node start The specific process of the overall estimation cost is as follows:
path node dissipation function value g (point) of robot path planning starting point start ) Is 0, the value of the heuristic function h (point) start ) Planning Euclidean distance from the starting point to the target point for the path, thereby obtaining the path node point of the starting point of the path planning start Overall estimation cost f (point) start )＝g(point start )+h(point start )；
Step 23: expanding the nodes; the specific process is as follows:
selecting the node with the minimum total estimation cost from the open node list as the current optimal node, and recording the node as the point * Finding points in a map network * Neighboring nodes, note
Figure FDA0003793098820000041
Wherein n is a group with point * The number of adjacent nodes;
searching from the current optimal node point * To adjacent nodes
Figure FDA0003793098820000042
Path of (1), path is noted
Figure FDA0003793098820000043
The path length is noted
Figure FDA0003793098820000044
The height difference between the two nodes is recorded as
Figure FDA0003793098820000045
Estimating neighboring nodes
Figure FDA0003793098820000046
The overall estimation cost, expressed as:
Figure FDA0003793098820000047
Figure FDA0003793098820000048
Figure FDA0003793098820000051
wherein,
Figure FDA0003793098820000052
for adjacent nodes
Figure FDA0003793098820000053
Dissipation function value, g (point) * ) Dissipation function value, K, for the current optimal node h For a set height cost factor, K h >0；，
Figure FDA0003793098820000054
For adjacent nodes
Figure FDA0003793098820000055
The function value is inspired, and the function value,
Figure FDA0003793098820000056
is composed of
Figure FDA0003793098820000057
The coordinates of the location within the world coordinate system,
Figure FDA0003793098820000058
the position coordinates of the path planning end point in the world coordinate system,
Figure FDA0003793098820000059
for adjacent nodes
Figure FDA00037930988200000510
An overall estimation cost;
step 24: updating the node list; the specific process is as follows:
step 241: judging neighboring nodes
Figure FDA00037930988200000511
Is located at the positionWhether the current node is already in the open node list;
1) if the adjacent node already exists in the open node list
Figure FDA00037930988200000512
Same-position node point old If yes, comparing the existing node points in the open node list old With adjacent node
Figure FDA00037930988200000513
Overall estimation cost of (c): if the adjacent node
Figure FDA00037930988200000514
The total estimation cost does not exceed point old The total estimated cost is that the existing node point in the open node list is used old Deleting, and connecting adjacent nodes
Figure FDA00037930988200000515
Adding the data into an open node list; if the adjacent node
Figure FDA00037930988200000516
The total estimated cost is greater than point old The total estimated cost of (2) is then reserved for the existing node point old ；
2) If the adjacent node does not exist in the open node list
Figure FDA00037930988200000517
If the nodes are in the same position, the adjacent nodes are connected
Figure FDA00037930988200000518
Adding the data into an open node list;
step 242: the current optimal node point * Deleting the data from the open node list and adding the data to the closed node list;
step 25: judging a target arrival state; the specific process is as follows:
if the current optimal node has reached the target node, go to step 26; if the target node is not reached, returning to the step 23;
step 26: generating a path; the specific process is as follows:
creating a path point list, starting from a target node, recording the position of the current node and writing the current node into the path point list, searching a father node of the current node, and executing loop iteration until the path node point at the starting point of the robot path planning is searched start And arranging the route point lists in a reverse order to obtain the planned route in the map network.
5. The wheeled robot path planning method applied to the cross-heterogeneous multi-layer space according to claim 4, wherein the method comprises the following steps: determining a temporary map network in the step 21; the specific process is as follows:
determining the current position and the target position of the robot and a two-dimensional grid map where the two points are located, and setting the two points of the current position and the target position as the starting point path node point of the path planning of the robot start And robot path planning end point dest Adding the route node list into a route node list of a map network as a temporary route node list;
establishing a Path node Point start Map of two-dimensional grid map to the area start Two-dimensional grid map of map construction starting point and map construction end point start1 、map start2 ；
Establishing a Path node Point dest Map of two-dimensional grid map to the area dest Two-dimensional grid map of map construction starting point and map construction end point dest1 、map dest2 ；
Map two-dimensional grid map start 、map dest 、map start1 、map start2 、map dest1 、map dest2 Adding the grid map list into a two-dimensional grid map list of a map network to serve as a temporary two-dimensional grid map list;
the temporary map network consists of a temporary path node list and a temporary two-dimensional raster map list and is set to be a undirected network.
6. The wheeled robot path planning method applied to the cross-heterogeneous multi-layer space according to claim 5, wherein the method comprises the following steps: in the step 23, the current optimal node point is searched * To adjacent nodes
Figure FDA0003793098820000061
A path of (a); the specific process is as follows:
step 231: transforming the path node position postures of the starting point and the end point of the two-dimensional grid map from the world coordinate system to the current map coordinate system F j Performing the following steps; the specific process is as follows:
setting the starting point and the end point of the path planning in the two-dimensional grid map as point respectively i1 And point i2 The position and attitude homogeneous transformation matrix in the world coordinate system is respectively
Figure FDA0003793098820000062
And
Figure FDA0003793098820000063
setting a position posture homogeneous transformation matrix of a map coordinate system in a world coordinate system in a two-dimensional grid map as
Figure FDA0003793098820000064
Then the position posture homogeneous transformation matrix of the path planning starting point and the terminal point in the current map coordinate system is as follows:
Figure FDA0003793098820000065
Figure FDA0003793098820000066
extracting the start point and the end point in the current map coordinate system F j The x coordinate and the y coordinate part in the position and posture homogeneous transformation matrix are marked as x sj ，y sj ，x dj ，y dj ：
Figure FDA0003793098820000067
In the formula m ij The ith row and the jth column of elements of the homogeneous transformation matrix M;
and mapping the position coordinates into a grid coordinate system to obtain grid coordinates, wherein the grid point coordinates of the starting point and the end point are respectively as follows:
Figure FDA0003793098820000071
Figure FDA0003793098820000072
wherein the cell start As the starting point grid node coordinates, (c) xs ,c ys ) As the starting grid node x and y coordinates, (o) xj ,o yj ) The coordinate of the origin of the two-dimensional grid map matrix in a map coordinate system is shown, f is the resolution of the two-dimensional grid map, cell dest As the endpoint grid node coordinates, (c) xd ,c yd ) The x coordinate and the y coordinate of the end point grid node are obtained;
step 232: search initialization: the specific process is as follows:
establishing an open grid node list and a closed grid node list, wherein the initial open grid node list and the closed grid node list are empty lists;
calculating a robot path planning starting point grid node cell start An overall estimation cost;
setting a grid node of a robot path planning starting point as a root node, namely, writing a father node into an open grid node list, wherein the father node is empty;
the grid node cell of the starting point of the path planning of the computing robot start The specific process of the overall estimation cost is as follows:
the dissipation function value of the grid node of the robot path planning starting point is 0, the heuristic function value is the Euclidean distance from the grid node of the path planning starting point to the grid node of the terminal point, and the grid node dissipation function value and the heuristic function value are added to obtain the total estimated cost of the grid node of the robot path planning starting point;
step 233: expanding grid nodes, namely expanding the current optimal grid node cell in the open grid node list * Obtaining neighboring nodes
Figure FDA0003793098820000073
Step 234: updating a grid node list; the specific process is as follows:
judging whether the adjacent nodes exist in the open grid node list or not
Figure FDA0003793098820000074
Grid node cell with same position old ；
If the adjacent node does not exist in the open grid node list
Figure FDA0003793098820000075
If the grid nodes are in the same position, the adjacent grid nodes are connected
Figure FDA0003793098820000081
Adding the list into an open grid node list;
if there is already a grid node adjacent to the open grid node list
Figure FDA0003793098820000082
Grid node cell with same position old Then, the existing grid node cell is compared old With adjacent grid nodes
Figure FDA0003793098820000083
If neighboring grid nodes
Figure FDA0003793098820000084
The total estimation cost is less than or equal to the existing grid node cell old Estimating the cost totally, namely, using the prior grid node cell in the open grid node list old Deleting, connecting adjacent grid nodes
Figure FDA0003793098820000085
Adding the list into an open grid node list; if adjacent grid nodes
Figure FDA0003793098820000086
The total estimation cost is larger than the existing grid node cell old The total estimated cost is reserved for the existing grid node cell old ；
Step 235: judging the arrival state of the end point grid node;
if the current optimal grid node cell * If the destination grid node has been reached, proceed to step 236; if the end grid node is not reached, return to step 233;
step 236: generating a path; the specific process is as follows:
and creating a path grid point list, starting from the end point grid node, recording the current grid node, searching a father node of the current grid node, performing iteration in a circulating mode until the start point grid node is searched, arranging the path grid point list in a reverse order to obtain a path grid node list, and converting grid coordinates in the path grid node list into map coordinate system coordinates to obtain a motion path in the two-dimensional grid map.
7. The wheeled robot path planning method applied to the cross-heterogeneous multi-layer space according to claim 6, wherein the method comprises the following steps: the expansion of the grid node in step 233 is performed by the current optimal grid node cell in the open grid node list * Obtaining neighboring nodes
Figure FDA0003793098820000087
The specific process is as follows:
from the openSelecting the grid node with the minimum total estimation cost from the grid node list as the current optimal grid node, and recording as the current optimal grid node
Figure FDA0003793098820000088
Figure FDA0003793098820000089
The coordinate point of the current optimal grid node is obtained;
1) if the current optimal grid node is not on the slope and stair area marked in the topographic map layer, 8 grid points adjacent to the current optimal grid node are selected and marked as
Figure FDA00037930988200000810
The grid coordinates are noted as:
Figure FDA00037930988200000811
wherein,
Figure FDA00037930988200000812
(Δ x) coordinates of grid points adjacent to the current optimal grid node k ,Δy k ) Moving distance for the current optimal grid node coordinate;
judging adjacent grid points
Figure FDA0003793098820000091
State if adjacent grid points
Figure FDA0003793098820000092
The corresponding value in the cost map layer is 1, which represents the adjacent grid point
Figure FDA0003793098820000093
Occupied by obstacles, and connecting the adjacent grid points
Figure FDA0003793098820000094
Judging as an invalid grid point and deleting; if adjacent grid points
Figure FDA0003793098820000095
If the corresponding value of the cost map layer is 0, estimating the adjacent grid points
Figure FDA0003793098820000096
The overall estimated cost value of;
the adjacent grid points
Figure FDA0003793098820000097
The overall estimated cost value solving process is as follows:
Figure FDA0003793098820000098
Figure FDA0003793098820000099
Figure FDA00037930988200000910
wherein, g (cell) * ) For the current optimal grid node dissipation function value,
Figure FDA00037930988200000911
for adjacent grid points
Figure FDA00037930988200000912
The value of the dissipation function is,
Figure FDA00037930988200000913
for adjacent grid points
Figure FDA00037930988200000914
The function value is inspired, and the function value,
Figure FDA00037930988200000915
for adjacent grid points
Figure FDA00037930988200000916
The overall estimated cost value of;
2) if the current optimal grid node is located on the slope and stair area marked in the topographic map layer, the current optimal grid node is expanded along the gradient direction and the opposite direction of the gradient direction of the slope and stair area marked, and the adjacent grid nodes of the current optimal grid node are marked as:
Figure FDA00037930988200000917
Figure FDA00037930988200000918
wherein,
Figure FDA00037930988200000919
f is the coordinate of the adjacent grid node of the current optimal grid node, and the resolution of the two-dimensional grid map;
judging the state of the adjacent grid nodes, if the corresponding value of the adjacent grid nodes in the cost map layer is 1, indicating that the adjacent grid nodes are occupied by the obstacles, and judging the adjacent grid nodes as invalid grid points and deleting the invalid grid points; if the corresponding value of the adjacent grid node in the cost map layer is 0, estimating the total estimated cost value of the adjacent grid node;
the solving process for estimating the total estimated cost value of the adjacent grid nodes comprises the following steps:
Figure FDA00037930988200000920
Figure FDA0003793098820000101
Figure FDA0003793098820000102
wherein,
Figure FDA0003793098820000103
are the nodes of the adjacent grid, and are,
Figure FDA0003793098820000104
is composed of
Figure FDA0003793098820000105
And
Figure FDA0003793098820000106
Figure FDA0003793098820000107
the dissipation function values for the adjacent grid nodes,
Figure FDA0003793098820000108
is the coordinate of the adjacent grid node(s),
Figure FDA0003793098820000109
is composed of
Figure FDA00037930988200001010
And
Figure FDA00037930988200001011
a function value is inspired for the adjacent grid nodes,
Figure FDA00037930988200001012
the cost value is estimated for the total of the neighboring grid nodes.
8. The method for planning the path of the wheeled robot spanning the heterogeneous multi-layer space according to claim 7, wherein the method comprises the following steps: in the step 236, the grid coordinates in the path grid node list are transformed into map coordinate system coordinates, so as to obtain a motion path in the two-dimensional grid map; the specific process is as follows:
length N j The list of path raster nodes is:
cellpath(point i1 ,point i2 )＝{(c xk ,c yk )|k＝1,2,…,N j }
wherein, c xk And c yk Is a path point grid coordinate;
transforming grid coordinates into map coordinate system coordinates
(x k ,y k )＝(c xk f+o xj ,c yk f+o yj )
Wherein (x) k ,y k ) Is a path point coordinate;
the motion path in the two-dimensional grid map can be obtained:
path(point i1 ,point i2 )＝{(x k ,y k )|k＝1,2,…,N j }。
9. the method for planning the path of the wheeled robot spanning the heterogeneous multi-layer space according to claim 8, wherein the method comprises the following steps: the planned path in the map network in the step 26 is represented by N point A path node and N point -1 two-dimensional grid map planned path component, denoted as:
Figure FDA00037930988200001013
Figure FDA00037930988200001014
wherein,
Figure FDA0003793098820000111
is as follows
Figure FDA0003793098820000112
Is first and second
Figure FDA0003793098820000113
Individual path nodes belonging to temporary path node set Points in temporary map network temp ，
Figure FDA0003793098820000114
To be within a two-dimensional grid map
Figure FDA0003793098820000115
To
Figure FDA0003793098820000116
(ii) a planned path of (x) s ,y s ) For the grid coordinates of the planned path points within the two-dimensional grid map,
Figure FDA0003793098820000117
the number of grid points on the planned path.
CN202210203947.7A 2022-03-02 2022-03-02 Wheeled robot path planning method applied to cross-heterogeneous multi-layer space Active CN114577214B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210203947.7A CN114577214B (en) 2022-03-02 2022-03-02 Wheeled robot path planning method applied to cross-heterogeneous multi-layer space

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210203947.7A CN114577214B (en) 2022-03-02 2022-03-02 Wheeled robot path planning method applied to cross-heterogeneous multi-layer space

Publications (2)

Publication Number Publication Date
CN114577214A CN114577214A (en) 2022-06-03
CN114577214B true CN114577214B (en) 2022-09-20

Family

ID=81772370

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210203947.7A Active CN114577214B (en) 2022-03-02 2022-03-02 Wheeled robot path planning method applied to cross-heterogeneous multi-layer space

Country Status (1)

Country Link
CN (1) CN114577214B (en)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114777793B (en) * 2022-06-16 2022-09-20 盈嘉互联（北京）科技有限公司 BIM map extraction and path planning method for any navigation subject
CN115080676B (en) * 2022-06-16 2024-06-18 重庆邮电大学 Dynamic management method for combined map of mobile robot
CN117485929B (en) * 2023-12-29 2024-03-19 中国电力工程顾问集团西南电力设计院有限公司 Unmanned material stacking and taking control system and method based on intelligent control

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2017173990A1 (en) * 2016-04-07 2017-10-12 北京进化者机器人科技有限公司 Method for planning shortest path in robot obstacle avoidance
CN107368076A (en) * 2017-07-31 2017-11-21 中南大学 Robot motion's pathdepth learns controlling planning method under a kind of intelligent environment
CN107677279A (en) * 2017-09-26 2018-02-09 上海思岚科技有限公司 It is a kind of to position the method and system for building figure
CN112197778A (en) * 2020-09-08 2021-01-08 南京理工大学 Wheeled airport border-patrol robot path planning method based on improved A-x algorithm
CN113033907A (en) * 2021-04-07 2021-06-25 上海钛米机器人股份有限公司 Path planning method and device, electronic equipment and storage medium
CN113110457A (en) * 2021-04-19 2021-07-13 杭州视熵科技有限公司 Autonomous coverage inspection method for intelligent robot in indoor complex dynamic environment
CN113269837A (en) * 2021-04-27 2021-08-17 西安交通大学 Positioning navigation method suitable for complex three-dimensional environment

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR102275300B1 (en) * 2019-07-05 2021-07-08 엘지전자 주식회사 Moving robot and control method thereof

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2017173990A1 (en) * 2016-04-07 2017-10-12 北京进化者机器人科技有限公司 Method for planning shortest path in robot obstacle avoidance
CN107368076A (en) * 2017-07-31 2017-11-21 中南大学 Robot motion's pathdepth learns controlling planning method under a kind of intelligent environment
CN107677279A (en) * 2017-09-26 2018-02-09 上海思岚科技有限公司 It is a kind of to position the method and system for building figure
CN112197778A (en) * 2020-09-08 2021-01-08 南京理工大学 Wheeled airport border-patrol robot path planning method based on improved A-x algorithm
CN113033907A (en) * 2021-04-07 2021-06-25 上海钛米机器人股份有限公司 Path planning method and device, electronic equipment and storage medium
CN113110457A (en) * 2021-04-19 2021-07-13 杭州视熵科技有限公司 Autonomous coverage inspection method for intelligent robot in indoor complex dynamic environment
CN113269837A (en) * 2021-04-27 2021-08-17 西安交通大学 Positioning navigation method suitable for complex three-dimensional environment

Also Published As

Publication number Publication date
CN114577214A (en) 2022-06-03

Similar Documents

Publication Publication Date Title
CN114577214B (en) Wheeled robot path planning method applied to cross-heterogeneous multi-layer space
Cao et al. TARE: A Hierarchical Framework for Efficiently Exploring Complex 3D Environments.
Abd Algfoor et al. A comprehensive study on pathfinding techniques for robotics and video games
CN111402339B (en) Real-time positioning method, device, system and storage medium
CN110703747A (en) Robot autonomous exploration method based on simplified generalized Voronoi diagram
CN108334080B (en) Automatic virtual wall generation method for robot navigation
CN114510057A (en) ROS-based mobile robot autonomous navigation method in indoor environment
CN110146085B (en) Unmanned aerial vehicle real-time avoidance rescheduling method based on graph building and rapid random tree exploration
CN113776534B (en) Unmanned aerial vehicle three-dimensional time-varying airspace navigation method based on three-dimensional subdivision grid
CN110471426A (en) Unmanned intelligent vehicle automatic Collision Avoidance method based on quantum wolf pack algorithm
Song et al. Online coverage and inspection planning for 3D modeling
CN112432648A (en) Real-time planning method for safe motion trail of mobile robot
CN113341991A (en) Path optimization method based on dynamic window and redundant node filtering
CN112835064B (en) Mapping positioning method, system, terminal and medium
CN110763247A (en) Robot path planning method based on combination of visual algorithm and greedy algorithm
CN114777793B (en) BIM map extraction and path planning method for any navigation subject
CN114967744A (en) Planning method for multi-unmanned aerial vehicle cooperative obstacle avoidance
Wu et al. A non-rigid hierarchical discrete grid structure and its application to UAVs conflict detection and path planning
Jarvis Distance transform based path planning for robot navigation
Si et al. TOM-Odometry: A generalized localization framework based on topological map and odometry
Jan et al. A computationally efficient complete area coverage algorithm for intelligent mobile robot navigation
CN116338729A (en) Three-dimensional laser radar navigation method based on multilayer map
CN116610109A (en) Gradient-based forward ant colony algorithm unmanned vehicle path planning method
CN114882339B (en) Coal mine roadway eyelet autonomous identification method based on real-time dense point cloud map
Zhai et al. Semantic enrichment of BIM with IndoorGML for quadruped robot navigation and automated 3D scanning

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant