CN115179290B - Mechanical arm and track control method and device thereof - Google Patents
Mechanical arm and track control method and device thereof Download PDFInfo
- Publication number
- CN115179290B CN115179290B CN202210864724.5A CN202210864724A CN115179290B CN 115179290 B CN115179290 B CN 115179290B CN 202210864724 A CN202210864724 A CN 202210864724A CN 115179290 B CN115179290 B CN 115179290B
- Authority
- CN
- China
- Prior art keywords
- track
- feedforward
- mechanical arm
- information
- value
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 74
- 230000007246 mechanism Effects 0.000 claims abstract description 52
- 230000004927 fusion Effects 0.000 claims abstract description 31
- 230000006870 function Effects 0.000 claims description 10
- 239000012636 effector Substances 0.000 claims description 9
- 238000013528 artificial neural network Methods 0.000 claims description 6
- 238000004590 computer program Methods 0.000 claims description 6
- 238000002474 experimental method Methods 0.000 claims description 5
- 238000013135 deep learning Methods 0.000 claims description 4
- 238000006073 displacement reaction Methods 0.000 claims description 4
- 239000011159 matrix material Substances 0.000 claims description 4
- 238000005381 potential energy Methods 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 4
- 238000012706 support-vector machine Methods 0.000 claims description 4
- 239000006185 dispersion Substances 0.000 claims description 3
- 230000004044 response Effects 0.000 description 5
- 238000004422 calculation algorithm Methods 0.000 description 4
- 238000001914 filtration Methods 0.000 description 4
- 230000007547 defect Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000013178 mathematical model Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 230000001276 controlling effect Effects 0.000 description 2
- 230000001351 cycling effect Effects 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 238000012544 monitoring process Methods 0.000 description 2
- 238000007781 pre-processing Methods 0.000 description 2
- 238000007670 refining Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000013499 data model Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 238000005507 spraying Methods 0.000 description 1
- 238000003466 welding Methods 0.000 description 1
Abstract
The invention belongs to the technical field of track tracking of industrial mechanical arms, and discloses a mechanical arm and a track control method and a track control device thereof, wherein the method comprises the following steps: (1) Acquiring track information of the mechanical arm, converting the track information into joint space, further presetting track rough-dividing nodes, and solving through a mechanism model to obtain a feedforward moment value; meanwhile, presetting track subdivision nodes and solving through a data driving model to obtain a feedforward moment value; (2) Adopting a tracking filter to perform data fusion on the obtained two feedforward torque values so as to obtain a new feedforward torque value; (3) The track information of the mechanical arm is discretized by adopting a discretization initialization tool, and the discretized track information is aligned with the dimension of a new feedforward torque value to be used as the input of a feedforward compensation channel, so that the input is processed by a feedback controller to obtain control information, and the track of the mechanical arm is controlled. The invention improves the real-time performance of the control of the mechanical arm and the track tracking precision.
Description
Technical Field
The invention belongs to the technical field related to track tracking control of industrial mechanical arms, and particularly relates to a mechanical arm and a track control method and device thereof.
Background
When the industrial mechanical arm is used for welding and spraying, the motion track of the end effector is fixed, and high dynamic response and high precision requirements are required. Although the traditional feedback control can ensure the track accuracy, hysteresis is serious under the condition of high-speed movement, and the performance requirement of quick response cannot be met.
The feedforward control is an open loop control system for directly controlling the controlled variable, and is characterized in that after disturbance is generated, the controlled variable is controlled according to the magnitude of disturbance action before the controlled variable is not changed so as to compensate the influence of the disturbance action on the controlled variable. The feedforward control system can improve the time response characteristic, avoid fluctuation and hysteresis of reaction caused by overpositive during negative feedback adjustment, and enable adjustment control to be faster and more accurate.
How to realize the high robustness, sensitivity and precision of the mechanical arm control program under the complex working condition is a key problem of the development of high-performance motion of the mechanical arm, and the feedback control of introducing feedforward compensation has proven to be an effective solution. For a task track needing to consider both speed and precision, the high-gain feedforward compensation channel can quickly respond to the preset track, the tail end of the actuator is quickly preset to a rough position, and disturbance is further regulated by the low-gain feedback channel, so that the control precision is improved.
At present, two modes for acquiring feedforward moment values of all joints of a mechanical arm are mainly available, one mode is based on a mechanism model: according to the method, a DH coordinate system is established for the mechanical arm from the dynamic characteristics of the mechanical arm, and a feedforward moment value is solved through inverse dynamics according to task track information preset by an end effector. However, the method does not consider energy change caused by flexible deformation of the connecting rod, and friction factors are difficult to accurately measure, so that the established dynamic model is greatly different from the actual model; the second is based on a data driven model: according to the method, a plurality of experiments are carried out to obtain data sets containing track information and corresponding feedforward torque value information, and the neural network is trained through the data sets, so that the feedforward torque value is obtained. However, the data source of the method is a measured value, interference exists, generalization capability is poor, and the method has a good effect on only trained tracks.
Disclosure of Invention
Aiming at the defects or improvement demands of the prior art, the invention provides a mechanical arm and a track control method and a track control device thereof, wherein the method organically combines a traditional mechanism model with a data driving model, and performs data fusion on a feedforward moment value obtained based on the mechanism model and the data driving model from the probability angle through follow the trail of filtering algorithm, so that the obtained feedforward value inherits the advantages of the two models, and the real-time performance of mechanical arm control and track tracking precision are improved. Meanwhile, the method acquires the mathematical model of the flexible deformation of the mechanical arm by adopting a mode method, so that the accuracy of the mechanism model is further improved.
To achieve the above object, according to one aspect of the present invention, there is provided a trajectory control method of a robot arm, the method comprising the steps of:
(1) Acquiring track information of the mechanical arm, converting the track information into joint space, further presetting track rough-dividing nodes, and solving through a mechanism model to obtain a feedforward moment value; meanwhile, presetting track subdivision nodes and solving through a data driving model to obtain a feedforward moment value;
(2) Adopting a tracking filter to perform data fusion on the obtained two feedforward torque values so as to obtain a new feedforward torque value;
(3) The track information of the mechanical arm is discretized by adopting a discretization initialization tool, the discretized track information is aligned with the dimension of a new feedforward torque value to serve as the input of a feedforward compensation channel, the input is further processed by a feedback controller to obtain control information, and the control information is output to the mechanical arm to control the track of the mechanical arm.
Further, in the step (3), the obtained feedforward torque value is converted into a motor current value, the motor current value and the track information are used as input of a feedforward channel, meanwhile, the position information of the mechanical arm end effector and the current information of each joint are monitored in real time, and deviation of the obtained position information and the current information and preset information is used as input of a feedback channel.
Further, the step of obtaining the feedforward torque value through solving the mechanism model comprises the following substeps:
(1) Acquiring a vibration mode function h j (x) of the connecting rod by a hypothesis mode method;
(2) Acquiring the speed and displacement of each connecting rod of the space manipulator through a jacobian matrix;
(3) Solving kinetic energy and potential energy of the space manipulator;
(4) And establishing a flexible dynamic model of the space manipulator by a Lagrangian method:
and then solving a feedforward moment value under the corresponding task track.
Further, the feedforward moment value obtained by solving the data driving model mainly comprises the following substeps:
(1) Obtaining a data set containing track information and moment values corresponding to the track information through experiments;
(2) A support vector machine or a neural network method is selected to perform deep learning on the obtained data set to obtain parameter weight w and bias b;
(3) And constructing a function tau=f (q, q', w, b) by the trained network model, and solving a feedforward moment value under the corresponding task track.
Further, the probability distribution of the predicted value obtained by the mechanism model and the data driving model is fused, the predicted value of the mechanism model is used for providing a state initial value, follow the trail of is continuously carried out on the predicted value of the data driving model, the optimal probability distribution is obtained, and then the feedforward compensation moment value is obtained.
Further, the probability distribution obtained by the mechanism model is thatWherein the method comprises the steps ofDetermining according to the deviation between the mechanism model and the measured true value; the probability distribution obtained by the data driven model is thatWherein the method comprises the steps ofDetermining optimal weights based on sensor error decisionsK is synchronously updated along with the circulation; multiplying the obtained probabilities to obtain new probabilities, and then completing data fusion to obtain new feedforward moment values; wherein the new probability distribution is:
the invention also provides a track control device of the mechanical arm, which comprises a readable storage medium and a processor, wherein the readable storage medium stores a computer program; the processor is configured to read and execute the computer program stored in the readable storage medium, so that the processor executes the trajectory control method of the robot arm as described above.
The invention also provides a track control device of the mechanical arm, which comprises a data fusion module and a control compensation module; wherein,
The data fusion module is used for acquiring track information of the mechanical arm and converting the track information into joint space, further presetting track rough-division nodes and solving through a mechanism model to obtain a feedforward moment value; meanwhile, presetting track subdivision nodes and solving through a data driving model to obtain a feedforward moment value; a tracking filter is adopted to conduct data fusion on the obtained two feedforward torque values so as to obtain a new feedforward torque value;
The control compensation module is used for dispersing track information of the mechanical arm by adopting a discrete initialization tool, aligning the track information after the dispersion with the dimension of a new feedforward torque value from the data fusion module to be used as input of a feedforward compensation channel, processing the input through the feedback controller to obtain control information, and outputting the control information to the mechanical arm to carry out track control on the mechanical arm.
The invention provides a mechanical arm, which adopts the track control method of the mechanical arm as claimed in any one of claims 1-6 to carry out track control on the mechanical arm.
The invention provides a mechanical arm, which comprises the track control device of the mechanical arm.
In general, compared with the prior art, the mechanical arm and the track control method and device thereof have the following advantages:
1. The method adds data fusion, the obtained feedforward torque value not only keeps the applicability of the mechanism model to most tracks and ensures that the mechanism model falls in a reasonable value interval, but also filters noise interference caused by a data driving model, so that the obtained feedforward torque value is smooth and accurate.
2. The control compensation is adopted, the obtained feedforward torque value is used as the input of a compensation channel, and the real-time performance and the track tracking precision of the mechanical arm control are improved; meanwhile, the control compensation module performs feedback control from the position loop and the current loop, and control accuracy during control of the mechanical arm is further improved.
3. According to the method, the mathematical model of the flexible deformation of the mechanical arm is obtained through a hypothesis mode method, and the accuracy of the mechanism model is further improved.
Drawings
Fig. 1 is a schematic diagram of a planning structure of a trajectory control method of a mechanical arm according to the present invention;
FIG. 2 is a workflow diagram of a data fusion module of the track control device provided by the present invention;
FIG. 3 is a flowchart of the operation of the tracking filter of the trajectory control device provided by the present invention;
fig. 4 is an overall schematic diagram of a data fusion module of the track control device provided by the invention;
fig. 5 is a detailed flowchart of a track control method of a mechanical arm provided by the invention.
Detailed Description
The present invention will be described in further detail with reference to the drawings and examples, in order to make the objects, technical solutions and advantages of the present invention more apparent. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the invention. In addition, the technical features of the embodiments of the present invention described below may be combined with each other as long as they do not collide with each other.
Referring to fig. 1, the present invention provides a track control method for a mechanical arm, which introduces feedforward compensation to implement quick response of the mechanical arm in order to compensate for hysteresis defects existing in feedback control, thereby improving control performance of the industrial mechanical arm. The method introduces a data driving model for overcoming the defects of simplified errors, high calculation cost and the like of a mechanism model, organically combines the traditional mechanism model with the data driving model, performs data fusion on a feedforward moment value obtained based on the mechanism model and the data driving model from the probability angle through follow the trail of filtering algorithm, inherits the advantages of the two models and improves the real-time performance and track tracking precision of mechanical arm control. Meanwhile, the method acquires the mathematical model of the flexible deformation of the mechanical arm by adopting a mode method, so that the accuracy of the mechanism model is further improved.
The method can be used for any point-to-point or continuous track by various industrial mechanical arms to obtain a feedforward moment value with reasonable, smooth and accurate value interval, and meanwhile, track information and the feedforward value are aligned through a discrete initialization tool and then are used as the input of the mechanical arms, feedback control is carried out from a position loop and a current loop, and the real-time performance and track tracking precision of mechanical arm control are enhanced.
The track control method mainly comprises the following steps:
Step one, track information of a mechanical arm is obtained and converted into joint space, and then track rough dividing nodes are preset and a feedforward moment value is obtained through solving of a mechanism model; meanwhile, track subdivision nodes are preset, and a feedforward moment value is obtained through solving of a data driving model.
The feedforward moment value is obtained through solving a mechanism model, and the method comprises the following substeps:
(1) The vibration mode function h j (x) of the connecting rod is obtained through a hypothesis mode method.
(2) And acquiring the speed and displacement of each connecting rod of the space manipulator through the jacobian matrix.
(3) And solving the kinetic energy and potential energy of the space manipulator.
(4) And establishing a flexible dynamic model of the space manipulator by a Lagrangian method:
and then solving a feedforward moment value under the corresponding task track.
The feedforward moment value obtained by solving the data driving model mainly comprises the following substeps:
(1) And obtaining a data set containing the track information and the moment value corresponding to the track information through experiments.
(2) The method includes, but is not limited to, support vector machine and neural network method, and deep learning is carried out on the obtained data set to obtain parameter weight w and bias b.
(3) And constructing a function tau=f (q, q', w, b) by the trained network model, and solving a feedforward moment value under the corresponding task track.
And secondly, carrying out data fusion on the obtained two feedforward torque values by adopting a tracking filter so as to obtain a new feedforward torque value. The method specifically comprises the following substeps:
(1) And acquiring a task track.
(2) Discretizing the task track to obtain coarse-grained tracks of n discrete points and fine-grained tracks of n multiplied by k discrete points respectively; n is the outer cycle count, k is the inner cycle number, and n and k are set by themselves. For the mechanism model, the feedforward value solving speed is slower, but the method accords with the approximate range; for this purpose, the task trajectory is roughly divided into n nodes for providing state initial values. For a data driven model, feedforward values can be quickly solved, but the universality is not strong, so that the task track is subdivided into n×k nodes for dynamic update.
(3) Calculating a feedforward moment value of the coarse-grained trajectory based on a mechanism model, and marking the feedforward moment value as x i, i= … n; the feedforward moment value of the fine-grained trajectory is calculated based on the data-driven model and is denoted as z ij, i= … n, j= … k.
(4) Uncertainty is introduced based on the calculated probability distribution of feedforward torque values. Wherein the probability distribution obtained by the mechanism model is as followsWherein the method comprises the steps ofDetermining according to the deviation between the mechanism model and the measured true value; the probability distribution obtained by the data driven model is thatWherein the method comprises the steps ofBased on the sensor error.
(5) Determining optimal weightsK is synchronously updated along with the circulation; the concentration degree of the normal distribution depends on the variance, the smaller the variance is, the more concentrated the corresponding probability distribution is, the obtained two probabilities are multiplied, and the new probability distribution is as follows:
Determining optimal weights K is synchronously updated along with the circulation; recording the optimal weight asWhere x' =x i+K(zij-xi),The new probability distribution variance obtained after multiplication is smaller, and the accuracy of the feedforward value can be improved by cycling the process. Wherein the optimal weight K is updated synchronously with the cycle.
(6) And executing internal circulation, updating the optimal weight through follow the trail of filtering algorithm and refining the feedforward torque value.
(7) Executing the outer loop, and circularly executing the step six to obtain a series of estimated valuesAnd the feedforward torque value is transmitted to the control compensation module as a feedforward torque value.
And thirdly, dispersing the track information of the mechanical arm by adopting a dispersing initialization tool, aligning the track information after dispersing with the dimension of a new feedforward torque value to serve as the input of a feedforward compensation channel, processing the input through a feedback controller to obtain control information, and outputting the control information to the mechanical arm to carry out track control on the mechanical arm.
The obtained feedforward torque value is converted into a motor current value, the motor current value and the track information are used as input of a feedforward channel, meanwhile, the position information of an end effector of the mechanical arm and the current information of each joint are monitored in real time, and deviation of the obtained position information and the current information and preset information is used as input of a feedback channel.
The present invention also provides a trajectory control device of a robot arm, the control device including a readable storage medium storing a computer program which, when executed, performs the trajectory control method of a robot arm as described above.
The robot arm control device further includes a processor for executing the computer program stored in the readable storage medium, so that the processor executes the trajectory control method of the robot arm as described above.
The invention also provides a mechanical arm, which comprises the mechanical arm control device. Or the mechanical arm adopts the track control method of the mechanical arm to carry out track control.
In one embodiment, the track control device comprises a data fusion module and a control compensation module, wherein the input of the data fusion module is task track information; outputting as a fused feedforward value; the method has the main functions that according to task track information given by an upper computer, prediction is carried out through a mechanism model and a data driving model solver respectively, and a more accurate feedforward moment value is obtained through fusion of a tracking filter. The module has the following characteristics:
And the feedforward moment value of the corresponding track can be rapidly solved through a data driving model.
For complex preset tracks, the mechanism model can ensure that the solved moment value falls in a reasonable value interval, and then the data driving model is used for dynamic correction.
The data fusion module comprises the following functional components:
a mechanism model solver: solving a feedforward moment value according to a mechanical arm dynamics model;
data driven model solver: solving and obtaining a feedforward moment value through a neural network;
Tracking filter: and fusing probability distribution of the predicted values obtained by the mechanism model and the data driving model, providing a state initial value by the predicted values of the mechanism model, and continuously carrying out follow the trail of on the predicted values of the data driving model to obtain more concentrated probability distribution, thereby obtaining more accurate feedforward compensation moment values.
The general steps of the data fusion module are as follows:
Step one, the upper computer issues task space track information and activates a data fusion module.
And secondly, roughly dividing and subdividing the task track into a plurality of nodes, synchronously issuing the roughly divided nodes to a mechanism model, and issuing the subdivided nodes to a data driving model for solving.
And thirdly, obtaining probability distribution of the two through a tracking filter, wherein the fusion process is divided into an inner loop and an outer loop, the inner loop takes a predicted value of a mechanism model as a state initial value, the predicted value of a data driving model is continuously updated follow the trail of, the outer loop obtains a feedforward moment value corresponding to each node of a task track, and the obtained feedforward moment value is taken as input of a control compensation module.
The input of the control compensation module is track information of the upper computer; the feedforward moment value output by the data fusion module and the position and current return value monitored by the mechanical arm in real time; outputting the optimized control compensation information; the main function of the method is to align the obtained feedforward torque value with the track information, and feed back the feedforward torque value from the position loop and the current loop to generate control compensation information which can reach the task track, and the control compensation information and the task track are used as the input of the mechanical arm control. The module has the following characteristics:
1. The feedforward compensation directly controls the motor current of the mechanical arm control system, and the response speed is high.
2. The feedforward control can be used for quickly presetting the end effector to the approximate area of the task track, so that the burden of feedback control is reduced, and the deadbeat control can be realized theoretically.
The control compensation module comprises the following functional components:
Discrete initialization tool: receiving a frequency discrete track according to a mechanical arm control signal, and aligning track information with a feedforward value obtained by a data fusion module in dimension;
And a feedback controller: and comparing the position of the end effector obtained by real-time monitoring with the current information as feedback and an expected value, and inputting control compensation information to the mechanical arm through a feedback controller.
The general steps for controlling the compensation module are as follows:
step one, the discrete initialization tool performs discrete on preset task track information according to the frequency of the control signal received by the mechanical arm. The feedforward torque value is aligned with the dimensions of the joint space angle, angular velocity and angular acceleration.
And step two, converting the obtained feedforward torque value into a motor current value, and taking the motor current value and track information as input of a feedforward channel.
And thirdly, inputting the measured actual current value and the end effector track information into a feedback channel.
Referring to fig. 2,3 and 4, the module includes a preprocessing stage for establishing a mechanism model and a data driving model, and a formal working stage for receiving track information and immediately solving the result.
The specific steps of the mechanism model establishment in the pretreatment stage are as follows:
step one, obtaining a vibration mode function h j (x) of the connecting rod by a hypothesis mode method
And secondly, acquiring the speed and displacement of each connecting rod of the space manipulator through the jacobian matrix.
And thirdly, solving kinetic energy and potential energy of the space manipulator.
Step four, a flexible dynamics model of the space manipulator is established through a Lagrangian method:
And solving a feedforward moment value under the corresponding task track.
The preprocessing stage comprises the following steps of:
And step one, performing a large number of experiments to obtain a data set containing track information and moment values corresponding to the track information.
And step two, selecting a support vector machine and a neural network method to perform deep learning on the data set obtained in the step one to obtain the parameter weight w and the bias b.
And thirdly, constructing a function tau=f (q, q', w, b) by the trained network model, and solving a feedforward moment value under the corresponding task track.
The formal working stage specifically comprises the following steps:
s201: the data fusion module reads the track issued by the upper computer and converts the track into joint space.
S202: and obtaining a feedforward moment value through a mechanism model solver.
S203: and obtaining a feedforward moment value through a data driving model solver. Wherein S202 and S203 can be performed in parallel.
S204: and carrying out data fusion on the two data through a tracking filter to obtain a feedforward moment value.
The tracking filter workflow is shown in fig. 3, and the specific steps are as follows:
step one: acquiring a task track from an upper layer application;
Step two: discretizing the task track to obtain coarse-grained tracks of n discrete points and fine-grained tracks of n multiplied by k discrete points respectively; n is an outer circulation count, k is an inner circulation number, and n and k are set by themselves; for the mechanism model, the feedforward value solving speed is slower, but the method accords with the approximate range; for this purpose, the task trajectory is roughly divided into n nodes for providing state initial values. For a data driven model, feedforward values can be quickly solved, but the universality is not strong, so that the task track is subdivided into n×k nodes for dynamic update.
Step three: calculating a feedforward moment value of the coarse-grained trajectory based on a mechanism model, and marking the feedforward moment value as x i, i= … n; calculating a feedforward moment value of the fine-grained trajectory based on the data driving model, and recording as z ij, i= … n, j= … k;
step four: and obtaining probability distribution of the feedforward torque value based on the calculated value, and introducing uncertainty. Wherein the probability distribution obtained by the mechanism model is as follows Wherein the method comprises the steps ofDetermining according to the deviation between the mechanism model and the measured true value; the probability distribution obtained by the data driven model is thatWherein the method comprises the steps ofBased on the sensor error.
Step five: determining optimal weightsK is synchronously updated along with the circulation; the concentration degree of the normal distribution depends on the variance, the smaller the variance is, the more concentrated the corresponding probability distribution is, the two obtained in the step four are multiplied, and the new probability distribution is as follows:
Determining optimal weights K is synchronously updated along with the circulation; recording the optimal weight asWhere x' =x i+K(zij-xi),The new probability distribution variance obtained after multiplication is smaller, and the accuracy of the feedforward value can be improved by cycling the process. Wherein the optimal weight K is updated synchronously with the cycle.
Step six: and executing internal circulation, updating the optimal weight through follow the trail of filtering algorithm and refining the feedforward torque value.
The mechanism model derived x i is used as a state initial value, and a series of observations z ij provided by the data model are dynamically updated. Taking the ith cycle as an example: x i and z i1 are fused through the optimal weight K in the fifth step, and a more accurate estimated true value is obtainedAnd then toFused with z i2, at which time the weight K is updated,ObtainingThe above process is continuously cycled until j=k. The specific iterative process is as follows:
(1) Calculating optimal weight:
(2) Predicting the next state from the current state: x' ij＝xi+K(zij-xi),
(3) Updating the optimal weight:
(4) Substituting the calculated x 'ij into x i in (2), substituting K' into K in (2), and iteratively calculating the next optimal state.
Step seven: executing the outer loop, and circularly executing the step six to obtain a series of estimated valuesAnd the feedforward torque value is transmitted to the control compensation module as a feedforward torque value.
Referring to fig. 5, the method specifically includes the following steps:
S501: and the control compensation module reads the feedforward torque value output by the data fusion module and track information of the upper computer, and converts the feedforward value into a motor current value.
S502: the discrete initialization tool performs dispersion on track information according to the frequency of a control signal received by the robot, aligns the track information with the received feedforward torque value dimension, and takes the track information and the feedforward torque value dimension as the input of a feedforward compensation channel.
S503: and monitoring the position information of the mechanical arm end effector and the current information of each joint in real time, and taking the deviation between the obtained position information machine and the current information and the preset information as the input of a feedback channel.
S504: the LQR method is selected, but not limited to, PID, as a feedback controller to receive the signal of S503 and output control information to the robot arm.
The invention provides a smooth and accurate feedforward moment value with reasonable value range for the task track preset by the upper computer, and ensures the applicability to most of the tracks through a mechanism model solver; the feedforward value of the general track can be quickly solved through a data driving model solver, so that the practicability and universality of track control are improved; meanwhile, a control compensation module is added, and feedback control is carried out on the mechanical arm from the position ring and the current ring, so that the control precision of the six-axis mechanical arm is improved.
It will be readily appreciated by those skilled in the art that the foregoing description is merely a preferred embodiment of the invention and is not intended to limit the invention, but any modifications, equivalents, improvements or alternatives falling within the spirit and principles of the invention are intended to be included within the scope of the invention.
Claims (8)
1. The track control method of the mechanical arm is characterized by comprising the following steps of:
(1) Acquiring track information of the mechanical arm, converting the track information into joint space, further presetting track rough-dividing nodes, and solving through a mechanism model to obtain a feedforward moment value; meanwhile, presetting track subdivision nodes and solving through a data driving model to obtain a feedforward moment value;
(2) Adopting a tracking filter to perform data fusion on the obtained two feedforward torque values so as to obtain a new feedforward torque value;
(3) Dispersing track information of the mechanical arm by adopting a discrete initialization tool, aligning the track information after dispersing with the dimension of a new feedforward torque value to serve as input of a feedforward compensation channel, processing the input through a feedback controller to obtain control information, and outputting the control information to the mechanical arm to carry out track control on the mechanical arm;
fusing probability distribution of the predicted values obtained by the mechanism model and the data driving model, providing a state initial value by the predicted values of the mechanism model, and continuously carrying out follow the trail of on the predicted values of the data driving model to obtain optimal probability distribution, so as to obtain a feedforward compensation moment value;
The probability distribution obtained by the mechanism model is that Wherein the method comprises the steps ofDetermining according to the deviation between the mechanism model and the measured true value; the probability distribution obtained by the data driven model is thatWherein the method comprises the steps ofDetermining optimal weights based on sensor error decisionsK is synchronously updated along with the circulation; multiplying the obtained probabilities to obtain new probabilities, and then completing data fusion to obtain new feedforward moment values; wherein the new probability distribution is:
2. The trajectory control method of a robot arm according to claim 1, characterized in that: in the step (3), the obtained feedforward torque value is converted into a motor current value, the motor current value and the track information are used as input of a feedforward channel, meanwhile, the position information of the mechanical arm end effector and the current information of each joint are monitored in real time, and the deviation of the obtained position information and the current information and preset information is used as input of a feedback channel.
3. The trajectory control method of a robot arm according to claim 1, characterized in that: the feedforward moment value is obtained through solving a mechanism model, and the method comprises the following substeps:
(1) Acquiring a vibration mode function h j (x) of the connecting rod by a hypothesis mode method;
(2) Acquiring the speed and displacement of each connecting rod of the space manipulator through a jacobian matrix;
(3) Solving kinetic energy and potential energy of the space manipulator;
(4) And establishing a flexible dynamic model of the space manipulator by a Lagrangian method:
and then solving a feedforward moment value under the corresponding task track.
4. The trajectory control method of a robot arm according to claim 1, characterized in that: the feedforward moment value obtained by solving the data driving model mainly comprises the following substeps:
(1) Obtaining a data set containing track information and moment values corresponding to the track information through experiments;
(2) A support vector machine or a neural network method is selected to perform deep learning on the obtained data set to obtain parameter weight w and bias b;
(3) And constructing a function tau=f (q, q', w, b) by the trained network model, and solving a feedforward moment value under the corresponding task track.
5. The track control device of the mechanical arm is characterized in that: the track control device comprises a readable storage medium and a processor, wherein the readable storage medium stores a computer program; the processor is configured to read and execute a computer program stored in the readable storage medium, so that the processor executes the trajectory control method of the robot arm according to any one of claims 1 to 4.
6. The track control device of the mechanical arm is characterized in that: the track control device comprises a data fusion module and a control compensation module; wherein,
The data fusion module is used for acquiring track information of the mechanical arm and converting the track information into joint space, further presetting track rough-division nodes and solving through a mechanism model to obtain a feedforward moment value; meanwhile, presetting track subdivision nodes and solving through a data driving model to obtain a feedforward moment value; a tracking filter is adopted to conduct data fusion on the obtained two feedforward torque values so as to obtain a new feedforward torque value;
The control compensation module is used for dispersing track information of the mechanical arm by adopting a discrete initialization tool, aligning the track information after the dispersion with the dimension of a new feedforward torque value from the data fusion module to be used as input of a feedforward compensation channel, processing the input through a feedback controller to obtain control information, and outputting the control information to the mechanical arm to carry out track control on the mechanical arm;
fusing probability distribution of the predicted values obtained by the mechanism model and the data driving model, providing a state initial value by the predicted values of the mechanism model, and continuously carrying out follow the trail of on the predicted values of the data driving model to obtain optimal probability distribution, so as to obtain a feedforward compensation moment value;
The probability distribution obtained by the mechanism model is that Wherein the method comprises the steps ofDetermining according to the deviation between the mechanism model and the measured true value; the probability distribution obtained by the data driven model is thatWherein the method comprises the steps ofDetermining optimal weights based on sensor error decisionsK is synchronously updated along with the circulation; multiplying the obtained probabilities to obtain new probabilities, and then completing data fusion to obtain new feedforward moment values; wherein the new probability distribution is:
7. the utility model provides a mechanical arm which characterized in that: the mechanical arm performs track control on the mechanical arm by adopting the track control method of the mechanical arm according to any one of claims 1-4.
8. The utility model provides a mechanical arm which characterized in that: the robot arm comprising the trajectory control device of the robot arm according to any one of claims 5 to 6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210864724.5A CN115179290B (en) | 2022-07-21 | Mechanical arm and track control method and device thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210864724.5A CN115179290B (en) | 2022-07-21 | Mechanical arm and track control method and device thereof |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115179290A CN115179290A (en) | 2022-10-14 |
| CN115179290B true CN115179290B (en) | 2024-07-02 |
Family
ID=
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111496781A (en) * | 2020-03-17 | 2020-08-07 | 浙江大学 | Mechanical arm modeling, controlling and monitoring integrated system driven by digital twin |
| CN112247992A (en) * | 2020-11-02 | 2021-01-22 | 中国科学院深圳先进技术研究院 | Robot feedforward torque compensation method |
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111496781A (en) * | 2020-03-17 | 2020-08-07 | 浙江大学 | Mechanical arm modeling, controlling and monitoring integrated system driven by digital twin |
| CN112247992A (en) * | 2020-11-02 | 2021-01-22 | 中国科学院深圳先进技术研究院 | Robot feedforward torque compensation method |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Xiang et al. | Robust fuzzy 3D path following for autonomous underwater vehicle subject to uncertainties | |
| Li et al. | Trajectory-tracking control of mobile robot systems incorporating neural-dynamic optimized model predictive approach | |
| Lin | A reinforcement learning adaptive fuzzy controller for robots | |
| Ogbemhe et al. | Towards achieving a fully intelligent robotic arc welding: a review | |
| Yue et al. | An efficient model predictive control for trajectory tracking of wheeled inverted pendulum vehicles with various physical constraints | |
| Hu et al. | A reinforcement learning neural network for robotic manipulator control | |
| Sun | Kinematics model identification and motion control of robot based on fast learning neural network | |
| CN112571420A (en) | Dual-function model prediction control method under unknown parameters | |
| Perrusquía et al. | Robot position/force control in unknown environment using hybrid reinforcement learning | |
| Zhou et al. | Robust Kalman filtering with long short-term memory for image-based visual servo control | |
| Howell et al. | CALIPSO: A differentiable solver for trajectory optimization with conic and complementarity constraints | |
| Tang et al. | Trajectory tracking of robotic manipulators with constraints based on model predictive control | |
| Komeno et al. | Deep koopman with control: Spectral analysis of soft robot dynamics | |
| Korayem et al. | Regulation of cost function weighting matrices in control of WMR using MLP neural networks | |
| CN115179290B (en) | Mechanical arm and track control method and device thereof | |
| West et al. | A new approach to improve the parameter estimation accuracy in robotic manipulators using a multi-objective output error identification technique | |
| Tang et al. | Actively learning Gaussian process dynamical systems through global and local explorations | |
| CN115179290A (en) | Mechanical arm and track control method and device thereof | |
| Afzali et al. | A Modified Convergence DDPG Algorithm for Robotic Manipulation | |
| Kumar et al. | Neuro‐fuzzy control applied to multiple cooperating robots | |
| Grisales-Ramirez et al. | Multi-objective optimal path planning for mobile robots using state space cell mapping | |
| Shang et al. | Fuzzy adaptive control of coal gangue sorting parallel robot with variable load | |
| Ren et al. | Discrete-time parallel robot motion control using adaptive neuro-fuzzy inference system based on improved subtractive clustering | |
| Bharathi et al. | Path planning of unmanned mobile robot in unknown obstacle environments for multi switching control tracking using deep reinforcement learning | |
| Nakai et al. | Quadrotor Trajectory Tracking Under Output Delay: Strategies and Challenges |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant |