CN115464650A - Construction method of redundant manipulator obstacle avoidance model for dynamic obstacles - Google Patents
Construction method of redundant manipulator obstacle avoidance model for dynamic obstacles Download PDFInfo
- Publication number
- CN115464650A CN115464650A CN202211147769.7A CN202211147769A CN115464650A CN 115464650 A CN115464650 A CN 115464650A CN 202211147769 A CN202211147769 A CN 202211147769A CN 115464650 A CN115464650 A CN 115464650A
- Authority
- CN
- China
- Prior art keywords
- cost
- obstacle
- mechanical arm
- collision
- triangular
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
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
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
-
- 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)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
The invention discloses a construction method of a redundant manipulator obstacle avoidance model for a dynamic obstacle, belongs to the field of motion planning and dynamic obstacle avoidance of redundant manipulators, and aims to solve the problem that the mechanical arm is weak in pre-judgment capability for collision of the dynamic obstacle due to the fact that a continuous obstacle avoidance model capable of reflecting the geometric configuration of the mechanical arm and having the capability of predicting and judging whether the dynamic obstacle collides with the mechanical arm is lacked in the prior art. The mechanical arm connecting rod is regarded as the side of a triangle instead of being dispersed into a point set or a ball set, and the inverse kinematics solution of the redundant mechanical arm is calculated by combining the self-motion item of the gradient projection method, so that the problem of dynamic obstacle avoidance of the mechanical arm in the task execution process is solved.
Description
Technical Field
The invention belongs to the field of motion planning and dynamic obstacle avoidance of redundant manipulator, and particularly relates to a construction method of a redundant manipulator obstacle avoidance model for a dynamic obstacle.
Background
When a robotic arm is operated in a dynamic environment, moving obstacles can affect the motion and task performance of the robotic arm. This requires the robotic arm to have real-time obstacle avoidance capabilities. The degree of freedom of the redundant manipulator is more than that required by a working space, and the redundant manipulator has infinite configurations aiming at the same pose of the end manipulator. Therefore, the problem to be solved by the technology is the dynamic obstacle avoidance of the redundant manipulator body. The difficulty of the problem lies in how to model the body of the mechanical arm and reflect the motion relation between the mechanical arm and the obstacle.
At present, many obstacle avoidance methods based on mechanical arm kinematics exist, and the essential difference of the methods lies in different ways of constructing an obstacle avoidance model between a mechanical arm and an obstacle. Current obstacle avoidance methods can be divided into distance-based methods and velocity-based methods, depending on the type of information used in the model.
Distance-based methods are mainly based on position information of obstacles and mechanical arms. The most typical distance-based method is the obstacle avoidance method in document [1 ]. And taking a point on the mechanical arm body closest to the obstacle as an obstacle avoidance point, and giving the speed far away from the obstacle to the point. The documents [2] and [4] solve the problem of trajectory planning by combining an optimization method based on a collision detection method in computer graphics. In [2], the author calculates the boundary of an obstacle by estimating the trajectory of the moving obstacle in a short time, and establishes a cost function based on the distance between the discrete sphere model of the robot and the boundary of the position of the obstacle to realize trajectory planning. In document [3], the author proposes to use the swept volume as an obstacle avoidance model in successive time steps of the robot arm, and calculate the cost according to the distance between the swept volume and the obstacle. In document [4], the authors propose a strict convex hull generation method and generate collision-free humanoid robot optimization actions by calculating continuous gradient approach distances.
In a dynamic obstacle environment, the distance-based modeling method loses speed information of obstacles, lacks the prediction capability of whether collision occurs or not, causes the mechanical arm to react slowly when the mechanical arm avoids the dynamic obstacles, and reduces the safety of the system. When a person is in motion, they tend to predict the trajectory of objects within the field of view. If they think that the object is moving towards themselves, they take action to avoid it in advance even if the object is still relatively far away from them. Therefore, in order to achieve dynamic obstacle avoidance better, the motion state of the obstacle needs to be considered. In document [5], the author models the mechanical arm as a series of discrete spheres along a connecting rod and applies a velocity barrier method to the mechanical arm, thereby introducing velocity information of a velocity barrier into an obstacle avoidance model.
However, the current obstacle avoidance method is mainly a distance-based obstacle avoidance method, and the current obstacle avoidance method regards the mechanical arm link as a point set or a ball set. When the number of sampling points is too small, the discrete model can cause the joint speed to be discontinuous; when the number of sampling points is too large, the discrete model may cause an excessive amount of calculation. Therefore, the problem in the field of dynamic obstacle avoidance at present is that a continuous obstacle avoidance model based on speed information modeling is lacked. The model should be able to represent the geometric configuration of the mechanical arm and have the capability of predicting and judging whether a dynamic obstacle collides with the mechanical arm.
From the topological perspective, the triangular relationship between two adjacent connecting rods in a three-dimensional space is kept unchanged, so that the geometric configuration of the redundant mechanical arm can be described by the movement and deformation of a triangle, and the adaptive adjustment of the mechanical arm in a dynamic environment is converted into a dynamic obstacle avoidance of the triangle. The arm links are considered as sides of a triangle rather than being discretized into a collection of points or spheres. In order to solve the problems, a triangular collision surface model and three cost functions based on the model are provided, and the kinematics inverse solution of the redundant mechanical arm is calculated by combining the self-motion term of the gradient projection method [6], so that the problem of dynamic obstacle avoidance of the mechanical arm in the task execution process is solved.
Non-patent documents cited in the background art:
[1]A.A.Maciejewski and C.A.Klein,“Obstacle Avoidance for Kinematically Redundant Manipulators in Dynamically Varying Environments,”The International Journal of Robotics Research,vol.4,no.3,pp.109–117,Sep.1985.
[2]C.Park,J.Pan,and D.Manocha,“ITOMP:Incremental trajectory optimization for real-time replanning in dynamic environments,”in Proc.Int.Conf.Automat.Planning and Scheduling(ICAPS),2012,pp.207–215.
[3]J.Schulman,J.Ho,A.Lee,I.Awwal,H.Bradlow,and P.Abbeel,“Finding Locally Optimal,Collision-Free Trajectories with Sequential Convex Optimization,”Jun.2013.
[4]A.Escande,S.Miossec,and A.Kheddar,“Continuous gradient proximity distance for humanoids free-collision optimized-postures,”in 2007 7th IEEE-RAS International Conference on Humanoid Robots,Pittsburgh,PA,USA,Nov.2007,pp.188–195.
[5]L.Zhao,J.Zhao,and H.Liu,“Solving the Inverse Kinematics Problem of Multiple Redundant Manipulators with Collision Avoidance in Dynamic Environments,”J Intell Robot Syst,vol.101,no.2,p.30,Feb.2021.
[6]“Automatic Supervisory Control of the Configuration and Behavior of Multibody Mechanisms,”IEEE Trans.Syst.,Man,Cybern.,vol.7,no.12,pp.868–871,1977.
disclosure of Invention
The invention aims to solve the problem that the prior art lacks a continuous obstacle avoidance model which can embody the geometric configuration of a mechanical arm and has the capability of predicting and judging whether a dynamic obstacle collides with the mechanical arm, so that the capability of predicting and judging the collision of the mechanical arm on the dynamic obstacle in actual work is weaker, and further develops a construction method of a redundant mechanical arm obstacle avoidance model for the dynamic obstacle;
a construction method of a redundant manipulator obstacle avoidance model for a dynamic obstacle is realized by the following steps:
the method comprises the following steps: selecting two adjacent connecting rods in the mechanical arm as two sides of a triangle, connecting the starting point of the first side with the end point of the second side to form the triangle of the motion surface of the mechanical arm connecting rod in a three-dimensional space, and simultaneously providing three cost functions of motion state cost, head-on collision cost and approach time cost based on the geometric configuration of the triangle to form a triangle collision surface obstacle avoidance model for judging the relative motion relationship between the triangle and an obstacle;
step two: analyzing and constructing the cost of the motion state of the mechanical arm connecting rod forming a triangle in the three-dimensional space;
step three: analyzing and constructing the head-on collision cost of a triangle formed by the mechanical arm connecting rod in a three-dimensional space;
step four: analyzing and constructing the approaching time cost of the mechanical arm connecting rod for forming a triangle in the three-dimensional space;
step five: providing a total cost value H of the triangular collision surface model based on three cost values of the motion state cost, the head-on collision cost and the approach time cost obtained in the second step to the fourth step, wherein collision can be generated only if the three cost values of the motion state cost, the head-on collision cost and the approach time cost are not 0;
further, the specific construction method of the motion state cost in the second step is as follows:
step two, firstly: calculating unit normal vector N of barrier movement speed direction and plane P i Inner product of [ ] i I.e. angle of inclusion gamma i Cosine value of (d):
η i ＝W·(L i ×L i+1 ) (1)
in the formula: w is the unit tangent vector of the movement locus of the obstacle, L i And L i+1 Vectors of the ith arm rod and the (i + 1) th arm rod of the mechanical arm respectively;
step two: dividing the three-dimensional space into regions on the basis of a plane P, wherein a normal vector N is defined i The pointed region is the front side, the other side is the back side, and the motion state cost CMS of the front side is constructed f Expression (c):
wherein alpha is a motion proportionality coefficient;
step two and step three: dividing the three-dimensional space into regions on the basis of a plane P, wherein a normal vector N is defined i The pointed region is the front side, the other side is the back side, and the motion state cost CMS of the back side is constructed b Expression (c):
wherein alpha is a motion proportionality coefficient;
step two, four: calculating the inner product eta according to the step two i Selecting an expression of a motion state cost CMS in the region of the obstacle in the three-dimensional space, wherein the region of the obstacle in the three-dimensional space refers to the front side or the back side of the obstacle on the triangular collision surface;
further, the specific construction method of the head-on collision cost in the third step is as follows:
step three, firstly: constructing a plane equation of a space plane P where the triangular collision surface model is located:
A i x+B i y+C i z+D i ＝0 (4)
in the formula: a. The i ,B i And C i Unit normal vector N of plane P respectively i Three coordinate components in the x, y, z direction, D i Can pass through vertex V i Three-dimensional coordinates (x) of i ,y i ,z i ) Is obtained by i ＝-(A i x i +B i y i +C i Z i )；
Step three: build past obstacle position X obs And the linear equation parallel to the moving direction of the obstacle:
(x-x obs )/m＝(y-y obs )/n＝(z-z obs )/p (5)
in the formula: (x) obs ,y obs ,z obs ) Is the position X of the obstacle obs M, n, p are three coordinate components of a unit tangent vector W of the motion trail of the barrier in the directions of x, y and z;
step three: and C, relaxing the triangular collision surface model constructed in the step three, relaxing the original triangle according to a proportion epsilon, wherein epsilon is a relaxation coefficient, and the original triangle can correspond to the actual geometric volume of the actual mechanical arm:
in the formula:
the vertex position vector of the original triangular collision surface is obtained;
the vertex position vector of the collision surface of the relaxed triangle is shown;
step three and four: obstacle position X obtained based on step three or two obs And defining the head-on collision cost by a linear equation parallel to the movement direction of the obstacle and the triangular collision surface model relaxed in the third step:
in the formula: s i Is the center of gravity of the triangular collision surface, the position deviation delta i For the projection point C of the obstacle in the plane P in the direction of motion i And O i Distance between, mu i Is C i The maximum distance between the three triangular vertexes, and beta is a collision proportion coefficient;
further, the specific construction method of the approach time cost in the fourth step is as follows:
in the formula: d i Is an obstacle position X obs At a distance from the plane P,
the magnitude of the movement speed of the obstacle is shown, and rho is an approaching time proportionality coefficient;
further, the specific construction method of the total cost value H in the step five is as follows:
total cost value H of ith triangular collision surface i The method is obtained by adopting a product of three cost values of motion state cost, head-on collision cost and approach time cost, and the expression is as follows:
H i ＝CMS·CHOC·CAT (9)
in the formula: CMS represents the kinetic state cost, CHOC represents the head-on collision cost, CAT represents the approach time cost,
when the number of the arms of the redundant manipulator is large and more than one triangular collision surface is formed, the total cost value H of each triangular collision surface i And (5) overlapping to obtain the total cost value H of the whole mechanical arm.
Wherein H i Represents the total cost value of the ith triangular collision surface, H represents the total cost value of the mechanical arm, and w i Represents H i In the ratio of H, w i 1 is taken.
Compared with the beneficial effects generated by the prior art, the application has the following advantages:
the invention provides a construction method of a redundant manipulator obstacle avoidance model aiming at a dynamic obstacle, which can accurately predict whether collision occurs according to the motion condition of the obstacle. If the model judges that collision occurs, obstacle avoidance action is immediately carried out without the limitation of the distance range. The obstacle avoidance method based on the model improves the response speed of obstacle avoidance actions, realizes active obstacle avoidance of the mechanical arm, and improves the safety of the system. The other advantage is that the geometric continuity of the model ensures the smoothness of obstacle avoidance movement without considering the sampling number when the mechanical arm is changed into a discrete model, and the problems of large calculation amount caused by overlarge sampling number of the discrete model and discontinuous mechanical arm joint speed caused by the overlarge sampling number of the discrete model are solved.
Drawings
FIG. 1 is a simplified model of the construction of a robotic arm and obstacle in the present application;
FIG. 2 is an overview of a triangular collision surface model constructed in the present application;
FIG. 3 is a motion state cost construction diagram in the present application;
FIG. 4 is a head-on collision cost construction diagram in the present application;
FIG. 5 is a graph of the approach time cost components in the present application;
FIG. 6 is a flow chart of a mathematical calculation of a triangular collision surface model in the method described in the present application;
FIG. 7 is a block diagram of the overall control of the method described in the present application;
FIG. 8 is a graph showing the angle and angular velocity of the joints of the robot arm in a single triangular collision surface simulation experiment (simulation using a triangular collision surface method);
fig. 9 is a change curve of the joint angle and the angular velocity of the mechanical arm in a single triangular collision surface simulation experiment (an experiment using a triangular collision surface method);
fig. 10 is a change curve of the joint angle and the angular velocity of the mechanical arm in the simulation experiment of the obstacle avoidance method (simulation using the obstacle avoidance method);
fig. 11 is a change curve of the joint angle and the angular velocity of the mechanical arm in the simulation experiment of the obstacle avoidance method (experiment using the obstacle avoidance method);
FIG. 12 is a graph of variation of child values versus total cost values in a single triangular collision surface simulation experiment (simulation using a triangular collision surface method);
FIG. 13 is a graph of variation of the offspring value and the total cost value in a single triangular collision surface simulation experiment (experiment using the triangular collision surface method);
FIG. 14 is a schematic diagram of a triangular collision surface variation in a single triangular collision surface simulation experiment;
FIG. 15 is a comparison of critical distances between a single triangular collision surface simulation experiment and an obstacle avoidance point simulation experiment;
FIG. 16 is a graph showing the angle and angular velocity of the joints of the robot arm in a double-triangular collision surface simulation test (simulation using a triangular collision surface method);
fig. 17 is a change curve of the joint angle and the angular velocity of the mechanical arm in the double-triangular collision surface simulation experiment (experiment using the triangular collision surface method);
fig. 18 is a change curve of the joint angle and the angular velocity of the mechanical arm in the simulation experiment of the obstacle avoidance method (simulation using the obstacle avoidance method);
fig. 19 is a change curve of a joint angle and an angular velocity of a mechanical arm in an obstacle avoidance method simulation experiment (an experiment using an obstacle avoidance method);
FIG. 20 is a graph showing the change in the two triangular collision surface sub-cost values in a simulation experiment of a double triangular collision surface (simulation using the triangular collision surface method)
FIG. 21 is a graph showing the total cost value variation of two triangular collision surfaces in a simulation experiment of a double triangular collision surface (simulation using a triangular collision surface method)
FIG. 22 is a graph showing the change in the cost values of two triangular collision surfaces in a simulation test of a double triangular collision surface (test using the triangular collision surface method)
FIG. 23 is a graph showing the total cost value change of two triangular collision surfaces in a simulation experiment of a double triangular collision surface (experiment using the triangular collision surface method)
Fig. 24 is a comparison of critical distances between a double-triangular collision surface simulation experiment and an obstacle avoidance point method simulation experiment.
In FIG. 1, X obs Indicating the position of the obstacle, X obj Indicating the position of the target point, O, at which the robot arm performs the gripping task B For the basic coordinate system sigma of the mechanical arm B Of origin, x B ,y B ,z B Respectively, the xyz axes, L of the coordinate system 1 ,L 2 ,L 3 ,L 4 ,L 5 ,L 6 ,L 7 ,L n An arm lever representing a robot arm;
in fig. 2, P represents the plane of the triangle formed by two adjacent arms, and the plane P divides the space into two parts, which are called front and back, and are respectively denoted as f and b, and θ i Showing two adjacent arms L i And L i+1 The included angle between them;
in the context of figure 7 of the drawings,
representing the gradient of the total cost value H
J denotes the Jacobian matrix of the arm, J + Expressing the pseudo-inverse of the Jacobian matrix, I being the unit matrix, (I-J) + J) For the projection operator, X represents the position of the end-of-arm manipulator,
representing a desired velocity of movement of the end-of-arm manipulator to track the target point,
and
respectively representing the speeds of movement of the obstacle and the target point, K P And k is a scaling factor, and k is,
representing the angular velocity of the arm joint at the d-th discrete time step, Δ T representing the time interval, Δ Θ d Represents the mechanical arm joint angle increment at the d-th discrete time step, Θ d+1 Arm joint angle command, z, representing the d +1 discrete time step -1 Representing a delay of one sample period, U out Representing the system output, i.e. Θ d+1 。
Detailed Description
The first embodiment is as follows: the embodiment is described with reference to fig. 1 to 7, and the embodiment provides a method for constructing a redundant manipulator obstacle avoidance model for a dynamic obstacle, where the method is implemented by the following steps:
the method comprises the following steps: selecting two adjacent connecting rods in the mechanical arm as two sides of a triangle, connecting the starting point of the first side with the end point of the second side to form the triangle of the motion surface of the mechanical arm connecting rod in a three-dimensional space, and simultaneously providing three cost functions of motion state cost, head-on collision cost and approach time cost based on the geometric configuration of the triangle to form a triangle collision surface obstacle avoidance model for judging the relative motion relationship between the triangle and an obstacle;
step two: analyzing and constructing the cost of the motion state of the mechanical arm connecting rod forming a triangle in the three-dimensional space;
step three: analyzing and constructing the head-on collision cost of a triangle formed by the mechanical arm connecting rod in a three-dimensional space;
step four: analyzing and constructing the approaching time cost of a triangle formed by the mechanical arm connecting rod in a three-dimensional space;
step five: and providing a total cost value H of the triangular collision face model based on the three cost values of the motion state cost, the head-on collision cost and the approach time cost obtained in the second step to the fourth step, wherein collision can only occur if the three cost values of the motion state cost, the head-on collision cost and the approach time cost are not 0.
In this embodiment, fig. 6 is simplified to two steps shown by a dotted line frame in fig. 7, the position of the obstacle is obtained by a vision system, and the vertex of the triangle is obtained by the positive kinematics calculation of the mechanical arm. The gradient projection method decomposes the motion of the redundant manipulator into two terms, the motion of the end effector and the self-motion of the manipulator. A gradient projection method is used in the control strategy, the total cost value is calculated and the gradient is substituted into a self-motion item of the gradient projection method, a P control method for tracking the target pose is substituted into a terminal manipulator motion item, and finally the purpose that the redundant manipulator finishes obstacle avoidance while tracking the target pose is achieved.
The second embodiment is as follows: the present embodiment is described with reference to fig. 1 to 7, and the present embodiment is different from the first embodiment in that the method for constructing the exercise state cost in the step two is as follows:
step two, firstly: calculating unit normal vector N of barrier movement speed direction and plane P i Inner product of [ ] i I.e. angle of inclusion gamma i Cosine value of (d):
η i ＝W·(L i ×L i+1 ) (1)
in the formula: w is the unit tangent vector of the movement locus of the obstacle, L i And L i+1 Vectors of the ith arm rod and the (i + 1) th arm rod of the mechanical arm are respectively obtained;
step two: dividing the three-dimensional space into regions on the basis of a plane P, wherein a normal vector N is defined i The pointed region is the front side, the other side is the back side, and the motion state cost CMS of the front side is constructed f The expression of (c):
wherein alpha is a motion proportionality coefficient;
step two and step three: dividing the three-dimensional space into regions on the basis of a plane P, wherein a normal vector N is defined i The pointed region is the front side, the other side is the back side, and the motion state cost CMS of the front side is constructed b Expression (c):
wherein alpha is a motion proportionality coefficient;
step two, four: calculating the inner product eta according to the step two i And selecting an expression of the motion state cost CMS in the region of the obstacle in the three-dimensional space, wherein the region of the obstacle in the three-dimensional space refers to the obstacle on the front side or the back side of the triangular collision surface. Other components and connection modes are the same as those of the first embodiment.
The third concrete implementation mode: the second embodiment is different from the second embodiment in that a specific construction method of the head-on collision cost in the third step is as follows:
step three, firstly: constructing a plane equation of a space plane P where the triangular collision surface model is located:
A i x+B i y+C i z+D i ＝0 (4)
in the formula: a. The i ,B i And C i Unit normal vector N of plane P respectively i Three coordinate components in the x, y, z direction, D i Can pass through the vertex V i Three-dimensional coordinates (x) of i ,y i ,z i ) Is obtained by i ＝-(A i x i +B i y i +C i z i )；
Step three: build past obstacle position X obs And the equation of a straight line parallel to the moving direction of the obstacle:
(x-x obs )/m＝(y-y obs )/n＝(z-z obs )/p (5)
in the formula: (x) obs ,y obs ,z obs ) Is the position X of the obstacle obs M, n, p are three coordinate components of a unit tangent vector W of the motion trail of the obstacle in the directions of x, y and z;
step three: and (3) relaxing the triangular collision surface model constructed in the step three, relaxing the original triangle according to the proportion of the epsilon, wherein the epsilon is a relaxation coefficient, so that the original triangle can correspond to the actual geometric volume of the actual mechanical arm:
in the formula:
the vertex position vector of the original triangular collision surface is obtained;
the vertex position vector of the collision surface of the relaxed triangle is shown;
step three and four: obstacle position X obtained based on step three or two obs And defining the head-on collision cost by a linear equation parallel to the movement direction of the obstacle and the triangular collision surface model relaxed in the third step:
in the formula: o is i Is the center of gravity of the triangular collision surface, the position deviation delta i For the projection point C of the obstacle in the plane P in the direction of motion i And O i Distance between, mu i Is C i And the maximum distance between the three triangle vertexes, and beta is a collision proportion coefficient. Other components and connection modes are the same as those of the second embodiment.
In this embodiment, the motion state cost and the head-on collision cost are used to describe the relative motion relationship between the obstacle motion direction and the triangular collision surface. We consider that when both cost values are not zero, i.e. the obstacle is moving towards the plane P and the projected point of the obstacle along the direction of movement on the plane P is in the relaxed triangular collision plane, the obstacle will collide with the triangle. In addition, we also increase the cost of approach time to describe the urgency of the time when the collision occurred.
The fourth concrete implementation mode: the embodiment is described with reference to fig. 1 to fig. 7, and the difference between the embodiment and the specific embodiment is that the method for specifically constructing the approach time cost in the fourth step is as follows:
in the formula: d i Is the position X of an obstacle obs At a distance from the plane P,
and p is an approaching time proportionality coefficient, which is the size of the movement speed of the obstacle. Other components and connection modes are the same as those of the third embodiment.
The fifth concrete implementation mode: the present embodiment is described with reference to fig. 1 to 7, and the present embodiment is different from the fourth embodiment in that a specific method for constructing the total cost value H in the step five is as follows:
total cost value H of ith triangular collision surface i Using kinematic state costs, head-on collision costs and forcesThe near time cost is obtained by the product of three cost values, and the expression is as follows:
H i ＝CMS·CHOC·CAT (9)
in the formula: CMS represents the motion state cost, CHOC represents the head-on collision cost, and CAT represents the approach time cost;
when the number of the arms of the redundant manipulator is large and more than one triangular collision surface is formed, the total cost value H of each triangular collision surface i And (5) overlapping to obtain the total cost value H of the whole mechanical arm.
Wherein H i Represents the total cost value of the ith triangular collision surface, H represents the total cost value of the mechanical arm, and w i Represents H i In the ratio of H, w i 1 is taken. The other components and the connection mode are the same as those of the fourth embodiment.
The present invention is not limited to the above embodiments, and any person skilled in the art can make some changes or modifications to the above embodiments without departing from the scope of the invention, but still all simple modifications, equivalent changes and modifications to the above embodiments according to the technical spirit of the invention shall fall within the scope of the invention.
Example (b):
example 1: the embodiment is a simulation experiment performed on the basis of a single triangular collision surface, and common formats of the existing mechanical arm are both double-arm structures as experiment bases;
two sets of simulation and experiments are provided in the embodiment, and the triangular collision surface method provided by the application is compared with the traditional collision point method. In the simulation, an obstacle with uniform acceleration linear motion is arranged close to the mechanical arm, and a yellow table tennis ball is taken as a moving obstacle in the experiment, so that the mechanical arm can realize obstacle avoidance action under the condition of executing tasks (such as ensuring the position of an end manipulator to be unchanged in the simulation and the experiment).
Comparing the starting time of the two methods in fig. 8 and fig. 10 when the joint angle and the speed change, it can be seen that the time of using the triangular collision surface method to generate the obstacle avoidance motion is earlier than the time of using the obstacle avoidance point method to generate the obstacle avoidance motion, which improves the safety of the system. The changes in joint angle and joint velocity in the experiments shown in fig. 9 and 11 were the same as in the simulation. After the obstacle is captured by the vision system, whether the obstacle collides in the current motion state is judged by using the triangular collision plane model. And if collision occurs, taking obstacle avoidance action, otherwise, taking no action. At first, the obstacle speed is low due to the long distance, and the obstacle avoidance effect is weak. But the effect is gradually enhanced along with the approaching of the barrier until the collision problem is solved, and the barrier cannot collide with the mechanical arm while keeping the current movement speed direction unchanged.
Fig. 12 and 13 show the change of the cost value in the simulation and experiment of the triangular collision surface method, and it can be seen that in this case, mainly due to the decrease of the motion state cost and the head-on collision cost, the total cost value is finally reduced to 0, so as to implement the obstacle avoidance action. Fig. 14 shows the change of the original triangular collision surface (solid line) and the relaxed triangular collision surface (broken line) in the obstacle avoidance process, and the double-dashed line represents the projection point of the obstacle in the plane P along the speed direction. In fig. 15, the critical distance (the shortest distance between the obstacle and the mechanical arm) of the two methods is compared, and it can be seen that the triangular collision surface method proposed by us can ensure a larger critical distance, thereby ensuring the safety of the system.
Example 2: the embodiment is a simulation experiment performed based on a double-triangular collision surface, and is based on the so-called experiment foundation that the common formats of the existing mechanical arms are both double-arm structures
In the embodiment, the situation of two triangular collision surfaces is further analyzed, the two fingers are added at the tail end of the mechanical arm, and the tail end manipulator is used as one side of a triangle due to the long relative length of the tail end manipulator, so that the mechanical arm forms two triangular collision surfaces, the tail end manipulator of the mechanical arm realizes obstacle avoidance in the process of keeping the posture unchanged, and as can be seen from the comparison between the change of the joint angle and the angular speed of the mechanical arm in fig. 16 to fig. 19, the response speed of the triangular collision surface method is higher than that of the obstacle avoidance point method;
fig. 20 to 23 show the change of the cost value of the triangular collision surface method in the simulation and experiment processes, and it can be seen that in the obstacle avoidance situation, the total cost value of the two triangular collision surfaces is reduced due to the rapid reduction of the head-on collision cost of the two triangular collision surfaces. Fig. 24 compares the critical distances for the two methods, and it can be seen that the triangular collision surface method maintains a larger critical distance for the robotic arm.
Simulation and experiments of the two triangular collision surfaces can verify that the number of the triangular collision surfaces can be increased along with the increase of the number of the longer arm rods in the mechanical arm, so that effective obstacle avoidance can be realized under the condition of a plurality of triangles, and the three-dimensional robot has adaptability and expandability.
Claims (5)
1. A construction method of a redundant manipulator obstacle avoidance model for a dynamic obstacle is characterized by comprising the following steps: the method is realized by the following steps:
the method comprises the following steps: selecting two adjacent connecting rods in the mechanical arm as two sides of a triangle, connecting the starting point of the first side with the end point of the second side to form the triangle of the motion surface of the mechanical arm connecting rod in a three-dimensional space, and simultaneously providing three cost functions of motion state cost, head-on collision cost and approach time cost based on the geometric configuration of the triangle to form a triangle collision surface obstacle avoidance model for judging the relative motion relationship between the triangle and an obstacle;
step two: analyzing and constructing the cost of the motion state of the mechanical arm connecting rod forming a triangle in the three-dimensional space;
step three: analyzing and constructing the head-on collision cost of a triangle formed by the mechanical arm connecting rod in a three-dimensional space;
step four: analyzing and constructing the approaching time cost of the mechanical arm connecting rod for forming a triangle in the three-dimensional space;
step five: and providing a total cost value H of the triangular collision surface model based on three cost values of the motion state cost, the head-on collision cost and the approach time cost obtained in the second step to the fourth step, wherein collision can be generated only if the three cost values of the motion state cost, the head-on collision cost and the approach time cost are not 0.
2. The method for constructing the redundant manipulator obstacle avoidance model for the dynamic obstacle according to claim 1, wherein the method comprises the following steps: the specific construction method of the motion state cost in the second step is as follows:
step two, firstly: calculating unit normal vector N of barrier movement speed direction and plane P i Inner product of [ ] i I.e. angle of inclusion gamma i Cosine value of (d):
η i ＝W·(L i ×L i+1 ) (1)
in the formula: w is the unit tangent vector of the movement locus of the obstacle, L i And L i+1 Vectors of the ith arm rod and the (i + 1) th arm rod of the mechanical arm respectively;
step two: dividing the three-dimensional space into regions on the basis of a plane P, wherein a normal vector N is defined i The pointed region is the front side, the other side is the back side, and the motion state cost CMS of the front side is constructed f Expression (c):
wherein alpha is a motion proportionality coefficient;
step two and step three: dividing the three-dimensional space into regions on the basis of a plane P, wherein a normal vector N is defined i The pointed region is the front side, the other side is the back side, and the motion state cost CMS of the back side is constructed b Expression (c):
wherein alpha is a motion proportionality coefficient;
step two, four: calculating the inner product eta according to the two steps and the one step i And selecting an expression of the motion state cost CMS in the region of the obstacle in the three-dimensional space, wherein the region of the obstacle in the three-dimensional space refers to the obstacle on the front side or the back side of the triangular collision surface.
3. The method for constructing the redundant manipulator obstacle avoidance model for the dynamic obstacle according to claim 2, wherein the method comprises the following steps: the specific construction method of the head-on collision cost in the third step is as follows:
step three, firstly: constructing a plane equation of a space plane P where the triangular collision surface model is located:
A i x+B i y+C i z+D i ＝0 (4)
in the formula: a. The i ,B i And C i Respectively, unit normal vector N of plane P i Three coordinate components in the x, y, z direction, D i Can pass through the vertex V i Three-dimensional coordinates (x) of i ,y i ,z i ) Is obtained by i ＝-(A i x i +B i y i +C i z i )；
Step three: build past obstacle position X obs And the linear equation parallel to the moving direction of the obstacle:
(x-x obs )/m＝(y-y obs )/n＝(z-z obs )/p (5)
in the formula: (x) obs ,y obs ,z obs ) Is the position X of the obstacle obs M, n, p are three coordinate components of a unit tangent vector W of the motion trail of the obstacle in the directions of x, y and z;
step three: and (3) relaxing the triangular collision surface model constructed in the step three, relaxing the original triangle according to the proportion of the epsilon, wherein the epsilon is a relaxation coefficient, so that the original triangle can correspond to the actual geometric volume of the actual mechanical arm:
in the formula:
the vertex position vector of the original triangular collision surface is obtained;
the vertex position vector of the collision surface of the relaxed triangle is shown;
step three and four: obstacle position X obtained based on step three or two obs And defining the head-on collision cost by a linear equation parallel to the movement direction of the obstacle and the triangular collision surface model relaxed in the third step:
in the formula: o is i Is the center of gravity of the triangular collision surface, the position deviation delta i As projected point C of barrier in plane P along moving direction i And O i Distance between, mu i Is C i And the maximum distance between the three triangle vertexes, and beta is a collision proportion coefficient.
4. The method for constructing the redundant manipulator obstacle avoidance model for the dynamic obstacle according to claim 3, wherein the method comprises the following steps: the method for constructing the approach time cost in the fourth step comprises the following steps:
in the formula: d i Is the position X of an obstacle obs At a distance from the plane P,
and p is an approaching time proportionality coefficient, which is the size of the movement speed of the obstacle.
5. The method for constructing the redundant manipulator obstacle avoidance model for the dynamic obstacle according to claim 4, wherein the method comprises the following steps: the concrete construction method of the total cost value H in the step five is as follows:
total cost value H of ith triangular collision surface i The method is obtained by adopting a mode of multiplying three cost values of motion state cost, head-on collision cost and approach time cost, and the expression is as follows:
H i ＝CMS·CHOC·CAT (9)
in the formula: CMS represents the motion state cost, CHOC represents the head-on collision cost, and CAT represents the approach time cost;
when the number of the arms of the redundant manipulator is large and more than one triangular collision surface is formed, the total cost value H of each triangular collision surface i And (5) overlapping to obtain the total cost value H of the whole mechanical arm.
Wherein H i Represents the total cost value of the ith triangular collision surface, H represents the total cost value of the mechanical arm, and w i Represents H i In the ratio of H, w i 1 is taken.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211147769.7A CN115464650B (en) | 2022-09-19 | 2022-09-19 | Construction method of redundancy manipulator obstacle avoidance model for dynamic obstacle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211147769.7A CN115464650B (en) | 2022-09-19 | 2022-09-19 | Construction method of redundancy manipulator obstacle avoidance model for dynamic obstacle |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115464650A true CN115464650A (en) | 2022-12-13 |
| CN115464650B CN115464650B (en) | 2023-05-05 |
Family
ID=84371303
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211147769.7A Active CN115464650B (en) | 2022-09-19 | 2022-09-19 | Construction method of redundancy manipulator obstacle avoidance model for dynamic obstacle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115464650B (en) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105690410A (en) * | 2014-12-09 | 2016-06-22 | 丰田自动车株式会社 | Collision avoidance method, control device, and program |
| CN106584461A (en) * | 2016-12-21 | 2017-04-26 | 西安科技大学 | Method for optimizing inverse kinematic humanoid-arm configuration of 7-freedom-degree humanoid mechanical arm under multi-constraint condition |
| CN107729637A (en) * | 2017-10-09 | 2018-02-23 | 燕山大学 | Redundant degree of freedom manipulator motion planning and evaluation method based on arm shape angle range |
| CN110653805A (en) * | 2019-10-10 | 2020-01-07 | 西安科技大学 | Task constraint path planning method for seven-degree-of-freedom redundant manipulator in Cartesian space |
| CN110712203A (en) * | 2019-09-26 | 2020-01-21 | 苏州苏相机器人智能装备有限公司 | 7-degree-of-freedom mechanical arm inverse kinematics solving algorithm |
| CN114800491A (en) * | 2022-03-30 | 2022-07-29 | 北京精密机电控制设备研究所 | Redundant mechanical arm zero-space obstacle avoidance planning method |
| CN114998276A (en) * | 2022-06-14 | 2022-09-02 | 中国矿业大学 | Robot dynamic obstacle real-time detection method based on three-dimensional point cloud |
-
2022
- 2022-09-19 CN CN202211147769.7A patent/CN115464650B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105690410A (en) * | 2014-12-09 | 2016-06-22 | 丰田自动车株式会社 | Collision avoidance method, control device, and program |
| CN106584461A (en) * | 2016-12-21 | 2017-04-26 | 西安科技大学 | Method for optimizing inverse kinematic humanoid-arm configuration of 7-freedom-degree humanoid mechanical arm under multi-constraint condition |
| CN107729637A (en) * | 2017-10-09 | 2018-02-23 | 燕山大学 | Redundant degree of freedom manipulator motion planning and evaluation method based on arm shape angle range |
| CN110712203A (en) * | 2019-09-26 | 2020-01-21 | 苏州苏相机器人智能装备有限公司 | 7-degree-of-freedom mechanical arm inverse kinematics solving algorithm |
| CN110653805A (en) * | 2019-10-10 | 2020-01-07 | 西安科技大学 | Task constraint path planning method for seven-degree-of-freedom redundant manipulator in Cartesian space |
| CN114800491A (en) * | 2022-03-30 | 2022-07-29 | 北京精密机电控制设备研究所 | Redundant mechanical arm zero-space obstacle avoidance planning method |
| CN114998276A (en) * | 2022-06-14 | 2022-09-02 | 中国矿业大学 | Robot dynamic obstacle real-time detection method based on three-dimensional point cloud |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115464650B (en) | 2023-05-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Chen et al. | Collision-free motion planning for human-robot collaborative safety under cartesian constraint | |
| CN108908331B (en) | Obstacle avoidance method and system for super-redundant flexible robot and computer storage medium | |
| EP1972415B1 (en) | Robots with collision avoidance functionality | |
| JP2022539324A (en) | Motion planning for multiple robots in a shared workspace | |
| CN113442140B (en) | Cartesian space obstacle avoidance planning method based on Bezier optimization | |
| Mu et al. | A hybrid obstacle-avoidance method of spatial hyper-redundant manipulators for servicing in confined space | |
| CN114274129A (en) | Mechanical arm motion planning and control method, system and medium based on visual guidance | |
| Wu et al. | Interval search genetic algorithm based on trajectory to solve inverse kinematics of redundant manipulators and its application | |
| Bhuiyan et al. | Deep-Reinforcement-Learning-based Path Planning for Industrial Robots using Distance Sensors as Observation | |
| CN115464650B (en) | Construction method of redundancy manipulator obstacle avoidance model for dynamic obstacle | |
| Dai | Collision-free motion of an articulated kinematic chain in a dynamic environment | |
| Ye et al. | Trajectory planning of 7-DOF redundant manipulator based on ROS platform | |
| Spencer et al. | Collision avoidance techniques for tele-operated and autonomous manipulators in overlapping workspaces | |
| Murakami et al. | Motion planning for catching a light-weight ball with high-speed visual feedback | |
| Luo et al. | Segmented hybrid motion-force control for a hyper-redundant space manipulator | |
| JP7115090B2 (en) | Acceleration adjustment device and acceleration adjustment program | |
| CN113146637A (en) | Robot Cartesian space motion planning method | |
| Jin et al. | A synthetic algorithm for tracking a moving object in a multiple-dynamic obstacles environment based on kinematically planar redundant manipulators | |
| Mu et al. | Collision-free trajectory planning of redundant space manipulators based on pseudo-distance | |
| Qian et al. | Path planning approach for redundant manipulator based on Jacobian pseudoinverse-RRT algorithm | |
| Li et al. | Research on Obstacle Avoidance of a 7-DOF Manipulator | |
| Moosavian et al. | Forward Kinematics Development of a 3-RRS Parallel Manipulator for Real-Time Control | |
| JPH06238581A (en) | Modelling of objective, judgement of interference between objectives, and device therefor | |
| CN114643581B (en) | Double-mechanical-arm collision avoidance track planning method and system based on improved artificial potential field method | |
| Liu et al. | Task Planning of Manipulator Based on Dynamic Space Constraint and Torque Sensor |
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 |