CN111702753B - Redundant mechanical arm inverse priority impedance control system and control method - Google Patents

Redundant mechanical arm inverse priority impedance control system and control method Download PDF

Info

Publication number
CN111702753B
CN111702753B CN202010369736.1A CN202010369736A CN111702753B CN 111702753 B CN111702753 B CN 111702753B CN 202010369736 A CN202010369736 A CN 202010369736A CN 111702753 B CN111702753 B CN 111702753B
Authority
CN
China
Prior art keywords
control
task
manipulator
priority
inverse
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
Application number
CN202010369736.1A
Other languages
Chinese (zh)
Other versions
CN111702753A (en
Inventor
刘海燕
苏宇
李敏斯
林春兰
吴雪颖
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangxi University of Science and Technology
Original Assignee
Guangxi University of Science and Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Guangxi University of Science and Technology filed Critical Guangxi University of Science and Technology
Priority to CN202010369736.1A priority Critical patent/CN111702753B/en
Publication of CN111702753A publication Critical patent/CN111702753A/en
Application granted granted Critical
Publication of CN111702753B publication Critical patent/CN111702753B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)

Abstract

The invention discloses a redundant mechanical arm reverse priority impedance control system and a control method. The manipulator redundancy mechanical arm reverse priority impedance control device belongs to the technical field of manipulator redundancy mechanical arm reverse priority impedance control, and facilitates recognition of grabbed objects. The manipulator comprises a movable moving platform, a manipulator and a control console for controlling the manipulator; the manipulator comprises a mechanical arm, a mounting seat, a vertical column, an output gripper and a vertical cylinder; the mechanical arm comprises a vertical lifting moving platform, an arm section I, an arm section II, an arm section III and an arm section IV; a vertical rail is arranged on the left surface of the vertical column, and the vertical lifting mobile platform is vertically and slidably arranged on the vertical rail up and down; the lower end of the vertical column is fixedly connected to the upper surface of the mounting seat, the cylinder seat of the vertical cylinder is fixedly connected to the upper surface of the mounting seat positioned on the left side of the vertical track, and the upper end of the telescopic rod of the vertical cylinder is fixedly connected to the lower surface of the vertical lifting moving table; the mounting seat is fixed on the mobile platform. The front end of the manipulator is provided with a camera.

Description

Redundant mechanical arm inverse priority impedance control system and control method
Technical Field
The invention relates to the technical field of reverse priority impedance control of manipulator redundant manipulator, in particular to a reverse priority impedance control system and a reverse priority impedance control method for the redundant manipulator.
Background
The control method adopted by the industrial robot at present is that each joint on the manipulator is taken as an independent servo mechanism, namely each axis corresponds to a server, and each server is controlled by a bus and is uniformly controlled and coordinated by a controller;
the mechanical arm with six degrees of freedom is the mechanical arm with the minimum degree of freedom for finishing space positioning, and the mechanical arms with more than six degrees of freedom are uniformly called redundant mechanical arms;
the existing mechanical arm impedance control method of the mechanical arm cannot realize the expected impedance control task under different hierarchical structures, so that the method which can enable the redundant mechanical arm of the mechanical arm to realize the expected impedance control task under different hierarchical structures is very necessary.
Disclosure of Invention
The invention aims to overcome the defect that the expected impedance control task cannot be realized by the existing impedance control method of the mechanical arm under different hierarchical structures, and provides a method which can control the balance of a mechanical arm, is convenient for the mechanical arm to move and is convenient for identifying and grabbing objects; and the other is a redundant mechanical arm reverse priority impedance control system and a redundant mechanical arm reverse priority impedance control method which can enable the redundant mechanical arm of the mechanical arm to realize the expected impedance control task under different hierarchical structures.
The technical problem is solved by the following technical scheme:
the redundant mechanical arm reverse priority impedance control system comprises a mechanical arm and a control console for controlling the mechanical arm; the device also comprises a movable moving platform; the manipulator comprises a mechanical arm, a mounting seat, a vertical column, an output gripper and a vertical cylinder; the mounting seat is fixed on the mobile platform;
the mechanical arm comprises a vertical lifting moving platform, an arm section I, an arm section II, an arm section III and an arm section IV;
a vertical rail is arranged on the left surface of the vertical column, and the vertical lifting mobile platform is vertically arranged on the vertical rail in a sliding manner; the lower end of the vertical column is fixedly connected to the upper surface of the mounting seat, the cylinder seat of the vertical cylinder is fixedly connected to the upper surface of the mounting seat positioned on the left side of the vertical track, the telescopic rod of the vertical cylinder is vertically arranged upwards, and the upper end of the telescopic rod of the vertical cylinder is fixedly connected to the lower surface of the vertical lifting mobile platform; the vertical lifting mobile platform can move up and down along a vertical track under the driving of a telescopic rod of a vertical cylinder to form a first degree of freedom;
the arm section I comprises an A1 section pipe and an A2 section pipe which is telescopically connected in a left pipe orifice of the A1 section pipe, a first cylinder with a telescopic rod horizontally arranged towards the left is fixedly arranged at the right end in the A1 section pipe, and the telescopic rod of the first cylinder is fixedly connected at the right end of the A2 section pipe;
the arm section II comprises a section B1 pipe and a section B2 pipe which is telescopically connected in a left pipe orifice of the section B1 pipe, a second cylinder with a telescopic rod horizontally arranged towards the left is fixedly arranged at the right end in the section B1 pipe, and the telescopic rod of the second cylinder is fixedly connected at the right end of the section B2 pipe;
a first horizontal rotating shaft driven by a first speed reduction motor is arranged at the left end of the vertical lifting moving platform, and the right end of the A1 section pipe is fixedly connected to the first horizontal rotating shaft, so that the first arm section can horizontally rotate to form a second degree of freedom; a first electromagnetic brake capable of controlling the first horizontal rotating shaft to rotate is further arranged on the first horizontal rotating shaft;
the left end of the section A2 pipe is provided with a second horizontal rotating shaft driven by a second speed reducing motor, and the right end of the section B1 pipe is fixedly connected to the second horizontal rotating shaft, so that the arm section II can horizontally rotate to form a third degree of freedom; a second electromagnetic brake capable of controlling the second horizontal rotating shaft to rotate is further arranged on the second horizontal rotating shaft;
the left end of the section B2 pipe is provided with a third horizontal rotating shaft driven by a third speed reducing motor, and the right end of the arm section III is fixedly connected to the third horizontal rotating shaft, so that the arm section III can horizontally rotate to form a fourth degree of freedom; a third electromagnetic brake capable of controlling the third horizontal rotating shaft to rotate is further arranged on the third horizontal rotating shaft;
the left end of the arm section III is provided with a first transverse vertical rotating shaft which is driven by a fourth speed reducing motor and can rotate on the left vertical surface and the right vertical surface, and the right end of the arm section IV is fixedly connected to the first transverse vertical rotating shaft, so that the arm section IV can vertically rotate on the left vertical surface and the right vertical surface to form a fifth degree of freedom; a fourth electromagnetic brake capable of controlling the first transverse vertical rotating shaft to rotate is further arranged on the first transverse vertical rotating shaft;
the left end of the arm section four is provided with a first longitudinal vertical rotating shaft which is driven by a fifth speed reducing motor and can rotate on the front vertical surface and the rear vertical surface, and the right end of the output gripper is fixedly connected to the first longitudinal vertical rotating shaft, so that the right end of the output gripper can vertically rotate on the front vertical surface and the rear vertical surface to form a sixth degree of freedom; a fifth electromagnetic brake capable of controlling the first longitudinal vertical rotating shaft to rotate is further arranged on the first longitudinal vertical rotating shaft;
the A2 section of pipe can stretch and move left and right in the A1 section of pipe to form a seventh degree of freedom under the drive of a telescopic rod of the first cylinder;
the section B2 pipe can stretch and move left and right in the section B1 pipe to form an eighth degree of freedom under the drive of a telescopic rod of the second cylinder;
the left end of a first horizontal pipe is horizontally and fixedly connected to the right surface of the vertical column, a balance adjusting block is arranged in the first horizontal pipe in a sliding mode from left to right, a balance adjusting cylinder with a telescopic rod facing right horizontally is fixedly connected to the left end of the first horizontal pipe, and the right end of the telescopic rod of the balance adjusting cylinder is fixedly connected to the balance adjusting block;
the control end of No. one electromagnetic brake, the control end of No. two electromagnetic brakes, the control end of No. three electromagnetic brakes, the control end of No. four electromagnetic brakes, the control end of No. five electromagnetic brakes, the control end of a gear motor, the control end of No. two gear motor, the control end of No. three gear motor, the control end of No. four gear motor, the control end of No. five gear motor, the control end of a cylinder, the control end of No. two cylinders, the control end of balance adjustment cylinder and the control end of vertical cylinder are respectively control connection on the control cabinet.
The outer surface of the first longitudinal vertical rotating shaft is fixedly sleeved with an annular ring, a first moving block which is driven by a surrounding motor and can move along the annular ring is arranged on the annular ring, and a camera is arranged on the first moving block; the control end of the camera and the control end of the surrounding motor are respectively connected with the console.
The object balance adjusting cylinder is convenient to recognize and grab, the balance of the vertical column can be controlled by controlling the left and right movement of the balance adjusting block, and the balance of the manipulator is further controlled. The manipulator is convenient to move. Because some mechanical arms of the invention have eight degrees of freedom, the invention has good flexibility and high reliability and is easy to complete control tasks.
A redundant mechanical arm reverse priority impedance control method comprises the following steps:
step 1, establishing a kinematic model of the redundant mechanical arm, and giving a gradient direction strategy of a zero space vector of the redundant mechanical arm;
step 2, establishing a task priority solving strategy for obtaining a singularity elimination algorithm through a singular robust solution;
step 3, establishing a singular robust solution inverse kinematics analysis model;
step 4, establishing a reverse priority control strategy of the multi-task redundant mechanical arm;
step 5, simplifying a reverse control equation of the redundant mechanical arm with the main task and the secondary task;
step 6, establishing a reverse priority control strategy of the manipulator;
step 7, solving the relation between the external force and the joint acceleration in the reverse priority impedance control of the manipulator by adopting the joint speed, thereby obtaining the reverse priority impedance control guarantee of the manipulator;
and 8, expanding the reverse priority calculation of the position control space to the reverse priority calculation of the force control space, thereby obtaining the overall framework of the speed-level reverse priority impedance control of the manipulator.
The motion of the redundant mechanical arm in the joint space is derived according to the reverse order; then, cartesian impedance control and inverse-priority impedance control are combined, the problem of inverse hierarchical impedance control is solved, and Cartesian impedance control behaviors are divided into high-priority impedance control and low-priority impedance control. The high-priority impedance control task does not interfere with the low-priority impedance control task, and the motion in the joint space is influenced in the reverse order and needs to work in a corresponding projection operator; and finally, a high-priority impedance control task is realized, and deformation caused by singularity possibly occurring in the low-priority impedance control task is avoided. Thus, the proposed inverse-priority impedance control method enables the redundant robotic arm to achieve the desired impedance control task at the appropriate hierarchical structure.
Preferably, a redundant mechanical arm kinematics model is established, and a gradient direction strategy implementation process of a redundant mechanical arm null-space vector is given as follows:
the pose and the speed of the end effector in the Cartesian space are defined as x,
Figure GDA0003990435810000031
The angular position and angular velocity of the joint space are q,
Figure GDA0003990435810000032
J is a Jacobian matrix of the robot with n degrees of freedom, wherein x belongs to R n ，
Figure GDA0003990435810000033
Figure GDA0003990435810000034
J∈R mn (ii) a The positive kinematic equation for a redundant degree of freedom robotic arm can be described by:
Figure GDA0003990435810000035
equation (1) is also referred to as the kinematic velocity model of the mechanical arm;
considering the solution of the least squares method, the optimal problem can be listed as:
Figure GDA0003990435810000036
the solution of formula (1) can be optimized by finding the best
Figure GDA0003990435810000037
To solve the problem;
Figure GDA0003990435810000038
thus, the pseudo-inverse solution of equation (1) can be expressed as:
Figure GDA0003990435810000039
in the formula J + Pseudo-inverse of the Jacobian matrix
I-identity matrix
Figure GDA00039904358100000310
-arbitrary null space vector
Figure GDA0003990435810000041
Minimum norm solution, defining hand movements
Figure GDA0003990435810000042
Homogeneous solution, no action at the end
Equation (4) represents the position and attitude control of the end effector; adding any residual error into the formula (4) to obtain a general expression containing a null space; the multi-task optimization can be realized on a zero vector by using the equation;
however, the above equation ignores the ill-conditioned state of the jacobian matrix; the regularization equation may be modified by adding additional regularization values,
Figure GDA0003990435810000043
wherein λ ≧ 0 is a weighting matrix,
Figure GDA0003990435810000044
is a weighting coefficient, and satisfies
Figure GDA0003990435810000045
The solution to the above equation can be expressed as:
Figure GDA0003990435810000046
equation (7) is also referred to as a redundant manipulator kinematics model;
the joint limit function of the joint limit gradient direction of the position-dependent scalar index of the null-space vector of the redundant manipulator is as follows:
Figure GDA0003990435810000047
preferably, a task priority solving strategy for obtaining the singularity elimination algorithm through a singular robust solution is established as follows:
in the solution of the redundant mechanical arm of the Jacobian matrix, an optimization task is realized in a null space of a main task; the reverse task kinematics is established on the basis of the forward task kinematics:
x＝J i q i＝1，2 (9)
wherein
Figure GDA0003990435810000048
And
Figure GDA0003990435810000049
shows task1 and task2
The inverse kinematics equation of the redundant manipulator is obtained from expression (9) as follows:
Figure GDA00039904358100000410
task1 is used as a main Task, and Task2 is used as an auxiliary Task; that is to say that the temperature of the molten steel is,
Figure GDA00039904358100000411
is that
Figure GDA00039904358100000412
Is implemented in the null space of (1); the final inverse kinematics expression for the redundant manipulator is as follows:
Figure GDA00039904358100000413
wherein
Figure GDA00039904358100000414
Figure GDA00039904358100000415
Is a projection matrix which gives the applicable range of the secondary task to the primary task;
Figure GDA00039904358100000416
and
Figure GDA00039904358100000417
is the desired commanded speed;
Figure GDA00039904358100000418
is the main task of the method, and the method comprises the following steps of,
Figure GDA00039904358100000419
is a secondary task;
if two related tasks are interdependent, then the corresponding Jacobian matrix is singular; if the task jacobian matrix is singular, the corresponding task is not satisfied; in this case, the jacobian correlation matrix will be singularities, defined as algorithmic singularities;
that is, if
Figure GDA0003990435810000051
Where ρ (·) is the rank of the matrix;
it is clear that the singularity of the algorithm is caused by task conflicts between the secondary tasks and the primary task; in addition, task priority based redundant robotic arm inverse kinematics aims to provide better control over the effectiveness of the primary tasks;
therefore, the position control direction is used as a main task, so that the position ensures the accuracy of the control direction task; then, establishing a task priority solving strategy equation for obtaining a singularity elimination algorithm through singular robust solution:
Figure GDA0003990435810000052
preferably, the singular robust solution inverse kinematics analysis model is established as follows:
the motion singularity can occur based on the Jacobian pseudo-inverse solution, which is caused by the secondary matrix; for the motion singularity problem, a DLS (damped least squares) solution should also be given;
the cost function for the DLS solution can be modified to:
Figure GDA0003990435810000053
thus, the singular robust pseudo-inverse solution of the above equation can be expressed as:
Figure GDA0003990435810000054
equation (15) is a singular robust solution inverse kinematics analysis model, and λ = η is set 2 I, the DLS solution is equivalent to an additional regularization solution, and a scalar value eta balances the task precision and singularity;
for the calculation of the pseudo-inverse solution of the Jacobian matrix, the singular value SVD decomposition form of the Jacobian matrix can be given
J＝U∑V T (16)
Wherein U is E.R m×m ，V∈R n×n ，∑∈R m×n U is a column vector U i A unitary matrix of V is formed by column vectors V i A unitary matrix of composition, Σ being a block matrix of m × n diagonal matrices containing the singular values σ of J i More than or equal to 0 contains n-m zero column vectors in descending order;
Figure GDA0003990435810000055
wherein r is less than or equal to m is the rank of the matrix J;
for motion singularity, with reference to the singular value decomposition SVD required to compute the pseudo-inverse solution, the large resulting joint velocity is due to the smallest singular value rapidly approaching 0, as follows:
Figure GDA0003990435810000056
factor lambda 0 Will affect the singularity, λ 0 The higher the value is, the larger the damping is, and the closer the joint velocity is to the singular point;in addition, the strategy for defining the variable damping factor is also different; we can get
Figure GDA0003990435810000061
From the above equation, we can see that the parameter δ > 0 monitors the smallest singular value.
Preferably, establishing a reverse priority control strategy of the multitask redundant mechanical arm as follows;
introducing an inverse-priority projection matrix
Figure GDA0003990435810000062
The matrix comprises a null space of corresponding elements of the lowest priority l-k-1 task independent of the kth task, so derived
Figure GDA0003990435810000063
Wherein J i|j Is a jacobian matrix associated with all components of the i-th task that are linearly independent of the j-th task;
therefore, the priority derivation formula is as follows:
Figure GDA0003990435810000064
in the above derivation, k = l, l-1, …,1, where l is a positive integer; initial value
Figure GDA0003990435810000065
To give a general form of computation of the linear independent Jacobian matrix J, the inverse augmented Jacobian matrix is defined as:
Figure GDA0003990435810000066
have therein
Figure GDA0003990435810000067
Wherein
Figure GDA0003990435810000068
To represent
Figure GDA0003990435810000069
A row of (2);
therefore, the method is not to be taken,
Figure GDA00039904358100000610
the pseudo-inverse solution of (a) can be expressed as:
Figure GDA00039904358100000611
and
Figure GDA00039904358100000612
wherein, T R Representation matrix
Figure GDA00039904358100000613
Expansion of (2);
the final inverse priority projection can be written as:
Figure GDA00039904358100000614
thus, we can derive the expression of the pseudo-inverse solution:
Figure GDA0003990435810000071
the reverse priority control strategy equation of the multitask redundant mechanical arm is established as follows:
Figure GDA0003990435810000072
preferably, the inverse control equations for a redundant robotic arm having a primary task and a secondary task are simplified as follows:
for a redundant mechanical arm with six degrees of freedom or seven degrees of freedom, the redundant mechanical arm does not have enough degrees of freedom to complete tasks of multiple layers; it is necessary to carry out a dual task priority control; that is, the motion control of the manipulator is a primary task and a secondary task;
the equations for the inverse control of a redundant robotic arm having primary and secondary tasks are as follows
Figure GDA0003990435810000073
The above formula is very different from the previous expression (11), but the algorithmic framework is similar; in the above-mentioned equations, the process of the present invention,
Figure GDA0003990435810000074
is a secondary task that is to be performed,
Figure GDA0003990435810000075
is the main task; the main task is realized in a designated null space of the main task; the core point of reverse priority being a projection matrix
Figure GDA0003990435810000076
Calculating (1);
Figure GDA0003990435810000077
is expressed as formula (30):
Figure GDA0003990435810000078
using similar guides as in previous equations (22) - (28), we can obtain a simplified inverse control equation for a redundant robotic arm with primary and secondary tasks:
Figure GDA0003990435810000079
preferably, the reverse priority control strategy for the manipulator is established as follows:
the dynamics of the manipulator in force control space can be written as:
Figure GDA00039904358100000710
where X is the position in Cartesian space, M (X) is an inertial matrix,
Figure GDA00039904358100000711
non-linear force, F input control force, F e Is the contact force;
furthermore, the input joint moments are obtained on the basis of a conversion of the Jacobian matrix
τ＝J T (q)F (33)
The desired equation of motion of the manipulator in force control space may be defined as follows:
Figure GDA00039904358100000712
wherein M is d And B d Is an inertia and damping matrix; f d Is a command force, F e Is the contact force;
thus, the relationship between the environment and the manipulator response can be written as
Figure GDA00039904358100000713
The combination of the above two equations is as follows
Figure GDA00039904358100000714
From the above equation, it can be seen that if M e 、B e And K e Known, then M d And B d Will affect the system response;
force control enables the manipulator to interact with the environment or human; in addition, in some cases, it is not necessary to implement omnidirectional force control and guarantee omnidirectional force control, that is, sometimes we only want to guarantee the force tracking control accuracy in a certain direction;
it is therefore necessary to perform a hierarchical force control of the manipulator; that is, it is necessary to present a new hierarchical force control framework; from the above equation we can derive the desired hierarchical force control relationship as follows
Figure GDA0003990435810000081
Figure GDA0003990435810000082
The integral of these two equations can be written as
Figure GDA0003990435810000083
Figure GDA0003990435810000084
If the robot end effector is able to track the desired cartesian velocity of
Figure GDA0003990435810000085
And
Figure GDA0003990435810000086
accurate force control of the manipulator can be achieved; the relation between the Cartesian velocity and the joint velocity should be based on inverse priority control; thus, the equation for the inverse priority control strategy for the manipulator can be derived:
Figure GDA0003990435810000087
the joint velocity required by the above equation will ensure the force control of the manipulator; it is worth mentioning that the force control law is only a speed step control law, which relies on an inner speed loop control; if the internal position control effect is good, accurate force control can be realized; because the inner speed ring control can realize low-frequency position tracking, the outer force ring can realize low-frequency force tracking.
Preferably, the relationship between the external force and the joint acceleration in the inverse-priority impedance control of the manipulator is solved by using the joint velocity, so that the inverse-priority impedance control of the manipulator is ensured in the following manner:
when the manipulator implements force control, the manipulator functions as an initiator to some extent, that is, the manipulator is ready to respond to the external environment; when the mechanical arm works as an impedance control model, the mechanical arm passively responds to an external force;
the corresponding impedance relationship between the external force and the joint acceleration can be expressed as
Figure GDA0003990435810000088
Figure GDA0003990435810000089
The reference speed can be expressed as
Figure GDA00039904358100000810
Figure GDA0003990435810000091
Therefore, the inverse priority impedance control of the manipulator guarantees the expression:
Figure GDA0003990435810000092
preferably, the overall framework implementation of extending the inverse-priority computation of the position control space to the inverse-priority computation of the force control space to obtain manipulator velocity-level inverse-priority impedance control is as follows:
hybrid impedance applications are a combination of the two strategies mentioned above, i.e. the cartesian task can be divided into two cases: the first is a position control subspace, in which the impedance control is implemented; the second is a force control subspace in which force control is implemented;
thus selecting a selection matrix; the relationship between the external force and the position response is as follows
Figure GDA0003990435810000093
Figure GDA0003990435810000094
So a simplified form of the desired speed can be expressed as
Figure GDA0003990435810000095
Figure GDA0003990435810000096
Then we get a solution based on reverse priority
Figure GDA0003990435810000097
Considering n-layer tasks, the corresponding impedance control task also belongs to n-layer framework, therefore, the overall framework expression of the manipulator speed level inverse priority impedance control is as follows
Figure GDA0003990435810000098
The expression (52) solves the problem that the reverse priority calculation of the position control space is expanded to the manipulator reverse priority mixed impedance control of the reverse priority calculation of the force control space, so that the redundant manipulator of the manipulator can realize the expected impedance control task under different hierarchical structures.
The invention can achieve the following effects:
the invention can control the balance of the manipulator, the manipulator is convenient to move and recognize and grab objects, and the redundant manipulator of the manipulator can realize expected impedance control tasks under different hierarchical structures.
Drawings
FIG. 1 is a schematic representation of the dynamics of force control of the present invention.
FIG. 2 is a schematic diagram of the dynamics of the impedance control of the present invention.
FIG. 3 is a schematic diagram of the dynamics of the hybrid impedance control of the present invention.
Fig. 4 is a schematic diagram of a seven-degree-of-freedom robot connection structure according to an embodiment of the present invention.
Fig. 5 is a schematic block diagram of a circuit principle connection structure according to an embodiment of the present invention.
Detailed Description
The invention is further described with reference to the following figures and examples.
In an embodiment, the redundant robot arm inverse priority impedance control system, as shown in fig. 4 and 5, includes a robot and a console S31 for controlling the robot; also includes a movable moving platform S41; the manipulator comprises a mechanical arm, a mounting seat S1, a vertical column S2, an output gripper S12 and a vertical cylinder S23; the mounting seat is fixed on the mobile platform;
the mechanical arm comprises a vertical lifting mobile platform S3, a first arm section S6, a second arm section S7, a third arm section S8 and a fourth arm section S10;
a vertical rail S24 is arranged on the left surface of the vertical column, and the vertical lifting mobile platform is vertically arranged on the vertical rail in a sliding manner; the lower end of the vertical column is fixedly connected to the upper surface of the mounting seat, the cylinder seat S21 of the vertical cylinder is fixedly connected to the upper surface of the mounting seat positioned on the left side of the vertical track, the telescopic rod S22 of the vertical cylinder is arranged vertically upwards, and the upper end of the telescopic rod of the vertical cylinder is fixedly connected to the lower surface of the vertical lifting moving table; the vertical lifting mobile platform can move up and down along a vertical track under the driving of a telescopic rod of a vertical cylinder to form a first degree of freedom;
the first arm section comprises an A1 section pipe S13 and an A2 section pipe S14 which is telescopically connected in a left pipe orifice of the A1 section pipe, a first air cylinder S25 with a telescopic rod horizontally arranged towards the left is fixedly arranged at the right end in the A1 section pipe, and the telescopic rod of the first air cylinder is fixedly connected at the right end of the A2 section pipe;
the arm section II comprises a section B1 pipe S16 and a section B2 pipe S17 which is telescopically connected in a left pipe orifice of the section B1 pipe, a second cylinder 37 with a telescopic rod horizontally arranged towards the left is fixedly arranged at the right end in the section B1 pipe, and the telescopic rod of the second cylinder is fixedly connected at the right end of the section B2 pipe;
a first horizontal rotating shaft S4 driven by a first speed reducing motor S26 is arranged at the left end of the vertical lifting moving platform, and the right end of the A1 section pipe is fixedly connected to the first horizontal rotating shaft, so that the first arm section can horizontally rotate to form a second degree of freedom; a first electromagnetic brake S32 capable of controlling the first horizontal rotating shaft to rotate is further arranged on the first horizontal rotating shaft;
the left end of the section A2 of the pipe is provided with a second horizontal rotating shaft S15 driven by a second speed reducing motor S27, and the right end of the section B1 of the pipe is fixedly connected to the second horizontal rotating shaft, so that the arm section II can horizontally rotate to form a third degree of freedom; a second electromagnetic brake S33 capable of controlling the second horizontal rotating shaft to rotate is further arranged on the second horizontal rotating shaft;
the left end of the section B2 pipe is provided with a third horizontal rotating shaft S18 driven by a third speed reducing motor S28, and the right end of the arm section III is fixedly connected to the third horizontal rotating shaft, so that the arm section III can horizontally rotate to form a fourth degree of freedom; a third electromagnetic brake S34 capable of controlling the third horizontal rotating shaft to rotate is further arranged on the third horizontal rotating shaft;
a first transverse vertical rotating shaft S9 which is driven by a fourth speed reducing motor S29 and can rotate on the left vertical surface and the right vertical surface is arranged at the left end of the arm section III, and the right end of the arm section IV is fixedly connected to the first transverse vertical rotating shaft, so that the arm section IV can vertically rotate on the left vertical surface and the right vertical surface to form a fifth degree of freedom; a fourth electromagnetic brake S35 capable of controlling the first transverse vertical rotating shaft to rotate is further arranged on the first transverse vertical rotating shaft;
a first longitudinal vertical rotating shaft S11 which is driven by a fifth speed reducing motor S30 and can rotate on the front vertical surface and the rear vertical surface is arranged at the left end of the arm section four, and the right end of the output gripper is fixedly connected to the first longitudinal vertical rotating shaft, so that the right end of the output gripper can vertically rotate on the front vertical surface and the rear vertical surface to form a sixth degree of freedom; a fifth electromagnetic brake S36 capable of controlling the first longitudinal vertical rotating shaft to rotate is further arranged on the first longitudinal vertical rotating shaft;
the A2 section of pipe can stretch and move left and right in the A1 section of pipe to form a seventh degree of freedom under the drive of a telescopic rod of the first cylinder;
the section B2 pipe can stretch and move left and right in the section B1 pipe to form an eighth degree of freedom under the drive of a telescopic rod of the second cylinder;
the left end of a first horizontal pipe S39 is horizontally and fixedly connected to the right surface of the vertical column, a balance adjusting block S40 is arranged in the first horizontal pipe in a sliding mode from left to right, a balance adjusting cylinder S38 with a telescopic rod facing horizontally is fixedly connected to the left end of the first horizontal pipe, and the right end of the telescopic rod of the balance adjusting cylinder is fixedly connected to the balance adjusting block;
the control end of No. one electromagnetic brake, the control end of No. two electromagnetic brakes, the control end of No. three electromagnetic brakes, the control end of No. four electromagnetic brakes, the control end of No. five electromagnetic brakes, the control end of a gear motor, the control end of No. two gear motor, the control end of No. three gear motor, the control end of No. four gear motor, the control end of No. five gear motor, the control end of a cylinder, the control end of No. two cylinders, the control end of balance adjustment cylinder and the control end of vertical cylinder are respectively control connection on the control cabinet.
An annular ring S42 is fixedly sleeved on the outer surface of the first longitudinal vertical rotating shaft, a first moving block S44 which is driven by a surrounding motor S43 and can move along the annular ring is arranged on the annular ring, and a camera S45 is arranged on the first moving block; the control end of the camera and the control end of the surrounding motor are respectively connected with the control console.
The camera can rotate along the output tongs, is convenient for snatch the object.
The seventh degree of freedom can enable the mechanical arm to extend freely, and the eighth degree of freedom can enable the mechanical arm to extend freely, so that the operation range and flexibility are greatly increased.
The balance adjusting cylinder controls the balance of the vertical column by moving the balance adjusting block left and right, and then the balance of the manipulator is controlled. The manipulator is convenient to move. The mobile platform comprises an automobile.
Because some mechanical arms of the invention have eight degrees of freedom, the invention has good flexibility and high reliability and is easy to complete control tasks.
A redundant robotic arm inverse priority impedance control method, as shown in figures 1-3. The method comprises the following steps:
step 1, establishing a redundant mechanical arm kinematics model, and giving a gradient direction strategy implementation process of a redundant mechanical arm zero space vector as follows:
the pose and the speed of the end effector in the Cartesian space are defined as x,
Figure GDA0003990435810000111
The angular position and angular velocity of the joint space are q,
Figure GDA0003990435810000112
J is a Jacobian matrix of the robot with n degrees of freedom, wherein x belongs to R n ，
Figure GDA0003990435810000113
Figure GDA0003990435810000114
J∈R mn (ii) a The positive kinematic equation of the redundant degree of freedom mechanical arm can be usedThe formula is described as follows:
Figure GDA0003990435810000121
equation (1) is also referred to as the kinematic velocity model of the mechanical arm;
considering the solution of the least squares method, the optimal problem can be listed as:
Figure GDA0003990435810000122
the solution of formula (1) can be optimized by finding the best
Figure GDA0003990435810000123
To solve the problem;
Figure GDA0003990435810000124
thus, the pseudo-inverse solution of equation (1) can be expressed as:
Figure GDA0003990435810000125
in the formula J + Pseudo-inverse of the Jacobian matrix
I-identity matrix
Figure GDA0003990435810000126
-arbitrary null space vector
Figure GDA0003990435810000127
Minimum norm solution, defining hand movements
Figure GDA0003990435810000128
Homogeneous solution, no action at the end
Equation (4) represents the position and attitude control of the end effector; adding any residual error into the formula (4) to obtain a general expression containing a null space; the multi-task optimization can be realized on a zero vector by using the equation;
however, the above equation ignores the ill-conditioned state of the jacobian matrix; the regularization equation may be modified by adding additional regularization values,
Figure GDA0003990435810000129
wherein λ ≧ 0 is a weighting matrix,
Figure GDA00039904358100001210
is a weighting coefficient, and satisfies
Figure GDA00039904358100001214
The solution to the above equation can be expressed as:
Figure GDA00039904358100001211
equation (7) is also referred to as a redundant manipulator kinematics model;
the joint limit function of the joint limit gradient direction of the position-dependent scalar index of the null-space vector of the redundant manipulator is as follows:
Figure GDA00039904358100001212
step 2, establishing a task priority solving strategy for obtaining a singularity elimination algorithm through a singular robust solution as follows:
in the solution of the redundant mechanical arm of the Jacobian matrix, an optimization task is realized in a null space of a main task; the reverse task kinematics is established on the basis of the forward task kinematics:
Figure GDA00039904358100001213
wherein
Figure GDA0003990435810000131
And
Figure GDA0003990435810000132
shows task1 and task2
The inverse kinematics equation of the redundant manipulator is obtained from expression (9) as follows:
Figure GDA0003990435810000133
task1 as the main Task and Task2 as the auxiliary Task; that is to say that the position of the first electrode,
Figure GDA0003990435810000134
is that
Figure GDA0003990435810000135
Is implemented in the null space of (1); the final inverse kinematics expression for the redundant manipulator is as follows:
Figure GDA0003990435810000136
wherein
Figure GDA0003990435810000137
Figure GDA0003990435810000138
Is a projection matrix which gives the applicable range of the secondary task to the primary task;
Figure GDA0003990435810000139
and
Figure GDA00039904358100001310
is the desired commanded speed;
Figure GDA00039904358100001311
is the main task of the method, and the method comprises the following steps of,
Figure GDA00039904358100001312
is a secondary task;
if two related tasks are interdependent, then the corresponding Jacobian matrix is singular; if the task jacobian matrix is singular, the corresponding task is not satisfied; in this case, the jacobian correlation matrix will be singularities, defined as algorithmic singularities;
that is, if
Figure GDA00039904358100001313
Where ρ (·) is the rank of the matrix;
it is clear that the singularity of the algorithm is caused by task conflicts between the secondary tasks and the primary task; in addition, redundant robotic arm inverse kinematics based on task priority aims to provide better effectiveness in controlling the primary tasks;
therefore, the position control direction is used as a main task, so that the position ensures the accuracy of the control direction task; then, establishing a task priority solving strategy equation for obtaining a singularity elimination algorithm through singular robust solution:
Figure GDA00039904358100001314
step 3, establishing a singular robust solution inverse kinematics analysis model as follows:
the motion singularity can occur based on the Jacobian pseudo-inverse solution, which is caused by the secondary matrix; for the motion singularity problem, DLS (damped least squares) solution should also be given;
the cost function for the DLS solution can be modified to:
Figure GDA00039904358100001315
thus, the singular robust pseudo-inverse solution of the above equation can be expressed as:
Figure GDA00039904358100001316
equation (15) is a singular robust solution inverse kinematics analysis model, and λ = η is set 2 I, the DLS solution is equivalent to an additional regularization solution, and a scalar value eta balances the task precision and singularity;
for the calculation of the pseudo-inverse solution of the Jacobian matrix, the singular value SVD decomposition form of the Jacobian matrix can be given
J＝U∑V T (16)
Wherein U is E.R m×m ，V∈R n×n ，∑∈R m×n U is a column vector U i A unitary matrix of V is formed by column vectors V i Formed unitary matrix, sigma being a block matrix of m x n diagonal matrices containing the singular values sigma of J i More than or equal to 0 contains n-m zero column vectors in descending order;
Figure GDA0003990435810000141
wherein r is less than or equal to m is the rank of the matrix J;
for motion singularity, with reference to the singular value decomposition SVD required to compute the pseudo-inverse solution, the large resulting joint velocity is due to the smallest singular value rapidly approaching 0, as follows:
Figure GDA0003990435810000142
factor lambda 0 Will affect the singularity, λ 0 The higher the value is, the larger the damping is, and the closer the joint velocity is to the singular point; in addition, strategies to define variable damping factorsAlso different; we can get
Figure GDA0003990435810000143
From the above equation, we can see that the parameter δ > 0 monitors the smallest singular value.
Step 4, establishing a reverse priority control strategy of the multi-task redundant mechanical arm as follows;
introducing an inverse-priority projection matrix
Figure GDA0003990435810000144
The matrix comprises a null-space of corresponding elements of the lowest priority l-k-1 task independent of the kth task, so that it is derived
Figure GDA0003990435810000145
Wherein J i|j Is a jacobian matrix associated with all components of the i-th task that are linearly independent of the j-th task;
therefore, the priority derivation formula is as follows:
Figure GDA0003990435810000146
in the above derivation, k = l, l-1, …,1, where l is a positive integer; initial value
Figure GDA0003990435810000147
To give a general form of computation of the linear independent Jacobian matrix J, the inverse augmented Jacobian matrix is defined as:
Figure GDA0003990435810000148
have therein
Figure GDA0003990435810000149
Wherein
Figure GDA00039904358100001410
To represent
Figure GDA00039904358100001411
A row of (2);
therefore, the method is not to be taken,
Figure GDA0003990435810000151
the pseudo-inverse solution of (a) can be expressed as:
Figure GDA0003990435810000152
and
Figure GDA0003990435810000153
wherein, T R Representation matrix
Figure GDA0003990435810000154
Expansion of (2);
the final inverse priority projection can be written as:
Figure GDA0003990435810000155
thus, we can derive the expression of the pseudo-inverse solution:
Figure GDA0003990435810000156
the reverse priority control strategy equation of the multi-task redundant mechanical arm is established as follows:
Figure GDA0003990435810000157
and 5, simplifying the reverse control equation of the redundant mechanical arm with the primary task and the secondary task as follows:
for a redundant mechanical arm with six degrees of freedom or seven degrees of freedom, the redundant mechanical arm does not have enough degrees of freedom to complete tasks of multiple layers; it is necessary to carry out a dual task priority control; that is, the motion control of the manipulator is a primary task and a secondary task;
the equations for the inverse control of a redundant robotic arm having primary and secondary tasks are as follows
Figure GDA0003990435810000158
The above formula is very different from the previous expression (11), but the algorithmic framework is similar; in the above-mentioned equations, the process of the present invention,
Figure GDA0003990435810000159
is a secondary task that is to be performed,
Figure GDA00039904358100001510
is the main task; the main task is realized in a designated null space of the main task; the core point of reverse priority being a projection matrix
Figure GDA00039904358100001511
Calculating;
Figure GDA00039904358100001512
is expressed as formula (30):
Figure GDA00039904358100001513
using similar guides as in previous equations (22) - (28), we can obtain a simplified inverse control equation for a redundant robotic arm with primary and secondary tasks:
Figure GDA00039904358100001514
step 6, establishing a reverse priority control strategy of the manipulator as follows:
the dynamics of the manipulator in force control space can be written as:
Figure GDA00039904358100001515
where X is the position in Cartesian space, M (X) is an inertial matrix,
Figure GDA00039904358100001516
non-linear force, F input control force, F e Is the contact force;
furthermore, the input joint moment can be obtained based on the conversion of the Jacobian matrix
τ＝J T (q)F (33)
The desired equation of motion of the manipulator in force control space may be defined as follows:
Figure GDA0003990435810000161
wherein M is d And B d Is the inertia and damping matrix; f d Is a command force, F e Is the contact force;
the kinetic profile of force control is shown in FIG. 1;
thus, the relationship between the environment and the manipulator response can be written as
Figure GDA0003990435810000162
The combination of the above two equations is as follows
Figure GDA0003990435810000163
From the above equation can be seenOut if M e 、B e And K e Known, then M d And B d Will affect the system response;
force control enables the manipulator to interact with the environment or human; in addition, in some cases, it is not necessary to implement omnidirectional force control and guarantee omnidirectional force control, that is, sometimes we only want to guarantee the force tracking control accuracy in a certain direction;
for example, when the manipulator interacts with the planer, only precise force tracking control needs to be maintained in the vertical direction, while precise force tracking control is not needed in the other direction; in other cases, position direction force control is more important than attitude direction force control;
it is therefore necessary to perform a hierarchical force control of the manipulator; that is, it is necessary to provide a new hierarchical force control framework; from the above equation we can derive the desired hierarchical force control relationship as follows
Figure GDA0003990435810000164
Figure GDA0003990435810000165
The integral of these two equations can be written as
Figure GDA0003990435810000166
Figure GDA0003990435810000167
If the robot end effector is able to track the desired cartesian velocity of
Figure GDA0003990435810000168
And
Figure GDA0003990435810000169
accurate force control of the manipulator can be achieved; the relation between the Cartesian velocity and the joint velocity should be based on inverse priority control; thus, the equation for the inverse priority control strategy for the manipulator can be derived:
Figure GDA00039904358100001610
the joint velocity required by the above equation will ensure force control of the manipulator; it is worth mentioning that the force control law is only a speed step control law, which relies on an inner speed loop control; if the inner position control effect is good, accurate force control can be realized; because the inner speed ring control can realize low-frequency position tracking, the outer force ring can realize low-frequency force tracking.
And 7, solving the relation between the external force and the joint acceleration in the reverse priority impedance control of the manipulator by adopting the joint speed, so as to obtain the realization mode of ensuring the reverse priority impedance control of the manipulator, wherein the realization mode comprises the following steps:
when the manipulator implements force control, the manipulator functions as an initiator to some extent, that is, the manipulator is ready to respond to the external environment; when the mechanical arm works as an impedance control model, the mechanical arm passively responds to an external force; the kinetic scheme of impedance control is shown in fig. 2;
the corresponding impedance relationship between the external force and the joint acceleration can be expressed as
Figure GDA0003990435810000171
Figure GDA0003990435810000172
The reference speed can be expressed as
Figure GDA0003990435810000173
Figure GDA0003990435810000174
Therefore, the inverse priority impedance control of the manipulator guarantees the expression:
Figure GDA0003990435810000175
and 8, expanding the reverse priority calculation of the position control space to the reverse priority calculation of the force control space, so as to obtain the overall framework implementation mode of the manipulator speed-level reverse priority impedance control, wherein the overall framework implementation mode comprises the following steps:
hybrid impedance applications are a combination of the two strategies mentioned above, i.e. the cartesian task can be divided into two cases: the first is a position control subspace, in which the impedance control is implemented; the second is a force control subspace in which force control is implemented;
thus selecting a selection matrix; the relationship between the external force and the position response is as follows
Figure GDA0003990435810000176
Figure GDA0003990435810000177
So a simplified form of the desired speed can be expressed as
Figure GDA0003990435810000178
Figure GDA0003990435810000179
Then we get a solution based on reverse priority
Figure GDA0003990435810000181
The kinetic scheme of the hybrid impedance control is shown in FIG. 3;
considering n-layer tasks, the corresponding impedance control task also belongs to n-layer framework, therefore, the overall framework expression of the manipulator speed level inverse priority impedance control is as follows
Figure GDA0003990435810000182
Expression (52) solves the problem of extending the reverse-priority computation of the position control space into manipulator reverse-priority hybrid impedance control of the reverse-priority computation of the force control space; the redundant mechanical arm of the manipulator can realize expected impedance control tasks under different hierarchical structures.

Claims (1)

1. A redundant manipulator inverse priority impedance control method is characterized by comprising the following steps:
step 1, establishing a kinematic model of the redundant mechanical arm, and providing a gradient direction strategy of a zero-space vector of the redundant mechanical arm;
step 2, establishing a task priority solving strategy for obtaining a singularity elimination algorithm through a singular robust solution;
step 3, establishing a singular robust solution inverse kinematics analysis model;
step 4, establishing a reverse priority control strategy of the multi-task redundant mechanical arm;
step 5, simplifying a reverse control equation of the redundant mechanical arm with the main task and the secondary task;
step 6, establishing a reverse priority control strategy of the manipulator;
step 7, solving the relation between the external force and the joint acceleration in the reverse priority impedance control of the manipulator by adopting the joint speed, thereby obtaining the reverse priority impedance control guarantee of the manipulator;
step 8, expanding the reverse priority calculation of the position control space to the reverse priority calculation of the force control space, thereby obtaining the overall framework of the speed-level reverse priority impedance control of the manipulator;
establishing a kinematic model of the redundant manipulator, and giving a gradient direction strategy of a zero space vector of the redundant manipulator, wherein the implementation process comprises the following steps:
the pose and the speed of the end effector in the Cartesian space are defined as x,
Figure FDA0003990435800000011
The angular position and angular velocity of the joint space are q,
Figure FDA0003990435800000012
J is a Jacobian matrix of the robot with n degrees of freedom, wherein x belongs to R n ，
Figure FDA0003990435800000013
J∈R mn (ii) a The positive kinematic equation for a redundant degree of freedom robotic arm can be described by:
Figure FDA0003990435800000014
equation (1) is also referred to as a robot arm kinematics velocity model;
considering the solution of the least squares method, the optimal problem can be listed as:
Figure FDA0003990435800000021
the solution of equation (1) can be optimized by finding the best solution
Figure FDA0003990435800000022
To solve the problem;
Figure FDA0003990435800000023
thus, the pseudo-inverse solution of equation (1) can be expressed as:
Figure FDA0003990435800000024
in the formula J + Pseudo-inverse of the Jacobian matrix
I-identity matrix
Figure FDA0003990435800000025
-arbitrary null space vector
Figure FDA0003990435800000026
Minimum norm solution, defining hand movements
Figure FDA0003990435800000027
Homogeneous solution, no action at the end
Equation (4) represents the position and attitude control of the end effector; adding any residual error into the formula (4) to obtain a general expression containing a null space; the multi-task optimization can be realized on a zero vector by using the equation;
however, the above equation ignores the ill-conditioned state of the jacobian matrix; the regularization equation may be modified by adding additional regularization values,
Figure FDA0003990435800000028
where λ ≧ 0 is the weighting matrix,
Figure FDA0003990435800000029
is a weighting coefficient, and satisfies
Figure FDA00039904358000000210
The solution to the above equation can be expressed as:
Figure FDA00039904358000000211
equation (7) is also referred to as a redundant manipulator kinematics model;
the joint limit function of the joint limit gradient direction of the position-dependent scalar index of the null-space vector of the redundant manipulator is as follows:
Figure FDA0003990435800000031
the task priority solving strategy for obtaining the algorithm for eliminating the singularity through the singularity robust solution is established as follows:
in the solution of the redundant mechanical arm of the Jacobian matrix, an optimization task is realized in a null space of a main task; the reverse task kinematics is established on the basis of the forward task kinematics:
Figure FDA0003990435800000032
wherein
Figure FDA0003990435800000033
And
Figure FDA0003990435800000034
shows task1 and task2
The inverse kinematics equation of the redundant manipulator is obtained from expression (9) as follows:
Figure FDA0003990435800000035
task1 is used as a main Task, and Task2 is used as an auxiliary Task; that is to say that the position of the first electrode,
Figure FDA0003990435800000036
is that
Figure FDA0003990435800000037
Is implemented in the null space of (1); the final inverse kinematics expression for the redundant manipulator is as follows:
Figure FDA0003990435800000038
wherein
Figure FDA0003990435800000039
Figure FDA00039904358000000310
Is a projection matrix which gives the applicable range of the secondary task to the primary task;
Figure FDA00039904358000000311
and
Figure FDA00039904358000000312
is the desired commanded speed;
Figure FDA00039904358000000313
is the main task of the method, and the method comprises the following steps of,
Figure FDA00039904358000000314
is a secondary task;
if two related tasks are interdependent, then the corresponding Jacobian matrix is singular; if the task jacobian matrix is singular, the corresponding task is not satisfied; in this case, the jacobian correlation matrix will be singularities, defined as algorithmic singularities;
that is, if
Figure FDA0003990435800000041
Where ρ (·) is the rank of the matrix;
it is clear that the singularity of the algorithm is caused by task conflicts between the secondary tasks and the primary task; in addition, task priority based redundant robotic arm inverse kinematics aims to provide better control over the effectiveness of the primary tasks;
therefore, the position control direction is used as a main task, so that the position ensures the accuracy of the control direction task; then, establishing a task priority solving strategy equation for obtaining a singularity elimination algorithm through singular robust solution:
Figure FDA0003990435800000042
the singular robust solution inverse kinematics analysis model is established as follows:
the motion singularity can occur based on the Jacobian pseudo-inverse solution, which is caused by the secondary matrix; for the motion singularity problem, a DLS (damped least squares) solution should also be given;
the cost function for the DLS solution can be modified to:
Figure FDA0003990435800000043
thus, the singular robust pseudo-inverse solution of the above equation can be expressed as:
Figure FDA0003990435800000044
equation (15) is the singular robust solution inverse kinematics analysis model, and let λ = η 2 I, the above DLS solution is equivalent toAdding a regularization solution, wherein the scalar value eta balances the task precision and singularity;
for the calculation of the pseudo-inverse solution of the Jacobian matrix, the singular value SVD decomposition form of the Jacobian matrix can be given
J＝U∑V T (16)
Wherein U is E.R m×m ，V∈R n×n ，∑∈R m×n U is a column vector U i A unitary matrix of V is formed by column vectors V i A unitary matrix of composition, Σ being a block matrix of m × n diagonal matrices containing the singular values σ of J i More than or equal to 0 contains n-m zero column vectors in descending order;
Figure FDA0003990435800000051
wherein r ≦ m is the rank of the matrix J;
for motion singularity, with reference to the singular value decomposition SVD required to compute the pseudo-inverse solution, the large resulting joint velocity is due to the smallest singular value rapidly approaching 0, as follows:
Figure FDA0003990435800000052
factor lambda 0 Will affect the singularity, λ 0 The higher the value is, the larger the damping is, and the closer the joint velocity is to the singular point; in addition, the strategy for defining the variable damping factor is also different; we can get
Figure FDA0003990435800000053
From the above equation, we can see that the parameter δ > 0 monitors the smallest singular value;
establishing a reverse priority control strategy of the multi-task redundant mechanical arm as follows;
introducing an inverse-priority projection matrix
Figure FDA0003990435800000054
The matrix comprises a null space of corresponding elements of the lowest priority l-k-1 task independent of the kth task, so derived
Figure FDA0003990435800000055
Figure FDA0003990435800000056
Wherein J i|j Is a jacobian matrix associated with all components of the i-th task that are linearly independent of the j-th task;
therefore, the priority derivation formula is as follows:
Figure FDA0003990435800000061
in the above derivation, k = l, l-1, …,1, where l is a positive integer; initial value
Figure FDA0003990435800000062
To give a general form of computation of the linear independent Jacobian matrix J, the inverse augmented Jacobian matrix is defined as:
Figure FDA0003990435800000063
have therein
Figure FDA0003990435800000064
Wherein
Figure FDA0003990435800000065
Represent
Figure FDA0003990435800000066
A row of (2);
therefore, the method is not to be taken,
Figure FDA0003990435800000067
the pseudo-inverse solution of (a) can be expressed as:
Figure FDA0003990435800000068
and
Figure FDA0003990435800000069
wherein, T k Representation matrix
Figure FDA00039904358000000610
Expansion of (2);
the final inverse priority projection can be written as:
Figure FDA00039904358000000611
thus, we can find the expression of the pseudo-inverse solution:
Figure FDA0003990435800000071
the reverse priority control strategy equation of the multitask redundant mechanical arm is established as follows:
Figure FDA0003990435800000072
the inverse control equations for a redundant robotic arm having a primary task and a secondary task are simplified as follows:
for a redundant mechanical arm with six degrees of freedom or seven degrees of freedom, the redundant mechanical arm does not have enough degrees of freedom to complete tasks of multiple layers; it is necessary to carry out a dual task priority control; that is, the motion control of the manipulator is a primary task and a secondary task;
the inverse control equation for a redundant robotic arm having a primary task and a secondary task is as follows
Figure FDA0003990435800000073
The above formula is very different from the previous expression (11), but the algorithmic framework is similar; in the above-mentioned equations, the process of the present invention,
Figure FDA0003990435800000074
is a secondary task that is to be performed,
Figure FDA0003990435800000075
is the main task; the main task is realized in a designated null space of the main task; the core point of reverse priority being a projection matrix
Figure FDA0003990435800000076
Calculating (1);
Figure FDA0003990435800000077
is expressed as formula (30):
Figure FDA0003990435800000078
using similar guides as in previous equations (22) - (28), we can obtain a simplified inverse control equation for a redundant robotic arm with primary and secondary tasks:
Figure FDA0003990435800000079
the reverse priority control strategy of the manipulator is established as follows:
the dynamics of the manipulator in force control space can be written as:
Figure FDA0003990435800000081
where X is the position in Cartesian space, M (X) is an inertial matrix,
Figure FDA0003990435800000082
non-linear force, F input control force, F e Is the contact force;
furthermore, the input joint moments are obtained on the basis of a conversion of the Jacobian matrix
τ＝J T (q)F (33)
The desired equation of motion of the manipulator in force control space may be defined as follows:
Figure FDA0003990435800000083
wherein M is d And B d Is an inertia and damping matrix; f d Is a command force, F e Is the contact force;
thus, the relationship between the environment and the manipulator response can be written as
Figure FDA0003990435800000084
The combination of the above two equations is as follows
Figure FDA0003990435800000085
As can be seen from the above equation, if M is e 、B e And K e Known, then M d And B d Will affect the system response;
force control enables the manipulator to interact with the environment or human; in addition, in some cases, it is not necessary to implement omnidirectional force control and guarantee omnidirectional force control, that is, sometimes we only want to guarantee the force tracking control accuracy in a certain direction;
it is therefore necessary to apply graduated force control to the manipulator; that is, it is necessary to provide a new hierarchical force control framework; from the above equation we can derive the desired hierarchical force control relationship as follows
Figure FDA0003990435800000086
Figure FDA0003990435800000087
The integral of these two equations can be written as
Figure FDA0003990435800000091
Figure FDA0003990435800000092
If the robot end effector is able to track the desired cartesian velocity of
Figure FDA0003990435800000093
And
Figure FDA0003990435800000094
accurate force control of the manipulator can be achieved; the relation between the Cartesian velocity and the joint velocity should be based on inverse priority control; thus, the equation for the inverse priority control strategy for the manipulator can be derived:
Figure FDA0003990435800000095
the joint velocity required by the above equation will ensure force control of the manipulator; it is worth mentioning that the force control law is only a speed step control law, which relies on an inner speed loop control; if the internal position control effect is good, accurate force control can be realized; because the inner speed ring can realize low-frequency position tracking, the outer force ring can realize low-frequency force tracking;
the relationship between the external force and the joint acceleration in the inverse-priority impedance control of the manipulator is solved by adopting the joint speed, so that the realization mode of ensuring the inverse-priority impedance control of the manipulator is as follows:
when the manipulator implements force control, the manipulator functions as an initiator to some extent, that is, the manipulator is ready to respond to the external environment; when the mechanical arm
When the robot arm works as an impedance control model, the robot arm passively responds to an external force;
the corresponding impedance relationship between the external force and the joint acceleration can be expressed as
Figure FDA0003990435800000096
Figure FDA0003990435800000097
The reference speed can be expressed as
Figure FDA0003990435800000101
Figure FDA0003990435800000102
Therefore, the inverse priority impedance control of the manipulator guarantees the expression:
Figure FDA0003990435800000103
the overall framework implementation of extending the inverse-priority computation of the position control space to the inverse-priority computation of the force control space to obtain the manipulator velocity-level inverse-priority impedance control is as follows:
hybrid impedance applications are a combination of the two strategies mentioned above, i.e. the cartesian task can be divided into two cases: the first is a position control subspace, in which the impedance control is implemented; the second is a force control subspace in which force control is implemented;
thus selecting a selection matrix; the relationship between the external force and the position response is as follows
Figure FDA0003990435800000104
Figure FDA0003990435800000105
So a simplified representation of the desired speed can be expressed as
Figure FDA0003990435800000106
Figure FDA0003990435800000107
Then we get a solution based on reverse priority
Figure FDA0003990435800000108
Considering the n-layer task, the corresponding impedance control task also belongs to the n-layer framework, therefore, the overall framework expression of the manipulator speed grade inverse priority impedance control is as follows
Figure FDA0003990435800000111
Expression (52) solves the problem of extending the reverse-priority computation of the position control space into manipulator reverse-priority hybrid impedance control of the reverse-priority computation of the force control space; the redundant mechanical arm of the manipulator can realize expected impedance control tasks under different hierarchical structures;
the redundant mechanical arm reverse priority impedance control system is suitable for a redundant mechanical arm reverse priority impedance control method and comprises a mechanical arm, a control console (S31) for controlling the mechanical arm and a movable moving platform (S41); the manipulator comprises a mechanical arm, a mounting seat (S1), a vertical column (S2), an output gripper (S12) and a vertical cylinder (S23); the mounting seat is fixed on the mobile platform;
the mechanical arm comprises a vertical lifting moving platform (S3), a first arm section (S6), a second arm section (S7), a third arm section (S8) and a fourth arm section (S10);
a vertical rail (S24) is arranged on the left surface of the vertical column, and the vertical lifting mobile platform is vertically arranged on the vertical rail in a sliding manner; the lower end of the vertical column is fixedly connected to the upper surface of the mounting seat, a cylinder seat (S21) of the vertical cylinder is fixedly connected to the upper surface of the mounting seat positioned on the left side of the vertical track, a telescopic rod (S22) of the vertical cylinder is vertically arranged upwards, and the upper end of the telescopic rod of the vertical cylinder is fixedly connected to the lower surface of the vertical lifting mobile platform; the vertical lifting mobile platform can move up and down along a vertical track under the driving of a telescopic rod of a vertical cylinder to form a first degree of freedom;
the first arm section comprises an A1 section pipe (S13) and an A2 section pipe (S14) which is telescopically connected in a left pipe orifice of the A1 section pipe, a first cylinder (S25) with a telescopic rod horizontally arranged towards the left is fixedly arranged at the right end in the A1 section pipe, and the telescopic rod of the first cylinder is fixedly connected at the right end of the A2 section pipe;
the arm section II comprises a section B1 pipe (S16) and a section B2 pipe (S17) which is telescopically connected in a left pipe orifice of the section B1 pipe, a second cylinder (37) with a telescopic rod horizontally arranged towards the left is fixedly arranged at the right end in the section B1 pipe, and the telescopic rod of the second cylinder is fixedly connected at the right end of the section B2 pipe;
a first horizontal rotating shaft (S4) driven by a first speed reducing motor (S26) is arranged at the left end of the vertical lifting moving platform;
the right end of the section A1 of pipe is fixedly connected to a first horizontal rotating shaft, so that the first arm section can horizontally rotate to form a second degree of freedom; a first electromagnetic brake (S32) capable of controlling the first horizontal rotating shaft to rotate is further arranged on the first horizontal rotating shaft;
the left end of the section A2 of the pipe is provided with a second horizontal rotating shaft (S15) driven by a second speed reducing motor (S27), and the right end of the section B1 of the pipe is fixedly connected to the second horizontal rotating shaft, so that the arm section II can horizontally rotate to form a third degree of freedom; a second electromagnetic brake (S33) capable of controlling the second horizontal rotating shaft to rotate is further arranged on the second horizontal rotating shaft;
the left end of the section B2 pipe is provided with a third horizontal rotating shaft (S18) driven by a third speed reducing motor (S28), and the right end of the arm section III is fixedly connected to the third horizontal rotating shaft, so that the arm section III can horizontally rotate to form a fourth degree of freedom; a third electromagnetic brake (S34) capable of controlling the third horizontal rotating shaft to rotate is further arranged on the third horizontal rotating shaft;
a first transverse vertical rotating shaft (S9) which is driven by a fourth speed reducing motor (S29) and can rotate on the left vertical surface and the right vertical surface is arranged at the left end of the arm section III, and the right end of the arm section IV is fixedly connected to the first transverse vertical rotating shaft, so that the arm section IV can vertically rotate on the left vertical surface and the right vertical surface to form a fifth degree of freedom; a fourth electromagnetic brake (S35) capable of controlling the first transverse vertical rotating shaft to rotate is further arranged on the first transverse vertical rotating shaft;
a first longitudinal vertical rotating shaft (S11) which is driven by a fifth speed reducing motor (S30) and can rotate on the front vertical surface and the rear vertical surface is arranged at the left end of the arm section four, and the right end of the output gripper is fixedly connected to the first longitudinal vertical rotating shaft, so that the right end of the output gripper can vertically rotate on the front vertical surface and the rear vertical surface to form a sixth degree of freedom; a fifth electromagnetic brake (S36) capable of controlling the first longitudinal vertical rotating shaft to rotate is further arranged on the first longitudinal vertical rotating shaft;
the A2 section of pipe can stretch and move left and right in the A1 section of pipe to form a seventh degree of freedom under the drive of a telescopic rod of the first cylinder;
the section B2 pipe can stretch and move left and right in the section B1 pipe to form an eighth degree of freedom under the drive of a telescopic rod of the second cylinder;
the left end of a first horizontal pipe (S39) is horizontally and fixedly connected to the right surface of the vertical column, a balance adjusting block (S40) is arranged in the first horizontal pipe in a sliding mode from left to right, a balance adjusting cylinder (S38) with a telescopic rod facing right horizontally is fixedly connected to the left end of the first horizontal pipe, and the right end of the telescopic rod of the balance adjusting cylinder is fixedly connected to the balance adjusting block;
the control end of the first electromagnetic brake, the control end of the second electromagnetic brake, the control end of the third electromagnetic brake, the control end of the fourth electromagnetic brake, the control end of the fifth electromagnetic brake, the control end of the first speed reducing motor, the control end of the second speed reducing motor, the control end of the third speed reducing motor, the control end of the fourth speed reducing motor, the control end of the fifth speed reducing motor, the control end of the first air cylinder, the control end of the second air cylinder, the control end of the balance adjusting air cylinder and the control end of the vertical air cylinder are respectively connected to a control console in a control mode;
an annular ring (S42) is fixedly sleeved on the outer surface of the first longitudinal vertical rotating shaft, a first moving block (S44) which is driven by a surrounding motor (S43) and can move along the annular ring is arranged on the annular ring, and a camera (S45) is arranged on the first moving block; the control end of the camera and the control end of the surrounding motor are respectively connected with the console.
CN202010369736.1A 2020-04-30 2020-04-30 Redundant mechanical arm inverse priority impedance control system and control method Active CN111702753B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010369736.1A CN111702753B (en) 2020-04-30 2020-04-30 Redundant mechanical arm inverse priority impedance control system and control method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010369736.1A CN111702753B (en) 2020-04-30 2020-04-30 Redundant mechanical arm inverse priority impedance control system and control method

Publications (2)

Publication Number Publication Date
CN111702753A CN111702753A (en) 2020-09-25
CN111702753B true CN111702753B (en) 2023-02-14

Family

ID=72537114

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010369736.1A Active CN111702753B (en) 2020-04-30 2020-04-30 Redundant mechanical arm inverse priority impedance control system and control method

Country Status (1)

Country Link
CN (1) CN111702753B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113858239B (en) * 2021-09-24 2023-08-15 华能山东石岛湾核电有限公司 Mechanical arm
CN114469642B (en) * 2022-01-20 2024-05-10 深圳华鹊景医疗科技有限公司 Rehabilitation robot control method and device and rehabilitation robot

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105619400A (en) * 2016-02-24 2016-06-01 青岛万龙智控科技有限公司 Six-degree-of-freedom manipulator
CN108098786B (en) * 2017-12-19 2020-12-04 扬州大学 Endoscopic mechanical arm for fusion reactor
CN108621163A (en) * 2018-05-10 2018-10-09 同济大学 A kind of redundancy tow-armed robot cooperation control method towards remittance tenon technique
CN209631457U (en) * 2018-12-17 2019-11-15 清研同创机器人（天津）有限公司 A kind of Omni-mobile formula robot spraying system
CN110104216B (en) * 2019-01-28 2022-09-06 西北工业大学深圳研究院 Collaborative path planning method for kinematic redundancy double-arm space robot

Also Published As

Publication number Publication date
CN111702753A (en) 2020-09-25

Similar Documents

Publication Publication Date Title
US8600554B2 (en) System and method for robot trajectory generation with continuous accelerations
CN111702753B (en) Redundant mechanical arm inverse priority impedance control system and control method
CN111687835B (en) System and method for controlling reverse priority impedance of redundant mechanical arm of underwater mechanical arm
Yan et al. A 5-DOF redundantly actuated parallel mechanism for large tilting five-face machining
Humaidi et al. Adaptive control of parallel manipulator in Cartesian space
Chung et al. Torque optimizing control with singularity-robustness for kinematically redundant robots
Uzunoğlu et al. A multi-priority controller for industrial macro-micro manipulation
CN111687834B (en) System and method for controlling reverse priority impedance of redundant mechanical arm of mobile mechanical arm
CN111687832B (en) System and method for controlling inverse priority impedance of redundant mechanical arm of space manipulator
Wimböck et al. Experimental study on dynamic reactionless motions with DLR's humanoid robot Justin
Nemec et al. Null space velocity control with dynamically consistent pseudo-inverse
Shayya et al. A novel (3T-1R) redundant parallel mechanism with large operational workspace and rotational capability
Prempraneerach Delta parallel robot workspace and dynamic trajectory tracking of delta parallel robot
Wen et al. Motion planning for image-based visual servoing of an underwater vehicle-manipulator system in task-priority frameworks
CN111687833B (en) System and method for controlling impedance of inverse priority of manipulator
Seraji Motion control of mobile manipulators
Hishinuma et al. Singularity-consistent vibration suppression control with a redundant manipulator mounted on a flexible base
Xuan et al. Adaptive hierarchical sliding mode control using an artificial neural network for a ballbot system with uncertainties
Shadpey et al. Compliant motion control and redundancy resolution for kinematically redundant manipulators
Lu et al. Mechatronic design of dynamically decoupled manipulators based on the control performance improvement
CN118226875B (en) Safety track tracking control method for underwater unmanned ship under comprehensive constraint
Soylu et al. Development of a coordinated controller for underwater vehicle-manipulator systems
Carmichael et al. Preliminary Analysis of a Redundancy Resolution Method for Mobile Manipulators used in Physical Human Robot Interaction
Nuchkrua et al. Contouring control of 5-DOF manipulator robot arm based on equivalent errors
Marais et al. Maximising wrenches for kinematically redundant systems with experiments on UVMS

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