CN114589701B - Damping least square-based multi-joint mechanical arm obstacle avoidance inverse kinematics method - Google Patents
Damping least square-based multi-joint mechanical arm obstacle avoidance inverse kinematics method Download PDFInfo
- Publication number
- CN114589701B CN114589701B CN202210418696.4A CN202210418696A CN114589701B CN 114589701 B CN114589701 B CN 114589701B CN 202210418696 A CN202210418696 A CN 202210418696A CN 114589701 B CN114589701 B CN 114589701B
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- obstacle
- joint
- joint mechanical
- repulsive force
- 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 45
- 238000013016 damping Methods 0.000 title claims abstract description 14
- 239000011159 matrix material Substances 0.000 claims abstract description 30
- 238000006243 chemical reaction Methods 0.000 claims abstract description 15
- 238000005457 optimization Methods 0.000 claims abstract description 13
- 238000005381 potential energy Methods 0.000 claims description 13
- 230000004888 barrier function Effects 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 4
- 230000001186 cumulative effect Effects 0.000 claims description 3
- 238000012897 Levenberg–Marquardt algorithm Methods 0.000 claims 2
- 238000005562 fading Methods 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 4
- 230000009466 transformation Effects 0.000 description 4
- 238000013459 approach Methods 0.000 description 2
- 230000007423 decrease Effects 0.000 description 2
- 230000036544 posture Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 240000007594 Oryza sativa Species 0.000 description 1
- 235000007164 Oryza sativa Nutrition 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 239000002131 composite material Substances 0.000 description 1
- 239000012636 effector Substances 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 235000009566 rice Nutrition 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
- 230000017105 transposition Effects 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/1612—Programme controls characterised by the hand, wrist, grip control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J17/00—Joints
-
- 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/1628—Programme controls characterised by the control loop
- B25J9/1643—Programme controls characterised by the control loop redundant control
-
- 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Health & Medical Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Orthopedic Medicine & Surgery (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a damping least square based multi-joint mechanical arm obstacle avoidance inverse kinematics method. Comprising the following steps: establishing a D-H joint coordinate system according to the structure of the multi-joint mechanical arm, and then solving the coordinate conversion relation and the jacobian matrix of the positive kinematics of the multi-joint mechanical arm; calculating the total virtual repulsive force of the obstacle to each connecting rod in the multi-joint mechanical arm according to the relative position relation between the obstacle and each connecting rod in the multi-joint mechanical arm and the coordinate conversion relation of the positive kinematics of the multi-joint mechanical arm; and finally, based on a damping least square method, establishing an inverse kinematics optimization objective function of the multi-joint mechanical arm according to the jacobian matrix and the virtual repulsive force, and solving the inverse kinematics optimization function of the multi-joint mechanical arm by adopting a numerical iteration method to obtain the angles of all joints corresponding to the terminal pose of the multi-joint mechanical arm. According to the invention, the obstacle avoidance planning and the inverse kinematics solving process of the mechanical arm are fused, the flow of an obstacle avoidance method is simplified, and the real-time performance of obstacle avoidance of the mechanical arm is ensured.
Description
Technical Field
The invention belongs to a reverse kinematics method of a multi-joint mechanical arm in the field of mechanical arm kinematics, and particularly relates to a damping least square-based obstacle avoidance reverse kinematics method of the multi-joint mechanical arm.
Background
The multi-joint mechanical arm is one of redundant mechanical arms, and has more redundant degrees of freedom besides three degrees of freedom of positions and three degrees of freedom of postures, so that the self-movement characteristic of the multi-joint mechanical arm is caused, namely, all joints can still move under the condition of ensuring the fixation of the tail end pose. Therefore, the mechanical arm has high movement flexibility, can finish obstacle avoidance while the pose of the tail end is unchanged, and is very suitable for operation in complex environments.
The first step of obstacle avoidance of the multi-joint mechanical arm is solving of inverse kinematics, due to the extremely large redundancy of degrees of freedom, the inverse kinematics analytical solution of the multi-joint mechanical arm is not always available, even if the analytical solution exists, a plurality of solutions which are difficult to screen are always accompanied, and the complexity of the inverse kinematics of the multi-joint mechanical arm is considered, so that the conventional inverse kinematics solving method generally does not consider obstacle constraint.
Disclosure of Invention
In order to overcome the difficulty brought by the redundant freedom degree of the multi-joint mechanical arm, the flexibility of the multi-joint mechanical arm is fully utilized under the obstacle avoidance requirement, and the obstacle avoidance inverse kinematics method of the multi-joint mechanical arm based on damping least square is provided for improving the operation performance of the multi-joint mechanical arm with the redundant freedom degree under a complex environment. Aiming at the problem of real-time obstacle avoidance of the multi-joint mechanical arm in the motion process, the invention designs the inverse kinematics optimization function of the mechanical arm based on a damping least square method by giving the pose of the tail end, the space coordinates of the obstacle and the size of the obstacle of the mechanical arm, adopts iterative solution to obtain the joint angle which finally meets the obstacle avoidance condition, and simplifies the complexity of the obstacle avoidance method of the multi-joint mechanical arm.
In order to achieve the above purpose, the technical scheme of the invention specifically comprises the following steps:
the invention comprises the following steps:
the first step: establishing a D-H joint coordinate system according to the structure of the multi-joint mechanical arm, and solving a coordinate conversion relation and a jacobian matrix of the positive kinematics of the multi-joint mechanical arm based on the D-H joint coordinate system;
and a second step of: calculating the total virtual repulsive force of the obstacle to each connecting rod in the multi-joint mechanical arm according to the relative position relation between the obstacle and each connecting rod in the multi-joint mechanical arm and the coordinate conversion relation of the positive kinematics of the multi-joint mechanical arm;
and a third step of: based on a damping least square method, an inverse kinematics optimization objective function of the multi-joint mechanical arm is established according to the jacobian matrix and the virtual repulsive force, and the inverse kinematics optimization function of the multi-joint mechanical arm is solved by adopting a numerical iteration method, so that each joint angle corresponding to the tail end pose of the multi-joint mechanical arm is obtained.
The second step specifically comprises the following steps:
2.1 According to the relative position relation between the barrier and the connecting rods in the multi-joint mechanical arm and the coordinate conversion relation of the positive kinematics of the multi-joint mechanical arm, calculating the acting direction of virtual repulsive force of each barrier to each connecting rod in the multi-joint mechanical arm and the corresponding virtual repulsive force potential energy;
2.2 Multiplying the action direction of virtual repulsive force of each obstacle to the current connecting rod in the multi-joint mechanical arm by the corresponding virtual repulsive force potential energy to obtain virtual repulsive force of each obstacle to the current connecting rod in the multi-joint mechanical arm, and summing the virtual repulsive forces to obtain the total virtual repulsive force of the obstacle to the current connecting rod in the multi-joint mechanical arm;
2.3 Repeating 2.1) -2.2), calculating and obtaining the total virtual repulsive force of the obstacle to the rest connecting rods in the multi-joint mechanical arm.
The direction of the virtual repulsive force of each obstacle to each connecting rod in the multi-joint mechanical arm is determined by the relative position between the obstacle and the connecting rod, in particular:
for the action direction of virtual repulsive force of the obstacle k to the connecting rod i, the geometric center O of the obstacle k and two end points P of the connecting rod i A 、P B Distance OP between A 、OP B Two end points P of the connecting rod i A 、P B Connected to form an axis P A P B The relationship between them is divided into the following three cases:
when the geometric center O of the obstacle k is at the axis P of the connecting rod i A P B Projection onto P B On one side of the extension line, the distance d between the obstacle k and the connecting rod i is in space ik Satisfy d ik ＝|OP B |-R-r k R is the radius of the circle where the end part of the connecting rod i is positioned, R k Is half of the maximum diameter of the obstacle, the virtual repulsive force acts in the direction of
When the geometric center O of the obstacle k is at the axis P of the connecting rod i A P B Projection onto P A On one side of the extension line, the distance d between the obstacle k and the connecting rod i is in space ik Satisfy d ik ＝|OP A |-R-r k The virtual repulsive force acts in the direction of
When the geometric center O of the obstacle k is at the axis P of the connecting rod i A P B The projection on the axis P A P B The distance d between the obstacle k and the link i ik Satisfy d ik ＝|OP v |-R-r k The virtual repulsive force acts in the direction of
The calculation formula of the virtual repulsive potential energy of each obstacle to the current connecting rod in the multi-joint mechanical arm is as follows:
wherein E is ik Representing the virtual repulsive potential energy of an obstacle k acting on a connecting rod i r For the repulsive force coefficient of the obstacle, d 0 Distance of influence of obstacle d ik A spatial distance from the obstacle k to the link i;
in the third step, the formula of the inverse kinematics optimization objective function of the multi-joint mechanical arm is as follows:
wherein J is a jacobian matrix of the multi-joint mechanical arm, F force (q) represents the cumulative force of the virtual repulsive force on the joint,f force (q) is the component of the virtual repulsive force acting on the link, q represents the respective joint angles of the multi-joint mechanical arm, λ 2 Representing a regular term coefficient, x representing the end position of the multi-joint mechanical arm, II representing a binary norm operation, min representing a minimum operation, and gamma representing a repulsive force decay coefficient related to the number of iterations p.
In the third step, in the solving process of the inverse kinematics optimization objective function by adopting the numerical iteration method, the updated terminal pose is obtained by adjusting the angle q of each joint, and the updated terminal pose and the preset terminal pose are used according to the updated terminal pose 0 T E Calculating pose residual errors, and obtaining final joint angles q after the pose residual errors are minimized, wherein the formula in the solving process is as follows:
H p ＝(J p T J p +λ 2 I) -1
γ＝σ p
wherein H is p Is the Hessian matrix at the p-th iteration, J p Is the jacobian matrix at the p-th iteration, e p Is the pose residual error at the p-th iteration, lambda 2 Is a regular term coefficient, tr () represents an operation of converting the homogeneous matrix into a vector form, and γ is a repulsive force decay coefficient related to the number of iterations p;representing a positive kinematic relationship, σ is a single-step repulsive force decay coefficient, T represents a transpose operation, q p 、q p+1 Each joint angle at the p-th and p+1-th iterations is shown.
And adjacent connecting rods of the multi-joint mechanical arm are connected by adopting universal joints.
Compared with the prior art, the invention has the beneficial effects that:
1. the inverse kinematics method of the multi-joint mechanical arm is provided, the mechanical arm joint angle combination meeting the obstacle avoidance condition can be obtained under the condition that the basic information (information such as the number of joints, the length of each joint and the like) and the obstacle information (space position and size) of the mechanical arm are known, and the redundant degree of freedom of the multi-joint mechanical arm is fully utilized to meet the requirement of obstacle avoidance operation.
2. The relative position conditions of various barriers and the mechanical arm connecting rod are comprehensively considered, a virtual repulsive force expression based on potential energy is provided, a virtual repulsive force expression with clear physical meaning is defined, and the operation safety of the multi-joint mechanical arm under the condition of the barriers is ensured.
3. The damping least square equation taking virtual repulsive force into consideration is solved by using a numerical iteration method, and the accuracy and instantaneity of the inverse kinematics solution are still met under the condition that the equation is not resolved.
Drawings
FIG. 1 is a schematic view of the exterior of a multi-joint robotic arm;
FIG. 2 is a flow chart of method steps;
FIG. 3 is a schematic diagram of a coordinate system definition;
FIG. 4 is a schematic view of obstacle distance calculation;
FIG. 5 is a diagram of the obstacle avoidance process of the multi-joint mechanical arm demonstrated in Matlab of the present invention;
fig. 6 is a diagram of the end pose error result of the multi-joint mechanical arm in Matlab simulation experiments.
Detailed Description
The solution process of the present invention is described in more detail below with reference to examples and drawings.
In this embodiment, the connecting rods of the multi-joint mechanical arm are numbered i=1, 2, …, I, and are sequentially ordered from the base to the outside, the total number is I, each section of connecting rod is in a cylindrical shape, the length is L, the radius is R, and the connecting rods are connected by adopting universal joints, as shown in fig. 1.
For the universal joint at the ith connecting rod of the mechanical arm, two mutually perpendicular rotation angles theta are adopted i 、Describing two degrees of rotational freedom of the gimbal, the rotational angle about the z-axis for the coordinate system { O-xyz } at the center of the gimbal is θ i The rotation angle around the y-axis is +.>As shown in FIG. 3, the joint angle can then be defined as +.>
As shown in fig. 2, the present invention includes the steps of:
the first step: establishing a D-H joint coordinate system according to the structure of the multi-joint mechanical arm, and solving a coordinate conversion relation and a jacobian matrix of the positive kinematics of the multi-joint mechanical arm based on the D-H joint coordinate system;
in particular, a D-H coordinate system { O } is established in the center of the universal joint n -x n y n z n N=1, 2, …, N indicates the number of rotational degrees of freedom, n=2i, i.e. there are two rotational degrees of freedom in the vertical direction per gimbal, the coordinates at the gimbal being defined as shown in fig. 3.
The positive kinematics of a robotic arm can generally be expressed as a function of:
it indicates the pose ζ of the end effector E Is a function of the joint angle q. According to the D-H method, homogeneous transformation is adopted, the expression of the method is a simple product of a single connecting rod transformation matrix, and the coordinate transformation relation of positive kinematics can be obtained as follows:
ξ E ～ 0 T E ＝ 0 A 1 · 1 A 2 … n-1 A n
wherein,is a homogeneous matrix of 4×4, consisting of a rotation matrix R 3×3 And translation vector T 3×1 The composition of the composite material comprises the components, n-1 A n is a transformation matrix between D-H coordinate systems, and can be expressed as:
wherein q is n ,α n ,a n ,d n Parameters of the robot arm linkage are described in the D-H method.
Meanwhile, in order to facilitate subsequent inverse kinematics analysis, a jacobian matrix of the mechanical arm needs to be obtained, and the jacobian matrix is in the form of:
the jacobian matrix can be derived from a formula of the positive kinematics of the manipulator.
And a second step of: calculating the total virtual repulsive force of the obstacle to each connecting rod in the multi-joint mechanical arm according to the relative position relation between the obstacle and each connecting rod in the multi-joint mechanical arm and the coordinate conversion relation of the positive kinematics of the multi-joint mechanical arm;
the second step is specifically as follows:
2.1 According to the relative position relation between the barrier and the connecting rods in the multi-joint mechanical arm and the coordinate conversion relation of the positive kinematics of the multi-joint mechanical arm, calculating the acting direction of virtual repulsive force of each barrier to each connecting rod in the multi-joint mechanical arm and the corresponding virtual repulsive force potential energy;
the direction of the virtual repulsive force of each obstacle to each link in the multi-joint mechanical arm is determined by the relative position between the obstacle and the link, specifically:
for the direction of the virtual repulsive force of the obstacle k to the link i, as shown in fig. 4, the geometric center O of the obstacle k and the two end points P of the link i A 、P B Distance OP between A 、OP B Two end points P of the connecting rod i A 、P B Connected to form an axis P A P B The relationship between the two is divided into three cases in which the position P of the front and rear end points of the link i A 、P B The method can be obtained by calculating a conversion matrix between D-H coordinate systems and calculating the coordinate conversion relation of positive kinematics:
as shown in fig. 4 (a), when the geometric center O of the obstacle k is at the axis P of the link i A P B Projection onto P B On one side of extension, i.e. |OP B | 2 +|P A P B | 2 ≤|OP A | 2 Spatial distance d of obstacle k to link i ik Satisfy d ik ＝|OP B |-R-r k R is the radius of the circle where the end part of the connecting rod i is positioned, R k Is half of the maximum diameter of the obstacle, the virtual repulsive force acts in the direction ofCorresponding virtual repulsive force E f Satisfy->
As shown in fig. 4 (b), when the geometric center O of the obstacle k is at the axis P of the link i A P B Projection onto P A On one side of extension, i.e. |OP A | 2 +|P A P B | 2 ≤|OP B | 2 Spatial distance d of obstacle k to link i ik Satisfy d ik ＝|OP A |-R-r k The virtual repulsive force acts in the direction ofCorresponding virtual repulsive force E f Satisfy->
As shown in fig. 4 (c), when the geometric center O of the obstacle k is at the axis P of the link i A P B Projection ontoLocated at axis P A P B The distance d between the obstacle k and the link i ik Satisfy d ik ＝|OP v |-R-r k The virtual repulsive force acts in the direction ofCorresponding virtual repulsive force E f Satisfy->
The calculation formula of virtual repulsive potential energy of each obstacle to the current connecting rod in the multi-joint mechanical arm is as follows:
wherein E is ik Representing the virtual repulsive potential energy of an obstacle k acting on a connecting rod i r For the repulsive force coefficient of the obstacle, d 0 Distance of influence of obstacle d ik A spatial distance from the obstacle k to the link i; in a specific implementation, the geometric center of the obstacle is the center of the surrounding sphere, the maximum diameter of the obstacle is the diameter of the surrounding sphere, and the radius of the surrounding sphere is r k The connecting rod being cylindrical, i.e. d ik Representing the spatial distance of the cylindrical connecting rod from the surrounding sphere.
2.2 Multiplying the action direction of virtual repulsive force of each obstacle to the current connecting rod in the multi-joint mechanical arm by the corresponding virtual repulsive force potential energy to obtain virtual repulsive force of each obstacle to the current connecting rod in the multi-joint mechanical arm, and summing the virtual repulsive forces to obtain the total virtual repulsive force of the obstacle to the current connecting rod in the multi-joint mechanical arm;
in particular, all the virtual repulsive forces are vector added, then the virtual repulsive forces are converted into the coordinate systems of all joints through coordinate conversion, 0 R B for point P B The rotation matrix under the base coordinate system can calculate the point P according to the coordinate conversion relation of the positive kinematics B Is a transform matrix T of (a) B And is decomposed to obtain the total virtual repulsive force E of the obstacle in the space to the connecting rod i i The method comprises the following steps:
E i ＝ 0 R B -1 ·∑E ik
the posture of each connecting rod i is represented by theta i 、Two joint angle controls, thus acting on θ i 、/>Joint repulsive force of (2)Respectively E i The components in the y-axis and z-axis, so the joint repulsive force of the link i constitutes a vector of:
2.3 Repeating 2.1) -2.2), calculating and obtaining the total virtual repulsive force of the obstacle to the rest connecting rods in the multi-joint mechanical arm.
The inverse kinematics solution of the mechanical arm is given an end pose matrix xi E Obtaining the joint angleIs expressed as:
in the third step, on the basis of a damping least square method, considering the influence of virtual repulsive force on joints, the formula of the inverse kinematics optimization objective function of the multi-joint mechanical arm is as follows:
wherein J is a jacobian matrix of the multi-joint mechanical arm, F force (q) represents the cumulative force of the virtual repulsive force on the joint,f force (q) is the component of the virtual repulsive force acting on the link, q represents the respective joint angles of the multi-joint mechanical arm, λ 2 Representing the regular term coefficients, in this embodiment +.>This may make the final pose error smaller, x represents the end position of the mechanical arm, ii represents the operation of taking the binary norm, min represents the operation of taking the minimum, γ is the repulsive force decay coefficient for iterative solution, and is related to the number of iterations p, and gradually decreases during the iteration, in a specific form as shown below. The formula for optimizing the objective function shows that the influence of repulsive force is reduced as much as possible while the tail end tracking track error of the mechanical arm is minimum and the joint speed norm is minimum, and for the multi-joint mechanical arm, the pose of the multi-joint mechanical arm obtained by inverse kinematics solution can be far away from the obstacle and the joint limit position.
In the third step, in the solving process of solving the inverse kinematics optimization objective function by adopting a numerical iteration method, the updated terminal pose is obtained by adjusting the angle q of each joint, whereinT represents a transposition operation such that the updated terminal pose continuously approaches the preset terminal pose, and the terminal pose is updated according to the updated terminal pose and the preset terminal pose 0 T E After the pose residual error is calculated and minimized in iteration, in specific implementation, after the pose residual error is smaller than a preset threshold, namely the pose residual error is minimized, stopping iteration, and obtaining the final angles q of all joints, wherein the formula in the solving process is as follows:
H p ＝(J p T J p +λ 2 I) -1
γ＝σ p
wherein H is p Is the Hessian matrix at the p-th iteration, J p Is the jacobian matrix at the p-th iteration, e p Is the pose residual error in the p-th iteration, meets the requirement of e p ＝[dv x ,dv y ,dv z ,dω x ,dω x ,dω z ] T ，dv x 、dv y 、dv z Representing the residual error of the position in the x, y and z axes, dω x 、dω x 、dω z Respectively representing the angle residual errors in the x, y and z axes lambda 2 Is a regular term coefficient, tr () represents an operation of converting the homogeneous matrix into a vector form, γ is a repulsive force decay coefficient related to the number of iterations p, it is a single-step repulsive force decay coefficient as the number of iterations decreases, σ is a constant, σ e (0, 1), and σ=0.5 is taken in this embodiment;representing positive kinematic relationships, T representing the transpose operation, q p 、q p+1 Each joint angle at the p-th and p+1-th iterations is shown.
Several iterations of the beginning, f force (q) guiding the joint away from the obstacle or joint limit position rapidly, avoiding the influence of local minimum, f in the later stage of iteration force The coefficient gamma of (q) approaches 0, so that the iteration converges to the target pose more quickly, and the convergence of the improved algorithm is ensured.
The Matlab simulation obstacle avoidance process diagram of the invention is shown in (a) - (d) of fig. 5, and it can be seen that the multi-joint mechanical arm can successfully avoid the obstacle under the condition of moving the obstacle and can follow the expected terminal pose. As shown in FIGS. 6 (a) and (b), the actual and desired end positions have an error of less than 10 -10 Rice, actual and desired endsThe error of the angle is less than 10 -10 rad, the accuracy of the inverse kinematics solution is met.
Claims (5)
1. The damping least square based multi-joint mechanical arm obstacle avoidance inverse kinematics method is characterized by comprising the following steps of:
the first step: establishing a D-H joint coordinate system according to the structure of the multi-joint mechanical arm, and solving a coordinate conversion relation and a jacobian matrix of the positive kinematics of the multi-joint mechanical arm based on the D-H joint coordinate system;
and a second step of: calculating the total virtual repulsive force of the obstacle to each connecting rod in the multi-joint mechanical arm according to the relative position relation between the obstacle and each connecting rod in the multi-joint mechanical arm and the coordinate conversion relation of the positive kinematics of the multi-joint mechanical arm;
and a third step of: based on a damping least square method, establishing an inverse kinematics optimization objective function of the multi-joint mechanical arm according to the jacobian matrix and the virtual repulsive force, and solving the inverse kinematics optimization function of the multi-joint mechanical arm by adopting a numerical iteration method to obtain each joint angle corresponding to the terminal pose of the multi-joint mechanical arm;
in the third step, the formula of the inverse kinematics optimization objective function of the multi-joint mechanical arm is as follows:
wherein J is a jacobian matrix of the multi-joint mechanical arm, F force (q) represents the cumulative force of the virtual repulsive force on the joint,f force (q) is the component of the virtual repulsive force acting on the link, q represents the respective joint angles of the multi-joint mechanical arm, λ 2 The method comprises the steps of representing a regular term coefficient, wherein x represents the tail end position of a multi-joint mechanical arm, min represents a two-norm operation, min represents a minimum operation, and gamma is a repulsive force fading coefficient related to the iteration times p;
said third stepIn the solving process of solving the inverse kinematics optimization objective function by adopting a numerical iteration method, the updated terminal pose is obtained by adjusting the angle q of each joint, and the updated terminal pose and the preset terminal pose are used according to the updated terminal pose 0 T E Calculating pose residual errors, and obtaining final joint angles q after the pose residual errors are minimized, wherein the formula in the solving process is as follows:
e p ＝tr( 0 T E -κ(q p ))
H p ＝(J p T J p +λ 2 I) -1
γ＝σ p
wherein H is p Is the Hessian matrix at the p-th iteration, J p Is the jacobian matrix at the p-th iteration, e p Is the pose residual error at the p-th iteration, lambda 2 Is a regular term coefficient, tr () represents an operation of converting the homogeneous matrix into a vector form, and γ is a repulsive force decay coefficient related to the number of iterations p; kappa () represents a positive kinematic relationship, sigma is a single-step repulsive decay coefficient, T represents a transpose operation, q p 、q p+1 Each joint angle at the p-th and p+1-th iterations is shown.
2. The method for obstacle avoidance inverse kinematics of a multi-joint manipulator based on damped least squares according to claim 1, wherein the second step is specifically:
2.1 According to the relative position relation between the barrier and the connecting rods in the multi-joint mechanical arm and the coordinate conversion relation of the positive kinematics of the multi-joint mechanical arm, calculating the acting direction of virtual repulsive force of each barrier to each connecting rod in the multi-joint mechanical arm and the corresponding virtual repulsive force potential energy;
2.2 Multiplying the action direction of virtual repulsive force of each obstacle to the current connecting rod in the multi-joint mechanical arm by the corresponding virtual repulsive force potential energy to obtain virtual repulsive force of each obstacle to the current connecting rod in the multi-joint mechanical arm, and summing the virtual repulsive forces to obtain the total virtual repulsive force of the obstacle to the current connecting rod in the multi-joint mechanical arm;
2.3 Repeating 2.1) -2.2), calculating and obtaining the total virtual repulsive force of the obstacle to the rest connecting rods in the multi-joint mechanical arm.
3. The method of inverse kinematics for obstacle avoidance of a multi-joint manipulator based on damped least squares according to claim 2, wherein the direction of the virtual repulsive force of the respective obstacle to each link in the multi-joint manipulator is determined by the relative position between the obstacle and the link, in particular:
for the action direction of virtual repulsive force of the obstacle k to the connecting rod i, the geometric center O of the obstacle k and two end points P of the connecting rod i A 、P B Distance OP between A 、OP B Two end points P of the connecting rod i A 、P B Connected to form an axis P A P B The relationship between them is divided into the following three cases:
when the geometric center O of the obstacle k is at the axis P of the connecting rod i A P B Projection onto P B On one side of the extension line, the distance d between the obstacle k and the connecting rod i is in space ik Satisfy d ik ＝|OP B |-R-r k R is the radius of the circle where the end part of the connecting rod i is positioned, R k Is half of the maximum diameter of the obstacle, the virtual repulsive force acts in the direction of
When the geometric center O of the obstacle k is at the axis P of the connecting rod i A P B Projection onto P A On one side of the extension line, the distance d between the obstacle k and the connecting rod i is in space ik Satisfy d ik ＝|OP A |-R-r k The virtual repulsive force acts in the direction of
In the geometry of the obstacle kThe heart O being at the axis P of the connecting rod i A P B The projection on the axis P A P B The distance d between the obstacle k and the link i ik Satisfy d ik ＝|OP v |-R-r k The virtual repulsive force acts in the direction of
4. The method for obstacle avoidance inverse kinematics of the multi-joint mechanical arm based on damping least square according to claim 2, wherein the calculation formula of the virtual repulsive force potential energy of each obstacle to the current connecting rod in the multi-joint mechanical arm is as follows:
wherein E is ik Representing the virtual repulsive potential energy of an obstacle k acting on a connecting rod i r For the repulsive force coefficient of the obstacle, d 0 Distance of influence of obstacle d ik Is the spatial distance of obstacle k to link i.
5. The method for obstacle avoidance inverse kinematics of a multi-joint mechanical arm based on damping least squares according to any one of claims 1-4, wherein adjacent links of the multi-joint mechanical arm are connected by adopting universal joints.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210418696.4A CN114589701B (en) | 2022-04-20 | 2022-04-20 | Damping least square-based multi-joint mechanical arm obstacle avoidance inverse kinematics method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210418696.4A CN114589701B (en) | 2022-04-20 | 2022-04-20 | Damping least square-based multi-joint mechanical arm obstacle avoidance inverse kinematics method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114589701A CN114589701A (en) | 2022-06-07 |
| CN114589701B true CN114589701B (en) | 2024-04-09 |
Family
ID=81813089
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210418696.4A Active CN114589701B (en) | 2022-04-20 | 2022-04-20 | Damping least square-based multi-joint mechanical arm obstacle avoidance inverse kinematics method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114589701B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116352714B (en) * | 2023-04-11 | 2023-09-26 | 广东工业大学 | Mechanical arm obstacle avoidance path planning method |
| CN117290980B (en) * | 2023-11-27 | 2024-02-02 | 江西格如灵科技股份有限公司 | Mechanical arm simulation method and system based on Unity platform |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5499320A (en) * | 1993-03-24 | 1996-03-12 | The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration | Extended task space control for robotic manipulators |
| 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 |
| CN111515932A (en) * | 2020-04-23 | 2020-08-11 | 东华大学 | Man-machine co-fusion assembly line implementation method based on artificial potential field and reinforcement learning |
| CN111687835A (en) * | 2020-04-30 | 2020-09-22 | 广西科技大学 | Reverse priority impedance control system and method for redundant manipulator of underwater manipulator |
| CN112698574A (en) * | 2020-12-29 | 2021-04-23 | 南京理工大学 | Hybrid task priority based double-arm space robot coordination control method |
| CN113334379A (en) * | 2021-05-30 | 2021-09-03 | 南京理工大学 | Master-slave following and collision avoidance method based on virtual force |
-
2022
- 2022-04-20 CN CN202210418696.4A patent/CN114589701B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5499320A (en) * | 1993-03-24 | 1996-03-12 | The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration | Extended task space control for robotic manipulators |
| 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 |
| CN111515932A (en) * | 2020-04-23 | 2020-08-11 | 东华大学 | Man-machine co-fusion assembly line implementation method based on artificial potential field and reinforcement learning |
| CN111687835A (en) * | 2020-04-30 | 2020-09-22 | 广西科技大学 | Reverse priority impedance control system and method for redundant manipulator of underwater manipulator |
| CN112698574A (en) * | 2020-12-29 | 2021-04-23 | 南京理工大学 | Hybrid task priority based double-arm space robot coordination control method |
| CN113334379A (en) * | 2021-05-30 | 2021-09-03 | 南京理工大学 | Master-slave following and collision avoidance method based on virtual force |
Non-Patent Citations (1)
| Title |
|---|
| RRT-GoalBias and Path Smoothing Based Motion Planning of Mobile Manipulators with Obstacle Avoidance;. Shao et al.;《2021 IEEE International Conference on Real-time Computing and Robotics (RCAR)》;全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114589701A (en) | 2022-06-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114589701B (en) | Damping least square-based multi-joint mechanical arm obstacle avoidance inverse kinematics method | |
| CN112091979B (en) | Seven-degree-of-freedom mechanical arm limiting optimization method based on position-level inverse kinematics | |
| CN108908331B (en) | Obstacle avoidance method and system for super-redundant flexible robot and computer storage medium | |
| CN106166749B (en) | A kind of motion track planing method of space movement multi-arm robot | |
| CN113146610B (en) | Mechanical arm tail end track tracking algorithm based on zero space obstacle avoidance | |
| CN111761582B (en) | Mobile mechanical arm obstacle avoidance planning method based on random sampling | |
| CN112605996B (en) | Model-free collision avoidance control method for redundant mechanical arm | |
| CN108908347B (en) | Fault-tolerant repetitive motion planning method for redundant mobile mechanical arm | |
| CN109344477B (en) | 6-degree-of-freedom mechanical arm inverse kinematics solving method | |
| CN114227672A (en) | Mechanical arm safety collision trajectory planning method and device, storage medium and equipment | |
| Mu et al. | A hybrid obstacle-avoidance method of spatial hyper-redundant manipulators for servicing in confined space | |
| CN114800491B (en) | Redundant mechanical arm zero-space obstacle avoidance planning method | |
| CN115416016A (en) | Mechanical arm obstacle avoidance path planning method based on improved artificial potential field method | |
| CN113043286A (en) | Multi-degree-of-freedom mechanical arm real-time obstacle avoidance path planning system and method | |
| CN113878571A (en) | Configuration optimization comprehensive method for seven-degree-of-freedom cooperative robot | |
| CN116909274A (en) | Robot autonomous man-machine collision prevention method and system | |
| CN108638057B (en) | Double-arm motion planning method for humanoid robot | |
| CN117325143A (en) | Redundant mechanical arm singular position type lower kinematics optimization method | |
| CN114274145A (en) | Real-time obstacle avoidance method for multiple mechanical arms in laparoscopic surgery | |
| Qian et al. | Path planning approach for redundant manipulator based on Jacobian pseudoinverse-RRT algorithm | |
| Xiang et al. | An optimal trajectory control strategy for underwater welding robot | |
| CN113084797A (en) | Dynamic cooperative control method for double-arm redundant mechanical arm based on task decomposition | |
| Fang et al. | RRT-based motion planning with sampling in redundancy space for robots with anthropomorphic arms | |
| Abulhail et al. | Social spider optimization for solving inverse kinematics for both humanoid robotic arms | |
| Bhardwaj et al. | An unsupervised neural network approach for inverse kinematics solution of manipulator following kalman filter based trajectory |
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 | ||
| CB02 | Change of applicant information |
Country or region after: China Address after: 316021 Zhoushan campus of Zhejiang University, No.1 Zheda Road, Dinghai District, Zhoushan City, Zhejiang Province Applicant after: ZHEJIANG University Address before: 310058 Yuhang Tang Road, Xihu District, Hangzhou, Zhejiang 866 Applicant before: ZHEJIANG University Country or region before: China |
|
| CB02 | Change of applicant information | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |