CN118131779A - Constrained trajectory planning method, equipment and storage medium for mobile operation robot - Google Patents
Constrained trajectory planning method, equipment and storage medium for mobile operation robot Download PDFInfo
- Publication number
- CN118131779A CN118131779A CN202410552620.XA CN202410552620A CN118131779A CN 118131779 A CN118131779 A CN 118131779A CN 202410552620 A CN202410552620 A CN 202410552620A CN 118131779 A CN118131779 A CN 118131779A
- Authority
- CN
- China
- Prior art keywords
- constraint
- configuration
- trajectory planning
- optimal
- track
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 78
- 238000003860 storage Methods 0.000 title claims abstract description 9
- 238000005070 sampling Methods 0.000 claims abstract description 46
- 238000010276 construction Methods 0.000 claims abstract description 9
- 238000002940 Newton-Raphson method Methods 0.000 claims description 6
- 230000006870 function Effects 0.000 claims description 5
- 238000005457 optimization Methods 0.000 claims description 4
- 230000001133 acceleration Effects 0.000 claims description 3
- 238000005520 cutting process Methods 0.000 claims description 3
- 238000013507 mapping Methods 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 238000012795 verification Methods 0.000 claims description 3
- 238000004590 computer program Methods 0.000 claims description 2
- 239000000126 substance Substances 0.000 description 3
- 238000001514 detection method Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- -1 carrier Substances 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 239000003153 chemical reaction reagent Substances 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 239000000306 component Substances 0.000 description 1
- 230000003111 delayed effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000009472 formulation Methods 0.000 description 1
- 239000012535 impurity Substances 0.000 description 1
- 239000004615 ingredient Substances 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000002035 prolonged effect Effects 0.000 description 1
- 238000007493 shaping process Methods 0.000 description 1
- 239000007858 starting material Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Landscapes
- Manipulator (AREA)
Abstract
The invention discloses a constrained trajectory planning method, equipment and a storage medium for a mobile operation robot, belonging to the field of robot trajectory planning, comprising the following steps: step 1, setting constraint and constructing an approximate graph of a constraint manifold; step 2, reconstructing an operation scene of the mobile operation robot to obtain an operation scene model; step 3, executing an optimal trajectory planning algorithm PRM on the approximation graph obtained in the step 1 and the operation scene model obtained in the step 2 to obtain a roadmap containing an optimal trajectory; step 4, in the operation of the mobile operation robot, performing online track planning on the route map obtained in the step 3 by adopting LazyPRM algorithm to obtain an optimal track; and 5, the mobile operation robot executes the optimal track obtained in the step 4 to finish the operation process. The method can directly obtain the configuration meeting the constraint on the approximate graph by adopting the off-line construction and on-line sampling modes of the constraint manifold approximate graph, reduces the calculated amount and realizes faster track planning speed.
Description
Technical Field
The invention relates to the field of mobile operation robot track planning, in particular to a constrained track planning method of a mobile operation robot based on a manifold approximate probability roadmap.
Background
Mobile manipulator robots often need to perform a series of constrained manipulation tasks safely and efficiently in complex scenarios, such as in chemical laboratories, between different instruments to complete a series of different chemical experiments, inside space stations, such as opening/rotating valves. These scenes are characterized by numerous and dense obstacles, which can make it difficult to find a collision-free trajectory that satisfies the constraints, but they are generally fixed and only subject to movement positioning errors or occasional placement of small objects. Existing constrained trajectory planning methods based on sampling generally include both reject sampling and constrained shaping. The sampling rejection method is simple to realize, but has low efficiency, and especially when the constraint conditions are strict, the sampling rejection is frequently generated, so that the planning time is greatly prolonged. While the constrained manifold approach improves sampling efficiency, collision detection is still required, which introduces a non-negligible time overhead in complex scenarios such as chemical laboratories. The existing track planning method is difficult to meet the requirement of obtaining a track with enough high quality while the planning time is as short as possible.
In view of this, the present invention has been made.
Disclosure of Invention
The invention aims to provide a constrained trajectory planning method, constrained trajectory planning equipment and a storage medium for a mobile operation robot, which can enable the mobile operation robot to obtain a trajectory with high enough and stable quality in a short time, improve the operation efficiency of the robot and further solve the technical problems in the prior art.
The invention aims at realizing the following technical scheme:
A constrained trajectory planning method for a mobile operation robot is used for completing constrained trajectory planning operation of the mobile operation robot in a complex environment, and comprises the following steps:
step 1, setting constraint and constructing an approximate graph of a constraint manifold;
step 2, reconstructing an operation scene of the mobile operation robot to obtain an operation scene model;
Step 3, executing an optimal trajectory planning algorithm PRM on the approximation graph obtained in the step 1 and the operation scene model obtained in the step 2 to obtain a roadmap containing an optimal trajectory;
Step 4, performing online track planning on the roadmap containing the optimal track obtained in the step 3 by adopting LazyPRM algorithm in the operation process of the mobile operation robot to obtain the optimal track;
And 5, the mobile operation robot executes the optimal track obtained in the step 4 to finish the operation process.
Compared with the prior art, the constrained trajectory planning method, the constrained trajectory planning device and the storage medium for the mobile operation robot have the beneficial effects that:
By adopting the off-line construction and on-line sampling modes of the constraint manifold approximation graph, the configuration meeting the constraint can be directly obtained on the approximation graph, the calculated amount is reduced, and the faster track planning speed is realized. Constructing a route map by utilizing an optimal track planning algorithm PRM, and carrying out online track planning by utilizing a LazyPRM algorithm, so that the quality of the track and the consistency of the track obtained by multiple planning are ensured. The method can enable the mobile operation robot to obtain a stable track with high enough quality in a short time, and improves the operation efficiency of the mobile operation robot.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings that are needed in the description of the embodiments will be briefly described below, it being obvious that the drawings in the following description are only some embodiments of the present invention, and that other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
Fig. 1 is a flowchart of a constrained trajectory planning method for a mobile manipulator robot according to an embodiment of the present invention.
Fig. 2 is a schematic diagram of an approximation graph for approximating a constrained manifold in a constrained trajectory planning method according to an embodiment of the present invention.
Fig. 3 is a schematic diagram of a constrained trajectory planning method according to an embodiment of the present invention for representing a measure of a difference between two shapes on a constrained manifold.
Detailed Description
The technical scheme in the embodiment of the invention is clearly and completely described below in combination with the specific content of the invention; it will be apparent that the described embodiments are only some embodiments of the invention, but not all embodiments, which do not constitute limitations of the invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to fall within the scope of the invention.
The terms that may be used herein will first be described as follows:
The term "and/or" is intended to mean that either or both may be implemented, e.g., X and/or Y are intended to include both the cases of "X" or "Y" and the cases of "X and Y".
The terms "comprises," "comprising," "includes," "including," "has," "having" or other similar referents are to be construed to cover a non-exclusive inclusion. For example: including a particular feature (e.g., a starting material, component, ingredient, carrier, formulation, material, dimension, part, means, mechanism, apparatus, step, procedure, method, reaction condition, processing condition, parameter, algorithm, signal, data, product or article of manufacture, etc.), should be construed as including not only a particular feature but also other features known in the art that are not explicitly recited.
The term "consisting of … …" is meant to exclude any technical feature element not explicitly listed. If such term is used in a claim, the term will cause the claim to be closed, such that it does not include technical features other than those specifically listed, except for conventional impurities associated therewith. If the term is intended to appear in only a clause of a claim, it is intended to limit only the elements explicitly recited in that clause, and the elements recited in other clauses are not excluded from the overall claim.
Unless specifically stated or limited otherwise, the terms "mounted," "connected," "secured," and the like should be construed broadly to include, for example: the connecting device can be fixedly connected, detachably connected or integrally connected; can be mechanically or electrically connected; can be directly connected or indirectly connected through an intermediate medium, and can be communication between two elements. The specific meaning of the terms herein above will be understood by those of ordinary skill in the art as the case may be.
The terms "center," "longitudinal," "transverse," "length," "width," "thickness," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," "outer," "clockwise," "counterclockwise," etc. refer to an orientation or positional relationship based on that shown in the drawings, merely for ease of description and to simplify the description, and do not explicitly or implicitly indicate that the apparatus or element in question must have a particular orientation, be constructed and operated in a particular orientation, and therefore should not be construed as limiting the present disclosure.
The constrained trajectory planning method for the mobile manipulator robot provided by the invention is described in detail below. What is not described in detail in the embodiments of the present invention belongs to the prior art known to those skilled in the art. The specific conditions are not noted in the examples of the present invention and are carried out according to the conditions conventional in the art or suggested by the manufacturer. The reagents or apparatus used in the examples of the present invention were conventional products commercially available without the manufacturer's knowledge.
As shown in fig. 1, an embodiment of the present invention provides a constrained trajectory planning method for a mobile manipulator robot, which is used for performing constrained trajectory planning completion operation on the mobile manipulator robot in a complex environment, and includes the following steps:
step 1, setting constraint and constructing an approximate graph of a constraint manifold;
step 2, reconstructing an operation scene of the mobile operation robot to obtain an operation scene model;
Step 3, executing an optimal trajectory planning algorithm PRM on the approximation graph obtained in the step 1 and the operation scene model obtained in the step 2 to obtain a roadmap containing an optimal trajectory;
And 4, performing online track planning on the roadmap containing the optimal track obtained in the step 3 by adopting LazyPRM algorithm in the operation process of the mobile operation robot to obtain the optimal track.
And 5, the mobile operation robot executes the optimal track obtained in the step 4 to finish the operation process.
Preferably, in step 1 of the above method, a point set V and an edge set E are sequentially generated according to a set constraint, and then the constraint is set and an approximate graph of a constraint manifold is constructed according to the following manner, where the approximate graph is shown in fig. 2, and includes:
Step 11, carrying out random uniform sampling in the whole configuration space A to obtain an initial configuration X 0, and then projecting the configuration X 0 onto the constraint manifold X by using an iterative optimization method to obtain a configuration X i;
Step 12, performing difference verification on each bit shape x i obtained in the step 11 and each bit shape x j in the point set V, and adding the bit shape x i meeting the difference index into the point set V;
step 13, repeating the steps 11 and 12 until the positions in the point set V reach the designated number, and completing the construction of the point set V;
Step 14, for the bit shape x k in the set of points V, searching for the nearest ortho shape x n of the bit shape x k in the set of points V using the euclidean distance;
step 15, connecting the position x k in step 14 with the nearest adjacent position x n of the position x k by using a straight line, judging the validity of the connection, and adding the valid connection into the edge set E;
Step 16, repeating the step 14 and the step 15 until the connection of the configuration x k reaches the designated number or all configurations in the point set V are tried to be connected, and completing the construction of the edge set E;
Step 17, using the point set V as the vertex set, each vertex in the vertex set corresponding to a configuration satisfying the constraint F, and using the edge set E as the edge set to construct the undirected graph Using undirected graphsAn approximation graph of the constraint manifold X is shown.
Preferably, the iterative optimization method in step 11 of the above method employs the Newton-Raphson method.
Preferably, in step 12 of the above method, the difference index is:
；
；
；
Wherein ε is a constant, set to 0.1; alpha is a constant and is set to 0.45; ρ is a constant, set to 1; By cutting space/> Log mapping at: /(I)；/>Representation/>Is a transpose of (2); /(I)And/>The tangent space/>, respectively, of the constraint manifold at position x i And bit shape/>Space of department/>Is a group of radicals satisfying:
；
Wherein, Is the jacobian matrix of constraint function F at x; i is a k x k unit array.
Referring to fig. 3, preferably, in step 15 of the above method, the configuration x k and the nearest ortho-configuration x n of the configuration x k in step 14 are connected using a straight line and the validity of the connection is judged, including:
equidistant sampling of straight lines to obtain a plurality of configurations ，/>，/>And the following judgment is carried out:
；
wherein F is a constraint function; is a constant, set to 0.1, when all sampling bits are shaped/> When the above-mentioned conditions are satisfied, the configuration/>, is confirmedAnd/>Connection between/>Is effective.
Preferably, in step 3 of the above method, an optimal trajectory planning algorithm PRM is executed on the approximation graph obtained in step 1 and the operation scene model obtained in step 2 in the following manner, to obtain a roadmap including an optimal trajectory, including:
Sampling by adopting two sampling modes of uniform random sampling and neighbor random sampling by using an optimal trajectory planning algorithm PRM; wherein the uniform random sampling is performed by uniformly random sampling an integer The bit shape obtained by sampling is the bit shape at the ith index position in the approximate graph, and n s is the bit shape number in the point set;
according to whether the bit shape x belongs to the point set V, the neighbor random sampling is divided into two cases:
(21) When (when) When, can sample the collection/>, of the bit shapesExpressed as:
；
Wherein, Representing that there is an active connection between a set of positions x and x n, the set of sampable positions V n is a set of positions x n connected to the set of positions x and having a distance d or less; d is a constant, set to 1;
(22) When (when) Or/>When the method is used, uniform random sampling is carried out in a sphere with the configuration x as the center and d as the radius, and the configuration meeting the constraint is obtained by using the Newton-Raphson method as the sampling configuration.
Preferably, in step 4 of the above method, in the operation process of the mobile operation robot, an online track planning is performed on the roadmap including the optimal track obtained in step 3 by adopting LazyPRM algorithm in the following manner, so as to obtain the optimal track, including:
Step 41, reconstructing the operation scene to obtain a reconstruction model;
Step 42, loading the route map obtained in the step 3 and the reconstruction model obtained in the step 41, and performing track planning by using LazyPRM algorithm to obtain a track;
Step 43, calculating the speed and acceleration of the track according to the constraint of the mobile operation robot for the track obtained in step 42.
Preferably, in step 2 of the above method, the operating scene is reconstructed by using a SLAM method or NeRF method to obtain an operating scene model.
The embodiment of the invention also provides a processing device, which comprises:
At least one memory for storing one or more programs;
at least one processor capable of executing one or more programs stored in the memory, which when executed by the processor, enable the processor to implement the methods of the present invention.
The embodiments of the present invention further provide a readable storage medium storing a computer program which, when executed by a processor, is capable of implementing the method of the present invention.
In summary, the track planning method of the embodiment of the invention can directly obtain the configuration meeting the constraint on the approximate graph by adopting the off-line construction and on-line sampling modes of the constraint manifold approximate graph, thereby reducing the calculated amount and realizing faster track planning speed. And constructing a route map by using PRM, carrying out online track planning by LazyPRM, and ensuring the quality of the track and the consistency of the track obtained by multiple planning. The method can enable the mobile operation robot to obtain a stable track with high enough quality in a short time, and improves the operation efficiency of the mobile operation robot.
In order to clearly demonstrate the technical scheme and the technical effects provided by the invention, the constrained trajectory planning method of the mobile manipulator robot provided by the embodiment of the invention is described in detail below by using specific embodiments.
Example 1
As shown in fig. 1, an embodiment of the present invention provides a constrained trajectory planning method for a mobile operation robot, which is a constrained trajectory planning method for a mobile operation robot based on a manifold approximate probability roadmap, and is used for controlling the mobile operation robot to operate, and includes:
step 1, setting constraint and constructing an approximate graph of a constraint manifold;
Specifically, the step 1 includes the following steps:
Step 11, carrying out random uniform sampling in the whole potential form space A to obtain an initial potential form X 0, and then projecting the potential form X 0 onto the constraint manifold X by using a Newton-Raphson method to obtain a potential form X i;
Step 12, performing difference verification on each of the configuration x i and the configuration x j in the point set V obtained in step 11, as shown in fig. 3, a difference index is:
；
；
；
Wherein ε is a constant, set to 0.1; alpha is a constant and is set to 0.45; ρ is a constant, set to 1; By cutting space/> Log mapping at: /(I)；/>Representation/>Is a transpose of (2); /(I)And/>The tangent space/>, respectively, of the constraint manifold at position x i And bit shape/>Space of department/>Is a group of radicals satisfying:
；
Wherein, Is the jacobian matrix of constraint function F at x; i is a k x k unit array;
If the bit shape x i meets the difference index, adding the bit shape x i into the point set V;
and 13, repeating the steps 11 and 12 until the positions in the point set V reach the specified number, and completing the construction of the point set V.
Step 14, for the position x k in the point set V, searching the nearest position x n in the point set V by using the euclidean distance;
Step 15, connecting the configuration x k in step 14 with the nearest neighbor shape x n by using a straight line, and equidistant sampling the straight line to obtain a plurality of configurations ，/>，/>And the following judgment is carried out:
；
Wherein, Is a small amount only when all sampling configurations/>All satisfy the above formula, the configuration/>And/>Connection between/>Effectively, adding the same into an edge set E;
Step 16, repeating step 14 and step 15 until the configuration is the same All the configurations in the set of points V or the specified number of connections have been tried. This is done for each of the configurations in V, resulting in an edge set E.
Preferably, in the step 1, the undirected graph constructed by using the point set V and the edge set EThe approximation of the constraint manifold X is shown in fig. 2. Where the set of points V is a set of vertices in the approximation graph, each vertex in the set corresponding to a potential shape that satisfies the constraint F. Edge set E is a collection of edges in the approximation graph, edges being possible paths for connecting different vertices, and spanning any edge does not leave the constraint manifold.
And 2, reconstructing the operation scene to obtain an operation scene model. Alternatively, the operational scene may be reconstructed using the SLAM method or NeRF method.
And step 3, executing an optimal trajectory planning algorithm PRM on the approximation graph obtained in the step 1 and the operation scene model obtained in the step 2 to obtain a roadmap containing the optimal trajectory.
In the step3, the PRM algorithm is two sampling modes, i.e. uniform random sampling and neighbor random sampling; wherein, for uniform random sampling, the number of bit shapes in the point set is as followsThus using uniform random sampling of an integerThe method realizes uniform random sampling, and the sampled configuration is the configuration at the ith index position in the approximate graph. For neighbor random sampling, two cases are classified according to whether the bit shape x belongs to the point set V:
(1) When (when) When, can sample the collection/>, of the bit shapesThe expression is as follows:
；
Wherein, Representing that there is an active connection to position form x and position form x n, V n is a collection of position forms x n connected to position form x and at a distance d or less;
(2) When (when) Or/>When the method is used, uniform random sampling is carried out in a sphere with a given shape x as a center and d as a radius, and the shape meeting the constraint is obtained by using the Newton-Raphson method as a sampling shape.
Step 4, in the operation process of the mobile operation robot, performing track planning by adopting LazyPRM algorithm on the basis of the route map obtained in the step 3 to obtain an optimal track;
specifically, the step 4 includes the following steps:
Step 41, reconstructing the operation scene to obtain an operation scene model; optionally, the operating scene model can be reconstructed by using a SLAM method or NeRF method;
Step 42, loading the roadmap obtained in the step 3 and the reconstructed operation scene model obtained in the step 41, and performing track planning by using LazyPRM algorithm to obtain an optimal track;
and 43, calculating the speed and the acceleration of the optimal track obtained in the step 42 according to the constraint of the mobile operation robot.
In the above step 4, the reason why the track planning is performed by using the LazyPRM method is that, in the operation scene of the mobile operation robot, the scene when the roadmap is generated by using the step 3 and the scene where the step 4 is located have a certain difference in consideration of factors such as artificial interference or mobile positioning errors of the robot, and since the connection between all the positions in the roadmap is considered to be collision-free by the classical PRM method in the query stage, the track obtained by directly using the method may collide with the scene, which is unacceptable. However, the delayed collision detection strategy of LazyPRM method can detect the collision after finding the track, and only plan the part of the track where the collision occurs again, so that the problems are avoided, the speed of track planning is increased, and the track obtained by each planning is ensured to be basically consistent.
And 5, the mobile operation robot executes the optimal track obtained in the step 4 to finish the operation process.
In summary, the constrained trajectory planning method of the embodiment of the invention can directly obtain the configuration meeting the constraint on the approximate graph by adopting the off-line construction and on-line sampling modes of the constrained manifold approximate graph, thereby reducing the calculated amount and realizing faster trajectory planning speed. Constructing a route map by utilizing an optimal track planning algorithm PRM, and carrying out online track planning by utilizing a LazyPRM algorithm, so that the quality of the track and the consistency of the track obtained by multiple planning are ensured. The method can enable the mobile operation robot to obtain a stable track with high enough quality in a short time, and improves the operation efficiency of the mobile operation robot.
Those of ordinary skill in the art will appreciate that: all or part of the flow of the method implementing the above embodiments may be implemented by a program to instruct related hardware, where the program may be stored in a computer readable storage medium, and the program may include the flow of the embodiment of each method as described above when executed. The storage medium may be a magnetic disk, an optical disk, a Read-Only Memory (ROM), a random-access Memory (Random Access Memory, RAM), or the like.
The foregoing is only a preferred embodiment of the present invention, but the scope of the present invention is not limited thereto, and any changes or substitutions easily contemplated by those skilled in the art within the scope of the present invention should be included in the scope of the present invention. Therefore, the protection scope of the present invention should be subject to the protection scope of the claims. The information disclosed in the background section herein is only for enhancement of understanding of the general background of the invention and is not to be taken as an admission or any form of suggestion that this information forms the prior art already known to those of ordinary skill in the art.
Claims (10)
1. The constrained trajectory planning method for the mobile operation robot is characterized by comprising the following steps of:
step 1, setting constraint and constructing an approximate graph of a constraint manifold;
step 2, reconstructing an operation scene of the mobile operation robot to obtain an operation scene model;
Step 3, executing an optimal trajectory planning algorithm PRM on the approximation graph obtained in the step 1 and the operation scene model obtained in the step 2 to obtain a roadmap containing an optimal trajectory;
Step 4, performing online track planning on the roadmap containing the optimal track obtained in the step 3 by adopting LazyPRM algorithm in the operation process of the mobile operation robot to obtain the optimal track;
And 5, the mobile operation robot executes the optimal track obtained in the step 4 to finish the operation process.
2. The constrained trajectory planning method of mobile manipulator robot according to claim 1, wherein in step 1, the point set V and the edge set E are sequentially generated according to the set constraint, and then the constraint is set and an approximate graph of the constraint manifold is constructed according to the following manner, comprising:
Step 11, carrying out random uniform sampling in the whole configuration space A to obtain an initial configuration X 0, and then projecting the configuration X 0 onto the constraint manifold X by using an iterative optimization method to obtain a configuration X i;
Step 12, performing difference verification on each bit shape x i obtained in the step 11 and each bit shape x j in the point set V, and adding the bit shape x i meeting the difference index into the point set V;
step 13, repeating the steps 11 and 12 until the positions in the point set V reach the designated number, and completing the construction of the point set V;
Step 14, for the bit shape x k in the set of points V, searching for the nearest ortho shape x n of the bit shape x k in the set of points V using the euclidean distance;
step 15, connecting the position x k in step 14 with the nearest adjacent position x n of the position x k by using a straight line, judging the validity of the connection, and adding the valid connection into the edge set E;
Step 16, repeating the step 14 and the step 15 until the connection of the configuration x k reaches the designated number or all configurations in the point set V are tried to be connected, and completing the construction of the edge set E;
Step 17, using the point set V as the vertex set, each vertex in the vertex set corresponding to a configuration satisfying the constraint F, and using the edge set E as the edge set to construct the undirected graph With undirected graph/>An approximation graph of the constraint manifold X is shown.
3. The constrained trajectory planning method of mobile manipulator robots as claimed in claim 2, wherein the iterative optimization method of step 11 is a Newton-Raphson method.
4. The constrained trajectory planning method of mobile manipulator robots according to claim 2, wherein in step 12, the variability index is:
；
；
；
Wherein ε is a constant, set to 0.1; alpha is a constant and is set to 0.45; ρ is a constant, set to 1; By cutting space/> Log mapping at: /(I)；/>Representation/>Is a transpose of (2); /(I)AndThe tangent space/>, respectively, of the constraint manifold at position x i And bit shape/>Space of department/>Is a group of radicals satisfying:
；
Wherein, Is the jacobian matrix of constraint function F at x; i is a k x k unit array.
5. The constrained trajectory planning method of mobile manipulator robot of claim 2, wherein in step 15, connecting the configuration x k in step 14 and the nearest neighbor configuration x n of the configuration x k using a straight line and determining the validity of the connection comprises:
equidistant sampling of straight lines to obtain a plurality of configurations ，/>，/>And the following judgment is carried out:
；
wherein F is a constraint function; is a constant, set to 0.1, when all sampling bits are shaped/> When the above-mentioned conditions are satisfied, the configuration/>, is confirmedAnd/>Connection between/>Is effective.
6. The constrained trajectory planning method of any one of claims 1 to 5, wherein in step 3, an optimal trajectory planning algorithm PRM is performed on the approximation map obtained in step 1 and the operation scene model obtained in step 2, to obtain a roadmap including an optimal trajectory, in the following manner, including:
Sampling by adopting two sampling modes of uniform random sampling and neighbor random sampling by using an optimal trajectory planning algorithm PRM; wherein the uniform random sampling is performed by uniformly random sampling an integer Is implemented in such a way that the sampled bit pattern is approximately the bit pattern at the i-th index position in the graph,/>The number of configurations in the point set;
according to whether the bit shape x belongs to the point set V, the neighbor random sampling is divided into two cases:
(21) When (when) When, can sample the collection/>, of the bit shapesExpressed as:
；
Wherein, Representing that there is an active connection between a set of positions x and x n, the set of sampable positions V n is a set of positions x n connected to the set of positions x and having a distance d or less; d is a constant, set to 1;
(22) When (when) Or/>When the method is used, uniform random sampling is carried out in a sphere with the configuration x as the center and d as the radius, and the configuration meeting the constraint is obtained by using the Newton-Raphson method as the sampling configuration.
7. The constrained trajectory planning method of a mobile manipulator robot according to any one of claims 1 to 5, wherein in the step 4, during the operation of the mobile manipulator robot, performing online trajectory planning on the route map including the optimal trajectory obtained in the step 3 by adopting LazyPRM algorithm to obtain the optimal trajectory, and the method comprises the following steps:
Step 41, reconstructing the operation scene to obtain a reconstruction model;
Step 42, loading the route map obtained in the step3 and the reconstruction model obtained in the step 41, and performing track planning by using LazyPRM algorithm to obtain an optimal track;
and 43, calculating the speed and the acceleration of the optimal track obtained in the step 42 according to the constraint of the mobile operation robot.
8. The constrained trajectory planning method of mobile manipulator robots according to any one of claims 1 to 5, wherein in step 2, the manipulator scene is reconstructed using SLAM method or NeRF method to obtain the manipulator scene model.
9. A processing apparatus, comprising:
At least one memory for storing one or more programs;
At least one processor capable of executing one or more programs stored in the memory, which when executed by the processor, cause the processor to implement the method of any of claims 1-8.
10. A readable storage medium, characterized in that a computer program is stored, which, when being executed by a processor, is capable of implementing the method of any of claims 1-8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410552620.XA CN118131779B (en) | 2024-05-07 | 2024-05-07 | Constrained trajectory planning method, equipment and storage medium for mobile operation robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410552620.XA CN118131779B (en) | 2024-05-07 | 2024-05-07 | Constrained trajectory planning method, equipment and storage medium for mobile operation robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN118131779A true CN118131779A (en) | 2024-06-04 |
| CN118131779B CN118131779B (en) | 2024-08-16 |
Family
ID=91230530
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410552620.XA Active CN118131779B (en) | 2024-05-07 | 2024-05-07 | Constrained trajectory planning method, equipment and storage medium for mobile operation robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN118131779B (en) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20090087023A1 (en) * | 2007-09-27 | 2009-04-02 | Fatih M Porikli | Method and System for Detecting and Tracking Objects in Images |
| US20190240833A1 (en) * | 2018-02-05 | 2019-08-08 | Canon Kabushiki Kaisha | Trajectory generating method, and trajectory generating apparatus |
| CN114347005A (en) * | 2022-03-18 | 2022-04-15 | 中国科学技术大学 | Rope traction parallel robot continuous reconstruction planning method capable of avoiding obstacles |
| CN114740864A (en) * | 2022-05-07 | 2022-07-12 | 南京信息工程大学 | Mobile robot path planning method based on improved PRM and artificial potential field |
| US20230084968A1 (en) * | 2021-09-10 | 2023-03-16 | Honda Motor Co., Ltd. | Object manipulation |
| CN116736866A (en) * | 2023-08-10 | 2023-09-12 | 苏州观瑞汽车技术有限公司 | Property indoor robot path planning and trajectory control method and system |
-
2024
- 2024-05-07 CN CN202410552620.XA patent/CN118131779B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20090087023A1 (en) * | 2007-09-27 | 2009-04-02 | Fatih M Porikli | Method and System for Detecting and Tracking Objects in Images |
| US20190240833A1 (en) * | 2018-02-05 | 2019-08-08 | Canon Kabushiki Kaisha | Trajectory generating method, and trajectory generating apparatus |
| US20230084968A1 (en) * | 2021-09-10 | 2023-03-16 | Honda Motor Co., Ltd. | Object manipulation |
| CN114347005A (en) * | 2022-03-18 | 2022-04-15 | 中国科学技术大学 | Rope traction parallel robot continuous reconstruction planning method capable of avoiding obstacles |
| CN114740864A (en) * | 2022-05-07 | 2022-07-12 | 南京信息工程大学 | Mobile robot path planning method based on improved PRM and artificial potential field |
| CN116736866A (en) * | 2023-08-10 | 2023-09-12 | 苏州观瑞汽车技术有限公司 | Property indoor robot path planning and trajectory control method and system |
Non-Patent Citations (4)
| Title |
|---|
| S. KARAMAN 等: "ampling-based algorithms for optimal motion planning", THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH, vol. 30, no. 7, 31 December 2011 (2011-12-31), pages 846 - 894 * |
| 邓槟槟等: "6自由度绳索牵引并联机器人的快速终端滑模同步控制", 机械工程学报, vol. 58, no. 13, 31 July 2022 (2022-07-31), pages 50 - 58 * |
| 邹宇星等: "基于改进PRM的采摘机器人机械臂避障路径规划", 传感器与微系统, vol. 38, no. 1, 31 December 2019 (2019-12-31), pages 52 - 56 * |
| 陈星辰;肖南峰;: "基于LazyPRM算法的工业机械臂动态避障和轨迹规划及协调作业研究", 重庆理工大学学报(自然科学), no. 06, 15 June 2020 (2020-06-15), pages 160 - 166 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN118131779B (en) | 2024-08-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Arslan et al. | Use of relaxation methods in sampling-based algorithms for optimal motion planning | |
| Yershova et al. | Improving motion-planning algorithms by efficient nearest-neighbor searching | |
| US5159512A (en) | Construction of minkowski sums and derivatives morphological combinations of arbitrary polyhedra in cad/cam systems | |
| CN112684700B (en) | Multi-target searching and trapping control method and system for swarm robots | |
| CN107330214A (en) | Spatial configuration optimal method based on discretization Yu heuristic evolution algorithm | |
| CN114643586B (en) | Multi-finger dexterous hand grabbing gesture planning method based on deep neural network | |
| CN108229643B (en) | Method for identifying key protein by using drosophila optimization algorithm | |
| CN118131779B (en) | Constrained trajectory planning method, equipment and storage medium for mobile operation robot | |
| CN111427341B (en) | Robot shortest expected time target searching method based on probability map | |
| CN114399161A (en) | Multi-unmanned aerial vehicle cooperative task allocation method based on discrete mapping differential evolution algorithm | |
| CN115797418A (en) | Complex mechanical part measurement point cloud registration method and system based on improved ICP | |
| CN113093758A (en) | Improved AGV path planning algorithm based on grid map model | |
| CN113885567B (en) | Collaborative path planning method for multiple unmanned aerial vehicles based on conflict search | |
| CN114779788A (en) | Path planning method for improving A-algorithm | |
| CN115903818A (en) | Route planning method for transformer substation inspection robot | |
| CN115218916A (en) | Safety path planning method and device | |
| CN117109625B (en) | Unmanned path planning method based on improved PRM algorithm | |
| Shen et al. | Smart: self-morphing adaptive replanning tree | |
| CN114202662B (en) | Cutter characteristic point identification method and equipment combining adjacent cutter rail transverse geometric characteristics | |
| Zha et al. | Robot Motion Planning Method Based on Incremental High‐Dimensional Mixture Probabilistic Model | |
| CN109143222B (en) | Three-dimensional maneuvering target tracking method based on divide-and-conquer sampling particle filtering | |
| CN113628224A (en) | Room segmentation method based on three-dimensional Euclidean distance transformation | |
| Zhang et al. | Using automatically constructed view-independent relational model in 3D object recognition | |
| CN112947459B (en) | Efficient path planning method based on fast random expansion tree | |
| CN117873090B (en) | Automatic driving vehicle path planning method |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |