CN113601512B - General avoidance method and system for singular points of mechanical arm - Google Patents
General avoidance method and system for singular points of mechanical arm Download PDFInfo
- Publication number
- CN113601512B CN113601512B CN202110968731.5A CN202110968731A CN113601512B CN 113601512 B CN113601512 B CN 113601512B CN 202110968731 A CN202110968731 A CN 202110968731A CN 113601512 B CN113601512 B CN 113601512B
- Authority
- CN
- China
- Prior art keywords
- track
- mechanical arm
- module
- point
- joint
- 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
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
The invention discloses a universal avoidance method and a universal avoidance system for singular points of a mechanical arm, and belongs to the technical field of avoidance of the singular points of the mechanical arm; the technical problem to be solved is as follows: the improvement of a universal avoidance method of the singular point of the mechanical arm is provided; the technical scheme for solving the technical problems is as follows: establishing a kinematic model of the mechanical arm, establishing a reference joint coordinate system, calculating the rotation of each joint by using a rotation theory, and solving a Jacobian matrix; calculating a condition number by using the joint angle value of the track point and the Jacobian matrix; judging whether the mechanical arm is close to a singular point or not by using the condition number, after judging that the mechanical arm is close to the singular point, calculating a singular-avoiding track point by using a singular-avoiding algorithm until the condition number of the Jacobian matrix of the generated track point meets the state of being far away from the singular point and the track point returns to the original Cartesian track again; after planning the singularity-avoiding track, performing smooth processing and track interpolation on the planned singularity-avoiding track to obtain a final singularity-avoiding track; the invention is applied to the mechanical arm.
Description
Technical Field
The invention discloses a universal avoidance method and a universal avoidance system for a mechanical arm singular point, and belongs to the technical field of universal avoidance methods and systems for mechanical arm singular points.
Background
Research and use in the field of mechanical arms such as industrial robots, cooperative robots, medical auxiliary mechanical arms, and the like are also advancing. Especially in the industrial field, the appearance and the application of industrial robots overturn the traditional industrial mode, a large amount of manpower and material resources and labor production cost are saved, the production efficiency is greatly improved, production work in dangerous working environment is carried out by replacing personnel, and a large amount of personal accidents are avoided. Has great significance and plays an important role in promoting the automation degree of industrial production.
Meanwhile, due to the structural characteristics of the mechanical arm, the problem of singular points is inevitably encountered. The singular point problem of the mechanical arm is that the mechanical arm moves at a certain position in a Cartesian space at a limited speed, the mechanical arm loses one or more degrees of freedom, if the mechanical arm moves continuously, the required joint speed is very high or even infinite, the behavior on a Jacobian matrix is that the determinant value of the matrix is zero or close to zero, the Jacobian matrix is irreversible, the inverse solution of the Jacobian matrix is used for solving the joint speed failure or solving a larger joint speed, and the track tracking performance of the mechanical arm and the stability and reliability of the whole system are greatly reduced. The singular point problem is unavoidable in cartesian space motion planning of the robot arm, and a general solution is needed to reduce its impact.
The spherical wrist joint robot is a mechanical arm with the tail end intersected at one point in a continuous triaxial mode, and the characteristic that the tail end is intersected at one point in the continuous triaxial mode also enables the singularity analysis to be simpler when a Jacobi matrix is used; and the three axes at the tail end of the non-spherical wrist joint mechanical arm do not meet at one point, the tail end position of the non-spherical wrist joint mechanical arm is highly coupled with the attitude, the singularity analysis and avoidance by using a Jacobian matrix are difficult, and the calculation cost is high.
The Chinese patent with the application number of 202011439303.5 and the name of a singular point avoiding method and a system for an aspheric wrist mechanical arm discloses that singular factor derivation of the aspheric wrist mechanical arm is realized by a Jacobian matrix change method, but for a part of the aspheric wrist mechanical arm, due to high coupling of the tail end position and the posture and the particularity of the configuration, the Jacobian matrix is quite complex, and if only the Jacobian matrix is used for solving the numerical solution of the determinant thereof, the calculation cost is acceptable, but a series of transformation solving such as matrix transformation solving of a submatrix, matrix inversion of a submatrix, singular factor solving and the like is quite difficult.
Therefore, in order to solve the problem that different mechanical arms have different difficulties in singularity analysis and avoidance, the invention provides a universal singularity avoidance method and system suitable for most of mechanical arms.
Disclosure of Invention
In order to overcome the defects in the prior art, the invention aims to solve the technical problems that: the improvement of a universal avoidance method of the singular point of the mechanical arm is provided.
In order to solve the technical problems, the technical scheme adopted by the invention is as follows: a general avoidance method for a mechanical arm singularity comprises the following steps:
the method comprises the following steps: firstly, establishing a kinematics model of the mechanical arm, establishing a reference joint coordinate system, calculating the rotation of each joint by using a rotation theory, and solving a Jacobian matrix;
step two: converting the Cartesian locus into a joint locus through kinematics, and then obtaining a condition number by interpolation and using a joint angle value and a Jacobian matrix of a locus point;
step three: judging whether the mechanical arm is close to a singular point or not by using the condition number, and after the mechanical arm is judged to be close to the singular point, calculating a singularity-avoiding track point by using a singularity-avoiding algorithm until the condition number of the generated Jacobian matrix of the track point meets the state of being far away from the singular point and the track point returns to the original Cartesian track again;
step four: and after planning the singularity-avoiding track, performing smoothing processing and track interpolation on the planned singularity-avoiding track to obtain a final output singularity-avoiding track.
The first step specifically comprises the following processes:
firstly, a certain joint axis of the mechanical arm is used for establishing a reference coordinate system, and the vector of the direction of each joint axis is calculated by using a rotation theory according to the reference coordinate system
And position vector
Calculating jacobian matrix of mechanical arm by using rotation method, and calculating eachDirection vector of joint axis
And a position vector
Substituting a Jacobian matrix, the calculation formula of the Jacobian matrix J (theta) is as follows:
J(θ)＝[$ 1 $ 2 $ 3 ... $ n ]；
in the above formula:
and theta is the joint angle value of the track point.
The calculation formula of the number of the conditions in the step two is as follows:
the judgment condition for judging whether the mechanical arm is close to the singular point in the third step is as follows:
at a certain track point, if the corresponding condition number is in a decreasing trend and is at a singular influence threshold value c 0 And c _ sec, judging that the mechanical arm movement is approaching a singular point.
The step three of planning a singularity avoidance track by adopting a singularity avoidance algorithm comprises the following steps of:
the singularity avoidance algorithm adopts a velocity potential field algorithm to calculate the repulsion velocity and the attraction velocity, when the condition number c is in a decreasing trend, the process is close to a singularity point, and the track point judged as the singularity point is recorded as the initial point of the singularity avoidance track;
mapping a repulsive potential field formed by singular points into a joint space to obtain the repulsive velocity V of a joint j in the joint space rep Comprises the following steps:
in the above formula: k is the coefficient of repulsion, c _ sec is the safety threshold, c 0 As a singular influence threshold, c θj ' Condition number c vs. Joint Angle value θ j Partial derivatives of (d);
in the above formula:
(θ 1 ，θ 2 ，…，θ n ) The joint angle value of the track is obtained;
deriving the joint repulsion velocity to Cartesian space by using Jacobian matrix, and obtaining Cartesian space repulsion velocity V caused by singular point c_rep ；
The suction speed is Cartesian space speed and is divided into two parts, namely a driving speed V dri And the correction speed Va dj， Driving speed V dri And the original Cartesian space motion velocity V c In the same direction and with V dri ＝V c -V crep L c Wherein L is c The original Cartesian trajectory direction vector is obtained;
corrected velocity V adj Perpendicular to the original cartesian space trajectory, has a V adj = μ Δ x, where μ is the modified velocity coefficient and Δ x is the offset perpendicular to the cartesian trajectory;
using the Jacobian inverse matrix to convert V into V dri 、V adj Derived into the joint space, and V rep (θ j ) Summing to obtain the final joint velocity group;
and (3) running for a time step by using the joint speed of the group, moving to the next track point, continuously using a speed potential field method to obtain the joint speed group and the track point moved to the next track point by using the joint speed of the group until the newly generated track point meets the condition of breaking away from singularity, wherein the condition of breaking away from singularity is that the condition number is on a safety threshold value, and the generated track point returns to the original Cartesian track again.
A universal avoidance system of a mechanical arm singular point comprises a client control display module, a communication module, a motion control module, a motor driving module, peripheral equipment and a singular point avoidance planning module, wherein the communication module is used for communication between the client control display module and the motion control module, communication between the client control display module and the peripheral equipment, communication between the peripheral equipment and the motion control module, and communication between the motion control module and the motor driving module;
the client control display module is used for editing a control instruction by a user, setting the motion state of the mechanical arm, sending the set instruction and the target position mechanical arm state to the motion control module, and simultaneously displaying the parameter and the state of the mechanical arm;
the motion control module is used for receiving instructions sent by the client control display module, analyzing the motion of the mechanical arm, solving the positive and negative kinematics of the mechanical arm, planning the motion in a Cartesian space, interpolating a track and controlling peripheral equipment;
the singular point avoiding planning module is used for planning and calculating singular point avoiding tracks and smoothing the obtained tracks;
the motor driving module is used for converting the displacement in each interpolation period sent by the motion control module into the number of pulses, converting the number of the pulses into waveform signals and feeding the waveform signals back to the driving motor, and the feedback process of motor data.
The motion control module comprises a motion analysis module, a mechanical arm forward-inverse kinematics solving module, a Cartesian space motion planning module and a track interpolation module, wherein the motion analysis module is used for analyzing a motion control command compiled by a user into an internal command of the mechanical arm controller;
the mechanical arm forward and inverse kinematics solving module is used for realizing coordinate transformation operation between a mechanical arm joint angle coordinate system and a mechanical arm Cartesian space coordinate system;
the Cartesian space motion planning module is used for planning the motion track of the end effector in Cartesian space by the mechanical arm, sending the track points planned in Cartesian space to the mechanical arm forward and inverse kinematics solving module and converting the track points into joint angle values in joint space;
and the track interpolation module is used for outputting the Cartesian space track or the motion track obtained by the singularity point evasion planning module to the track interpolation module, interpolating the whole motion track in the joint space according to an interpolation period by the track interpolation module, and sending each axis displacement value in the interpolation period to the motor driving module.
The singular point avoiding planning module comprises a singular point judging and detecting module, a singular point avoiding track calculating module and a track smoothing module, wherein the singular point judging and detecting module is used for calculating whether the joint track point meets the condition number of approaching the singular point in real time in the track interpolation process;
the singular point avoiding track calculating module plans and avoids singular track points by using a planning algorithm mainly based on a speed potential field algorithm when the mechanical arm approaches a singular point in motion;
and the track smoothing module is used for smoothing the route obtained by planning, smoothing the track by using a quintic B-spline curve and outputting the track to the motion control module.
Compared with the prior art, the invention has the beneficial effects that: the universal avoidance method for the singular points of the mechanical arm only adopts the numerical solution of the Jacobian matrix of the mechanical arm, and saves the operation with high calculation cost such as transformation solution of the analytic solution of the Jacobian matrix. The method adopts the method of judging the orientation of the singular point by using the gradient and setting the rejection speed to avoid the singular point, thereby ensuring the existence of the inverse of the Jacobian matrix and carrying out the singular avoidance operation by using a method with lower calculation cost.
Drawings
The invention is further described below with reference to the accompanying drawings:
FIG. 1 is a flow chart of a universal avoidance method of the present invention;
FIG. 2 is a schematic diagram of a circuit structure of the general avoidance system of the present invention.
Detailed Description
As shown in fig. 1 and 2, the invention provides a universal mechanical arm singularity point avoiding method and system which have high reliability, do not need to calculate the specific position type of a singularity point in advance, simplify the singularity point analysis process and are suitable for most mechanical arms.
The singularity point avoiding method for the universal mechanical arm adopts the following technical scheme:
firstly, establishing a kinematic model of the mechanical arm, establishing a reference joint coordinate system, calculating the rotation of each joint by using a rotation theory, and solving a Jacobian matrix;
calculating a condition number of the Jacobian matrix according to the Jacobian matrix, judging whether the condition number is close to a singular point or not by using the condition number, and considering that the mechanical arm is close to the singular point when the condition number is reduced and is close to a safety threshold value;
after the fact that the mechanical arm is close to the singular point is judged, the mechanical arm uses a singularity avoiding algorithm, the algorithm main body searches for attraction speed and repulsion speed for a speed potential field algorithm, and therefore singularity avoiding track points are calculated until the condition number of a Jacobian matrix of the generated track points meets the state of being far away from the singular point and the track points return to the original Cartesian track again;
and after planning the singularity-avoiding track, performing smooth processing and track interpolation on the planned singularity-avoiding track.
The invention discloses a universal singularity avoiding method, which comprises the following specific steps:
firstly, a reference coordinate system is established by a certain joint axis of the mechanical arm, and the direction vector of each joint axis is calculated by using a rotation theory according to the reference coordinate system
And position vector
Calculating jacobian matrix of mechanical arm by using rotation method, and calculating direction vector of each joint axis
And a position vector
Substituting a Jacobian matrix, and calculating a Jacobian matrix J (theta) according to the following formula:
J(θ)＝[$ 1 $ 2 $ 3 ... $n]；
wherein
The Cartesian track is converted into a joint track through a kinematic transformation module of a motion control module, then a condition number c is obtained by interpolation and using a joint angle value theta of a track point, and the condition number formula is as follows:
at a certain locus point, if the corresponding condition number is in a decreasing trend, and the output is at a singular influence threshold value c n And c _ sec is a safety threshold, judging that the motion of the mechanical arm is approaching to a singular point, planning a singular avoidance trajectory by adopting a singular avoidance planning algorithm, and recording the trajectory point as a starting point of the singular avoidance trajectory;
and (4) calculating the repelling speed and the attracting speed by adopting a speed potential field algorithm to avoid singular tracks. When the condition number c is decreasing trend, the singular point process is approached. Mapping a repulsion potential field formed by singular points into a joint space to obtain the repulsion velocity V of the joint j in the joint space rep Comprises the following steps:
where k is the coefficient of repulsion, c _ sec is the safety threshold, c o As a singular influence threshold, c θj ' Condition number c vs. Joint value θ j Partial derivative of
Wherein
(θ 1 ，θ 2 ，…，θ n ) The value of the joint angle of the track is obtained.
Deriving the joint rejection velocity to Cartesian space by using a Jacobian matrix, and obtaining the rejection velocity V of the Cartesian space caused by singular points c_rep . The rejection speed effectively avoids the mechanical arm from being too close to a singular point, guarantees that the Jacobian matrix can still be effectively used, and enables the Jacobian matrix to be used for joint speed and Cartesian speed conversion.
The suction speed is Cartesian space speed and is divided into two parts, namely a driving speed V dri And correcting the velocity V adj . Driving speed V dri And the original Cartesian space motion velocity V c In the same direction and with V dri ＝V c -V crep L c Wherein L is c The original cartesian trajectory direction vector. Corrected velocity V adj Perpendicular to the original cartesian space trajectory, has a V adj = μ Δ x, wherein μ In order to correct the speed coefficient, deltax is the offset perpendicular to the Cartesian trajectory, and the speed is corrected to ensure that the mechanical arm can be corrected back to the original Cartesian trajectory when the mechanical arm is at a safe position far away from a singular point. Using the Jacobian inverse matrix to convert V into V dri 、V adj Derived into the joint space, and V rep (θ j ) And summing to obtain the final joint velocity group. And running a time step by using the set of joint speeds, moving to the next track point, continuously using a speed potential field method to obtain the joint speed set and the track point moved by using the set of joint speeds until the newly generated track point meets the condition of breaking away from singularity, wherein the condition is that the condition number is on a safety threshold value and the generated track point returns to the original Cartesian track again. And after finishing the singularity avoidance planning algorithm, performing track smoothing, performing smoothing treatment on the whole track by using a quintic B-spline curve, and performing interpolation on the smoothed track to obtain a final output singularity avoidance track.
The invention provides a system for realizing the method, which mainly comprises the following steps:
the motion control module is used for receiving instructions sent by the client control display module, analyzing the motion of the mechanical arm, solving the positive and negative kinematics of the mechanical arm, planning the motion in a Cartesian space, interpolating a track and controlling peripheral equipment;
the singular point avoiding planning module is used for planning and calculating the singular point avoiding track and smoothing the obtained track;
the client control display module is used for editing a control instruction by a user, setting the motion state of the mechanical arm, sending the set instruction and the target position mechanical arm state to the motion control module, and simultaneously displaying the parameters and the state of the mechanical arm;
the motor driving control module is used for converting the displacement in each interpolation period sent by the motion control module into the number of pulses, converting the number of the pulses into waveform signals and feeding the waveform signals back to the driving motor, and the feedback process of the motor data;
the communication module is used for the communication between the client control display module and the motion control module, the communication between the client control display module and peripheral equipment, the communication between the peripheral equipment and the motion control module and the communication between the motion control module and the motor driving module;
wherein the motion control module comprises:
the motion analysis module is used for analyzing a motion control instruction compiled by a user into an internal instruction of the mechanical arm controller;
the mechanical arm forward-inverse kinematics solving module is used for performing coordinate transformation operation between a mechanical arm joint angle coordinate system and a mechanical arm Cartesian space coordinate system;
the Cartesian space motion planning module is used for planning the motion trail of the end effector in Cartesian space by the mechanical arm, sending the Cartesian space planning trail points to the mechanical arm forward-inverse kinematics solving module and converting the Cartesian space planning trail points into joint angle values in joint space;
the track interpolation module outputs a Cartesian space track or a motion track obtained by the singular point evasion module to the track interpolation module, the track interpolation module interpolates the whole motion track in joint space according to an interpolation period, and all axis displacement values in the interpolation period are sent to the motor driving module;
the singularity avoidance planning module comprises:
the singular point judgment and detection module calculates whether the joint track points meet the condition of approaching singular points in real time in the track interpolation process, and if the condition number is reduced and the joint track points are safe to approach, the joint track points are the approaching singular points;
the singular point avoiding track calculating module is used for planning and avoiding singular track points by using a planning algorithm mainly based on a velocity potential field algorithm when the mechanical arm approaches singular points in motion;
and the track smoothing module is used for smoothing the route obtained by planning, smoothing the track by using a quintic B-spline curve and outputting the track to the motion control module.
The universal avoidance method of the mechanical arm singular point uses a rotation method to calculate a jacobian matrix of the mechanical arm; after a Jacobian matrix is constructed, adopting a Jacobian condition number as a singular point judgment condition; in the process of approaching to the singular point, a velocity potential field method is adopted to generate repulsion velocity and attraction velocity, the singular point is avoided, and the Jacobian matrix is guaranteed to be still effectively used.
It should be noted that, regarding the specific structure of the present invention, the connection relationship between the modules of each component adopted in the present invention is determined and can be achieved, except for the specific description in the embodiment, the specific connection relationship can bring corresponding technical effects, and the technical problem proposed by the present invention is solved on the premise of not depending on the execution of corresponding software programs, the types and connection manners of the components, modules and specific components in the present invention, except for the specific description, all belong to the prior art such as published patents, published papers and periodicals, or common general knowledge that can be acquired by the technicians in the field before the application date, and no description is needed, so that the technical solution provided by the present application is clear, complete and achievable, and the corresponding entity product can be reproduced or obtained according to the technical means.
Finally, it should be noted that: the above embodiments are only used to illustrate the technical solution of the present invention, and not to limit the same; while the invention has been described in detail and with reference to the foregoing embodiments, it will be understood by those skilled in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some or all of the technical features may be equivalently replaced; and the modifications or the substitutions do not make the essence of the corresponding technical solutions depart from the scope of the technical solutions of the embodiments of the present invention.
Claims (3)
1. A universal avoidance method for singular points of a mechanical arm is characterized by comprising the following steps: the system comprises a client control display module, a communication module, a motion control module, a motor driving module, peripheral equipment and a singularity avoidance planning module, wherein the communication module is used for communication between the client control display module and the motion control module, communication between the client control display module and the peripheral equipment, communication between the peripheral equipment and the motion control module and communication between the motion control module and the motor driving module;
the client control display module is used for editing a control instruction by a user, setting the motion state of the mechanical arm, sending the set instruction and the target position mechanical arm state to the motion control module, and displaying the parameter and the state of the mechanical arm;
the motion control module is used for receiving instructions sent by the client control display module, analyzing the motion of the mechanical arm, solving the positive and negative kinematics of the mechanical arm, planning the motion in a Cartesian space, interpolating a track and controlling peripheral equipment;
the singular point avoiding planning module is used for planning and calculating singular point avoiding tracks and smoothing the obtained tracks;
the motor driving module is used for converting the displacement in each interpolation period sent by the motion control module into the number of pulses, converting the number of the pulses into waveform signals and feeding the waveform signals back to the driving motor, and the feedback process of motor data;
the method comprises the following steps:
the method comprises the following steps: firstly, establishing a kinematic model of the mechanical arm, establishing a reference joint coordinate system, calculating the rotation of each joint by using a rotation theory, and solving a Jacobian matrix;
step two: converting the Cartesian locus into a joint locus through kinematics, and then obtaining a condition number by interpolation and using a joint angle value and a Jacobian matrix of a locus point;
step three: judging whether the mechanical arm is close to a singular point or not by using the condition number, and after the mechanical arm is judged to be close to the singular point, calculating a singularity-avoiding track point by using a singularity-avoiding algorithm until the condition number of the generated Jacobian matrix of the track point meets the state of being far away from the singular point and the track point returns to the original Cartesian track again;
step four: after planning the singularity-avoiding track, performing smooth processing and track interpolation on the planned singularity-avoiding track to obtain a final singularity-avoiding track;
the judgment condition for judging whether the mechanical arm is close to the singular point in the third step is as follows:
at a certain track point, if the corresponding condition number is in a decreasing trend and is at a singular influence threshold value c 0 And c _ sec is a safety threshold, judging that the motion of the mechanical arm is approaching a singular point;
the step three of planning a singularity-avoiding track by adopting a singularity-avoiding algorithm comprises the following steps of:
the singularity avoidance algorithm adopts a velocity potential field algorithm to calculate the repulsion velocity and the attraction velocity, when the condition number c is in a decreasing trend, the process is close to a singularity point, and the track point judged as the singularity point is recorded as the initial point of the singularity avoidance track;
mapping a repulsive potential field formed by singular points into a joint space to obtain the repulsive velocity V of the joint j in the joint space rep Comprises the following steps:
in the above formula: k is the coefficient of repulsion, c _ sec is the safety threshold, c 0 As a singular influence threshold, c θj ' Condition number c vs. Joint Angle value θ j Partial derivatives of (d);
in the above formula:
(θ 1 ，θ 2 ，...，θ n ) The joint angle value of the track is obtained;
deriving the joint repulsion velocity to Cartesian space by using Jacobian matrix, and obtaining Cartesian space repulsion velocity V caused by singular point c_rep ；
The suction speed is Cartesian space speed and is divided into two parts, namely a driving speed V dri And correcting the velocity V adj Driving speed V dri And the original Cartesian space motion velocity V c In the same direction and with V dri ＝V c -V c_rep L c Wherein L is c The original Cartesian trajectory direction vector is obtained;
correction velocity V adj Perpendicular to the original cartesian space trajectory, has a V adj = μ Δ x, where μ is the correction velocity coefficient and Δ x is the offset perpendicular to the cartesian trajectory;
using the Jacobian inverse matrix to convert V into V dri 、V adj Derived into the joint space, and V rep (θ j ) Summing to obtain a final joint velocity group;
the joint speed of the set is used for running for a time step, the set of joint speed is moved to the next track point, the speed potential field method is continuously used for solving the joint speed set and the track point moved to the set of joint speed until the newly generated track point meets the condition of breaking away from singularity, wherein the condition of breaking away from singularity is that the condition number is above the safety threshold value, and the generated track point returns to the original Cartesian track again;
the specific process of the step one is as follows:
firstly, a certain joint axis of the mechanical arm is used for establishing a reference coordinate system, and the vector of the direction of each joint axis is calculated by using a rotation theory according to the reference coordinate system
And a position vector
Calculating the Yaya of the mechanical arm by using a rotation methodComparing the matrix to the direction vector of each joint axis
And a position vector
Substituting the jacobian matrix, the calculation formula of the jacobian matrix J (theta) is as follows:
in the above formula:
theta is a joint angle value of the track point;
the calculation formula of the number of the conditions in the step two is as follows:
2. the universal avoidance method for the singularity of the mechanical arm according to claim 1, wherein the method comprises the following steps: the motion control module comprises a motion analysis module, a mechanical arm forward and inverse kinematics solving module, a Cartesian space motion planning module and a track interpolation module, wherein the motion analysis module is used for analyzing a motion control command compiled by a user into an internal command of the mechanical arm controller;
the mechanical arm forward-inverse kinematics solving module is used for realizing coordinate transformation operation between a mechanical arm joint angular coordinate system and a mechanical arm Cartesian space coordinate system;
the Cartesian space motion planning module is used for planning the motion track of the end effector in Cartesian space by the mechanical arm, sending the track points planned in Cartesian space to the mechanical arm forward-inverse kinematics solving module and converting the track points into joint angle values in joint space;
and the track interpolation module is used for outputting the Cartesian space track or the motion track obtained by the singularity point evasion planning module to the track interpolation module, interpolating the whole motion track in the joint space according to an interpolation period by the track interpolation module, and sending each axis displacement value in the interpolation period to the motor driving module.
3. The universal avoidance method of the singularity of the mechanical arm according to claim 2, wherein the method comprises the following steps: the singular point avoiding planning module comprises a singular point judging and detecting module, a singular point avoiding track calculating module and a track smoothing module, wherein the singular point judging and detecting module is used for calculating whether the joint track point meets the condition number close to the singular point in real time in the track interpolation process;
the singular point avoiding track calculating module plans the singular point avoiding track points by adopting a speed potential field algorithm as a planning algorithm when the singular point is close to the singular point in the motion of the mechanical arm;
and the track smoothing module is used for smoothing the route obtained by planning, smoothing the track by using a quintic B-spline curve and outputting the track to the motion control module.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110968731.5A CN113601512B (en) | 2021-08-23 | 2021-08-23 | General avoidance method and system for singular points of mechanical arm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110968731.5A CN113601512B (en) | 2021-08-23 | 2021-08-23 | General avoidance method and system for singular points of mechanical arm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113601512A CN113601512A (en) | 2021-11-05 |
| CN113601512B true CN113601512B (en) | 2022-12-02 |
Family
ID=78341630
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110968731.5A Active CN113601512B (en) | 2021-08-23 | 2021-08-23 | General avoidance method and system for singular points of mechanical arm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113601512B (en) |
Families Citing this family (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113927603B (en) * | 2021-11-15 | 2023-06-27 | 武汉联影智融医疗科技有限公司 | Mechanical arm dragging control method and device, computer equipment and storage medium |
| CN114211495B (en) * | 2021-12-29 | 2024-04-02 | 泓浒(苏州)半导体科技有限公司 | Self-adaptive track optimization method and system for semiconductor wafer conveying mechanical arm |
| CN114378830B (en) * | 2022-02-18 | 2024-02-20 | 深圳市大族机器人有限公司 | Robot wrist joint singular avoidance method and system |
| CN114571457B (en) * | 2022-03-17 | 2023-07-18 | 中科新松有限公司 | Singular pose avoidance method, apparatus, device and storage medium |
| CN116038686B (en) * | 2022-10-14 | 2023-12-08 | 深圳市大族机器人有限公司 | Robot singular point avoidance method, apparatus, computer device, and storage medium |
| CN116766228A (en) * | 2023-06-30 | 2023-09-19 | 埃斯顿(南京)医疗科技有限公司 | Robot interaction control method, equipment and medium |
| CN116985147B (en) * | 2023-09-27 | 2023-12-22 | 睿尔曼智能科技（北京）有限公司 | Mechanical arm inverse kinematics solving method and device |
| CN117798938B (en) * | 2024-03-01 | 2024-05-28 | 北京长木谷医疗科技股份有限公司 | Non-singular evaluation control method and device for multi-joint robot |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN204765597U (en) * | 2015-07-01 | 2015-11-18 | 太原理工大学 | Human sign detects intelligent bracelet |
| CN105629974A (en) * | 2016-02-04 | 2016-06-01 | 重庆大学 | Robot path planning method and system based on improved artificial potential field method |
| CN107234617A (en) * | 2017-07-10 | 2017-10-10 | 北京邮电大学 | A kind of obstacle-avoiding route planning method of the unrelated Artificial Potential Field guiding of avoidance task |
| CN107831680A (en) * | 2017-11-09 | 2018-03-23 | 哈尔滨工业大学 | The stratification of remote operating mechanical arm keeps away unusual method |
| CN112380655A (en) * | 2020-11-20 | 2021-02-19 | 华南理工大学 | Robot inverse kinematics solving method based on RS-CMSA algorithm |
| CN112589797A (en) * | 2020-12-11 | 2021-04-02 | 中国科学院合肥物质科学研究院 | Method and system for avoiding singular points of non-spherical wrist mechanical arm |
| CN112677153A (en) * | 2020-12-16 | 2021-04-20 | 东北林业大学 | Improved RRT algorithm and industrial robot path obstacle avoidance planning method |
-
2021
- 2021-08-23 CN CN202110968731.5A patent/CN113601512B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN204765597U (en) * | 2015-07-01 | 2015-11-18 | 太原理工大学 | Human sign detects intelligent bracelet |
| CN105629974A (en) * | 2016-02-04 | 2016-06-01 | 重庆大学 | Robot path planning method and system based on improved artificial potential field method |
| CN107234617A (en) * | 2017-07-10 | 2017-10-10 | 北京邮电大学 | A kind of obstacle-avoiding route planning method of the unrelated Artificial Potential Field guiding of avoidance task |
| CN107831680A (en) * | 2017-11-09 | 2018-03-23 | 哈尔滨工业大学 | The stratification of remote operating mechanical arm keeps away unusual method |
| CN112380655A (en) * | 2020-11-20 | 2021-02-19 | 华南理工大学 | Robot inverse kinematics solving method based on RS-CMSA algorithm |
| CN112589797A (en) * | 2020-12-11 | 2021-04-02 | 中国科学院合肥物质科学研究院 | Method and system for avoiding singular points of non-spherical wrist mechanical arm |
| CN112677153A (en) * | 2020-12-16 | 2021-04-20 | 东北林业大学 | Improved RRT algorithm and industrial robot path obstacle avoidance planning method |
Non-Patent Citations (1)
| Title |
|---|
| Collision and Singularity Avoidance Path Planning of 6-DOF Dual-Arm Manipulator;Dong-Eon Kim;《Collision and Singularity Avoidance Path Planning of 6-DOF Dual-Arm Manipulator》;ICIRA;20180811;第195-207页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113601512A (en) | 2021-11-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113601512B (en) | General avoidance method and system for singular points of mechanical arm | |
| CN110421547B (en) | Double-arm robot cooperative impedance control method based on estimation dynamics model | |
| CN112757306B (en) | Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm | |
| CN106647282B (en) | Six-degree-of-freedom robot trajectory planning method considering tail end motion error | |
| CN113601509B (en) | Multi-degree-of-freedom mechanical arm flexible control method and system | |
| CN106475999B (en) | The acceleration control method of Dual-Arm Coordination based on impedance model under hard conditions | |
| CN112372630B (en) | Multi-mechanical-arm cooperative polishing force compliance control method and system | |
| JPH09109072A (en) | Control method for redundant manipulator | |
| CN110605721A (en) | Mechanical arm dragging teaching method based on terminal six-dimensional force sensor | |
| CN110977990A (en) | Mechanical arm dragging teaching method based on terminal six-dimensional force sensor | |
| CN111230873B (en) | Teaching learning-based collaborative handling control system and method | |
| CN108908347B (en) | Fault-tolerant repetitive motion planning method for redundant mobile mechanical arm | |
| CN110673544B (en) | Upper limb rehabilitation robot control method based on adaptive online learning | |
| CN114670191B (en) | Seven-degree-of-freedom elbow offset mechanical arm track planning method | |
| CN104647379A (en) | Dual-arm robot movement control method under non-linear condition of driver | |
| CN112405519A (en) | Motion trajectory planning method for loading and unloading robot | |
| CN109822550B (en) | High-efficiency high-precision teaching method for complex curved surface robot | |
| CN111890349A (en) | Four-degree-of-freedom mechanical arm motion planning method | |
| CN109623812B (en) | Mechanical arm trajectory planning method considering spacecraft body attitude motion | |
| CN110695994B (en) | Finite time planning method for cooperative repetitive motion of double-arm manipulator | |
| CN111309002A (en) | Wheel type mobile robot obstacle avoidance method and system based on vector | |
| CN111515928B (en) | Mechanical arm motion control system | |
| CN113334385A (en) | Planning method for smooth transition between linear tracks of self-driven articulated arm measuring machine | |
| CN107085432B (en) | Target track tracking method of mobile robot | |
| CN114131617B (en) | Intelligent compliant control method and device for industrial 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 |