CN106547989A - Position inner ring impedance control algorithm with flexibility of joint/armed lever flexible mechanical arm - Google Patents

Abstract

The invention provides a kind of impedance control algorithm of the position inner ring with flexibility of joint and armed lever flexible mechanical arm, belongs to intelligent algorithm optimization field, and realizes the motion simulation of two flexibility of linking rod joints/flexibility armed lever robot contact operation.This method carries out armed lever discrete analysis using hypothesis modal method, and adopts Kane establishing equation Manipulator Dynamics, design attitude inner ring impedance control while building control emulation platform based on Simulink, to realize robot contact operational movement.Simulation result shows that the presence of flexible characteristic can cause the deformation of armed lever, designed impedance controller realize the track following of flexible mechanical arm and contact force control.

Description

Position Inner Loop Impedance Control Algorithm for Manipulators with Joint Flexibility/Roll Flexibility

technical field

The invention belongs to the field of intelligent algorithm optimization, and in particular relates to a position inner loop impedance control considering joint flexibility and flexible contact operation of an arm.

Background technique

With the rapid development of space technology, the use of robotic arms to assist astronauts in completing various operations has become a research hotspot. The relatively large length-to-width ratio of space robotic arms is generally regarded as a slender rod, which will produce elastic deformation during operation. The presence of flexible components such as harmonic reducers and torque sensors in the joints makes the joints deform flexibly. The elastic deformation coupling of joint flexibility/arm flexibility during movement will cause difficulties in the active control of the manipulator. It is necessary to design a controller that considers joint flexibility/arm flexibility.

Force-position hybrid control is to realize position and force control simultaneously in an independent manner. Theoretically, the position and force of the manipulator are two orthogonal subspaces that are complementary, and a diagonal matrix is used to divide the space into two subspaces. The normal direction on the surface of the object that the arm touches is the force control subspace, at this time only force control is performed without position control; while the tangent direction of the surface is the position control subspace, only position control is performed without force control. However, when the position control and force control are considered separately, the force-position hybrid control method requires a sufficiently detailed description of the task, and the switching of the control direction must be considered during the implementation process, that is, a faster calculation speed and a larger calculation amount are required. The theory of force-position hybrid control is clear, but it is difficult to implement.

As an effective compliance control of manipulators, impedance control has been gradually applied to various fields. Impedance control provides a framework for unified control of free motion space and constrained motion space. It does not directly control the desired position and force, but achieves the purpose of compliant control by adjusting the dynamic relationship between the position deviation and force at the end of the mechanical arm. Simultaneously convert the force feedback signal into position and velocity corrections. Impedance control does not require precise off-line task planning. It has strong adaptability to the mutual conversion performance between free motion and constrained motion, and has strong robustness to system disturbances and uncertainties. Its task planning volume and The amount of real-time calculation is small, and there is no need to switch control modes. It is better than force-position hybrid control in many engineering applications.

Many scholars have conducted research on impedance control. In the existing research, there are conventional impedance control algorithms designed for rigid manipulators; adaptive impedance control is designed for environmental stiffness and position uncertainty, but the application is limited to flexible link manipulators; at the same time Aiming at the real-time adjustment of impedance parameters and other issues, there are studies and designs of fuzzy adaptive impedance controllers. However, there have been no related reports on space manipulators with both joint flexibility and arm flexibility.

Contents of the invention

In order to solve the above problems, this patent designs a position inner loop impedance control algorithm for the dynamic model of the flexible joint and the flexible arm.

The dynamic model of joint flexibility/arm flexibility is established based on Kane equation and hypothetical mode method.

According to the above simplified description, the Kane method is used to establish the dynamic model, and the generalized coordinates of the flexible joint/flexible arm are selected as y={α θ q}, where α=θ _m /N, θ _m is the motor Rotation angle, N is the reduction ratio, θ, q represent joint rotation angle and modal coordinates, respectively.

Then the generalized inertial force of the system can be written as:

The generalized active force of the system can be written as:

Among them, n is the number of degrees of freedom of the system, γ represents the number of modal coordinates, Indicates the deviating velocity of any point on the arm to the generalized coordinates, represents the acceleration at any point on the arm, ω represents the deformation at any point, τ _m represents the driving torque of the motor, k represents the stiffness of the linear torsion spring, ε=α-θ represents the linear torsional deformation angle of the joint, EI represents the bending stiffness of the section, F _yi represents the generalized active force generated by the motor drive force, the internal force of the flexible joint, and the internal force of the flexible arm, respectively, represents the generalized inertial force.

Then according to the generalized active force and the generalized inertial force:

After removing the high-order coupling terms, the kinetic equation can be written as follows:

Wherein, J _m represents the moment of inertia of the joint motor, τ _ext =J ^T F _ext , and J ^T is the transposition of the Jacobian matrix.

Research on Impedance Control Algorithm of Inner Loop Impedance Control of Manipulator Position Considering Joint Flexibility and Arm Flexibility

Set up the second-order target impedance control model:

Among them: M _d , is the target inertia, B _d is the target damping, and K _d is the target stiffness. E=X _d -X represents the position error, X _d represents the expected target position of the end, X represents the actual position, F _ext =K _e (X _d -X) is the force on the end of the robot in contact with the environment, and K _e is the environment stiffness. For the above dynamics, the formula for joint rotation angle can be written:

where p=p(θ,q).

Then it can be written as an expression only about the joint rotation angle:

Through the conversion relationship between the joint space and the operation space, the control law of the position inner loop impedance can be finally obtained:

in: x _f = x _d -E, β ₃ = J _m k ^-1 J ^T (p),

Description of drawings

Fig. 1 Structure diagram of position inner loop impedance control

Figure 2 The trajectory of the end of the two-degree-of-freedom manipulator

Figure 3 The trajectory error of the two-degree-of-freedom manipulator in the x direction

Figure 4 Trajectory error in the y direction of the two-degree-of-freedom manipulator

Figure 5 Two-degree-of-freedom mechanical arm x-direction force control

Figure 6 Two-degree-of-freedom manipulator y-direction force control

detailed description

Taking the space two-link flexible manipulator as the simulation research object, the impedance control algorithm simulation platform is built based on the Simulink platform, and the response of the manipulator to different control algorithms is compared and analyzed.

Based on the System Function function, the position inner loop impedance control algorithm is built in Matlab/Simulink, and the fourth-order Runge-Kutta method with fixed step size is used. The simulation step size is set to 0.01s and the simulation time is 5s. The initial joint position of the manipulator is θ ₁ =π/2, θ ₂ =-π/2, and the initial angular velocity of the joint is 0. During the simulation process, the expected movement trajectory of the manipulator is as follows: The desired contact force is 5N.

Take k _p as 5, k _d as 12, and the control parameters are:

Finally, the simulation diagram can be obtained, as shown in Figure 1, which represents the actual trajectory of the end of the manipulator under the control environment; Figure 2 and Figure 3 respectively compare the trajectory tracking error under the position inner loop impedance control algorithm. Figure 4 and Figure 5 respectively compare the force control under the position inner loop impedance control, and finally it can be seen that the designed control algorithm can realize joint flexibility/arm flexibility control, which proves the effectiveness of the control algorithm.

Claims (1)

1. A position inner loop impedance controller considering joint flexibility and arm bar flexibility, is characterized in that, comprising: Research on Impedance Control Algorithm of Inner Loop Impedance Control of Manipulator Position Considering Joint Flexibility and Arm Flexibility Set the target impedance control model: Among them: M _d is the target inertia, B _d is the target damping, and K _d is the target stiffness. E=X _d -X represents the position error, X _d represents the expected target position of the end, X represents the actual position, F _ext =K _e (X _d -X) is the force on the end of the robot in contact with the environment, and K _e is the environment stiffness. For the above dynamics, the formula for joint rotation angle can be written: Then it can be written as an expression only about the joint rotation angle: $\begin{array}{c}{\mathrm{<span\; class="notranslate">\; J</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; m</span>}}\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; m</span>}}\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; m</span>}}\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; C</span>}<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; ,</span>\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; J</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}\mathrm{<span\; class="notranslate">\; x</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>\underset{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; J</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; )</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; f</span>}}}_{\mathrm{<span\; class="notranslate">\; e</span>}\mathrm{<span\; class="notranslate">\; x</span>}\mathrm{<span\; class="notranslate">\; t</span>}}}<span\; class="notranslate">\; -</span>\underset{<span\; class="notranslate">\; \&OverBar;</span>}{{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; J</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; )</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; f</span>}}}_{\mathrm{<span\; class="notranslate">\; e</span>}\mathrm{<span\; class="notranslate">\; x</span>}\mathrm{<span\; class="notranslate">\; t</span>}}}<span\; class="notranslate">\; +</span>\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; m</span>}\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; C</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; ,</span>\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\\ \mathrm{<span\; class="notranslate">\; K</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; J</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}\mathrm{<span\; class="notranslate">\; x</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}\end{array}$ Through the conversion relationship between the joint space and the operation space, the control law of the position inner loop impedance can be finally obtained: $\mathrm{<span\; class="notranslate">\; \&tau;</span>}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; J</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; MJ</span>}}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; u</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}\mathrm{<span\; class="notranslate">\; x</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>\underset{<span\; class="notranslate">\; \&OverBar;</span>}{{\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; f</span>}}}_{\mathrm{<span\; class="notranslate">\; e</span>}\mathrm{<span\; class="notranslate">\; x</span>}\mathrm{<span\; class="notranslate">\; t</span>}}}<span\; class="notranslate">\; -</span>\underset{<span\; class="notranslate">\; \&OverBar;</span>}{{\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}{\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; f</span>}}}_{\mathrm{<span\; class="notranslate">\; e</span>}\mathrm{<span\; class="notranslate">\; x</span>}\mathrm{<span\; class="notranslate">\; t</span>}}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; G</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; ;</span>$ in: x _f = x _d -E, β ₃ = J _m k ^{- 1} J ^T (p),