CN115993817A - Autonomous exploration method, device and medium for tensor field driven hierarchical path planning - Google Patents
Autonomous exploration method, device and medium for tensor field driven hierarchical path planning Download PDFInfo
- Publication number
- CN115993817A CN115993817A CN202211230385.1A CN202211230385A CN115993817A CN 115993817 A CN115993817 A CN 115993817A CN 202211230385 A CN202211230385 A CN 202211230385A CN 115993817 A CN115993817 A CN 115993817A
- Authority
- CN
- China
- Prior art keywords
- point
- degradation
- points
- boundary
- tensor field
- 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
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Abstract
The invention discloses an autonomous exploration method, device and medium for tensor field driving hierarchical path planning, which comprises the steps of constructing a scene tensor field according to depth point cloud information and acquiring a degradation point set P; solving on-line travel business problem based on degradation point set P to obtain degradation point sequence Q d The method comprises the steps of carrying out a first treatment on the surface of the Grouping the boundary sets F of the unexplored space to obtain a boundary grouping set Q F Splitting and filtering boundaries outside the room; grouping set Q by boundary F Determining the next boundary point to be accessed, calculating a global path T through an A-algorithm, and influencing and updating the tensor field by using the global path T; and calculating the linear speed and the angular speed of the robot along the particle advection of the tensor field, and guiding the robot to reach the target. The invention can eliminate a great amount of time-consuming recalculation of the paths, is suitable for robot exploration of complex indoor scenes, and can greatly improve the efficiency of autonomous exploration while guaranteeing the exploration completeness.
Description
Technical Field
The invention relates to the technical field of path planning of robots, in particular to an autonomous exploration method, device and medium for tensor field driven hierarchical path planning.
Background
Autonomous mobile robots are an important research topic of robotics and have been widely used in the fields of industrial and agricultural production, transportation, services, medical treatment, military and the like. With the complexity of application environments, human beings have put higher demands on the autonomy and intelligence of mobile robots, and especially under unknown complex environments, autonomous exploration of robots has become a difficult and important problem. Several methods for autonomous exploration by robots are currently common:
1) LiDAR-based methods: liDAR sensors can provide a 360 degree field of view (FOV) for a robot, but are relatively expensive. Moreover, most ground robots do autonomous exploration requiring expensive LiDARs to navigate and explore, since RGB-D cameras only provide one view of the environment, rather than 360 degree coverage as LiDARs, which makes autonomous exploration based on RGB-D cameras inherently a natural complexity and presents a series of problems that would normally fail if these methods were used directly on robots with RGB-D.
2) RGB-D based method: with the widespread use of RGB-D cameras such as RealSense and Kinect for robots, the perception of the environment by the robots can be achieved at low cost. Compared with LiDAR, RGB-D can be miniaturized, light, and cost is lower. But the RGB-D camera has the following problems: route detour problem: the robot based on RGB-D can only sense the environmental information of a certain azimuth at the same time, if the route planning is unreasonable, the robot is very easy to turn back in a large range, and the exploration efficiency is low. Because RGB-D is smaller than LiDAR field of view, robot local obstacle avoidance is more difficult, especially in complex scene or narrow space, very easily takes place the collision. Therefore, the problems of small perception range, low efficiency and easy collision caused by the limitation of the FOV exist, most of the current RGB-D-based methods cannot overcome the defects of the RGB-D-based methods in the perception range, so that frequent steering and detouring are required to ensure the integrity of the exploration scene, and the exploration efficiency is generally low.
3) Boundary-based exploration method: the robot is guided to explore by the boundary between free space and unexplored space. It can help the robot to explore more completely. However, these methods tend to result in erroneous decisions due to lack of global optimization, greedy nature. Generally, the boundary-based exploration method prefers to approach the boundary of the robot as the next target, regardless of the subsequent actions. Because of greedy strategies, overlong tracks can be generated, particularly in complex indoor scenes, multiple times of entering and exiting the same room can often occur, and route roundabout is caused, so that exploration efficiency is affected.
On this basis, XU et al (Xu K, zheng L, yan Z, yan G, zhang E, niessner M, deussen O, cohen-Or D, huang H.Autonomos reconstruction of unknown indoor scenes guided by time-varyying tensor fields. ACM Transactions On Graphics (TOG), 2017,36 (6): 1-15) disclose tensor field methods for smooth scene reconstruction that treat the topological skeleton of the tensor field as an undirected graph, containing all the degradation points and separation lines connecting them. The time-varying tensor field can be updated in real time to directly guide the robot to move. It proves that the topology of the tensor field is sufficient to achieve efficient global path routing in the partially reconstructed scene, although the FOV of RGB-D is limited. However, the method has poor effect when the scene is large in scale and complex in structure, and is easy to cause incomplete final scene exploration because the routes on static degradation points in the tensor field cannot support global path optimization.
Disclosure of Invention
The invention aims to solve the technical problems: aiming at the problems in the prior art, the invention provides an autonomous exploration method, device and medium for tensor field driven hierarchical path planning, which is based on the improvement of a robot navigation frame based on tensor fields, can better utilize a local topological structure to perform global path optimization, can eliminate a large amount of time-consuming recalculation of paths, is suitable for exploration of robots in complex indoor scenes, and can greatly improve the efficiency of autonomous exploration while guaranteeing the exploration completeness.
In order to solve the technical problems, the invention adopts the following technical scheme:
an autonomous exploration method of tensor field driven hierarchical path planning, comprising:
s101, constructing a scene tensor field according to depth point cloud information perceived by an input RGB-D camera, and detecting degradation points on the field Jing Zhangliang field to obtain a degradation point set P;
s102, solving the online travel business problem based on the degradation point set P to obtain a degradation point sequence Q corresponding to the optimal access sequence d ；
S103, depending on degradation point sequence Q d Grouping the boundary sets F of the unexplored space to obtain a boundary grouping set Q F ；
S104, for boundary group set Q F Is to cross the boundary group F of the room k Splitting the degradation points to obtain boundaries outside the room, filtering the boundaries outside the room, regarding the boundaries as new degradation points, and inserting the new degradation points into a degradation point set P;
s105, according to the boundary group set Q F Determining the next boundary point to be accessed, calculating a global path T through an A-algorithm, and influencing and updating the tensor field by using the global path T;
s106, calculating the linear speed and the angular speed of the robot along the particle advection of the tensor field, and guiding the robot to reach the target.
Optionally, the degradation point set P obtained by detecting degradation points on the field Jing Zhangliang field in step S101 is obtained by combining two parts of the degradation point set obtained on the field Jing Zhangliang field and the extended degradation point set obtained by expanding the scene coverage fully.
Optionally, step S102 includes:
s201, finding the position c from the robot in the degradation point set r The nearest degradation point p 0 ；
S202, at the position c away from the robot r The nearest degradation point p 0 On the basis of the above, solving the online travel business problem shown in the following formula based on the degradation point set P to obtain a degradation point sequence Q corresponding to the optimal access sequence d ：
In the above-mentioned method, the step of,
representing on-line travel business questions, p 0 For leaving the robot position c r Recent degenerated spots, < >>
For degenerate point sequence Q d Is the first degradation point of +.>
For degenerate point sequence Q d I-th degradation point of>
For degenerate point sequence Q d In the (i+1) th degenerate point, dist is the path length between two degenerate points, and n is the degenerate point sequence Q d Is the number of degenerated points of (1), wherein degenerated points +.>
And +.>
All from the degenerate point set P.
Optionally, step S103 includes:
s301, aiming at any boundary point F in the boundary set F m Respectively calculating boundary points f m And degenerate point sequence Q d Any point of degradation in (a)
And determining the nearest degradation point of Euclidean distance +.>
S302, based on the grouping function of the following formula, the boundary point f m Grouping to the point of degradation
Corresponding packet F k ；
In the above-mentioned method, the step of,
as a grouping function, d f Is a threshold value, f m As boundary point, minimum distance d k (f m ) The functional expression of (2) is:
in the above-mentioned method, the step of,
is equal to the boundary point f m The degradation point with the nearest Euclidean distance, k is the set +.>
Element in->
Representing a sequence of degenerate points Q d Index sets in (a);
s303, a boundary point f satisfying the following formula m Adding a degradation point set P:
|f m -d k (f m )|>d f ，
in the above, d f Is a threshold value.
Optionally, in step S104, the boundary outside the room is obtained as { F } k -S(F k ) And S (F) k ) Is a splitting function, and has:
S(F k )＝{f k |f k ∈F k ,ν≥d g ∨(μ<d p ∧ν<d g )}，
in the above, f k For boundary packet F k In (c), v is the position c of the robot r And packet F k The distance between the two points mu is the boundary point f k Path length with robot, d p And d g The threshold value, V represents or operates, and V represents and operates.
Optionally, affecting and updating the tensor field with the global path T in step S105 includes:
s401, projecting grid cells in the input grid map onto the floor, and performing a specified distance d on the center of the projected boundary point s Selecting constraint points of a group of planes, sorting the constraint points according to the path direction to obtain a constraint point set Pc, and projecting a global path T to the constraint point set Pc;
s402, respectively aiming at any two adjacent constraint points p in the constraint point set Pc c-1 (x c-1 ,y c-1 ) And p c (x c ,y c ) Calculating a constraint point p c (x c ,y c ) Is a conventional element (S) c ,T c ) Wherein S is c To influence the x-direction vector of the normal element of the field, T c A y-direction vector that is a conventional element affecting the field;
s403, based on arbitrary constraint point p c (x c ,y c ) Is a conventional element (S) c ,T c ) Calculating a constraint point p c (x c ,y c ) The basis tensor field T of (2) c (p)；
S404, a basis tensor field T of all constraint points through Gaussian radial basis functions c (p) summing to obtain the final tensor field.
Optionally, the constraint point p in step S402 c (x c ,y c ) Is a conventional element (S) c ,T c ) The expression of the calculation function of (c) is:
S c ＝x c -x c-1 ，
T c ＝y c -y c-1 ，
in the above, (x) c ,y c ) To restrictPoint p c (x c ,y c ) Plane coordinates of (x) c-1 ,y c-1 ) As constraint point p c-1 (x c-1 ,y c-1 ) Is a plane coordinate of (c).
Optionally, the function expression for summing the resulting tensor field in step S404 is:
in the above formula, T (p) is the final tensor field, T c (p) is each constraint point p c (x c ,y c ) The basis tensor field T of (2) c (p), d is an attenuation coefficient, sigma is a Gaussian bandwidth, p is a passable point on a robot motion plane, p c Is a constraint point.
In addition, the invention also provides an autonomous exploration device for tensor field driven hierarchical path planning, which comprises a microprocessor and a memory which are connected with each other, wherein the microprocessor is programmed or configured to execute the autonomous exploration method for tensor field driven hierarchical path planning.
Furthermore, the present invention provides a computer readable storage medium having stored therein a computer program for being programmed or configured by a microprocessor to perform an autonomous exploration method of the tensor field driven hierarchical path planning.
Compared with the prior art, the invention has the following advantages:
1. the equipment cost is low: when the method is adopted, the robot only needs to use the robot provided with the RGB-D camera for navigation and exploration, and compared with a scheme based on Lidar, the method has lower cost.
2. Global perceptibility is strong: the method only uses the RGB-D camera to perform positioning, navigation and exploration, overcomes the limitation of a visual field (Fov), and enhances the global perception capability of the robot.
3. The exploration efficiency is high: the method of the invention adopts the dynamically expanded degradation points to provide good reference for forming the optimized scene structure topology, and enhances the global perception capability of the robot, thereby improving the efficiency. At the same time, a boundary packet-based exploration strategy was introduced to perform fine-level local scanning. The two strategies jointly form a hierarchical exploration method, global path optimization can be better carried out by utilizing a local topological structure, a large amount of time-consuming recalculation of paths can be eliminated, the method is suitable for robot exploration of complex indoor scenes, and the efficiency of autonomous exploration can be greatly improved while the exploration completeness is ensured.
Drawings
FIG. 1 is a schematic diagram of a basic flow of a method according to an embodiment of the present invention.
Fig. 2 is a schematic diagram of a degenerate point set P obtained in an embodiment of the present invention.
Fig. 3 is a schematic view of an access sequence of a degenerate point set according to an embodiment of the present invention.
Fig. 4 is a schematic diagram of a boundary set (yellow triangle) in an embodiment of the present invention.
Fig. 5 is a schematic diagram showing a comparison of no packet splitting (a) and no packet splitting (b) in the embodiment of the present invention.
Fig. 6 is a schematic diagram of calculating a global path by an a-algorithm according to an embodiment of the present invention.
Fig. 7 is a schematic diagram of a particle advection planned path along a tensor field in an embodiment of the present invention.
FIG. 8 is a comparative schematic diagram of the exploration path with and without the method of the present invention.
Fig. 9 is a schematic diagram of paths obtained under different scenarios by the method of the present embodiment.
Detailed Description
As shown in fig. 1, the present embodiment provides an autonomous exploration method for tensor field driven hierarchical path planning, including:
s101, constructing a scene tensor field according to depth point cloud information perceived by an input RGB-D camera, and detecting degradation points on the field Jing Zhangliang field to obtain a degradation point set P; the degradation points in the tensor field are used as anchor points, and rough topology is generated by solving the online TSP problem, so that the effectiveness of the tensor field driving method in a complex environment is remarkably improved;
s102, solving the online travel business problem based on the degradation point set P to obtain a degradation point sequence Q corresponding to the optimal access sequence d ；
S103, depending on degradation point sequence Q d Grouping the boundary sets F of the unexplored space to obtain a boundary grouping set Q F The method comprises the steps of carrying out a first treatment on the surface of the Dynamic grouping depending on degradation points can improve exploration efficiency while ensuring scanning integrity;
s104, for boundary group set Q F Is to cross the boundary group F of the room k Splitting the degradation points to obtain boundaries outside the room, filtering the boundaries outside the room, regarding the boundaries as new degradation points, and inserting the new degradation points into a degradation point set P;
s105, according to the boundary group set Q F Determining the next boundary point to be accessed, calculating a global path T through an A-algorithm, and influencing and updating the tensor field by using the global path T;
s106, calculating the linear speed and the angular speed of the robot along the particle advection of the tensor field, and guiding the robot to reach the target. After step S106, steps S101 to S106 may be continuously and circularly performed until the robot scans the entire scene.
In step S101, a scene tensor field is constructed according to depth point cloud information sensed by the input RGB-D camera, and degradation points are detected on the field Jing Zhangliang as in the prior art, which can be found in literature (Xu K, zheng L, yan Z, yan G, zhang E, niess ner M, deissen O, cohen-Or D, huang h.automomous reconstruction of unknown indoor scenes guided by time-varying tensor fields.acm Transactions On Graphics (TOG), 2017,36 (6): 1-15), so its details are not described herein. The tensor field T on the two-dimensional plane D is a smooth tensor value function associated with each point p e D by a two-dimensional tensor T (p), which can be expressed as:
in the above, τ 11 (p)～τ 22 (p) are elements of two-dimensional tensors, respectively, and can be represented by the general formula τ ij And (5) expression.If and only if tau ij ＝τ ji The tensor is symmetrical. The tensors T mentioned in the method of this embodiment are symmetrical and can be uniquely decomposed into an isotropic part S and an anisotropic part a:
in the above formula, lambda is the coefficient of the unit vector corresponding to the homopolar part, mu is the characteristic value of A, theta is the tangential direction angle of a certain point on the projection contour of the obstacle plane, wherein mu is more than 0, and the characteristic value of each anisotropic part A is + -mu.
The anisotropic part A of the point p is A (p), which is equivalent to two orthogonal eigenvector fields E when A (p) noteq0 1 (p) and E 2 (p)：
E 1 (p)＝μ(p)e 1 (p)，
E 2 (p)＝μ(p)e 2 (p)，
In the above formula, μ (p) is a point on the projection contour of the obstacle plane of point p, e 1 (p) and e 2 (p) are unit feature vectors corresponding to the feature values μ and- μ, respectively. Thus E is 1 (p) and E 2 (p) is the primary and secondary eigenvectors of the anisotropic portion A. Principal eigenvector E in the present method 1 (p) guiding the robot. If and only if the anisotropic part a of the point p is a (p) satisfying a (p) =0, the point p is a degenerate point of the tensor field T (p), a coarse topology, i.e. a degenerate point set, can be constructed with the degenerate points. In this embodiment, the degradation point set P obtained by detecting degradation points on the field Jing Zhangliang field in step S101 is obtained by combining two parts, i.e., the degradation point set obtained on the field Jing Zhangliang field (as shown by the green circle in fig. 2) and the extended degradation point set obtained by expanding the scene coverage fully (as shown by the red circle in fig. 2). Assume that the set of degradation points obtained over field Jing Zhangliang field is p=p 0 ,p 1 ,…,p i ,p i E, D, expanding to obtain an expanded degradation point set as P * ＝p 0 ,p 1 ,…,p j And has
If access to the degradation point p i We from P≡P-P i These points are deleted. As shown in fig. 3, where gray circles represent degradation points that have been accessed, green circles represent degradation points that are obtained over the field Jing Zhangliang field, red circles represent extended degradation points, and the numbers in the circles represent the order of degradation points. Thus, the merging of the resulting degenerate point set P can be expressed as:
P←P∪P * ＝p 0 ,p 1 ,…,p n ,n＝i+j，
in the exploration process, a group of degradation points (degradation point set) is obtained from a scene tensor field, in order to fully expand the scene coverage (expand the degradation point set), and then, an optimal degradation point sequence is obtained based on the online TSP of the degradation point dynamic expansion solution. We express the path length between the degradation points i and j as dist (p i ,p j ). Giving a degradation point p 0 ,p 1 ,…,p n On-line traveller's question (TSP question) is defined as finding the sequence pi=pi 1 ,…,π n I=1, …, n, from p 0 Initially, the total access route of the arrangement is minimized. Then the degradation point
Is selected as the next target location. Note that the online travel question (TSP question) will find the best access order and end at any point, instead of returning to the start point, which may prevent the solver of the online travel question (TSP question) from creating an excessively long trajectory. In this embodiment, step S102 solves the online travel business problem (TSP problem) based on the degradation point set P to obtain a degradation point sequence Q corresponding to the optimal access order d The method comprises the steps of carrying out a first treatment on the surface of the On-line traveller's problem (TSP problem) refers to the fact that the traveller's problem (TSP problem) is solved for continuously updated degradation points to maintain their optimal order of access, so that a rough topology for navigation can be dynamically generated. Specifically, step S102 includes:
s201, finding the position c from the robot in the degradation point set r Recent withdrawal ofThe melting point p 0 The method comprises the steps of carrying out a first treatment on the surface of the This strategy may prevent the robot from moving far to begin access.
S202, at the position c away from the robot r The nearest degradation point p 0 On the basis of the above, solving the online travel business problem shown in the following formula based on the degradation point set P to obtain a degradation point sequence Q corresponding to the optimal access sequence d ：
In the above-mentioned method, the step of,
representing on-line travel business questions, p 0 For leaving the robot position c r Recent degenerated spots, < >>
For degenerate point sequence Q d Is the first degradation point of +.>
For degenerate point sequence Q d I-th degradation point of>
For degenerate point sequence Q d In the (i+1) th degenerate point, dist is the path length between two degenerate points, and n is the degenerate point sequence Q d Is the number of degenerated points of (1), wherein degenerated points +.>
And +.>
All from the degenerate point set P. After selecting the starting point, we calculate the distance from the current point to the next point as the distance cost optimized by the objective function. The euclidean distance is typically used to calculate the spatial distance. However, when there is an obstacle (e.g., wall, furniture) between the two points, the length of the measurement path is inaccurate.Thus, the present embodiment specifically utilizes A * The algorithm (P.E.Hart, N.J.Nilsson, and B.Raphael. A formal basis for the heuristic determination of minimum cost paths in graphs IEEE Trans. Syst. Sci. And Cybernetics, SSC-4 (2): 100-107, 1968) performs the calculation of the path length dist, thereby ensuring that the whole access sequence path is shortest. It is noted that A is adopted * The algorithm calculates the path length between two points as the existing method, and in this embodiment, only the application of the method is involved, and no improvement of the method is involved, so its details will not be described here.
To compute exploration, our method scans the scene at a fine granularity level based on a coarse topology. We have planned an optimal access topology based on degradation points and can explore along this topology. Meanwhile, in order to cover the scene completely, a boundary-based mode is introduced, the boundary is between free space and unexplored space, and the boundary in the map is eliminated, which means that the scene scanning is complete. Therefore, we dynamically group the boundaries around the degradation points. The key idea is that the global path generated by the topological skeleton of the degradation point covers the whole scene, providing a reasonable basis for boundary grouping. Furthermore, the efficiency of exploration is improved by accessing groups rather than individual boundaries.
As shown in fig. 4, step S103 is for obtaining a degradation point sequence Q based on d (numbered circles) grouping the boundary sets F (yellow triangles) to obtain a boundary grouping sequence Q F (boundaries corresponding to each numbered circle). In this embodiment, step S103 includes:
s301, aiming at any boundary point F in the boundary set F m Respectively calculating boundary points f m And degenerate point sequence Q d Any point of degradation in (a)
And determining the nearest degradation point of Euclidean distance +.>
S302, based on the grouping function of the following formula, the boundary point f m Grouping to the point of degradation
Corresponding packet F k (can be expressed as F k ←F k ∪f m )；
In the above-mentioned method, the step of,
as a grouping function, d f For the threshold value (which can be valued according to actual needs, for example, the specific value is 2m in the embodiment), f m As boundary point, minimum distance d k (f m ) The functional expression of (2) is:
in the above-mentioned method, the step of,
is equal to the boundary point f m The degradation point with the nearest Euclidean distance, k is the set +.>
Element in->
Representing a sequence of degenerate points Q d Index sets in (a); />
S303, a boundary point f satisfying the following formula m Adding a set of degenerate points P (which may be expressed as P+.P ≡f) m )：
|f m -d k (f m )|>d f ，
In the above, d f Is a threshold value.
After groupingWe scan each packet F along the degradation point k . During the scanning process, if boundary f k ∈F k Is accessed from packet F k Delete (F) k ←F k -f k ). When packet F k When in space, the degradation point
Is also considered to have accessed and deleted +.>
We then group the sequence Q by boundary F Find the next degradation point and scan the corresponding group.
However, it is not sufficient to merely group the boundaries, because an obstacle (e.g., a wall) is sometimes located between two boundaries of the same group, and the robot makes a long-distance detour when it goes to both boundaries. As shown in fig. 5 (a), when the robot is in the upper left corner, the room explores the boundary group F i At the time, go out scan F i The remaining boundaries (blue trace) and return to the room for access to the next group F j (pink trace) is not sensible. To solve this problem, we would split dynamically as the group spans two rooms. Step S104 for boundary packet set Q F Is to cross the boundary group F of the room k Splitting the degradation points to obtain boundaries outside the room, filtering the boundaries outside the room, regarding the boundaries as new degradation points, and inserting the new degradation points into a degradation point set P; specifically, in this embodiment, in step S104, the boundary outside the room obtained by splitting is { F k -S(F k ) And S (F) k ) Is a splitting function, and has:
S(F k )＝{f k |f k ∈F k ,ν≥d g ∨(μ<d p ∧ν<d g )}，
in the above, f k For boundary packet F k In (c), v is the position c of the robot r And packet F k The distance between the two points mu is the boundary point f k Path length with robot, d p And d g The threshold value, V represents or operates, and V represents and operates.
Wherein the robot is at position c r And packet F k The calculation function expression of the distance v between the two is as follows:
ν＝||c r ,p k ||，
ounuo, c r Indicating the position of the robot, p k Representing packet F k Is the position c of the robot r And packet F k The position p of the corresponding degradation point of (2) k And euclidean distance measurement between. f (f) k ∈F k The path length with the robot is μ=dist (c r ,f k ) Similar to the foregoing, can be made from the existing A * And (5) calculating by an algorithm. As shown in fig. 5 (b), after grouping (green circle), the planned trajectory is shorter, which reduces the exploration time.
In the present embodiment, step S105 is used for grouping the sets Q according to the boundary F The next boundary point to be accessed is determined, a global path T is calculated through an A-algorithm, and the tensor field is influenced and updated by using the global path T. The process input is boundary packet set Q F The tensor field and the grid map are taken as inputs and the output is the changed tensor field.
Existing methods directly take the path defined by the particles of the field advection as the robot motion path. However, when the scene is complex, the path is often discontinuous and ambiguous. We adopt A * Algorithm (P.E.Hart, N.J.Nilsson, and B.Raphael. A formal basis for the heuristic determination of minimum cost paths in graphs IEEE Trans. Syst. Sci. And Cybernetics, SSC-4 (2): 100-107, 1968) to find the shortest collision-free path between the current location and the next explored target point. As shown in fig. 6, through a * The algorithm calculates a global path (green arrow) where the yellow triangle remains as a boundary point. One fact is that when it faces a large scene, A * The computation is very time consuming.
To reduce the frequency of the algorithm a calculation, we found that the path that can be planned with the algorithm a changed the surroundingsThe robot can update the trajectory path along the new advection as shown in fig. 7, where (a) represents a based on a * The algorithm plans a path but is blocked by obstacles in unexplored areas. (b) The robot moves along the path, the surrounding tensor field changes, and a new advection is provided for obstacle avoidance. The positive feedback between the planned path and the tensor field thus helps to further guide the robot in determining better motion. Thus, the frequency of the algorithm calculation can be significantly reduced.
In this embodiment, affecting and updating the tensor field with the global path T in step S105 includes:
s401, projecting grid cells in the input grid map onto the floor, and performing a specified distance d on the center of the projected boundary point s (the value may be taken according to practical needs, for example, d in the present embodiment) s =0.2m) to select a set of planar constraint points and sort them by path direction to obtain a constraint point set Pc, and project a global path T to the constraint point set Pc;
s402, respectively aiming at any two adjacent constraint points p in the constraint point set Pc c-1 (x c-1 ,y c-1 ) And p c (x c ,y c ) Calculating a constraint point p c (x c ,y c ) Is a conventional element (S) c ,T c ) Wherein S is c To influence the x-direction vector of the normal element of the field, T c A y-direction vector that is a conventional element affecting the field;
s403, based on arbitrary constraint point p c (x c ,y c ) Is a conventional element (S) c ,T c ) Calculating a constraint point p c (x c ,y c ) The basis tensor field T of (2) c (p)；
S404, a basis tensor field T of all constraint points through Gaussian radial basis functions c (p) summing to obtain the final tensor field.
In this embodiment, the constraint point p in step S402 c (x c ,y c ) Is a conventional element (S) c ,T c ) The expression of the calculation function of (c) is:
S c ＝x c -x c-1 ，
T c ＝y c -y c-1 ，
in the above, (x) c ,y c ) As constraint point p c (x c ,y c ) Plane coordinates of (x) c-1 ,y c-1 ) As constraint point p c-1 (x c-1 ,y c-1 ) Is a plane coordinate of (c).
In this embodiment, the function expression for summing up the final tensor field in step S404 is:
in the above formula, T (p) is the final tensor field, T c (p) is each constraint point p c (x c ,y c ) The basis tensor field T of (2) c (p), d is an attenuation coefficient, sigma is a Gaussian bandwidth, p is a passable point on a robot motion plane, p c Is a constraint point. The global path can influence the field updating process, and the degradation point of the path can disappear, so that the problem of discontinuity or blurring is solved. Furthermore, when the obstacle blocks the global path, the robot can advect particles along the field without having to perform the algorithm calculation again. In this embodiment, the damping constant d=1. The gaussian bandwidth σ can be used to control the range of influence of the fundamental field, with the value set to σ=2.5ds.
In order to verify the method of the present embodiment, the autonomous search method using the tensor field driven hierarchical path plan of the present embodiment and the autonomous search method not using the tensor field driven hierarchical path plan are compared, respectively, as shown in fig. 8, where (a) is a search path of the autonomous search method using the tensor field driven hierarchical path plan of the present embodiment, and (b) is a search path of the autonomous search method not using the tensor field driven hierarchical path plan of the present embodiment. The autonomous exploration method of tensor field driven hierarchical path planning of the embodiment provides a rough topological structure (boundary grouping set Q) matched with a scene structure for a robot F ) For the robot to navigate and provide fine granularity level scanning (split sub-step SGroup acquisition). In the case where the integrity of the scan scene is similar, the path in (a) of fig. 8 is 52.734m, but the path in (b) of fig. 8 is 105.282m, which doubles the search efficiency. In addition, the paths obtained by adopting the autonomous exploration method for the tensor field driven hierarchical path planning in the embodiment to autonomously explore different scenes are shown in fig. 9, and tests show that the autonomous exploration method for the tensor field driven hierarchical path planning in the embodiment realizes impressive high-quality scanning scene coverage and provides reasonable planning paths for all the scenes. Compared with other methods based on RGB-D cameras, the autonomous exploration method of the tensor field driven hierarchical path planning of the embodiment obtains more complete scanning results than the autonomous exploration method, which benefits from the hierarchical exploration of the tensor field driven hierarchical path planning.
In summary, the present embodiment method utilizes a boundary and tensor field drive based method. In order to further release the global optimization capability of the tensor field, sparse degradation points are adopted as anchor points, so that the local topological structure is better utilized for global path optimization. In particular, we have found that points of degradation are typically created at connection points of the structure, such as entrances and galleries. Since these anchors are dynamically generated during the exploration process, we represent them as extended nodes in the traveler problem (TSP) to measure the optimal path to form a coarse topology, which can help the robot avoid traveling during long-distance cyclic exploration. Furthermore, a boundary packet based scanning strategy is proposed for fine-grained level exploration. We dynamically group the boundaries around the anchor point and generate a local fine-level exploration path for a complete scan. Therefore, a hierarchical exploration strategy is introduced to maintain exploration efficiency while guaranteeing scan integrity. In addition, the tensor field of the method of the present embodiment also supports a movement strategy to avoid collisions based on particle advection. A is that * The algorithm is the most common method to find the shortest collision-free path to the next explore target point. However, in case of large environments, it is time consuming. The prior work based on LiDAR scanning adopts the technologies such as offline searching strategy and the like to search local motion paths, and has high efficiency. However, it is highly dependent onLiDAR provides a complete FOV. In contrast, our framework can eliminate a large, time-consuming recalculation by tensor fields, which can be updated in real-time and do not require FOV. Thus, the depth scan movement strategy of the present embodiment method based on tensor fields may be similar to or even better than the latest alternatives to LiDAR scanning. The method of the embodiment adopts the degradation points in the tensor field as anchor points, and generates rough topology by solving the online TSP problem, which remarkably improves the effectiveness of the tensor field driving method in a complex environment; the method of the embodiment is based on a scanning strategy of the front-edge grouping and is used for exploring the fine granularity level. The dynamic grouping strategy depending on the degradation points can improve the exploration efficiency while guaranteeing the scanning integrity; the method of the embodiment provides good reference for forming the optimized scene structure topology by adopting the dynamically expanded degradation points, and enhances the global perception capability of the robot, thereby improving the efficiency. At the same time, a boundary packet-based exploration strategy was introduced to perform fine-level local scanning. The two strategies jointly form a hierarchical exploration method, global path optimization can be better carried out by utilizing a local topological structure, a large amount of time-consuming recalculation of paths can be eliminated, the method is suitable for robot exploration of complex indoor scenes, and the efficiency of autonomous exploration can be greatly improved while the exploration completeness is ensured.
In addition, the embodiment also provides an autonomous exploration device for tensor field driving hierarchical path planning, which comprises a microprocessor and a memory which are connected with each other, wherein the microprocessor is programmed or configured to execute the autonomous exploration method for tensor field driving hierarchical path planning. Furthermore, the present embodiment also provides a computer readable storage medium having stored therein a computer program for being programmed or configured by a microprocessor to perform the autonomous exploration method of tensor field driven hierarchical path planning.
It will be appreciated by those skilled in the art that embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-readable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein. The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems) and computer program products according to embodiments of the application. It will be understood that each flow and/or block of the flowchart illustrations and/or block diagrams, and combinations of flows and/or blocks in the flowchart illustrations and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks. These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks. These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
The above description is only a preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above examples, and all technical solutions belonging to the concept of the present invention belong to the protection scope of the present invention. It should be noted that modifications and adaptations to the present invention may occur to one skilled in the art without departing from the principles of the present invention and are intended to be within the scope of the present invention.
Claims (10)
1. An autonomous exploration method for tensor field driven hierarchical path planning, comprising:
s101, constructing a scene tensor field according to depth point cloud information perceived by an input RGB-D camera, and detecting degradation points on the field Jing Zhangliang field to obtain a degradation point set P;
s102, solving the online travel business problem based on the degradation point set P to obtain a degradation point sequence Q corresponding to the optimal access sequence d ；
S103, depending on degradation point sequence Q d Grouping the boundary sets F of the unexplored space to obtain a boundary grouping set Q F ；
S104, for boundary group set Q F Is to cross the boundary group F of the room k Splitting the degradation points to obtain boundaries outside the room, filtering the boundaries outside the room, regarding the boundaries as new degradation points, and inserting the new degradation points into a degradation point set P;
s105, according to the boundary group set Q F Determining the next boundary point to be accessed, calculating a global path T through an A-algorithm, and influencing and updating the tensor field by using the global path T;
s106, calculating the linear speed and the angular speed of the robot along the particle advection of the tensor field, and guiding the robot to reach the target.
2. The autonomous exploration method of tensor field driven hierarchical path planning of claim 1, wherein the set of degenerate points P obtained by detecting degenerate points on field Jing Zhangliang in step S101 is obtained by combining two parts of a set of degenerate points obtained on field Jing Zhangliang and an extended set of degenerate points obtained by expanding the scene coverage by fully performing degenerate points.
3. The autonomous exploration method of tensor field driven hierarchical path planning of claim 1, wherein step S102 comprises:
s201, finding the position away from the robot in the degradation point setc r The nearest degradation point p 0 ；
S202, at the position c away from the robot r The nearest degradation point p 0 On the basis of the above, solving the online travel business problem shown in the following formula based on the degradation point set P to obtain a degradation point sequence Q corresponding to the optimal access sequence d ：
In the above-mentioned method, the step of,
representing on-line travel business questions, p 0 For leaving the robot position c r Recent degenerated spots, < >>
For degenerate point sequence Q d Is the first degradation point of +.>
For degenerate point sequence Q d I-th degradation point of>
For degenerate point sequence Q d In the (i+1) th degenerate point, dist is the path length between two degenerate points, and n is the degenerate point sequence Q d Is the number of degenerated points of (1), wherein degenerated points +.>
And +.>
All from the degenerate point set P.
4. The autonomous exploration method of tensor field driven hierarchical path planning of claim 1, comprising in step S103:
s301, aiming at any boundary point F in the boundary set F m Respectively calculating boundary points f m And degenerate point sequence Q d Any point of degradation in (a)
And determining the nearest degradation point of Euclidean distance +.>
S302, based on the grouping function of the following formula, the boundary point f m Grouping to the point of degradation
Corresponding packet F k ；
In the above-mentioned method, the step of,
as a grouping function, d f Is a threshold value, f m As boundary point, minimum distance d k (f m ) The functional expression of (2) is:
in the above-mentioned method, the step of,
is equal to the boundary point f m The degradation point with the nearest Euclidean distance, k is the set +.>
Element in->
Representing a sequence of degenerate points Q d Index sets in (a);
s303, a boundary point f satisfying the following formula m Adding a degradation point set P:
|f m -d k (f m )|>d f ，
in the above, d f Is a threshold value.
5. The autonomous exploration method of tensor field driven hierarchical path planning of claim 1, wherein said splitting in step S104 yields { F k -S(F k ) And S (F) k ) Is a splitting function, and has:
S(F k )＝{f k |f k ∈F k ,ν≥d g ∨(μ<d p ∧ν<d g )}，
in the above, f k For boundary packet F k In (c), v is the position c of the robot r And packet F k The distance between the two points mu is the boundary point f k Path length with robot, d p And d g The threshold value, V represents or operates, and V represents and operates.
6. The autonomous exploration method of a tensor field driven hierarchical path planning of claim 1, wherein affecting and updating the tensor field with the global path T in step S105 comprises:
s401, projecting grid cells in the input grid map onto the floor, and performing a specified distance d on the center of the projected boundary point s Selecting constraint points of a group of planes, sorting the constraint points according to the path direction to obtain a constraint point set Pc, and projecting a global path T to the constraint point set Pc;
s402, respectively aiming at any two adjacent constraint points p in the constraint point set Pc c-1 (x c-1 ,y c-1 ) And p c (x c ,y c ) Calculating constraint pointsp c (x c ,y c ) Is a conventional element (S) c ,T c ) Wherein S is c To influence the x-direction vector of the normal element of the field, T c A y-direction vector that is a conventional element affecting the field;
s403, based on arbitrary constraint point p c (x c ,y c ) Is a conventional element (S) c ,T c ) Calculating a constraint point p c (x c ,y c ) The basis tensor field T of (2) c (p)；
S404, a basis tensor field T of all constraint points through Gaussian radial basis functions c (p) summing to obtain the final tensor field.
7. The autonomous exploration method of tensor field driven hierarchical path planning of claim 6, wherein said constraint point p in step S402 c (x c ,y c ) Is a conventional element (S) c ,T c ) The expression of the calculation function of (c) is:
S c ＝x c -x c-1 ，
T c ＝y c -y c-1 ，
in the above, (x) c ,y c ) As constraint point p c (x c ,y c ) Plane coordinates of (x) c-1 ,y c-1 ) As constraint point p c-1 (x c-1 ,y c-1 ) Is a plane coordinate of (c).
8. The autonomous discovery method of tensor field driven hierarchical path planning of claim 6, characterized by summing in step S404 to obtain a final tensor field having a functional expression:
in the above formula, T (p) is the final tensor field, T c (p) is each constraint point p c (x c ,y c ) The basis tensor field T of (2) c (p), d is the attenuation coefficient, σ is the Gaussian bandwidth, p is the machinePassable points on the plane of human movement, p c Is a constraint point.
9. An autonomous exploration device for tensor field driven hierarchical path planning comprising a microprocessor and a memory connected to each other, characterized in that the microprocessor is programmed or configured to perform the autonomous exploration method for tensor field driven hierarchical path planning according to any one of claims 1 to 8.
10. A computer readable storage medium having a computer program stored therein, wherein the computer program is for programming or configuring by a microprocessor to perform the autonomous exploration method of tensor field driven hierarchical path planning of any of claims 1 to 8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211230385.1A CN115993817A (en) | 2022-09-30 | 2022-09-30 | Autonomous exploration method, device and medium for tensor field driven hierarchical path planning |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211230385.1A CN115993817A (en) | 2022-09-30 | 2022-09-30 | Autonomous exploration method, device and medium for tensor field driven hierarchical path planning |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115993817A true CN115993817A (en) | 2023-04-21 |
Family
ID=85989403
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211230385.1A Pending CN115993817A (en) | 2022-09-30 | 2022-09-30 | Autonomous exploration method, device and medium for tensor field driven hierarchical path planning |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115993817A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117470253A (en) * | 2023-12-28 | 2024-01-30 | 中国人民解放军国防科技大学 | Tensor field-based robot path planning method, device, equipment and medium |
-
2022
- 2022-09-30 CN CN202211230385.1A patent/CN115993817A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117470253A (en) * | 2023-12-28 | 2024-01-30 | 中国人民解放军国防科技大学 | Tensor field-based robot path planning method, device, equipment and medium |
| CN117470253B (en) * | 2023-12-28 | 2024-03-22 | 中国人民解放军国防科技大学 | Tensor field-based robot path planning method, device, equipment and medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107436148B (en) | Robot navigation method and device based on multiple maps | |
| JP5685380B2 (en) | Route generation apparatus using grid map and operation method thereof | |
| US10408630B2 (en) | Method and apparatus for two-stage planning | |
| Liu et al. | An autonomous path planning method for unmanned aerial vehicle based on a tangent intersection and target guidance strategy | |
| Konolige et al. | Navigation in hybrid metric-topological maps | |
| Goto et al. | The CMU system for mobile robot navigation | |
| CN112000754A (en) | Map construction method and device, storage medium and computer equipment | |
| CN106774347A (en) | Robot path planning method, device and robot under indoor dynamic environment | |
| US20030229442A1 (en) | Architecture for real-time maintenance of distributed mission plans | |
| Li et al. | Hybrid filtering framework based robust localization for industrial vehicles | |
| CN114879660B (en) | Robot environment sensing method based on target drive | |
| CN115993817A (en) | Autonomous exploration method, device and medium for tensor field driven hierarchical path planning | |
| Peltzer et al. | Fig-op: Exploring large-scale unknown environments on a fixed time budget | |
| Jeon et al. | An entry-exit path planner for an autonomous tractor in a paddy field | |
| CN113433937B (en) | Hierarchical navigation obstacle avoidance system and hierarchical navigation obstacle avoidance method based on heuristic exploration | |
| Zhao et al. | Complete coverage path planning scheme for autonomous navigation ROS-based robots | |
| Stentz et al. | The CMU navigational architecture | |
| Muravyev et al. | Evaluation of Topological Mapping Methods in Indoor Environments | |
| Vokřínek et al. | Cooperative agent navigation in partially unknown urban environments | |
| KR102529332B1 (en) | Robot-based optimal indoor delivery path planning method with context map | |
| Karakaya et al. | A novel local motion planner: Navibug | |
| US11022446B2 (en) | Method and apparatus for two-stage planning | |
| Mills | Informative View Planning for Autonomous Exploration in Unstructured 3D Environments | |
| Cieslewski et al. | A Representation for Exploration that is Robust to State Estimate Drift | |
| Sasongko et al. | An integrated exploration and observation planning for an efficient indoor 3D mapping |
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 |