CN113263496A - Method for optimizing path of six-degree-of-freedom mechanical arm and computer equipment - Google Patents

Method for optimizing path of six-degree-of-freedom mechanical arm and computer equipment Download PDF

Info

Publication number
CN113263496A
CN113263496A CN202110353992.6A CN202110353992A CN113263496A CN 113263496 A CN113263496 A CN 113263496A CN 202110353992 A CN202110353992 A CN 202110353992A CN 113263496 A CN113263496 A CN 113263496A
Authority
CN
China
Prior art keywords
path
mechanical arm
singular
calculating
vicinity
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110353992.6A
Other languages
Chinese (zh)
Other versions
CN113263496B (en
Inventor
刘金祥
高景一
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Radio Measurement
Original Assignee
Beijing Institute of Radio Measurement
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Radio Measurement filed Critical Beijing Institute of Radio Measurement
Priority to CN202110353992.6A priority Critical patent/CN113263496B/en
Publication of CN113263496A publication Critical patent/CN113263496A/en
Application granted granted Critical
Publication of CN113263496B publication Critical patent/CN113263496B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1661—Programme controls characterised by programming, planning systems for manipulators characterised by task planning, object-oriented languages
    • 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/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

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 method for optimizing a path of a six-degree-of-freedom mechanical arm and computer equipment, wherein the method comprises the following steps: judging the state of the mechanical arm, and if the mechanical arm is in a static state, identifying whether a space path passes through the vicinity of a singular configuration of the mechanical arm by using a discrimination method; if the robot is in a moving state, planning a track by using a tracking algorithm to enable the robot arm to stably track a spatial path passing through the vicinity of the singular configuration; the operation time of the discrimination algorithm provided by the invention is less than 1 second, and whether the singular point passes through can be judged before the mechanical arm executes the actual action, so that the response speed of the control command is not influenced. The technical scheme of the invention can avoid sudden change of angular velocity, and the dynamic error of path tracking is less than 1 mm.

Description

Method for optimizing path of six-degree-of-freedom mechanical arm and computer equipment
Technical Field
The invention relates to the field of industrial robots. In particular to a method and computer equipment for optimizing a path of a six-degree-of-freedom mechanical arm.
Background
A six-degree-of-freedom serial mechanical arm is a mechanical arm widely applied in modern industry. The six-degree-of-freedom mechanical arm has a singular configuration and is a serious hidden danger when the robot works. At the singular point, in order for the end effector to acquire velocities in certain directions in cartesian space, the angular velocity of the joint solved from the inverse kinematics of the robot tends to infinity, which indicates that the end effector has lost motion capability in at least one direction.
The singular configurations of robots can be divided into two categories: 1. due to structural limitation, each joint angle can only move within a certain working range, and the robot is in a singular configuration at the boundary of a working space; 2. the singular configurations inside the workspace are typically caused by the coincidence of two or more joint axes. Boundary singularities are easily identified, and singular configurations inside the working space are key research objects. When the mechanical arm moves according to the joint angle, the problem of internal singularity does not exist, and the internal singularity appears under the condition that the mechanical arm moves according to the Cartesian space path.
Typical internal singular configurations include: 1. the axes of the 4 shafts and the 6 shafts are collinear, and the axes of the 5 shafts and the 4 shafts and the 6 shafts are intersected; 2. the 6 shaft is positioned on the axis of the 1 shaft; 3. the intersection points of the axes of the 2 shafts and the 3 shafts and the axes of the 4 shafts, the 5 shafts and the 6 shafts are on the same straight line.
In an actual scene, the mechanical arm not only completely loses one or more degrees of freedom at a singular point, but also has deteriorated kinematic characteristics near the singular point, so that the mechanical arm should be prevented from appearing near the singular point as much as possible. The calculation of the rotation angles of the six joints in the singular poses can provide reliable basis for teaching and trajectory planning, and the calculation method has important significance for designing and using the mechanical arm.
Disclosure of Invention
To solve at least one of the above problems, an object of the present invention is to provide a method for optimizing a path of a six-degree-of-freedom robot arm, comprising:
judging the state of the mechanical arm, and if the mechanical arm is in a static state, identifying whether a space path passes through the vicinity of a singular configuration of the mechanical arm by using a discrimination method; if the robot is in a moving state, planning a track by using a tracking algorithm to enable the robot arm to stably track a spatial path passing through the vicinity of the singular configuration;
the discrimination algorithm comprises: calculating a path function according to the starting point space coordinate, the end point space coordinate, the time interval between two adjacent points and the movement duration of the path; calculating the condition number of the current position and the derivative of the condition number to time to judge whether the output path of the mechanical arm passes through the vicinity of the singular configuration;
the tracking algorithm includes: if the mechanical arm is in a motion state, obtaining a starting point space coordinate p according to a motion path of the mechanical arm0End point space coordinate pnAnd a movement duration tf(ii) a Calculating a condition number for a current location and its condition number pairsAnd the time derivative is used for judging whether the output path of the dynamic mechanical arm passes through the vicinity of the singular position or not and carrying out path optimization according to the judgment result.
In a possible implementation manner, the calculating a path function according to the starting point space coordinate, the ending point space coordinate, the time interval between two adjacent points, and the movement duration of the path specifically includes:
setting the space velocity and the acceleration of the starting point and the ending point as 0, and obtaining a space coordinate p of the starting point according to a known path0End point space coordinate pnTime interval delta t and movement time length t of two adjacent pointsfAnd a path function is obtained by utilizing polynomial interpolation:
Figure BDA0003002994150000021
wherein p isiI is a natural number and n or less, and a coefficient a of the polynomial0＝p0，a1＝a2＝0，a3＝10(pn-p0)/tf 3，a4＝15(pn-p0)/tf 4，a5＝6(pn-p0)/tf 5。
In one possible implementation, the calculated position point PiAnd the condition number derivative of the condition number with respect to time, where PiDenotes the ith point on the path, 0<i<(n +1), i is a natural number; judging whether the mechanical arm output path passes through the vicinity of the singular configuration specifically comprises:
s0011: the computing robot arm is located at piJoint angle q corresponding to timeiJacobian matrix J (q)i) And the Jacobian matrix J (q)i) Condition number of (k)iAnd its derivative with time
Figure BDA0003002994150000022
κi＝σmax(J(qi))/σmin(J(qi)) (2)
Wherein, J (q)i) Is shown at piJoint angle q corresponding to timeiThe Jacobian matrix is obtained, the parameters and the joint angles of the mechanical arm are obtained, and sigma ismax(J(qi) A) and σmin(J(qi) Respectively represent J (q)i) Can be calculated by the pair J (q) of the maximum singular value and the minimum singular valuei) Singular value decomposition is carried out to obtain;
s0012: if κi> epsilon or
Figure BDA0003002994150000023
The output path of the mechanical arm passes by the vicinity of the singular configuration, and the algorithm is finished; if κiIs less than or equal to epsilon and
Figure BDA0003002994150000024
the output path of the mechanical arm does not pass through the vicinity of the singular configuration, and the singular mark s is made to be 0;
s0013: calculating the joint angle of the next position according to formula (3):
qi+1＝qi+J-1(qi)(pi+1-pi) (3)
judging whether i +1 is equal to n: if i +1 is equal to n, the algorithm is ended; and if the i +1 is smaller than n, jumping to the step S0011 to judge the next position.
The calculating the condition number and the condition number derivative with time of the current position comprises:
s0020: obtaining the space coordinate p of the starting point according to the motion path0End point space coordinate pnAnd a movement duration tf；
S0030: starting from the starting point, according to the position point PiThe jacobian matrix J (q) of the current configuration is calculatedi) In which P isiDenotes the ith point on the path, 0<i<(n + 1); and calculating a condition number k according to equation (2)iAnd its derivative with time
Figure BDA0003002994150000031
The interpolation interval Δ t is 0.005 second.
In a possible implementation manner, the determining whether the output path of the kinematic mechanical arm passes through the vicinity of the singular position and performing path optimization according to the determination result includes:
s0041: if κi> epsilon or
Figure BDA0003002994150000032
And the output path of the mechanical arm passes through the vicinity of the singular configuration, the singular mark s is made to be 1, and the damping coefficient lambda is calculated:
let lambda0＝λ0’＝10-5Setting the threshold value ε to 106、ε’＝104The damping coefficient λ is calculated according to equation (4):
Figure BDA0003002994150000033
wherein the parameters ε, ε' and λ0And λ0' is a set threshold;
calculating the space velocity according to equation (5)
Figure BDA0003002994150000034
Figure BDA0003002994150000035
vx、vyAnd vzRepresenting the speed, omega, of translation of the end of the arm along the x, y and z axes of the base coordinate system, respectivelyx、ωyAnd ωzAngular velocities representing rotation of the robot arm tip about the x-axis, y-axis, and z-axis, respectively:
Figure BDA0003002994150000036
wherein a is1＝a2＝0，a3＝10(pn-p0)/tf 3，a4＝15(pn-p0)/tf 4，a5＝6(pn-p0)/tf 5
Calculating the six-axis angular velocity according to equation (6)
Figure BDA0003002994150000037
Figure BDA0003002994150000038
Figure BDA0003002994150000039
Wherein ω is1 ω2 ω3 ω4 ω5 ω6Respectively representing the rotation angular velocities of 6 joints, wherein lambda is a damping coefficient, and I is a unit matrix; jump to step S0060.
The judging whether the output path of the dynamic mechanical arm passes through the vicinity of the singular position or not and the path optimization according to the judgment result further comprises:
s0042: if κiIs less than or equal to epsilon and
Figure BDA00030029941500000310
judging the value of a singular mark s, and if the value of the singular mark s is 1, using the current actual position of the mechanical arm
Figure BDA00030029941500000311
As a starting point, the path is re-planned:
Figure BDA00030029941500000312
coefficients of the polynomial
Figure BDA0003002994150000041
Figure BDA0003002994150000042
Calculating space velocity
Figure BDA0003002994150000043
Figure BDA0003002994150000044
Calculating the six-axis angular velocity according to equation (9)
Figure BDA0003002994150000045
Figure BDA0003002994150000046
The value of the singular flag S is changed to 0 and the process jumps to step S0060.
S0043: if κiIs less than or equal to epsilon and
Figure BDA0003002994150000047
judging the value of a singular mark s, if the value of the singular mark s is 0, calculating the six-axis angular velocity according to the formula (9)
Figure BDA0003002994150000048
Jump to step S0060.
Said step S0060 comprises:
the joint angle at the next position is calculated according to equation (10):
Figure BDA0003002994150000049
let qi+1Moving the robot arm to q for a target joint angle thereofi+1If said next position is a joint angle qi+1And (4) ending the algorithm for ending, otherwise returning to the step S0030 of the tracking algorithm and continuing to execute.
A second aspect of the present application provides a computer readable storage medium having a computer program stored thereon, wherein the program, when executed by a processor, implements the method provided by the first aspect of the present application.
A third aspect of the present application provides a computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing the method provided by the first aspect of the present application when executing the program.
The invention has the following beneficial effects:
the technical scheme of the invention automatically judges whether the space path passes through the vicinity of the singular configuration of the mechanical arm according to the condition number of the Jacobian matrix, and can stably track the singular path. The operation time of the discrimination algorithm provided by the invention is less than 1 second, and whether the singular point passes through can be judged before the mechanical arm executes the actual action, so that the response speed of the control command is not influenced. The technical scheme of the invention can avoid sudden change of the angular speed, the dynamic error of path tracking is less than 1 mm, and the precision is higher.
Drawings
FIG. 1 is a diagram illustrating steps of a method for a six-DOF robot arm to track a singular path according to an embodiment of the present invention;
FIG. 2 is a diagram illustrating steps of a discrimination algorithm of a method for tracking a singular path by a six-degree-of-freedom mechanical arm according to an embodiment of the present invention;
FIG. 3 is a diagram illustrating steps of a tracking algorithm of a method for tracking a singular path by a six-degree-of-freedom mechanical arm according to an embodiment of the present invention;
fig. 4 shows a schematic configuration of a computer system.
Detailed Description
In order to more clearly illustrate the invention, the invention is further described below with reference to preferred embodiments and the accompanying drawings. Similar parts in the figures are denoted by the same reference numerals. It is to be understood by persons skilled in the art that the following detailed description is illustrative and not restrictive, and is not to be taken as limiting the scope of the invention.
One embodiment of the present invention provides a method for optimizing a path of a six-degree-of-freedom robot arm, as shown in figure 1,
in this embodiment, when a path of a six-degree-of-freedom mechanical arm is planned, whether a spatial path passes through the vicinity of a singular configuration of the mechanical arm is identified by a discrimination algorithm before the mechanical arm moves, and if the spatial path passes through the vicinity of the singular configuration, the kinematics characteristics of the mechanical arm are deteriorated;
through a discrimination algorithm, before the movement of the mechanical arm, identifying whether a space path passes through the vicinity of a singular configuration of the mechanical arm comprises the following steps: calculating a path function according to the starting point space coordinate, the end point space coordinate, the time interval between two adjacent points and the movement duration of the path; calculate the current position (position point P)i) Judging whether the output path of the mechanical arm passes through the vicinity of a singular configuration or not according to the condition number and the derivative of the condition number to time;
as shown in fig. 2, the method specifically includes:
setting the space velocity and the acceleration of the starting point and the ending point as 0, and obtaining a space coordinate p of the starting point according to a known path0End point space coordinate pnTime interval delta t and movement time length t of two adjacent pointsfAnd calculating the coefficient of a fifth-order polynomial: a is0＝p0，a1＝a2＝0，a3＝10(pn-p0)/tf 3，a4＝15(pn-p0)/tf 4，a5＝6(pn-p0)/tf 5；
Utilizing a fifth-order polynomial to interpolate to obtain a path function:
Figure BDA0003002994150000051
wherein p isiI is a natural number and is n or less.
Calculating a position point PiCondition number of (current position) and its condition number derivative with time, where PiDenotes the ith point on the path, 0<i<(n + 1); judging whether the mechanical arm output path passes through the vicinity of the singular configuration specifically comprises:
s0011: the computing robot arm is located at piTime pairCorresponding joint angle qiJacobian matrix J (q)i) And the Jacobian matrix J (q)i) Condition number of (k)iAnd its derivative with time
Figure BDA0003002994150000061
κi＝σmax(J(qi))/σmin(J(qi)) (2)
Wherein, J (q)i) Is shown at piJoint angle q corresponding to timeiThe Jacobian matrix is obtained, the parameters and the joint angles of the mechanical arm are obtained, and sigma ismax(J(qi) A) and σmin(J(qi) Respectively represent J (q)i) Can be calculated by the pair J (q) of the maximum singular value and the minimum singular valuei) Singular value decomposition is carried out to obtain; setting the threshold ε to 106、ε’＝104；
S0012: if κi> epsilon or
Figure BDA0003002994150000062
The output path of the mechanical arm passes by the vicinity of the singular configuration, and the algorithm is finished; if κiIs less than or equal to epsilon and
Figure BDA0003002994150000063
the output path of the mechanical arm does not pass through the vicinity of the singular configuration, and the singular mark s is made to be 0;
s0013: the next position (position point P) is calculated according to the formula (3)(i+1)) The joint angle of (a):
qi+1＝qi+J-1(qi)(pi+1-pi) (3)
judging whether i +1 is equal to n: if i +1 is equal to n, the algorithm is ended; and if the i +1 is smaller than n, jumping to the step S0011 to judge the next position.
In this embodiment, when planning a path of a six-degree-of-freedom mechanical arm, a track is planned through a tracking algorithm when the mechanical arm moves, so that the mechanical arm stably tracks a spatial path passing through a vicinity of a singular configuration;
the tracking algorithm includes: if the mechanical arm is in a motion state, obtaining a starting point space coordinate p according to a motion path of the mechanical arm0End point space coordinate pnAnd a movement duration tf(ii) a Calculating the condition number of the current position and the derivative of the condition number to time, judging whether the output path of the dynamic manipulator passes through the vicinity of the singular position or not, and optimizing the path according to the judgment result;
as shown in fig. 3, the method specifically includes:
s0020: the interpolation interval delta t is equal to 0.005 second, and the initialization singular flag S is equal to 0; obtaining the space coordinate p of the starting point according to the motion path0End point space coordinate pnAnd a movement duration tf；
The calculating the condition number and the condition number derivative with time of the current position comprises:
s0030: starting from the starting point, according to the position point PiThe jacobian matrix J (q) of the current configuration is calculatedi) In which P isiDenotes the ith point on the path, 0<i<(n + 1); and calculating a condition number k according to equation (2)iAnd its derivative with time
Figure BDA0003002994150000064
The judging whether the output path of the dynamic mechanical arm passes through the vicinity of the singular position or not and the path optimization according to the judgment result comprises the following steps:
s0041: if κi> epsilon or
Figure BDA0003002994150000071
And the output path of the mechanical arm passes through the vicinity of the singular configuration, the singular mark s is made to be 1, and the damping coefficient lambda is calculated:
let lambda0＝λ0’＝10-5The damping coefficient λ is calculated according to equation (4):
Figure BDA0003002994150000072
wherein the parameters ε, ε' and λ0And λ0' is a set threshold;
calculating the space velocity according to equation (5)
Figure BDA0003002994150000073
Figure BDA0003002994150000074
vx、vyAnd vzRepresenting the speed, omega, of translation of the end of the arm along the x, y and z axes of the base coordinate system, respectivelyx、ωyAnd ωzAngular velocities representing rotation of the robot arm tip about the x-axis, y-axis, and z-axis, respectively:
Figure BDA0003002994150000075
wherein a is1＝a2＝0，a3＝10(pn-p0)/tf 3，a4＝15(pn-p0)/tf 4，a5＝6(pn-p0)/tf 5
Calculating the six-axis angular velocity according to equation (6)
Figure BDA0003002994150000076
Figure BDA0003002994150000077
Figure BDA0003002994150000078
Wherein ω is1 ω2 ω3 ω4 ω5 ω6Respectively representing the rotation angular velocities of 6 joints, wherein lambda is a damping coefficient, and I is a unit matrix; jump to step S0060.
The judging whether the output path of the dynamic mechanical arm passes through the vicinity of the singular position or not and the path optimization according to the judgment result further comprises:
s0042: if κiIs less than or equal to epsilon and
Figure BDA0003002994150000079
judging the value of a singular mark s, and if the value of the singular mark s is 1, using the current actual position of the mechanical arm
Figure BDA00030029941500000710
As a starting point, the path is re-planned:
Figure BDA00030029941500000711
coefficients of the polynomial
Figure BDA00030029941500000712
Figure BDA00030029941500000713
Calculating space velocity
Figure BDA00030029941500000714
Figure BDA0003002994150000081
Calculating the six-axis angular velocity according to equation (9)
Figure BDA0003002994150000082
Figure BDA0003002994150000083
The value of the singular flag S is changed to 0 and the process jumps to step S0060.
Further comprising:
s0043: if κiIs less than or equal to epsilon and
Figure BDA0003002994150000084
judging the value of a singular mark s, if the value of the singular mark s is 0, calculating the six-axis angular velocity according to the formula (9)
Figure BDA0003002994150000085
Jump to step S0060.
Said step S0060 comprises:
the joint angle at the next position is calculated according to equation (10):
Figure BDA0003002994150000086
let qi+1Moving the robot arm to q for a target joint angle thereofi+1If said next position is a joint angle qi+1And (4) ending the algorithm for ending, otherwise returning to the step S0030 of the tracking algorithm and continuing to execute.
A second embodiment of the invention provides a computer-readable storage medium having stored thereon computer instructions which, when executed by a processor, implement the method provided by the above-described embodiments. In practice, the computer-readable storage medium may take any combination of one or more computer-readable media. The computer readable medium may be a computer readable signal medium or a computer readable storage medium.
A computer readable storage medium may be, for example, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination of the foregoing. More specific examples (a non-exhaustive list) of the computer readable storage medium would include the following: an electrical connection having one or more wires, a portable computer diskette, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing. In the present embodiment, a computer readable storage medium may be any tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device.
A computer readable signal medium may include a propagated data signal with computer readable program code embodied therein, for example, in baseband or as part of a carrier wave. Such a propagated data signal may take many forms, including, but not limited to, electro-magnetic, optical, or any suitable combination thereof. A computer readable signal medium may also be any computer readable medium that is not a computer readable storage medium and that can communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device.
A third embodiment of the present invention provides a computer system adapted to implement the rolling production method provided by the above-described embodiments, as shown in fig. 4, including a central processing module (CPU) that can perform various appropriate actions and processes according to a program stored in a Read Only Memory (ROM) or a program loaded from a storage section into a Random Access Memory (RAM). In the RAM, various programs and data necessary for the operation of the computer system are also stored. The CPU, ROM, and RAM are connected thereto via a bus. An input/output (I/O) interface is also connected to the bus.
It should be understood that the above-mentioned embodiments of the present invention are only examples for clearly illustrating the present invention, and are not intended to limit the embodiments of the present invention, and it will be obvious to those skilled in the art that other variations or modifications may be made on the basis of the above description, and all embodiments may not be exhaustive, and all obvious variations or modifications may be included within the scope of the present invention.

Claims (10)

1. A method for optimizing a path of a six-degree-of-freedom mechanical arm is characterized by comprising the following steps:
judging the state of the mechanical arm, and if the mechanical arm is in a static state, identifying whether a space path passes through the vicinity of a singular configuration of the mechanical arm by using a discrimination method; if the robot is in a moving state, planning a track by using a tracking algorithm to enable the robot arm to stably track a spatial path passing through the vicinity of the singular configuration;
the discrimination algorithm comprises: calculating a path function according to the starting point space coordinate, the end point space coordinate, the time interval between two adjacent points and the movement duration of the path; calculating the condition number of the current position and the derivative of the condition number to time, and judging whether the output path of the mechanical arm passes through the vicinity of a singular configuration;
the tracking algorithm includes: if the mechanical arm is in a motion state, obtaining a starting point space coordinate p according to a motion path of the mechanical arm0End point space coordinate pnAnd a movement duration tf(ii) a And calculating the condition number of the current position and the derivative of the condition number to time, judging whether the output path of the dynamic mechanical arm passes through the vicinity of the singular position or not, and optimizing the path according to the judgment result.
2. The method of claim 1,
the calculating the path function according to the starting point space coordinate, the ending point space coordinate, the time interval between two adjacent points and the movement duration of the path specifically comprises:
setting the space velocity and the acceleration of the starting point and the ending point as 0, and obtaining a space coordinate p of the starting point according to a known path0End point space coordinate pnTime interval delta t and movement time length t of two adjacent pointsfAnd a path function is obtained by utilizing polynomial interpolation:
Figure FDA0003002994140000011
wherein p isiI is a natural number and n or less, and a coefficient a of the polynomial0＝p0，a1＝a2＝0，a3＝10(pn-p0)/tf 3，a4＝15(pn-p0)/tf 4，a5＝6(pn-p0)/tf 5。
3. The method of claim 1,
the calculating the condition number of the current position and the derivative of the condition number to time, and judging whether the mechanical arm output path passes through the vicinity of the singular configuration specifically comprises:
s0011: calculating the mechanical arm is located at the current position (position point P)i) Corresponding joint angle qiJacobian matrix J (q)i) And the Jacobian matrix J (q)i) Condition number of (k)iAnd its derivative with time
Figure FDA0003002994140000012
Wherein P isiDenotes the ith point on the path, 0<i<(n +1), i is a natural number;
κi＝σmax(J(qi))/σmin(J(qi)) (2)
wherein, J (q)i) Is shown at piJoint angle q corresponding to timeiThe Jacobian matrix is obtained, the parameters and the joint angles of the mechanical arm are obtained, and sigma ismax(J(qi) A) and σmin(J(qi) Respectively represent J (q)i) Can be calculated by the pair J (q) of the maximum singular value and the minimum singular valuei) Singular value decomposition is carried out to obtain;
s0012: setting the threshold ε to 106、ε’＝104If κi> epsilon or
Figure FDA0003002994140000021
The output path of the mechanical arm passes by the vicinity of the singular configuration, and the algorithm is finished; if κiIs less than or equal to epsilon and
Figure FDA0003002994140000022
the output path of the mechanical arm does not pass through the vicinity of the singular configuration, and the singular mark s is made to be 0;
s0013: calculating the joint angle of the next position according to formula (3):
qi+1＝qi+J-1(qi)(pi+1-pi) (3)
judging whether i +1 is equal to n: if i +1 is equal to n, the algorithm is ended; and if the i +1 is smaller than n, jumping to the step S0011 to judge the next position.
4. The method of claim 1,
the calculating the condition number and the condition number derivative with time of the current position comprises:
s0020: obtaining the space coordinate p of the starting point according to the motion path0End point space coordinate pnAnd a movement duration tf；
S0030: starting from the starting point, according to the position point PiThe jacobian matrix J (q) of the current configuration is calculatedi) In which P isiDenotes the ith point on the path, 0<i<(n + 1); and calculating a condition number k according to equation (2)iAnd its derivative with time
Figure FDA0003002994140000023
The interpolation interval Δ t is 0.005 second.
5. The method of claim 1,
the judging whether the output path of the dynamic mechanical arm passes through the vicinity of the singular position or not and the path optimization according to the judgment result comprises the following steps:
s0041: if κi> epsilon or
Figure FDA0003002994140000029
And the output path of the mechanical arm passes through the vicinity of the singular configuration, the singular mark s is made to be 1, and the damping coefficient lambda is calculated:
let lambda0＝λ0’＝10-5The damping coefficient λ is calculated according to equation (4):
Figure FDA0003002994140000024
wherein the parameters ε, ε' and λ0And λ0' is a set threshold;
calculating the space velocity according to equation (5)
Figure FDA0003002994140000025
Figure FDA0003002994140000026
vx、vyAnd vzRepresenting the speed, omega, of translation of the end of the arm along the x, y and z axes of the base coordinate system, respectivelyx、ωyAnd ωzAngular velocities representing rotation of the robot arm tip about the x-axis, y-axis, and z-axis, respectively:
Figure FDA0003002994140000027
wherein a is1＝a2＝0，a3＝10(pn-p0)/tf 3，a4＝15(pn-p0)/tf 4，a5＝6(pn-p0)/tf 5
Calculating the six-axis angular velocity according to equation (6)
Figure FDA0003002994140000028
Figure FDA0003002994140000031
Figure FDA0003002994140000032
Wherein ω is1 ω2 ω3 ω4 ω5 ω6Respectively representing the rotation angular velocities of 6 joints, wherein lambda is a damping coefficient, and I is a unit matrix; jump to step S0060.
6. The method of claim 5,
the judging whether the output path of the dynamic mechanical arm passes through the vicinity of the singular position or not and the path optimization according to the judgment result further comprises:
s0042: if κiIs less than or equal to epsilon and
Figure FDA0003002994140000033
judging the value of a singular mark s, and if the value of the singular mark s is 1, using the current actual position of the mechanical arm
Figure FDA0003002994140000034
As a starting point, the path is re-planned:
Figure FDA0003002994140000035
coefficients of the polynomial
Figure FDA0003002994140000036
Figure FDA0003002994140000037
Calculating space velocity
Figure FDA0003002994140000038
Figure FDA0003002994140000039
Calculating the six-axis angular velocity according to equation (9)
Figure FDA00030029941400000310
Figure FDA00030029941400000311
The value of the singular flag S is changed to 0 and the process jumps to step S0060.
7. The method of claim 5,
further comprising:
s0043: if κiIs less than or equal to epsilon and
Figure FDA00030029941400000312
judging the value of a singular mark s, if the value of the singular mark s is 0, calculating the six-axis angular velocity according to the formula (9)
Figure FDA00030029941400000313
Jump to step S0060.
8. The method of claim 5, 6 or 7,
said step S0060 comprises:
the joint angle at the next position is calculated according to equation (10):
Figure FDA0003002994140000041
let qi+1Moving the robot arm to q for a target joint angle thereofi+1If said next position is a joint angle qi+1And (4) ending the algorithm for ending, otherwise returning to the step S0030 of the tracking algorithm and continuing to execute.
9. A computer-readable storage medium having stored thereon computer instructions, wherein,
the instructions, when executed by a processor, implement the steps of the method of any one of claims 1 to 8.
10. A computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, characterized in that the processor implements the method according to any of claims 1-8 when executing the program.
CN202110353992.6A 2021-04-01 2021-04-01 Method for optimizing path of six-degree-of-freedom mechanical arm and computer equipment Active CN113263496B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110353992.6A CN113263496B (en) 2021-04-01 2021-04-01 Method for optimizing path of six-degree-of-freedom mechanical arm and computer equipment

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110353992.6A CN113263496B (en) 2021-04-01 2021-04-01 Method for optimizing path of six-degree-of-freedom mechanical arm and computer equipment

Publications (2)

Publication Number Publication Date
CN113263496A true CN113263496A (en) 2021-08-17
CN113263496B CN113263496B (en) 2022-09-23

Family

ID=77228413

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110353992.6A Active CN113263496B (en) 2021-04-01 2021-04-01 Method for optimizing path of six-degree-of-freedom mechanical arm and computer equipment

Country Status (1)

Country Link
CN (1) CN113263496B (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114378830A (en) * 2022-02-18 2022-04-22 深圳市大族机器人有限公司 Robot wrist joint singularity avoidance method and system
CN114800523A (en) * 2022-05-26 2022-07-29 江西省智能产业技术创新研究院 Mechanical arm track correction method, system, computer and readable storage medium

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO1995023054A1 (en) * 1994-02-25 1995-08-31 Uk Robotics Limited Manipulator controller
CN101505925A (en) * 2006-08-31 2009-08-12 波音公司 Approaching and compensating for machine kinematic singularities
CN102528554A (en) * 2010-12-09 2012-07-04 中国科学院沈阳计算技术研究所有限公司 Trajectory optimization method of singular region by virtue of five-axis machining
US8532825B1 (en) * 2010-08-25 2013-09-10 The Boeing Company Software compensation for kinematically singular machines
CN103909522A (en) * 2014-03-19 2014-07-09 华南理工大学 Method of six-DOF industrial robot passing singular region
CN106003057A (en) * 2016-05-18 2016-10-12 北京空间飞行器总体设计部 Rapid judging method for configuration singularity of mechanical arm with redundant degree of freedom
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
CN108638067A (en) * 2018-05-17 2018-10-12 北京邮电大学 A kind of serious prevention of degeneracy strategy of space manipulator movenent performance
CN109310473A (en) * 2016-09-19 2019-02-05 直观外科手术操作公司 Base portion positioning system and associated method for controllable arm
CN110802600A (en) * 2019-11-28 2020-02-18 合肥工业大学 Singularity processing method of six-degree-of-freedom articulated robot

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO1995023054A1 (en) * 1994-02-25 1995-08-31 Uk Robotics Limited Manipulator controller
CN101505925A (en) * 2006-08-31 2009-08-12 波音公司 Approaching and compensating for machine kinematic singularities
US8532825B1 (en) * 2010-08-25 2013-09-10 The Boeing Company Software compensation for kinematically singular machines
CN102528554A (en) * 2010-12-09 2012-07-04 中国科学院沈阳计算技术研究所有限公司 Trajectory optimization method of singular region by virtue of five-axis machining
CN103909522A (en) * 2014-03-19 2014-07-09 华南理工大学 Method of six-DOF industrial robot passing singular region
CN106003057A (en) * 2016-05-18 2016-10-12 北京空间飞行器总体设计部 Rapid judging method for configuration singularity of mechanical arm with redundant degree of freedom
CN109310473A (en) * 2016-09-19 2019-02-05 直观外科手术操作公司 Base portion positioning system and associated method for controllable arm
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
CN108638067A (en) * 2018-05-17 2018-10-12 北京邮电大学 A kind of serious prevention of degeneracy strategy of space manipulator movenent performance
CN110802600A (en) * 2019-11-28 2020-02-18 合肥工业大学 Singularity processing method of six-degree-of-freedom articulated robot

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
谢碧云等: "机器人运动灵活性问题的研究", 《高技术通讯》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114378830A (en) * 2022-02-18 2022-04-22 深圳市大族机器人有限公司 Robot wrist joint singularity avoidance method and system
CN114378830B (en) * 2022-02-18 2024-02-20 深圳市大族机器人有限公司 Robot wrist joint singular avoidance method and system
CN114800523A (en) * 2022-05-26 2022-07-29 江西省智能产业技术创新研究院 Mechanical arm track correction method, system, computer and readable storage medium
CN114800523B (en) * 2022-05-26 2023-12-01 江西省智能产业技术创新研究院 Mechanical arm track correction method, system, computer and readable storage medium

Also Published As

Publication number Publication date
CN113263496B (en) 2022-09-23

Similar Documents

Publication Publication Date Title
Almusawi et al. A new artificial neural network approach in solving inverse kinematics of robotic arm (denso vp6242)
Lu et al. Collision-free and smooth joint motion planning for six-axis industrial robots by redundancy optimization
Palmer et al. Real-time method for tip following navigation of continuum snake arm robots
JP5754454B2 (en) Robot picking system and workpiece manufacturing method
CN111278613B (en) Calibration device, calibration method, and control device
Baeten et al. Hybrid vision/force control at corners in planar robotic-contour following
US8560122B2 (en) Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same
CN113263496B (en) Method for optimizing path of six-degree-of-freedom mechanical arm and computer equipment
JP5025598B2 (en) Interference check control apparatus and interference check control method
JP2019177436A (en) Robot control device, method for determining angle of joint of robot, and program
CN112025772A (en) Mechanical arm autonomous calibration method based on visual measurement
Ahmad et al. Shape recovery from robot contour-tracking with force feedback
JPH0457688A (en) Control device for robot in inertial system
CN116125906A (en) Motion planning method, device and equipment for numerical control machining and storage medium
JP2020171989A (en) Robot teaching system
CN112476435B (en) Calibration method and calibration device for gravity acceleration direction and storage medium
CN114378809A (en) Singularity-free kinematic parameterization of soft robotic manipulators
JP2014065098A (en) Robot device, orbit simulation device for articulated robot and orbit creation method
Bellakehal et al. Force/position control of parallel robots using exteroceptive pose measurements
Xue et al. The posture optimization method based on deformation index in robotic milling process
He et al. A strategy for large workpiece assembly based on hybrid impedance control
JP2013111684A (en) Robot arm control device and control method, robot, robot arm control program, and integrated electronic circuit for controlling robot arm
Angel et al. Parametric identification of a delta type parallel robot
CN115284298A (en) Singularity avoidance method and device for robot, terminal and medium
CN112135718B (en) Control of robots

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