CN115416016A - Mechanical arm obstacle avoidance path planning method based on improved artificial potential field method - Google Patents
Mechanical arm obstacle avoidance path planning method based on improved artificial potential field method Download PDFInfo
- Publication number
- CN115416016A CN115416016A CN202210973726.8A CN202210973726A CN115416016A CN 115416016 A CN115416016 A CN 115416016A CN 202210973726 A CN202210973726 A CN 202210973726A CN 115416016 A CN115416016 A CN 115416016A
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- joint
- potential field
- 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.)
- Pending
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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
The invention relates to a mechanical arm obstacle avoidance path planning method based on an improved artificial potential field method, which is characterized in that a mechanical arm and an obstacle are modeled to obtain the position and the posture of each joint of the mechanical arm in space; and constructing an objective function by using an improved artificial potential field method after collision detection, optimizing based on the joint space of the mechanical arm, searching and constructing a middle point by using a variable step length when the motion of the mechanical arm falls into a local minimum value, and smoothing the track by using a polynomial interpolation method if the motion of the mechanical arm falls into the local minimum value to obtain the complete running track of each joint of the mechanical arm. The obstacle avoidance path planning method for the mechanical arm is characterized in that an improved manual potential field method is adopted to plan the obstacle avoidance path of the mechanical arm, the problem of shaking in a complex obstacle environment can be solved, meanwhile, a variable step length searching method is combined, the mechanical arm can jump out a local minimum value in the movement process, then, a quintic polynomial interpolation method is combined to optimize the movement track of the mechanical arm, the mechanical arm is stable and smooth in operation, and the mechanical arm can avoid obstacles while moving.
Description
Technical Field
The invention relates to the technical field of program control manipulators, in particular to a manipulator obstacle avoidance path planning method based on an improved artificial potential field method.
Background
Along with the emergence in a large number of intelligent factory, the arm becomes the indispensable component part in the mill, has improved the production efficiency in mill greatly. Because the working environment of the mechanical arm is more and more complex, the higher and higher requirements are provided for the working task of the mechanical arm, the collision emergency stop control of the mechanical arm is different from the traditional collision emergency stop control of the mechanical arm, and the mechanical arm adopting the intelligent obstacle avoidance method is more flexible and intelligent.
Since the obstacle models are irregular in general, it is necessary to model the obstacles, and the obstacles are generally enveloped by a bounding box method, and common bounding box models include bounding boxes, bounding balls, bounding ellipses, and the like. The mechanical arm connecting rod is also simplified, the connecting rod is equivalent to a line segment, the collision detection problem is converted into the geometric problem from the line segment to the obstacle model, and the calculation is simplified. The team of the Yangmai teacher adopts the collision detection of the distance from the wrist joint of the mechanical arm to the obstacle, and the problem that the accuracy of the calculated distance is not enough exists.
Because inverse operation is required to be carried out in a Cartesian space, a large amount of operation is involved, even singular solutions appear, and for example, the problems of large operation amount, optimal solutions, singular values and the like caused by inevitable solution of a team of teachers from a graduate to the redundant mechanical arm are solved. The traditional artificial potential field method is easy to trap in a local minimum value when the search step length is small, is difficult to trap in the local minimum value when the search step length is too large, but can cause the problems of discrete path points, not smooth enough, and can not completely guarantee obstacle avoidance, and can not overcome the problem of shaking of the mechanical arm in operation.
Therefore, the mechanical arm obstacle avoidance path planning method based on the improved artificial potential field method is a new control method provided aiming at the defects of the traditional mechanical arm in the obstacle avoidance process, can obtain a better obstacle avoidance effect, enables the operation of the mechanical arm to be more stable and smooth, and meets the production work task.
Disclosure of Invention
The invention solves the problems in the prior art, and provides a mechanical arm obstacle avoidance path planning method based on an improved artificial potential field method, which adopts the shortest distance from an obstacle to a connecting rod for collision detection, is different from a Cartesian space, utilizes the improved artificial potential field method to establish a target function in a joint space, combines the characteristics of two step lengths in the prior art for optimization, firstly adopts a large step length for searching, selects a point closest to the obstacle as a middle point, then adopts a small step length for searching, reaches the middle point first and then reaches a target point, and simultaneously needs to be fitted between the path points, combines a polynomial interpolation method in the joint space for track optimization, so that the mechanical arm can plan a path in a complex environment, and can simultaneously meet more complex work tasks.
The technical scheme adopted by the invention is that a mechanical arm obstacle avoidance path planning method based on an improved artificial potential field method comprises the following steps:
step 1: respectively modeling the mechanical arm and the barrier, and calculating the position and the posture of each joint of the mechanical arm in space through positive kinematics;
and 2, step: performing collision detection on the mechanical arm and the barrier, traversing the adjacent joint combination of the mechanical arm, if collision exists, omitting a group of currently corresponding joint angles, and if not, performing the next step;
and step 3: based on joint space optimization of the mechanical arm, constructing an objective function by using an improved artificial potential field method, and enabling the mechanical arm to move along the direction of the objective function which is reduced to the fastest speed until the mechanical arm can reach the vicinity of a target point;
and 4, step 4: judging whether the motion of the mechanical arm falls into a local minimum value, if so, searching and constructing an intermediate point in a variable step length mode, and carrying out the next step, otherwise, directly carrying out the next step; the traditional method for judging the local minimum value comprises the steps of measuring the distance or judging the surrounding potential field, observing that the angle fluctuation of each joint is in a smaller range when the mechanical arm runs for a certain iteration number and does not reach the vicinity of a target point, and judging that the local minimum value falls into;
and 5: the trajectory is smoothed by a polynomial interpolation method to obtain a complete running trajectory of each joint of the mechanical arm, and the adopted interpolation function is a quintic polynomial interpolation function, so that the continuity of angular velocity and angular acceleration of the joints in the motion process can be ensured, and constraint conditions can be increased.
Preferably, in step 1, the kinematic modeling of the mechanical arm includes the following steps:
step 1.1: a coordinate system is established at each joint of the mechanical arm, and the establishment rule of the coordinate system is to determine a z-axis at first, wherein the z-axis direction is vertical to a rotation plane of the joint, and the positive direction is determined according to the right-hand rule. The x-axis direction is then determined, the x-axis being along the common perpendicular to the adjacent z-axis. y is finally determined by a right-hand determination. Obtaining the specific position of each joint of the mechanical arm in space according to the rotation transformation relation between adjacent coordinate systems, further determining the current state of the mechanical arm, and simultaneously simplifying the shape of the mechanical arm by adopting a cylindrical envelope method to ensure that the expansion radius is r 0 ；
Step 1.2: according to parameter information among all connecting rods of the mechanical arm, including the length of the connecting rod, the torsion of the connecting rod, the offset of the connecting rod and the joint corner, carrying out DH method modeling to obtain a pose transformation relation among adjacent joint coordinate systems; the DH modeling method is a modeling method proposed by Denavit and Hartenberg, mainly used in robot kinematics, the method establishes a coordinate system on each connecting rod, realizes the transformation of coordinates on two connecting rods through homogeneous coordinate transformation, and can establish the relation of a first coordinate system and a last coordinate system by using the homogeneous coordinate transformation for many times in a system with a plurality of connecting rods connected in series.
Step 1.3: obtaining the position and attitude of each joint coordinate system of the mechanical arm relative to the base coordinate system of the mechanical arm base in a three-dimensional space by positive kinematics; the pose transformation relation of each joint relative to a base coordinate system can be deduced by calculating the pose transformation relation of adjacent joints, wherein the pose of the first joint is
The pose transformation relationship between the first joint and the second joint is
Obtain the second joint position and posture of
The pose transformation relationship between the second joint and the third joint is
Obtain the third joint position and posture of
By analogy, the nth joint position is
Is calculated by the formula
Step 1.4: obtaining the three-dimensional coordinates of each joint of the mechanical arm in space according to the pose of each joint of the mechanical arm, further obtaining the mathematical expression of the line segment equivalent to the mechanical arm,
wherein (x) 1 ,y 1 ,z 1 )、(x 2 ,y 2 ,z 2 ) Respectively are coordinates of two ends of the line segment.
Preferably, the relative pose of the connecting rod i in the connecting rod { i-1} coordinate system is obtained by performing rotational translation on the { i-1} coordinate system, and the homogeneous transformation expression of the rotational translation around the joint axis z axis and the rotation and translation around the common vertical line x axis is as follows:
wherein Rot is a rotation matrix, and coordinates are multiplied by Rot to obtain a rotated attitude; and similarly, the Trans is a translation matrix, and the coordinates are multiplied by the Trans to obtain the translated posture.
Transformation of a coordinate system in space generally refers to rotation and/or translation.
According to the principle of DH modeling, a coordinate system is firstly rotated by theta angle around a z-axis and translated by d along the z-axis i And then α is performed around the x-axis i Angular rotation and translation along the x-axis i To obtain the pose transformation relation between the adjacent joint coordinate systems,
wherein S = sin θ i 、C＝cosθ i Is the angle between the common normal of one joint and the next joint and the common normal of the previous joint, i.e. the joint angle, is a variable and is defined as the angle between the common normal of one joint and the next joint and the common normal thereof and the previous joint around the joint axis, d i The distance along the joint axis of a common normal to a joint and the last common normal, i.e. the link offset, is constant, a i Is the common normal length between the axes of the two joints, i.e. the length of the connecting rod, alpha i Is the angle by which the axis of one joint is rotated about their common perpendicular with respect to the axis of the other joint, i.e. the link torsion, is constant.
Preferably, in step 1, the obstacle is modeled by using an envelope box model, for example, the obstacle is processed by using a bounding sphere model, so that the radius of the bounding sphere is r 1 。
Preferably, in the collision detection, the distance d from each link of the mechanical arm to the center of the obstacle is calculated, and if d is larger than or equal to r 0 +r 1 +d safe No collision will occur, otherwise a collision will occur, wherein d safe For a safe distance, r 0 Radius of the arm, r 1 Is the outer diameter of the obstruction;
let the coordinates of both ends of any connecting rod be A (x) 1 ,y 1 ,z 1 ),B(x 2 ,y 2 ,z 2 ) The center coordinate of the obstacle is O (x) 0 ,y 0 ,z 0 ) Making a perpendicular line to the line segment AB through O, and setting the vertical foot as N (x) N ,y N ,z N ) (ii) a The following vector is calculated:
according to vector vertical relation
To obtain
(x N -x 0 )(x 2 -x 1 )+(y N -y 0 )(y 2 -y 1 )+(z N -z 0 )(z 2 -z 1 )＝0；
Point N is on straight line AB, collinear according to the vector
And
the two lines are collinear with each other,
substituting the coordinates of the N points into the formula to obtain
And then substituting k into the above equation to obtain the coordinate of the foot, wherein when the coordinate of the foot is in the coordinate range between two points of the line segment, the foot is ON the line segment, the shortest distance d is the distance of ON, otherwise d is the minimum distance between OA and OB.
Preferably, the artificial potential field method mainly includes a function of attractive force and a function of repulsive force, and the attractive force and the repulsive force interact with each other, so that the mechanical arm can avoid the obstacle. Wherein the range of action of the attractive force is global and the range of action of the repulsive force is local. In the step 3, the objective function constructed by the improved artificial potential field method is
Taking Nlink as the number of connecting rods, nobs as the number of obstacles, n and m as indexes of the connecting rods and the obstacles respectively, theta as each joint angle of the mechanical arm, uatt as a gravitational function and Urep as a repulsive force function; when the objective function is for a six degree of freedom robotic arm, the corresponding set of joint angles (θ) is six.
Preferably, the gravity function is calculated by the euclidean distance between the end of the robot arm and the target point, and is,
wherein xi is a gravitation coefficient, q is a group of joint angles of the mechanical arm,
(x 0 ,y 0 ,z 0 ) As the end coordinates of the arm, (x) g ,y g ,z g ) Is the target point.
Preferably, the repulsion function is calculated according to the distance between the mechanical arm connecting rod and the barrier, the repulsion potential of the barrier to all connecting rods needs to be calculated, when the environment of the barrier is complex, in order to prevent the sudden change of the edge of the repulsion potential field, the repulsion function is improved, a cubic polynomial factor is introduced, the repulsion function is,
wherein δ is a repulsive coefficient, p 0 Is the repulsive force range of the obstacle (beyond this range, the repulsive force is zero), and p (q) is the link-to-obstacle distance.
Preferably, in the step 4, constructing the intermediate point by using the variable step size search includes the following steps:
step 4.1: defining a step length lambda based on the initial angle of each joint of the mechanical arm;
step 4.2: searching in adjacent joint space by the mechanical arm to obtain a series of joint angle combinations C n ＝{λ-θ 1 +λ,λ-θ 2 +λ,λ-θ 3 +λ,…,λ-θ n + λ } where θ is 1 ～θ n For each joint angle of the arm, C n All joint combinations representing adjacent joint spaces of the mechanical arm;
step 4.3: performing collision detection, traversing joint angle combinations, and deleting joint combinations which can collide; calculating the resultant potential energy, namely F (theta), of the remaining joint angle combinations, selecting a group of joint combinations with the minimum resultant potential energy as an optimal joint combination, and calculating the position of the tail end of the corresponding mechanical arm;
step 4.4: repeating the step 4.2 and the step 4.3 by taking the optimal joint combination as an optimal solution to generate an adjacent joint combination until the distance between the tail end of the mechanical arm and the target point is less than a preset value, and if d is less than 0.1m, carrying out the next step;
step 4.5: firstly, adopting a large step length, such as lambda =5 degrees, repeating the steps from 4.1 to 4.4, searching in a joint space of a mechanical arm, selecting a point closest to an obstacle as a middle point when obtaining a discrete path point of the tail end of the mechanical arm in the space, and simultaneously obtaining a group of joint angles corresponding to the middle point;
step 4.6: and (3) searching by adopting a small step length, such as lambda =1 degrees, taking the middle point as a target point, moving the mechanical arm from the initial point to the middle point, then moving the mechanical arm from the middle point to the target point until the distance between the tail end of the mechanical arm and the target point is smaller than a threshold value, and finishing.
In the invention, a large step length is adopted, for example, the joint angles swing up and down by 5 degrees, so that the situation of falling into a local minimum value is not easy, then a plurality of groups of joint angles are obtained by calculation according to the steps 4.1-4.4, each group of joint angles corresponds to one point, so that a plurality of discrete points in space are corresponding, a point closest to an obstacle is selected as a middle point between the points, and then a small step length is adopted, for example, the joint angles swing up and down by 1 degree, so that the mechanical arm can move from the initial point to the middle point and then move from the middle point to a target point.
Preferably, for discrete path points generated by small-step search, corresponding joint angle combinations are obtained, that is, the optimal set of joint angles calculated by each step, that is, the set of combinations with the smallest resultant energy, are constructed to construct an interpolation function, and θ (t) = a 0 +a 1 t+a 2 t 2 +a 3 t 3 +a 4 t 4 +a 5 t 5 Interpolating functions as a function of the angles connecting adjacent joints, such as the relationship of the change in time from one angle to the next;
so as to obtain the composite material,
wherein, theta,
Respectively, angle, angular velocity, angular acceleration, t, at the interpolation point f Setting the angular velocity and the angular acceleration as certain constants for the interpolation time of adjacent path points, substituting constraint conditions into a polynomial equation to obtain a polynomial coefficient,
and substituting the polynomial coefficients into an interpolation function to obtain a complete quintic polynomial interpolation function expression, and taking the interpolation function expression as a complete running track of each joint of the mechanical arm.
The invention relates to a mechanical arm obstacle avoidance path planning method based on an improved artificial potential field method, which is provided for the problem of obstacle avoidance path planning of a mechanical arm, and comprises the steps of firstly obtaining a DH parameter model of the mechanical arm through known mechanical arm structure parameter information, then carrying out kinematic modeling on the mechanical arm, and obtaining a pose transformation relation of adjacent joints of the mechanical arm; simultaneously, modeling is carried out on the barrier, a bounding box model is adopted for modeling, a model of the barrier can be obtained, then collision detection is carried out, and a geometric relation between the distance between the mechanical arm and the barrier is established; constructing a target function in a joint space of the mechanical arm by using an improved artificial potential field method, searching in an adjacent joint space of the mechanical arm, excluding the collided joint combination, finding a joint combination with the minimum target function in the rest joint combinations as a current optimal joint combination, continuously searching in the adjacent joint space on the basis, and repeating the previous iteration process until a tail end point of the mechanical arm reaches the vicinity of a target point; aiming at the problem of local minimum which the mechanical arm may encounter in the moving process, the method of variable step length searching is adopted for improvement, when the mechanical arm is judged to be trapped in the local minimum, large step length searching is adopted, a plurality of path points can be generated in a Cartesian space, then a point closest to an obstacle is selected as a middle point, the middle point is taken as a target point by adopting a small step length searching method, the mechanical arm moves to the middle point, and then the mechanical arm moves to the target point from the middle point, so that the problem of the local minimum can be solved; aiming at the problem of shaking of the mechanical arm in the obstacle avoidance process, the track of the mechanical arm is processed by adopting a quintic polynomial interpolation method, and the mechanical arm is constructed by adopting a quintic polynomial interpolation method in the joint space of the mechanical arm, so that the mechanical arm can run more stably and smoothly in the joint space.
The method has the advantages that the obstacle avoidance path planning of the mechanical arm is carried out by adopting an improved artificial potential field method, the shaking problem under the complex obstacle environment can be eliminated, meanwhile, the variable step length searching method is combined, the mechanical arm can jump out the local minimum value in the moving process, the running track of the mechanical arm is optimized by combining the quintic polynomial interpolation method, the mechanical arm can run more stably and smoothly, and the mechanical arm can avoid the obstacle while moving.
Drawings
FIG. 1 is a simplified model block diagram of a robotic arm according to the present invention;
FIG. 2 is a diagram of an envelope model of an obstacle according to the present invention;
FIG. 3 is a drawing showing the coordinate system of each joint on the robot arm according to the present invention;
FIG. 4 is a diagram of a simulated obstacle avoidance scene of a mechanical arm in the present invention;
fig. 5 is a flowchart of a method for planning an obstacle avoidance path of a mechanical arm according to the present invention.
Detailed Description
The present invention is described in further detail with reference to the following examples, but the scope of the present invention is not limited thereto.
Referring to fig. 1 to 4, a method for planning an obstacle avoidance path of a mechanical arm based on an improved artificial potential field method includes the following steps:
1) First, a robot arm and an obstacle are modeled
The mechanical arm is subjected to kinematic modeling, and a coordinate system is established at each joint of the mechanical arm, as shown in fig. 3, wherein each coordinate system x i y i z i Corresponding to the ith joint; and then, the specific position of each joint of the mechanical arm in the space can be obtained according to the rotation transformation relation between the adjacent coordinate systems, and the current state of the mechanical arm is further determined. Meanwhile, the shape of the mechanical arm is simplified by adopting a cylindrical envelope method, and the assumed expansion radius is r 0 As shown in fig. 1.
And carrying out DH method modeling according to parameter information among all the connecting rods of the mechanical arm to obtain a mathematical relationship among the adjacent connecting rods. The relative pose of the connecting rod i in the connecting rod { i-1} coordinate system is obtained by performing rotation translation on the { i-1} coordinate system, and the homogeneous transformation expression of the rotation translation around the joint axis z axis and the rotation and translation around the common vertical line x axis is as follows:
according to the DH modeling principle. The coordinate system firstly rotates and translates around the z axis and then rotates and translates around the x axis, and the pose transformation relation between the adjacent connecting rod coordinate systems is as follows:
wherein S = sin θ i ，C＝cosθ i Is the angle between the common normal of one joint and the next joint and the common normal of the previous joint, i.e. the joint angle, is variable, d i The distance along the joint axis, i.e. the link offset, between the common normal of a joint and the previous common normal is constant, a i Is the common normal length between the axes of the two joints, i.e. the length of the connecting rod, alpha i The angle by which the axis of one joint is rotated about their common perpendicular, i.e. the link torsion, relative to the axis of the other joint is constant.
And deducing the pose of each joint coordinate system of the mechanical arm relative to the base coordinate system in a three-dimensional space through positive kinematics. The pose transformation relation of each joint relative to the base coordinate system can be deduced by calculating the pose transformation relation of the adjacent joints. The first joint position is
The second joint position is
The third joint position and posture is
By analogy, the nth joint position is
The calculation formula is as follows:
according to the pose information of each joint of the mechanical arm, the three-dimensional coordinates of each joint of the mechanical arm in the space can be obtained, and further, the mathematical expression of the line segment equivalent to the mechanical arm can be obtained.
Wherein (x) 1 ,y 1 ,z 1 )、(x 2 ,y 2 ,z 2 ) Respectively are coordinates of two ends of the line segment.
Modeling an obstacle using an envelope box model, where the obstacle is treated with a bounding sphere model assuming a bounding sphere radius r 1 。
2) Performing collision detection, calculating the distance d from each link of the mechanical arm to the center of the obstacle, and defining a safe distance d safe . If d is greater than or equal to r 0 +r 1 +d safe No collision will occur, otherwise a collision will occur.
3) An objective function is constructed in a joint space of the mechanical arm by using an improved artificial potential field method, and the aim is to enable the mechanical arm to move along the direction of the objective function which is reduced to the fastest speed until the mechanical arm can reach the vicinity of a target point.
The manual potential field method mainly comprises a function of attractive force and a function of repulsive force, and the attractive force and the repulsive force interact with each other, so that the mechanical arm can avoid the obstacle. Wherein the range of action of the attractive force is global and the range of action of the repulsive force is local. Firstly, constructing a gravity function, wherein the gravity function is obtained by calculating the Euclidean distance between the tail end of the mechanical arm and a target point, and the calculation formula is as follows:
wherein: xi is a gravity coefficient, and 30 is taken during simulation;
q is a set of joint angles of the mechanical arm,
(x 0 ,y 0 ,z 0 ) In the form of the end coordinates,
(x g ,y g ,z g ) Is the target point.
The repulsion function is calculated according to the distance between the mechanical arm connecting rod and the barrier, the repulsion potential energy of the barrier to all the connecting rods needs to be calculated, when the environment of the barrier is complex, in order to prevent the sudden change of the edge of the repulsion potential field, the repulsion function is improved, a cubic polynomial factor is introduced, and the construction function expression is as follows:
wherein: delta is the coefficient of repulsion force,
p 0 is a range of repulsive force of the obstacle,
and p (q) is the distance from the connecting rod to the obstacle.
Constructing an objective function in the joint space, and adding the above gravitational potential energy and repulsive potential energy together, wherein the formula is as follows:
wherein: nlink is the number of connecting rods, and Nobs is the number of obstacles.
4) And judging whether the local minimum value is trapped, and adopting a next step length-variable searching method when the local minimum value is trapped. The traditional method for judging the local minimum value is to measure the distance or judge the surrounding potential field, and when the mechanical arm runs for a certain iteration number and does not reach a target point, the angle fluctuation of each joint is observed in a smaller range, so that the local minimum value can be judged.
5) Method for constructing intermediate point by adopting variable step length search to avoid local minimum problem
Firstly, defining a large step length lambda for an initial angle of each joint of the mechanical arm, and then searching the mechanical arm in an adjacent joint space to obtain a series of joint angle combinations:
C n ＝{λ-θ 1 +λ,λ-θ 2 +λ,λ-θ 3 +λ,λ-θ 4 +λ,λ-θ 5 +λ,λ-θ 6 +λ}
wherein theta is 1 ～θ 6 For each joint angle of the arm, C n All joint combinations representing adjacent joint spaces of the robotic arm.
And then searching among the joint angle combinations, firstly performing collision detection, removing the combination if the calculated result is collided, then calculating the combined potential energy in the rest combinations, selecting a group of joint combinations with the minimum combined potential energy as the optimal joint combination, then regenerating a series of adjacent joint combinations by taking the current combination as the optimal solution, and repeating iterative optimization until the tail end of the mechanical arm can reach the vicinity of a target point. In this way, discrete path points are obtained in the working space of the mechanical arm, and a point close to the obstacle is selected as an intermediate point, and a group of joint angles corresponding to the intermediate point is obtained. And then searching by adopting a small step length, taking the middle point as a target point, moving the mechanical arm from the initial point to the middle point, and then moving the mechanical arm from the middle point to the target point until the tail end of the mechanical arm reaches the position near the target point.
6) Method for smoothing track by utilizing polynomial interpolation
According to the steps, joint angle combinations corresponding to a plurality of discrete rows of points in the mechanical arm obstacle avoidance path can be obtained, and then the motion function of each joint is constructed by combining an interpolation function method. The interpolation function adopted by the method is a quintic polynomial interpolation function, so that the continuity of the angular velocity and the angular acceleration of the joint in the motion process can be ensured, and the constraint condition can be increased.
θ(t)＝a 0 +a 1 t+a 2 t 2 +a 3 t 3 +a 4 t 4 +a 5 t 5
Wherein theta,
Respectively, angle, angular velocity, angular acceleration, t, at the interpolation point f Is the interpolation time of the adjacent path points. For convenient operation, the angular velocity and the angular acceleration may be set to be constant values, and then the above constraint conditions are substituted into the polynomial equation, so as to obtain the polynomial coefficient as follows.
The polynomial coefficient is substituted into the polynomial expression to obtain a complete fifth-order polynomial interpolation function expression, and further obtain the running track of each joint angle of the mechanical arm changing along with time.
The above examples merely represent one embodiment of the present invention and are not to be construed as limiting the scope of the invention. It should be noted that a person skilled in the art could make several alternative designs without departing from the inventive concept, which falls within the scope of the invention.
Claims (10)
1. A mechanical arm obstacle avoidance path planning method based on an improved artificial potential field method is characterized by comprising the following steps: the method comprises the following steps:
step 1: respectively modeling the mechanical arm and the barrier, and calculating the position and the posture of each joint of the mechanical arm in the space through positive kinematics;
and 2, step: performing collision detection on the mechanical arm and the barrier, traversing adjacent joint combinations of the mechanical arm, if collision exists, omitting a group of currently corresponding joint combinations, and if not, performing the next step;
and 3, step 3: based on joint space optimization of the mechanical arm, constructing a target function by using an improved artificial potential field method, and enabling the mechanical arm to move along the direction of the target function in the fastest direction until the mechanical arm can reach the vicinity of a target point;
and 4, step 4: judging whether the motion of the mechanical arm falls into a local minimum value, if so, searching and constructing an intermediate point in a variable step length mode, and carrying out the next step, otherwise, directly carrying out the next step;
and 5: and smoothing the track by utilizing a polynomial interpolation method to obtain the complete running track of each joint of the mechanical arm.
2. The mechanical arm obstacle avoidance path planning method based on the improved artificial potential field method as claimed in claim 1, characterized in that: in the step 1, modeling the mechanical arm includes the following steps:
step 1.1: establishing a coordinate system at each joint of the mechanical arm, obtaining the specific position of each joint of the mechanical arm in space according to the rotation transformation relation between adjacent coordinate systems, further determining the current state of the mechanical arm, and simplifying the shape of the mechanical arm by adopting a cylindrical envelope method to ensure that the expansion radius is r 0 ；
Step 1.2: according to parameter information among all connecting rods of the mechanical arm, carrying out DH method modeling to obtain a pose transformation relation among adjacent joint coordinate systems;
step 1.3: obtaining the position and attitude of each joint coordinate system of the mechanical arm relative to the base coordinate system of the mechanical arm base in a three-dimensional space by positive kinematics;
step 1.4: and obtaining the three-dimensional coordinates of each joint of the mechanical arm in the space according to the pose of each joint of the mechanical arm.
3. The mechanical arm obstacle avoidance path planning method based on the improved artificial potential field method as claimed in claim 2, characterized in that: the pose transformation relation is that,
wherein S = sin θ i 、C＝cosθ i Is the angle between the common normal of one joint and the next joint and the common normal of the previous joint, and is defined as a jointThe angle of rotation of the joint about the joint axis, d, of the joint's common normal to the next joint and its common normal to the previous joint i Is the distance of the common normal of a joint and the last common normal along the joint axis, a i Is the common normal length between the axes of the two joints, alpha i Is the angle by which the axis of one joint is rotated about their common perpendicular with respect to the axis of the other joint.
4. The mechanical arm obstacle avoidance path planning method based on the improved artificial potential field method as claimed in claim 1, characterized in that: in the step 1, an envelope box model is adopted to model the obstacle, a surrounding ball model is adopted to process the obstacle, and the radius of the surrounding ball is r 1 。
5. The mechanical arm obstacle avoidance path planning method based on the improved artificial potential field method as claimed in claim 1, characterized in that: in collision detection, calculating the distance d from each connecting rod of the mechanical arm to the center of the barrier, and if d is larger than or equal to r 0 +r 1 +d safe No collision will occur otherwise a collision will occur, wherein d safe For a safe distance, r 0 Radius of the arm, r 1 Is the outer diameter of the obstacle.
6. The mechanical arm obstacle avoidance path planning method based on the improved artificial potential field method as claimed in claim 1, characterized in that: in the step 3, the objective function constructed by the improved artificial potential field method is
Nlink is used as the number of connecting rods, nobs is used as the number of obstacles, n and m are respectively used as indexes of the connecting rods and the obstacles, theta is used as each joint angle of the mechanical arm, uatt is used as a gravitational function, and Urep is used as a repulsive force function.
7. The mechanical arm obstacle avoidance path planning method based on the improved artificial potential field method as claimed in claim 6, characterized in that: the function of the attractive force is that,
wherein xi is a gravitation coefficient, q is a group of joint angles of the mechanical arm,
(x 0 ,y 0 ,z 0 ) Is the end coordinate of the arm, (x) g ,y g ,z g ) Is the target point.
8. The mechanical arm obstacle avoidance path planning method based on the improved artificial potential field method as claimed in claim 6, characterized in that: the function of the repulsive force is such that,
wherein δ is a repulsive coefficient, p 0 P (q) is the distance from the link to the obstacle.
9. The mechanical arm obstacle avoidance path planning method based on the improved artificial potential field method as claimed in claim 1, characterized in that: in the step 4, constructing the intermediate point by adopting variable step size searching comprises the following steps:
step 4.1: defining a step length lambda based on the initial angle of each joint of the mechanical arm;
step 4.2: searching in adjacent joint space by mechanical arm to obtain a series of joint angle combinations C n ＝{λ-θ 1 +λ,λ-θ 2 +λ,λ-θ 3 +λ,λ-θ 4 +λ,λ-θ 5 +λ,λ-θ 6 + λ }, where θ 1 ～θ 6 For each joint angle of the arm, C n All joint combinations representing adjacent joint spaces of the mechanical arm;
step 4.3: based on the joint angle combination, deleting the combination which can generate collision; calculating the combined potential energy of the rest joint angle combinations, and selecting a group of joint combinations with the minimum combined potential energy as the optimal joint combination;
step 4.4: taking the optimal joint combination as an optimal solution, repeating the step 4.2 and the step 4.3 to generate an adjacent joint combination until the distance between the tail end of the mechanical arm and the target point is less than a preset value, and if d is less than 0.1m, carrying out the next step;
step 4.5: obtaining a plurality of discrete path points in the working space of the mechanical arm, selecting a point close to the obstacle as a middle point, and simultaneously obtaining a group of joint angles corresponding to the middle point;
step 4.6: and (3) searching by adopting a small step length, taking the middle point as a target point, moving the mechanical arm from the initial point to the middle point, then moving the mechanical arm from the middle point to the target point until the distance between the tail end of the mechanical arm and the target point is less than a threshold value, and finishing.
10. The mechanical arm obstacle avoidance path planning method based on the improved artificial potential field method as claimed in claim 9, wherein: for discrete path points generated by small step search, corresponding joint angle combinations are obtained, an interpolation function is constructed,
θ(t)＝a 0 +a 1 t+a 2 t 2 +a 3 t 3 +a 4 t 4 +a 5 t 5 ，
so as to obtain the compound with the characteristics of,
wherein, theta,
Respectively, angle, angular velocity, angular acceleration, t, at the interpolation point f The interpolation time of the adjacent path points is substituted into the polynomial equation to obtain the polynomial coefficient,
and substituting the polynomial coefficients into the interpolation function to obtain a complete quintic polynomial interpolation function expression, and taking the interpolation function expression as a complete running track of each joint of the mechanical arm.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210973726.8A CN115416016A (en) | 2022-08-15 | 2022-08-15 | Mechanical arm obstacle avoidance path planning method based on improved artificial potential field method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210973726.8A CN115416016A (en) | 2022-08-15 | 2022-08-15 | Mechanical arm obstacle avoidance path planning method based on improved artificial potential field method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115416016A true CN115416016A (en) | 2022-12-02 |
Family
ID=84198654
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210973726.8A Pending CN115416016A (en) | 2022-08-15 | 2022-08-15 | Mechanical arm obstacle avoidance path planning method based on improved artificial potential field method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115416016A (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115933688A (en) * | 2022-12-28 | 2023-04-07 | 南京衍构科技有限公司 | Multi-robot cooperative work obstacle avoidance method, system, equipment and storage medium |
| CN116352714A (en) * | 2023-04-11 | 2023-06-30 | 广东工业大学 | Mechanical arm obstacle avoidance path planning method |
| CN117162098A (en) * | 2023-10-07 | 2023-12-05 | 合肥市普适数孪科技有限公司 | Autonomous planning system and method for robot gesture in narrow space |
-
2022
- 2022-08-15 CN CN202210973726.8A patent/CN115416016A/en active Pending
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115933688A (en) * | 2022-12-28 | 2023-04-07 | 南京衍构科技有限公司 | Multi-robot cooperative work obstacle avoidance method, system, equipment and storage medium |
| CN115933688B (en) * | 2022-12-28 | 2024-03-29 | 南京衍构科技有限公司 | Multi-robot cooperative work obstacle avoidance method, system, equipment and storage medium |
| CN116352714A (en) * | 2023-04-11 | 2023-06-30 | 广东工业大学 | Mechanical arm obstacle avoidance path planning method |
| CN116352714B (en) * | 2023-04-11 | 2023-09-26 | 广东工业大学 | Mechanical arm obstacle avoidance path planning method |
| CN117162098A (en) * | 2023-10-07 | 2023-12-05 | 合肥市普适数孪科技有限公司 | Autonomous planning system and method for robot gesture in narrow space |
| CN117162098B (en) * | 2023-10-07 | 2024-05-03 | 合肥市普适数孪科技有限公司 | Autonomous planning system and method for robot gesture in narrow space |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110228069B (en) | Online obstacle avoidance motion planning method for mechanical arm | |
| CN108908331B (en) | Obstacle avoidance method and system for super-redundant flexible robot and computer storage medium | |
| CN115416016A (en) | Mechanical arm obstacle avoidance path planning method based on improved artificial potential field method | |
| CN113352319B (en) | Redundant mechanical arm obstacle avoidance trajectory planning method based on improved fast expansion random tree | |
| CN107490965B (en) | Multi-constraint trajectory planning method for space free floating mechanical arm | |
| CN108356819B (en) | Industrial mechanical arm collision-free path planning method based on improved A-x algorithm | |
| US9411335B2 (en) | Method and apparatus to plan motion path of robot | |
| US8825209B2 (en) | Method and apparatus to plan motion path of robot | |
| CN110682286B (en) | Real-time obstacle avoidance method for cooperative robot | |
| CN108068113B (en) | 7-DOF humanoid arm flying object operation minimum acceleration trajectory optimization | |
| CN110653805A (en) | Task constraint path planning method for seven-degree-of-freedom redundant manipulator in Cartesian space | |
| CN114147708B (en) | Mechanical arm obstacle avoidance path planning method based on improved longhorn beetle whisker search algorithm | |
| CN110561419B (en) | Arm-shaped line constraint flexible robot track planning method and device | |
| CN111761582A (en) | Mobile mechanical arm obstacle avoidance planning method based on random sampling | |
| CN113442140B (en) | Cartesian space obstacle avoidance planning method based on Bezier optimization | |
| CN114939872B (en) | MIRRT-Connect algorithm-based intelligent storage redundant mechanical arm dynamic obstacle avoidance motion planning method | |
| CN115890670A (en) | Method for training motion trail of seven-degree-of-freedom redundant mechanical arm based on intensive deep learning | |
| Liu et al. | Improved RRT path planning algorithm for humanoid robotic arm | |
| CN113650011B (en) | Method and device for planning splicing path of mechanical arm | |
| Chen et al. | Optimizing the obstacle avoidance trajectory and positioning error of robotic manipulators using multigroup ant colony and quantum behaved particle swarm optimization algorithms | |
| Vazquez-Santiago et al. | Motion planning for kinematically redundant mobile manipulators with genetic algorithm, pose interpolation, and inverse kinematics | |
| Ji et al. | E-RRT*: Path Planning for Hyper-Redundant Manipulators | |
| Zhuang et al. | Obstacle avoidance path planning for apple picking robotic arm incorporating artificial potential field and a* algorithm | |
| CN113043278B (en) | Mechanical arm track planning method based on improved whale searching method | |
| Ata et al. | COLLISION-FREE TRAJECTORY PLANNING FOR MANIPULATORS USING GENERALIZED PATTERN SEARCH. |
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 |