CN113199460B - Nonlinear musculoskeletal robot control method, system and device - Google Patents
Nonlinear musculoskeletal robot control method, system and device Download PDFInfo
- Publication number
- CN113199460B CN113199460B CN202110562679.3A CN202110562679A CN113199460B CN 113199460 B CN113199460 B CN 113199460B CN 202110562679 A CN202110562679 A CN 202110562679A CN 113199460 B CN113199460 B CN 113199460B
- Authority
- CN
- China
- Prior art keywords
- robot
- muscle
- length
- force
- nonlinear
- 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 55
- 210000003205 muscle Anatomy 0.000 claims abstract description 133
- 210000001087 myotubule Anatomy 0.000 claims abstract description 49
- 230000008859 change Effects 0.000 claims abstract description 32
- 210000002435 tendon Anatomy 0.000 claims abstract description 25
- 239000011159 matrix material Substances 0.000 claims abstract description 23
- 238000009826 distribution Methods 0.000 claims description 7
- 230000004044 response Effects 0.000 claims description 6
- 239000000126 substance Substances 0.000 claims description 4
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 12
- 230000004913 activation Effects 0.000 description 6
- 230000008569 process Effects 0.000 description 6
- 239000011664 nicotinic acid Substances 0.000 description 5
- 238000004088 simulation Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 238000013461 design Methods 0.000 description 3
- 230000003042 antagnostic effect Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000003860 storage Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 210000000245 forearm Anatomy 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 210000001503 joint Anatomy 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000004118 muscle contraction Effects 0.000 description 1
- 210000002346 musculoskeletal system Anatomy 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000009877 rendering Methods 0.000 description 1
- 210000000323 shoulder joint Anatomy 0.000 description 1
- 210000003857 wrist joint Anatomy 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/0006—Exoskeletons, i.e. resembling a human figure
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
-
- 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/1612—Programme controls characterised by the hand, wrist, grip control
-
- 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/1628—Programme controls characterised by the control loop
-
- 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
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Health & Medical Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Orthopedic Medicine & Surgery (AREA)
- Manipulator (AREA)
Abstract
The invention belongs to the field of control of a musculoskeletal robot, and particularly relates to a nonlinear musculoskeletal robot control method, system and device, aiming at solving the problems of low control precision and high difficulty of the existing stay-supported skeletal robot. The invention comprises the following steps: according to the inverse of Lagrange dynamics, the input of a feedforward controller is carried out, and the joint torque of the robot is calculated; combining the force arm matrix of the robot to obtain the muscle force and tendon length of each muscle of the robot, and calculating the length of each muscle of the robot; obtaining the muscle fiber change speed through backward difference; constructing a feedforward controller of the robot based on the muscle force, the muscle fiber length and the change speed of each muscle of the robot; adjusting the output signal of the feedforward controller through a PID controller; and controlling the robot to move according to the expected track according to the adjusted control signal. The feedforward controller of the nonlinear musculoskeletal robot has robustness and strong applicability.
Description
Technical Field
The invention belongs to the field of control of a musculoskeletal robot, and particularly relates to a nonlinear musculoskeletal robot control method, system and device.
Background
At present, in the field of automated production and assembly, not only the control precision of a robot arm performing operation is highly required, but also the flexibility and robustness of the robot are highly required. The existing mechanical arm has high position control precision, but the flexibility and the flexibility are poor, so that fine operation tasks are difficult to complete, for example, the assembly of various electronic devices in the 3C industry is very difficult. At present, the assembly work in the field is mainly completed manually, and the rapid and repeated labor is a disability to human mind and body. To liberate this type of labor to a greater extent, it is important to develop a robot arm with human-like compliance and to design a robust controller.
The existing schemes for improving the flexibility of the mechanical arm generally have two types: firstly, the operation of the motor is flexible by improving the control method of the traditional joint connecting rod motor, for example, impedance control is adopted; however, the method needs high force sensing precision, has high control frequency requirement and strict communication requirement, and therefore, the robustness is difficult to guarantee. And secondly, the human-like musculoskeletal mechanical arm is realized through a bionic structure, and the flexibility assembly operation are expected to be realized by means of structural characteristics and muscle driving characteristics of a human. The existing bionic musculoskeletal system is generally driven by simplified human-like muscle arrangement and pneumatic muscles, and has the defects that the driving characteristics of biological muscles cannot be accurately simulated, the pneumatic control precision is low, and the influence of air pressure and temperature is large; and a motor stay wire is adopted as a mechanical arm of a driving and transmission system, and the mechanical arm has certain development, but has poor control precision and human-like flexibility and does not have assembling operation capability.
Disclosure of Invention
In order to solve the problems in the prior art, namely the problems of unreasonable layout of the stay wires of the existing stay wire type skeletal robot, insufficient bionic degree of the output characteristics of the stay wire force, low redundancy, low control precision and high difficulty, the invention provides a nonlinear musculoskeletal robot control method, which comprises the following steps:
step S10, according to the inverse of Lagrange dynamics, the input of a feedforward controller is carried out, the joint torque of the robot is calculated, and the muscle force of each muscle of the robot is obtained by combining the moment arm matrix of the robot; the feed forward controller input comprises a desired joint angle and a desired joint angular velocity;
step S20, acquiring the tendon length of each muscle of the robot based on the functional relationship between the muscle force and the tendon length of the Thelen2003 muscle dynamics model, and acquiring the length of each muscle of the robot according to the geometric distribution relationship of the muscle of the robot on the skeleton;
step S30, calculating the muscle fiber length of each muscle of the robot based on the length of each muscle and the length of the tendon of the robot, and obtaining the muscle fiber change speed through backward difference;
step S40, constructing a robot feedforward controller based on the muscle force, the muscle fiber length and the change speed of each muscle of the robot;
step S50, to get t j The error between the desired joint angle and the response joint angle at that moment is used as the input signal of a PID controller, and is passed through the PID controller t j+1 Adjusting the output signal of the robot feedforward controller by the output signal at the moment;
step S60, based on the adjusted t j+1 And controlling the robot to move according to the expected track by the control signal at the moment.
In some preferred embodiments, the joint torque of the robot is calculated by:
wherein tau represents the joint torque of the robot, M represents a link inertia matrix, C represents a Coriolis force and centrifugal force matrix, G represents a gravity change matrix, theta,Andrepresenting the desired joint angle, the desired joint angular velocity and the desired joint angular acceleration, respectively.
In some preferred embodiments, the muscle force of each muscle of the robot is calculated by:
F TD =W -1 τ
wherein, F TD Represents muscle force, tau represents joint torque of the robot, W represents moment arm matrix, W -1 Is the inverse matrix of W.
In some preferred embodiments, the muscle fiber length of each muscle of the robot is calculated by:
l M =(l MT -l TD )/cosα
wherein l M Represents the length of the muscle fiber,/ MT Represents the length of the muscle,/ TD Represents the tendon length and α represents the pinnate angle.
In some preferred embodiments, the muscle fiber change speed is calculated by:
wherein i M (t) speed of change of muscle fiber with time t as variable,/ M (t) and l M (t-1) represents the muscle fiber length at time t and time t-1, respectively, and Δ t represents the time variation at time t and time t-1.
In some preferred embodiments, the robot feedforward controller activation signal model is:
wherein a (t) is a control signal of the robot feedforward controller at the time t, l M (t) and i M (t) represents the muscle fiber length and the muscle fiber change speed at time t, respectively, F TD (t) represents the muscle force at time t, F PE Representing the force generated by passive elongation of the muscle fibres, f l Representing the force-length coefficient, f v Representing the force-velocity coefficient.
In some preferred embodiments, the PID controller t j+1 An output signal at a time, represented as:
wherein k is p Is a proportionality coefficient, k i Is an integral coefficient, k d Is a differential coefficient, u tj Is a PID controller t j The output signal of the time of day is,represents t j Desired joint angle at all timesAnd responsive joint angleThe error of (a) is detected,representing joint angular velocity errors.
In some preferred embodiments, said F PE 、f l 、f v Obtained by functional relationship of Thelen2003 muscle dynamics model.
In another aspect of the present invention, a nonlinear musculoskeletal robot control system is presented, the system comprising the following modules:
the muscle force acquisition module is configured to calculate joint torque of the robot according to the inverse of Lagrange dynamics and the input of a feedforward controller, and acquire the muscle force of each muscle of the robot by combining with a moment arm matrix of the robot; the feed forward controller input comprises a desired joint angle and a desired joint angular velocity;
the muscle length acquisition module is configured to acquire the tendon length of each muscle of the robot based on the functional relationship between the muscle force and the tendon length of the Thelen2003 muscle dynamics model, and acquire the length of each muscle of the robot according to the geometric distribution relationship of the muscle of the robot on the skeleton;
a muscle fiber change speed acquisition module configured to calculate a muscle fiber length of each muscle of the robot based on the length of each muscle and the length of the tendon of the robot, and acquire a muscle fiber change speed by backward difference;
a feedforward controller building module configured to build a robot feedforward controller based on a muscle force, a muscle fiber length, and a change speed of each muscle of the robot;
a feedback adjustment module configured to adjust t j Desired joint angle at all timesAnd responsive joint angleAs an input signal to a PID controller by means of which t j+1 Adjusting the output signal of the robot feedforward controller by the output signal at the moment;
a robot control module configured to adjust t based on the measured value j+1 And controlling the robot to move according to the expected track by the control signal at the moment.
In a third aspect of the present invention, a nonlinear musculoskeletal robot control apparatus is presented, comprising:
at least one processor; and
a memory communicatively coupled to at least one of the processors; wherein the content of the first and second substances,
the memory stores instructions executable by the processor for execution by the processor to implement the nonlinear musculoskeletal robot control method described above.
The invention has the beneficial effects that:
(1) the nonlinear musculoskeletal robot control method introduces biological muscle dynamics in order to improve the bionic characteristics of the existing stay-supported robot, combines the muscle force output with the states of activation (control) signals, muscle length, muscle contraction speed (muscle fiber change speed) and the like, fuses a plurality of spatial variables, improves the integrity and robustness of the whole system, and enables the whole system to have better control performance.
(2) The nonlinear musculoskeletal robot control method aims at the problems that the existing robot is low in redundancy and only outputs pulling force by combining the characteristic of biological muscle force, antagonistic muscle pairs are added to each degree of freedom, the redundancy of the whole system is improved, and the possibility of multiple groups of control signals is provided for task completion.
(3) The nonlinear musculoskeletal robot control method provided by the invention considers the problems of strong nonlinearity and multiple solutions, combines the dynamics of biological muscles and the redundancy characteristic of antagonistic muscle pairs, provides a control signal solution space, determines the robust range of system trajectory tracking control, and realizes accurate trajectory tracking control.
(4) According to the nonlinear musculoskeletal robot control method, the error between the expected joint angle and the response joint angle is used as the input of the PID controller, the output of the PID controller is used for output adjustment of the feedforward controller, the robustness of a model can be improved finally, the stability of track tracking is effectively improved, and the real-time error is reduced.
Drawings
Other features, objects and advantages of the present application will become more apparent upon reading of the following detailed description of non-limiting embodiments thereof, made with reference to the accompanying drawings in which:
FIG. 1 is a schematic diagram of a feedforward control and PID control fusion control of a nonlinear musculoskeletal robot control method of the present invention;
FIG. 2 is a musculoskeletal robot model of a nonlinear musculoskeletal robot control method of the present invention;
FIG. 3 is a block diagram of a musculoskeletal robot dynamics model of a nonlinear musculoskeletal robot control method of the present invention;
FIG. 4 is a schematic representation of a muscle control signal solution space for the nonlinear musculoskeletal robot control method of the present invention;
FIG. 5 is a schematic diagram of a comparison of a desired trajectory and an actual motion trajectory for one embodiment of the nonlinear musculoskeletal robot control method of the present invention;
FIG. 6 is a schematic diagram of a robot simulation control after PID control is added in an embodiment of the nonlinear musculoskeletal robot control method of the present invention;
fig. 7 is a diagram of real-time error of the terminal position in the robot simulation result after adding PID control according to an embodiment of the nonlinear musculoskeletal robot control method of the present invention.
Detailed Description
The present application will be described in further detail with reference to the following drawings and examples. It is to be understood that the specific embodiments described herein are merely illustrative of the relevant invention and not restrictive of the invention. It should be noted that, for convenience of description, only the portions related to the related invention are shown in the drawings.
It should be noted that, in the present application, the embodiments and features of the embodiments may be combined with each other without conflict. The present application will be described in detail below with reference to the embodiments with reference to the attached drawings.
The invention provides a nonlinear musculoskeletal robot control method, which aims at the problems that the existing guyed musculoskeletal robot system is unreasonable in guyed layout, insufficient in guyed force output characteristic bionic degree and the like, muscle force points are arranged and designed, a biological muscle force-length-speed-activation signal model is introduced, biological muscle nonlinearity, strong coupling and other characteristics are reserved, and the whole system has higher redundancy and robustness, so that the flexibility and control stability of the system are improved, and technical and theoretical support is provided for realizing flexible 3C industrial assembly operation. According to the system established by the invention, a robust feedforward controller is provided, and accurate track tracking control can be realized by combining PID regulation; and a control signal solution space is provided by combining the redundancy characteristic of the system, and the robust range of the system trajectory tracking control is determined.
The invention discloses a nonlinear musculoskeletal robot control method, which comprises the following steps:
step S10, according to the inverse of Lagrange dynamics, the input of a feedforward controller is carried out, the joint torque of the robot is calculated, and the muscle force of each muscle of the robot is obtained by combining with the moment arm matrix of the robot; the feed forward controller input comprises a desired joint angle and a desired joint angular velocity;
step S20, acquiring the tendon length of each muscle of the robot based on the functional relationship between the muscle force and the tendon length of the Thelen2003 muscle dynamics model, and acquiring the length of each muscle of the robot according to the geometric distribution relationship of the muscle of the robot on the skeleton;
step S30, calculating the muscle fiber length of each muscle of the robot based on the length of each muscle and the length of the tendon of the robot, and obtaining the muscle fiber change speed through backward difference;
step S40, constructing a feedforward controller of the robot based on the muscle force, the muscle fiber length and the change speed of each muscle of the robot;
step S50, to get t j The error between the desired joint angle and the response joint angle at that moment is used as the input signal of a PID controller, and the PID controller t j+1 Adjusting the output signal of the robot feedforward controller by the output signal at the moment;
step S60, based on the adjusted t j+1 And controlling the robot to move according to the expected track by the control signal at the moment.
In order to more clearly describe the nonlinear musculoskeletal robot control method of the present invention, the following describes the steps of the embodiment of the present invention in detail with reference to fig. 1.
The nonlinear musculoskeletal robot control method of the first embodiment of the present invention includes steps S10-S60, each of which is described in detail as follows:
step S10, according to the inverse of Lagrange dynamics, the input of a feedforward controller is carried out, the joint torque of the robot is calculated, and the muscle force of each muscle of the robot is obtained by combining with the moment arm matrix of the robot; the feed forward controller input includes a desired joint angle and a desired joint angular velocity.
The joint torque of the robot is calculated by the following formula (1):
wherein tau represents the joint torque of the robot, M represents a link inertia matrix, C represents a Coriolis force and centrifugal force matrix, G represents a gravity change matrix, theta,Andrepresenting the desired joint angle, the desired joint angular velocity and the desired joint angular acceleration, respectively.
The calculation method of the muscle force of each muscle of the robot is shown as the formula (2):
F TD =W -1 τ (2)
wherein, F TD Represents muscle force, tau represents joint torque of the robot, W represents moment arm matrix, W -1 Is the inverse matrix of W.
Step S20, acquiring the tendon length of each muscle of the robot based on the functional relationship between the muscle force and the tendon length of the Thelen2003 muscle dynamics model, and acquiring the length of each muscle of the robot according to the geometric distribution relationship of the muscle of the robot on the skeleton.
And step S30, calculating the muscle fiber length of each muscle of the robot based on the length of each muscle and the length of the tendon of the robot, and acquiring the muscle fiber change speed through backward difference.
The calculation method of the muscle fiber length of each muscle of the robot is shown in the formula (3):
l M =(l MT -l TD )/cosα (3)
wherein l M Represents the length of the muscle fiber,/ MT Represents the length of the muscle, l TD Represents the tendon length and α represents the pinnate angle.
The muscle fiber change speed is calculated according to the following formula (4):
wherein the content of the first and second substances,speed of change of muscle fiber, l, representing time t as variable M (t) and l M (t-1) represents the muscle fiber length at time t and at time t-1, respectively, and Δt represents the time variation of time t and time t-1.
And step S40, constructing a robot feedforward controller based on the muscle force, the muscle fiber length and the change speed of each muscle of the robot.
The robot feedforward controller activation signal model is as shown in equation (5):
wherein a (t) is a control signal of the robot feedforward controller at time t, l M (t) andrespectively representing the length of the muscle fiber and the speed of change of the muscle fiber at time t, F TD (t) represents the muscle force at time t, F PE Representing the force generated by passive elongation of the muscle fibres, f l Representing the force-length coefficient, f v Representing the force-velocity coefficient.
F PE 、f l 、f v Obtained by functional relationship of Thelen2003 muscle dynamics model.
As shown in fig. 2, a model of a musculoskeletal robot, which is a control method of a nonlinear musculoskeletal robot according to the present invention, includes two rotary joints driven by six stay muscles, and a block diagram of a kinetic model thereof is shown in fig. 3, a model of a robot constructed by using a design method of a feedforward controller proposed by the present invention is created, a solution space shown in fig. 4 is obtained, a trajectory tracking task is performed, and a trajectory tracking effect achieved by the method is shown in fig. 5.
The upper diagram of fig. 5 shows the activation signal solution space that can be taken by the muscle 1 and the muscle 2 to complete the trajectory tracking task, the upper diagram of fig. 5 shows the activation signal space of the muscles 3 and 4, and the lower diagram of fig. 5 shows the activation signal partial solution space of the muscles 5 and 6, which is characterized in that the curve in any one space along the time axis can be matched with other muscles to complete the task.
The control signal solution space of fig. 4 is not a specific solution but a range of control signals obtained by the designed calculation method of the feedforward controller, and all the control signal combinations in the range can complete the track tracking task. Assuming that the control signal is disturbed but still within the space shown, the control task can still be done, thus rendering the feed forward controller of the present invention robust.
The dark lines in fig. 4 indicate that a group of control signals are arbitrarily taken from the space and are fed into the established six-muscle double-joint system, and the dotted lines in fig. 5 show that the tracking effect of the method is good.
Step S50, to sum t j The error between the desired joint angle and the response joint angle at that moment is used as the input signal of a PID controller, and the PID controller t j+1 And adjusting the output signal of the robot feedforward controller by the output signal at the moment.
PID controller t j+1 An output signal at a time is expressed by equation (6):
wherein k is p Is a proportionality coefficient, k i Is an integral coefficient, k d In order to be the differential coefficient,is a PID controller t j The output signal of the time of day is,represents t j Desired joint angle at any momentAnd responsive joint angleThe error of (a) is detected,representing joint angular velocity errors.
Step S60, based on the adjusted t j+1 And controlling the robot to move according to the expected track by the control signal at the moment.
The invention verifies the effect of PID in practical application by a simulation method, as shown in FIG. 6, which is a robot simulation control schematic diagram after PID control is added in one embodiment of the nonlinear musculoskeletal robot control method of the invention, and the built human-simulated muscle arm model has 7 degrees of freedom: respectively 3 degrees of freedom of a shoulder joint, 1 degree of freedom of an axis joint, 1 degree of freedom of forearm torsion and 2 degrees of freedom of a wrist joint. The angular displacement of the ith joint at the time t is recorded asThe corresponding motion state space is as shown in equation (7):
fig. 6 shows that the almost completely overlapped lines at the tail end of the mechanical arm are the expected track at the tail end of the mechanical arm and the real-time response track at the tail end of the mechanical arm, fig. 7 shows a real-time error diagram of the tail end position in the simulation result of the robot after adding the PID control according to an embodiment of the nonlinear musculoskeletal robot control method of the present invention, and it can be seen from the diagram that the method of the present invention can realize the track tracking under the real-time error of 4mm, and it can be seen that the mechanical arm model controlled by the method of the present invention has relatively stable motion.
Although the foregoing embodiments have described the steps in the foregoing sequence, those skilled in the art will understand that, in order to achieve the effect of the present embodiment, different steps are not necessarily performed in such a sequence, and may be performed simultaneously (in parallel) or in an inverse sequence, and these simple variations are within the scope of the present invention.
The nonlinear musculoskeletal robot control system of a second embodiment of the invention includes the following modules:
the muscle force acquisition module is configured to calculate joint torque of the robot according to the inverse of Lagrange dynamics and the input of a feedforward controller, and acquire the muscle force of each muscle of the robot by combining with a moment arm matrix of the robot; the feed forward controller input comprises a desired joint angle and a desired joint angular velocity;
the muscle length acquisition module is configured to acquire the tendon length of each muscle of the robot based on the functional relationship between the muscle force and the tendon length of the Thelen2003 muscle dynamics model, and acquire the length of each muscle of the robot according to the geometric distribution relationship of the muscle of the robot on the skeleton;
the muscle fiber change speed acquisition module is configured to calculate the muscle fiber length of each muscle of the robot based on the length of each muscle and the length of the tendon of the robot, and acquire the muscle fiber change speed through backward difference;
a feedforward controller building module configured to build a robot feedforward controller based on a muscle force, a muscle fiber length, and a change speed of each muscle of the robot;
a feedback adjustment module configured to adjust t j Desired joint angle at any momentAnd responsive joint angleAs an input signal to a PID controller by means of which t j+1 Adjusting the output signal of the robot feedforward controller by the output signal at the moment;
a robot control module configured to adjust t based on the measured value j+1 And controlling the robot to move according to the expected track by the control signal at the moment.
It can be clearly understood by those skilled in the art that, for convenience and brevity of description, the specific working process and related description of the system described above may refer to the corresponding process in the foregoing method embodiments, and will not be described herein again.
It should be noted that, the nonlinear musculoskeletal robot control system provided in the above embodiment is only exemplified by the division of the above functional modules, and in practical applications, the above functions may be allocated to different functional modules according to needs, that is, the modules or steps in the embodiment of the present invention are further decomposed or combined, for example, the modules in the above embodiment may be combined into one module, or may be further split into a plurality of sub-modules, so as to complete all or part of the above described functions. The names of the modules and steps involved in the embodiments of the present invention are only for distinguishing the modules or steps, and are not to be construed as unduly limiting the present invention.
A nonlinear musculoskeletal robot control apparatus of a third embodiment of the present invention includes:
at least one processor; and a memory communicatively coupled to at least one of the processors; wherein the memory stores instructions executable by the processor for execution by the processor to implement the non-linear musculoskeletal robot control method described above.
It can be clearly understood by those skilled in the art that, for convenience and brevity of description, the specific working processes and related descriptions of the storage device and the processing device described above may refer to the corresponding processes in the foregoing method embodiments, and are not described herein again.
Those of skill in the art will appreciate that the various illustrative modules, method steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware, computer software, or combinations of both, and that programs corresponding to the software modules, method steps may be located in Random Access Memory (RAM), memory, Read Only Memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art. To clearly illustrate this interchangeability of electronic hardware and software, various illustrative components and steps have been described above generally in terms of their functionality. Whether such functionality is implemented as electronic hardware or software depends upon the particular application and design constraints imposed on the solution. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.
The terms "first," "second," and the like are used for distinguishing between similar elements and not necessarily for describing or implying a particular order or sequence.
The terms "comprises," "comprising," or any other similar term are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus.
So far, the technical solutions of the present invention have been described in connection with the preferred embodiments shown in the drawings, but it is easily understood by those skilled in the art that the scope of the present invention is obviously not limited to these specific embodiments. Equivalent changes or substitutions of related technical features can be made by those skilled in the art without departing from the principle of the invention, and the technical scheme after the changes or substitutions can be within the protection scope of the invention.
Claims (10)
1. A nonlinear musculoskeletal robot control method, the method comprising:
step S10, according to the inverse of Lagrange dynamics, the input of a feedforward controller is carried out, the joint torque of the robot is calculated, and the muscle force of each muscle of the robot is obtained by combining with the moment arm matrix of the robot; the feed forward controller input comprises a desired joint angle and a desired joint angular velocity;
step S20, acquiring the tendon length of each muscle of the robot based on the functional relationship between the muscle force and the tendon length of the Thelen2003 muscle dynamics model, and acquiring the length of each muscle of the robot according to the geometric distribution relationship of the muscle of the robot on the skeleton;
step S30, calculating the muscle fiber length of each muscle of the robot based on the length of each muscle and the length of the tendon of the robot, and obtaining the muscle fiber change speed through backward difference;
step S40, constructing a robot feedforward controller based on the muscle force, the muscle fiber length and the change speed of each muscle of the robot;
step S50, to get t j The error between the desired joint angle and the response joint angle at that moment is used as the input signal of a PID controller, and the PID controller t j+1 Adjusting the output signal of the robot feedforward controller by the output signal at the moment;
step S60, based on the adjusted t j+1 And controlling the robot to move according to the expected track by the control signal at the moment.
2. The nonlinear musculoskeletal robot control method of claim 1, wherein joint torques of the robot are calculated by:
wherein tau represents the joint torque of the robot, M represents a link inertia matrix, C represents a Coriolis force and centrifugal force matrix, G represents a gravity change matrix, theta,Andrepresenting the desired joint angle, the desired joint angular velocity and the desired joint angular acceleration, respectively.
3. The nonlinear musculoskeletal robot control method of claim 1, wherein the muscle force of each muscle of the robot is calculated by:
F TD =W -1 τ
wherein, F TD Representing muscle force, tau representing joint torque of the robot, W representing moment arm matrix, W -1 Is an inverse matrix of W。
4. The nonlinear musculoskeletal robot control method of claim 1, wherein a muscle fiber length of each muscle of the robot is calculated by:
l M =(l MT -l TD )/cosα
wherein l M Represents the length of the muscle fiber,/ MT Represents the length of the muscle,/ TD Represents the tendon length and α represents the pinnate angle.
5. The nonlinear musculoskeletal robot control method of claim 1, wherein the muscle fiber change speed is calculated by:
6. The nonlinear musculoskeletal robot control method of claim 1, wherein the robot feedforward controller activates the signal model as:
wherein a (t) is a control signal of the robot feedforward controller at the time t, l M (t) andrespectively representing the muscle fiber at time tLength of dimension and speed of change of muscle fiber, F TD (t) represents the muscle force at time t, F PE Force generated by passive elongation of muscle fibers, f l Representing the force-length coefficient, f v Representing the force-velocity coefficient.
7. The nonlinear musculoskeletal robot control method according to claim 1, characterized in that the PID controller t j+1 An output signal at a time, represented as:
wherein k is p Is a proportionality coefficient, k i Is an integral coefficient, k d In order to be the differential coefficient,is a PID controller t j The output signal of the time of day,represents t j Desired joint angle at any momentAnd responsive joint angleThe error of (a) is detected,representing joint angular velocity errors.
8. The nonlinear musculoskeletal robot control method of claim 6, in which F PE 、f l 、f v Obtained by functional relationship of Thelen2003 muscle dynamics model.
9. A nonlinear musculoskeletal robot control system, the system comprising:
the muscle force acquisition module is configured to calculate joint torque of the robot according to the inverse of Lagrange dynamics and the input of a feedforward controller, and acquire the muscle force of each muscle of the robot by combining with a moment arm matrix of the robot; the feed forward controller input comprises a desired joint angle and a desired joint angular velocity;
the muscle length acquisition module is configured to acquire the tendon length of each muscle of the robot based on the functional relationship between the muscle force and the tendon length of the Thelen2003 muscle dynamics model, and acquire the length of each muscle of the robot according to the geometric distribution relationship of the muscle of the robot on the skeleton;
the muscle fiber change speed acquisition module is configured to calculate the muscle fiber length of each muscle of the robot based on the length of each muscle and the length of the tendon of the robot, and acquire the muscle fiber change speed through backward difference;
a feedforward controller construction module configured to construct a robot feedforward controller based on the muscle force, muscle fiber length and change speed of each muscle of the robot;
a feedback adjustment module configured to adjust t j Desired joint angle at any momentAnd responsive joint angleAs an input signal to a PID controller by means of which t j+1 Adjusting the output signal of the robot feedforward controller by the output signal at the moment;
a robot control module configured to adjust t based on the measured value j+1 And controlling the robot to move according to the expected track by the control signal at the moment.
10. A nonlinear musculoskeletal robot control apparatus, comprising:
at least one processor; and
a memory communicatively coupled to at least one of the processors; wherein the content of the first and second substances,
the memory stores instructions executable by the processor for execution by the processor to implement the nonlinear musculoskeletal robot control method of any one of claims 1-8.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110562679.3A CN113199460B (en) | 2021-05-24 | 2021-05-24 | Nonlinear musculoskeletal robot control method, system and device |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110562679.3A CN113199460B (en) | 2021-05-24 | 2021-05-24 | Nonlinear musculoskeletal robot control method, system and device |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113199460A CN113199460A (en) | 2021-08-03 |
CN113199460B true CN113199460B (en) | 2022-09-02 |
Family
ID=77022877
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110562679.3A Active CN113199460B (en) | 2021-05-24 | 2021-05-24 | Nonlinear musculoskeletal robot control method, system and device |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113199460B (en) |
Families Citing this family (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114310858A (en) * | 2021-12-22 | 2022-04-12 | 中国农业银行股份有限公司山东省分行 | Agile robot |
CN114700942A (en) * | 2022-03-25 | 2022-07-05 | 宁波慈溪生物医学工程研究所 | Upper limb robot optimization method and device and upper limb robot |
CN114918914B (en) * | 2022-04-26 | 2024-03-22 | 中国科学院自动化研究所 | Simulation control system and simulation device for human musculature |
CN115543094B (en) * | 2022-11-28 | 2023-05-30 | 杭州轻宇宙科技有限公司 | Interaction method, system and electronic equipment of digital twin virtual person and human body |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108446442B (en) * | 2018-02-12 | 2020-03-13 | 中国科学院自动化研究所 | Method for simplifying upper limb model of neuromuscular-like skeletal robot |
CN108785997B (en) * | 2018-05-30 | 2021-01-08 | 燕山大学 | Compliance control method of lower limb rehabilitation robot based on variable admittance |
CN109044352B (en) * | 2018-06-22 | 2021-03-02 | 福州大学 | Method for determining artificial intelligence input variable for predicting human body joint moment |
CN112757275B (en) * | 2020-12-30 | 2022-02-25 | 中国科学院自动化研究所 | Method, system and device for controlling musculoskeletal system based on speed precision balance |
-
2021
- 2021-05-24 CN CN202110562679.3A patent/CN113199460B/en active Active
Also Published As
Publication number | Publication date |
---|---|
CN113199460A (en) | 2021-08-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113199460B (en) | Nonlinear musculoskeletal robot control method, system and device | |
Wang et al. | Uncalibrated visual tracking control without visual velocity | |
CN107530879B (en) | Multi-axis machine simulator, design support device for operation command device, design support device for motor control device, and motor capacity selection device | |
CN108714896A (en) | A kind of robot flexibility torque feed forward control method based on kinetics of deformable bodies model | |
Guida et al. | Modeling techniques for kinematic analysis of a six-axis robotic arm | |
CN110154024B (en) | Assembly control method based on long-term and short-term memory neural network incremental model | |
CN108381555A (en) | The design method of level controller under a kind of flight Work robot redundancy mechanical arm | |
CN108446442B (en) | Method for simplifying upper limb model of neuromuscular-like skeletal robot | |
CN100435050C (en) | Control method for acrobatic motion of pendulums of under-actuated double pendulum system | |
Fadli et al. | Human gesture imitation on NAO humanoid robot using kinect based on inverse kinematics method | |
Long et al. | Robotic arm simulation by using matlab and robotics toolbox for industry application | |
CN110909438B (en) | Dynamic model-based control method for light-load joint type parallel robot | |
Bratchikov et al. | Development of digital twin for robotic arm | |
CN110253573A (en) | A kind of system ambiguous control method of flexible mechanical arm based on series-parallel estimation model | |
CN1440856A (en) | Internet control system for remotely controlling robots to play chess | |
CN116533247A (en) | Method for realizing multi-mode grabbing task of mechanical arm based on DMP and convolutional neural network | |
CN114800523B (en) | Mechanical arm track correction method, system, computer and readable storage medium | |
Zhou et al. | Coordination control of dual‐arm modular robot based on position feedback using Optotrak3020 | |
Sáenz Zamarrón et al. | Educational robot arm development | |
CN117325143A (en) | Redundant mechanical arm singular position type lower kinematics optimization method | |
JP2020179440A (en) | Display method | |
CN114253136B (en) | Combined satellite simulator control method based on discrete high-order full-drive system | |
Raafat et al. | Introduction to Robotics-Mathematical Issues | |
CN115981144B (en) | Global nonlinear sliding mode finite time control method for gear inspection robot | |
CN115533922B (en) | Pose relation calibration method and device, computer equipment and readable storage medium |
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 |