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 PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 94
- 230000008569 process Effects 0.000 claims abstract description 54
- 238000013507 mapping Methods 0.000 claims abstract description 10
- 238000010276 construction Methods 0.000 claims description 31
- 230000036544 posture Effects 0.000 claims description 31
- 239000011159 matrix material Substances 0.000 claims description 29
- 230000003068 static effect Effects 0.000 claims description 25
- 230000009466 transformation Effects 0.000 claims description 16
- 238000006243 chemical reaction Methods 0.000 claims description 8
- 230000004888 barrier function Effects 0.000 claims description 5
- 238000005259 measurement Methods 0.000 claims description 4
- 230000001131 transforming effect Effects 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 3
- 241000287196 Asthenes Species 0.000 claims description 2
- 230000017105 transposition Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000004140 cleaning Methods 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
- 230000000717 retained effect Effects 0.000 description 1
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
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
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
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:
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 , 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
Wherein,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:
wherein,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:
wherein,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;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:
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:
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, noteWherein 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 nodesPath of (1), path is notedThe path length is notedThe height difference between the two nodes is recorded as
Estimating neighboring nodesTotal estimated cost, wherein neighboring nodesThe 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) Cost value of, neighboring nodeHeuristic function values as neighboring nodesEuclidean distance to the target point, expressed as:
wherein,for adjacent nodesDissipation function value, g (point) * ) Dissipation function value, K, for the current optimal node h For a set height cost factor, K h >0;,For adjacent nodesThe function value is inspired, and the function value,is composed ofPosition coordinates in the world coordinate system (comprising three coordinate components of x, y and z) Planning the position coordinates (comprising three coordinate components of x, y and z) of the destination in the world coordinate system for the path For adjacent nodesAn overall estimation cost;
step 24: updating the node list; the specific process is as follows:
1) if the adjacent node already exists in the open node listSame-position node point old If yes, comparing the existing node points in the open node list old With adjacent nodeOverall estimation cost of (2): if the adjacent nodeThe 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 nodesAdding the data into an open node list; if the adjacent nodeThe 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 listIf the nodes with the same position are located, the adjacent nodes are connectedAdding 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 nodesA 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 isAndsetting a position posture homogeneous transformation matrix of a map coordinate system in a world coordinate system in a two-dimensional grid map asThen the position and posture homogeneous transformation matrix of the path planning starting point and the ending point in the current map coordinate system is:
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 :
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:
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
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 notGrid node cell with same position old ;
If the adjacent node does not exist in the open grid node listIf the grid nodes are in the same position, the adjacent grid nodes are connectedAdding the list into an open grid node list;
if there is already a grid node adjacent to the open grid node listGrid node cell with same position old Then, the existing grid node cell is compared old With adjacent grid nodesIf neighboring grid nodesThe 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 nodesAdding the list into an open grid node list; if adjacent grid nodesThe 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 nodesThe 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 nodeJudging the state of the grid state in each map layer:
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 asThe grid coordinates are noted as:
wherein,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 pointsState if adjacent grid pointsThe corresponding value in the cost map layer is 1, which represents the adjacent grid pointOccupied by obstacles, and connecting the adjacent grid pointsJudging as an invalid grid point and deleting; if adjacent grid pointsIf the corresponding value of the cost map layer is 0, estimating the adjacent grid pointsThe overall estimated cost value of;
adjacent grid pointsThe 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 pointCost value of, adjacent grid pointsHeuristic function value of current adjacencyGrid pointThe euclidean distance to the end grid node, expressed as:
wherein, g (cell) * ) For the current optimal grid node dissipation function value,for adjacent grid pointsThe value of the dissipation function is,for adjacent grid pointsThe function value is inspired, and the function value,for adjacent grid pointsThe 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:
wherein,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:
wherein,are the nodes of the adjacent grid, and are,is composed ofAnd the dissipation function values are for the adjacent grid nodes,are the coordinates of the nodes of the adjacent grid,is composed ofAnd a function value is inspired for the adjacent grid nodes,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:
wherein,is as followsIs first and secondIndividual path nodes belonging to temporary path node set Points in temporary map network temp ,To be within a two-dimensional grid mapTo(ii) a planned path of (x) s ,y s ) For the grid coordinates of the planned path points within the two-dimensional grid map,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
Wherein,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:
wherein,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:
wherein,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;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:
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:
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, noteWherein n is a group with point * The number of adjacent nodes;
searching from the current optimal node point * To adjacent nodesPath of (1), path is notedThe path length is notedThe height difference between the two nodes is recorded as
wherein,for adjacent nodesDissipation function value, g (point) * ) Dissipation function value, K, for the current optimal node h For a set height cost factor, K h >0;,For adjacent nodesThe function value is inspired, and the function value,is composed ofThe coordinates of the location within the world coordinate system,the position coordinates of the path planning end point in the world coordinate system,for adjacent nodesAn overall estimation cost;
step 24: updating the node list; the specific process is as follows:
step 241: judging neighboring nodesIs 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 listSame-position node point old If yes, comparing the existing node points in the open node list old With adjacent nodeOverall estimation cost of (c): if the adjacent nodeThe 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 nodesAdding the data into an open node list; if the adjacent nodeThe 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 listIf the nodes are in the same position, the adjacent nodes are connectedAdding 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 nodesA 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 respectivelyAndsetting a position posture homogeneous transformation matrix of a map coordinate system in a world coordinate system in a two-dimensional grid map asThen 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:
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 :
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:
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
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 notGrid node cell with same position old ;
If the adjacent node does not exist in the open grid node listIf the grid nodes are in the same position, the adjacent grid nodes are connectedAdding the list into an open grid node list;
if there is already a grid node adjacent to the open grid node listGrid node cell with same position old Then, the existing grid node cell is compared old With adjacent grid nodesIf neighboring grid nodesThe 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 nodesAdding the list into an open grid node list; if adjacent grid nodesThe 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 nodesThe 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
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 asThe grid coordinates are noted as:
wherein,(Δ 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 pointsState if adjacent grid pointsThe corresponding value in the cost map layer is 1, which represents the adjacent grid pointOccupied by obstacles, and connecting the adjacent grid pointsJudging as an invalid grid point and deleting; if adjacent grid pointsIf the corresponding value of the cost map layer is 0, estimating the adjacent grid pointsThe overall estimated cost value of;
wherein, g (cell) * ) For the current optimal grid node dissipation function value,for adjacent grid pointsThe value of the dissipation function is,for adjacent grid pointsThe function value is inspired, and the function value,for adjacent grid pointsThe 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:
wherein,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:
wherein,are the nodes of the adjacent grid, and are,is composed ofAnd the dissipation function values for the adjacent grid nodes,is the coordinate of the adjacent grid node(s),is composed ofAnda function value is inspired for the adjacent grid nodes,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:
wherein,is as followsIs first and secondIndividual path nodes belonging to temporary path node set Points in temporary map network temp ,To be within a two-dimensional grid mapTo(ii) a planned path of (x) s ,y s ) For the grid coordinates of the planned path points within the two-dimensional grid map,the number of grid points on the planned path.
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)
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)
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)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR102275300B1 (en) * | 2019-07-05 | 2021-07-08 | 엘지전자 주식회사 | Moving robot and control method thereof |
-
2022
- 2022-03-02 CN CN202210203947.7A patent/CN114577214B/en active Active
Patent Citations (7)
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 |