CN111687834A - Reverse priority impedance control system and method for redundant mechanical arm of mobile manipulator - Google Patents

Abstract

The invention discloses a mobile manipulator redundant manipulator arm inverse priority impedance control system and a control method. The invention belongs to the technical field of inverse priority impedance control of a manipulator redundant manipulator. The manipulator is easy to move, and includes a movable mobile platform, a manipulator and a console for controlling the manipulator; the manipulator includes a manipulator, a mounting seat, a vertical column, and an output gripper. and vertical cylinder; the mechanical arm includes a vertical lifting mobile platform, arm section 1, arm section 2, arm section 3 and arm section 4; a vertical track is arranged on the left surface of the vertical column, and the vertical lifting mobile platform goes up and down The vertical sliding is arranged on the vertical track; the lower end of the vertical column is fixedly connected to the upper surface of the mounting seat, and the cylinder seat of the vertical cylinder is fixedly connected to the upper surface of the mounting seat located on the left side of the vertical track. The upper end of the telescopic rod of the air cylinder is fixedly connected to the lower surface of the vertical lifting mobile platform; the mounting seat is fixed on the mobile platform.

Description

Inverse priority impedance control system and control method of mobile manipulator redundant manipulator

technical field

The invention relates to the technical field of inverse priority impedance control of a redundant manipulator of a manipulator, in particular to a control system and a control method of the inverse priority impedance of a redundant manipulator of a mobile manipulator.

Background technique

At present, the control method adopted by industrial robots is to treat each joint on the manipulator as a separate servo mechanism, that is, each axis corresponds to a servo, and each servo is controlled by the bus, which is controlled and coordinated by the controller.

A manipulator with six degrees of freedom is a manipulator with the minimum number of degrees of freedom that can complete spatial positioning. Manipulators with more than six degrees of freedom are collectively referred to as redundant manipulators;

At present, the impedance control method of the manipulator cannot achieve the desired impedance control task under different hierarchical structures. Therefore, it is very important to design a method that can enable the redundant manipulator of the manipulator to achieve the desired impedance control task under different hierarchical structures. necessary.

SUMMARY OF THE INVENTION

The present invention is to solve the problem that the existing mechanical arm impedance control method cannot achieve the desired impedance control task under different hierarchical structures, and provides a method that can firstly control the balance of the manipulator, and the manipulator is easy to move; An inverse priority impedance control system and control method for a redundant manipulator of a mobile manipulator, which can enable a redundant manipulator of a manipulator to achieve a desired impedance control task.

The above technical problems are solved by the following technical solutions:

A mobile manipulator redundant manipulator arm inverse priority impedance control system, including a manipulator and a console for controlling the manipulator; and a movable mobile platform; the manipulator includes a manipulator, a mounting seat, a vertical column, an output gripper and a vertical Cylinder; the mounting seat is fixed on the mobile platform;

The manipulator includes a vertical lift moving table, arm section 1, arm section 2, arm section 3 and arm section 4;

A vertical track is arranged on the left surface of the vertical column, and the vertical lifting and moving platform is vertically slid up and down on the vertical track; the lower end of the vertical column is fixedly connected to the upper surface of the mounting seat, and the cylinder of the vertical cylinder The seat is fixedly connected to the upper surface of the mounting seat located on the left side of the vertical track, the telescopic rod 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 mobile platform; The vertical lift mobile platform can move up and down along the vertical track under the driving of the telescopic rod of the vertical cylinder to form the first degree of freedom;

The first arm section includes the A1 section pipe and the A2 section pipe telescopically connected in the left nozzle of the A1 section pipe. The right end of the A1 section pipe is fixed with a No. 1 cylinder with a telescopic rod horizontally arranged to the left, and the telescopic rod of the No. 1 cylinder is fixed. Connect to the right end of the A2 section pipe;

The second arm section includes the B1 section pipe and the B2 section pipe telescopically connected in the left nozzle of the B1 section pipe. The right end of the B1 section pipe is fixed with the No. 2 cylinder with the telescopic rod horizontally arranged to the left, and the telescopic rod of the No. 2 cylinder is fixed. Connected to the right end of the B2 segment pipe;

There is a No. 1 horizontal rotating shaft driven by a No. 1 geared motor on the left end of the vertical lifting mobile platform, and the right end of the A1 section pipe is fixedly connected to the No. 1 horizontal rotating shaft, so that the first arm can rotate horizontally to form the second Degree of freedom; No. 1 electromagnetic brake that can control the rotation of No. 1 horizontal rotating shaft is also provided on No. 1 horizontal rotating shaft;

There is a No. 2 horizontal rotating shaft driven by a No. 2 gear motor on the left end of the A2 section tube, and the right end of the B1 section tube is fixedly connected to the No. 2 horizontal rotating shaft, so that the second arm section can rotate horizontally to form the third degree of freedom ; There is also a No. 2 electromagnetic brake on the No. 2 horizontal rotating shaft, which can control the rotation of the No. 2 horizontal rotating shaft;

There is a No. 3 horizontal rotating shaft driven by a No. 3 reduction motor on the left end of the B2 section tube, and the right end of the third arm is fixedly connected to the No. 3 horizontal rotating shaft, so that the third arm can rotate horizontally to form the fourth degree of freedom ; There is also a No. 3 electromagnetic brake on the No. 3 horizontal rotating shaft, which can control the rotation of the No. 3 horizontal rotating shaft;

The left end of the arm section 3 is provided with a No. 1 horizontal vertical rotating shaft driven by a No. 4 reduction motor that can rotate on the left and right vertical planes, and the right end of the arm section 4 is fixedly connected to the No. 1 horizontal vertical rotating shaft , so that the arm section 4 can rotate vertically on the left and right vertical planes to form the fifth degree of freedom; the No. 1 horizontal vertical rotating shaft is also provided with a No. 4 electromagnetic brake that can control the rotation of the No. 1 horizontal vertical rotating shaft;

The left end of the arm section 4 is provided with a No. 1 longitudinal vertical rotation shaft driven by a No. 5 reduction motor that can rotate on the front and rear vertical planes, and the right end of the output gripper is fixedly connected to the No. 1 longitudinal vertical rotation shaft , so that the right end of the output gripper can rotate vertically on the front and rear vertical planes to form the sixth degree of freedom; the No. 1 longitudinal vertical rotating shaft is also provided with a No. 5 electromagnetic brake that can control the rotation of the No. 1 longitudinal vertical rotating shaft ;

The A2 section tube can be telescopically moved left and right in the A1 section tube under the drive of the telescopic rod of the No. 1 cylinder to form the seventh degree of freedom;

The B2 section tube can be telescopically moved left and right in the B1 section tube under the drive of the telescopic rod of the No. 2 cylinder to form the eighth degree of freedom;

The left end of a No. 1 horizontal pipe is fixed horizontally on the right surface of the vertical column, and a balance adjusting block is slid left and right in the No. 1 horizontal pipe. Adjust the cylinder, the right end of the telescopic rod of the balance adjustment cylinder is fixedly connected to the balance adjustment block;

The control terminal of No. 1 electromagnetic brake, the control terminal of No. 2 electromagnetic brake, the control terminal of No. 3 electromagnetic brake, the control terminal of No. 4 electromagnetic brake, the control terminal of No. 5 electromagnetic brake, the control terminal of No. 1 reduction motor, the control terminal of No. 2 electromagnetic brake The control end of the gear motor, the control end of the No. 3 gear motor, the control end of the No. 4 gear motor, the control end of the No. 5 gear motor, the control end of the No. 1 cylinder, the control end of the No. 2 cylinder, the control end of the balance adjustment cylinder and the control end of the vertical cylinder are respectively controlled and connected to the console.

The balance adjustment cylinder controls the left and right movement of the balance adjustment block to control the balance of the vertical column, thereby controlling the balance of the manipulator. Easy to move the manipulator. Since some of the manipulators of the present invention have eight degrees of freedom, they have good flexibility and high reliability, and are easy to complete control tasks.

A mobile manipulator redundant manipulator inverse priority impedance control method, comprising the following steps:

Step 1. Establish a kinematic model of the redundant manipulator, and give the gradient direction strategy of the zero-space vector of the redundant manipulator;

Step 2. Establish a task priority solution strategy for obtaining the singularity elimination algorithm through the singular robust solution;

Step 3. Establish a singular robust solution inverse kinematics analysis model;

Step 4. Establish an inverse priority control strategy for the multi-task redundant robotic arm;

Step 5. Simplify the reverse control equation of the redundant manipulator with primary tasks and secondary tasks;

Step 6, establish the inverse priority control strategy of the manipulator;

Step 7. Use the joint speed to solve the relationship between the external force and the joint acceleration in the inverse priority impedance control of the manipulator, so as to obtain the guarantee of the inverse priority impedance control of the manipulator;

Step 8: Extend the inverse priority calculation of the position control space to the inverse priority calculation of the force control space, so as to obtain the overall framework of the inverse priority impedance control of the speed level of the manipulator.

In this scheme, the motion of the redundant manipulator in the joint space is derived in the reverse order; then the Cartesian impedance control and the inverse priority impedance control are combined to solve the inverse hierarchical impedance control problem, and the Cartesian impedance control behavior is divided into For high priority impedance control and low priority impedance control. Among them, the high-priority impedance control task will not interfere with the low-priority impedance control task, and the motion in the joint space will be affected in the reverse order, and work in the corresponding projection operator; finally, the high-priority impedance control task is realized, avoiding the low Deformations due to singularities that may occur in preferential impedance control tasks. Therefore, the proposed inverse-priority impedance control method enables redundant manipulators to achieve the desired impedance control tasks under an appropriate hierarchical structure.

As an option, the kinematic model of the redundant manipulator is established, and the gradient direction strategy of the null space vector of the redundant manipulator is given. The implementation process is as follows:

其关节空间的角位置、角速度分别为q、

J为n自由度机器人的雅可比矩阵，其中x∈Rⁿ，

J∈R^m-n；冗余自由度机械臂的正运动学方程可用下式描述：Define the pose and velocity of the end effector in Cartesian space as x,

The angular position and angular velocity of its joint space are q,

J is the Jacobian matrix of the n-degree-of-freedom robot, where x ∈ R ⁿ ,

J∈R ^mn ; the forward kinematic equation of the redundant degree-of-freedom manipulator can be described by the following formula:

Equation (1) is also called the kinematic velocity model of the manipulator;

Considering the solution of least squares, the optimal problem can be listed as:

来解决；The solution of equation (1) can be obtained by finding the best

to solve;

Therefore, the pseudo-inverse solution of equation (1) can be expressed as:

where J ⁺ ——the pseudo-inverse of the Jacobian matrix

I - identity matrix

——任意零空间矢量

— Arbitrary null space vector

——最小范数解，定义了手的动作

- the minimum norm solution, which defines the motion of the hand

——齐次解，在末端不产生动作

- Homogeneous solution, no action at the end

Equation (4) represents the position and attitude control of the end effector; adding arbitrary residuals to Equation (4) can obtain a general expression including the null space; using the above equations, multi-task optimization can be achieved on the zero vector;

However, the above equation ignores the ill-conditioned Jacobian; the regularization equation can be modified by adding additional regularization values,

为加权系数，并且满足where λ≥0 is the weighting matrix,

is the weighting coefficient, and it satisfies

The solution of the above equation can be expressed as:

Equation (7) is also called the 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 an option, the task priority solution strategy for obtaining the singularity elimination algorithm through the singular robust solution is as follows:

In the redundant manipulator solution of the Jacobian matrix, the optimization task is realized in the null space of the main task; the inverse task kinematics is established on the basis of the forward task kinematics:

和

表示task1和task2in

and

Indicates task1 and task2

From expression (5), the inverse kinematics equation of the redundant manipulator is:

是在

的零空间中实现的；冗余机械臂的最终逆运动学表达式如下所示：Task1 as the main task and task2 as the auxiliary task; that is,

is in

is implemented in the null space of ; the final inverse kinematics expression for the redundant manipulator is as follows:

in

是投影矩阵，它给出了次要任务到主要任务的适用范围；

和

是所需的命令速度；

是主要任务，

是次要任务；

is the projection matrix, which gives the applicability of the secondary task to the primary task;

and

is the desired command speed;

is the main task,

is a secondary task;

If two correlated tasks are interdependent, the corresponding Jacobian matrix is singular; if the task Jacobian is singular, the corresponding task is unsatisfied; in this case, the Jacobian correlation matrix will be Singularity, defined as algorithm singularity;

That is, if

where ρ( ) is the rank of the matrix;

It is obvious that the singularity of the algorithm is caused by the task conflict between the secondary task and the primary task; moreover, the inverse kinematics of the redundant manipulator based on the task priority aims to provide better control over the effectiveness of the primary task;

Therefore, let the position control the direction as the main task, so that the position guarantees the accuracy of the control direction task; then establish the task priority solving strategy equation of the singularity elimination algorithm through the singular robust solution:

As an option, the inverse kinematic analysis model of the singular robust solution is established as follows:

Based on the Jacobian pseudo-inverse solution, there will be kinematic singularity, which is caused by the sub-matrix; for the kinematic singularity problem, the DLS (damped least squares) solution should also be given;

The cost function for the DLS solution can be modified as:

Therefore, the singular robust pseudo-inverse solution of the above equation can be expressed as:

Equation (15) is the inverse kinematic analysis model of the singular robust solution. Let λ=η ² I, the above DLS solution is equivalent to the additional regularization solution, and the scalar value η balances the task accuracy 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)

where U∈R ^m×m , V∈R ^n×n , ∑∈R ^m×n , U is a univariate matrix composed of column vectors _ui , _V is a univariate matrix composed of column vectors vi, ∑ is m× A block matrix of n-diagonal matrices containing the singular values σ _i ≥ 0 of J containing nm zero-column vectors in decreasing order;

where r≤m is the rank of matrix J;

For motion singularity, referring to the singular value decomposition SVD required to calculate the pseudo-inverse solution, the large joint velocity is due to the rapid approach of the smallest singular value to 0, as follows:

The factor _λ0 will affect the singularity, the higher the value of _λ0 , the greater the damping and the closer the joint velocity is to the singularity; in addition, the strategies for defining variable damping factors are different; we can obtain

From the above equation, we can see that the parameter δ>0 monitors the smallest singular value.

As an option, the inverse priority control strategy for establishing a multi-task redundant manipulator is as follows;

该矩阵包括了不依赖于第k个任务的最低优先级l-k-1任务的相应元素的零空间，所以得出Introducing the Inverse Preferential Projection Matrix

This matrix includes the null space of the corresponding elements of the lowest priority lk-1 task independent of the k-th task, so we get

where J _i|j is the Jacobian matrix associated with all components of the i-th task linearly independent of the j-th task;

Therefore, the priority derivation formula is as follows:

In the above derivation, k=l,l-1,...,1; initial value

In order to give the general computational form of the linearly independent Jacobian matrix J, the inverse augmented Jacobian matrix is defined as:

there is

表示

的行；in

express

the line;

的伪逆解就可表示为：So,

The pseudo-inverse solution can be expressed as:

and

的扩充；where T _k represents the matrix

expansion;

The final reverse priority projection can be written as:

Thus, we can obtain the expression for the pseudo-inverse solution:

The inverse priority control strategy equation for establishing the multi-task redundant manipulator is as follows:

Preferably, the inverse governing equations of redundant manipulators with primary and secondary tasks are simplified as follows:

For 6DOF or 7DOF redundant manipulators, there are not enough 6DOF DOFs to complete multiple levels of tasks; it is necessary to implement dual-task priority control; that is, the motion control of the manipulator is the main task and secondary tasks;

The inverse control equations for redundant manipulators with primary and secondary tasks are as follows

是次要任务，

是主任务；主任务在主任务的指定零空间中实现；反向优先级的核心点是投影矩阵

的计算；

的表达式如公式(30)：The above equation is quite different from the previous expression (11), but the algorithm framework is similar; in the above equation,

is a secondary task,

is the main task; the main task is implemented in the designated null space of the main task; the core point of the reverse priority is the projection matrix

calculation;

is expressed as formula (30):

Using similar derivations in the previous equations (22)-(28), the simplified inverse control equation of the redundant manipulator with primary and secondary tasks can be obtained:

As an option, the inverse priority control strategy of the manipulator is established as follows:

The dynamics of the manipulator in the force-controlled space can be written as:

是非线性力，F是输入控制力，F_e是接触力；where X is the position in Cartesian space, M(X) is the inertia matrix,

is the nonlinear force, F is the input control force, and F _e is the contact force;

In addition, the transformation based on the Jacobian matrix can obtain the input joint torque

τ=J ^T (q)F (33)

The desired motion equation of the manipulator in the force control space can be defined as follows:

where M _d and B _d are the inertia and damping matrices; F _d is the command force, and F _e is the contact force;

Therefore, the relationship between the environment and the manipulator response can be written as

The combination of the above two equations is as follows

As can be seen from the above equation, if _{Me, Be and Ke are known, the adjustment of M d and B d will affect the system} response;

Force control enables the manipulator to interact with the environment or humans; in addition, in some cases, it is not necessary to achieve full force control, and it is not necessary to guarantee full force control, that is, sometimes we just want to guarantee a certain Directional force tracking control accuracy;

Therefore, it is necessary to perform hierarchical force control on the manipulator; that is, it is necessary to give a new hierarchical force control framework; from the above equations, we can obtain the desired hierarchical force control relationship as follows

So the integral formula of these two equations can be written as

和

则可实现机械手的精确力控制；笛卡尔速度与关节速度的关系应借鉴逆优先控制；因此，可得到机械手的逆优先力控制策略的方程：If the robotic end effector can track the desired Cartesian velocity as

and

Then the precise force control of the manipulator can be realized; the relationship between the Cartesian speed and the joint speed should be learned from the inverse priority control; therefore, the equation of the manipulator's inverse priority force control strategy can be obtained:

The joint speed required by the above equation will ensure the force control of the manipulator; it is worth mentioning that the above force control law is only a speed level control law, which depends on the inner velocity loop control; if the inner position control effect is good, it can achieve accurate control. Force control; since the inner velocity loop control can realize low frequency position tracking, the outer force loop can realize low frequency force tracking.

As an option, the joint velocity is used to solve the relationship between the external force and the joint acceleration in the inverse preferential impedance control of the manipulator, so as to obtain the guarantee of the inverse preferential impedance control of the manipulator as follows:

When the manipulator implements force control, the manipulator plays the role of the initiator to a certain extent, that is, the manipulator is ready to respond to the external environment; when the manipulator works as an impedance control model, the manipulator responds passively external force;

The corresponding impedance relationship between external force and joint acceleration can be expressed as

The reference speed can be expressed as

Therefore, the guaranteed expression for the inverse priority impedance control of the manipulator is:

Preferably, the inverse priority calculation of the position control space is extended to the inverse priority calculation of the force control space, so as to obtain the overall framework of the inverse priority impedance control of the speed level of the manipulator. The implementation method is as follows:

The hybrid impedance application is a combination of the above two strategies, that is, the Cartesian task can be divided into two cases: the first is the position control subspace, in which the impedance control is realized; the second is the force control subspace , the force control is implemented in this subspace;

Therefore a selection matrix is chosen; the relationship between the external force and the position response is as follows

So the simplified form of the desired velocity can be expressed as

Then we got the solution based on reverse priority

Considering the n-layer task, the corresponding impedance control task also belongs to the n-layer framework. Therefore, the overall framework expression of the inverse priority impedance control of the manipulator speed class is as follows:

Expression (52) solves the problem of extending the inverse priority calculation of the position control space to the inverse priority hybrid impedance control of the manipulator in the inverse priority calculation of the force control space, which can make the redundant manipulator of the manipulator achieve the desired expectations under different hierarchical structures. impedance control task.

The present invention can achieve the following effects:

The invention can control the balance of the manipulator, the manipulator is easy to move, and the redundant manipulator of the manipulator can realize the desired impedance control task under different hierarchical structures.

Description of drawings

Figure 1 is a schematic diagram of the kinetic scheme of the force control of the present invention.

FIG. 2 is a schematic diagram of the kinetic scheme of the impedance control of the present invention.

FIG. 3 is a schematic diagram of the kinetic scheme of the hybrid impedance control of the present invention.

FIG. 4 is a schematic diagram of a connection structure of a seven-degree-of-freedom manipulator 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 ways

The present invention will be further described below with reference to the accompanying drawings and embodiments.

In an embodiment, a mobile manipulator redundant manipulator inverse priority impedance control system, as shown in FIG. 4 and FIG. 5 , includes a manipulator and a console S31 for controlling the manipulator; also includes a movable mobile platform S41; the manipulator includes a manipulator , mounting seat S1, vertical column S2, output gripper S12 and vertical cylinder S23; the mounting seat is fixed on the mobile platform;

The robotic arm includes a vertical lift mobile platform S3, an arm segment 1 S6, an arm segment 2 S7, an arm segment 3 S8 and an arm segment 4 S10;

A vertical rail S24 is provided on the left surface of the vertical column, and the vertical lifting and moving platform is vertically slid up and down on the vertical rail; the lower end of the vertical column is fixedly connected to the upper surface of the mounting seat, and the vertical cylinder The cylinder seat S21 is fixedly connected on the upper surface of the mounting seat located on the left side of the vertical track, the telescopic rod S22 of the vertical cylinder is arranged vertically upward, and the upper end of the telescopic rod of the vertical cylinder is fixedly connected to the lower part of the vertical lifting mobile platform. On the surface; the vertical lifting mobile platform can move up and down along the vertical track under the driving of the telescopic rod of the vertical cylinder to form the first degree of freedom;

The first arm section includes the A1 section pipe S13 and the A2 section pipe S14 telescopically connected in the left nozzle of the A1 section pipe. The right end of the A1 section pipe is fixed with the No. 1 cylinder S25 with the telescopic rod horizontally arranged to the left. The telescopic rod is fixedly connected to the right end of the A2 section pipe;

The second arm section includes the B1 section pipe S16 and the B2 section pipe S17 telescopically connected in the left nozzle of the B1 section pipe. The right end of the B1 section pipe is fixed with a No. 2 cylinder 37 with a telescopic rod horizontally arranged to the left. The telescopic rod is fixedly connected to the right end of the B2 section pipe;

At the left end of the vertical lift mobile platform, there is a No. 1 horizontal rotating shaft S4 driven by a No. 1 geared motor S26, and the right end of the A1 section pipe is fixedly connected to the No. 1 horizontal rotating shaft, so that the first arm can rotate horizontally to form The second degree of freedom; the No. 1 horizontal rotating shaft is also provided with a No. 1 electromagnetic brake S32 that can control the rotation of the No. 1 horizontal rotating shaft;

The left end of the A2 section tube is provided with a No. 2 horizontal rotating shaft S15 driven by a No. 2 reduction motor S27, and the right end of the B1 section tube is fixedly connected to the No. 2 horizontal rotating shaft, so that the second arm can rotate horizontally to form the third Degree of freedom; on the No. 2 horizontal rotating shaft, there is also a No. 2 electromagnetic brake S33 that can control the rotation of the No. 2 horizontal rotating shaft;

The left end of the B2 section tube is provided with a No. 3 horizontal rotating shaft S18 driven by a No. 3 geared motor S28, and the right end of the third arm is fixedly connected to the No. 3 horizontal rotating shaft, so that the third arm can rotate horizontally to form the fourth Degree of freedom; there is also a No. 3 electromagnetic brake S34 on the No. 3 horizontal rotating shaft, which can control the No. 3 horizontal rotating shaft to rotate;

The left end of the arm section 3 is provided with a No. 1 horizontal vertical rotation shaft S9 driven by a No. 4 reduction motor S29 that can rotate on the left and right vertical planes, and the right end of the arm section 4 is fixedly connected to the No. 1 horizontal vertical rotation. On the shaft, the arm section 4 can be rotated vertically on the left and right vertical planes to form the fifth degree of freedom; the No. 1 horizontal vertical rotating shaft is also provided with a No. 4 electromagnetic brake that can control the rotation of the No. 1 horizontal and vertical rotating shaft. S35;

The left end of the arm section 4 is provided with a No. 1 longitudinal vertical rotation shaft S11 driven by a No. 5 geared motor S30 that can rotate on the front and rear vertical planes, and the right end of the output gripper is fixedly connected to the No. 1 longitudinal vertical rotation. On the shaft, the right end of the output gripper can be rotated vertically on the front and rear vertical planes to form a sixth degree of freedom; on the No. 1 vertical vertical rotation shaft, there is also a No. 5 vertical rotation shaft that can control the rotation of the No. 1 vertical vertical rotation shaft Electromagnetic brake S36;

The A2 section tube can be telescopically moved left and right in the A1 section tube under the drive of the telescopic rod of the No. 1 cylinder to form the seventh degree of freedom;

The B2 section tube can be telescopically moved left and right in the B1 section tube under the drive of the telescopic rod of the No. 2 cylinder to form the eighth degree of freedom;

The left end of a No. 1 horizontal pipe S39 is horizontally and fixedly connected to the right surface of the vertical column, and a balance adjustment block S40 is slid left and right in the No. 1 horizontal pipe. The left end of the No. 1 horizontal pipe is fixedly connected with a telescopic rod that faces horizontally to the right. The balance adjustment cylinder S38, the right end of the telescopic rod of the balance adjustment cylinder is fixedly connected to the balance adjustment block;

The control terminal of No. 1 electromagnetic brake, the control terminal of No. 2 electromagnetic brake, the control terminal of No. 3 electromagnetic brake, the control terminal of No. 4 electromagnetic brake, the control terminal of No. 5 electromagnetic brake, the control terminal of No. 1 reduction motor, the control terminal of No. 2 electromagnetic brake The control end of the gear motor, the control end of the No. 3 gear motor, the control end of the No. 4 gear motor, the control end of the No. 5 gear motor, the control end of the No. 1 cylinder, the control end of the No. 2 cylinder, the control end of the balance adjustment cylinder and the control end of the vertical cylinder are respectively controlled and connected to the console. The seventh degree of freedom allows the manipulator to extend freely, and the eighth degree of freedom enables the manipulator to extend freely, greatly increasing the working range and flexibility.

The balance adjustment cylinder controls the left and right movement of the balance adjustment block to control the balance of the vertical column, thereby controlling the balance of the manipulator. The manipulator is easy to move. Mobile platforms include automobiles.

Since some of the manipulators of the present invention have eight degrees of freedom, they have good flexibility and high reliability, and are easy to complete control tasks.

See Figure 1-3 for the inverse priority impedance control method of the redundant manipulator of the mobile manipulator. It includes the following steps:

Step 1. Establish the kinematic model of the redundant manipulator, and give the gradient direction strategy of the zero-space vector of the redundant manipulator. The realization process is as follows:

其关节空间的角位置、角速度分别为q、

J为n自由度机器人的雅可比矩阵，其中x∈Rⁿ，

J∈R^m-n；冗余自由度机械臂的正运动学方程可用下式描述：Define the pose and velocity of the end effector in Cartesian space as x,

The angular position and angular velocity of its joint space are q,

J is the Jacobian matrix of the n-degree-of-freedom robot, where x ∈ R ⁿ ,

J∈R ^mn ; the forward kinematic equation of the redundant degree-of-freedom manipulator can be described by the following formula:

Equation (1) is also called the kinematic velocity model of the manipulator;

Considering the solution of least squares, the optimal problem can be listed as:

来解决；The solution of equation (1) can be obtained by finding the best

to solve;

Therefore, the pseudo-inverse solution of equation (1) can be expressed as:

where J ⁺ ——the pseudo-inverse of the Jacobian matrix

I - identity matrix

——任意零空间矢量

— Arbitrary null space vector

——最小范数解，定义了手的动作

- the minimum norm solution, which defines the motion of the hand

——齐次解，在末端不产生动作

- Homogeneous solution, no action at the end

Equation (4) represents the position and attitude control of the end effector; adding arbitrary residuals to Equation (4) can obtain a general expression including the null space; using the above equations, multi-task optimization can be achieved on the zero vector;

However, the above equation ignores the ill-conditioned Jacobian; the regularization equation can be modified by adding additional regularization values,

为加权系数，并且满足where λ≥0 is the weighting matrix,

is the weighting coefficient, and it satisfies

The solution of the above equation can be expressed as:

Equation (7) is also called the 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:

Step 2. Establish a task priority solution strategy for obtaining the singularity elimination algorithm through the singular robust solution as follows:

In the redundant manipulator solution of the Jacobian matrix, the optimization task is realized in the null space of the main task; the inverse task kinematics is established on the basis of the forward task kinematics:

和

表示task1和task2in

and

Indicates task1 and task2

From expression (5), the inverse kinematics equation of the redundant manipulator is:

是在

的零空间中实现的；冗余机械臂的最终逆运动学表达式如下所示：Task1 as the main task and task2 as the auxiliary task; that is,

is in

is implemented in the null space of ; the final inverse kinematics expression for the redundant manipulator is as follows:

in

是投影矩阵，它给出了次要任务到主要任务的适用范围；

和

是所需的命令速度；

是主要任务，

是次要任务；

is the projection matrix, which gives the applicability of the secondary task to the primary task;

and

is the desired command speed;

is the main task,

is a secondary task;

If two correlated tasks are interdependent, the corresponding Jacobian matrix is singular; if the task Jacobian is singular, the corresponding task is unsatisfied; in this case, the Jacobian correlation matrix will be Singularity, defined as algorithm singularity;

That is, if

where ρ( ) is the rank of the matrix;

It is obvious that the singularity of the algorithm is caused by the task conflict between the secondary task and the primary task; moreover, the inverse kinematics of the redundant manipulator based on the task priority aims to provide better control over the effectiveness of the primary task;

Therefore, let the position control the direction as the main task, so that the position guarantees the accuracy of the control direction task; then establish the task priority solving strategy equation of the singularity elimination algorithm through the singular robust solution:

Step 3. Establish a singular robust solution inverse kinematics analysis model as follows:

Based on the Jacobian pseudo-inverse solution, there will be kinematic singularity, which is caused by the sub-matrix; for the kinematic singularity problem, the DLS (damped least squares) solution should also be given;

The cost function for the DLS solution can be modified as:

Therefore, the singular robust pseudo-inverse solution of the above equation can be expressed as:

Equation (15) is the inverse kinematic analysis model of the singular robust solution. Let λ=η ² I, the above DLS solution is equivalent to the additional regularization solution, and the scalar value η balances the task accuracy 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)

where U∈R ^m×m , V∈R ^n×n , ∑∈R ^m×n , U is a unary matrix consisting of column vectors _ui , V is a unary matrix consisting of column vectors vi, ∑ is m× _n A block matrix of n-diagonal matrices containing the singular values σ _i ≥ 0 of J containing nm zero-column vectors in decreasing order;

where r≤m is the rank of matrix J;

For motion singularity, referring to the singular value decomposition SVD required to calculate the pseudo-inverse solution, the large joint velocity is due to the rapid approach of the smallest singular value to 0, as follows:

The factor _λ0 will affect the singularity, the higher the value of _λ0 , the greater the damping and the closer the joint velocity is to the singularity; in addition, the strategies for defining variable damping factors are different; we can obtain

From the above equation, we can see that the parameter δ>0 monitors the smallest singular value.

Step 4. The inverse priority control strategy for establishing the multi-task redundant manipulator is as follows;

该矩阵包括了不依赖于第k个任务的最低优先级l-k-1任务的相应元素的零空间，所以得出Introducing the Inverse Preferential Projection Matrix

This matrix includes the null space of the corresponding elements of the lowest priority lk-1 task independent of the k-th task, so we get

where J _ij is the Jacobian matrix associated with all components of the i-th task linearly independent of the j-th task;

Therefore, the priority derivation formula is as follows:

In the above derivation, k=l,l-1,...,1; initial value

In order to give the general computational form of the linearly independent Jacobian matrix J, the inverse augmented Jacobian matrix is defined as:

there is

表示

的行；in

express

the line;

的伪逆解就可表示为：So,

The pseudo-inverse solution can be expressed as:

and

的扩充；where T _k represents the matrix

expansion;

The final reverse priority projection can be written as:

Thus, we can obtain the expression for the pseudo-inverse solution:

The inverse priority control strategy equation for establishing the multi-task redundant manipulator is as follows:

Step 5. Simplify the inverse control equation of the redundant manipulator with primary tasks and secondary tasks as follows:

For 6DOF or 7DOF redundant manipulators, there are not enough 6DOF DOFs to complete multiple levels of tasks; it is necessary to implement dual-task priority control; that is, the motion control of the manipulator is the main task and secondary tasks;

The inverse control equations for redundant manipulators with primary and secondary tasks are as follows

是次要任务，

是主任务；主任务在主任务的指定零空间中实现；反向优先级的核心点是投影矩阵

的计算；

的表达式如公式(30)：The above equation is quite different from the previous expression (11), but the algorithm framework is similar; in the above equation,

is a secondary task,

is the main task; the main task is implemented in the designated null space of the main task; the core point of the reverse priority is the projection matrix

calculation;

is expressed as formula (30):

Using similar derivations in the previous equations (22)-(28), the simplified inverse control equation of the redundant manipulator with primary and secondary tasks can be obtained:

Step 6. Establish the inverse priority control strategy of the manipulator as follows:

The dynamics of the manipulator in the force-controlled space can be written as:

是非线性力，F是输入控制力，F_e是接触力；where X is the position in Cartesian space, M(X) is the inertia matrix,

is the nonlinear force, F is the input control force, and F _e is the contact force;

In addition, the transformation based on the Jacobian matrix can obtain the input joint torque

τ=J ^T (q)F (33)

The desired motion equation of the manipulator in the force control space can be defined as follows:

where M _d and B _d are the inertia and damping matrices; F _d is the command force, and F _e is the contact force;

The dynamic scheme of force control is shown in Figure 1;

Therefore, the relationship between the environment and the manipulator response can be written as

The combination of the above two equations is as follows

As can be seen from the above equation, if _{Me, Be and Ke are known, the adjustment of M d and B d will affect the system} response;

Force control enables the manipulator to interact with the environment or humans; in addition, in some cases, it is not necessary to achieve full force control, and it is not necessary to guarantee full force control, that is, sometimes we just want to guarantee a certain Directional force tracking control accuracy;

For example, when the manipulator interacts with the planer, it is only necessary to maintain precise force-tracking control in the vertical direction, but not in the other direction; in other cases, position-direction force control is more efficient than attitude-direction force control important;

Therefore, it is necessary to perform hierarchical force control on the manipulator; that is, it is necessary to give a new hierarchical force control framework; from the above equations, we can obtain the desired hierarchical force control relationship as follows

So the integral formula of these two equations can be written as

和

则可实现机械手的精确力控制；笛卡尔速度与关节速度的关系应借鉴逆优先控制；因此，可得到机械手的逆优先力控制策略的方程：If the robotic end effector can track the desired Cartesian velocity as

and

Then the precise force control of the manipulator can be realized; the relationship between the Cartesian speed and the joint speed should be learned from the inverse priority control; therefore, the equation of the manipulator's inverse priority force control strategy can be obtained:

The joint speed required by the above equation will ensure the force control of the manipulator; it is worth mentioning that the above force control law is only a speed level control law, which depends on the inner velocity loop control; if the inner position control effect is good, it can achieve accurate control. Force control; since the inner velocity loop control can realize low frequency position tracking, the outer force loop can realize low frequency force tracking.

Step 7. Use the joint speed to solve the relationship between the external force and the joint acceleration in the inverse priority impedance control of the manipulator, so as to obtain the realization method of the inverse priority impedance control guarantee of the manipulator as follows:

When the manipulator implements force control, the manipulator plays the role of the initiator to a certain extent, that is, the manipulator is ready to respond to the external environment; when the manipulator works as an impedance control model, the manipulator responds passively External force; the dynamic scheme of impedance control is shown in Figure 2;

The corresponding impedance relationship between external force and joint acceleration can be expressed as

The reference speed can be expressed as

Therefore, the guaranteed expression for the inverse priority impedance control of the manipulator is:

Step 8. Extend the inverse priority calculation of the position control space to the inverse priority calculation of the force control space, so as to obtain the overall framework of the inverse priority impedance control of the speed level of the manipulator. The implementation method is as follows:

The hybrid impedance application is a combination of the above two strategies, that is, the Cartesian task can be divided into two cases: the first is the position control subspace, in which the impedance control is realized; the second is the force control subspace , the force control is implemented in this subspace;

Therefore a selection matrix is chosen; the relationship between the external force and the position response is as follows

So the simplified form of the desired velocity can be expressed as

Then we got the solution based on reverse priority

The kinetic scheme of hybrid impedance control is shown in Figure 3;

Considering the n-layer task, the corresponding impedance control task also belongs to the n-layer framework. Therefore, the overall framework expression of the inverse priority impedance control of the manipulator speed class is as follows:

Expression (52) solves the inverse priority hybrid impedance control of the manipulator by extending the inverse priority calculation of the position control space to the inverse priority calculation of the force control space; it can make the redundant manipulator of the manipulator realize the expectation under different hierarchical structures. impedance control task.

Claims (10)

1. A redundant robot arm inverse priority impedance control system for a mobile robot, comprising a robot and a console for controlling the robot (S31); characterized in that the device also comprises a movable moving platform (S41); the robot includes a robot 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, 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 of pipe (S13) and an A2 section of pipe (S14) which is telescopically connected in a left pipe orifice of the A1 section of 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 of pipe, and the telescopic rod of the first air cylinder is fixedly connected at the right end of the A2 section of 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;

the left end of the vertical lifting mobile platform is provided with a first horizontal rotating shaft (S4) driven by a first speed reducing motor (S26), and the right end of the A1 section of 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 rotation of the first horizontal rotating shaft is also arranged on the first horizontal rotating shaft;

the left end of the A2 section of pipe is provided with a second horizontal rotating shaft (S15) driven by a second speed reducing motor (S27), and the right end of the B1 section of pipe is fixedly connected to the second horizontal rotating shaft, so that the second arm section 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 also arranged on the second horizontal rotating shaft;

the left end of the B2 section 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 also 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 vertical rotating shaft to rotate is further arranged on the first vertical rotating shaft;

the section A2 of the pipe can be driven by the telescopic rod of the first cylinder to move in a telescopic manner from side to side in the section A1 of the pipe to form a seventh degree of freedom;

the B2 section of pipe can be driven by the telescopic rod of the second cylinder to move in the B1 section of pipe in a left-right telescopic manner to form an eighth degree of freedom;

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.

2. A reverse priority impedance control method for a redundant mechanical arm of a mobile manipulator is characterized by comprising 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.

3. The reverse priority impedance control method for the redundant manipulator of the mobile manipulator according to claim 2, wherein the implementation process of establishing a kinematic model of the redundant manipulator and giving a gradient direction strategy of a zero space vector of the redundant manipulator is as follows:

the pose and the speed of the end effector in the Cartesian space are defined as x,

The angular position and angular velocity of the joint space are q,

J is a Jacobian matrix of the n-degree-of-freedom robot, where x ∈ Rⁿ，

J∈R^m-n(ii) a The positive kinematic equation for a redundant degree of freedom robotic arm can be described by:

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:

the solution of formula (1) can be optimized by finding the best

To solve the problem;

thus, the pseudo-inverse solution of equation (1) can be expressed as:

in the formula J⁺Pseudo-inverse of the Jacobian matrix

I-identity matrix

-arbitrary null space vector

Minimum norm solution, defining hand movements

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,

where λ ≧ 0 is the weighting matrix,

is a weighting coefficient, and satisfies

The solution to the above equation can be expressed as:

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:

4. the inverse priority impedance control method for the redundant manipulator of the mobile manipulator according to claim 3, wherein 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:

wherein

And

representation task1 and task2

The inverse kinematics equation of the redundant manipulator is obtained from expression (5) as follows:

task1 as the main Task and Task2 as the auxiliary Task; that is, task2

Is at task1

Is implemented in the null space of (1); the final inverse kinematics expression for the redundant manipulator is as follows:

wherein

Is a projection matrix which gives the applicable range of the secondary task to the primary task;

and

is the desired commanded speed;

is the main task of the method, and the method comprises the following steps of,

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

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:

5. the method for controlling the inverse priority impedance of the redundant manipulator of the mobile manipulator according to claim 4, wherein a 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:

thus, the singular robust pseudo-inverse solution of the above equation can be expressed as:

equation (15) is a singular robust solution inverse kinematics analysis model, where λ is η²I, the DLS solution is equivalent to an additional regularization solution, and the scalar value η 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 ∈ R^m×m，V∈R^n×n，∑∈R^m×nU is a column vector U_IA unitary matrix of V is formed by column vectors V_IThe constituent unary matrix, ∑, is a block matrix of m × n diagonal matrices containing the singular values σ of J_iMore than or equal to 0 contains n-m zero column vectors in descending order;

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:

factor lambda₀Will affect the singularity, λ₀The higher the value is, the larger the damping is, and the closer the joint velocity is to the singular point; in addition, the definition canThe strategy of varying the damping factor is also different; we can get

From the above equation, we can see that the parameter > 0 monitors the smallest singular value.

6. The method according to claim 5, wherein a reverse priority control strategy for the multitasking redundant robot is established as follows;

introducing an inverse-priority projection matrix

The matrix comprises a null space of corresponding elements of the lowest priority l-k-1 task independent of the kth task, so derived

Wherein J_i|jIs 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:

in the above derivation, k ═ l, l-1, …, 1; initial value

To give a general form of computation of the linear independent Jacobian matrix J, the inverse augmented Jacobian matrix is defined as:

have therein

Wherein

To represent

A row of (2);

therefore, the method is not to be taken,

the pseudo-inverse solution of (a) can be expressed as:

and

wherein, T_kRepresentation matrix

Expansion of (2);

the final inverse priority projection can be written as:

thus, we can derive the expression of the pseudo-inverse solution:

the reverse priority control strategy equation of the multi-task redundant mechanical arm is established as follows:

7. the method according to claim 6, wherein the inverse control equation of the redundant robot having the primary task and the secondary task is simplified as follows:

for a six-degree-of-freedom or seven-degree-of-freedom redundant manipulator, there are not enough six-degree-of-freedom DOF to complete multiple levels of tasks; 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

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,

is a secondary task that is to be performed,

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

Calculating (1);

is expressed as formula (30):

using similar guides as in previous equations (22) - (28), a simplified inverse control equation for a redundant robotic arm having a primary task and a secondary task can be obtained:

8. the method of claim 7, wherein the reverse priority impedance control strategy for the redundant robot arm of the transfer robot is established as follows:

the dynamics of the manipulator in force control space can be written as:

where X is the position in Cartesian space, M (X) is an inertial matrix,

non-linear force, F input control force, F_eIs 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:

wherein M is_dAnd B_dIs an inertia and damping matrix; f_dIs a command force, F_eIs the contact force;

thus, the relationship between the environment and the manipulator response can be written as

The combination of the above two equations is as follows

As can be seen from the above equation, if M is_e、B_eAnd K_eKnown, then M_dAnd B_dWill 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 provide a new hierarchical force control framework; from the above equation we can derive the desired hierarchical force control relationship as follows

The integral of these two equations can be written as

If the robot end effector is able to track the desired cartesian velocity of

And

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:

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 control can realize low-frequency position tracking, the outer force ring can realize low-frequency force tracking.

9. The method according to claim 8, wherein the relationship between an external force and a joint acceleration in the reverse priority impedance control of the manipulator is solved by using a joint velocity, and the reverse priority impedance control of the manipulator is ensured by:

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

The reference speed can be expressed as

Therefore, the inverse priority impedance control of the manipulator guarantees the expression:

10. the method of claim 9, wherein the overall framework for achieving the manipulator velocity-level inverse-priority impedance control by extending the inverse-priority computation of the position control space to the inverse-priority computation of the force control space is implemented 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

So a simplified form of the desired speed can be expressed as

Then we get a solution based on reverse priority

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

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 the expected impedance control task under different hierarchical structures.

Images