CN110877337B - Intelligent control method, chip and system for three-link manipulator - Google Patents

Intelligent control method, chip and system for three-link manipulator Download PDF

Info

Publication number
CN110877337B
CN110877337B CN201911203827.1A CN201911203827A CN110877337B CN 110877337 B CN110877337 B CN 110877337B CN 201911203827 A CN201911203827 A CN 201911203827A CN 110877337 B CN110877337 B CN 110877337B
Authority
CN
China
Prior art keywords
formula
coordinate system
joint
axis
rotation angle
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.)
Expired - Fee Related
Application number
CN201911203827.1A
Other languages
Chinese (zh)
Other versions
CN110877337A (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.)
Wuhan University of Science and Engineering WUSE
Original Assignee
Wuhan University of Science and Engineering WUSE
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 Wuhan University of Science and Engineering WUSE filed Critical Wuhan University of Science and Engineering WUSE
Priority to CN201911203827.1A priority Critical patent/CN110877337B/en
Publication of CN110877337A publication Critical patent/CN110877337A/en
Application granted granted Critical
Publication of CN110877337B publication Critical patent/CN110877337B/en
Expired - Fee Related 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/1602—Programme controls characterised by the control system, structure, architecture
    • 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

Abstract

The invention relates to an intelligent control method, a chip and a system for a three-link manipulator1The second joint needs to rotate around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3And then controlling the first joint to rotate around the z-axis of the first coordinate system by a first rotation angle theta1The second joint rotates around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3And the coordinates of the tail end of the third connecting rod accurately reach the position of the preset tail end coordinates, so that the three-connecting-rod mechanical arm is accurately controlled.

Description

Intelligent control method, chip and system for three-link manipulator
Technical Field
The invention relates to the technical field of three-connecting-rod manipulator intelligence, in particular to a three-connecting-rod manipulator intelligent control method, a chip and a system.
Background
With the continuous development and progress of automation technology, the application of the three-link manipulator in the field of logistics automation is increasingly wide, the requirement is also continuously improved, and the research on the related technology of the three-link industrial manipulator is also deepened and developed. The three-link manipulator can accurately complete tasks, the accuracy, precision and stability of the three-link manipulator in the motion process are not negligible, and particularly the accurate control of the tail end position of the three-link manipulator is realized. However, the three-link manipulator is a complex sequential joint structure, and the position research situation is complex, so how to realize accurate control of the three-link manipulator is an urgent technical problem to be solved in the industry.
Disclosure of Invention
The invention aims to solve the technical problem of the prior art and provides a three-link manipulator intelligent control method, a chip and a system.
The invention discloses an intelligent control method of a three-link manipulator, which adopts the technical scheme as follows:
s1, according to the preset terminal coordinates (P)x，Py，Pz) Calculating a first rotation angle theta of the first joint required to rotate around the z-axis of the first coordinate system1The second joint needs to rotate around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3；
S2, controlling the first joint to rotate around the z axis of the first coordinate system by a first rotation angle theta1The second joint rotates around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint needs to rotate around the z-axis of a third coordinate system by a third rotation angle theta3；
Wherein the three-link manipulator comprises a first link, a second link and a third link which are connected in sequence, the first joint is the head end of the first link,
the second joint is a joint of the tail end of the first joint and the head end of the second connecting rod,
the third joint is a joint between the tail end of the second connecting rod and the head end of the third connecting rod;
establishing the first coordinate system by taking the position of the first joint as an origin, and taking the first joint as a base of the three-link manipulator;
establishing the second coordinate system with the position of the second joint as an origin;
establishing a third coordinate system by taking the position of the third joint as an origin;
the preset end coordinate (P)x，Py，Pz) The coordinates of the tail end of the third connecting rod on the first coordinate system.
The intelligent control method of the three-connecting-rod manipulator has the following beneficial effects: respectively establishing a first coordinate system, a second coordinate system and a third coordinate system by taking a first joint, a second joint and a third joint of the three-link manipulator as original points, and calculating a first rotation angle theta of the first joint required to rotate around a z-axis of the first coordinate system according to preset terminal coordinates1The second joint needs to rotate around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3And then controlling the first joint to rotate around the z-axis of the first coordinate system by a first rotation angle theta1The second joint rotates around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3And the coordinate of the tail end of the third connecting rod accurately reaches the position of the preset tail end coordinate, and only three parameters need to be controlled: first rotation angle theta1A second rotation angle theta2And a third rotation angle theta3The three-connecting-rod mechanical arm can be accurately controlled, and therefore the three-connecting-rod mechanical arm intelligent control method capable of accurately controlling the three-connecting-rod mechanical arm is realized.
On the basis of the scheme, the intelligent control method for the three-link manipulator can be further improved as follows.
Further, still include: establishing a fourth coordinate system by taking the position of the fourth joint as an origin;
and in any two adjacent coordinate systems of the first coordinate system, the second coordinate system, the third coordinate system and the fourth coordinate system, the x axis of the former coordinate system is perpendicular to and intersects with the z axis of the next coordinate system.
Adopting the further scheme as aboveThe beneficial effects are that: by setting the relation between the first coordinate system, the second coordinate system, the third coordinate system and the fourth coordinate system, the first rotation angle theta is facilitated1A second rotation angle theta2And a third rotation angle theta3And (6) performing calculation.
Further, S1 specifically includes the following steps:
s10, setting a first formula as: a is1＝0，α1＝0，d1＝0，
Figure BDA0002296504640000031
The second formula is: a is2＝A1，
Figure BDA0002296504640000032
d2＝0，
Figure BDA0002296504640000033
The third formula is: a is3＝A2，α3＝0，d3＝0，
Figure BDA0002296504640000034
The fourth formula is: a is4＝A3，
Figure BDA0002296504640000035
d3＝D3，Θ4＝0；
S11, deducing a sixteenth formula, a seventeenth formula and an eighteenth formula according to the first formula to the fourth formula;
according to the sixteenth formula:
Figure BDA0002296504640000036
calculating the first rotation angle theta1；
According to the seventeenth formula:
Figure BDA0002296504640000037
calculating the second rotation angle theta2；
According to the eighteenth formula:
Figure BDA0002296504640000038
calculating the second rotation angle theta3；
Wherein the content of the first and second substances,
Figure BDA0002296504640000041
Figure BDA0002296504640000042
i＝1,2,3,4，aiindicates the ith joint cis xi-1Axis from zi-1To ziThe spatial distance of the shaft;
αirepresenting the ith joint winding xi-1Axis from zi-1To ziThe angle of the shaft;
didenotes the ith joint order ziAxis from xi-1To xiThe spatial distance of the shaft;
Θiindicating the i-th articulation ziAxis from xi-1To xiThe spatial distance of the shaft;
xian axis represents an x-axis of an ith coordinate system among the first to fourth coordinates;
zian axis represents a z-axis of an ith coordinate system among the first to fourth coordinates;
Figure BDA0002296504640000043
representing an angle between an x-axis of the second coordinate system and an x-axis of the first coordinate system;
Figure BDA0002296504640000044
representing an angle between an x-axis of the third coordinate system and an x-axis of the second coordinate system.
Further, S11 specifically includes the following steps:
s110, converting alphaiExpressed by a matrix, a fifth formula is obtained:
Figure BDA0002296504640000045
a is toiExpressed in a matrix, a sixth formula is obtained:
Figure BDA0002296504640000046
will thetaiExpressed in a matrix, a seventh formula is obtained:
Figure BDA0002296504640000047
will diExpressed in a matrix, an eighth equation is obtained:
Figure BDA0002296504640000048
multiplying the fifth formula, the sixth formula, the seventh formula, and the eighth formula to obtain a ninth formula:
Figure BDA0002296504640000051
s111, substituting the first formula, the second formula, the third formula and the fourth formula into the ninth formula, and then performing matrix multiplication to obtain a tenth formula:
Figure BDA0002296504640000052
wherein the content of the first and second substances,
Figure BDA0002296504640000053
s112, representing the tail end of the third connecting rod by using a matrix to obtain an eleventh formula:
Figure BDA0002296504640000054
and A ═ T1Labeled as twelfth formula;
s113, multiplying both sides of the twelfth formula0T1Inverse matrix of
Figure BDA0002296504640000055
A thirteenth formula is derived:oT1 -1A＝oT1 -1T1wherein, in the step (A),
Figure BDA0002296504640000056
s114, according to the fourteenth formula:
Figure BDA0002296504640000057
calculate out
Figure BDA0002296504640000058
Wherein, Px1＝-Px sinΘ1-Py cosΘ1；Py1＝-Px cosΘ1-Py sinΘ1；Pz1＝Pz；
According to a fifteenth formula:
Figure BDA0002296504640000061
calculate outoT1 -1T1；
S115, deducing the sixteenth formula, the seventeenth formula and the eighteenth formula according to the fourteenth formula and the fifteenth formula.
The technical scheme of the chip is as follows: the intelligent control method of the three-link manipulator is used for executing any one of the intelligent control methods.
The chip of the invention has the beneficial effects that: the chip respectively establishes a first coordinate system, a second coordinate system and a third coordinate system by taking a first joint, a second joint and a third joint of the three-link manipulator as original points, and calculates a first rotation angle theta of the first joint required to rotate around a z-axis of the first coordinate system according to preset terminal coordinates1The second joint needs to rotate around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint needs to rotate around the z-axis of the first coordinate system by a third rotation angle theta3Then the chip controls the first joint to rotate around the z axis of the first coordinate system by a first rotation angle theta1The second joint rotates around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3And the coordinate of the tail end of the third connecting rod accurately reaches the position of the preset tail end coordinate, and only three parameters need to be controlled: first rotation angle theta1A second rotation angle theta2And a third rotation angle theta3The three-link manipulator can be accurately controlled.
The technical scheme of the three-connecting-rod mechanical arm intelligent control system is as follows:
the chip comprises three motors, the three motors correspond to the first joint, the second joint and the third joint one by one, the chip is a PLC (programmable logic controller), and the PLC controls the first joint to rotate around the z axis of the first coordinate system by a first rotation angle theta by controlling the corresponding motors1The second joint rotates around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint needs to rotate around the z-axis of a third coordinate system by a third rotation angle theta3。
The three-connecting-rod mechanical arm intelligent control system has the following beneficial effects:
respectively establishing by taking a first joint, a second joint and a third joint of the three-link manipulator as original pointsA first coordinate system, a second coordinate system and a third coordinate system, and a first rotation angle theta of the first joint required to rotate around the z axis of the first coordinate system is calculated according to the preset terminal coordinates1The second joint needs to rotate around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint needs to rotate around the z-axis of the first coordinate system by a third rotation angle theta3Then the chip controls the first joint to rotate around the z axis of the first coordinate system by a first rotation angle theta1The second joint rotates around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3And the coordinate of the tail end of the third connecting rod accurately reaches the position of the preset tail end coordinate, and only three parameters need to be controlled: first rotation angle theta1A second rotation angle theta2And a third rotation angle theta3The three-link manipulator can be accurately controlled.
The chip can adopt a PLC controller, and the PLC controller controls a motor to control a corresponding motor to control the first joint to rotate around the z axis of the first coordinate system by a first rotation angle theta1The second joint rotates around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint needs to rotate around the z-axis of a third coordinate system by a third rotation angle theta3And the structure is simple.
Drawings
Fig. 1 is a schematic flow chart of an intelligent control method for a three-link manipulator according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of a calculation model of an intelligent control method for a three-link manipulator according to an embodiment of the present invention;
fig. 3 is a schematic structural diagram of an intelligent control system of a three-link manipulator according to an embodiment of the present invention.
Fig. 4 is a schematic control logic diagram of an intelligent control system for a three-link manipulator according to an embodiment of the present invention.
Detailed Description
As shown in fig. 1, an intelligent control method for a three-link manipulator according to an embodiment of the present invention includes the following steps:
s1, according to the preset terminal coordinates (P)x，Py，Pz) Calculating a first rotation angle theta of the first joint 1 required to rotate around the z-axis of the first coordinate system1The second joint 3 needs to rotate around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint 5 needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3；
S2, controlling the first joint 1 to rotate around the z axis of the first coordinate system by a first rotation angle theta1The second joint 3 is rotated by a second rotation angle theta around the z-axis of the second coordinate system2And the third joint 5 needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3；
Wherein the three-connecting-rod mechanical hand comprises a first connecting rod 2, a second connecting rod 4 and a third connecting rod 6 which are connected in sequence,
the first joint 1 is a head end of the first connecting rod 2,
the second joint 3 is a joint between the tail end of the first joint 1 and the head end of the second link 4,
the third joint 5 is a connection part of the tail end of the second connecting rod 4 and the head end of the third connecting rod 6;
establishing the first coordinate system by taking the position of the first joint 1 as an origin, and taking the first joint 1 as a base of the three-link manipulator;
establishing the second coordinate system with the position of the second joint 3 as an origin;
establishing the third coordinate system by taking the position of the third joint 5 as an origin;
the preset end coordinate (P)x，Py，Pz) Is the coordinate of the end of the third link 6 on the first coordinate system.
Respectively establishing a first coordinate system, a second coordinate system and a third coordinate system by taking a first joint 1, a second joint 3 and a third joint 5 of the three-link manipulator as original points, and calculating a first rotation angle theta of the first joint 1 required to rotate around a z axis of the first coordinate system according to preset terminal coordinates1The second joint 3 is requiredRotated by a second rotation angle theta around the z-axis of a second coordinate system2And the third joint 5 needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3And then controls the first joint 1 to rotate around the z-axis of the first coordinate system by a first rotation angle theta1The second joint 3 rotates around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint 5 needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3The coordinates of the tail end of the third connecting rod 6 accurately reach the position of the preset tail end coordinates, and only three parameters need to be controlled: first rotation angle theta1A second rotation angle theta2And a third rotation angle theta3The three-connecting-rod mechanical arm can be accurately controlled, and therefore the three-connecting-rod mechanical arm intelligent control method capable of accurately controlling the three-connecting-rod mechanical arm is realized.
Preferably, in the above technical solution, the method further comprises: establishing a fourth coordinate system with the position of the fourth joint 7 as an origin; and in any two adjacent coordinate systems of the first coordinate system, the second coordinate system, the third coordinate system and the fourth coordinate system, the x axis of the former coordinate system is perpendicular to and intersects with the z axis of the next coordinate system. By setting the relation between the first coordinate system, the second coordinate system, the third coordinate system and the fourth coordinate system, the first rotation angle theta is facilitated1A second rotation angle theta2And a third rotation angle theta3And (6) performing calculation.
Preferably, in the above technical solution, S1 specifically includes the following steps:
s10, setting a first formula as: a is1＝0，α1＝0，d1＝0，
Figure BDA0002296504640000091
The second formula is: a is2＝A1，
Figure BDA0002296504640000092
d2＝0，
Figure BDA0002296504640000093
The third formula is: a is3＝A2，α3＝0，d3＝0，
Figure BDA0002296504640000094
The fourth formula is: a is4＝A3，
Figure BDA0002296504640000095
d3＝D3，Θ4＝0；
S11, deducing a sixteenth formula, a seventeenth formula and an eighteenth formula according to the first formula to the fourth formula;
according to the sixteenth formula:
Figure BDA0002296504640000096
calculating the first rotation angle theta1；
According to the seventeenth formula:
Figure BDA0002296504640000097
calculating the second rotation angle theta2；
According to the eighteenth formula:
Figure BDA0002296504640000101
calculating the second rotation angle theta3；
Wherein the content of the first and second substances,
Figure BDA0002296504640000102
Figure BDA0002296504640000103
i＝1,2,3,4，aiindicates the ith joint cis xi-1Axis from zi-1To ziThe spatial distance of the shaft;
αirepresenting the ith joint winding xi-1Axis from zi-1To ziThe angle of the shaft;
didenotes the ith joint order ziAxis from xi-1To xiThe spatial distance of the shaft;
Θiindicating the i-th articulation ziAxis from xi-1To xiThe spatial distance of the shaft;
xian axis represents an x-axis of an ith coordinate system among the first to fourth coordinates;
zian axis represents a z-axis of an ith coordinate system among the first to fourth coordinates;
Figure BDA0002296504640000104
representing an angle between an x-axis of the second coordinate system and an x-axis of the first coordinate system;
Figure BDA0002296504640000105
representing an angle between an x-axis of the third coordinate system and an x-axis of the second coordinate system.
Preferably, in the above technical solution, S11 specifically includes the following steps:
s110, converting alphaiExpressed by a matrix, a fifth formula is obtained:
Figure BDA0002296504640000106
a is toiExpressed in a matrix, a sixth formula is obtained:
Figure BDA0002296504640000107
will thetaiExpressed in a matrix, a seventh formula is obtained:
Figure BDA0002296504640000108
will diExpressed in a matrix, an eighth equation is obtained:
Figure BDA0002296504640000111
multiplying the fifth formula, the sixth formula, the seventh formula, and the eighth formula to obtain a ninth formula:
Figure BDA0002296504640000112
s111, substituting the first formula, the second formula, the third formula and the fourth formula into the ninth formula, and then performing matrix multiplication to obtain a tenth formula:
Figure BDA0002296504640000113
wherein the content of the first and second substances,
Figure BDA0002296504640000114
s112, representing the tail end of the third connecting rod 6 by using a matrix to obtain an eleventh formula:
Figure BDA0002296504640000115
and A ═ T1Labeled as twelfth formula;
s113, multiplying both sides of the twelfth formula0T1Inverse matrix of
Figure BDA0002296504640000116
A thirteenth formula is derived:oT1 -1A＝oT1 -1T1wherein, in the step (A),
Figure BDA0002296504640000117
s114, according to the fourteenth formula:
Figure BDA0002296504640000121
calculate out
Figure BDA0002296504640000122
Wherein, Px1＝-Px sinΘ1-Py cosΘ1；Py1＝-Px cosΘ1-Py sinΘ1；Pz1＝Pz；
According to a fifteenth formula:
Figure BDA0002296504640000123
calculate outoT1 -1T1；
S115, deducing the sixteenth formula, the seventeenth formula and the eighteenth formula according to the fourteenth formula and the fifteenth formula.
The following describes an intelligent control method for a three-link manipulator in the present application in more detail with reference to fig. 2:
in FIG. 2, the X-axis, y-axis and z-axis of the first coordinate system are labeled X, respectively1、Y1And Z1(ii) a The X-axis, y-axis and z-axis of the second coordinate system are respectively marked as X2、Y2And Z2(ii) a The X-axis, y-axis and z-axis of the third coordinate system are respectively marked as X1、Y3And Z3(ii) a The X-axis, y-axis and z-axis of the fourth coordinate system are respectively marked as X4、Y4And Z4(ii) a In the three-link manipulator, the driving joints can drive the corresponding links to perform corresponding actions, which is a conventional technique and is not described herein.
Establishing a first coordinate system, a second coordinate system, a third coordinate system and a fourth coordinate system according to the above contents, and setting a first formula as follows: a is1＝0，α1＝0，d1＝0，
Figure BDA0002296504640000124
The second formula is: a is2＝A1，
Figure BDA0002296504640000125
d2＝0，
Figure BDA0002296504640000126
The third formula is: a is3＝A2，α3＝0，d3＝0，
Figure BDA0002296504640000127
The fourth formula is: a is4＝A3，
Figure BDA0002296504640000128
d3＝D3，Θ4＝0；
Will be alphaiExpressed by a matrix, a fifth formula is obtained:
Figure BDA0002296504640000131
a is toiExpressed in a matrix, a sixth formula is obtained:
Figure BDA0002296504640000132
will thetaiExpressed in a matrix, a seventh formula is obtained:
Figure BDA0002296504640000133
will diExpressed in a matrix, an eighth equation is obtained:
Figure BDA0002296504640000134
multiplying the fifth formula, the sixth formula, the seventh formula, and the eighth formula to obtain a ninth formula:
Figure BDA0002296504640000135
substituting the first formula, the second formula, the third formula and the fourth formula into the ninth formula, and then performing matrix multiplication to obtain a tenth formula:
Figure BDA0002296504640000136
wherein the content of the first and second substances,
Figure BDA0002296504640000137
expressing the tail end of the third connecting rod 6 by a matrix to obtain an eleventh formula:
Figure BDA0002296504640000138
and A ═ T1Labeled as twelfth formula;
s113, multiplying both sides of the twelfth formula0T1Inverse matrix of
Figure BDA0002296504640000141
A thirteenth formula is derived:oT1 -1A＝oT1 -1T1wherein, in the step (A),
Figure BDA0002296504640000142
according to a fourteenth formula:
Figure BDA0002296504640000143
calculate out
Figure BDA0002296504640000144
Wherein, Px1＝-Px sinΘ1-Py cosΘ1；Py1＝-Px cosΘ1-Py sinΘ1；Pz1＝Pz；
According to a fifteenth formula:
Figure BDA0002296504640000145
calculate outoT1 -1T1；
In the fourteenth and fifteenth formulas, i.e.oT1 -1A and0T1 -1T1is equal to 0, a sixteenth formula can be obtained:
Figure BDA0002296504640000146
in the fourteenth and fifteenth formulas, i.e.oT1 -1A and0T1 -1T1and the elements of the third row and the fourth column are equal, a seventeenth formula can be obtained:
Figure BDA0002296504640000147
and eighteenth formula:
Figure BDA0002296504640000148
wherein the content of the first and second substances,
Figure BDA0002296504640000151
obtaining the preset terminal coordinates (P) of the three-link manipulator according to the sixteenth formula, the seventeenth formula and the eighteenth formulax，Py，Pz) At a first rotation angle theta1The first stepTwo rotation angle theta2And a third rotation angle theta3Thereby controlling the first joint 1 to rotate a first rotation angle theta around the z-axis of the first coordinate system1The second joint 3 is rotated by a second rotation angle theta around the z-axis of the second coordinate system2And the third joint 5 needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3And the three-connecting-rod mechanical arm is accurately controlled.
Wherein, the distance units are m, the angle units are rad, which is convenient for calculation, and c2、s2、c23And s23Are dimensionless intermediate variables.
The chip provided by the embodiment of the invention is used for executing the intelligent control method for the three-link manipulator provided by any one of the embodiments.
The chip respectively establishes a first coordinate system, a second coordinate system and a third coordinate system by taking a first joint 1, a second joint 3 and a third joint 5 of the three-link manipulator as original points, and calculates a first rotation angle theta of the first joint 1 required to rotate around a z axis of the first coordinate system according to preset terminal coordinates1The second joint 3 needs to rotate around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint 5 needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3And then controls the first joint 1 to rotate around the z-axis of the first coordinate system by a first rotation angle theta1The second joint 3 rotates around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint 5 needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3The coordinates of the tail end of the third connecting rod 6 accurately reach the position of the preset tail end coordinates, and only three parameters need to be controlled: first rotation angle theta1A second rotation angle theta2And a third rotation angle theta3The three-link manipulator can be accurately controlled, wherein the chip can be a PLC controller or a CPU and the like.
As shown in fig. 3 and 4, the three-link manipulator intelligent control system according to the embodiment of the present invention adopts one chip in the above embodiments, and further includes three motors, and the three motors are connected with the three motorsThe chips correspond to the first joints 1, the second joints 3 and the third joints 5 one by one, and the chips are PLC controllers which control the first joints 1 to rotate around the z axis of the first coordinate system by a first rotation angle theta through controlling corresponding motors1The second joint 3 is rotated by a second rotation angle theta around the z-axis of the second coordinate system2And the third joint 5 needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3。
The chip respectively establishes a first coordinate system, a second coordinate system and a third coordinate system by taking a first joint 1, a second joint 3 and a third joint 5 of the three-link manipulator as original points, and calculates a first rotation angle theta of the first joint 1 required to rotate around a z axis of the first coordinate system according to preset terminal coordinates1The second joint 3 needs to rotate around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint 5 needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3And then controls the first joint 1 to rotate around the z-axis of the first coordinate system by a first rotation angle theta1The second joint 3 rotates around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint 5 needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3The coordinates of the tail end of the third connecting rod 6 accurately reach the position of the preset tail end coordinates, and only three parameters need to be controlled: first rotation angle theta1A second rotation angle theta2And a third rotation angle theta3The three-link manipulator can be accurately controlled.
When the chip is a PLC controller, the PLC controller controls the motor to control the corresponding motor to control the first joint 1 to rotate around the z-axis of the first coordinate system by a first rotation angle theta1The second joint 3 is rotated by a second rotation angle theta around the z-axis of the second coordinate system2And the third joint 5 needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3And the structure is simple.
In detail, a motor corresponding to the first joint 1 is labeled as M1, a motor corresponding to the second joint 3 is labeled as M2, and a motor corresponding to the third joint 5 is labeled as M3; the frequency converter corresponding to M1 is marked as a 1# frequency converter, the frequency converter corresponding to M2 is marked as a 2# frequency converter, and the frequency converter corresponding to M3 is marked as a 3# frequency converter;
the PLC controller is systematically set through a monitoring center computer, namely A1、A2、A3、D3、
Figure BDA0002296504640000161
And preset end coordinates (P)x，Py，Pz) The sequence is stored in a memory database of the PLC controller, and the preset terminal coordinates (P) of the three-link manipulator are obtained according to a sixteenth formula, a seventeenth formula and an eighteenth formulax，Py，Pz) At a first rotation angle theta1A second rotation angle theta2And a third rotation angle theta3(ii) a Then, the PLC controller sends instructions to the 1# frequency converter, the 2# frequency converter and the 3# frequency converter respectively, and then M1, M2 and M3 are controlled to drive the first joint 1 to rotate around the z axis of the first coordinate system by a first rotation angle theta1The second joint 3 is rotated by a second rotation angle theta around the z-axis of the second coordinate system2And the third joint 5 needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3Thereby realizing the accurate control of the three-link manipulator.
In the present invention, the terms "first", "second" are used for descriptive purposes only and are not to be construed as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one such feature. In the description of the present invention, "a plurality" means at least two, e.g., two, three, etc., unless specifically limited otherwise.
In the description herein, references to the description of the term "one embodiment," "some embodiments," "an example," "a specific example," or "some examples," etc., mean that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the invention. In this specification, the schematic representations of the terms used above are not necessarily intended to refer to the same embodiment or example. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples. Furthermore, various embodiments or examples and features of different embodiments or examples described in this specification can be combined and combined by one skilled in the art without contradiction.
Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention, and that variations, modifications, substitutions and alterations can be made to the above embodiments by those of ordinary skill in the art within the scope of the present invention.

Claims (4)

1. An intelligent control method for a three-link manipulator is characterized by comprising the following steps:
s1, according to the preset terminal coordinates (P)x，Py，Pz) Calculating a first rotation angle theta of the first joint required to rotate around the z-axis of the first coordinate system1The second joint needs to rotate around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint needs to rotate around the z-axis of the third coordinate system by a third rotation angle theta3；
S2, controlling the first joint to rotate around the z axis of the first coordinate system by a first rotation angle theta1The second joint rotates around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint needs to rotate around the z-axis of a third coordinate system by a third rotation angle theta3；
Wherein the three-connecting-rod mechanical arm comprises a first connecting rod, a second connecting rod and a third connecting rod which are connected in sequence,
the first joint is the head end of the first connecting rod,
the second joint is a joint of the tail end of the first joint and the head end of the second connecting rod,
the third joint is a joint between the tail end of the second connecting rod and the head end of the third connecting rod;
establishing the first coordinate system by taking the position of the first joint as an origin, and taking the first joint as a base of the three-link manipulator;
establishing the second coordinate system with the position of the second joint as an origin;
establishing a third coordinate system by taking the position of the third joint as an origin;
the preset end coordinate (P)x，Py，Pz) Coordinates of the tail end of the third connecting rod on the first coordinate system;
further comprising: establishing a fourth coordinate system by taking the position of the fourth joint as an origin;
in any two adjacent coordinate systems of the first coordinate system, the second coordinate system, the third coordinate system and the fourth coordinate system, the x axis of the former coordinate system is perpendicular to and intersects with the z axis of the next coordinate system;
s1 specifically includes the following steps:
s10, setting a first formula as: a is1＝0，α1＝0，d1＝0，
Figure FDA0002712170330000021
The second formula is: a is2＝A1，
Figure FDA0002712170330000022
d2＝0，
Figure FDA0002712170330000023
The third formula is: a is3＝A2，α3＝0，d3＝0，
Figure FDA0002712170330000024
The fourth formula is: a is4＝A3，
Figure FDA0002712170330000025
d3＝D3，Θ4＝0；
S11, deducing a sixteenth formula, a seventeenth formula and an eighteenth formula according to the first formula to the fourth formula;
according to the sixteenth formula:
Figure FDA0002712170330000026
calculating the first rotation angle theta1；
According to the seventeenth formula:
Figure FDA0002712170330000027
calculating the second rotation angle theta2；
According to the eighteenth formula:
Figure FDA0002712170330000028
calculating the second rotation angle theta3；
Wherein the content of the first and second substances,
Figure FDA0002712170330000029
Figure FDA00027121703300000210
i＝1,2,3,4，aiindicates the ith joint cis xi-1Axis from zi-1To ziThe spatial distance of the shaft;
αirepresenting the ith joint winding xi-1Axis from zi-1To ziThe angle of the shaft;
didenotes the ith joint order ziAxis from xi-1To xiThe spatial distance of the shaft;
Θiindicating the i-th articulation ziShaft drivenxi-1To xiThe spatial distance of the shaft;
xian axis represents an x-axis of an ith coordinate system among the first to fourth coordinates;
zian axis represents a z-axis of an ith coordinate system among the first to fourth coordinates;
Figure FDA0002712170330000031
representing an angle between an x-axis of the second coordinate system and an x-axis of the first coordinate system;
Figure FDA0002712170330000032
representing an angle between an x-axis of the third coordinate system and an x-axis of the second coordinate system.
2. The intelligent control method of the three-link manipulator according to claim 1, wherein the step S11 specifically includes the steps of:
s110, converting alphaiExpressed by a matrix, a fifth formula is obtained:
Figure FDA0002712170330000033
a is toiExpressed in a matrix, a sixth formula is obtained:
Figure FDA0002712170330000034
will thetaiExpressed in a matrix, a seventh formula is obtained:
Figure FDA0002712170330000035
will diExpressed in a matrix, an eighth equation is obtained:
Figure FDA0002712170330000036
multiplying the fifth formula, the sixth formula, the seventh formula, and the eighth formula to obtain a ninth formula:
Figure FDA0002712170330000037
；
s111, substituting the first formula, the second formula, the third formula and the fourth formula into the ninth formula, and then performing matrix multiplication to obtain a tenth formula:
Figure FDA0002712170330000041
wherein the content of the first and second substances,
Figure FDA0002712170330000042
s112, representing the tail end of the third connecting rod by using a matrix to obtain an eleventh formula:
Figure FDA0002712170330000043
and A ═ T1Labeled as twelfth formula;
s113, multiplying both sides of the twelfth formula0T1Inverse matrix of
Figure FDA0002712170330000044
A thirteenth formula is derived:oT1 -1A＝oT1 -1T1wherein, in the step (A),
Figure FDA0002712170330000045
s114, according to the fourteenth formula:
Figure FDA0002712170330000046
calculate out
Figure FDA0002712170330000047
Wherein, Px1＝-PxsinΘ1-PycosΘ1；Py1＝-PxcosΘ1-PysinΘ1；Pz1＝Pz；
According to a fifteenth formula:
Figure FDA0002712170330000048
calculate outoT1 -1T1；
S115, deducing the sixteenth formula, the seventeenth formula and the eighteenth formula according to the fourteenth formula and the fifteenth formula.
3. A chip, wherein the chip performs the intelligent control method of the three-bar robot according to claim 1 or 2.
4. An intelligent control system of a three-link manipulator, which is characterized in that the chip of claim 3 is adopted, the intelligent control system further comprises three motors, the three motors correspond to the first joint, the second joint and the third joint one by one, the chip is a PLC controller, and the PLC controller controls the corresponding motors to further control the first joint to rotate around the z axis of the first coordinate system by a first rotation angle theta1The second joint rotates around the z-axis of the second coordinate system by a second rotation angle theta2And the third joint needs to rotate around the z-axis of a third coordinate system by a third rotation angle theta3。
CN201911203827.1A 2019-11-29 2019-11-29 Intelligent control method, chip and system for three-link manipulator Expired - Fee Related CN110877337B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911203827.1A CN110877337B (en) 2019-11-29 2019-11-29 Intelligent control method, chip and system for three-link manipulator

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911203827.1A CN110877337B (en) 2019-11-29 2019-11-29 Intelligent control method, chip and system for three-link manipulator

Publications (2)

Publication Number Publication Date
CN110877337A CN110877337A (en) 2020-03-13
CN110877337B true CN110877337B (en) 2020-11-24

Family

ID=69729763

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911203827.1A Expired - Fee Related CN110877337B (en) 2019-11-29 2019-11-29 Intelligent control method, chip and system for three-link manipulator

Country Status (1)

Country Link
CN (1) CN110877337B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111673716A (en) * 2020-06-08 2020-09-18 中冶南方城市建设工程技术有限公司 Dexterous hand positioning method and control system
CN113805528A (en) * 2021-10-11 2021-12-17 深圳华数机器人有限公司 Linkage control method of glass machine and manipulator based on numerical control double-channel system

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8660694B2 (en) * 2008-04-29 2014-02-25 Winfried Lurz Method for computer-aided movement planning of a robot
CN106826813A (en) * 2016-12-12 2017-06-13 杭州宇芯机器人科技有限公司 The robot stabilized motion control method of the foot point assessment that falls with multiple constraint is modeled based on curve matching
CN206899222U (en) * 2017-02-10 2018-01-19 东方电气成都智能科技有限公司 A kind of multi-joint mechanical arm system based on mobile robot
CN108500966A (en) * 2018-03-30 2018-09-07 阜阳师范学院 A kind of novel radio mechanical arm and control method
CN108582152A (en) * 2018-05-14 2018-09-28 电子科技大学 A kind of articulation mechanism with the rotation of Three Degree Of Freedom adjustable point
CN109262610A (en) * 2018-08-30 2019-01-25 珠海格力电器股份有限公司 Solve method, system and the robot of series connection multi-freedom robot end pose

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107263478B (en) * 2017-07-11 2020-03-17 西南科技大学 Method for calculating clamping force of manipulator claw of teleoperation robot and control method thereof

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8660694B2 (en) * 2008-04-29 2014-02-25 Winfried Lurz Method for computer-aided movement planning of a robot
CN106826813A (en) * 2016-12-12 2017-06-13 杭州宇芯机器人科技有限公司 The robot stabilized motion control method of the foot point assessment that falls with multiple constraint is modeled based on curve matching
CN206899222U (en) * 2017-02-10 2018-01-19 东方电气成都智能科技有限公司 A kind of multi-joint mechanical arm system based on mobile robot
CN108500966A (en) * 2018-03-30 2018-09-07 阜阳师范学院 A kind of novel radio mechanical arm and control method
CN108582152A (en) * 2018-05-14 2018-09-28 电子科技大学 A kind of articulation mechanism with the rotation of Three Degree Of Freedom adjustable point
CN109262610A (en) * 2018-08-30 2019-01-25 珠海格力电器股份有限公司 Solve method, system and the robot of series connection multi-freedom robot end pose

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
移动机械臂的运动控制与轨迹规划算法研究;郑秀娟;《中国优秀硕士学位论文全文数据库 信息科技辑》;20130228;正文第42-57页 *
郑秀娟.移动机械臂的运动控制与轨迹规划算法研究.《中国优秀硕士学位论文全文数据库 信息科技辑》.2013,正文第42-57页. *

Also Published As

Publication number Publication date
CN110877337A (en) 2020-03-13

Similar Documents

Publication Publication Date Title
US9248572B2 (en) Axis angle determination method for six-axis robot and control apparatus for six-axis robot
CN107443382A (en) Industrial robot structure parameter error recognizes and compensation method
EP0062244A1 (en) Robot locus control system
CN110877337B (en) Intelligent control method, chip and system for three-link manipulator
CN110757454A (en) Path planning method and device for cooperative rotation of double robots
CN113510690B (en) Inverse kinematics solving method and system for four-degree-of-freedom series robot
CN110653826A (en) Real-time robot trajectory planning method oriented to conveyor belt target following
CN110561440A (en) multi-objective planning method for acceleration layer of redundant manipulator
CN111775145B (en) Control system of serial-parallel robot
CN109623825B (en) Movement track planning method, device, equipment and storage medium
JP2010167515A (en) Multi-axial robot and speed controller for the same
CN108406769A (en) The joint rotation angle of serial manipulator determines method and device
CN113715016B (en) Robot grabbing method, system, device and medium based on 3D vision
CN114274147B (en) Target tracking control method and device, mechanical arm control equipment and storage medium
JP2006260010A (en) Apparatus and method for controlling robot, and computer program
Aparnathi et al. The novel of six axes robotic arm for industrial applications
Abdelaal A study of robot control programing for an industrial robotic arm
CN109648563B (en) Method for controlling motion of serial robot and computer storage medium
Tomas et al. Fuzzy control of robotic arm implemented in PLC
Liu et al. Trajectory planning of robot manipulators based on unit quaternion
CN114888796A (en) Industrial robot trajectory planning principle display device and method
Han et al. A general 6-DOF industrial robot arm control system based on Linux and FPGA
Shuhua et al. Trajectory planning of 6-DOF manipulator based on combination function method
CN109531573B (en) Robot posture smooth path generation method based on sample lines
Fuping et al. Research on inverse kinematics of robot based on motion controller

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
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20201124

Termination date: 20211129

CF01 Termination of patent right due to non-payment of annual fee