CN108705532B - Mechanical arm obstacle avoidance path planning method and device and storage device - Google Patents
Mechanical arm obstacle avoidance path planning method and device and storage device Download PDFInfo
- Publication number
- CN108705532B CN108705532B CN201810380815.5A CN201810380815A CN108705532B CN 108705532 B CN108705532 B CN 108705532B CN 201810380815 A CN201810380815 A CN 201810380815A CN 108705532 B CN108705532 B CN 108705532B
- Authority
- CN
- China
- Prior art keywords
- space
- mechanical arm
- node
- obstacle avoidance
- obstacle
- 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.)
- Expired - Fee Related
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
- B25J9/1666—Avoiding collision or forbidden zones
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1674—Programme controls characterised by safety, monitoring, diagnostic
- B25J9/1676—Avoiding collision or forbidden zones
Abstract
The invention provides a mechanical arm obstacle avoidance path planning method, equipment and storage equipment, wherein the mechanical arm obstacle avoidance path planning method adopts an improved A-x algorithm to search an obstacle avoidance path from a starting node to a target node at the tail end of a mechanical arm, so that the problem of collision between the mechanical arm and an obstacle is solved, the obstacle avoidance path is subjected to smooth processing according to a cubic uniform B-spline curve method, and the problem of large control error caused by shaking of the mechanical arm is solved; provided are a mechanical arm obstacle avoidance path planning device and a storage device, which are used for realizing a mechanical arm obstacle avoidance path planning method. The invention has the beneficial effects that: according to the technical scheme provided by the invention, the mechanical arm is prevented from colliding with the barrier in the working process, the control error of the mechanical arm is reduced, the path tracking precision of the mechanical arm is improved, the mechanical vibration generated in the movement process of the mechanical arm is reduced, and the service life of the mechanical arm is prolonged.
Description
Technical Field
The invention relates to the technical field of mechanical arm obstacle avoidance, in particular to a method and equipment for planning an obstacle avoidance path of a mechanical arm and storage equipment.
Background
In the development trend of the human science and technology, the robot technology becomes one of the inevitable development trends of high and new technology with wide application range, flexible use mode and unique requirements in special operation, and therefore, the robot technology is more and more concerned by various countries. In recent years, the use of robotics in the aerospace field has become increasingly important. Robotic arms have undoubtedly played a crucial role in the aerospace field. The mechanical arm has strong and weak working performance and high and low stability performance, and directly influences various aspects of on-orbit operation. Obstacle avoidance path planning is an important link of the mechanical arm in the motion process, and is related to the accuracy and stability of the mechanical arm in the moving process and the response time. In particular, in a complex environment, the mechanical arm needs to plan a feasible working path under the condition of ensuring the safety of the mechanical arm. At present, most of researches on mechanical arm obstacle avoidance path planning do not consider the real-time obstacle avoidance path planning problem and the situation of dynamic obstacles in the environment, so that the related method cannot be generally applied to more complex scenes. Therefore, the method has important significance in researching how to realize the real-time obstacle avoidance path planning of the mechanical arm and considering the situation of the obstacle in the environment.
Disclosure of Invention
In order to solve the above problems, the present invention provides a method, a device and a storage device for planning an obstacle avoidance path of a robot arm, and the method for planning the obstacle avoidance path of the robot arm mainly includes the following steps:
s101: detecting pose data of a working target and an obstacle through an external sensor in a mechanical arm, taking an initial position of the tail end of the mechanical arm as an initial node of obstacle avoidance path planning, and taking the position of the working target as a target node of the obstacle avoidance path planning;
s102: performing three-dimensional modeling on the operation space of the mechanical arm by adopting a C space method to obtain a gridding model of a C space;
s103: searching an obstacle avoidance path in a gridded C space by adopting an improved A-x algorithm according to the gridded model to obtain an obstacle avoidance path from a starting node to a target node at the tail end of the mechanical arm;
s104: and smoothing the obstacle avoidance path by adopting a cubic uniform B-spline curve method, and taking the smoothed obstacle avoidance path as a final path planning result.
Further, in step S102, using a grid method, the C space is represented by several equal cubes, each of the cubes corresponds to a separate index value, the index value corresponding to the cube in the obstacle space is index (X, Y, Z), and the index value corresponding to the cube in the free space is index (X, Y, Z); (m, n, l) in the index values index (m, n, l) respectively represent the number of the cubes in the x, y and z directions from the origin 0 to the index point, and if the length of the side of the cube is length, the coordinates at the index point are:
wherein，
Or
And obtaining a gridding model of the C space according to the coordinates of each index point in the free space and the obstacle space.
Further, in step S103, an evaluation function of the improved a-algorithm is established as shown in formula (1):
f(n)＝g(n)+h(n) (1)
wherein, (n) is a valuation function of the current node, g (n) is the actual cost from the starting node to the current node and represents the priority trend of the search breadth, and h (n) is a heuristic function and represents the estimation cost of the optimal path from the current node to the target node, so that heuristic information in the search is embodied;
the process of searching the obstacle avoidance path by adopting the improved A-star algorithm comprises the following steps:
s201: adding the coordinates S (x, y, z) of the initial node to the open list;
s202: does the open list check for empty? If yes, go to step S203; if not, go to step S204;
s203: ending the path search, and searching the obstacle avoidance path;
s204: selecting an uncalculated node in the open list, and calculating f (n) values of all nodes;
s205: determine if the f (n) value of the node is minimal? If yes, go to step S206; if not, go to step S207; f (n) the node with the smallest value is called the current node;
s206: substituting the path points into an inverse kinematics equation to obtain an inverse solution, and selecting a group of inverse solutions by adopting a time-energy optimal method, wherein the inverse solution is a joint angle of the tail end of the mechanical arm to a target node; substituting the joint angle into the positive kinematics equation by adopting a D-H method to obtain a joint line segment equation, and then going to step S208;
s207: deleting the node from the open list, adding the node into a closed list, and returning to the step S202;
s208: solving the position relation between the joint line segment equation and the obstacle envelope obtained by adopting a cuboid envelope method, and carrying out collision detection;
s209: is the joint line segment equation determined to collide with the obstacle envelope? If yes, go to step S210; if not, go to step S211;
s210: is there an undetected inverse solution? If yes, go to step S212; if not, go back to step S202;
s211: adding the current node into the open list, and then going to step S213;
s212: discarding the current joint angle, selecting a group of inverse solutions from the remaining inverse solutions, and returning to the step S206;
s213: setting the current node as a father node, expanding a subsequent node, and adding the generated subsequent node into the open list;
s214: establishing a pointer for the subsequent node to return to the current node;
s215: according to the pointer, determine whether a target node is found at the end of the mechanical arm? If yes, go to step S216; if not, go back to step S202;
s216: and finally finding the obstacle avoidance path and storing the obstacle avoidance path through collision detection according to the starting node, the path point and the target node.
Further, in step S104, the process of performing smoothing on the obstacle avoidance path by using the cubic uniform B-spline includes: every four adjacent control points can determine a section of the cubic uniform B-spline curve, and n +2 control points can determine n-1 cubic uniform B-spline curve columns { S }1,S2,…,Sn-1}, single curve SiIs a function of the parameter u, as shown in equation (2):
wherein U is a parametric array, and U is [ 1U U ]2u3]，u∈[0,1]And M is a constant coefficient matrix of the cubic uniform B spline curve，CiTo control the lattice, and Ci＝[ωi-1ci-1ωiciωi+1ci+1ωi+2ci+2]，ωiIs a weight factor matrix, and ωi＝[ωi-1ωiωi+1ωi+2]，i＝1,2，…,n-1；
Carrying out normalization processing on the cubic uniform B spline curves, sequentially splicing the local parameters u e [0,1] of each cubic uniform B spline curve to obtain global parameters t e [0, n-1], wherein the cubic uniform B spline curves are expressed as piecewise functions under the global parameters:
wherein t epsilon [0, n-1] is a global parameter, S (i) is the ith cubic uniform B spline curve, and i is 1,2, …, n-2;
and smoothing the obstacle avoidance path according to the cubic uniform B-spline curve, so that the mechanical arm can stably reach a target node from a starting node along the obstacle avoidance path and avoid an obstacle, and the path tracking precision of the mechanical arm is improved.
The storage device stores instructions and data for realizing a mechanical arm obstacle avoidance path planning method.
An obstacle avoidance path planning apparatus for a robot arm, comprising: a processor and the storage device; and the processor loads and executes the instructions and data in the storage device to realize the mechanical arm obstacle avoidance path planning method.
The technical scheme provided by the invention has the beneficial effects that: according to the technical scheme provided by the invention, the mechanical arm is prevented from colliding with the barrier in the working process, the control error of the mechanical arm is reduced, the path tracking precision of the mechanical arm is improved, the mechanical vibration generated in the movement process of the mechanical arm is reduced, and the service life of the mechanical arm is prolonged.
Drawings
The invention will be further described with reference to the accompanying drawings and examples, in which:
fig. 1 is a flowchart of a method for planning an obstacle avoidance path of a robot arm according to an embodiment of the present invention;
fig. 2 is a flow chart of a path search using the modified a-algorithm in the embodiment of the present invention;
fig. 3 is a schematic diagram of the operation of the hardware device in the embodiment of the present invention.
Detailed Description
For a more clear understanding of the technical features, objects and effects of the present invention, embodiments of the present invention will now be described in detail with reference to the accompanying drawings.
The embodiment of the invention provides a method and equipment for planning an obstacle avoidance path of a mechanical arm and storage equipment.
Referring to fig. 1, fig. 1 is a flowchart of a method for planning an obstacle avoidance path of a robot arm in an embodiment of the present invention, which specifically includes the following steps:
s101: obtaining pose data of a working target and an obstacle through an external sensor in a mechanical arm, taking an initial position of the tail end of the mechanical arm as an initial node of obstacle avoidance path planning, and taking the position of the working target as a target node of the obstacle avoidance path planning;
s102: performing three-dimensional modeling on the operation space of the mechanical arm by adopting a C space method to obtain a gridding model of a C space; the C space is a parameter space for representing the degree of freedom of an object, the C space is divided into a free space and an obstacle space, the free space is a C space where the mechanical arm and other fixed objects do not interfere with each other, the obstacle space is a C space where the C obstacle is located, an obstacle in the C space is called a C obstacle, the obstacle space needs to be expanded to avoid collision between the mechanical arm and the C obstacle, and the expansion process of the obstacle space is as follows: simplifying the shape of the mechanical arm into a cylinder, taking the radius of the large arm of the mechanical arm as d, expanding the space of the barrier outwards by taking the C barrier as the center of a circle and the radius of the d as the radius in all directions, and taking all cubes intersected with the expansion range into the barrierA space; using a grid method to represent the C space by several equal cubes, each of the cubes corresponds to a separate index value, the index value corresponding to the cube in the obstacle space is index (X, Y, Z), the index value corresponding to the cube in the free space is index (X, Y, Z), the index value index (X, Y, Z) corresponds to a certain fixed point of the cube in the free space, and may be any vertex among 8 vertices of the cube, the vertex is referred to as an index point, and the index points in the free space must be selected in the same direction, in this embodiment, the vertex of the cube closest to origin 0 is uniformly selected as the index point; (m, n, l) in the index values index (m, n, l) respectively represent the number of the cubes in the x, y and z directions from the origin 0 to the index point, and if the length of the side of the cube is length, the coordinates at the index point are:
wherein the content of the first and second substances,
or
Obtaining a gridding model of a C space according to the coordinates of each index point in the free space and the barrier space;
s103: according to the gridding model, performing path search in the gridded free space by adopting an improved A-x algorithm to obtain an obstacle avoidance path from a starting node to an ending node of the tail end of the mechanical arm; taking the grid width as a basic step length in a path searching process, wherein a connecting line segment of any two nodes in the grid can form a path in a C space, and two conditions for judging the path safety are as follows: the two end points of the path must be in free space and the path cannot intersect or be tangent to the C-barrier; wherein, the valuation function for establishing the improved A-algorithm is shown in formula (1):
f(n)＝g(n)+h(n) (1)
wherein, (n) is a valuation function of the current node, g (n) is the actual cost from the starting node to the current node and represents the priority trend of the search breadth, and h (n) is a heuristic function and represents the estimation cost of the optimal path from the current node to the target node, so that heuristic information in the search is embodied;
the estimated cost h (i) is the distance between the current node and the target node in the C space, as shown in formula (2):
wherein h (i) is the distance between the current node and the target node in C space, (x)i,yi,zi) As coordinates of the current node, (x)d,yd,zd) Coordinates of the target node;
referring to fig. 2, fig. 2 is a flowchart of performing a path search by using an improved a-algorithm in the embodiment of the present invention, which specifically includes:
s201: adding the coordinates S (x, y, z) of the initial node to the open list;
s202: does the open list check for empty? If yes, go to step S203; if not, go to step S204;
s203: ending the path search, and searching the obstacle avoidance path;
s204: selecting an uncalculated node in the open list, and calculating f (n) values of all nodes;
s205: determine if the f (n) value of the node is minimal? If yes, go to step S206; if not, go to step S207; f (n) the node with the smallest value is called the current node;
s206: substituting the path points into an inverse kinematics equation to obtain an inverse solution, and selecting a group of inverse solutions by adopting a time-energy optimal method, wherein the inverse solution is a joint angle of the tail end of the mechanical arm to a target node; substituting the joint angle into the positive kinematics equation by adopting a D-H method to obtain a joint line segment equation, and then going to step S208; selecting the path points by using an evaluation function f (n) in a dynamic step length mode; the method for selecting the inverse solution by adopting the time-energy optimal method comprises the following steps: if the C obstacle does not exist in the space C, the shortest stroke principle is adopted, namely the angular movement distance of the mechanical arm joint with the large size is small, and the angular movement distance of the mechanical arm joint with the small size is large; if the C space has the C obstacle, if a plurality of groups of feasible solutions are obtained, selecting 1 group of optimal solutions by combining a shortest route principle, wherein the selection of the optimal solutions is related to the selection of the path points, namely, the plurality of groups of feasible solutions are respectively subjected to difference with the solution of the previous path point, comparing the difference values, and selecting 1 group of feasible solutions with the minimum difference value as the optimal solution; in an embodiment, taking a six-degree-of-freedom mechanical arm as an example, the optimization steps of the shortest stroke principle are as follows:
s301: knowing the joint angles theta of the robot arm1、θ2、θ3、θ4、θ5、θ6；
S302: obtaining 8 sets of inverse solutions theta through the positive and inverse kinematics equations of the mechanical armn1、θn2、θn3、θn4、θn5、θn6Wherein n is 1,2,3, …, 8;
s303: deleting part of infeasible solutions according to the variation range of each joint angle of the mechanical arm, wherein the variation range of each joint solution is as follows: min (theta)ni)≤θi≤max(θni)，n＝1,2,3,…,8，i＝1,2,3,…,6；
S304: optimizing the solution set of each joint angle of the mechanical arm in the step S303 according to the shortest stroke principle to obtain 1 group of optimal solutions, namely min0T1(θn1-θ1),1T2(θn2-θ2),2T3(θn3-θ3)],n＝1,2,3,...,8；
S207: deleting the node from the open list, adding the node into a closed list, and returning to the step S202;
s208: solving the position relation between the joint line segment equation and the obstacle envelope, and carrying out collision detection;
s209: is the joint line segment equation determined to collide with the obstacle envelope? If yes, go to step S210; if not, go to stepStep S211; by adopting a cuboid enveloping method, the cuboids enveloping L barriers are respectively { O }1,O2,…,OLDetecting a line segment connected with each joint of the mechanical arm and the { O }1,O2,…,OLWhether each surface has an intersection point or not is judged, the collision problem is converted into the position relation between the space straight-line segment and the quadrangle, and the position relation is judged by the following method: 2 end points p on the known space line segment1(x1,y1,z1) And p2(x2,y2,z2) And the normal vector M of the space quadrangle is equal to [ M ═ M1m2m3]And coordinates of four vertexes, and the position relation of the line segment and the plane can be judged by the relation between a space line segment direction vector and a space quadrilateral normal vector, wherein the space line segment direction vector T is [ x ]2-x1y2-y1z2-z1](ii) a If M and T are equal to 0, the space line segment is parallel to the plane and has no intersection point; if M.T is not equal to 0, the space line segment and the plane have an intersection point, and the intersection point is defined as p0(x0,y0,z0) (ii) a Determine whether the intersection is a collision point? If so, two conditions are satisfied: (1) the point of intersection p0(x0,y0,z0) On a spatial line segment and the intersection point are in a spatial quadrilateral; (2) the front and the back of a cuboid enveloped by the obstacles are parallel to a z axis, the left and the right are parallel to a y axis, the upper and the lower surfaces are parallel to an x axis, and taking an upper plane parallel to the x axis as an example, 4 vertexes of the upper plane are as follows: s1(x3,y3,z3)，s2(x4,y4,z4)，s3(x5,y5,z5)，s4(x6,y6,z6) If so: min (z)1,z2)≤z0≤max(z1,z2)，min(x3,x4)≤x0≤max(x3,x4)，min(y3,y4)≤y0≤max(y3,y4) Then the intersection point p0(x0,y0,z0) Is a collision point; if not, the intersection point p is0(x0,y0,z0) Not a collision point;
s210: is there an undetected inverse solution? If yes, go to step S212; if not, go back to step S202;
s211: adding the current node into the open list, and then going to step S213;
s212: discarding the current joint angle, selecting a group of inverse solutions from the remaining inverse solutions, and returning to the step S206;
s213: setting the current node as a father node, expanding a subsequent node, and adding the generated subsequent node into the open list; the selection principle of the subsequent node is as follows: on the premise that the connection line path of the current node and the subsequent node does not collide with the C barrier, the node with the minimum f value;
s214: establishing a pointer for the subsequent node to return to the current node;
s215: according to the pointer, determine whether a target node is found at the end of the mechanical arm? If yes, go to step S216; if not, go back to step S202;
s216: finally finding the obstacle avoidance path through collision detection according to the starting node, the path point and the target node, and storing the obstacle avoidance path;
s104: smoothing the obstacle avoidance path by adopting a cubic uniform B-spline curve method, and taking the smoothed obstacle avoidance path as a final path planning result; the process of adopting the cubic uniform B-spline to carry out smoothing treatment on the obstacle avoidance path comprises the following steps: every four adjacent control points can determine a section of the cubic uniform B-spline curve, and n +2 control points can determine n-1 cubic uniform B-spline curve columns { S }1,S2,…,Sn-1}, single curve SiIs a function of the parameter u, as shown in equation (3):
wherein U is a parametric array, and U is [ 1U U ]2u3]，u∈[0,1]M is the constant coefficient matrix of the cubic uniform B spline curve, CiTo control the lattice, and Ci＝[ωi-1ci-1ωiciωi+1ci+1ωi+2ci+2]，ωiIs a weight factor matrix, and ωi＝[ωi-1ωiωi+1ωi+2]，i＝1,2，…,n-1；
Carrying out normalization processing on the cubic uniform B spline curves, sequentially splicing the local parameters u e [0,1] of each cubic uniform B spline curve to obtain global parameters t e [0, n-1], wherein the cubic uniform B spline curves are expressed as piecewise functions under the global parameters:
wherein t epsilon [0, n-1] is a global parameter, S (i) is the ith cubic uniform B spline curve, and i is 1,2, …, n-2;
and smoothing the obstacle avoidance path according to the cubic uniform B-spline curve, so that the mechanical arm can stably reach a target node from a starting node along the obstacle avoidance path and avoid an obstacle, and the path tracking precision of the mechanical arm is improved.
Referring to fig. 3, fig. 3 is a schematic diagram of a hardware device according to an embodiment of the present invention, where the hardware device specifically includes: provided are a mechanical arm obstacle avoidance path planning device 301, a processor 302 and a storage device 303.
An obstacle avoidance path planning device 301 for a robot arm: the obstacle avoidance path planning device 301 for the mechanical arm implements the obstacle avoidance path planning method for the mechanical arm.
The processor 302: the processor 302 loads and executes the instructions and data in the storage device 303 to implement the method for planning obstacle avoidance path of the robot arm.
The storage device 303: the storage device 303 stores instructions and data; the storage device 303 is used to implement the method for planning the obstacle avoidance path of the robot arm.
The invention has the beneficial effects that: according to the technical scheme provided by the invention, the mechanical arm is prevented from colliding with the barrier in the working process, the control error of the mechanical arm is reduced, the path tracking precision of the mechanical arm is improved, the mechanical vibration generated in the movement process of the mechanical arm is reduced, and the service life of the mechanical arm is prolonged.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents, improvements and the like that fall within the spirit and principle of the present invention are intended to be included therein.
Claims (4)
1. A method for planning obstacle avoidance paths of a mechanical arm is characterized by comprising the following steps: the method comprises the following steps:
s101: detecting pose data of a working target and an obstacle through an external sensor in a mechanical arm, taking an initial position of the tail end of the mechanical arm as an initial node of obstacle avoidance path planning, and taking the position of the working target as a target node of the obstacle avoidance path planning;
s102: performing three-dimensional modeling on the operation space of the mechanical arm by adopting a C space method to obtain a gridding model of a C space;
the C space is used for representing a parameter space of the degree of freedom of an object, the C space is divided into a free space and an obstacle space, the free space is a C space where the mechanical arm and other fixed objects do not interfere with each other, an obstacle in the C space is called a C obstacle, the obstacle space is a C space where the C obstacle is located, in order to avoid collision between the mechanical arm and the C obstacle, the obstacle space needs to be expanded, and the expansion process of the obstacle space is as follows: simplifying the shape of the mechanical arm into a cylinder, taking the radius of the large arm of the mechanical arm as d, outwards extending the barrier space by taking the C barrier as a circle center and the radius of the large arm of the mechanical arm as d, and taking all cubes intersected with the extension range into the barrier space; using a grid method to represent the C space by a plurality of equal cubes, wherein each cube corresponds to oneA plurality of individual index values, an index value corresponding to the cube in the obstacle space being index (X, Y, Z), an index value corresponding to the cube in the free space being index (X, Y, Z), the index value index (X, Y, Z) corresponding to any one of 8 vertices of the cube in the free space, the vertices being referred to as index points, the selection of the index points in the free space having to be in the same direction; (m, n, l) in the index values index (m, n, l) respectively represent the number of the cubes in the x, y and z directions from the origin 0 to the index point, and if the length of the side of the cube is length, the coordinates at the index point are:
wherein the content of the first and second substances,
or
Obtaining a gridding model of a C space according to the coordinates of each index point in the free space and the barrier space;
s103: searching an obstacle avoidance path in a gridded C space by adopting an improved A-x algorithm according to the gridded model to obtain an obstacle avoidance path from a starting node to a target node at the tail end of the mechanical arm; taking the grid width as a basic step length in a path searching process, wherein a connecting line segment of any two nodes in the grid can form a path in a C space, and two conditions for judging the path safety are as follows: the two end points of the path must be in free space and the path cannot intersect or be tangent to the C-barrier;
the valuation function for establishing the improved A-algorithm is shown in formula (1):
f(n)＝g(n)+h(n) (1)
wherein, (n) is a valuation function of the current node, g (n) is the actual cost from the starting node to the current node and represents the priority trend of the search breadth, and h (n) is a heuristic function and represents the estimation cost of the optimal path from the current node to the target node, so that heuristic information in the search is embodied;
the estimated cost h (i) is the distance between the current node and the target node in the C space, as shown in formula (2):
wherein h (i) is the distance between the current node and the target node in C space, (x)i,yi,zi) As coordinates of the current node, (x)d,yd,zd) Coordinates of the target node;
the process of searching the obstacle avoidance path by adopting the improved A-star algorithm comprises the following steps:
s201: adding the coordinates S (x, y, z) of the start node to the open list;
s202: detecting whether the open list is empty; if yes, go to step S203; if not, go to step S204;
s203: ending the path search, and searching the obstacle avoidance path;
s204: selecting an uncalculated node in the open list, and calculating f (n) values of all nodes;
s205: judging whether the f (n) value of the node is minimum; if yes, go to step S206; if not, go to step S207; f (n) the node with the smallest value is called the current node;
s206: substituting the path points into an inverse kinematics equation to obtain an inverse solution, and selecting a group of inverse solutions by adopting a time-energy optimal method, wherein the inverse solution is a joint angle of the tail end of the mechanical arm to a target node; selecting the path points by using an evaluation function f (n) in a dynamic step length mode; substituting the joint angle into the positive kinematics equation by adopting a D-H method to obtain a joint line segment equation, and then going to step S208;
s207: deleting the node from the open list, adding the node into a closed list, and returning to the step S202;
s208: solving the position relation between the joint line segment equation and the obstacle envelope obtained by adopting a cuboid envelope method, and carrying out collision detection;
s209: judging whether the joint line segment equation collides with the barrier envelope; if yes, go to step S210; if not, go to step S211;
by adopting a cuboid enveloping method, the cuboids enveloping L barriers are respectively { O }1，O2，…，OLDetecting a line segment connected with each joint of the mechanical arm and the { O }1，O2，…，OLWhether each surface has an intersection point or not is judged, the collision problem is converted into the position relation between the space straight-line segment and the quadrangle, and the position relation is judged by the following method: 2 end points p on the known space line segment1(x1，y1，z1) And p2(x2，y2，z2) The normal vector M of the space quadrangle and the coordinates of the four vertexes are known by space analytic geometry knowledge, and the position relation of the line segment and the plane can be judged by the relation between the direction vector of the space line segment and the normal vector of the space quadrangle, wherein the direction vector T of the space line segment is [ x ═ x [ ]2-x1y2-y1z2-z1](ii) a If M and T are equal to 0, the space line segment is parallel to the plane and has no intersection point; if M.T is not equal to 0, the space line segment and the plane have an intersection point, and the intersection point is defined as p0(x0，y0，z0) (ii) a Judging whether the intersection point is a collision point; if so, two conditions are satisfied: (1) the point of intersection p0(x0，y0，z0) On a spatial line segment and the intersection point are in a spatial quadrilateral; (2) the front and the back of a cuboid enveloped by the obstacles are parallel to a z axis, the left and the right are parallel to a y axis, the upper and the lower surfaces are parallel to an x axis, and taking an upper plane parallel to the x axis as an example, 4 vertexes of the upper plane are as follows: s1(x3，y3，z3)，s2(x4，y4，z4)，s3(x5，y5，z5)，s4(x6，y6，z6) If so: min (z)1，z2)≤z0≤max(z1，z2)，min(x3，x4)≤x0≤max(x3，x4)，min(y3，y4)≤y0≤max(y3，y4) Then the intersection point p0(x0，yo，z0) Is a collision point; if not, the intersection point p is0(x0，y0，z0) Not a collision point;
s210: judging whether undetected inverse solutions exist; if yes, go to step S212; if not, go back to step S202;
s211: adding the current node into the open list, and then going to step S213;
s212: discarding the current joint angle, selecting a group of solutions from the remaining inverse solutions, and returning to the step S206;
s213: setting the current node as a father node, expanding a subsequent node, and adding the generated subsequent node into the open list;
s214: establishing a pointer for the subsequent node to return to the current node;
s215: judging whether the tail end of the mechanical arm finds a target node or not according to the pointer; if yes, go to step S216; if not, go back to step S202;
s216: finally finding the obstacle avoidance path through collision detection according to the starting node, the path point and the target node, and storing the obstacle avoidance path;
s104: and smoothing the obstacle avoidance path by adopting a cubic uniform B-spline curve method, and taking the smoothed obstacle avoidance path as a final path planning result.
2. The method for planning obstacle avoidance paths of a mechanical arm according to claim 1, wherein: in step S104, the process of performing smoothing processing on the obstacle avoidance path by using the cubic uniform B-spline method includes: every four adjacent control points can determine a section of the cubic uniform B-spline curve, and n +2 control points can determine n-1 cubic uniform B-spline curve columns { S }1，S2，…，Sn-1}, single curve SiIs a function of the parameter u, as shown in equation (2):
wherein U is a parametric array, and U is [ 1U U ]2u3]，u∈[0，1]M is the constant coefficient matrix of the cubic uniform B spline curve, CiTo control the lattice, and Ci＝[ωi-1ci-1ωiciωi+1ci+1ωi+2ci+2]，ωiIs a weight factor matrix, and ωi＝[ωi-1ωiωi+1ωi+2]，i＝1，2，…，n-1；
Carrying out normalization processing on the cubic uniform B spline curves, sequentially splicing the local parameters u e [0,1] of each cubic uniform B spline curve to obtain global parameters t e [0, n-1], wherein the cubic uniform B spline curves are expressed as piecewise functions under the global parameters:
wherein t epsilon [0, n-1] is a global parameter, S (i) is the ith cubic uniform B spline curve, and i is 1,2, …, n-2;
and smoothing the obstacle avoidance path according to the cubic uniform B-spline curve, so that the mechanical arm can stably reach a target node from a starting node along the obstacle avoidance path and avoid an obstacle, and the path tracking precision of the mechanical arm is improved.
3. A storage device, characterized by: the storage device stores instructions and data for implementing the method for planning the obstacle avoidance path of the mechanical arm as claimed in any one of claims 1 to 2.
4. The utility model provides an obstacle-avoiding path planning equipment for robotic arm which characterized in that: the method comprises the following steps: a processor and a storage device; the processor loads and executes instructions and data in the storage device to realize the mechanical arm obstacle avoidance path planning method according to any one of claims 1 to 2.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810380815.5A CN108705532B (en) | 2018-04-25 | 2018-04-25 | Mechanical arm obstacle avoidance path planning method and device and storage device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810380815.5A CN108705532B (en) | 2018-04-25 | 2018-04-25 | Mechanical arm obstacle avoidance path planning method and device and storage device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108705532A CN108705532A (en) | 2018-10-26 |
| CN108705532B true CN108705532B (en) | 2020-10-30 |
Family
ID=63866914
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810380815.5A Expired - Fee Related CN108705532B (en) | 2018-04-25 | 2018-04-25 | Mechanical arm obstacle avoidance path planning method and device and storage device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108705532B (en) |
Families Citing this family (24)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109296019A (en) * | 2018-11-08 | 2019-02-01 | 北京拓疆者智能科技有限公司 | Digger arm motion control method, system and device |
| CN109682336B (en) * | 2018-12-28 | 2020-12-29 | 上海理工大学 | Automatic planning and optimizing method for three-coordinate measurement path for vehicle body precision detection |
| CN109986554A (en) * | 2019-01-21 | 2019-07-09 | 中国船舶重工集团公司第七一六研究所 | A kind of geometry method for optimally controlling of industrial robot path planning |
| CN109857110A (en) * | 2019-02-13 | 2019-06-07 | 广州视源电子科技股份有限公司 | Motion planning method, device, equipment and computer readable storage medium |
| CN109828236A (en) * | 2019-02-14 | 2019-05-31 | 中南大学 | A kind of microseism/acoustic emission source locating method in labyrinth containing dead zone |
| CN109872324A (en) * | 2019-03-20 | 2019-06-11 | 苏州博众机器人有限公司 | Ground obstacle detection method, device, equipment and storage medium |
| CN109927035A (en) * | 2019-04-08 | 2019-06-25 | 济南大学 | A kind of mapping method of multi-arm robot C- space line obstacle |
| CN109910015B (en) * | 2019-04-12 | 2021-02-23 | 成都天富若博特科技有限责任公司 | Tail end path planning algorithm of mortar spraying and smearing construction robot |
| CN110216674B (en) * | 2019-06-20 | 2021-10-01 | 北京科技大学 | Visual servo obstacle avoidance system of redundant degree of freedom mechanical arm |
| CN110340890A (en) * | 2019-06-27 | 2019-10-18 | 北京控制工程研究所 | A kind of space manipulator overall situation is without touching Trajectory Planning System |
| CN112238452B (en) * | 2019-07-19 | 2022-06-03 | 顺丰科技有限公司 | Mechanical arm path planning method and device, industrial control equipment and storage medium |
| CN110531698B (en) * | 2019-08-16 | 2022-03-11 | 达闼机器人有限公司 | Mechanical arm path planning method and device and mechanical arm |
| CN112642741B (en) * | 2019-10-12 | 2023-02-28 | 顺丰科技有限公司 | Article sorting method and device, industrial control equipment and storage medium |
| CN110744543B (en) * | 2019-10-25 | 2021-02-19 | 华南理工大学 | Improved PRM obstacle avoidance motion planning method based on UR3 mechanical arm |
| WO2021101564A1 (en) | 2019-11-22 | 2021-05-27 | Edda Technology, Inc. | A deterministic robot path planning method for obstacle avoidance |
| CN111055274B (en) * | 2019-11-28 | 2021-12-17 | 深圳优地科技有限公司 | Robot path smoothing method and robot |
| CN111015656A (en) * | 2019-12-19 | 2020-04-17 | 佛山科学技术学院 | Control method and device for robot to actively avoid obstacle and storage medium |
| CN110919661B (en) * | 2019-12-26 | 2022-11-22 | 中国科学院沈阳自动化研究所 | Motion planning method for mechanical arm in glove box closed space |
| CN111735601B (en) * | 2020-08-04 | 2021-03-30 | 中国空气动力研究与发展中心低速空气动力研究所 | Wall collision prevention method for double-engine refueling wind tunnel test supporting device |
| CN112549016A (en) * | 2020-10-21 | 2021-03-26 | 西安工程大学 | Mechanical arm motion planning method |
| CN113442170B (en) * | 2021-06-28 | 2023-12-01 | 国网上海市电力公司 | Method and system for reversely splitting and calculating redundant nodes of mechanical arm path |
| CN113589809B (en) * | 2021-07-26 | 2024-04-05 | 江苏徐工工程机械研究院有限公司 | Work track planning method and device for obstacle-avoidance excavator working device |
| CN116385489B (en) * | 2023-04-04 | 2023-10-03 | 森丽康科技(北京)有限公司 | RGBD camera-based target following method and system |
| CN116652956B (en) * | 2023-06-20 | 2024-03-22 | 上海微亿智造科技有限公司 | Photographing path self-adaptive planning method and device for appearance detection |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102520718B (en) * | 2011-12-02 | 2013-06-05 | 上海大学 | Physical modeling-based robot obstacle avoidance path planning method |
| CN105955280A (en) * | 2016-07-19 | 2016-09-21 | Tcl集团股份有限公司 | Mobile robot path planning and obstacle avoidance method and system |
| CN106441303B (en) * | 2016-09-30 | 2019-11-01 | 哈尔滨工程大学 | It is a kind of based on the paths planning method that can search for continuous neighborhood A* algorithm |
| CN107953334A (en) * | 2017-12-25 | 2018-04-24 | 深圳禾思众成科技有限公司 | A kind of industrial machinery arm Collision Free Path Planning based on A star algorithms |
-
2018
- 2018-04-25 CN CN201810380815.5A patent/CN108705532B/en not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| CN108705532A (en) | 2018-10-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108705532B (en) | Mechanical arm obstacle avoidance path planning method and device and storage device | |
| CN111504325B (en) | Global path planning method based on weighted A-algorithm of enlarged search neighborhood | |
| US11300964B2 (en) | Method and system for updating occupancy map for a robotic system | |
| CN110744543B (en) | Improved PRM obstacle avoidance motion planning method based on UR3 mechanical arm | |
| CN113618742B (en) | Robot obstacle avoidance method and device and robot | |
| JP2826138B2 (en) | Mobile body interference check device | |
| Ruan et al. | Efficient exact collision detection between ellipsoids and superquadrics via closed-form Minkowski sums | |
| JP6848761B2 (en) | Distance evaluation method between objects and interference evaluation method between relatively moving objects | |
| Freitas et al. | High precision trajectory planning on freeform surfaces for robotic manipulators | |
| CN111598941A (en) | Method, device and equipment for measuring gradient of tower and storage medium | |
| CN116451493A (en) | Fluid simulation method for complex structure | |
| CN111815684B (en) | Space multivariate feature registration optimization method and device based on unified residual error model | |
| Shiang et al. | Estimate of minimum distance between convex polyhedra based on enclosed ellipsoids | |
| CN114596360A (en) | Double-stage active instant positioning and graph building algorithm based on graph topology | |
| Quin et al. | Exploring in 3D with a climbing robot: Selecting the next best base position on arbitrarily-oriented surfaces | |
| Kong et al. | An efficient collision detection algorithm for the dual-robot coordination system | |
| Fragkopoulos et al. | Dynamic efficient collision checking method of robot arm paths in configuration space | |
| Ward et al. | Free space mapping and motion planning in configuration space for mobile manipulators | |
| XIANG et al. | Three-dimensional path planning of manipulator based on improved RRT algorithm | |
| Xing et al. | Real-time robot path planning using rapid visible tree | |
| Chen et al. | A GPU mapping system for real-time robot motion planning | |
| CN115930969B (en) | Path planning method and device for mobile robot, electronic equipment and storage medium | |
| JP3391657B2 (en) | Inter-object distance calculation device and inter-object distance calculation program storage medium | |
| Silva et al. | Velocity fields using nurbs with distortion control for structural shape optimization | |
| Fischer et al. | Semi-automatic navigation on 3d triangle meshes using bvp based path-planning |
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 | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20201030 |
|
| CF01 | Termination of patent right due to non-payment of annual fee |