CN115026816B - Mechanical arm tail end obstacle avoidance method based on virtual force - Google Patents
Mechanical arm tail end obstacle avoidance method based on virtual force Download PDFInfo
- Publication number
- CN115026816B CN115026816B CN202210643516.2A CN202210643516A CN115026816B CN 115026816 B CN115026816 B CN 115026816B CN 202210643516 A CN202210643516 A CN 202210643516A CN 115026816 B CN115026816 B CN 115026816B
- Authority
- CN
- China
- Prior art keywords
- obstacle
- mechanical arm
- tail end
- distance
- obstacle avoidance
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 72
- 239000012636 effector Substances 0.000 claims abstract description 31
- 238000004088 simulation Methods 0.000 claims abstract description 13
- 239000011159 matrix material Substances 0.000 claims description 33
- 230000009466 transformation Effects 0.000 claims description 27
- 238000004422 calculation algorithm Methods 0.000 claims description 5
- 238000012937 correction Methods 0.000 claims description 4
- 230000000694 effects Effects 0.000 claims description 4
- 238000013507 mapping Methods 0.000 claims description 4
- 238000013459 approach Methods 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 abstract description 4
- 230000007547 defect Effects 0.000 abstract description 3
- 238000013519 translation Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 6
- 238000004458 analytical method Methods 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000009776 industrial production Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
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/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)
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a virtual force-based mechanical arm tail end obstacle avoidance method, and belongs to the technical field of mechanical arm motion planning. According to the virtual force-based mechanical arm tail end obstacle avoidance method, the pseudo distance concept is utilized to construct the anti-collision constraint method, each obstacle comprises an analytical expression of a hypersurface, constraint conditions between the obstacle and the mechanical arm are expressed more qualitatively, complicated calculation in simulation is simplified, and the virtual force-based collision constraint-free formula is simple and easy to update the mechanical arm obstacle avoidance motion in real time. In the solving of the end obstacle avoidance problem, the defect that the traditional method only considers the distance is changed, and the dynamic repulsive force can generate smooth virtual force repulsive force through different speeds, distances and moving directions between the obstacle and the tail end of the mechanical arm, so that the obstacle avoidance speed is increased for the end effector, and the problem that the obstacle appears on the expected track of the tail end is solved.
Description
Technical Field
The invention relates to the technical field of mechanical arm motion planning, in particular to a mechanical arm tail end obstacle avoidance method based on virtual force.
Background
Trajectory planning of robotic arms is now a subject of a hot spot in the robot industry, which has been widely used in industrial production and industrial manufacturing. The main task of trajectory planning is to find an optimal collision-free configuration between the initial position and the final position. If the mechanical arm collides with surrounding objects in the working process of an unstructured environment, the task completion quality is affected, and even safety problems are caused, so that the obstacle avoidance method has become an important skill in the robot working process. And the decision-making force self-adaption is introduced to filter the pre-planned track in real time, so that the smoothness and stability of the robot motion are ensured. Since in practice the complexity of the obstacle model is different, the expression with euclidean distance is also more cumbersome and cannot be statistically expressed, which will lead to a poor tracking accuracy of the end trajectory.
The main task of the mechanical arm track planning is tail end track tracking and obstacle avoidance, the traditional obstacle avoidance method realizes joint obstacle avoidance through self-movement, when the obstacle appears on a tail end expected track, the track tracking task and the obstacle avoidance task can conflict, and the closed-loop control method for master-slave task conversion, which is proposed by a plurality of scholars at home and abroad, can effectively solve the problems, but the minimum Euclidean distance between the mechanical arm and the obstacle is difficult to detect along with the appearance of a plurality of obstacles, the calculation is complicated, and the mechanical arm has the problems of discontinuous movement and low error precision.
Disclosure of Invention
1. Technical problem to be solved
The present application aims to provide a virtual force based robot arm tip obstacle avoidance method that is advantageous over the prior art described in the background art in at least one aspect.
2. Technical proposal
The aim of the invention is achieved by the following technical scheme:
A mechanical arm tail end obstacle avoidance method based on virtual force comprises the following steps:
S1, establishing a mechanical arm kinematics model: establishing a mechanical arm kinematics model by adopting a standard D-H method, and carrying out coordinate transformation by using a homogeneous transformation matrix of a mechanical arm joint coordinate system relative to a base coordinate system:
In formula (1), i (i=1, 2, …, n) is a robot arm joint number, cos θ i,sinθi,cosαi-1,sinαi-1 is abbreviated as cθ i,sθi,cαi-1,sαi-1;Rot(xi-1,αi-1 for simplicity of expression as a transformation matrix for α i-1 angular translation about the x i-1 axis, trans (x i-1,ai-1) is a transformation matrix for α i-1 angular translation along the x i-1 axis, rot (z i,θi) is a transformation matrix for θ i angular translation about the z i axis, trans (z i,di) is a transformation matrix for d i angular translation about the z i axis;
the transformation matrix of the tail end of the mechanical arm relative to the basic coordinate system passes through the transformation matrix of each rod piece The method comprises the following steps:
S2, building a mechanical arm-obstacle model: the modeling of various obstacles is unified in a simple and original mode, the traditional Euclidean distance is replaced, and the position of any point q c (x, y, z) in the working space of the mechanical arm under an obstacle coordinate system o obs-xobsyobszobs is calculated and used for judging the pseudo distance between the mechanical arm connecting rod and the obstacle; pseudo-range is used to describe the positional relationship between the mechanical arm and the spatial obstacle, and the basic idea is to establish the distance condition between the mechanical arm and the obstacle through a nonlinear function equation.
S3, calculating a pseudo distance minimum value: the minimum pseudo distance value between the mechanical arm and the obstacle is expressed by adopting a vector method so as to clearly express the constraint condition between the obstacle and the mechanical arm;
S4, improving a virtual repulsive force function: improving the virtual repulsive force function in consideration of the speed and the movement direction of the end effector, and solving a negative gradient function; bringing the minimum pseudo distance value obtained in the step S3 into an improved virtual repulsive force function, obtaining virtual repulsive force between the tail end of the mechanical arm and the obstacle, and planning the tail end track of the mechanical arm;
S5, realizing a terminal obstacle avoidance task by utilizing an algorithm: and (3) bringing the virtual repulsive force obtained in the step (S4) into a mechanical arm terminal obstacle avoidance method to realize the mechanical arm terminal obstacle avoidance.
Further, in step S2, a mechanical arm-obstacle model is built, and the method for constructing an anti-collision constraint by using the pseudo-distance concept includes a hypersurface analysis expression, so that constraint conditions between the obstacle and the mechanical arm are more qualitatively expressed, a coordinate system is built by taking the geometric center of the obstacle as an origin, and a hypersurface pseudo-distance analysis equation from a space point to an obstacle sphere is as follows:
Where S p (x, y, z) is the pseudo-distance in the obstacle coordinate space, q c (x, y, z) is the coordinate position of any point in space, and (x 0,y0,z0) is the hypersurface center point. R s＝robs+ri is a preset obstacle safety distance, R obs is an obstacle envelope radius, and R i is a simulated mechanical arm thickness radius.
Furthermore, in step S2, the pseudo distance between the mechanical arm connecting rod and the obstacle is determined, the corresponding pseudo distance is calculated at any point q c (x, y, z) in the task space, in order to meet the safety of the obstacle avoidance process, the minimum pseudo distance S p,min(qc in the obstacle avoidance process is always larger than the preset threshold d pm of the obstacle envelope,The mechanical arm is in a safe state.
Further, the vector method in step S3 is expressed as: s p(x,y,z)＝XTQX+BT X+C
Wherein ,X＝[x,y,z]T,Q＝diag[1/Rs 2,1/Rs 2,1/Rs 2],B＝[-2x0/Rs 2,-2y0/Rs 2,-2z0/Rs 2]T,Q、B、C are constant.
Further, in step S4, the trajectory of the end of the mechanical arm is planned by using the virtual attractive and repulsive force, that is, a novel virtual repulsive force is generated between the obstacle envelope body and the end effector, and the trajectory of the end effector is corrected to complete the situation that the obstacle appears on the expected trajectory.
Further, in step S4, the repulsive force function is:
Where F dyn (x, v) is the virtual force generated by velocity and position, S p (x) is the pseudo-distance between the end of the robot arm and the obstacle, and β is a constant to adjust the effect of the velocity vector angle θ, which represents the angle between the current velocity vector v and the end effector position q c (position relative to the obstacle).
Further, in step S4, the formula is based onCalculating when theta is atWhen the robot approaches an obstacle, the dynamic repulsive field starts to work; when the value of theta is atIn the inner period, the robot is far away from the obstacle, and the dynamic repulsive field does not work.
Further, in step S5, the end speed of the robot arm is determined based on the virtual repulsive force generated in step S4Correcting, namely correcting a kinematic equation of a traditional obstacle avoidance algorithm:
Wherein, The jacobian matrix J is the angular velocity vector of the mechanical arm jointAt the position ofLinear mapping of [ (]N, m is the dimension of joint space and operation space), J 0 is a jacobian matrix corresponding to the minimum euclidean distance projection point of the obstacle on the connecting rod, n=i-J +J,I∈Rn×n is an identity matrix, J + represents pseudo-inversion of the matrix, and J +＝JT(JJT)-1; The speed adjustment amount for the virtual repulsive force F dyn (x, v) is generated.
Further, when the obstacle appears on the expected track of the end effector in step S5, that is, the closest distance point is the position of the end effector of the mechanical arm, the track of the movement speed of the end is changed, the end effector is continuously approaching the obstacle, the minimum pseudo distance is reduced, and meanwhile, the θ is the following distanceWhen the virtual repulsive force is continuously increased, a larger speed is generated for correction, and the end effector acts on the end effector to avoid the obstacle on the expected track.
3. Advantageous effects
Compared with the prior art, the invention has the advantages that:
(1) According to the virtual force-based mechanical arm tail end obstacle avoidance method, the nearest point and the minimum pseudo distance value to the obstacle on the mechanical arm connecting rod are obtained through the vector method, the problem that the obstacle cannot be avoided when the obstacle appears on the expected track is solved by comparing the distance value with the preset threshold value, meanwhile, the expected track is quickly restored after the obstacle avoidance is completed, the track tracking task is realized, and the virtual force-based mechanical arm tail end obstacle avoidance method has very important practical significance;
(2) According to the virtual force-based mechanical arm tail end obstacle avoidance method, the pseudo distance concept is utilized to construct the anti-collision constraint method, each obstacle comprises an analytical expression of a hypersurface, constraint conditions between the obstacle and the mechanical arm are expressed more qualitatively, complicated calculation in simulation is simplified, and the virtual force-based collision constraint-free formula is simple and easy to update the mechanical arm obstacle avoidance motion in real time.
(3) According to the mechanical arm tail end obstacle avoidance method based on the virtual force, in solving the problem of tail end obstacle avoidance, the defect that only the distance is considered in the traditional method is changed, smooth virtual force repulsive force can be generated by dynamic repulsive fields through different speeds, distances and moving directions between the obstacle and the mechanical arm tail end, the obstacle avoidance speed is increased for the tail end actuator, the problem that the obstacle appears on a tail end expected track is solved, and meanwhile, the robot is guaranteed to recover to an original motion state after passing through the obstacle;
(4) According to the mechanical arm tail end obstacle avoidance method based on the virtual force, simulation results show that when the enveloping obstacle suddenly appears on the expected task track of the tail end actuator, the tail end actuator can stably avoid the obstacle and quickly recover to the expected track when the nearest distance between the tail end actuator and the obstacle is about 0.02m, meanwhile, the mechanical arm can accurately reach the target position, and the tail end obstacle avoidance method is verified to be capable of realizing tail end obstacle avoidance.
Drawings
FIG. 1 is a flow chart of an obstacle avoidance method of the present invention.
FIG. 2 is a diagram of a 7-DOF mechanical arm kinematic model.
FIG. 3 is a schematic view of the closest point of the robot arm and the obstacle sphere.
Fig. 4 is a schematic view of the end repulsive force angle.
FIG. 5 is a schematic diagram of the robot arm linkage obstacle avoidance process of the method of the present invention.
Fig. 6 is a schematic diagram of minimum pseudo-range variation during obstacle avoidance.
Fig. 7 is a schematic diagram of a robot arm tip trajectory error.
FIG. 8 shows the magnitude of the virtual force variation during the obstacle avoidance process.
Fig. 9 is a schematic view of the change of the angle of each joint of the mechanical arm.
Detailed Description
For a further understanding of the present invention, the present invention will be described in detail with reference to the drawings and examples.
Examples
The method for avoiding the obstacle at the tail end of the mechanical arm based on the virtual force in the embodiment is as shown in figure 1,
S1, establishing a mechanical arm kinematics model: establishing a mechanical arm kinematics model by adopting a standard D-H method, and carrying out coordinate transformation by using a homogeneous transformation matrix of a mechanical arm joint coordinate system relative to a base coordinate system:
In formula (1), i (i=1, 2, …, n) is a robot arm joint number, cos θ i,sinθi,cosαi-1,sinαi-1 is abbreviated as cθ i,sθi,cαi-1,sαi-1;Rot(xi-1,αi-1 for simplicity of expression as a transformation matrix for α i-1 angular translation about the x i-1 axis, trans (x i-1,ai-1) is a transformation matrix for α i-1 angular translation along the x i-1 axis, rot (z i,θi) is a transformation matrix for θ i angular translation about the z i axis, trans (z i,di) is a transformation matrix for d i angular translation about the z i axis;
the transformation matrix of the tail end of the mechanical arm relative to the basic coordinate system passes through the transformation matrix of each rod piece The method comprises the following steps:
The specific parameters are shown in table 1,
The coordinates of each node point under the base mark are obtained.
The transformation matrix of the tail end of the mechanical arm relative to the basic coordinate system passes through the transformation matrix of each rod pieceThe method comprises the following steps: the mapping relation between the speed of the end effector of the robot and the speed of the joint, and the transformation matrix reflecting the relation between the speed of the end effector and the speed of the joint is called a jacobian matrix and is expressed as a formula (2): In the method, in the process of the invention, Represents the speed of avoiding the obstacle, J is the angular velocity vector of the joint of the mechanical armAt the end velocity vectorLinear mapping of [ (]N, m is the dimension of joint space and working space).
S2, building a mechanical arm-obstacle model: in an actual test, the obstacle avoidance control method does not need to be accurately calculated, and only the operation space of the mechanical arm is calculated so as not to collide with an enveloping body where the obstacle is located. The application introduces pseudo distance as collision-free distance discrimination index to replace Euclidean distance, uses analytic function to represent multi-shape obstacle, uses geometric center of obstacle as origin to build coordinate system, and uses hypersurface pseudo distance analytic equation from space point to obstacle sphere as follows
Modeling irregularly shaped obstacles using ideal geometric spheres, i.e. the hypersurface pseudo-distance expression from space point to obstacle is
In the method, in the process of the invention,Representing pseudo-ranges in the obstacle coordinate space,Representing the obstacle envelope shape function, s a＝(h1,h2,h3, m, n, p) is the geometric parameter of the obstacle bag He Ti function, q c (x, y, z) is the coordinate position of any point in space, and (x 0,y0,z0) is the hypersurface center point. R s＝robs+ri, which is a preset obstacle safety distance, R obs is a given obstacle radius, and R i is a maximum radius of a fitting connecting rod cylinder.
Fig. 3 shows that any point q c (x, y, z) in the robot arm task space, based on equation (2), a corresponding pseudo-distance is calculated, in order to meet the safety of the obstacle avoidance process, the minimum pseudo-distance S p,min(qc) in the obstacle avoidance process is always greater than the preset threshold d pm of the obstacle envelope,Judging whether the mechanical arm connecting rod is just contacted with the surface of the obstacle or not, and judging whether the mechanical arm is in a safe state or not. According to the method for constructing the anti-collision constraint by using the pseudo-distance concept, each obstacle comprises an analytical expression of a hypersurface, constraint conditions between the obstacle and the manipulator are expressed more qualitatively, complicated calculation in simulation is simplified, and the obstacle avoidance movement of the manipulator is updated simply and easily in real time based on a virtual force collision-free constraint formula.
S3, calculating a pseudo distance minimum value: expressing a minimum pseudo distance value between the mechanical arm and the obstacle by adopting a vector method; the vector method is expressed as: s p(x,y,z)＝XTQX+BT X+C
Wherein ,X＝[x,y,z]T,Q＝diag[1/Rs 2,1/Rs 2,1/Rs 2],B＝[-2x0/Rs 2,-2y0/Rs 2,-2z0/Rs 2]T,Q、B、C are constant. The nearest point and minimum pseudo distance value from the obstacle on the mechanical arm connecting rod are obtained through a vector method, the problem that obstacle avoidance cannot be completed when the obstacle appears on the expected track is solved according to comparison of the distance value and a preset threshold value, meanwhile, the expected track is quickly restored after obstacle avoidance is completed, a track tracking task is realized, and the method has very important practical significance.
S4, improving a virtual repulsive force function: the end track of the mechanical arm is planned by adopting virtual attractive and repulsive force, namely a novel virtual repulsive force is generated between the obstacle enveloping body and the end effector, the end effector track is corrected, the traditional potential field method only plans the distance between the end effector and the obstacle, the influence of the speed and the movement direction of the end effector is ignored, and the obstacle avoidance movement only considers the distance to be insufficient, so that the possibility of collision can be caused. To solve these problems, the repulsive potential field function is improved as follows:
where F dyn (x, v) is the virtual force generated by velocity and position, S p (x) is the pseudo-distance between the end of the arm and the obstacle, and β is a constant, the effect of the velocity vector angle θ, shown in FIG. 4 as θ, is the angle between the current velocity vector v and the end effector position q c (position relative to the obstacle) can be adjusted. And may be calculated using a formula. When theta is at When the robot approaches an obstacle, the dynamic repulsive field starts to work; when the value of theta is atIn the inner, the robot is far away from the obstacle, and the dynamic repulsive force field should not work.
S5, realizing a terminal obstacle avoidance task by utilizing an algorithm: the terminal obstacle avoidance method based on the virtual force allows three factors of speed, position and direction to be considered by repulsive force generated by the dynamic potential field, so that the obstacle avoidance behavior of the robot can be improved. The repulsive force is inversely proportional to the distance to the obstacle s p, proportional to the velocity v, and when the velocity v is zero or s p is greater than the threshold, the repulsive force is also related to the angle between the velocity vector v and the direction vector to the obstacle, and if the angle θ is less than 90 ° (i.e., the end effector is away from the obstacle), the repulsive force is zero.
Virtual repulsive force generated based on the above is applied to tail end speed of mechanical armAnd (3) performing correction, namely:
Second item The avoidance speed related to the avoidance obstacle is related to the closest point on the arm link to the obstacle.For the speed adjustment amount generated by the virtual repulsive force, when the obstacle appears on the expected track of the end effector, namely the nearest distance point is the position of the end effector of the mechanical arm, the end movement speed track is changed, the end effector is continuously approaching the obstacle, the minimum pseudo distance is reduced, and meanwhile, the theta is positioned atThe virtual repulsive force is continuously increased, larger speed is generated for correction, and the end effector acts on the end effector to avoid the obstacle on the expected track. In the problem solving of the terminal obstacle avoidance, the defect that the traditional method only considers the distance is changed, smooth virtual force repulsive force can be generated by the dynamic repulsive field through different speeds, distances and moving directions between the obstacle and the tail end of the mechanical arm, the obstacle avoidance speed is increased for the terminal executor, the problem that the obstacle appears on the expected track of the tail end is solved, and meanwhile, the robot is guaranteed to recover to the original motion state after passing through the obstacle.
S6, simulation experiment and analysis
In order to verify the effectiveness and feasibility of the obstacle avoidance method provided by the invention, the method is applied to a LBRiiwa R280 seven-degree-of-freedom mechanical arm model as shown in fig. 2, and simulation experiments are carried out in matlab2020a software.
End desired trajectory: the starting point position and the end position are 0.3,0.3,0.6, and the expected track is:
initial joint angle of mechanical arm:
q＝(-0.1639，0.7263，1.2678，2.0117，-0.8201，-2.0944，-1.4991)
equivalent cylinder radius R=0.03m of mechanical arm connecting rod
Presetting an obstacle environment: experimental simulation is carried out under the condition that a plurality of obstacles exist, the effectiveness of the obstacle avoidance method is ensured, three spherical envelopments with the same size are preset for the obstacles, the radius r=0.03m, and the spherical center coordinates are respectively as follows by taking a mechanical arm base as a reference system
Obstacle ball 1 (zero space): o obs1 = (0.3,0,0.4);
obstacle ball 2 (zero space): o obs2 = (0,0.1,0.6);
Obstacle ball 3 (at the end locus): o obs3 = (0.1,0.3,0.6);
there are obstacles present both at the null space and on the end desired trajectory;
a pseudo-range threshold d pm = 3, corresponding to a euclidean distance of 0.06m;
An obstacle appears at both the null space and the end desired trajectory at O obs1.
From fig. 5, it can be known that the mechanical arm successfully avoids all the obstacles according to the expected track through the change of the configuration, and in combination with fig. 6, the minimum pseudo distance between the mechanical arm and the obstacles is far greater than the pseudo distance threshold value within 0-0.9s in simulation time, at this time, the obstacles have no influence on the track tracking precision of the tail end of the mechanical arm, and continue to move according to the expected track, and fig. 7 shows that the track error is within 0.02m, and the tracking precision is higher than that of the traditional obstacle avoidance method. The end effector can stably avoid obstacles and quickly recover to the expected rail, meanwhile, the mechanical arm can accurately reach the target position, and the end obstacle avoidance method is verified to be capable of realizing end obstacle avoidance.
As shown in fig. 5, when t=0.9 s, the tail end of the mechanical arm enters the dangerous area of the obstacle 3, and as shown in fig. 8, the virtual repulsive force between the tail end of the mechanical arm and the obstacle is rapidly increased, the virtual repulsive force generates a position increment for the tail end of the mechanical arm, the high-precision tracking of the tail end track is sacrificed to start to rapidly perform obstacle avoidance measures, the minimum pseudo distance is kept to be about 2, the small-amplitude jitter occurs because the preset positions of the obstacles 2 and 3 are relatively close, the mechanical arm is caused by emergency obstacle avoidance, the practical situation is met, and meanwhile, the method provided by the invention has higher sensitivity and effectiveness, and has better practicability compared with the traditional obstacle avoidance method. When the minimum pseudo distance of the simulation time T=2.3-3 s is gradually increased, the mechanical arm leaves the obstacle dangerous area, the obstacle avoidance movement starts to weaken, the tail end track tracking error gradually reduces and finally converges to a preset expected track, the initial point is returned, and the simulation is completed.
Meanwhile, as can be seen from the mechanical arm joint angle diagram of fig. 9, the shake phenomenon does not occur in the whole movement process, the joint position is stable, the phenomenon t=0-0.5 s which occurs at the beginning of the mechanical arm joint velocity diagram simulation of fig. 9 is probably due to the adaptability problem of some uncertainty parameters and methods of the mechanical arm model itself, the joint velocity has small-amplitude linear change, the actual track error on the Y axis is suddenly increased, the problem needs to be further solved in later scientific research, then the mechanical arm quickly resumes obstacle avoidance movement, the movement velocity is continuous, smooth and stable, and the simulation verifies the effectiveness of the method.
The invention and its embodiments have been described above by way of illustration and not limitation, and the invention is illustrated in the accompanying drawings and described in the drawings in which the actual structure is not limited thereto. Therefore, if one of ordinary skill in the art is informed by this disclosure, the structural mode and the embodiments similar to the technical scheme are not creatively designed without departing from the gist of the present invention.
Claims (2)
1. The mechanical arm tail end obstacle avoidance method based on the virtual force is characterized by comprising the following steps of:
S1, establishing a mechanical arm kinematics model: establishing a mechanical arm kinematics model by adopting a standard D-H method, and carrying out coordinate transformation by using a homogeneous transformation matrix of a mechanical arm joint coordinate system relative to a base coordinate system:
Where i=1, 2, …, n, is the robot arm joint number, cos θ i,sinθi,cosαi-1,sinαi-1 is abbreviated as cθ i,sθi,cαi-1,sαi-1;Rot(xi-1,αi-1) is a transformation matrix that is translated by α i-1 about the x i-1 axis, trans (x i-1,ai-1) is a transformation matrix that is translated by α i-1 along the x i-1 axis, rot (zi, θ i) is a transformation matrix that is translated by θ i about the z i axis, trans (z i,di) is a transformation matrix that is translated by d i about the z i axis;
the transformation matrix of the tail end of the mechanical arm relative to the basic coordinate system passes through the transformation matrix of each rod piece The method comprises the following steps:
s2, building a mechanical arm-obstacle model: calculating the position of any point q c (x, y, z) in the working space of the mechanical arm under an obstacle coordinate system o obs-xobsyobszobs, and judging the pseudo distance between the mechanical arm connecting rod and the obstacle;
S3, calculating a pseudo distance minimum value: expressing a minimum pseudo distance value between the mechanical arm and the obstacle by adopting a vector method;
S4, improving a virtual repulsive force function: improving the virtual repulsive force function in consideration of the speed and the movement direction of the end effector, and solving a negative gradient function; bringing the minimum pseudo distance value obtained in the step S3 into an improved virtual repulsive force function, obtaining virtual repulsive force between the tail end of the mechanical arm and the obstacle, and planning the tail end track of the mechanical arm;
S5, realizing a terminal obstacle avoidance task by utilizing an algorithm: the virtual repulsive force obtained in the step S4 is carried into a mechanical arm tail end obstacle avoidance method, so that the mechanical arm tail end obstacle avoidance is realized;
In the step S2, a mechanical arm-obstacle model is built, the method for constructing anti-collision constraint by using the pseudo-distance concept is used, each obstacle comprises an analytical expression of a hypersurface, constraint conditions between the obstacle and the mechanical arm are more qualitatively expressed, a coordinate system is built by taking the geometric center of the obstacle as an origin, and a hypersurface pseudo-distance analytical equation from a space point to an obstacle sphere is as follows:
Wherein S p (x, y, z) is the pseudo distance in the obstacle coordinate space and (x 0,y0,z0) is the hypersurface center point; r s＝robs+ri is a preset obstacle safety distance, R obs is an obstacle enveloping body radius, and R i is a thickness radius of the simulation mechanical arm;
The vector method in the step S3 is expressed as follows: s p(x,y,z)＝XTQX+BT X+C; wherein ,X＝[x,y,z]T,Q＝diag[1/Rs 2,1/Rs 2,1/Rs 2],B＝[-2x0/Rs 2,-2y0/Rs 2,-2z0/Rs 2]T,Q、B、C are constant;
The repulsive force function in the step S4 is as follows:
Where F dyn (x, v) is the virtual force generated by velocity and position, S p (x) is the pseudo-distance between the end of the robot arm and the obstacle, β is a constant to adjust the effect of the velocity vector angle θ, θ representing the angle between the current velocity vector v and the end effector position q c, q c is the position relative to the obstacle; In the form of a gradient, Representing the negative gradient of the gravitational field function; Representing the gradient of the pseudo-range;
in the step S4, the formula is based Calculating when theta is atWhen the robot approaches an obstacle, the dynamic repulsive field starts to work; when the value of theta is atWhen the robot is far away from the obstacle, the dynamic repulsive field does not work;
in the step S5, the end speed of the mechanical arm is determined based on the virtual repulsive force generated in the step S4 Correcting, namely correcting a kinematic equation of a traditional obstacle avoidance algorithm:
Wherein, Is the terminal velocity vector; jacobian matrix J is the angular velocity vector of the joint of the mechanical armAt the position ofIs a linear mapping of (2); J epsilon R n×m, n, m are dimensions of joint space and operation space; j 0 is a jacobian matrix corresponding to a projection point of the minimum euclidean distance of the obstacle on the connecting rod, n=i-J +J,I∈Rn×n is a unit matrix, and J + represents pseudo-inversion of the matrix, and J +＝JT(JJT)-1; A speed adjustment amount for the virtual repulsive force F dyn (x, v); x d is the desired trajectory.
2. The method of claim 1, wherein when the obstacle appears on the expected trajectory of the end effector, the step S5 is to change the trajectory of the end movement speed by taking the closest point as the position of the end effector of the mechanical arm, and the end effector is continuously approaching the obstacle, the minimum pseudo distance is reduced, and θ is at the same timeWhen the virtual repulsive force is continuously increased, a larger speed is generated for correction, and the end effector acts on the end effector to avoid the obstacle on the expected track.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210643516.2A CN115026816B (en) | 2022-06-09 | 2022-06-09 | Mechanical arm tail end obstacle avoidance method based on virtual force |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210643516.2A CN115026816B (en) | 2022-06-09 | 2022-06-09 | Mechanical arm tail end obstacle avoidance method based on virtual force |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115026816A CN115026816A (en) | 2022-09-09 |
| CN115026816B true CN115026816B (en) | 2024-09-20 |
Family
ID=83122069
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210643516.2A Active CN115026816B (en) | 2022-06-09 | 2022-06-09 | Mechanical arm tail end obstacle avoidance method based on virtual force |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115026816B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115502960B (en) * | 2022-10-28 | 2023-08-08 | 深圳市深科达智能装备股份有限公司 | Terminal assembly, working device and control method thereof |
| CN118418145B (en) * | 2024-07-05 | 2024-10-15 | 中联重科股份有限公司 | Obstacle avoidance control method and device for mechanical arm, electronic equipment and storage medium |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108326849A (en) * | 2018-01-04 | 2018-07-27 | 浙江大学 | A kind of multi-degree-of-freemechanical mechanical arm dynamic obstacle avoidance paths planning method based on modified embedded-atom method |
| CN108908331A (en) * | 2018-07-13 | 2018-11-30 | 哈尔滨工业大学（深圳） | The barrier-avoiding method and system, computer storage medium of super redundancy flexible robot |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP4738472B2 (en) * | 2008-12-05 | 2011-08-03 | 株式会社東芝 | Mobility control device having obstacle avoidance function |
| CN113146610B (en) * | 2020-12-08 | 2022-04-19 | 安徽工业大学 | Mechanical arm tail end track tracking algorithm based on zero space obstacle avoidance |
| CN112783194A (en) * | 2020-12-18 | 2021-05-11 | 上海电力股份有限公司吴泾热电厂 | Obstacle avoidance method for unmanned aerial vehicle flying in indoor coal yard |
| CN113334379A (en) * | 2021-05-30 | 2021-09-03 | 南京理工大学 | Master-slave following and collision avoidance method based on virtual force |
| CN114571469B (en) * | 2022-05-05 | 2022-07-26 | 北京科技大学 | Zero-space real-time obstacle avoidance control method and system for mechanical arm |
-
2022
- 2022-06-09 CN CN202210643516.2A patent/CN115026816B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108326849A (en) * | 2018-01-04 | 2018-07-27 | 浙江大学 | A kind of multi-degree-of-freemechanical mechanical arm dynamic obstacle avoidance paths planning method based on modified embedded-atom method |
| CN108908331A (en) * | 2018-07-13 | 2018-11-30 | 哈尔滨工业大学（深圳） | The barrier-avoiding method and system, computer storage medium of super redundancy flexible robot |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115026816A (en) | 2022-09-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111168675B (en) | Dynamic obstacle avoidance motion planning method for mechanical arm of household service robot | |
| CN107490965B (en) | Multi-constraint trajectory planning method for space free floating mechanical arm | |
| CN101612734B (en) | Pipeline spraying robot and operation track planning method thereof | |
| CN115026816B (en) | Mechanical arm tail end obstacle avoidance method based on virtual force | |
| CN102646148B (en) | Motion trajectory planning method of mechanical arm of humanoid robot for preventing collision | |
| CN108241339B (en) | Motion solving and configuration control method of humanoid mechanical arm | |
| CN110597268B (en) | Wheel type mobile robot track tracking control method based on cascade system theory | |
| CN113146610B (en) | Mechanical arm tail end track tracking algorithm based on zero space obstacle avoidance | |
| CN107966907B (en) | Obstacle avoidance solution applied to redundant manipulator | |
| CN105183009B (en) | A kind of redundant mechanical arm method for controlling trajectory | |
| CN108908347B (en) | Fault-tolerant repetitive motion planning method for redundant mobile mechanical arm | |
| CN110653137B (en) | Spraying method for keeping spray head vertical to spraying surface | |
| CN103240739A (en) | Automatic switching control method for decentralization and centralization of mobile manipulators | |
| CN109623812B (en) | Mechanical arm trajectory planning method considering spacecraft body attitude motion | |
| CN111421540A (en) | Mechanical arm motion control method | |
| CN115674208B (en) | Robot vibration suppression device, control method and robot | |
| CN111208830B (en) | Three-closed-loop formation track tracking control method for wheeled mobile robot | |
| CN110900613A (en) | Non-stop control method and system for mobile robot | |
| CN115256371B (en) | Neural network-based method for presetting movement time of road rescue mobile mechanical arm | |
| Liu et al. | An automated method to calibrate industrial robot kinematic parameters using Spherical Surface constraint approach | |
| US20230302645A1 (en) | Method of robot dynamic motion planning and control | |
| Yang et al. | An obstacle avoidance and trajectory tracking algorithm for redundant manipulator end | |
| CN114851184B (en) | Reinforced learning rewarding value calculating method for industrial robot | |
| Jia et al. | Coordination of a nonholonomic mobile platform and an on-board manipulator | |
| CN114200836A (en) | Two-step planning method for track of quadruped wheel-leg robot |
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 |