CN115366115B - Mechanical arm control method and control system thereof and computer readable storage medium - Google Patents

Mechanical arm control method and control system thereof and computer readable storage medium Download PDF

Info

Publication number
CN115366115B
CN115366115B CN202211300005.7A CN202211300005A CN115366115B CN 115366115 B CN115366115 B CN 115366115B CN 202211300005 A CN202211300005 A CN 202211300005A CN 115366115 B CN115366115 B CN 115366115B
Authority
CN
China
Prior art keywords
motion
mechanical arm
speed
motor
control
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202211300005.7A
Other languages
Chinese (zh)
Other versions
CN115366115A (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.)
Guangzhou Weimou Medical Instrument Co ltd
Original Assignee
Guangzhou Weimou Medical Instrument Co ltd
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 Guangzhou Weimou Medical Instrument Co ltd filed Critical Guangzhou Weimou Medical Instrument Co ltd
Priority to CN202211300005.7A priority Critical patent/CN115366115B/en
Publication of CN115366115A publication Critical patent/CN115366115A/en
Application granted granted Critical
Publication of CN115366115B publication Critical patent/CN115366115B/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/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • 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/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor

Abstract

The invention relates to a mechanical arm control method, which comprises the following steps: acquiring the current state of each motor on the mechanical arm; step two: setting a motion track of the mechanical arm; step three: setting a motion instruction of the mechanical arm according to the motion track; step four: sending the set motion instruction to the mechanical arm, and enabling the mechanical arm to respond and move; step five: and setting a mechanical arm movement stopping condition, judging the condition, and stopping the movement of the mechanical arm after the condition is met. Also included are robotic arm control systems and computer-readable storage media that implement the methods. The method directly executes the control of original state constraint on the robot motion; and the compensation control is designed, so that the robot joint can follow the given set position track and set speed with gradual and stable errors, and meanwhile, all signals in a control closed loop are bounded, namely all joint states cannot exceed a limit boundary, so that the problem of overshoot of the mechanical arm in the motion process is avoided, and the positioning precision and the tracking precision are improved.

Description

Mechanical arm control method and control system thereof and computer readable storage medium
Technical Field
The present invention relates to the field of robot arm control, and more particularly, to a robot arm control method, a robot arm control system, and a computer-readable storage medium.
Background
With the development of the robot industry, surgical robots are developed to solve the difficult problems of high-difficulty and high-precision surgery in the medical industry. Particularly, surgical operations such as ophthalmic operations, which require precise operations, can be performed more safely and stably by replacing a human hand with a robot arm. In the medical industry, there are various kinds of auxiliary equipment such as surgical robots. However, the movement control planning route of some devices becomes a difficult problem, for example, the ophthalmic surgical robot needs to move the robot to the vicinity of the ocular surface of the patient and then perform the next operation, and if manual operation is used, the operation precision is difficult to control, so that a replaceable mechanical arm and an internal corresponding structural algorithm are urgently needed, however, the movement of the mechanical arm on the current market on a plane is easy to be limited, and the problem that a large error exists between the actual movement track and the set movement track exists.
In order to solve the problem of motion errors, in the prior art, the motion trajectory or the motion speed is often adjusted by introducing error compensation. But the accuracy of the motion is improved even after compensating for motion errors. However, when the mechanical arm is applied to an operation, due to the limited range of the operation and other damages caused by the out-of-range, when the adjustment is performed by error compensation under a fine motion trajectory, due to the problem of the amplitude of the error, the mechanical arm is easy to generate an overshoot problem in the motion process, and the positioning accuracy and the tracking accuracy of the mechanical arm are affected.
Disclosure of Invention
In order to overcome the problem of overshoot in the operation process of the robot in the prior art, the invention provides the mechanical arm control method, and the problem that the mechanical arm cannot overshoot is avoided.
In order to solve the technical problems, the invention adopts the technical scheme that: the method for controlling the mechanical arm comprises the steps that the corresponding seven-axis mechanical arm comprises two displacement degrees of freedom and 5 rotation degrees of freedom, wherein the first axis and the seventh axis are displacement degrees of freedom, and the second axis, the third axis, the fourth axis, the fifth axis and the sixth axis are rotation degrees of freedom. The first shaft, the second shaft, the third shaft, the fourth shaft, the fifth shaft, the sixth shaft and the seventh shaft respectively correspond to motors No. 1, no. 2, no. 3, no. 4, no. 5, no. 6 and No. 7. Six connecting rod motion through 1 to 6 motors carry out motion control to the arm, place end effector on 7 motors. The established coordinate system is expressed, and the displacement of the No. 1 motor is taken as the origin of the coordinate system
Figure 874360DEST_PATH_IMAGE001
The axis direction of the No. 1 motor is taken as a z axis, and the positive direction is upward; the direction of the connecting rod when the No. 2 motor is at 0 degree is taken as an x axis, and the positive direction faces to the right; the direction of the connecting rod with the motor No. 2 at 90 degrees is the y axis, and the positive direction is forward.
The control method comprises the following steps:
the method comprises the following steps: acquiring the current state of each motor on the mechanical arm;
step two: setting the motion track of the mechanical arm and setting the position of each joint of the mechanical armPlacing track
Figure 636255DEST_PATH_IMAGE002
And velocity trajectory
Figure 577666DEST_PATH_IMAGE003
(ii) a In the present invention, i is expressed as a numerical value of the corresponding motor No. 1-6, such as when i =1, the position trajectory
Figure 19012DEST_PATH_IMAGE004
The position track of No. 1 motor, that is, the position track of No. 1 docking corresponding shutdown.
Step three: setting a motion instruction of the mechanical arm according to the motion track;
the second order kinetic equation of the servo system model of the robot arm is expressed in the form:
Figure 209822DEST_PATH_IMAGE005
wherein the content of the first and second substances,
Figure 902971DEST_PATH_IMAGE006
is a matrix of the quality of the system,
Figure 43097DEST_PATH_IMAGE007
is the term for the coriolis force,
Figure 593027DEST_PATH_IMAGE008
is a gravitational potential energy term.
Figure 474395DEST_PATH_IMAGE009
The system is subjected to all forces and moments;
Figure 95869DEST_PATH_IMAGE010
，
Figure 746293DEST_PATH_IMAGE011
and
Figure 280174DEST_PATH_IMAGE012
the angular position vector, the angular velocity vector and the angular acceleration vector of the electric drive mechanical arm joint are respectively.
Figure 383259DEST_PATH_IMAGE013
Figure 542845DEST_PATH_IMAGE014
Figure 110093DEST_PATH_IMAGE015
Wherein the content of the first and second substances,
Figure 939509DEST_PATH_IMAGE016
the position, velocity and acceleration of the mechanical arm joint 1 are respectively;
Figure 405256DEST_PATH_IMAGE017
the angular positions, angular velocities and angular accelerations of the mechanical arm joints 2 to 6, respectively;
defining joint state variables
Figure 243899DEST_PATH_IMAGE018
Defining a normal vector for the joint state constraint boundary
Figure 400074DEST_PATH_IMAGE019
So that
Figure 790604DEST_PATH_IMAGE020
。
Writing the mechanical arm servo system into the following state space form:
Figure 868281DEST_PATH_IMAGE021
the error vector of the mechanical arm servo system is as follows:
Figure 400630DEST_PATH_IMAGE022
wherein the content of the first and second substances,
Figure 880153DEST_PATH_IMAGE023
is the position track of the mechanical arm joint,
Figure 176005DEST_PATH_IMAGE003
the velocity trajectory of the mechanical arm joint is shown;
Figure 740979DEST_PATH_IMAGE024
in order to be able to track the error in position,
Figure 718162DEST_PATH_IMAGE025
is the velocity following error.
The integral barrier lyapunov function V is as follows:
Figure 927557DEST_PATH_IMAGE026
wherein the content of the first and second substances,
Figure 535256DEST_PATH_IMAGE027
，
Figure 446580DEST_PATH_IMAGE028
is a tiny normal number;
Figure 165138DEST_PATH_IMAGE029
is a positively determined monotonically decreasing function and is continuously derivable from
Figure 415991DEST_PATH_IMAGE030
And is and
Figure 69957DEST_PATH_IMAGE031
is satisfied with a first derivative of
Figure 343943DEST_PATH_IMAGE032
Wherein
Figure 990825DEST_PATH_IMAGE033
And
Figure 96185DEST_PATH_IMAGE034
the position tracking error of the robot mechanical arm joint can be finally converged gradually and stably.
The controller based on the mechanical arm servo system model
Figure 45686DEST_PATH_IMAGE035
The following were used:
Figure 682335DEST_PATH_IMAGE036
wherein, the first and the second end of the pipe are connected with each other,
Figure 8274DEST_PATH_IMAGE037
Figure 30457DEST_PATH_IMAGE038
the design process of the controller by a reverse pushing method is as follows:
Figure 150859DEST_PATH_IMAGE039
the first derivative of (a) is as follows:
Figure 461755DEST_PATH_IMAGE040
wherein, the first and the second end of the pipe are connected with each other,
Figure 198242DEST_PATH_IMAGE041
the following speed controllers were designed:
Figure 950298DEST_PATH_IMAGE042
wherein, the first and the second end of the pipe are connected with each other,
Figure 631815DEST_PATH_IMAGE043
the speed control gain parameter in positive number form has
Figure 164427DEST_PATH_IMAGE044
The following Lyapunov equation containing the velocity following error was designed:
Figure 566590DEST_PATH_IMAGE045
for is to
Figure 314097DEST_PATH_IMAGE046
Derivation can be obtained:
Figure 838619DEST_PATH_IMAGE047
therefore, the motion controller based on the mechanical arm servo system model comprises the following components:
Figure 796211DEST_PATH_IMAGE048
wherein, the first and the second end of the pipe are connected with each other,
Figure 392277DEST_PATH_IMAGE049
the control rate can effectively stabilize position and speed following errors and obtain the index stability of the system;
Figure 118925DEST_PATH_IMAGE050
the speed follows the transposed matrix of the error.
The motion controller of the mechanical arm servo system model sets the position track of each joint of the mechanical arm in the second step
Figure 892977DEST_PATH_IMAGE002
And velocity trajectory
Figure 337865DEST_PATH_IMAGE003
(ii) a Reading the actual position of each joint of the mechanical arm by utilizing an encoder
Figure 144147DEST_PATH_IMAGE051
Calculating position tracking error
Figure 115514DEST_PATH_IMAGE024
Using position tracking errors
Figure 919522DEST_PATH_IMAGE024
For velocity trace
Figure 914023DEST_PATH_IMAGE003
Correcting, receiving the feedback of speed signal, and outputting speed following error
Figure 337045DEST_PATH_IMAGE025
(ii) a Feedback derived position tracking error
Figure 38285DEST_PATH_IMAGE024
And speed following error
Figure 137828DEST_PATH_IMAGE025
Form a control output
Figure 822887DEST_PATH_IMAGE052
Speed feedback output
Figure 970972DEST_PATH_IMAGE053
Location of the joint
Figure 670593DEST_PATH_IMAGE010
And speed status
Figure 816404DEST_PATH_IMAGE011
And the speed output of the motion controller forms a model compensation control output
Figure 519917DEST_PATH_IMAGE054
Finally, use in
Figure 534010DEST_PATH_IMAGE055
Get control input
Figure 209842DEST_PATH_IMAGE056
And obtaining a motion instruction for controlling the motor to move. The motion instruction can directly execute the control of original state constraint, namely, each joint of the mechanical arm can follow the given set position track with asymptotic stable error
Figure 401920DEST_PATH_IMAGE002
And setting a velocity profile
Figure 592730DEST_PATH_IMAGE003
And meanwhile, all signals in a control closed loop are guaranteed to be bounded, namely all joint states cannot exceed a limit boundary.
The concrete demonstration is as follows:
lyapull Ruff equation designed for this invention
Figure 285879DEST_PATH_IMAGE057
In a belt
Figure 206431DEST_PATH_IMAGE058
Can obtain the product
Figure 694044DEST_PATH_IMAGE059
. In a clear view of the above, it is known that,
Figure 450778DEST_PATH_IMAGE060
is positively defined and continuously conductive.
For is to
Figure 478777DEST_PATH_IMAGE061
Derivative and substitute speed controller
Figure 129201DEST_PATH_IMAGE062
And a motion controller
Figure 912350DEST_PATH_IMAGE063
The following can be obtained:
Figure 281014DEST_PATH_IMAGE064
in view of
Figure 191332DEST_PATH_IMAGE058
Is limited by
Figure 493001DEST_PATH_IMAGE065
And brought into the above formula, one can obtain:
Figure 587996DEST_PATH_IMAGE066
wherein the content of the first and second substances,
Figure 568590DEST_PATH_IMAGE067
wherein the content of the first and second substances,
Figure 141654DEST_PATH_IMAGE068
representing the minimum eigenvalue of the matrix. In conclusion, the system is gradually stable, i.e. the system can execute the original state constraint and the exponential convergence.
The output position and the output speed of each joint motor of arm follow the limited range of error, specifically as follows:
Figure 639107DEST_PATH_IMAGE069
in the formula (I), the compound is shown in the specification,
Figure 639424DEST_PATH_IMAGE070
representation matrix
Figure 841735DEST_PATH_IMAGE071
Is determined by the maximum characteristic value of the image,
Figure 280807DEST_PATH_IMAGE072
expressed as the Lyapull Ruff function
Figure 494750DEST_PATH_IMAGE073
Value at initial time t = 0;
step four: sending the set motion instruction to the mechanical arm, and enabling the mechanical arm to respond and move;
step five: and setting mechanical arm movement stopping conditions, judging the conditions in the mechanical arm movement process, and stopping the mechanical arm movement after the conditions are met.
The robotic arm may move in a position mode and/or a velocity mode. The position mode is that the mechanical arm automatically moves according to the set track under the control of a program according to the movement instruction. The speed mode is that an operator manually sends a motion command to the mechanical arm in a set motion command range so as to enable the mechanical arm to move.
Preferably, in the second step, in the position mode, the process of acquiring the position track of each motor is as follows:
s1.1: acquiring coordinate information of a current end effector;
s1.2: and setting the motion trail of the tail end, and reversely deducing the motion trail of each joint motor.
Preferably, the specific process of S1.1 is as follows:
s1.1.1, recording encoder information of motors from 1 to 6 of the current mechanical arm:
Figure 806914DEST_PATH_IMAGE074
the encoder values of the S1.1.2:1 to No. 6 motors are converted into the current motion positions of the motors:
Figure 371888DEST_PATH_IMAGE075
the method for calculating the numerical value rotation angle of the encoder comprises the following steps:
Figure 411388DEST_PATH_IMAGE076
s1.1.3, calculating a position matrix of a current coordinate system by using positive kinematics, wherein the specific form is as follows:
Figure 807734DEST_PATH_IMAGE077
in the formula (I), the compound is shown in the specification,
Figure 415433DEST_PATH_IMAGE078
the displacement of the No. 1 motor on the z axis;
Figure 77489DEST_PATH_IMAGE079
the rotation angles of motors No. 2, no. 3, no. 4, no. 5 and No. 6 are respectively;
Figure 796047DEST_PATH_IMAGE080
the distance between the No. 2 motor and the No. 3 motor, the distance between the No. 3 motor and the No. 4 motor and the distance between the No. 4 motor and the No. 5 motor are respectively;
setting point
Figure 46899DEST_PATH_IMAGE081
Motor positions of No. 1, 2, 3, 4, 5, 6 and 7 respectively
Figure 684554DEST_PATH_IMAGE082
Position of end RCM point, passing point
Figure 224120DEST_PATH_IMAGE083
Making a perpendicular line of the plane of the mechanical arm and intersecting the point
Figure 621734DEST_PATH_IMAGE084
. Connection point
Figure 727094DEST_PATH_IMAGE085
And with
Figure 676595DEST_PATH_IMAGE086
Two points, passing points
Figure 562512DEST_PATH_IMAGE087
Make the connecting line
Figure 888451DEST_PATH_IMAGE088
Perpendicular lines of (A) are intersected at the point
Figure 652577DEST_PATH_IMAGE089
，
Figure 772979DEST_PATH_IMAGE090
Is a point
Figure 83875DEST_PATH_IMAGE091
And point
Figure 72560DEST_PATH_IMAGE092
The distance of (d); passing point
Figure 824615DEST_PATH_IMAGE093
Make a connection
Figure 178236DEST_PATH_IMAGE094
Perpendicular lines of (A) are intersected at the point
Figure 789477DEST_PATH_IMAGE095
，
Figure 191639DEST_PATH_IMAGE096
Is a point
Figure 188414DEST_PATH_IMAGE097
And with
Figure 916199DEST_PATH_IMAGE098
The distance of (d);
Figure 670528DEST_PATH_IMAGE099
is a point
Figure 17327DEST_PATH_IMAGE100
And with
Figure 743975DEST_PATH_IMAGE101
Of the distance of (c).
Figure 501715DEST_PATH_IMAGE102
Is the length of the tip;
Figure 8920DEST_PATH_IMAGE103
the length of No. 2, no. 3, no. 4 and No. 5 motors on the z axis respectively.
Preferably, the specific process of S1.2 is as follows:
let the end-next motion cartesian coordinate system position be:
Figure 18464DEST_PATH_IMAGE104
wherein the content of the first and second substances,
Figure 474984DEST_PATH_IMAGE105
the coordinate values of the terminal RCM point in the coordinate system,
Figure 544572DEST_PATH_IMAGE106
respectively is the connection line of the origin of the coordinate system and the RCM point at the tail end on the plane
Figure 601389DEST_PATH_IMAGE107
Angle formed by the projection of (a).
According to the following calculation, the process is as follows,
Figure 211362DEST_PATH_IMAGE002
is a set position track vector of the mechanical arm joint,
firstly, the motion angles of No. 5 and No. 6 motors under the set end coordinates can be obtained through the position coordinates:
Figure 178181DEST_PATH_IMAGE108
it is known that
Figure 759948DEST_PATH_IMAGE109
、
Figure 445007DEST_PATH_IMAGE110
Then can calculate out
Figure 593091DEST_PATH_IMAGE111
The calculation method is as follows:
Figure 539051DEST_PATH_IMAGE112
definition of
Figure 419282DEST_PATH_IMAGE113
The specific form of (A) is as follows:
Figure 122796DEST_PATH_IMAGE114
Figure 153200DEST_PATH_IMAGE115
the motion angles of the motors No. 2, no. 3 and No. 4 under the set terminal coordinates are obtained as follows:
Figure 829032DEST_PATH_IMAGE116
Figure 535957DEST_PATH_IMAGE117
Figure 664450DEST_PATH_IMAGE118
the position of each joint motor is calculated by integrating the motion angles of No. 1 to No. 6
Figure 232965DEST_PATH_IMAGE119
。
Preferably, in the second step, in the speed mode, the motion command can be issued by the operating lever, and the steps of each control cycle of issuing the motion command by the operating lever are as follows:
(1) Acquiring the motion direction of a six-connecting-rod of a first motor to a six motor; the degrees of freedom in the operating rod correspond to the coordinate directions one by one, the encoder offset of the degrees of freedom in the operating rod at the moment is obtained and compared, and the direction corresponding to the maximum offset is the motion coordinate direction of the six-connecting-rod to be controlled next;
(2) Setting the gear of the movement speed of the six-connecting-rod, and judging whether the operating rod is mistakenly touched; three sections are divided according to the numerical range of the encoder in the operating rod, namely a stop gear [0, low ], a low gear [ low, high ] and a high gear [ high, max ]. Respectively comparing the offset of the obtained numerical value of the encoder of the operating rod with low and high, and if the offset is in a range of a stop gear, not performing motion control on the mechanical arm in the current operation, wherein the gear is used for avoiding motion control errors caused by mistaken touch of the operating rod; if the speed is in the low-speed gear range, setting the linear speed of the next end effector to be low speed vlow; if the speed is within the high-speed gear range, setting the moving linear speed of the next end effector as a high speed vhigh; during the control process of the operating rod, the function loop continuously carries out speed filing comparison and judgment on the offset of the encoder. Every time comparison and judgment is carried out, counting is carried out on the judgment, and the next motion control is allowed to be carried out only when the counting value reaches a certain set value;
(3) According to linear velocity of end effector
Figure 825621DEST_PATH_IMAGE120
Calculating angular velocity of movement of the end effector
Figure 313234DEST_PATH_IMAGE121
The concrete form is as follows:
Figure 53657DEST_PATH_IMAGE122
wherein r is the position vector of the end effector;
calculating the movement speed of each motor of the six-connecting rod in the following manner:
Figure 347235DEST_PATH_IMAGE123
in the formula (I), the compound is shown in the specification,
Figure 997659DEST_PATH_IMAGE124
respectively the linear speeds of the end effector moving in the directions of x, y and z axes;
Figure 531540DEST_PATH_IMAGE125
the angular velocities of motion in the xy, xz and yz axis planes respectively;
Figure 165783DEST_PATH_IMAGE126
the speed tracks from No. 1 motor to No. 6 motor are respectively, and the value is converted into the encoder value of the movement speed of each motor.
Preferably, in step three, in the position mode, according to the position
Figure 59790DEST_PATH_IMAGE127
Differential calculation of set speed
Figure 564721DEST_PATH_IMAGE128
；
In speed mode, according to speed
Figure 456453DEST_PATH_IMAGE129
Position is obtained by integral calculation
Figure 925131DEST_PATH_IMAGE130
。
Preferably, in step four, in the position mode, for the joint state constraint boundaries, normal vector is defined
Figure 763774DEST_PATH_IMAGE131
Satisfies the conditions
Figure 247845DEST_PATH_IMAGE132
Can do sports in time.
Under the speed mode, after meeting the set movement condition, making the mechanical arm move according to the control instruction; the exercise conditions were specifically as follows:
the first condition is as follows: defining a normal vector for the joint velocity limit boundary
Figure 310479DEST_PATH_IMAGE133
So that
Figure 388156DEST_PATH_IMAGE134
. Correcting the speed of each motor in the third step
Figure 374698DEST_PATH_IMAGE135
With a set speed threshold
Figure 854221DEST_PATH_IMAGE136
Comparing, and moving when the speed is less than a set speed threshold;
and a second condition: calculating the time required for the position of each joint to reach the boundary position
Figure 415652DEST_PATH_IMAGE138
Motion may be enabled when time is greater than zero.
The benefits of setting this motion condition are two: firstly, frequent position reading communication is replaced by rapid calculation, and control delay is reduced; secondly, the movement of the mechanical arm is ensured not to exceed the set safety range on the basis of the first point.
Preferably, in the step five, in the position mode, each motor of the mechanical arm completes the preset motion track; in the speed mode, the mechanical arm needs to track the motion of each motor in real time, so the following motion stop conditions are set in the control of the speed mode, and the motion stop conditions are specifically as follows:
starting timing after the motion control command is sent, and stopping motion when the motion time is more than or equal to the time for moving to the boundary position;
or the offset of the encoder in the operating rod is in the stop gear range, the operating rod is considered to be released, and the motion is stopped.
The benefits of setting this motion stop condition are two: firstly, the mechanical arm is effectively prevented from exceeding a set safety range; and secondly, the quick stop motion is realized in the remote control, the mechanical arm cannot shake due to slight mistaken touch of the hand, and the stability of the operation is improved.
A mechanical arm control system comprises a mechanical arm and electronic equipment, wherein the electronic equipment is electrically connected with the mechanical arm; the electronic device comprises a memory for storing a computer program and a processor for executing the computer program to implement the robot arm control method as described above.
A computer-readable storage medium for storing a computer program; wherein the computer program when executed by the processor implements a robot arm control method as described above.
Compared with the prior art, the beneficial effects are that: directly executing the control of original state constraint on the robot motion; and the compensation control is designed, so that the robot joint can follow the given set position track and set speed with a gradually stable error, and meanwhile, all signals in a control closed loop are bounded, namely all joint states cannot exceed a limit boundary, so that the problem of overshoot of the mechanical arm in the motion process is avoided, and the positioning precision and the tracking precision of the mechanical arm are improved.
Drawings
FIG. 1 is a schematic structural view of a seven-axis robotic arm of the present invention;
fig. 2 is a flowchart of a robot control method of the present invention.
Detailed Description
The drawings are for illustrative purposes only and are not to be construed as limiting the patent; for the purpose of better illustrating the embodiments, certain features of the drawings may be omitted, enlarged or reduced, and do not represent the size of an actual product; it will be understood by those skilled in the art that certain well-known structures in the drawings and descriptions thereof may be omitted. The positional relationships depicted in the drawings are for illustrative purposes only and are not to be construed as limiting the present patent.
The same or similar reference numerals in the drawings of the embodiments of the present invention correspond to the same or similar components; in the description of the present invention, it should be understood that if there are terms such as "upper", "lower", "left", "right", "long", "short", etc., indicating orientations or positional relationships based on the orientations or positional relationships shown in the drawings, it is only for convenience of description and simplicity of description, but does not indicate or imply that the device or element referred to must have a specific orientation, be constructed in a specific orientation, and be operated, and therefore, the terms describing the positional relationships in the drawings are only used for illustrative purposes and are not to be construed as limitations of the present patent, and specific meanings of the terms may be understood by those skilled in the art according to specific situations.
The technical scheme of the invention is further described in detail by the following specific embodiments in combination with the attached drawings:
example 1
Fig. 1-2 show an embodiment 1 of a robot arm control method, in which a corresponding seven-axis robot arm, as shown in fig. 1, includes two degrees of freedom of displacement and 5 degrees of freedom of rotation, where the first axis and the seventh axis are degrees of freedom of displacement, and the second axis, the third axis, the fourth axis, the fifth axis, and the fourth axis are degrees of freedom of displacementSix axes are rotational degrees of freedom. The first shaft, the second shaft, the third shaft, the fourth shaft, the fifth shaft, the sixth shaft and the seventh shaft respectively correspond to motors No. 1, no. 2, no. 3, no. 4, no. 5, no. 6 and No. 7. The mechanical arm is controlled by the movement of the six connecting rods of the No. 1 to No. 6 motors, and the No. 7 motor is provided with an end effector. The coordinate system expression is expressed by taking the displacement of the No. 1 motor as 0 as the origin of the coordinate system
Figure 777363DEST_PATH_IMAGE001
The axis direction of the No. 1 motor is taken as the z axis, and the positive direction is upward; the direction of the connecting rod when the No. 2 motor is at 0 degree is taken as an x axis, and the positive direction faces to the right; the direction of the connecting rod with the motor No. 2 at 90 degrees is the y axis, and the positive direction is forward.
The control method comprises the following steps:
the method comprises the following steps: acquiring the current state of each motor on the mechanical arm;
step two: setting the motion track of the mechanical arm and setting the position track of each joint of the mechanical arm
Figure 957809DEST_PATH_IMAGE002
And velocity trajectory
Figure 167204DEST_PATH_IMAGE003
；
Step three: setting a motion instruction of the mechanical arm according to the motion track;
the second order kinetic equation of the servo system model of the robot arm is expressed in the form:
Figure 571641DEST_PATH_IMAGE139
wherein, the first and the second end of the pipe are connected with each other,
Figure 358331DEST_PATH_IMAGE006
is a matrix of the quality of the system,
Figure 201522DEST_PATH_IMAGE007
is the term for the coriolis force,
Figure 655637DEST_PATH_IMAGE008
is a gravitational potential energy term.
Figure 309604DEST_PATH_IMAGE009
The system is subjected to all forces and moments;
Figure 380328DEST_PATH_IMAGE010
，
Figure 902576DEST_PATH_IMAGE011
and with
Figure 335832DEST_PATH_IMAGE012
The angular position vector, the angular velocity vector and the angular acceleration vector of the electric drive mechanical arm joint are respectively.
Figure 19754DEST_PATH_IMAGE013
Figure 919052DEST_PATH_IMAGE014
Figure 979412DEST_PATH_IMAGE015
Figure 204857DEST_PATH_IMAGE016
The position, velocity and acceleration of the mechanical arm joint 1 are respectively;
Figure 184314DEST_PATH_IMAGE017
the angular positions, angular velocities and angular accelerations of the robot joints 2 to 6, respectively;
defining joint state variables
Figure 432893DEST_PATH_IMAGE018
Defining a normal vector for the joint state constraint boundary
Figure 172310DEST_PATH_IMAGE019
So that
Figure 455524DEST_PATH_IMAGE020
。
Writing the robot servo system into the following state space form:
Figure 543566DEST_PATH_IMAGE021
the error vector of the mechanical arm servo system is as follows:
Figure 404074DEST_PATH_IMAGE022
wherein, the first and the second end of the pipe are connected with each other,
Figure 806237DEST_PATH_IMAGE023
is a position track vector of a mechanical arm joint,
Figure 819323DEST_PATH_IMAGE003
the velocity trajectory of the mechanical arm joint is defined;
Figure 78266DEST_PATH_IMAGE024
in order to be able to track the error in position,
Figure 301437DEST_PATH_IMAGE025
to speed follow error.
The integral barrier lyapunov function V is as follows:
Figure 631924DEST_PATH_IMAGE026
wherein, the first and the second end of the pipe are connected with each other,
Figure 624151DEST_PATH_IMAGE027
，
Figure 132624DEST_PATH_IMAGE028
is a tiny normal number;
Figure 639829DEST_PATH_IMAGE029
is a positively determined monotonically decreasing function and is continuously derivable from
Figure 649373DEST_PATH_IMAGE030
And is and
Figure 355161DEST_PATH_IMAGE031
is satisfied with
Figure 424748DEST_PATH_IMAGE032
In which
Figure 153670DEST_PATH_IMAGE033
And
Figure 856920DEST_PATH_IMAGE034
the position tracking error of the robot mechanical arm joint is a normal number, so that the position tracking error of the robot mechanical arm joint can be gradually and stably converged finally.
The controller based on the mechanical arm servo system model
Figure 558159DEST_PATH_IMAGE035
The following:
Figure 657702DEST_PATH_IMAGE036
wherein, the first and the second end of the pipe are connected with each other,
Figure 77182DEST_PATH_IMAGE037
Figure 225267DEST_PATH_IMAGE038
the design process of the controller by a reverse pushing method is as follows:
Figure 921959DEST_PATH_IMAGE039
the first derivative of (a) is as follows:
Figure 333348DEST_PATH_IMAGE040
wherein, the first and the second end of the pipe are connected with each other,
Figure 99179DEST_PATH_IMAGE041
the following speed controllers were designed:
Figure 254217DEST_PATH_IMAGE042
wherein, the first and the second end of the pipe are connected with each other,
Figure 726787DEST_PATH_IMAGE043
the speed control gain parameter, in positive form, then has:
Figure 184444DEST_PATH_IMAGE044
the following Lyapunov equation containing the velocity following error was designed:
Figure 578516DEST_PATH_IMAGE045
for is to
Figure 130720DEST_PATH_IMAGE046
Derivation can be obtained:
Figure 926638DEST_PATH_IMAGE047
therefore, the motion controller based on the mechanical arm servo system model comprises the following components:
Figure 210989DEST_PATH_IMAGE048
in the formula (I), the compound is shown in the specification,
Figure 967723DEST_PATH_IMAGE027
，
Figure 198984DEST_PATH_IMAGE028
is a tiny normal number;
Figure 974042DEST_PATH_IMAGE019
so that
Figure 694874DEST_PATH_IMAGE020
；
Figure 797959DEST_PATH_IMAGE140
A speed control gain parameter in the form of a positive number;
Figure 705348DEST_PATH_IMAGE141
the control rate can effectively stabilize position and speed following errors and obtain the exponential stability of the system;
Figure 210278DEST_PATH_IMAGE142
is the transposed matrix of the velocity following error.
The output position and the output speed of each joint motor of the mechanical arm are limited by following the error, and the method specifically comprises the following steps:
Figure 429907DEST_PATH_IMAGE069
in the formula (I), the compound is shown in the specification,
Figure 817026DEST_PATH_IMAGE070
representation matrix
Figure 390090DEST_PATH_IMAGE071
Is determined by the maximum characteristic value of (a),
Figure 890472DEST_PATH_IMAGE072
expressed as Lyapunov Functions
Figure 953106DEST_PATH_IMAGE073
Value at initial time t = 0.
The motion controller of the mechanical arm servo system model sets the position track of each joint of the mechanical arm in the second step
Figure 30784DEST_PATH_IMAGE002
And velocity trajectory
Figure 266593DEST_PATH_IMAGE003
(ii) a Reading the actual position of each joint of the mechanical arm by using an encoder
Figure 746116DEST_PATH_IMAGE051
Calculating position tracking error
Figure 58280DEST_PATH_IMAGE024
Using position tracking errors
Figure 419991DEST_PATH_IMAGE024
For velocity locus
Figure 600436DEST_PATH_IMAGE003
Correcting, receiving the feedback of speed signal, and outputting speed following error
Figure 793520DEST_PATH_IMAGE025
(ii) a Feedback derived position tracking error
Figure 401219DEST_PATH_IMAGE024
And speed following error
Figure 250227DEST_PATH_IMAGE025
Form a control output
Figure 109729DEST_PATH_IMAGE052
Speed feedback output
Figure 32686DEST_PATH_IMAGE053
Location of the joint
Figure 935920DEST_PATH_IMAGE010
And speed state
Figure 272223DEST_PATH_IMAGE011
And the speed output of the motion controller forms a model compensation control output
Figure 794472DEST_PATH_IMAGE054
Finally, use
Figure 981389DEST_PATH_IMAGE055
Get control input
Figure 665311DEST_PATH_IMAGE056
And obtaining a motion instruction for controlling the motor to move. The motion instruction can directly execute the control of original state constraint, namely, each joint of the mechanical arm can follow the given set position track with asymptotically stable error
Figure 816807DEST_PATH_IMAGE002
And setting the speed
Figure 673904DEST_PATH_IMAGE003
And meanwhile, all signals in a control closed loop are guaranteed to be bounded, namely all joint states cannot exceed a limit boundary.
In the present embodiment, the robot arm moves in a position mode, and the position locus of each joint
Figure 571453DEST_PATH_IMAGE002
The acquisition process comprises the following steps:
s1.1: acquiring coordinate information of a current end effector;
s1.1.1, recording encoder information of motors from 1 to 6 of the current mechanical arm:
Figure 832801DEST_PATH_IMAGE074
the encoder values of the S1.1.2:1 to No. 6 motors are converted into the current motion positions of the motors:
Figure 878118DEST_PATH_IMAGE075
in the formula (I), the compound is shown in the specification,
Figure 742169DEST_PATH_IMAGE078
is the displacement of the No. 1 motor;
Figure 150016DEST_PATH_IMAGE079
the rotation angles of motors No. 2, no. 3, no. 4, no. 5 and No. 6 are respectively;
the method for calculating the numerical value rotation angle of the encoder comprises the following steps:
Figure 441320DEST_PATH_IMAGE076
s1.1.3, calculating a position matrix of a current coordinate system by using positive kinematics, wherein the specific form is as follows:
Figure 786982DEST_PATH_IMAGE077
in the formula (I), the compound is shown in the specification,
Figure 251462DEST_PATH_IMAGE078
the displacement of the No. 1 motor moving on the z axis;
Figure 389182DEST_PATH_IMAGE079
the rotation angles of motors No. 2, no. 3, no. 4, no. 5 and No. 6 are respectively;
Figure 710442DEST_PATH_IMAGE080
the distance between the No. 2 motor and the No. 3 motor, the distance between the No. 3 motor and the No. 4 motor and the distance between the No. 4 motor and the No. 5 motor are respectively;
setting point
Figure 933613DEST_PATH_IMAGE081
Number 1, 2, 3, 4, 5, 6, 7 motor positions and points respectively
Figure 936204DEST_PATH_IMAGE082
Position of end RCM point, passing point
Figure 538217DEST_PATH_IMAGE083
Making the perpendicular line of the mechanical arm plane and intersecting at the point
Figure 436903DEST_PATH_IMAGE084
. Connection point
Figure 272004DEST_PATH_IMAGE085
And
Figure 15969DEST_PATH_IMAGE086
two points, passing points
Figure 925019DEST_PATH_IMAGE087
Make the connecting line
Figure 601464DEST_PATH_IMAGE088
Perpendicular lines of (A) are intersected at the point
Figure 533648DEST_PATH_IMAGE089
，
Figure 205938DEST_PATH_IMAGE090
Is a point
Figure 969494DEST_PATH_IMAGE091
And point
Figure 209983DEST_PATH_IMAGE092
The distance of (a); passing point
Figure 239250DEST_PATH_IMAGE093
Make a connection
Figure 387334DEST_PATH_IMAGE094
Perpendicular lines of (A) are intersected at the point
Figure 536556DEST_PATH_IMAGE095
，
Figure 744683DEST_PATH_IMAGE096
Is a point
Figure 651460DEST_PATH_IMAGE097
And with
Figure 150705DEST_PATH_IMAGE098
The distance of (d);
Figure 92116DEST_PATH_IMAGE099
is a point
Figure 471145DEST_PATH_IMAGE100
And with
Figure 724272DEST_PATH_IMAGE101
The distance of (a);
Figure 417421DEST_PATH_IMAGE102
is the length of the tip;
Figure 744497DEST_PATH_IMAGE103
the lengths of No. 2, no. 3, no. 4 and No. 5 motors on the z axis are respectively.
S1.2: setting a motion track of the tail end, and reversely deducing the motion track of each joint motor;
let the end-next motion cartesian coordinate system positions be:
Figure 373056DEST_PATH_IMAGE104
firstly, the motion angles of No. 5 and No. 6 motors under the set end coordinates can be obtained through the position coordinates:
Figure 988845DEST_PATH_IMAGE108
it is known that
Figure 610319DEST_PATH_IMAGE109
、
Figure 260744DEST_PATH_IMAGE110
Then can calculate out
Figure 715996DEST_PATH_IMAGE111
The calculation method is as follows:
Figure 951237DEST_PATH_IMAGE112
definition of
Figure 986189DEST_PATH_IMAGE143
The specific form of (A) is as follows:
Figure 615754DEST_PATH_IMAGE114
Figure 241907DEST_PATH_IMAGE115
the motion angles of the motors No. 2, no. 3 and No. 4 under the set terminal coordinates are obtained as follows:
Figure 97868DEST_PATH_IMAGE116
Figure 546298DEST_PATH_IMAGE117
Figure 905735DEST_PATH_IMAGE118
the position of each joint motor is calculated by integrating the motion angles of No. 1 to No. 6
Figure 296265DEST_PATH_IMAGE119
；
Step four: sending the set motion instruction to the mechanical arm, and enabling the mechanical arm to respond and move; in this embodiment, after the mechanical arm is judged to meet the set motion condition after meeting the motion condition, the mechanical arm is made to move according to the motion instruction; the motion conditions are as follows: defining normal vector for limiting boundary of joint state
Figure 170680DEST_PATH_IMAGE131
Satisfies the conditions
Figure 813014DEST_PATH_IMAGE132
The mechanical arm can move.
Step five: and setting mechanical arm movement stopping conditions, judging the conditions in the mechanical arm movement process, and stopping the mechanical arm movement after the conditions are met. The stop condition of the mechanical arm is that each motor completes the motion track set in advance.
The beneficial effects of the embodiment: the mechanical arm can automatically move along the set motion track and motion instruction, and the motion of the robot is directly controlled by the original state constraint; and the compensation control is designed, so that the robot joint can follow the given set position track and set speed with gradual and stable errors, and meanwhile, all signals in a control closed loop are bounded, namely all joint states cannot exceed a limit boundary, so that the problem of overshoot of the mechanical arm in the motion process is avoided, and the positioning precision and the tracking precision of the mechanical arm are improved.
Example 2
Embodiment 2 of a robot arm control method, wherein the corresponding seven-axis robot armAs shown in fig. 1, the two degrees of freedom displacement and 5 degrees of freedom rotation are included, wherein the first axis and the seventh axis are degrees of freedom displacement, and the second axis, the third axis, the fourth axis, the fifth axis and the sixth axis are degrees of freedom rotation. The first shaft, the second shaft, the third shaft, the fourth shaft, the fifth shaft, the sixth shaft and the seventh shaft respectively correspond to motors No. 1, no. 2, no. 3, no. 4, no. 5, no. 6 and No. 7. The mechanical arm is controlled by the movement of the six connecting rods of the No. 1 to No. 6 motors, and the No. 7 motor is provided with an end effector. The established coordinate system is expressed, and the displacement of the No. 1 motor is taken as the origin of the coordinate system
Figure 902324DEST_PATH_IMAGE001
The axis direction of the No. 1 motor is taken as the z axis, and the positive direction is upward; the direction of the connecting rod when the No. 2 motor is at 0 degree is taken as an x axis, and the positive direction faces to the right; the direction of the connecting rod with the motor No. 2 at 90 degrees is the y axis, and the positive direction is forward.
The control method comprises the following steps:
the method comprises the following steps: acquiring the current state of each motor on the mechanical arm;
step two: setting the motion track of the mechanical arm and setting the position track of each joint of the mechanical arm
Figure 339121DEST_PATH_IMAGE002
And velocity trajectory
Figure 700833DEST_PATH_IMAGE003
；
Step three: setting a motion instruction of the mechanical arm according to the motion track;
the motion controller based on the mechanical arm servo system model comprises the following components:
Figure 5912DEST_PATH_IMAGE144
wherein, the first and the second end of the pipe are connected with each other,
Figure 74362DEST_PATH_IMAGE027
，
Figure 478799DEST_PATH_IMAGE028
is a tiny normal number;
Figure 406435DEST_PATH_IMAGE019
so that
Figure 124992DEST_PATH_IMAGE020
；
Figure 703741DEST_PATH_IMAGE043
A speed control gain parameter in positive form;
Figure 216762DEST_PATH_IMAGE049
the control rate can effectively stabilize position and speed following errors and obtain the index stability of the system;
Figure 628764DEST_PATH_IMAGE050
is the velocity following error.
The output position and the output speed of each joint motor of the mechanical arm are limited by following the error, and the method comprises the following steps:
Figure 947750DEST_PATH_IMAGE069
in the formula (I), the compound is shown in the specification,
Figure 256371DEST_PATH_IMAGE070
representation matrix
Figure 64927DEST_PATH_IMAGE071
Is determined by the maximum characteristic value of the image,
Figure 91789DEST_PATH_IMAGE072
expressed as the Lyapull Ruff function
Figure 496357DEST_PATH_IMAGE073
Value at initial time t = 0.
The operation of the mechanical arm servo system modelThe controller sets the position track of each joint of the mechanical arm in the second step
Figure 784119DEST_PATH_IMAGE002
And velocity trajectory
Figure 248729DEST_PATH_IMAGE003
(ii) a Reading the actual position of each joint of the mechanical arm by utilizing an encoder
Figure 762887DEST_PATH_IMAGE051
Calculating position tracking error
Figure 423676DEST_PATH_IMAGE024
Using position tracking errors
Figure 565944DEST_PATH_IMAGE024
For velocity locus
Figure 857248DEST_PATH_IMAGE003
Correcting, receiving the feedback of speed signal, and outputting speed following error
Figure 468489DEST_PATH_IMAGE025
(ii) a Feedback derived position tracking error
Figure 932969DEST_PATH_IMAGE024
And speed following error
Figure 805110DEST_PATH_IMAGE025
Form a control output
Figure 657528DEST_PATH_IMAGE052
Speed feedback output
Figure 615120DEST_PATH_IMAGE053
Location of the joint
Figure 617711DEST_PATH_IMAGE010
And speed status
Figure 488234DEST_PATH_IMAGE011
And the speed output of the motion controller forms a model compensation control output
Figure 121340DEST_PATH_IMAGE054
Finally, use
Figure 690862DEST_PATH_IMAGE055
Get control input
Figure 497144DEST_PATH_IMAGE056
And obtaining a motion instruction for controlling the motor to move. The motion instruction can directly execute the control of original state constraint, namely, each joint of the mechanical arm can follow the given set position track with asymptotic stable error
Figure 609456DEST_PATH_IMAGE002
And setting the speed
Figure 288830DEST_PATH_IMAGE003
And meanwhile, all signals in a control closed loop are guaranteed to be bounded, namely all joint states cannot exceed a limit boundary.
Specifically, in the speed mode, the motion command may be issued by the operation lever, and the steps of each control cycle of issuing the motion command by the operation lever are as follows:
(1) Acquiring the motion direction of a six-connecting-rod of a first motor to a sixth motor; the degrees of freedom in the operating rod correspond to the coordinate directions one by one, the encoder offset of the degrees of freedom in the operating rod at the moment is obtained and compared, and the direction corresponding to the maximum offset is the motion coordinate direction of the six-connecting-rod to be controlled next;
(2) Setting the gear of the movement speed of the six-connecting-rod, and judging whether the operating rod is mistakenly touched; three sections are divided according to the numerical range of the encoder in the operating rod, namely a stop gear [0, low ], a low gear [ low, high ] and a high gear [ high, max ]. Respectively comparing the offset of the acquired numerical value of the encoder of the operating rod with low and high, and if the numerical value is in a range of stopping a gear, not performing motion control on the mechanical arm on the current operation, wherein the gear is used for avoiding motion control errors caused by mistaken touch of the operating rod; if the speed is in the low-speed gear range, setting the linear speed of the next end effector to be low speed vlow; if the speed is within the high-speed gear range, setting the moving linear speed of the next end effector as a high speed vhigh; during the control process of the operating rod, the function loop continuously carries out speed filing comparison and judgment on the offset of the encoder. Every time a comparison judgment is carried out, counting is carried out on the judgment, and the next motion control is allowed to be carried out only when the counting value reaches a certain set value;
(3) According to linear velocity of end effector
Figure 221014DEST_PATH_IMAGE120
Calculating the angular velocity of the movement of the end effector
Figure 893304DEST_PATH_IMAGE121
The concrete form is as follows:
Figure 656861DEST_PATH_IMAGE122
wherein r is a position vector of the end effector;
calculating the movement speed of each motor of the six-connecting rod in the following manner:
Figure 631770DEST_PATH_IMAGE123
in the formula (I), the compound is shown in the specification,
Figure 926616DEST_PATH_IMAGE124
respectively the linear speeds of the end effector moving in the directions of x, y and z axes;
Figure 277963DEST_PATH_IMAGE125
the angular velocities of motion in the xy, xz, yz axis planes, respectively;
Figure 896026DEST_PATH_IMAGE126
the speed tracks from No. 1 motor to No. 6 motor are respectively, and the value is converted into the encoder value of the movement speed of each motor.
Step four: sending the set motion instruction to the mechanical arm, and enabling the mechanical arm to respond and move; in this embodiment, after the mechanical arm is judged to meet the set motion condition after meeting the motion condition, the mechanical arm is made to move according to the motion instruction; the motion conditions are as follows: the first condition is as follows: defining a normal vector for the joint velocity limit boundary
Figure 166471DEST_PATH_IMAGE146
So that
Figure 73247DEST_PATH_IMAGE148
. Correcting the speeds of the motors in the third step
Figure 838072DEST_PATH_IMAGE149
With a set speed threshold
Figure 576220DEST_PATH_IMAGE150
Comparing, and moving when the speed is less than a set speed threshold;
and (2) carrying out a second condition: calculating the position of each joint
Figure DEST_PATH_IMAGE151
Reach the boundary position
Figure DEST_PATH_IMAGE152
Time required
Figure 486408DEST_PATH_IMAGE138
Motion may be enabled when time is greater than zero.
The benefits of setting this motion condition are two: firstly, frequent position reading communication is replaced by rapid calculation, and control delay is reduced; secondly, the movement of the mechanical arm is ensured not to exceed the set safety range on the basis of the first point.
Step five: and setting a mechanical arm movement stopping condition, judging the condition in the mechanical arm movement process, and stopping the movement of the mechanical arm after the condition is met. The stopping conditions of the robot arm are specifically as follows:
starting timing after the motion control instruction is sent, and stopping motion when the motion time is more than or equal to the time of moving to the boundary position;
or the offset of the encoder in the operating rod is in the stop gear range, the operating rod is considered to be released, and the motion is stopped.
The benefits of setting this motion stop condition are two: firstly, the mechanical arm is effectively prevented from exceeding a set safety range; and secondly, quick stop motion is realized in remote control, the mechanical arm cannot shake due to slight mistaken touch of a hand, and the stability of operation is improved.
Compared with embodiment 1, the mechanical arm of the present embodiment adopts a speed mode to move, and the speed mode is that an operator manually sends a motion instruction to the mechanical arm in a set motion instruction range so as to make the mechanical arm move, so that a flexible operation mode is provided for a user.
Example 3
An embodiment 3 of a robot arm control method is based on embodiments 1 and 2, and combines embodiments 1 and 1, and the robot arm control method can adopt a position mode or can be switched to a speed mode.
Compared with embodiment 1 or embodiment 2, this embodiment has two motion modes, and a user can select a corresponding motion mode to control the robot arm according to actual conditions, so that the actual use is more flexible.
Example 4
An embodiment of a robotic arm control system includes a robotic arm and an electronic device electrically connected to the robotic arm; the electronic device comprises a memory for storing a computer program and a processor for executing the computer program to implement the robot arm control method of any of the above embodiments.
Example 5
A computer-readable storage medium for storing a computer program; wherein the computer program when executed by the processor implements the robot arm control method of any of the above embodiments.
It should be understood that the above-described embodiments of the present invention are merely examples for clearly illustrating the present invention, and are not intended to limit the embodiments of the present invention. Other variations and modifications will be apparent to persons skilled in the art in light of the above description. And are neither required nor exhaustive of all embodiments. Any modification, equivalent replacement, and improvement made within the spirit and principle of the present invention should be included in the protection scope of the claims of the present invention.

Claims (10)

1. A mechanical arm control method is characterized by comprising the following steps:
the method comprises the following steps: acquiring the current state of each motor on the mechanical arm;
step two: setting the motion track of the mechanical arm, and setting the position track of each joint of the mechanical arm
Figure 985370DEST_PATH_IMAGE001
And velocity trajectory
Figure 97551DEST_PATH_IMAGE002
；
Step three: setting a motion instruction of the mechanical arm according to the motion track; according to the position track
Figure 321859DEST_PATH_IMAGE001
And velocity trajectory
Figure 648935DEST_PATH_IMAGE002
Reading the actual position of each joint of the mechanical arm by using an encoder
Figure 870969DEST_PATH_IMAGE003
Calculating the bitError of position tracking
Figure 283496DEST_PATH_IMAGE004
Using position tracking errors
Figure 45916DEST_PATH_IMAGE004
For velocity trace
Figure 417379DEST_PATH_IMAGE002
Correcting, receiving feedback of speed signal, and outputting speed following error
Figure 872631DEST_PATH_IMAGE005
(ii) a Feedback derived position tracking error
Figure 772454DEST_PATH_IMAGE004
And speed following error
Figure 276248DEST_PATH_IMAGE005
Form a control output
Figure 312337DEST_PATH_IMAGE006
Speed feedback output
Figure 125441DEST_PATH_IMAGE007
Location of the joint
Figure 512560DEST_PATH_IMAGE008
And speed state
Figure 616782DEST_PATH_IMAGE009
And the speed output of the motion controller forms a model compensation control output
Figure 710640DEST_PATH_IMAGE010
Finally, use in
Figure 507695DEST_PATH_IMAGE011
Get control input
Figure 116530DEST_PATH_IMAGE012
And obtaining a motion command for controlling the motor to move, wherein the specific representation form is as follows:
Figure 476974DEST_PATH_IMAGE013
Figure 487655DEST_PATH_IMAGE014
in the formula (I), the compound is shown in the specification,
Figure 455611DEST_PATH_IMAGE015
is a tiny normal number;
Figure 489426DEST_PATH_IMAGE016
so that
Figure 935451DEST_PATH_IMAGE017
A speed control gain parameter in the form of a positive number;
Figure 66218DEST_PATH_IMAGE018
Figure 392026DEST_PATH_IMAGE019
the method is a set of positive control gain parameters, and can effectively stabilize position and speed following errors and obtain the index stability of the system;
Figure 975454DEST_PATH_IMAGE020
is the transposed matrix of the velocity following error;
the output position and the output speed of each joint motor of the mechanical arm are limited by following the error, and the method comprises the following steps:
Figure 162853DEST_PATH_IMAGE021
in the formula (I), the compound is shown in the specification,
Figure 148126DEST_PATH_IMAGE022
representation matrix
Figure 457885DEST_PATH_IMAGE023
Is determined by the maximum characteristic value of (a),
Figure 548568DEST_PATH_IMAGE024
expressed as the Lyapull Ruff function
Figure 601974DEST_PATH_IMAGE025
Value at initial time t = 0;
step four: sending the set motion instruction to the mechanical arm, and enabling the mechanical arm to respond and move;
step five: and setting mechanical arm movement stopping conditions, judging the conditions in the mechanical arm movement process, and stopping the mechanical arm movement after the conditions are met.
2. The method according to claim 1, wherein in said second step, the robot arm moves in a position mode, and the locus of the position of each joint is determined
Figure 441754DEST_PATH_IMAGE001
The acquisition process comprises the following steps:
s1.1: acquiring coordinate information of a current end effector;
s1.2: and setting the motion trail of the tail end, and reversely pushing the motion trail of each joint motor.
3. The method for controlling the mechanical arm according to claim 2, wherein the specific process of S1.1 is as follows:
s1.1.1, recording encoder information of motors from 1 to 6 of the current mechanical arm:
Figure 860097DEST_PATH_IMAGE026
the encoder values of the No. S1.1.2:1 to No. 6 motors are converted into the current motion positions of the motors:
Figure 152538DEST_PATH_IMAGE027
in the formula (I), the compound is shown in the specification,
Figure 196587DEST_PATH_IMAGE028
the displacement of the No. 1 motor on the z axis;
Figure 890873DEST_PATH_IMAGE029
the rotation angles of No. 2, no. 3, no. 4, no. 5 and No. 6 motors respectively;
the method for calculating the numerical value rotation angle of the encoder comprises the following steps:
Figure 542434DEST_PATH_IMAGE030
s1.1.3, calculating a position matrix of a current coordinate system by using positive kinematics, wherein the specific form is as follows:
Figure 525434DEST_PATH_IMAGE031
in the formula (I), the compound is shown in the specification,
Figure 920643DEST_PATH_IMAGE028
the displacement of the No. 1 motor moving on the z axis;
Figure 656387DEST_PATH_IMAGE029
the rotation angles of No. 2, no. 3, no. 4, no. 5 and No. 6 motors respectively;
Figure 744429DEST_PATH_IMAGE032
the distance between the No. 2 motor and the No. 3 motor, the distance between the No. 3 motor and the No. 4 motor and the distance between the No. 4 motor and the No. 5 motor are respectively;
setting point
Figure 11462DEST_PATH_IMAGE033
Motor positions of No. 1, 2, 3, 4, 5, 6 and 7 respectively
Figure 882466DEST_PATH_IMAGE034
Position of end RCM point, passing point
Figure 551345DEST_PATH_IMAGE035
Making the perpendicular line of the mechanical arm plane and intersecting at the point
Figure 997238DEST_PATH_IMAGE036
(ii) a Connection point
Figure 485988DEST_PATH_IMAGE037
And with
Figure 488580DEST_PATH_IMAGE038
Two points, passing points
Figure 949648DEST_PATH_IMAGE039
Make a connection
Figure 113913DEST_PATH_IMAGE040
Perpendicular lines of (A) are intersected at the point
Figure 545419DEST_PATH_IMAGE041
，
Figure 86122DEST_PATH_IMAGE042
Is a point
Figure 401696DEST_PATH_IMAGE043
And point
Figure 2442DEST_PATH_IMAGE044
The distance of (a); passing point
Figure 465784DEST_PATH_IMAGE045
Make a connection
Figure 997129DEST_PATH_IMAGE046
Perpendicular lines of (A) are intersected at the point
Figure 495106DEST_PATH_IMAGE047
，
Figure 938857DEST_PATH_IMAGE048
Is a point
Figure 155075DEST_PATH_IMAGE049
And
Figure 37580DEST_PATH_IMAGE050
the distance of (a);
Figure 577015DEST_PATH_IMAGE051
is a point
Figure 253984DEST_PATH_IMAGE052
And
Figure 691918DEST_PATH_IMAGE053
the distance of (d);
Figure 315798DEST_PATH_IMAGE054
is the length of the tip;
Figure 522788DEST_PATH_IMAGE055
the lengths of No. 2, no. 3, no. 4 and No. 5 motors on the z axis are respectively.
4. The method for controlling a robot arm according to claim 3, wherein the specific process of S1.2 is as follows:
let the end-next motion cartesian coordinate system position be:
Figure 823188DEST_PATH_IMAGE056
firstly, the motion angles of No. 5 and No. 6 motors under the set terminal coordinates can be obtained through the position coordinates:
Figure 748419DEST_PATH_IMAGE057
it is known that
Figure 972727DEST_PATH_IMAGE058
、
Figure 237486DEST_PATH_IMAGE059
Then can calculate out
Figure 521837DEST_PATH_IMAGE060
The calculation method is as follows:
Figure 852805DEST_PATH_IMAGE061
definition of
Figure 880804DEST_PATH_IMAGE062
The specific form of (A) is as follows:
Figure 62387DEST_PATH_IMAGE063
Figure 455322DEST_PATH_IMAGE064
the motion angles of the motors No. 2, no. 3 and No. 4 under the set terminal coordinates are obtained as follows:
Figure 355145DEST_PATH_IMAGE065
Figure 921256DEST_PATH_IMAGE066
Figure 144295DEST_PATH_IMAGE067
the position of each joint motor is calculated by integrating the motion angles of No. 1 to No. 6
Figure 770449DEST_PATH_IMAGE068
。
5. The robot control method according to claim 4, wherein in the case where the robot arm moves in the position mode, in the fourth step, the robot arm is moved in accordance with the movement instruction after the set movement condition is satisfied; the motion conditions are as follows: defining normal vector for limiting boundary of joint state
Figure 829672DEST_PATH_IMAGE069
Satisfies the conditions
Figure 199473DEST_PATH_IMAGE070
The mechanical arm can move.
6. The robot arm control method according to any one of claims 1 to 5, wherein in the second step, the robot arm is moved in a velocity mode, the set movement command is performed by the operation lever, and the step of performing each control cycle of the movement command by the operation lever is as follows:
(1) Acquiring the motion direction of a six-connecting-rod of a first motor to a six motor; the degrees of freedom in the operating rod correspond to the coordinate directions one by one, the encoder offset of the degrees of freedom in the operating rod at the moment is obtained and compared, and the direction corresponding to the maximum offset is the motion coordinate direction of the six-connecting-rod to be controlled next;
(2) Setting the gear of the movement speed of the six connecting rods, and judging the mistaken touch prevention of the operating rod; dividing three intervals into stop gears [0, low ], low-speed gears [ low, high ] and high-speed gears [ high, max ] according to the numerical range of the encoder in the operating rod; respectively comparing the offset of the obtained numerical value of the encoder of the operating rod with low and high, and if the offset is in a range of a stop gear, not performing motion control on the mechanical arm in the current operation, wherein the gear is used for avoiding motion control errors caused by mistaken touch of the operating rod; if the speed is in the low-speed gear range, setting the linear speed of the next end effector to be low speed vlow; if the speed is in the high-speed gear interval, setting the motion linear speed of the next end effector to be high-speed vhigh; in the control process of the operating rod, the offset of the encoder is continuously compared and judged by speed filing in the function cycle; every time comparison and judgment is carried out, counting is carried out on the judgment, and the next motion control is allowed to be carried out only when the counting value reaches a certain set value;
(3) According to linear velocity of end effector
Figure 355648DEST_PATH_IMAGE071
Calculating the angular velocity of the movement of the end effector
Figure 339653DEST_PATH_IMAGE072
The concrete form is as follows:
Figure 948489DEST_PATH_IMAGE073
wherein r is the position vector of the end effector;
calculating the movement speed of each motor of the six-connecting rod in the following manner:
Figure 856402DEST_PATH_IMAGE074
in the formula (I), the compound is shown in the specification,
Figure 804767DEST_PATH_IMAGE075
the linear velocities of the end effector moving in the directions of the x axis, the y axis and the z axis respectively;
Figure 772723DEST_PATH_IMAGE076
the angular velocities of motion in the xy, xz and yz axis planes respectively;
Figure 55806DEST_PATH_IMAGE077
the speed tracks from No. 1 motor to No. 6 motor are respectively, and the value is converted into the encoder value of the movement speed of each motor.
7. The method according to claim 6, wherein in the third step, in a case where the robot arm moves in the speed mode, the robot arm is moved in accordance with the movement instruction after a predetermined movement condition is satisfied; the exercise conditions were specifically as follows:
the first condition is as follows: defining a normal vector for the joint velocity limit boundary
Figure 767410DEST_PATH_IMAGE078
So that
Figure 898177DEST_PATH_IMAGE079
(ii) a Correcting the speeds of the motors in the third step
Figure 974717DEST_PATH_IMAGE080
And a set speed threshold
Figure 482446DEST_PATH_IMAGE081
To carry outComparing, and performing movement when the speed is less than a set speed threshold;
and (2) carrying out a second condition: calculating the position of each joint
Figure 935424DEST_PATH_IMAGE082
Reach the boundary position
Figure 920698DEST_PATH_IMAGE083
Time required
Figure 417407DEST_PATH_IMAGE084
Motion may be enabled when time is greater than zero.
8. The robot arm control method according to claim 6, wherein the motion stop conditions are as follows:
starting timing after the motion control command is sent, and stopping motion when the motion time is more than or equal to the time for moving to the boundary position;
or the offset of the encoder in the operating rod is in the stop gear range, the operating rod is considered to be released, and the motion is stopped.
9. The mechanical arm control system is characterized by comprising a mechanical arm and electronic equipment, wherein the electronic equipment is electrically connected with the mechanical arm; the electronic device comprises a memory for storing a computer program and a processor for executing the computer program to implement the robot arm control method according to any of claims 1-8.
10. A computer-readable storage medium for storing a computer program; wherein the computer program, when executed by the processor, implements the robot arm control method of any of claims 1-8.
CN202211300005.7A 2022-10-24 2022-10-24 Mechanical arm control method and control system thereof and computer readable storage medium Active CN115366115B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211300005.7A CN115366115B (en) 2022-10-24 2022-10-24 Mechanical arm control method and control system thereof and computer readable storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211300005.7A CN115366115B (en) 2022-10-24 2022-10-24 Mechanical arm control method and control system thereof and computer readable storage medium

Publications (2)

Publication Number Publication Date
CN115366115A CN115366115A (en) 2022-11-22
CN115366115B true CN115366115B (en) 2023-01-13

Family

ID=84074395

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211300005.7A Active CN115366115B (en) 2022-10-24 2022-10-24 Mechanical arm control method and control system thereof and computer readable storage medium

Country Status (1)

Country Link
CN (1) CN115366115B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117549316B (en) * 2024-01-12 2024-03-12 唐山先河智能机器有限公司 Multi-axis mechanical arm control method and master-slave control system

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111152225A (en) * 2020-01-15 2020-05-15 北京科技大学 Uncertain mechanical arm fixed time trajectory tracking control method with input saturation
CN114310914A (en) * 2022-02-15 2022-04-12 南开大学 Fuzzy self-adaptive iterative trajectory tracking control method and system for multi-degree-of-freedom mechanical arm
CN114986498A (en) * 2022-05-18 2022-09-02 佛山汇真科技有限公司 Mobile operation arm cooperative control method
CN115202216A (en) * 2022-09-14 2022-10-18 沈阳工业大学 Anti-interference finite time control method of mechanical arm considering input constraint

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6347595B2 (en) * 2013-11-25 2018-06-27 キヤノン株式会社 Robot control method and robot control apparatus

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111152225A (en) * 2020-01-15 2020-05-15 北京科技大学 Uncertain mechanical arm fixed time trajectory tracking control method with input saturation
CN114310914A (en) * 2022-02-15 2022-04-12 南开大学 Fuzzy self-adaptive iterative trajectory tracking control method and system for multi-degree-of-freedom mechanical arm
CN114986498A (en) * 2022-05-18 2022-09-02 佛山汇真科技有限公司 Mobile operation arm cooperative control method
CN115202216A (en) * 2022-09-14 2022-10-18 沈阳工业大学 Anti-interference finite time control method of mechanical arm considering input constraint

Also Published As

Publication number Publication date
CN115366115A (en) 2022-11-22

Similar Documents

Publication Publication Date Title
CN109159151B (en) Mechanical arm space trajectory tracking dynamic compensation method and system
US9764462B2 (en) Robot apparatus and robot controlling method
EP0086950B1 (en) Method of controlling an industrial robot
US8600554B2 (en) System and method for robot trajectory generation with continuous accelerations
CN110900612B (en) Pose-synchronous six-axis industrial robot track smoothing method
CN113601512B (en) General avoidance method and system for singular points of mechanical arm
CN108908347B (en) Fault-tolerant repetitive motion planning method for redundant mobile mechanical arm
CN108189034B (en) Method for realizing continuous track of robot
CN115366115B (en) Mechanical arm control method and control system thereof and computer readable storage medium
CN111775145B (en) Control system of serial-parallel robot
US20180250816A1 (en) Control system, setting device, setting method, and storage device
JPWO2002066210A1 (en) Robot controller
CN111515928B (en) Mechanical arm motion control system
CN115723137A (en) Compliant task control method based on plane constant force
CN112405531B (en) Position domain impedance control method and device, storage medium and operation robot
CN114131617A (en) Intelligent compliance control method and device for industrial robot
CN116494250A (en) Mechanical arm control method, controller, medium and system based on speed compensation
JP2020171989A (en) Robot teaching system
CN114260896B (en) Flexible force control method and system for cooperative robot
CN110039511B (en) 8-axis linkage robot and control system and control method thereof
CN115808904A (en) Industrial robot arc auxiliary point passing track planning method
CN116276995B (en) Impedance control-based man-machine cooperation method with zero-space obstacle avoidance capability
CN117506937B (en) Weldment autonomous placement method based on multi-stage visual servo control
CN111687822B (en) Double-robot guiding track multi-space self-adaptive interpolation method
WO2020145403A1 (en) Control device

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
PE01 Entry into force of the registration of the contract for pledge of patent right
PE01 Entry into force of the registration of the contract for pledge of patent right

Denomination of invention: A robotic arm control method and its control system and computer-readable storage medium

Effective date of registration: 20231011

Granted publication date: 20230113

Pledgee: Bank of China Co.,Ltd. Nansha Branch of Guangdong Free Trade Pilot Area

Pledgor: Guangzhou Weimou Medical Instrument Co.,Ltd.

Registration number: Y2023980060743