Disclosure of Invention
In order to solve the technical problems, the invention provides a control method, a device and a control system for a mechanical arm, which can stably and rapidly control the mechanical arm.
In order to achieve the aim, the invention provides a control method of a mechanical arm, which comprises the steps of obtaining an initial position and a target position of the mechanical arm, inputting the initial position and the target position into a model prediction controller, outputting state parameters at the current moment according to the initial position and the target position at the model prediction controller, respectively inputting the state parameters at the current moment into the mechanical arm controller and a prediction model, outputting target parameters at the current moment according to the state parameters at the current moment at the mechanical arm controller, outputting predicted parameters at the next moment according to the state parameters at the current moment at the prediction model, adding the target parameters and the predicted parameters to obtain a combined parameter, and controlling the mechanical arm to move to the target position with the combined parameter. Therefore, the target parameter at the current moment is output through the mechanical arm, the predicted parameter at the next moment is output through the prediction model, the target parameter and the predicted parameter are added to obtain the combined parameter, and the mechanical arm is controlled to move to the target position according to the combined parameter, so that the motion of the mechanical arm is corrected by generating the predicted parameter through the prediction model, and the stability of the motion of the mechanical arm is improved.
In one embodiment of the invention, the method further comprises the steps of calculating the current position of the mechanical arm corresponding to the merging parameter, comparing the current position with a threshold value, adding the current position with the target position when the current position is larger than the threshold value, and inputting the added value of the current position and the target position into the model prediction controller. Therefore, a feedback loop is formed, and the stability and accuracy of the movement of the mechanical arm are further improved.
In an embodiment of the invention, the state parameters include an angle and an angular velocity of the mechanical arm. For this reason, stable control of the angle and the angular velocity is achieved.
In an embodiment of the present invention, the following formula is adopted to output the prediction parameters of the next moment according to the state parameters of the current moment in the prediction model, wherein x (k+1) =ax (k) +bu (k) y (k) =cx (k), a, B, C are constants, x (k+1) represents the angle vector of each joint in the mechanical arm input at the moment k+1, x (k) represents the angle vector of each joint in the mechanical arm input at the moment k, u (k) represents the angle vector of each joint in the mechanical arm input at the moment k, and y (k) represents the angle vector of each joint in the mechanical arm output at the moment k. For this purpose, it is achieved that the prediction model predicts the parameters at the next moment.
In one embodiment of the invention, outputting the state parameter at the current time at the model predictive controller based on the initial position and the target position includes outputting the state parameter at the current time at the model predictive controller based on the initial position and the target position using a minimization cost function. For this purpose, control of the model predictive controller is achieved.
The invention further provides a control device of the mechanical arm, which comprises an acquisition module, a first processing module, a second processing module and a calculation control module, wherein the acquisition module acquires an initial position and a target position of the mechanical arm, the initial position and the target position are input into a model prediction controller, the first processing module outputs state parameters at the current moment at the model prediction controller according to the initial position and the target position, the state parameters at the current moment are respectively input into the mechanical arm controller and the prediction model, the second processing module outputs target parameters at the current moment at the mechanical arm controller according to the state parameters at the current moment, the prediction parameters at the next moment are output at the prediction model according to the state parameters at the current moment, and the calculation control module adds the target parameters and the prediction parameters to obtain a combination parameter so as to control the mechanical arm to move to the target position with the combination parameter.
In one embodiment of the invention, the device further comprises a step of calculating the current position of the mechanical arm corresponding to the merging parameter, a step of comparing the current position with a threshold value, a step of adding the current position with the target position when the current position is larger than the threshold value, and a step of inputting the added value of the current position and the target position into the model prediction controller.
In an embodiment of the invention, the state parameters include an angle and an angular velocity of the mechanical arm.
In an embodiment of the present invention, the following formula is adopted to output the prediction parameters of the next moment according to the state parameters of the current moment in the prediction model, wherein x (k+1) =ax (k) +bu (k) y (k) =cx (k), a, B, C are constants, x (k+1) represents the angle vector of each joint in the mechanical arm input at the moment k+1, x (k) represents the angle vector of each joint in the mechanical arm input at the moment k, u (k) represents the angle vector of each joint in the mechanical arm input at the moment k, and y (k) represents the angle vector of each joint in the mechanical arm output at the moment k.
In one embodiment of the invention, outputting the state parameter at the current time at the model predictive controller based on the initial position and the target position includes outputting the state parameter at the current time at the model predictive controller based on the initial position and the target position using a minimization cost function.
The invention further provides a control system of the mechanical arm, which comprises a model prediction controller, a mechanical arm controller, a prediction model and an adder, wherein the model prediction controller outputs state parameters at the current moment according to the received initial position and the target position, the mechanical arm controller receives the state parameters at the current moment and outputs target parameters at the current moment according to the state parameters at the current moment, the prediction model receives the state parameters at the current moment and outputs predicted parameters at the next moment according to the state parameters at the current moment, and the adder adds the target parameters and the predicted parameters to obtain a combined parameter, and the combined parameter is used for controlling the mechanical arm to move to the target position with the combined parameter.
The invention also proposes a mechanical system comprising a robotic arm and a control system as claimed in claim 11 for controlling the robotic arm.
The invention also proposes an electronic device comprising a processor, a memory and instructions stored in said memory, wherein said instructions, when executed by said processor, implement a method as described above.
The invention also proposes a computer readable storage medium having stored thereon computer instructions which, when executed, perform a method according to the above.
Detailed Description
For a clearer understanding of technical features, objects, and effects of the present invention, a specific embodiment of the present invention will be described with reference to the accompanying drawings.
In the following description, numerous specific details are set forth in order to provide a thorough understanding of the present invention, but the present invention may be practiced in other ways than as described herein, and therefore the present invention is not limited to the specific embodiments disclosed below.
As used in the specification and in the claims, the terms "a," "an," "the," and/or "the" are not specific to a singular, but may include a plurality, unless the context clearly dictates otherwise. In general, the terms "comprises" and "comprising" merely indicate that the steps and elements are explicitly identified, and they do not constitute an exclusive list, as other steps or elements may be included in a method or apparatus.
The present invention provides a control method of a mechanical arm, fig. 1 is a flowchart of a control method 100 of a mechanical arm according to an embodiment of the present invention, fig. 2 is a control logic diagram of a mechanical arm according to an embodiment of the present invention, and the control method of a mechanical arm according to an embodiment of the present invention is described below with reference to fig. 1 and 2, as shown in fig. 1, the control method of a mechanical arm includes:
step 110, acquiring an initial position and a target position of the mechanical arm, and inputting the initial position and the target position into a model predictive controller.
The initial position of the mechanical arm can be obtained through a position sensor arranged on the mechanical arm, the target position of the mechanical arm can be obtained from a user through a man-machine interface, and the initial position and the target position of the mechanical arm can be coordinates under a mechanical arm coordinate system or coordinates under a world coordinate system. In fig. 2, the input 210 includes an initial position and a target position of the robot arm, which are input as positive feedback to the model predictive controller 220 via the first adder 270.
And 120, outputting the state parameters at the current moment at the model predictive controller according to the initial position and the target position, and respectively inputting the state parameters at the current moment into the mechanical arm controller and the predictive model.
After the initial position and the target position are input to the model predictive controller 220, the model predictive controller 220 outputs a state parameter at a current time according to the initial position and the target position using a mathematical model. In some embodiments, the state parameters include an angle and an angular velocity of the robotic arm. Taking a six-joint robot as an example, the output of the model predictive controller 220 may be a 12 x 1 matrix, where the first 6 rows are angles of the current moment of the 6 joints and the last 6 rows are angular velocities of the current moment of the 6 joints. In some embodiments, outputting the state parameter at the current time at the model predictive controller based on the initial position and the target position includes outputting the state parameter at the current time at the model predictive controller based on the initial position and the target position using a minimization cost function.
And 130, outputting a target parameter at the current moment at the mechanical arm controller according to the state parameter at the current moment, and outputting a predicted parameter at the next moment at the prediction model according to the state parameter at the current moment.
After the state parameter at the current moment is input to the mechanical arm controller 230, the mechanical arm controller 230 outputs the target parameter at the current moment according to the state parameter at the current moment by using a control algorithm, and for example, the output of the mechanical arm controller 230 may be a matrix of 6*1, where each row represents the target angles of 6 joints.
After the state parameter at the current moment is input to the prediction model 240, the prediction model 240 outputs the prediction parameter at the next moment according to the state parameter at the current moment, and taking a six-joint robot as an example, the output of the prediction model 240 may be a matrix of 6*1, and each row represents the prediction angles of 6 joints.
In some embodiments, the following formula may be used to output the predicted parameters at the next time in the prediction model based on the state parameters at the current time:
x(k+1)=Ax(k)+Bu(k)
y(k)=Cx(k)
Where a, B, and C are constants, x (k+1) represents an angle vector of each joint in the robot arm input at time k+1, x (k) represents an angle vector of each joint in the robot arm input at time k, u (k) represents an angle vector of each joint in the robot arm input at time k, and y (k) represents an angle vector of each joint in the robot arm output at time k.
And 140, adding the target parameter and the predicted parameter to obtain a combined parameter, and controlling the mechanical arm to move the combined parameter to the target position.
The output target parameter of the mechanical arm controller 230 is used as positive feedback to be output 260 through the second adder 280, the output predicted parameter of the prediction model 240 is used as negative feedback to be output 260 through the second adder 280, namely, the target parameter and the predicted parameter are added to obtain a combined parameter, the target parameter in the combined parameter is used as a positive value, and the predicted parameter is used as a negative value, so that the motion of the mechanical arm is corrected through the prediction model, and the stability of the motion of the mechanical arm is improved. The combining parameters are then sent to a robot driver (not shown in the figure) which controls the movement of the robot to the target position according to the combining parameters.
In some embodiments, the method 100 further includes calculating a current position of the robotic arm corresponding to the combining parameter, comparing the current position to a threshold, and adding the current position to the target position when the current position is greater than the threshold, and inputting the added value of the current position and the target position to the model predictive controller. In fig. 2, the combination parameter output by the second adder 280 is further input to the determining unit 250, the determining unit 250 calculates the current position of the mechanical arm corresponding to the combination parameter, compares the current position with a threshold, and if the current position is greater than the threshold, the current position is input to the first adder 270 as negative feedback, and the target position is input to the first adder 270 as positive input, otherwise, to the output 260. Therefore, a feedback loop is formed, and the stability and accuracy of the movement of the mechanical arm are further improved.
Fig. 7A-7G are schematic diagrams of simulation results of a 7-joint mechanical arm according to a control method according to an embodiment of the invention. Fig. 7A to 7G correspond to each joint, respectively, with time on the abscissa, in seconds(s), joint angle on the ordinate, in degrees (°), and the solid line is an angle simulation curve using the control method according to the embodiment of the present invention, and the dotted line is an angle simulation curve using the PID control method, and it can be seen from fig. 7A to 7G that the steady state can be achieved within 0.5 seconds, regardless of whether the control method according to the embodiment of the present invention or the PID control method is used, however, the fluctuation of the control method according to the embodiment of the present invention is smaller, that is, the stability is higher.
The embodiment of the invention provides a control method of a mechanical arm, which comprises the steps of outputting a target parameter at the current moment through the mechanical arm, outputting a predicted parameter at the next moment through a prediction model, adding the target parameter and the predicted parameter to obtain a combined parameter, and controlling the mechanical arm to move to a target position by the combined parameter, so that the motion of the mechanical arm is corrected by generating the predicted parameter through the prediction model, and the motion stability of the mechanical arm is improved.
The present invention further proposes a control device 300 for a robotic arm, and fig. 3 is a schematic diagram of a control device 300 for a robotic arm according to an embodiment of the present invention, as shown in fig. 3, the control device 300 for a robotic arm includes:
The acquiring module 310 acquires an initial position and a target position of the mechanical arm, and inputs the initial position and the target position into the model predictive controller.
The first processing module 320 outputs a state parameter at the current moment at the model predictive controller according to the initial position and the target position, and inputs the state parameter at the current moment into the mechanical arm controller and the predictive model respectively.
The second processing module 330 outputs the target parameter at the current moment at the mechanical arm controller according to the state parameter at the current moment, and outputs the predicted parameter at the next moment at the prediction model according to the state parameter at the current moment.
The calculation control module 340 sums the target parameter and the predicted parameter to obtain a combined parameter, and controls the mechanical arm to move the combined parameter to the target position.
In some embodiments, the apparatus 300 further includes calculating a current position of the robotic arm corresponding to the combining parameter, comparing the current position to a threshold, and adding the current position to the target position when the current position is greater than the threshold, and inputting the added value of the current position and the target position to the model predictive controller.
In some embodiments, the state parameters include an angle and an angular velocity of the robotic arm.
In some embodiments, the following formula is used to output the predicted parameters at the next time in the prediction model based on the state parameters at the current time:
x(k+1)=Ax(k)+Bu(k)
y(k)=Cx(k)
Where a, B, and C are constants, x (k+1) represents an angle vector of each joint in the robot arm input at time k+1, x (k) represents an angle vector of each joint in the robot arm input at time k, u (k) represents an angle vector of each joint in the robot arm input at time k, and y (k) represents an angle vector of each joint in the robot arm output at time k.
In some embodiments, outputting the state parameter at the current time at the model predictive controller based on the initial position and the target position includes outputting the state parameter at the current time at the model predictive controller based on the initial position and the target position using a minimization cost function.
The present invention further proposes a control system 400 for a robotic arm, and fig. 4 is a schematic diagram of a control system for a robotic arm according to an embodiment of the present invention, as shown in fig. 4, the control system 400 for a robotic arm includes:
The model predictive controller 410 outputs a state parameter at the current time based on the received initial position and target position.
The robot arm controller 420 receives the state parameter at the current time and outputs the target parameter at the current time according to the state parameter at the current time.
The prediction model 430 receives the state parameter at the current time and outputs the predicted parameter at the next time according to the state parameter at the current time.
The adder 440 adds the target parameter and the predicted parameter to obtain a combined parameter, and the combined parameter is used to control the robot arm to move the combined parameter to the target position.
The present invention also proposes a mechanical system 500, fig. 5 is a schematic diagram of a mechanical system 500 according to an embodiment of the present invention, and as shown in fig. 5, the mechanical system 500 includes a mechanical arm 520 and a control system 510 for controlling the mechanical arm of the mechanical arm 520, where the control system 510 of the mechanical arm may be the control system 400 of the mechanical arm.
The invention also proposes an electronic device 600. Fig. 6 is a schematic diagram of an electronic device 600 according to an embodiment of the invention. As shown in fig. 6, electronic device 600 includes a processor 610 and a memory 620, with instructions stored in memory 620, wherein the instructions when executed by processor 610 implement method 100 as described above.
The present invention also proposes a computer readable storage medium having stored thereon computer instructions which, when executed, perform the method 100 as described above.
Some aspects of the methods and apparatus of the present invention may be performed entirely by hardware, entirely by software (including firmware, resident software, micro-code, etc.) or by a combination of hardware and software. The above hardware or software may be referred to as a "data block," module, "" engine, "" unit, "" component, "or" system. The processor may be one or more Application Specific Integrated Circuits (ASICs), digital Signal Processors (DSPs), digital signal processing devices (DAPDs), programmable Logic Devices (PLDs), field Programmable Gate Arrays (FPGAs), processors, controllers, microcontrollers, microprocessors, or a combination thereof. Furthermore, aspects of the invention may take the form of a computer product, comprising computer-readable program code, embodied in one or more computer-readable media. For example, computer-readable media may include, but are not limited to, magnetic storage devices (e.g., hard disk, floppy disk, magnetic cassettes, optical disks, compact disks (e.g., compact Disks (CDs), digital Versatile Disks (DVDs)), smart cards, and flash memory devices (e.g., cards, sticks, key drives).
A flowchart is used herein to describe the operations performed by methods according to embodiments of the present application. It should be appreciated that the foregoing operations are not necessarily performed in order precisely. Rather, the various steps may be processed in reverse order or simultaneously. At the same time, other operations are added to or removed from these processes.
It should be understood that although the present disclosure has been described in terms of various embodiments, not every embodiment is provided with a separate technical solution, and this description is for clarity only, and those skilled in the art should consider the disclosure as a whole, and the technical solutions in the various embodiments may be combined appropriately to form other embodiments that will be understood by those skilled in the art.
The foregoing is illustrative of the present invention and is not to be construed as limiting the scope of the invention. Any equivalent alterations, modifications and combinations thereof will be effected by those skilled in the art without departing from the spirit and principles of this invention, and it is intended to be within the scope of the invention.