CN115930967A - Path planning method and device and computer storage medium - Google Patents
Path planning method and device and computer storage medium Download PDFInfo
- Publication number
- CN115930967A CN115930967A CN202310001389.0A CN202310001389A CN115930967A CN 115930967 A CN115930967 A CN 115930967A CN 202310001389 A CN202310001389 A CN 202310001389A CN 115930967 A CN115930967 A CN 115930967A
- Authority
- CN
- China
- Prior art keywords
- grid
- dimensional
- point cloud
- floor
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 68
- 238000004422 calculation algorithm Methods 0.000 description 19
- 238000010586 diagram Methods 0.000 description 7
- 238000013473 artificial intelligence Methods 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000004590 computer program Methods 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method 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
- 230000003287 optical effect Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
Images
Landscapes
- Instructional Devices (AREA)
Abstract
The application provides a path planning method, a device and a computer storage medium, comprising the following steps: acquiring three-dimensional point cloud data of an indoor scene; acquiring ground point cloud and first non-ground point cloud which are fitted by three-dimensional point cloud data; acquiring a second non-ground point cloud with the height lower than the preset height from the first non-ground point cloud according to the preset height; constructing a two-dimensional grid map according to the ground point cloud and the second non-ground point cloud, marking a non-accessible grid corresponding to the grid of the ground point cloud and the second non-ground point cloud, corresponding to the ground point cloud, marking a accessible grid without the second non-ground point cloud, and marking a non-accessible grid without the grid of the ground point cloud and the second non-ground point cloud; and inputting a starting point and an end point, and acquiring a planned path of the starting point and the end point according to the two-dimensional grid map. According to the method and the device, the non-ground point cloud with the intercepting height lower than the preset height is separated, and the mark of the grid where the ground point cloud is located is updated by utilizing the projection of the non-ground point cloud, so that the planning path is more accurate.
Description
Technical Field
The present application relates to the field of navigation application technologies, and in particular, to a path planning method and apparatus, and a computer storage medium.
Background
Navigation is a path planning process, but at present, common navigation is realized based on a two-dimensional plane map, and the navigation is mainly applied to outdoor scenes and is less applied to indoor scenes such as shopping malls.
Disclosure of Invention
In order to solve the technical problem, the present application provides a path planning method, an apparatus and a computer storage medium.
In order to solve the above technical problem, the present application provides a path planning method, where the path planning method includes:
acquiring three-dimensional point cloud data of an indoor scene; acquiring a ground point cloud and a first non-ground point cloud which are fitted by the three-dimensional point cloud data; acquiring a second non-ground point cloud lower than the preset height from the first non-ground point cloud according to the preset height; constructing a two-dimensional grid map according to the ground point cloud and the second non-ground point cloud, wherein grids corresponding to the ground point cloud and the second non-ground point cloud in the two-dimensional grid map are marked as non-accessible grids, the ground point cloud is corresponding to the grids, grids without the second non-ground point cloud are marked as accessible grids, and grids without the ground point cloud and the second non-ground point cloud are marked as non-accessible grids; and inputting a starting point and an end point, and acquiring a planned path of the starting point and the end point according to the two-dimensional grid map.
Wherein the obtaining of the ground point cloud and the first non-ground point cloud fitted to the three-dimensional point cloud data comprises: performing plane fitting on the ground plane of the three-dimensional point cloud data to obtain a fitting plane; calculating the distance from each data point in the three-dimensional point cloud data to the fitting plane according to the normal vector of the fitting plane; forming the ground point cloud by the data points with the distance less than or equal to a preset distance; and forming the data points with the distance larger than the preset distance into the first non-ground point cloud.
Wherein, after constructing the two-dimensional grid map according to the ground point cloud and the second non-ground point cloud, the method comprises the following steps: acquiring two-dimensional grid maps of different floors; and fusing the two-dimensional grid maps of all floors to obtain the three-dimensional grid map of the indoor scene, wherein the grid coordinates of the three-dimensional grid map comprise the grid coordinates of the two-dimensional grid map and the height coordinates of the floor.
Wherein the three-dimensional grid map further comprises a traffic grid; the path planning method further comprises the following steps: acquiring three-dimensional coordinates of traffic elements; acquiring the origin of coordinates of a two-dimensional grid map of the floor where the traffic element is located; and converting the three-dimensional coordinates into the traffic grids of the traffic elements according to the coordinate origin, wherein the height coordinates of the traffic grids are the height coordinates of the floors where the traffic grids are located.
Wherein, the inputting the starting point and the end point and obtaining the planned path of the starting point and the end point according to the two-dimensional grid map comprises: judging whether the starting point and the end point are on the same floor or not; if yes, searching a first traffic grid closest to the floor where the starting point is located according to the grid coordinate of the starting point; acquiring a second traffic grid corresponding to the first traffic grid on the floor where the terminal is located; acquiring a first planned path from the starting point to the first traffic grid and acquiring a second planned path from the second traffic grid to the terminal point; and generating a planned path of the starting point and the end point based on the first planned path and the second planned path.
Wherein, the obtaining the second traffic grid corresponding to the first traffic grid at the floor of the terminal comprises: acquiring an original point three-dimensional coordinate of an original point of a floor where the first traffic grid is located; determining a first three-dimensional coordinate of the first traffic grid according to the grid coordinate of the first traffic grid and the origin three-dimensional coordinate; determining second three-dimensional coordinates of the second traffic grid based on the first three-dimensional coordinates, wherein the first three-dimensional coordinates areXYOf coordinates with said second three-dimensional coordinatesXYThe coordinates being the same, of said second three-dimensional coordinatesZThe coordinate value of the axis is the height value of the floor where the axis is located; and acquiring the second traffic grid according to the second three-dimensional coordinate.
Wherein the traffic elements include elevators and stairways; the path planning method further comprises the following steps: when the starting point and the ending point are not positioned on the same floor, searching elevator grids on the floor where the starting point is positioned according to the grid coordinates of the starting point; and if the floor where the starting point is located does not have the elevator grid, searching the nearest stair grid on the floor where the starting point is located.
Wherein, the searching for the elevator grid on the floor where the starting point is located according to the grid coordinate of the starting point comprises the following steps: if the floor where the starting point is located has an elevator grid, judging whether the floor where the terminal point is located has a corresponding elevator grid; if not, acquiring a transfer floor which has a corresponding elevator grid and is closest to the floor where the terminal point is located, and determining a transfer elevator grid corresponding to the elevator grid at the transfer floor; searching a first transfer stair grid closest to the transfer elevator grid on the transfer floor; acquiring a second transfer stair grid corresponding to the first transfer stair grid on the floor where the terminal point is located; the obtaining of the planned path of the starting point and the end point according to the two-dimensional grid map comprises: acquiring a third planned path from the starting point to the elevator grid, a fourth planned path from the transfer elevator grid to the first transfer stair grid, and a fifth planned path from the second transfer stair grid to the terminal point; and generating the planned path of the starting point and the end point based on the third planned path, the fourth planned path and the fifth planned path.
In order to solve the above technical problem, the present application further provides a path planning apparatus, which includes a memory and a processor coupled to the memory;
the memory is used for storing program data, and the processor is used for executing the program data to realize the path planning method.
To solve the above technical problem, a computer storage medium for storing program data for implementing the above path planning method when executed by a computer.
Compared with the prior art, the beneficial effects of this application are: the method comprises the steps that a path planning device obtains three-dimensional point cloud data of an indoor scene; acquiring ground point cloud and first non-ground point cloud which are fitted by three-dimensional point cloud data; acquiring a second non-ground point cloud lower than the preset height from the first non-ground point cloud according to the preset height; constructing a two-dimensional grid map according to the ground point cloud and the second non-ground point cloud, wherein grids corresponding to the ground point cloud and the second non-ground point cloud in the two-dimensional grid map are marked as unviable grids, the ground point cloud is corresponding to the unviable grids, grids not having the second non-ground point cloud are marked as unviable grids, and grids not having the ground point cloud and the second non-ground point cloud are marked as unviable grids; and inputting a starting point and an end point, and acquiring a planned path of the starting point and the end point according to the two-dimensional grid map. According to the path planning method, the non-ground point clouds in the height lower than the preset height are separated and intercepted, and the marks of the grids where the ground point clouds are located are updated by projection of the non-ground point clouds, so that the path planning is more accurate.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed to be used in the description of the embodiments are briefly introduced below, and it is obvious that the drawings in the following description are only some embodiments of the present application, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts. Wherein:
fig. 1 is a schematic flowchart of a first embodiment of a path planning method provided in the present application;
fig. 2 is a schematic flow chart illustrating a sub-step of step S12 in the path planning method provided in the present application;
fig. 3 is a schematic overall flow chart of grid map construction of the path planning method provided in the present application;
FIG. 4 is a schematic overall flow chart of a path planning method provided in the present application;
fig. 5 is a schematic flow chart illustrating a sub-step of step S15 in the path planning method provided in the present application;
fig. 6 is a schematic flow chart illustrating a sub-step of step S153 in the path planning method provided in the present application;
fig. 7 is a schematic flowchart of another embodiment of a path planning method provided in the present application;
fig. 8 is another overall flowchart of the path planning method provided in the present application;
fig. 9 is a schematic structural diagram of a path planning apparatus provided in the present application;
fig. 10 is a schematic structural diagram of a computer storage medium provided in the present application.
Detailed Description
The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are only a part of the embodiments of the present application, and not all of the embodiments. All other embodiments obtained by a person of ordinary skill in the art based on the embodiments in the present application without making any creative effort belong to the protection scope of the present application.
The terms "first," "second," "third," "fourth," and the like in the description and in the claims of the present application and in the drawings described above, if any, are used for distinguishing between similar elements and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used is interchangeable under appropriate circumstances such that the embodiments of the application described herein are, for example, capable of operation in sequences other than those illustrated or otherwise described herein. Furthermore, the terms "comprises," "comprising," and "having," and any variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, system, article, or apparatus that comprises a list of steps or elements is not necessarily limited to those steps or elements expressly listed, but may include other steps or elements not expressly listed or inherent to such process, method, article, or apparatus.
Referring to fig. 1 in detail, fig. 1 is a schematic flow chart of a first embodiment of a path planning method provided in the present application.
The path planning method is applied to a path planning device, wherein the path planning device can be a server, a local terminal, or a system formed by the server and the local terminal in a matched mode. Correspondingly, each part, for example, each unit, subunit, module, and submodule, included in the path planning apparatus may all be disposed in the server, may all be disposed in the local terminal, and may also be disposed in the server and the local terminal, respectively.
Further, the server may be hardware or software. When the server is hardware, it may be implemented as a distributed server cluster composed of multiple servers, or may be implemented as a single server. When the server is software, it may be implemented as a plurality of software or software modules, for example, software or software modules for providing distributed servers, or as a single software or software module, and is not limited herein.
As shown in fig. 1, the specific steps are as follows:
step S11: and acquiring three-dimensional point cloud data of an indoor scene.
Specifically, the path planning device collects field data of the indoor scene, obtains three-dimensional model data of the indoor scene, obtains vertex coordinates of all the indoor scenes for all the three-dimensional model data of the indoor scene, obtains vertex data forming the three-dimensional model of the floor, and the vertex data form a piece of point cloud data
And performing up-sampling processing on the point cloud data to obtain compact point cloud data
。
Step S12: and acquiring ground point cloud and first non-ground point cloud which are fitted by the three-dimensional point cloud data.
Specifically, in an embodiment of the present application, the path planning apparatus fits a plane through a RANdom SAmple Consensus (RANdom SAmple Consensus) algorithm, and separates a ground point cloud and a first non-ground point cloud of an indoor scene. In other embodiments of the present application, the path planning device may further separate the ground point cloud and the first non-ground point cloud of the indoor scene through machine algorithm identification.
In other embodiments, the path planning apparatus may also employ other plane fitting algorithms, which are not listed here.
The first non-ground point cloud is all data point clouds except the ground point cloud.
Further, the present application proposes steps S121 to S124 as substeps of step S12, which are used for separating the ground point cloud and the non-ground point cloud, specifically referring to fig. 2 and 3, where fig. 2 is a schematic flow diagram of the substep of step S12 in the path planning method provided by the present application; fig. 3 is a schematic overall flow chart of the grid map construction of the path planning method provided in the present application.
As shown in fig. 2, the specific steps are as follows:
step S121: and performing plane fitting on the ground plane of the three-dimensional point cloud data to obtain a fitting plane.
Specifically, the path planning device performs plane fitting on a ground plane in the indoor point cloud data by using a RANSAC method to obtain a fitting plane, wherein a fitted plane equation is as follows:
step S122: and calculating the distance from each data point in the three-dimensional point cloud data to the fitting plane according to the normal vector of the fitting plane.
Specifically, assume that the fitted plane equation is
Wherein
Representing the normal vector of the plane.
To represent
Middle point
The distance to the fitting plane is calculated as follows:
step S123: and forming ground point cloud by using the data points with the distance less than or equal to the preset distance.
In particular, the path planner combines the distancesdIntegrating data points with a distance less than a preset distance to form a ground point cloud data set
. Namely:d<εis ground point cloud;d>εis a non-ground point.
Step S124: and forming a first non-ground point cloud by using the data points with the distance greater than the preset distance.
In particular, the path planner will map the distancedData points greater than the preset distance are integrated to form a first non-ground point cloud data set.
Through step S121 to step S124: the ground point cloud and the non-ground point cloud can be preliminarily separated.
Step S13: and acquiring a second non-ground point cloud with the height lower than the preset height from the first non-ground point cloud according to the preset height.
Specifically, the path planning device carries out point cloud along a first non-ground point according to a preset heightzIntercepting an axis, acquiring a second non-ground point cloud with the height lower than the preset height, and setting a second non-ground point cloud data set as
. The preset height can be the height of a door frame under an indoor scene or the height of other passageway boundaries needing to pass through, the preset height can be manually or automatically modified according to scene requirements, different preset heights can be set for different doors or passageways, the passing permission and instructions of the passageways can be further adjusted in a self-adaptive mode, and the map precision is improved.
In one embodiment of the present application, for non-ground point clouds
Intercepting the point cloud along the z direction according to the height of the door in the indoor scene, wherein the intercepting range is
In
To a z value not exceeding the door frame height, the intercepted point cloud data being
。
Step S14: and constructing a two-dimensional grid map according to the ground point cloud and the second non-ground point cloud.
The two-dimensional grid map is provided with a first non-ground point cloud, a second non-ground point cloud, a mesh mark corresponding to the first non-ground point cloud and a mesh mark corresponding to the second non-ground point cloud.
Specifically, the path planning device calculates the midpoint coordinates of the ground point cloud
And is noted as the maximum and minimum of x and y
And
then is followed by
A coordinate system is established for the origin of the image,xshaft andythe positive direction of the axis is the direction of increasing numerical value, the ground point cloud and the non-ground point cloud are subjected to grid division based on the ranges of x and y, and the grid size is recorded as
And assuming grid coordinates of
，Indicating that the grid is located atjGo to the firstkAnd (4) columns.
According to the origin of coordinates
And a grid size r, converting the position coordinates to grid coordinates by the following formula:
further, as shown in fig. 3, the path planning device makes a point cloud of the ground
Setting the divided grids as 1 to show that the grids are passable and the non-ground point clouds
Projecting the divided grids to the ground along the normal direction of the ground plane; if the point cloud data exists in the grid, setting the ground grid projected to the corresponding position on the ground as 0 to represent that the grid is not passable; if the point cloud data does not exist in the grid, the value of the ground grid at the corresponding position after projection is unchanged, and after all the grids are projected, the two-dimensional grid map data of the floor can be obtained
。
According to the path planning method, when the non-ground point clouds are projected to the ground, the point cloud data in the range from the minimum value of z in the point cloud to the value of z not exceeding the height of the door frame are intercepted and projected, so that the position of the door can be ensured to be passable, and the grid map is more accurate.
Step S15: and inputting a starting point and an end point, and acquiring a planned path of the starting point and the end point according to the two-dimensional grid map.
Specifically, the path planning device inputs a starting point and an end point, and plans the path according to the position of the starting point and the end point.
In an embodiment of the present application, the starting point and the ending point are on the same floor, and path planning is directly performed by using a path planning algorithm such as an a-algorithm or a Dijkstra algorithm. Specifically, a starting point grid coordinate and an end point grid coordinate of a starting point and an end point in a two-dimensional grid map are obtained, a passable grid on the two-dimensional grid map is subjected to path planning, and finally grid coordinates of all path points are converted into three-dimensional point cloud coordinates, and a planned path of an indoor scene is obtained.
In other embodiments of the present application, the starting point and the ending point may not be the same floor, and at this time, the path planning device performs path planning according to traffic grids of different traffic elements after fusing two-dimensional grid maps of different floors, specifically please refer to the following:
in an embodiment of the application, the path planning device can acquire two-dimensional grid maps of different floors, and further acquire a three-dimensional grid map of an indoor scene of multiple floors.
Referring to fig. 4, fig. 4 is a schematic overall flow chart of the path planning method provided in the present application. The method comprises the following specific steps:
as shown in fig. 4, the path planning apparatus acquires two-dimensional grid maps of different floors. And fusing the two-dimensional grid maps of all floors to obtain the three-dimensional grid map of the indoor scene.
The grid coordinates of the three-dimensional grid map comprise grid coordinates of the two-dimensional grid map and height coordinates of the floor where the two-dimensional grid map is located.
Specifically, for an indoor scene, after a real three-dimensional model of the whole scene is established, the path planning device respectively processes each floor independently according to the floor condition of the scene, and generates corresponding three-dimensional raster map data for navigation.
The path planning device firstly divides the corresponding floors of the three-dimensional model according to the actual floor information of the scene, obtains the three-dimensional model data corresponding to each floor, and assumes that the obtained floor model data set is a set of floor model data
The height information corresponding to each floor is
The height value here refers to the height of the ground in one floor. Where n represents the number of floors.
Route planning device according to floor
Corresponding height information, for the grid map data in step S14
Each grid of adding height values of
And recording the coordinates of each grid as
。
By the mode, the path planning device realizes the navigation and path planning among the indoor cross floors through the pre-established indoor three-dimensional model.
Further, in this embodiment, the three-dimensional grid map also includes traffic grids including, but not limited to, access locations or transit locations in single or multi-story indoor scenes, such as elevators, escalators, stairways, fire ladders, fire corridors, and the like.
The method comprises the following specific steps:
the path planning device obtains the three-dimensional coordinates of the traffic elements, obtains the origin of coordinates of the two-dimensional grid map of the floor where the traffic elements are located, and converts the three-dimensional coordinates into the traffic grid of the traffic elements according to the origin of coordinates. The process is identical to the coordinate conversion process in step S14, and will not be described herein.
Wherein, the height coordinate of the traffic grid is the height coordinate of the floor where the traffic grid is located.
Specifically, the path planning device performs target detection according to coordinate position input or artificial intelligence image recognition, acquires the positions of the traffic elements, acquires the three-dimensional coordinates of the traffic elements, and converts the three-dimensional coordinates into traffic grids of the traffic elements according to the coordinate origin by taking the position of each traffic element as the coordinate origin.
The traffic element can be any indoor traffic transfer means, such as an elevator, an escalator, a stair, a fire ladder, a fire passage, a corridor and the like.
In an embodiment of the present application, an indoor scene is a multi-floor, and a path planning device obtains a three-dimensional grid map of the entire multi-floor indoor scene by adding height data to a two-dimensional grid map, and in this embodiment, steps S151 to S155 are proposed as sub-steps of step S15 for generating a planned path according to the three-dimensional grid map and a traffic grid. Referring to fig. 5, fig. 5 is a schematic flow chart illustrating a sub-step of step S15 in the path planning method provided in the present application.
As shown in fig. 5, the specific steps are as follows:
step S151: and judging whether the starting point and the end point are positioned on the same floor.
Specifically, the route planning apparatus determines the floors to which the start point and the end point belong, assuming that the coordinates of the start point and the end point are respectively
And
according to the starting point and the end
Of point coordinates
And (3) judging floors of the starting point and the terminal point by values, wherein the specific judgment mode is as follows:
。
when in use
Satisfy the above formulaThen it means that the point is located on the floor
The corresponding grid map data is
The floors to which the starting point and the ending point belong are acquired, and if the starting point and the ending point do not belong to the same floor, step S152 is executed.
When the starting point and the ending point are on the same floor, it is assumed that the route planning device calculates the starting point in step S151
And an end point
The belonged floors are respectively
And
then their corresponding grid map data are
And
when is coming into contact with
When the starting point and the end point are on the same floor, the starting point is calculated by the following formula
And an end point
Adopting A-star algorithm or Dijkstra algorithm at grid position in grid mapThe equal path planning algorithm can directly plan a path from the starting point to the end point on the grid map.
Step S152: and searching the first traffic grid with the nearest distance on the floor where the starting point is located according to the grid coordinate of the starting point.
When in use
I.e. starting point
And an end point
When the floors are not on the same floor, the path planning device calculates the grid coordinates corresponding to the floors and the coordinates of the starting point and the ending point respectively according to the floors and the coordinates of the starting point and the ending point, and the grid coordinates of the starting point and the ending point are assumed to be respectively
And
and then, a first traffic grid closest to the starting point is firstly searched in the grid map of the floor where the starting point is located.
Wherein, in an embodiment of the present application, the first traffic grid closest to the origin is determined by modifying the grid priority of the grid.
In particular, the path planner is based on the floor
Position coordinates of elevators, escalators and stairways in grid map data
Finds the corresponding grid and modifies the value of the grid.For example, in a specific embodiment of the present application, for a staircase, the value of the grid at its corresponding location is modified to 2; for an escalator, the value of the grid of its corresponding position is modified to 3, and for an elevator, the value of the grid of its corresponding position is modified to 4. The larger the value, the higher the priority, and the more the floor is crossed.
In other embodiments of the present application, the setting of the priority may also be in other manners, and the specific manner is specifically determined according to the indoor scene.
In other embodiments of the present application, the priority of the traffic grid may also be adaptively changed according to the actual route distance, so as to determine the first traffic grid closest to the starting point. For example, in the path planning process, the original priority is that the elevator is larger than the stairs, when the starting point position is one floor, the end point position is two floors, and the distance between the stairs and the starting point position is larger than the distance between the elevator and the starting point position, the obtained elevator is too crowded, at the moment, the path planning is directly carried out through the stairs, the distance is shorter, the time is shorter, the priority is automatically changed, and the stairs closer to each other are used as a first traffic grid of the first priority, so that the path planning device can carry out the path planning more flexibly.
Step S153: and acquiring a second traffic grid corresponding to the first traffic grid at the floor where the terminal is located.
Specifically, the path planning device acquires a second traffic grid corresponding to the first traffic grid on a floor where the terminal is located.
For example, when the floor where the starting point is located can directly reach the floor where the ending point is located through an elevator, the first traffic grid can be the elevator grid at the starting point, the second traffic grid is the elevator grid corresponding to the ending point, and at the moment, the first traffic grid and the second traffic grid are the elevator grids corresponding to the ending pointx、yThe coordinates are the same as the coordinates of the other,zthe axis coordinate is the coordinate of the floor, namely the two coordinates have the same position and different heights.
It should be noted that, in other embodiments of the present application, the traffic elements corresponding to the first traffic grid and the second traffic grid may be stairs or elevators or other traffic elements that can be directly used for floor switching.
It should be noted that, the embodiments of the present application further include: when the starting position cannot directly reach the end position through one traffic element, the traffic element needs to be transferred, at the moment, the coordinates of the first traffic grid and the second traffic grid do not correspond to each other, and the first traffic grid and the second traffic grid are determined according to the grid where the transfer traffic element is located. Specifically, the steps are step S21 to step S28.
In particular, in one embodiment of the present application, the first traffic grid and the second traffic grid are both elevators, assuming that the first traffic grid is an elevator
The three-dimensional coordinate corresponding to the grid coordinate can be calculated according to the following formula
：
Further, firstly, the A-star algorithm is adopted to map the grid map
The path planning is carried out on the starting point and the elevator point, and the user can go to the desired floor after entering the elevator; wherein
Indicating floors
The coordinates corresponding to the origin of the image,
is a floor
Of (c) is measured.
Further, the present application proposes steps S1531-S1534 as the sub-steps of step S153 for acquiring the second traffic grid. Referring to fig. 6 specifically, fig. 6 is a schematic flow chart of the sub-step of step S153 in the path planning method provided in the present application.
As shown in fig. 6, the specific steps are as follows:
step S1531: and acquiring an original point three-dimensional coordinate of the original point of the floor where the first traffic grid is located.
Specifically, the path planning device obtains a first traffic grid
Floor of the building
Origin three-dimensional coordinates of the origin.
Step S1532: a first three-dimensional coordinate of the first traffic grid is determined from the grid coordinate of the first traffic grid and the origin three-dimensional coordinate.
In particular, the path planning device depends on the grid coordinates of the first traffic grid
Determining first three-dimensional coordinates of a first traffic grid according to the following formula
：
Step S1533: a second three-dimensional coordinate of a second traffic grid is determined based on the first three-dimensional coordinate.
Wherein the first three-dimensional coordinateXYOf coordinates with a second three-dimensional coordinateXYHaving the same coordinates, of a second three-dimensional coordinate
The axial coordinate value is the height value of the floor where the axial coordinate value is located.
Specifically, the path planning device does not change according to the first three-dimensional coordinatesXYCoordinates, only changing
Coordinates of the axes, determine second three-dimensional coordinates of the second traffic grid. For example, the three-dimensional coordinates of the first traffic grid are
Then the three-dimensional coordinates of the second traffic grid are
。
Step S1534: and acquiring a second traffic grid according to the second three-dimensional coordinate.
Specifically, the path planning device converts the second three-dimensional coordinates into grid coordinates to obtain a second traffic grid.
Through the steps S1531 to S1534, the second traffic grid can be obtained according to the first traffic grid for path planning, so as to implement the cross-floor path planning.
Step S154: a first planned path from the starting point to the first traffic grid is obtained, and a second planned path from the second traffic grid to the destination is obtained.
Specifically, the path planning device acquires a first planned path from a starting point to a first traffic grid and a second planned path from a second traffic grid to a terminal point,
step S155: and generating a planned path of the starting point and the end point based on the first planned path and the second planned path.
By the mode, the corresponding grid positions of the optional positions in the grid map can be calculated, path planning between the optional positions is realized, the path planning between different indoor floors can be realized by marking the elevator, the escalator and the stairs in the scene, and meanwhile, the elevator, the escalator and the stairs are provided with different priorities, so that the elevator, the escalator and the stairs can be guaranteed to go to other floors in the most convenient mode.
Further, an embodiment is provided in the present application, configured to determine a specific planned path according to an input starting point and an end point. Referring to fig. 7 and 8, fig. 7 is a schematic flow chart diagram of another embodiment of the path planning method provided in the present application; fig. 8 is another overall flow diagram of the planned path of the path planning method provided in the present application.
As shown in fig. 7, the specific steps are as follows:
step S21: and searching an elevator grid on the floor where the starting point is located according to the grid coordinate of the starting point when the starting point and the terminal point are not located on the same floor.
Specifically, as shown in FIG. 8, when
I.e. starting point
And end point
When the floor is not the same, the path planning device calculates the grid coordinates corresponding to the floor and the coordinates of the starting point and the end point according to the floor and the coordinates of the starting point and the end point, and the grid coordinates of the starting point and the end point are assumed to be respectively
And
and then preferentially searching the elevator grid closest to the starting point in the grid map of the floor where the starting point is located. The path planning device performs target detection according to coordinate position input or artificial intelligence image recognition, obtains the position of the elevator, and obtains the elevator grid through conversion of position coordinates and grid coordinates.
Step S22: and if the floor where the starting point is located does not have the elevator grid, searching the nearest stair grid on the floor where the starting point is located.
In particular, the path planner will automatically find the nearest staircase or stair grid if the floor does not detect the presence of an elevator grid.
It should be noted that, since a stair can reach any floor necessarily in an indoor scene, when there is no elevator grid at the starting point, the nearest stair grid is searched, and the stair corresponding to the stair grid can reach any floor or a floor where other traffic elements are located.
Step S23: and if the floor where the starting point is located has the elevator grid, judging whether the floor where the terminal point is located has the corresponding elevator grid.
Specifically, when the floor where the starting point is located has an elevator grid, whether the floor where the ending point is located has a corresponding elevator grid is further judged through grid coordinates, and if the floor where the ending point is located has the elevator grid corresponding to the starting point floor, the path planning device firstly adopts an A-x algorithm to perform grid map processing on the elevator grid map
And (4) planning the route of the starting point and the elevator point, and if the floor where the destination is located does not have the elevator grid corresponding to the elevator grid of the floor where the starting point is located after the elevator enters the elevator, namely the floor from the starting point to the destination cannot be reached through the elevator, continuing to execute the step S25.
Step S24: and acquiring a transfer floor which has a corresponding elevator grid and is closest to the floor where the terminal point is located, and determining the transfer elevator grid corresponding to the elevator grid at the transfer floor.
Specifically, the path planning device obtains an elevator grid corresponding to a floor where the terminal point is located and a floor where the starting point is located, and searches for a transfer floor closest to the floor where the terminal point is located. If the starting elevator can not reach the floor where the terminal point is located, the path planning device plans to the floor closest to the terminal point, then searches the position of the escalator or the stair closest to the elevator on the current floor after leaving the elevator, carries out path planning on the elevator points and the escalator points or the stair points in the grid map of the current floor by adopting an A-star algorithm, then goes to the floor where the terminal point is located through the escalator or the stair, and finally completes the path planning from the escalator points or the stair points to the terminal point in the grid map by adopting the A-star algorithm, thus completing the whole path planning process.
For example, when the starting point is one floor, the ending point is five floors, and elevator grids corresponding to one floor do not exist in five floors, that is, one elevator can reach five floors from one floor, if elevator grids corresponding to one floor exist in three floors, the three floors are determined as transfer floors, and the elevators in the three floors are transfer elevators corresponding to one floor of elevator.
Step S25: the first transit stairway grid closest to the transit elevator grid is searched for at the transit floor.
If it is used
If there is no elevator at the floor, the path planning device will go to find the nearest staircase to the starting point, and if there is no staircase, will eventually go to find the position of the nearest staircase. Suppose that the found position is
With corresponding coordinates of
Then, firstly, the A-star algorithm is adopted to map the grid map
Upper pair of start points and
the point carries out path planning and goes to the floor nearest to the starting floor by an escalator or stairs, and the floor is assumed to be the floor with the elevator
Then the coordinates become
The grid map at this time can be calculated according to the following formula
Grid coordinates of (2).
Step S26: and acquiring a second transit stair grid corresponding to the first transit stair grid at the floor where the terminal point is located.
Specifically, the path planning device acquires a second transit stair grid corresponding to the first transit stair at the destination floor at the floor where the destination is located.
Step S27: and acquiring a third planned path from the starting point to the elevator grid, a fourth planned path from the transfer elevator grid to the first transfer stair grid, and a fifth planned path from the second transfer stair grid to the terminal point.
Specifically, the path planning device acquires a third planned path from the starting point to the nearest elevator grid, a fourth planned path from the transfer elevator grid to the first transfer stair and a fifth planned path from the second transfer stair to the terminal point according to the positions of the starting point and the terminal point.
Step S28: and generating a planned path of the starting point and the end point based on the third planned path, the fourth planned path and the fifth planned path.
Specifically, the path planning device generates a planned path of the starting point and the end point according to the third planned path, the fourth planned path and the fifth planned path, and after the path planning from the starting point to the end point is completed, three-dimensional coordinates of all points on the path can be calculated and displayed on the three-dimensional model, so that people can conveniently navigate.
By the method, the corresponding grid positions of any positions in the grid map can be calculated, path planning among any positions is achieved, path planning among different indoor floors can be achieved by marking the elevator, the escalator and the stairs in the scene, and meanwhile, the elevator, the escalator and the stairs can be guaranteed to go to other floors in the most convenient mode by setting different priorities.
In the embodiment of the application, the path planning is performed by adopting an a-star algorithm, and compared with a Dijkstra algorithm, the calculation complexity of the algorithm is smaller. The method adopts the A-x algorithm to plan the path, is a global path planning method, and can plan an optimal path through the A-x algorithm by constructing a grid map of a scene and marking the grid where the obstacle is positioned as an unviable area and other grids as passable areas so as to realize navigation and path planning.
To implement the above path planning method, the present application further provides a path planning device, and specifically refer to fig. 9, where fig. 9 is a schematic structural diagram of the path planning device provided in the present application.
The path planning apparatus 400 of the present embodiment includes a processor 41, a memory 42, an input/output device 43, and a bus 44.
The processor 41, the memory 42, and the input/output device 43 are respectively connected to the bus 44, the memory 42 stores program data, and the processor 41 is configured to execute the program data to implement the path planning method according to the above embodiment.
In the embodiment of the present application, the processor 41 may also be referred to as a CPU (Central Processing Unit). The processor 41 may be an integrated circuit chip having signal processing capabilities. Processor 41 may also be a general purpose processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field Programmable Gate Array (FPGA) or other Programmable logic device, discrete Gate or transistor logic, discrete hardware components. A general purpose processor may be a microprocessor or the processor 41 may be any conventional processor or the like.
Please refer to fig. 10, fig. 10 is a schematic structural diagram of the computer storage medium provided in the present application, a computer program 51 is stored in the computer storage medium 500, and the computer program 51 is used to implement the path planning method according to the above embodiment when being executed by a processor.
Embodiments of the present application may be implemented in software functional units and may be stored in a computer readable storage medium when sold or used as a stand-alone product. Based on such understanding, the technical solution of the present application may be substantially implemented or contributed by the prior art, or all or part of the technical solution may be embodied in a software product, which is stored in a storage medium and includes instructions for causing a computer device (which may be a personal computer, a server, a network device, or the like) or a processor (processor) to execute all or part of the steps of the method according to the embodiments of the present application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk, or an optical disk, and various media capable of storing program codes.
The above description is only for the purpose of illustrating embodiments of the present application and is not intended to limit the scope of the present application, and all modifications of equivalent structures and equivalent processes, which are made by the contents of the specification and the drawings of the present application or are directly or indirectly applied to other related technical fields, are also included in the scope of the present application.
Claims (10)
1. A path planning method is characterized by comprising the following steps:
acquiring three-dimensional point cloud data of an indoor scene;
acquiring a ground point cloud and a first non-ground point cloud which are fitted by the three-dimensional point cloud data;
acquiring a second non-ground point cloud lower than the preset height from the first non-ground point cloud according to the preset height;
constructing a two-dimensional grid map according to the ground point cloud and the second non-ground point cloud, wherein grids corresponding to the ground point cloud and the second non-ground point cloud in the two-dimensional grid map are marked as unviable grids, the ground point cloud is corresponding to the unviable grids, grids not having the second non-ground point cloud are marked as unviable grids, and grids not having the ground point cloud and the second non-ground point cloud are marked as unviable grids;
and inputting a starting point and an end point, and acquiring a planned path of the starting point and the end point according to the two-dimensional grid map.
2. The path planning method according to claim 1,
the acquiring of the ground point cloud and the first non-ground point cloud fitted by the three-dimensional point cloud data comprises:
performing plane fitting on the ground plane of the three-dimensional point cloud data to obtain a fitting plane;
calculating the distance from each data point in the three-dimensional point cloud data to the fitting plane according to the normal vector of the fitting plane;
forming the ground point cloud by the data points with the distance less than or equal to a preset distance;
and forming the data points with the distance larger than the preset distance into the first non-ground point cloud.
3. The path planning method according to claim 1,
after the two-dimensional grid map is constructed according to the ground point cloud and the second non-ground point cloud, the method comprises the following steps:
acquiring two-dimensional grid maps of different floors;
and fusing the two-dimensional grid maps of all floors to obtain the three-dimensional grid map of the indoor scene, wherein the grid coordinates of the three-dimensional grid map comprise the grid coordinates of the two-dimensional grid map and the height coordinates of the floor.
4. The path planning method according to claim 3,
the three-dimensional grid map further comprises a traffic grid;
the path planning method further comprises the following steps:
acquiring three-dimensional coordinates of traffic elements;
acquiring a coordinate origin of a two-dimensional grid map of a floor where the traffic element is located;
and converting the three-dimensional coordinates into a traffic grid of the traffic element according to the coordinate origin, wherein the height coordinate of the traffic grid is the height coordinate of the floor where the traffic grid is located.
5. The path planning method according to claim 4,
inputting the starting point and the end point, and acquiring a planned path of the starting point and the end point according to the two-dimensional grid map, wherein the method comprises the following steps:
judging whether the starting point and the end point are on the same floor or not;
if not, searching a first traffic grid closest to the floor where the starting point is located according to the grid coordinate of the starting point;
acquiring a second traffic grid corresponding to the first traffic grid on the floor where the terminal is located;
obtaining a first planned path from the starting point to the first traffic grid and obtaining a second planned path from the second traffic grid to the terminal point;
and generating a planned path of the starting point and the end point based on the first planned path and the second planned path.
6. The path planning method according to claim 5,
the obtaining a second traffic grid corresponding to the first traffic grid at the floor where the terminal is located includes:
acquiring an original point three-dimensional coordinate of an original point of a floor where the first traffic grid is located;
determining a first three-dimensional coordinate of the first traffic grid according to the grid coordinate of the first traffic grid and the origin three-dimensional coordinate;
determining second three-dimensional coordinates of the second traffic grid based on the first three-dimensional coordinates, wherein the first three-dimensional coordinates areXYOf coordinates with said second three-dimensional coordinatesXYThe coordinates being the same, of said second three-dimensional coordinatesZThe coordinate value of the axis is the height value of the floor where the axis is located;
and acquiring the second traffic grid according to the second three-dimensional coordinate.
7. The path planning method according to claim 5,
the traffic elements include elevators and stairways;
the path planning method further comprises the following steps:
when the starting point and the ending point are not positioned on the same floor, searching elevator grids on the floor where the starting point is positioned according to the grid coordinates of the starting point;
and if the floor where the starting point is located does not have the elevator grid, searching the nearest stair grid on the floor where the starting point is located.
8. The path planning method according to claim 7,
the searching for the elevator grid on the floor where the starting point is located according to the grid coordinate of the starting point comprises the following steps:
if the floor where the starting point is located has an elevator grid, judging whether the floor where the terminal point is located has a corresponding elevator grid;
if not, acquiring a transfer floor which has a corresponding elevator grid and is closest to the floor where the terminal point is located, and determining a transfer elevator grid corresponding to the elevator grid at the transfer floor;
searching a first transfer stair grid closest to the transfer elevator grid on the transfer floor;
acquiring a second transfer stair grid corresponding to the first transfer stair grid on the floor where the terminal point is located;
the obtaining of the planned path of the starting point and the end point according to the two-dimensional grid map comprises:
acquiring a third planned path from the starting point to the elevator grid, a fourth planned path from the transfer elevator grid to the first transfer stair grid, and a fifth planned path from the second transfer stair grid to the terminal point;
and generating the planned path of the starting point and the end point based on the third planned path, the fourth planned path and the fifth planned path.
9. A path planner, wherein the path planner comprises a memory and a processor coupled to the memory;
wherein the memory is for storing program data and the processor is for executing the program data to implement the path planning method according to any one of claims 1 to 8.
10. A computer storage medium for storing program data which, when executed by a computer, is adapted to implement a path planning method according to any one of claims 1 to 8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310001389.0A CN115930967A (en) | 2023-01-03 | 2023-01-03 | Path planning method and device and computer storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310001389.0A CN115930967A (en) | 2023-01-03 | 2023-01-03 | Path planning method and device and computer storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115930967A true CN115930967A (en) | 2023-04-07 |
Family
ID=86697969
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310001389.0A Pending CN115930967A (en) | 2023-01-03 | 2023-01-03 | Path planning method and device and computer storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115930967A (en) |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20170046873A1 (en) * | 2015-04-14 | 2017-02-16 | ETAK Systems, LLC | Systems and methods for obtaining accurate 3d modeling data using uavs for cell sites |
| US20190156507A1 (en) * | 2016-10-10 | 2019-05-23 | Tencent Technology (Shenzhen) Company Limited | Method and apparatus for processing point cloud data and storage medium |
| CN111598916A (en) * | 2020-05-19 | 2020-08-28 | 金华航大北斗应用技术有限公司 | Preparation method of indoor occupancy grid map based on RGB-D information |
| CN111829531A (en) * | 2019-04-15 | 2020-10-27 | 北京京东尚科信息技术有限公司 | Two-dimensional map construction method and device, robot positioning system and storage medium |
| CN112484726A (en) * | 2020-09-30 | 2021-03-12 | 天津基点科技有限公司 | Unmanned aerial vehicle path planning method based on three-dimensional model |
| CN114299029A (en) * | 2021-12-29 | 2022-04-08 | 浙江大华技术股份有限公司 | Ground point cloud determining method and device, storage medium and electronic device |
| CN114353779A (en) * | 2021-09-30 | 2022-04-15 | 南京晨光集团有限责任公司 | Method for rapidly updating local cost map of robot by point cloud projection |
| CN114527753A (en) * | 2022-01-25 | 2022-05-24 | 中南大学 | Man-machine integrated building path planning method, computer device and program product |
| CN115406457A (en) * | 2022-08-30 | 2022-11-29 | 重庆长安汽车股份有限公司 | Driving region detection method, system, equipment and storage medium |
-
2023
- 2023-01-03 CN CN202310001389.0A patent/CN115930967A/en active Pending
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20170046873A1 (en) * | 2015-04-14 | 2017-02-16 | ETAK Systems, LLC | Systems and methods for obtaining accurate 3d modeling data using uavs for cell sites |
| US20190156507A1 (en) * | 2016-10-10 | 2019-05-23 | Tencent Technology (Shenzhen) Company Limited | Method and apparatus for processing point cloud data and storage medium |
| CN111829531A (en) * | 2019-04-15 | 2020-10-27 | 北京京东尚科信息技术有限公司 | Two-dimensional map construction method and device, robot positioning system and storage medium |
| CN111598916A (en) * | 2020-05-19 | 2020-08-28 | 金华航大北斗应用技术有限公司 | Preparation method of indoor occupancy grid map based on RGB-D information |
| CN112484726A (en) * | 2020-09-30 | 2021-03-12 | 天津基点科技有限公司 | Unmanned aerial vehicle path planning method based on three-dimensional model |
| CN114353779A (en) * | 2021-09-30 | 2022-04-15 | 南京晨光集团有限责任公司 | Method for rapidly updating local cost map of robot by point cloud projection |
| CN114299029A (en) * | 2021-12-29 | 2022-04-08 | 浙江大华技术股份有限公司 | Ground point cloud determining method and device, storage medium and electronic device |
| CN114527753A (en) * | 2022-01-25 | 2022-05-24 | 中南大学 | Man-machine integrated building path planning method, computer device and program product |
| CN115406457A (en) * | 2022-08-30 | 2022-11-29 | 重庆长安汽车股份有限公司 | Driving region detection method, system, equipment and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11199412B2 (en) | Collaborative creation of indoor maps | |
| Dong et al. | ViNav: A vision-based indoor navigation system for smartphones | |
| CN108700421B (en) | Method and system for assisting enhanced portable navigation using offline map information | |
| US10545026B1 (en) | System and method for tracking the position of a person and providing navigation assistance | |
| JP6783751B2 (en) | Methods and equipment to use portable navigation with improved quality of map information assistance | |
| US10547976B2 (en) | Method and system for assigning point of sale information | |
| CN111123198B (en) | User positioning and navigation method and system in building | |
| CN105737826B (en) | Pedestrian's indoor orientation method | |
| US20190170521A1 (en) | Method and system for fingerprinting survey | |
| KR101762507B1 (en) | Method and system for providing indoor and outdoor integrated positioning | |
| US20160286351A1 (en) | Indoor navigation anomaly detection | |
| TWI661210B (en) | Method and apparatus for establishing coordinate system and data structure product | |
| Fichtner et al. | Semantic enrichment of octree structured point clouds for multi‐story 3D pathfinding | |
| Ascher et al. | Multi-floor map matching in indoor environments for mobile platforms | |
| US20170089709A1 (en) | Polygonal routing | |
| US20160298969A1 (en) | Graceful sensor domain reliance transition for indoor navigation | |
| Heiniz et al. | Landmark-based navigation in complex buildings | |
| Chang et al. | Multimodal information integration for indoor navigation using a smartphone | |
| Lin et al. | Automating the generation of indoor space topology for 3D route planning using BIM and 3D-GIS techniques | |
| CN112987720A (en) | Multi-scale map construction method and construction device for mobile robot | |
| CN115930967A (en) | Path planning method and device and computer storage medium | |
| CN112015187A (en) | Semantic map construction method and system for intelligent mobile robot | |
| WO2023193424A1 (en) | Global navigation method for mobile robot in man-machine coexistence environment following pedestrian norm | |
| KR20170086456A (en) | Method and system for providing indoor and outdoor integrated positioning | |
| Shoushtari et al. | 3d indoor localization using 5g-based particle filtering and cad plans |
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 | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20230407 |
|
| RJ01 | Rejection of invention patent application after publication |