CN114800523A - Mechanical arm track correction method, system, computer and readable storage medium - Google Patents
Mechanical arm track correction method, system, computer and readable storage medium Download PDFInfo
- Publication number
- CN114800523A CN114800523A CN202210579112.1A CN202210579112A CN114800523A CN 114800523 A CN114800523 A CN 114800523A CN 202210579112 A CN202210579112 A CN 202210579112A CN 114800523 A CN114800523 A CN 114800523A
- Authority
- CN
- China
- Prior art keywords
- actuator
- path
- theoretical
- mechanical arm
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 59
- 238000003860 storage Methods 0.000 title claims abstract description 9
- 230000033001 locomotion Effects 0.000 claims abstract description 78
- 230000001133 acceleration Effects 0.000 claims abstract description 62
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 42
- 239000011159 matrix material Substances 0.000 claims description 14
- 230000006870 function Effects 0.000 claims description 11
- 238000004590 computer program Methods 0.000 claims description 10
- 238000004088 simulation Methods 0.000 claims description 7
- 230000017105 transposition Effects 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 6
- 230000008569 process Effects 0.000 abstract description 12
- 230000005540 biological transmission Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 239000000463 material Substances 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000009776 industrial production Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Abstract
The invention provides a method, a system, a computer and a readable storage medium for correcting a mechanical arm track, wherein the method comprises the steps of simulating a theoretical motion path to be passed by an actuator in advance through a path planning algorithm; respectively calculating theoretical motion tracks of all joints between every two path points, and acquiring theoretical linear acceleration and theoretical rotation angles generated by the actuator in the theoretical motion tracks; enabling the servo controller to control the actuator to move according to each path point in sequence; respectively acquiring the actual linear acceleration and the actual rotation angle generated by the actuator passing through each path point through an inertial sensor, and calculating an error value between an actual value and a theoretical value; and then, replanning the execution path of the actuator according to the error value so that the actuator moves according to the execution path. By the method, the error value of the mechanical arm can be acquired in real time in the motion process of the mechanical arm so as to correct the execution track of the mechanical arm in real time, and the method has a wide development prospect.
Description
Technical Field
The invention relates to the technical field of mechanical arms, in particular to a method and a system for correcting a track of a mechanical arm, a computer and a readable storage medium.
Background
With the advancement of technology and the rapid development of productivity, industrial robots are widely used in current industrial production. The control system of the existing mechanical arm is a nonlinear open-loop control system, and is influenced by parameter uncertainty and nonlinear interference in the actual control process of the mechanical arm, so that the track precision of the mechanical arm is reduced. In the prior art, in order to improve the track precision of the mechanical arm, on one hand, the influence of model uncertainty in a control system on the control precision is weakened by using the self-learning capability of a multilayer neural network and a robust sliding mode technology; on the other hand, the mechanical arm model is calibrated by fusing a sensor algorithm so as to improve the parameter precision of the mechanical arm model and improve the control precision of the mechanical arm.
However, in the above methods, parameters of the mechanical arm model need to be modified before the mechanical arm moves, and the mechanical arm model needs to be calibrated again after each modification, so that the service time of the mechanical arm is greatly shortened, and the working efficiency of the mechanical arm is reduced.
Disclosure of Invention
Based on this, the invention aims to provide a method, a system, a computer and a readable storage medium for correcting the track of a mechanical arm, so as to reduce the time required by the mechanical arm to improve the track precision when the mechanical arm runs under variable load.
The embodiment of the invention provides a method for correcting a track of a mechanical arm, which is applied to the mechanical arm, wherein the mechanical arm comprises a base and an actuator connected with the base, a plurality of joints and a servo controller are arranged between the base and the actuator, an inertial sensor is arranged in the actuator, and the method comprises the following steps:
simulating a theoretical motion path through which the actuator passes in advance by using a path planning algorithm, wherein the theoretical motion path comprises a plurality of path points;
respectively calculating theoretical motion tracks of all joints between every two path points through a preset function, and acquiring theoretical linear acceleration and theoretical rotation angles generated by the actuator in the theoretical motion tracks;
the theoretical movement path is sent to the servo controller, so that the servo controller controls the actuator to move according to each path point in sequence;
respectively acquiring an actual linear acceleration and an actual rotation angle generated by the actuator passing through each path point through the inertial sensor, and calculating error values between the actual linear acceleration and the actual rotation angle and between the theoretical linear acceleration and the theoretical rotation angle;
and rescheduling an execution path of the actuator according to the error value through the path planning algorithm, and sending the execution path to the servo controller so that the servo controller controls the actuator to move according to the execution path.
The invention has the beneficial effects that: simulating a theoretical motion path through which the actuator passes in advance through a path planning algorithm; respectively calculating theoretical motion tracks of joints between every two path points through a preset function, and acquiring theoretical linear acceleration and theoretical rotation angles generated by the actuator in the theoretical motion tracks; further, the theoretical movement path is issued to the servo controller, so that the servo controller controls the actuator to move according to each path point in sequence; then respectively acquiring the actual linear acceleration and the actual rotation angle generated by the actuator passing through each path point through an inertial sensor, and calculating error values between the actual linear acceleration and the actual rotation angle and between the theoretical linear acceleration and the theoretical rotation angle; and finally, replanning the execution path of the actuator according to the error value by the path planning algorithm, and issuing the execution path to the servo controller so that the servo controller controls the actuator to move according to the execution path. By means of the method, the error value of the current mechanical arm can be acquired in real time in the motion process of the mechanical arm, so that the execution track of the current mechanical arm can be corrected in real time, meanwhile, the track error of the current mechanical arm in operation can be solved in real time through the inertial sensor, the track precision of the mechanical arm is improved, the cost of the inertial sensor is low, the precision of the inertial sensor is high, the track correction result of the mechanical arm based on the inertial sensor in the operation process can be more and more accurate, the wide development prospect is achieved, and the method and the device are suitable for large-scale popularization and use.
Preferably, the step of simulating in advance a theoretical movement path to be traversed by the actuator through a path planning algorithm includes:
constructing a space coordinate system based on the base, and calculating a position variable and an attitude variable of the actuator in the space coordinate system, wherein the position variable is expressed by Cartesian coordinates: [ x, y, z ], the attitude variable being represented by an RPY angle: [ α, β, γ ];
respectively assuming a plurality of path points according to the position variable and the attitude variable based on the path planning algorithm, wherein the ith path point is represented as: p i ＝[x i y i z i α i β i γ i ] T ；
Where Pi denotes a column vector and T denotes transposition.
Preferably, the step of respectively acquiring, by the inertial sensor, an actual linear acceleration and an actual rotation angle generated by the actuator passing through each of the waypoints includes:
acquiring a rotation angle of the actuator relative to the base in the space coordinate system through the inertial sensor, and calculating a rotation matrix R according to the rotation angle so as to acquire an actual rotation angle of the actuator according to the rotation matrix R;
acquiring translational acceleration of the actuator relative to an x-axis, a y-axis and a z-axis in the space coordinate system through the inertial sensor
And according to said translational acceleration
And acquiring the actual linear acceleration of the actuator.
Preferably, the inertial sensor includes a magnetometer, two gyroscopes and an accelerometer, and the magnetometer, the two gyroscopes and the accelerometer are respectively and fixedly mounted on the surface of the actuator.
Preferably, after the step of rescheduling the execution path of the actuator according to the error value by the path planning algorithm and sending the execution path to the servo controller so that the servo controller controls the actuator to move according to the execution path, the method further includes:
and when the actuator finishes moving, sending the execution path to a control platform, and calculating the coincidence rate between the execution path and the theoretical movement path through the control platform.
A second aspect of the embodiments of the present invention provides a system for correcting a trajectory of a robot arm, which is applied to a robot arm, where the robot arm includes a base and an actuator connected to the base, a plurality of joints and a servo controller are disposed between the base and the actuator, an inertial sensor is disposed in the actuator, and the system includes:
the simulation module is used for simulating a theoretical motion path to be passed by the actuator in advance through a path planning algorithm, and the theoretical motion path comprises a plurality of path points;
the first calculation module is used for calculating theoretical motion tracks of all joints between every two path points through a preset function and acquiring theoretical linear acceleration and theoretical rotation angles generated by the actuator in the theoretical motion tracks;
the first execution module is used for issuing the theoretical motion path to the servo controller so that the servo controller controls the actuator to sequentially move according to each path point;
the second calculation module is used for respectively acquiring the actual linear acceleration and the actual rotation angle generated by the actuator passing through each path point through the inertial sensor, and calculating error values between the actual linear acceleration and the actual rotation angle and between the theoretical linear acceleration and the theoretical rotation angle;
and the second execution module is used for re-planning the execution path of the actuator according to the error value through the path planning algorithm and sending the execution path to the servo controller so that the servo controller controls the actuator to move according to the execution path.
In the above system for correcting a trajectory of a mechanical arm, the simulation module is specifically configured to:
constructing a space coordinate system based on the base, and calculating a position variable and an attitude variable of the actuator in the space coordinate system, wherein the position variable is expressed by Cartesian coordinates: [ x, y, z ], the attitude variable being represented by an RPY angle: [ α, β, γ ];
respectively assuming a plurality of path points according to the position variable and the attitude variable based on the path planning algorithm, wherein the ith path point is represented as: p i ＝[x i y i z i α i β i γ i ] T ；
Where Pi denotes a column vector and T denotes transposition.
In the above system for correcting a trajectory of a mechanical arm, the second calculation module is specifically configured to:
acquiring a rotation angle of the actuator relative to the base in the space coordinate system through the inertial sensor, and calculating a rotation matrix R according to the rotation angle so as to acquire an actual rotation angle of the actuator according to the rotation matrix R;
acquiring translational acceleration of the actuator relative to an x-axis, a y-axis and a z-axis in the space coordinate system through the inertial sensor
And according to said translational acceleration
Acquire the executionThe actual linear acceleration of the machine.
In the system for correcting the track of the mechanical arm, the inertial sensor comprises a magnetometer, two gyroscopes and an accelerometer, and the magnetometer, the two gyroscopes and the accelerometer are respectively and fixedly arranged on the surface of the actuator.
In the above mechanical arm trajectory correction system, the mechanical arm trajectory correction system further includes a transmission module, and the transmission module is specifically configured to:
and when the actuator finishes moving, sending the execution path to a control platform, and calculating the coincidence rate between the execution path and the theoretical movement path through the control platform.
A third aspect of the embodiments of the present invention provides a computer, including a memory, a processor, and a computer program stored in the memory and executable on the processor, where the processor implements the mechanical arm trajectory correction method as described above when executing the computer program.
A fourth aspect of the embodiments of the present invention provides a readable storage medium, on which a computer program is stored, where the computer program is executed by a processor to implement the robot arm trajectory correction method described above.
Additional aspects and advantages of the invention will be set forth in part in the description which follows and, in part, will be obvious from the description, or may be learned by practice of the invention.
Drawings
Fig. 1 is a flowchart of a method for correcting a trajectory of a mechanical arm according to a first embodiment of the present invention;
fig. 2 is a block diagram of a robot arm trajectory correction system according to a third embodiment of the present invention.
The following detailed description will further illustrate the invention in conjunction with the above-described figures.
Detailed Description
To facilitate an understanding of the invention, the invention will now be described more fully with reference to the accompanying drawings. Several embodiments of the invention are presented in the drawings. This invention may, however, be embodied in many different forms and should not be construed as limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete.
It will be understood that when an element is referred to as being "secured to" another element, it can be directly on the other element or intervening elements may also be present. When an element is referred to as being "connected" to another element, it can be directly connected to the other element or intervening elements may also be present. The terms "vertical," "horizontal," "left," "right," and the like as used herein are for illustrative purposes only.
Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs. The terminology used in the description of the invention herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. As used herein, the term "and/or" includes any and all combinations of one or more of the associated listed items.
In the prior art, parameters of the mechanical arm model need to be modified before the mechanical arm moves, and the mechanical arm model needs to be calibrated again after modification at each time, so that the service time of the mechanical arm is greatly shortened, and the working efficiency of the mechanical arm is reduced.
Referring to fig. 1, a mechanical arm trajectory correction method according to a first embodiment of the present invention is shown, and the mechanical arm trajectory correction method according to this embodiment can obtain an error value of a current mechanical arm in real time during a motion process of the mechanical arm, so as to correct an execution trajectory of the current mechanical arm in real time, and meanwhile, a trajectory error during a current operation of the mechanical arm can be solved in real time by an inertial sensor, so that a trajectory accuracy of the mechanical arm is improved, and the inertial sensor has a low cost and a high accuracy, so that a trajectory correction result of the mechanical arm based on the inertial sensor during the operation process is more and more accurate, and the method has a wide development prospect, and is suitable for wide popularization and use.
Specifically, the method for correcting the trajectory of the mechanical arm provided by the embodiment is applied to the mechanical arm, the mechanical arm comprises a base and an actuator connected with the base, a plurality of joints and a servo controller are arranged between the base and the actuator, and an inertial sensor is arranged in the actuator, and the method specifically comprises the following steps:
step S10, a theoretical motion path to be passed by the actuator is simulated in advance through a path planning algorithm, and the theoretical motion path comprises a plurality of path points;
specifically, in this embodiment, it should be noted that, a controller is preset inside the mechanical arm provided in this embodiment, and a path planning algorithm is pre-written inside the controller, where when the mechanical arm is started, the controller inside the mechanical arm can start the path planning algorithm.
Therefore, in this step, when the robot arm is started, the controller inside the robot arm immediately activates the internally written path planning algorithm, and at the same time, the controller simulates in advance a theoretical motion path to be passed by the actuator in the robot arm through the path planning algorithm, where the theoretical motion path includes a plurality of path points.
Step S20, respectively calculating theoretical motion tracks of the joints between every two path points through a preset function, and acquiring theoretical linear acceleration and theoretical rotation angles generated by the actuator in the theoretical motion tracks;
further, in this step, it is to be noted that, a polynomial interpolation algorithm is also written in advance in the controller inside the current mechanical arm, and in a specific implementation, the controller calculates theoretical motion trajectories of joints between every two path points through the polynomial interpolation algorithm, and obtains a theoretical linear acceleration and a theoretical rotation angle generated by the actuator in the theoretical motion trajectories.
Step S30, the theoretical movement path is sent to the servo controller, so that the servo controller controls the actuator to move according to each path point in sequence;
after the theoretical linear acceleration and the theoretical rotation angle of the current mechanical arm are simulated through the steps, the simulated theoretical movement path is issued to the servo controllers among the joints through the controllers, so that the joints can be controlled by the servo controllers to move, and finally the actuator is driven to move according to the path points.
Step S40, respectively acquiring, by the inertial sensor, an actual linear acceleration and an actual rotation angle generated by the actuator passing through each of the path points, and calculating error values between the actual linear acceleration and the actual rotation angle and between the theoretical linear acceleration and the theoretical rotation angle;
further, in this step, it should be noted that, after the actuator is controlled to start actual movement through the step S30, the step activates the inertial sensor mounted on the surface of the actuator, specifically, during the movement of the actuator, the inertial sensor can respectively collect the actual linear acceleration and the actual rotation angle generated when the actuator passes through each path point, and further, the inertial sensor transmits the collected actual linear acceleration and actual rotation angle to the controller, so that the controller calculates the error value between the current actual linear acceleration and actual rotation angle and the simulated theoretical linear acceleration and theoretical rotation angle.
And step S50, re-planning the execution path of the actuator according to the error value through the path planning algorithm, and sending the execution path to the servo controller, so that the servo controller controls the actuator to move according to the execution path.
Finally, in this step, it should be noted that after the error value is obtained in the above step, the controller in the current mechanical arm restarts its internal path planning algorithm, and re-plans the execution path of the actuator according to the calculated error value through the path planning algorithm again, and issues the re-planned execution path to the servo controller, so that each servo controller controls the actuator to move according to the re-planned execution path, thereby obtaining the error value of the current mechanical arm in real time during the movement of the mechanical arm, and correcting the execution trajectory of the current mechanical arm in real time.
When the device is used, a theoretical motion path through which the actuator passes is simulated in advance through a path planning algorithm; respectively calculating theoretical motion tracks of joints between every two path points through a preset function, and acquiring theoretical linear acceleration and theoretical rotation angles generated by the actuator in the theoretical motion tracks; further, the theoretical motion path is sent to a servo controller, so that the servo controller controls an actuator to move according to each path point in sequence; then respectively acquiring the actual linear acceleration and the actual rotation angle generated by the actuator passing through each path point through an inertial sensor, and calculating error values between the actual linear acceleration and the actual rotation angle and between the theoretical linear acceleration and the theoretical rotation angle; and finally, replanning the execution path of the actuator according to the error value by the path planning algorithm, and issuing the execution path to the servo controller so that the servo controller controls the actuator to move according to the execution path. By means of the method, the error value of the current mechanical arm can be acquired in real time in the motion process of the mechanical arm, so that the execution track of the current mechanical arm can be corrected in real time, meanwhile, the track error of the current mechanical arm in operation can be solved in real time through the inertial sensor, the track precision of the mechanical arm is improved, the cost of the inertial sensor is low, the precision of the inertial sensor is high, the track correction result of the mechanical arm based on the inertial sensor in the operation process can be more and more accurate, the wide development prospect is achieved, and the method and the device are suitable for large-scale popularization and use.
It should be noted that the above implementation process is only for illustrating the applicability of the present application, but this does not represent that the robot trajectory correction method of the present application has only the above implementation flow, and on the contrary, the robot trajectory correction method of the present application can be incorporated into the feasible embodiments of the present application as long as the method can be implemented.
In summary, the method for correcting the track of the mechanical arm provided by the embodiments of the present invention can obtain the error value of the current mechanical arm in real time in the motion process of the mechanical arm to correct the execution track of the current mechanical arm in real time, and meanwhile, the track error of the current mechanical arm in operation can be solved in real time by the inertial sensor, so that the track accuracy of the mechanical arm is improved.
The second embodiment of the present invention also provides a method for correcting a trajectory of a mechanical arm, where the method for correcting a trajectory of a mechanical arm provided in this embodiment specifically includes the following steps:
step S11, constructing a spatial coordinate system based on the base, and calculating a position variable and an attitude variable of the actuator in the spatial coordinate system, wherein the position variable is expressed by cartesian coordinates: [ x, y, z ]]The attitude variable is expressed by an RPY angle: [ alpha, beta, gamma ]](ii) a Respectively assuming a plurality of path points according to the position variable and the attitude variable based on the path planning algorithm, wherein the ith path point is represented as: p is i ＝[x i y i z i α i β i γ i ] T (ii) a Where Pi denotes a column vector and T denotes transposition.
Specifically, in the present embodiment, it should be noted that, in the field of a robot arm technology, each point on a motion trajectory of an actuator located at an end of the robot arm in an operation space of the robot arm includes three position variables and three attitude variables.
Therefore, in this embodiment, in order to facilitate the simulation of the theoretical motion path of the current mechanical arm, the present embodiment constructs a spatial coordinate system based on the base of the current mechanical arm, and calculates the position variable and the attitude variable of the current actuator in the constructed spatial coordinate system, where the position variable is expressed by cartesian coordinates: [ x, y, z ], the attitude variable being represented by an RPY angle: [ alpha, beta, gamma ].
Further, in the above-mentioned case,in this step, the controller may respectively assume a plurality of path points according to the constructed position variables and the posture variables based on the path planning algorithm, so as to simulate a theoretical motion path along which the current mechanical arm is to move and construct corresponding path points. Wherein the ith path point is represented as: p i ＝[x i y i z i α i β i γ i ] T (ii) a Where Pi denotes a column vector and T denotes transposition.
Step S21, respectively calculating theoretical motion tracks of the joints between every two path points through a preset function, and acquiring theoretical linear acceleration and theoretical rotation angles generated by the actuator in the theoretical motion tracks;
further, in this step, it should be noted that, the controller in the current mechanical arm may respectively calculate the theoretical motion trajectory of each joint between every two path points through a preset polynomial interpolation algorithm, and specifically, in this step, according to the constraint conditions of the current mechanical arm (such as the speed, the acceleration, the interpolation time, and the like of the actuator), the polynomial interpolation algorithm is used between every two path points to ensure the stability of the motion of the mechanical arm, and the solved joint variable is sent to the servo controller.
In addition, in the process of the movement of the actuator, the theoretical linear acceleration and the theoretical rotation angle generated by the current actuator in the theoretical movement track can be acquired in real time.
Step S31, the theoretical movement path is sent to the servo controller, so that the servo controller controls the actuator to move according to each path point in sequence;
it can be understood that, in this step, after the controller acquires the theoretical motion path of the current mechanical arm through the above steps, the controller issues the current theoretical motion path to the servo controllers between the joints, so that the servo controllers control the joints to move, and at the same time, the actuator is driven to move according to the path points in the current theoretical motion path.
A step S41 of acquiring a rotation angle of the actuator with respect to the base in the spatial coordinate system by the inertial sensor, and calculating a rotation matrix R from the rotation angle to acquire an actual rotation angle of the actuator from the rotation matrix R; acquiring translational acceleration of the actuator relative to an x-axis, a y-axis and a z-axis in the space coordinate system through the inertial sensor
And according to said translational acceleration
And acquiring the actual linear acceleration of the actuator.
Further, in this embodiment, it should be noted that the inertial sensor provided in this embodiment includes a magnetometer, two gyroscopes and an accelerometer, and when the inertial sensor is installed, the magnetometer, the two gyroscopes and the accelerometer are respectively and fixedly mounted on the surface of the actuator.
Specifically, in this step, the inertial sensor collects a rotation angle of the current actuator with respect to the base in the spatial coordinate system, and calculates a rotation matrix R according to the rotation angle, so as to finally obtain an actual rotation angle of the current actuator according to the rotation matrix R.
Furthermore, the step also acquires the translational acceleration of the current actuator relative to the x-axis, the y-axis and the z-axis in the space coordinate system through the inertial sensor
And according to the obtained translational acceleration
And acquiring the actual linear acceleration of the current actuator.
Step S51 of calculating error values between the actual linear acceleration and the actual rotation angle and the theoretical linear acceleration and the theoretical rotation angle;
specifically, in this step, it is assumed that the measured value of the rotation angle of the inertial sensor at the k-th route point is [ α [ ] k β k γ k ] T The translational acceleration value of the space coordinate system relative to the base is
More specifically, taking the x-axis direction as an example, at the k-th path point, the velocity of the actuator is
Whereby the error of the actual value from the theoretical value is
In the same way, the sum of Δ y can be found
The error value of the rotation angle can be directly obtained by subtracting the actual value from the theoretical value, so that the error value delta p of the pose of the actuator is obtained.
Step S61, re-planning the execution path of the actuator according to the error value through the path planning algorithm, and sending the execution path to the servo controller, so that the servo controller controls the actuator to move according to the execution path;
further, in this step, when the (k + 1) th path point needs to be planned, this step will use a polynomial interpolation algorithm to solve for p k+1 - Δ p and p k+2 And then the joint variables are issued to the servo controller, so that the execution path of the current actuator can be re-planned according to the error value, and the execution path is issued to the servo controller, so that the servo controller controls the actuator to move according to the execution path.
In addition, in this embodiment, it should be noted that, after the step of rescheduling the execution path of the actuator according to the error value by the path planning algorithm and sending the execution path to the servo controller so that the servo controller controls the actuator to move according to the execution path, the method further includes:
and step S71, when the actuator finishes moving, sending the execution path to a control platform, and calculating the coincidence rate between the execution path and the theoretical movement path through the control platform.
Finally, in this step, it should be noted that when the movement of the actuator of the robot arm is completed, the controller inside the robot arm sends the execution path finally executed by the actuator to the control platform, and further calculates the coincidence rate between the current execution path and the theoretical movement path that has just been simulated by the control platform, so as to observe the action error of the robot arm more intuitively.
It should be noted that the method provided by the second embodiment of the present invention, which implements the same principle and produces some technical effects as the first embodiment, can be referred to the first embodiment for providing corresponding contents for the sake of brief description, where this embodiment is not mentioned.
In summary, the method for correcting the track of the mechanical arm provided by the embodiments of the present invention can obtain the error value of the current mechanical arm in real time in the motion process of the mechanical arm to correct the execution track of the current mechanical arm in real time, and meanwhile, the track error of the current mechanical arm in operation can be solved in real time by the inertial sensor, so that the track accuracy of the mechanical arm is improved.
Referring to fig. 2, a robot track correction system according to a third embodiment of the present invention is applied to a robot, the robot includes a base and an actuator connected to the base, a plurality of joints and a servo controller are disposed between the base and the actuator, an inertial sensor is disposed in the actuator, and the system includes:
the simulation module 12 is configured to simulate a theoretical motion path to be passed through by the actuator in advance through a path planning algorithm, where the theoretical motion path includes a plurality of path points;
the first calculating module 22 is configured to calculate a theoretical motion trajectory of each joint between every two path points through a preset function, and obtain a theoretical linear acceleration and a theoretical rotation angle generated by the actuator in the theoretical motion trajectory;
the first executing module 32 is configured to issue the theoretical movement path to the servo controller, so that the servo controller controls the actuator to move sequentially according to each path point;
a second calculating module 42, configured to acquire, through the inertial sensor, an actual linear acceleration and an actual rotation angle generated by the actuator passing through each of the path points, and calculate error values between the actual linear acceleration and the actual rotation angle and between the theoretical linear acceleration and the theoretical rotation angle;
the second executing module 52 is configured to reschedule, according to the error value, an executing path of the actuator according to the path planning algorithm, and send the executing path to the servo controller, so that the servo controller controls the actuator to move according to the executing path.
In the above system for correcting a trajectory of a mechanical arm, the simulation module 12 is specifically configured to:
constructing a space coordinate system based on the base, and calculating a position variable and an attitude variable of the actuator in the space coordinate system, wherein the position variable is expressed by Cartesian coordinates: [ x, y, z ], the attitude variable being represented by an RPY angle: [ α, β, γ ];
respectively assuming a plurality of path points according to the position variable and the attitude variable based on the path planning algorithm, wherein the ith path point is represented as: p i ＝[x i y i z i α i β i γ i ] T ；
Where Pi denotes a column vector and T denotes transposition.
In the above system for correcting a trajectory of a mechanical arm, the second calculating module 42 is specifically configured to:
acquiring a rotation angle of the actuator relative to the base in the space coordinate system through the inertial sensor, and calculating a rotation matrix R according to the rotation angle so as to acquire an actual rotation angle of the actuator according to the rotation matrix R;
acquiring translational acceleration of the actuator relative to an x-axis, a y-axis and a z-axis in the space coordinate system through the inertial sensor
And according to said translational acceleration
And acquiring the actual linear acceleration of the actuator.
In the system for correcting the track of the mechanical arm, the inertial sensor comprises a magnetometer, two gyroscopes and an accelerometer, and the magnetometer, the two gyroscopes and the accelerometer are respectively and fixedly arranged on the surface of the actuator.
In the above system for correcting a robot arm trajectory, the system for correcting a robot arm trajectory further includes a transmission module 62, where the transmission module 62 is specifically configured to:
and when the actuator finishes moving, sending the execution path to a control platform, and calculating the coincidence rate between the execution path and the theoretical movement path through the control platform.
A fourth embodiment of the present invention provides a computer, including a memory, a processor, and a computer program stored on the memory and executable on the processor, wherein the processor, when executing the computer program, implements the robot arm trajectory correction method as provided in the first embodiment or the second embodiment.
A fifth embodiment of the present invention provides a readable storage medium on which a computer program is stored, the program, when executed by a processor, implementing the robot arm trajectory correction method provided in the first or second embodiment described above.
In summary, the method, the system, the computer and the readable storage medium for correcting the trajectory of the mechanical arm according to the embodiments of the present invention can obtain the error value of the current mechanical arm in real time during the motion of the mechanical arm to correct the execution trajectory of the current mechanical arm in real time, and meanwhile, the inertial sensor can solve the trajectory error during the operation of the current mechanical arm in real time, so as to improve the trajectory accuracy of the mechanical arm.
The above modules may be functional modules or program modules, and may be implemented by software or hardware. For a module implemented by hardware, the modules may be located in the same processor; or the modules can be respectively positioned in different processors in any combination.
The logic and/or steps represented in the flowcharts or otherwise described herein, e.g., an ordered listing of executable instructions that can be considered to implement logical functions, can be embodied in any computer-readable medium for use by or in connection with an instruction execution system, apparatus, or device, such as a computer-based system, processor-containing system, or other system that can fetch the instructions from the instruction execution system, apparatus, or device and execute the instructions. For the purposes of this description, a "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transport the program for use by or in connection with the instruction execution system, apparatus, or device.
More specific examples (a non-exhaustive list) of the computer-readable medium would include the following: an electrical connection (electronic device) having one or more wires, a portable computer diskette (magnetic device), a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disc read-only memory (CDROM). Additionally, the computer-readable medium could even be paper or another suitable medium upon which the program is printed, as the program can be electronically captured, via for instance optical scanning of the paper or other medium, then compiled, interpreted or otherwise processed in a suitable manner if necessary, and then stored in a computer memory.
It should be understood that portions of the present invention may be implemented in hardware, software, firmware, or a combination thereof. In the above embodiments, the various steps or methods may be implemented in software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, any one or combination of the following techniques, which are known in the art, may be used: a discrete logic circuit having a logic gate circuit for implementing a logic function on a data signal, an application specific integrated circuit having an appropriate combinational logic gate circuit, a Programmable Gate Array (PGA), a Field Programmable Gate Array (FPGA), or the like.
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 do not necessarily 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.
The above-mentioned embodiments only express several embodiments of the present invention, and the description thereof is more specific and detailed, but not construed as limiting the scope of the present invention. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the inventive concept, which falls within the scope of the present invention. Therefore, the protection scope of the present patent shall be subject to the appended claims.
Claims (10)
1. A method for correcting a track of a mechanical arm is applied to the mechanical arm, the mechanical arm comprises a base and an actuator connected with the base, a plurality of joints and a servo controller are arranged between the base and the actuator, and an inertial sensor is arranged in the actuator, and the method is characterized by comprising the following steps:
simulating a theoretical motion path through which the actuator passes in advance by using a path planning algorithm, wherein the theoretical motion path comprises a plurality of path points;
respectively calculating theoretical motion tracks of all joints between every two path points through a preset function, and acquiring theoretical linear acceleration and theoretical rotation angles generated by the actuator in the theoretical motion tracks;
the theoretical movement path is sent to the servo controller, so that the servo controller controls the actuator to move according to each path point in sequence;
respectively acquiring an actual linear acceleration and an actual rotation angle generated by the actuator passing through each path point through the inertial sensor, and calculating error values between the actual linear acceleration and the actual rotation angle and between the theoretical linear acceleration and the theoretical rotation angle;
and rescheduling an execution path of the actuator according to the error value through the path planning algorithm, and sending the execution path to the servo controller so that the servo controller controls the actuator to move according to the execution path.
2. The robot arm trajectory correction method according to claim 1, characterized in that: the step of simulating a theoretical motion path to be passed by the actuator in advance through a path planning algorithm comprises the following steps:
constructing a space coordinate system based on the base, and calculating a position variable and an attitude variable of the actuator in the space coordinate system, wherein the position variable is expressed by Cartesian coordinates: [ x, y, z ], said attitude variables being represented by RPY angles: [ α, β, γ ];
respectively assuming a plurality of path points according to the position variable and the attitude variable based on the path planning algorithm, wherein the ith path point is represented as: p i ＝[x i y i z i α i β i γ i ] T ；
Where Pi denotes a column vector and T denotes transposition.
3. The robot arm trajectory correction method according to claim 2, characterized in that: the step of respectively acquiring an actual linear acceleration and an actual rotation angle generated by the actuator passing through each path point by the inertial sensor comprises:
acquiring a rotation angle of the actuator relative to the base in the space coordinate system through the inertial sensor, and calculating a rotation matrix R according to the rotation angle so as to acquire an actual rotation angle of the actuator according to the rotation matrix R;
acquiring translational acceleration of the actuator relative to an x-axis, a y-axis and a z-axis in the space coordinate system through the inertial sensor
And according to said translational acceleration
And acquiring the actual linear acceleration of the actuator.
4. The robot arm trajectory correction method according to claim 1, characterized in that: the inertial sensor comprises a magnetometer, two gyroscopes and an accelerometer, wherein the magnetometer, the two gyroscopes and the accelerometer are respectively and fixedly arranged on the surface of the actuator.
5. The robot arm trajectory correction method according to claim 1, characterized in that: after the step of rescheduling the execution path of the actuator according to the error value by the path planning algorithm and sending the execution path to the servo controller so that the servo controller controls the actuator to move according to the execution path, the method further includes:
and when the actuator finishes moving, sending the execution path to a control platform, and calculating the coincidence rate between the execution path and the theoretical movement path through the control platform.
6. The utility model provides a mechanical arm orbit correction system, is applied to in the arm, the arm include the base and with the executor that the base is connected, be equipped with a plurality of joints and servo controller between base and the executor, be equipped with inertial sensor in the executor, its characterized in that, the system includes:
the simulation module is used for simulating a theoretical motion path to be passed by the actuator in advance through a path planning algorithm, and the theoretical motion path comprises a plurality of path points;
the first calculation module is used for calculating theoretical motion tracks of all joints between every two path points through a preset function and acquiring theoretical linear acceleration and theoretical rotation angles generated by the actuator in the theoretical motion tracks;
the first execution module is used for issuing the theoretical motion path to the servo controller so that the servo controller controls the actuator to sequentially move according to each path point;
the second calculation module is used for respectively acquiring the actual linear acceleration and the actual rotation angle generated by the actuator passing through each path point through the inertial sensor, and calculating error values between the actual linear acceleration and the actual rotation angle and between the theoretical linear acceleration and the theoretical rotation angle;
and the second execution module is used for re-planning the execution path of the actuator according to the error value through the path planning algorithm and sending the execution path to the servo controller so that the servo controller controls the actuator to move according to the execution path.
7. The robot arm trajectory correction system of claim 6, wherein: the simulation module is specifically configured to:
constructing a space coordinate system based on the base, and calculating a position variable and an attitude variable of the actuator in the space coordinate system, wherein the position variable is expressed by Cartesian coordinates: [ x, y, z ], the attitude variable being represented by an RPY angle: [ α, β, γ ];
respectively assuming a plurality of path points according to the position variable and the attitude variable based on the path planning algorithm, wherein the ith path point is represented as: p i ＝[x i y i z i α i β i γ i ] T ；
Where Pi denotes a column vector and T denotes transposition.
8. The robot arm trajectory correction system of claim 7, wherein: the second calculation module is specifically configured to:
acquiring a rotation angle of the actuator relative to the base in the space coordinate system through the inertial sensor, and calculating a rotation matrix R according to the rotation angle so as to acquire an actual rotation angle of the actuator according to the rotation matrix R;
acquiring translational acceleration of the actuator relative to an x-axis, a y-axis and a z-axis in the space coordinate system through the inertial sensor
And according to said translational acceleration
And acquiring the actual linear acceleration of the actuator.
9. A computer comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the processor implements the method of robot arm trajectory correction of any one of claims 1 to 5 when executing the computer program.
10. A readable storage medium on which a computer program is stored, the program, when executed by a processor, implementing a robot arm trajectory correction method according to any one of claims 1 to 5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210579112.1A CN114800523B (en) | 2022-05-26 | 2022-05-26 | Mechanical arm track correction method, system, computer and readable storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210579112.1A CN114800523B (en) | 2022-05-26 | 2022-05-26 | Mechanical arm track correction method, system, computer and readable storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114800523A true CN114800523A (en) | 2022-07-29 |
| CN114800523B CN114800523B (en) | 2023-12-01 |
Family
ID=82517476
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210579112.1A Active CN114800523B (en) | 2022-05-26 | 2022-05-26 | Mechanical arm track correction method, system, computer and readable storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114800523B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115984388A (en) * | 2023-02-28 | 2023-04-18 | 江西省智能产业技术创新研究院 | Spatial positioning accuracy evaluation method, system, storage medium and computer |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108839025A (en) * | 2018-07-12 | 2018-11-20 | 杭州电子科技大学 | A kind of motion planning method and device of mobile mechanical arm |
| CN109159151A (en) * | 2018-10-23 | 2019-01-08 | 北京无线电测量研究所 | A kind of mechanical arm space tracking tracking dynamic compensation method and system |
| CN109483529A (en) * | 2018-10-12 | 2019-03-19 | 华南智能机器人创新研究院 | A kind of mechanical arm method of servo-controlling, system and device based on screw theory |
| US20190184560A1 (en) * | 2017-01-19 | 2019-06-20 | Beijing University Of Technology | A Trajectory Planning Method For Six Degree-of-Freedom Robots Taking Into Account of End Effector Motion Error |
| CN111730599A (en) * | 2020-07-08 | 2020-10-02 | 深圳市优必选科技股份有限公司 | Impedance control method and device, impedance controller and robot |
| CN112171673A (en) * | 2020-09-24 | 2021-01-05 | 哈尔滨工业大学（深圳） | Robot arm operation control method, control apparatus, and computer-readable storage medium |
| CN113263496A (en) * | 2021-04-01 | 2021-08-17 | 北京无线电测量研究所 | Method for optimizing path of six-degree-of-freedom mechanical arm and computer equipment |
| CN113459109A (en) * | 2021-09-03 | 2021-10-01 | 季华实验室 | Mechanical arm path planning method and device, electronic equipment and storage medium |
| CN114227685A (en) * | 2021-12-28 | 2022-03-25 | 深圳市优必选科技股份有限公司 | Mechanical arm control method and device, computer readable storage medium and mechanical arm |
| WO2022088593A1 (en) * | 2020-10-26 | 2022-05-05 | 北京理工大学 | Robotic arm control method and device, and human-machine cooperation model training method |
-
2022
- 2022-05-26 CN CN202210579112.1A patent/CN114800523B/en active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20190184560A1 (en) * | 2017-01-19 | 2019-06-20 | Beijing University Of Technology | A Trajectory Planning Method For Six Degree-of-Freedom Robots Taking Into Account of End Effector Motion Error |
| CN108839025A (en) * | 2018-07-12 | 2018-11-20 | 杭州电子科技大学 | A kind of motion planning method and device of mobile mechanical arm |
| CN109483529A (en) * | 2018-10-12 | 2019-03-19 | 华南智能机器人创新研究院 | A kind of mechanical arm method of servo-controlling, system and device based on screw theory |
| CN109159151A (en) * | 2018-10-23 | 2019-01-08 | 北京无线电测量研究所 | A kind of mechanical arm space tracking tracking dynamic compensation method and system |
| CN111730599A (en) * | 2020-07-08 | 2020-10-02 | 深圳市优必选科技股份有限公司 | Impedance control method and device, impedance controller and robot |
| CN112171673A (en) * | 2020-09-24 | 2021-01-05 | 哈尔滨工业大学（深圳） | Robot arm operation control method, control apparatus, and computer-readable storage medium |
| WO2022088593A1 (en) * | 2020-10-26 | 2022-05-05 | 北京理工大学 | Robotic arm control method and device, and human-machine cooperation model training method |
| CN113263496A (en) * | 2021-04-01 | 2021-08-17 | 北京无线电测量研究所 | Method for optimizing path of six-degree-of-freedom mechanical arm and computer equipment |
| CN113459109A (en) * | 2021-09-03 | 2021-10-01 | 季华实验室 | Mechanical arm path planning method and device, electronic equipment and storage medium |
| CN114227685A (en) * | 2021-12-28 | 2022-03-25 | 深圳市优必选科技股份有限公司 | Mechanical arm control method and device, computer readable storage medium and mechanical arm |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115984388A (en) * | 2023-02-28 | 2023-04-18 | 江西省智能产业技术创新研究院 | Spatial positioning accuracy evaluation method, system, storage medium and computer |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114800523B (en) | 2023-12-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110355751B (en) | Control device and machine learning device | |
| CN109159151A (en) | A kind of mechanical arm space tracking tracking dynamic compensation method and system | |
| CN109849025B (en) | Vibration suppressing device | |
| Balaguer et al. | A climbing autonomous robot for inspection applications in 3d complex environments | |
| CN102135776B (en) | Industrial robot control method based on visual positioning | |
| CN111347431B (en) | Robot teaching spraying method and device for teaching hand-held tool | |
| KR20190105039A (en) | Method and system for programming a robot | |
| CN109397265A (en) | A kind of joint type industrial robot dragging teaching method based on kinetic model | |
| CN108568814A (en) | The control method of robot and robot | |
| US20110010008A1 (en) | Method And Device For Controlling A Manipulator | |
| CN110815190A (en) | Industrial robot dragging demonstration method and system | |
| CN111515928B (en) | Mechanical arm motion control system | |
| CN114603558B (en) | Hybrid space transition track planning method and device | |
| CN114800523B (en) | Mechanical arm track correction method, system, computer and readable storage medium | |
| Lange et al. | Learning accurate path control of industrial robots with joint elasticity | |
| CN110053042B (en) | Robot control device | |
| CN116038697A (en) | Jeans automatic spraying method and system based on manual teaching | |
| Tang et al. | Development of a laboratory HILs testbed system for small UAV helicopters | |
| JP2703767B2 (en) | Robot teaching data creation method | |
| US20220162834A1 (en) | Method for State Estimation of Position and Orientation of a Plurality of Movable Modules of a Common System | |
| Gu et al. | Dexterous obstacle-avoidance motion control of Rope Driven Snake Manipulator based on the bionic path following | |
| Gravdahl | Force estimation in robotic manipulators: Modeling, simulation and experiments | |
| Qi et al. | A lead-through robot programming approach using a 6-DOF wire-based motion tracking device | |
| JPH07210223A (en) | Method for evaluating operating time of robot and device therefor | |
| WO2020152018A1 (en) | Robot and method for controlling a mobile robot in an area |
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 |