CN111687834B - Inverse Priority Impedance Control System and Control Method for Redundant Manipulator Arm of Mobile Manipulator - Google Patents

Abstract

The invention discloses a mobile manipulator redundant mechanical arm reverse priority impedance control system and a control method. The mechanical arm is convenient to move and comprises a movable moving platform, a mechanical arm and a control console for controlling the mechanical arm; 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 moving table 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 at 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 mount pad is fixed on moving platform.

Description

Reverse priority impedance control system and control method for redundant mechanical arms of mobile manipulator

Technical Field

The present invention relates to the technical field of reverse priority impedance control of redundant manipulator arms of manipulators, and in particular to a reverse priority impedance control system and a control method for redundant manipulator arms of mobile manipulators.

Background Art

The control method currently used 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 a bus and uniformly controlled and coordinated by a controller;

A six-degree-of-freedom robot is a robot with the minimum number of degrees of freedom to complete spatial positioning. Robots with more than six degrees of freedom are collectively referred to as redundant robots.

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

Summary of the invention

The present invention aims to solve the deficiency that the existing robot arm impedance control method cannot achieve the desired impedance control task under different hierarchical structures, and provides a mobile robot arm redundant robot arm reverse priority impedance control system and control method which can control the balance of the robot arm to facilitate the movement of the robot arm and enable the redundant robot arms of the robot arm to achieve the desired impedance control task under different hierarchical structures.

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

A mobile manipulator redundant manipulator reverse priority impedance control system includes a manipulator and a console for controlling the manipulator; and also includes a movable mobile platform; the manipulator includes a manipulator 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 includes a vertical lifting and moving platform, arm segment one, arm segment two, arm segment three and arm segment four;

A vertical track is provided on the left surface of the vertical column, and the vertical lifting and moving platform is vertically slidably arranged on the vertical track; 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 located on the left side of the vertical track, the telescopic rod 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 surface of the vertical lifting and moving platform; the vertical lifting and moving platform can move up and down along the vertical track driven by the telescopic rod of the vertical cylinder, forming a first degree of freedom;

The arm section 1 includes an A1 section pipe and an A2 section pipe telescopically connected to the left pipe opening of the A1 section pipe. A No. 1 cylinder with a telescopic rod horizontally arranged to the left is fixedly provided at the right end of the A1 section pipe. The telescopic rod of the No. 1 cylinder is fixedly connected to the right end of the A2 section pipe.

The arm section 2 includes a B1 section pipe and a B2 section pipe telescopically connected to the left pipe opening of the B1 section pipe. A No. 2 cylinder with a telescopic rod horizontally arranged to the left is fixedly provided at the right end of the B1 section pipe. The telescopic rod of the No. 2 cylinder is fixedly connected to the right end of the B2 section pipe.

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

A second horizontal rotation axis driven by a second reduction motor is provided at the left end of the A2 section tube, and the right end of the B1 section tube is fixedly connected to the second horizontal rotation axis, so that the arm section 2 can rotate horizontally to form a third degree of freedom; a second electromagnetic brake is also provided on the second horizontal rotation axis to control the rotation of the second horizontal rotation axis;

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

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

At the left end of arm section 4, there is a No. 1 longitudinal vertical rotation axis driven by a No. 5 reduction motor and capable of rotating on the front and rear vertical planes. The right end of the output gripper is fixedly connected to the No. 1 longitudinal vertical rotation axis, 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 rotation axis is also provided with a No. 5 electromagnetic brake capable of controlling the rotation of the No. 1 longitudinal vertical rotation axis;

Driven by the telescopic rod of the No. 1 cylinder, the A2 section of pipe can move left and right in the A1 section of pipe to form the seventh degree of freedom;

Driven by the telescopic rod of the No. 2 cylinder, the B2 section of pipe can telescope left and right inside the B1 section of pipe to form the eighth degree of freedom;

The left end of a No. 1 horizontal tube is horizontally fixedly connected to the right surface of the vertical column. A balance adjustment block is provided in the No. 1 horizontal tube for sliding left and right. A balance adjustment cylinder with a telescopic rod facing horizontally to the right is fixedly connected to the left end of the No. 1 horizontal tube. The right end of the telescopic rod of the balance adjustment cylinder is fixedly connected to the balance adjustment block.

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

The balance adjustment cylinder controls the balance adjustment block to move left and right to control the balance of the vertical column, and then controls the balance of the manipulator. It is convenient for the manipulator to move. Since the manipulator of the present invention has eight degrees of freedom, it has good flexibility, high reliability, and is easy to complete the control task.

A method for reverse priority impedance control of redundant mechanical arms of a mobile manipulator comprises the following steps:

Step 1: Establish a kinematic model of the redundant manipulator and provide a gradient direction strategy of the redundant manipulator null space vector;

Step 2: Establish a task priority solution strategy for eliminating singularity through a singular robust solution;

Step 3, establishing a singular robust solution inverse kinematics analysis model;

Step 4: Establish an inverse priority control strategy for the multi-task redundant manipulator;

Step 5, simplifying the inverse control equation of the redundant manipulator with the primary task and the secondary task;

Step 6: Establish the inverse priority force control strategy of the manipulator;

Step 7: Use joint velocity to solve the relationship between external force and joint acceleration in the inverse priority impedance control of the manipulator, so as to obtain the inverse priority impedance control guarantee 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, thereby obtaining the overall framework of the manipulator speed-level inverse priority impedance control.

In this scheme, the motion of the redundant manipulator in the joint space is derived in reverse order; then the Cartesian impedance control is combined with the inverse priority impedance control to solve the inverse hierarchical impedance control problem, and the Cartesian impedance control behavior is divided into 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 opposite order, and it is necessary to work in the corresponding projection operator; finally, the high priority impedance control task is realized, avoiding the deformation caused by the singularity that may appear in the low priority impedance control task. Therefore, the proposed inverse priority impedance control method can enable the redundant manipulator to achieve the desired impedance control task under the appropriate hierarchy.

As a preferred method, a kinematic model of a redundant manipulator is established, and the gradient direction strategy implementation process of the redundant manipulator null space vector is given as follows:

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

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

J∈R^m×n；冗余自由度机械臂的正运动学方程可用下式描述：Define the position 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-DOF robot, where x∈R ⁿ ,

J∈R ^m×n ; The forward kinematics equation of the redundant degree of freedom manipulator can be described by the following formula:

Formula (1) is also called the robot arm kinematic velocity model;

Taking into account the solution of the least squares method, the optimal problem can be stated as:

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

to solve;

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

Where J ⁺ ——pseudo-inverse of Jacobian matrix

I——Identity Matrix

——任意零空间矢量

——Arbitrary null space vector

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

——Minimum norm solution, defining the hand movements

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

——Homogeneous solution, no action is produced at the end

Formula (4) represents the position and attitude control of the end effector. By adding any residual to Formula (4), a general expression including the null space can be obtained. The above equation can be used to realize multi-task optimization on the null vector.

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

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

is the weighting coefficient and satisfies

The solution to the above equation can be expressed as:

Formula (7) is also called the redundant manipulator kinematic model;

The joint constraint function of the joint constraint gradient direction of the position-dependent scalar index of the redundant manipulator null space vector is:

As a preferred method, the task priority solution strategy of eliminating singularity algorithm through singular robust solution is established as follows:

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

和

表示task1和task2in

and

Represents task1 and task2

From expression (9), the inverse kinematics equation of the redundant manipulator is obtained as follows:

是在task1

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

It is in task1

The final inverse kinematics expression of 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,

It is a secondary task;

If two related tasks are interdependent, the corresponding Jacobian matrix is singular; if the task Jacobian matrix is singular, the corresponding tasks are unsatisfiable; in this case, the Jacobian correlation matrix will be a singular point, defined as an algorithmic 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 and primary tasks; moreover, the redundant manipulator inverse kinematics based on task priority is designed to provide better effectiveness in controlling the primary task;

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

As a preferred method, a singular robust solution inverse kinematics analysis model is established as follows:

Based on the Jacobi pseudo-inverse solution, kinematic singularities will occur, which are caused by the secondary matrix; for the kinematic singularity problem, the DLS (damped least squares) solution should also be given;

The cost function of the DLS solution can be modified as follows:

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

Formula (15) is the inverse kinematics analysis model of the singular robust solution. Let λ = η ^Δ I. The above DLS solution is equivalent to the additional regularization solution. 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 one-element matrix consisting of column vectors u _i , V is a one-element matrix consisting of column vectors _vi , ∑ is a block matrix of an m×n diagonal matrix containing the singular values σ _i ≥ 0 of J in decreasing order and containing nm zero column vectors;

Where r≤m is the rank of the matrix J;

For motion singularity, refer to the singular value decomposition SVD required to calculate the pseudo-inverse solution. The large joint velocity is due to the small singular value rapidly approaching 0, as shown in the following formula:

The factor λ ₀ will affect the singularity. The higher the value of λ ₀ , the greater the damping and the closer the joint velocity is to the singularity point. In addition, the strategy for defining the variable damping factor is also different; we can get

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

As a preferred method, the inverse priority control strategy of the multi-task redundant manipulator is established as follows;

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

This matrix includes the null space of the corresponding elements of the lowest priority lk-1 tasks that do not depend on the kth task, so we have

where Ji _|j is the 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 = 1, l-1 _, ..., 1, where 1 is a positive integer; the initial value

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

There you have it

表示

的行；in

express

's line;

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

The pseudo-inverse solution of can be expressed as:

and

的扩充；Where T _k represents the matrix

expansion;

The final reverse priority projection can be written as:

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

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

Preferably, the inverse control equation of the redundant manipulator with primary and secondary tasks is simplified as follows:

For a six- or seven-degree-of-freedom redundant manipulator, there are not enough degrees of freedom 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 primary task and the secondary task;

The inverse control equation for a redundant manipulator with primary and secondary tasks is as follows

是次要任务，

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

的计算；

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

It's a secondary task.

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

Calculation of

The expression of is as follows:

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

As a preferred method, the inverse priority force control strategy of the manipulator is established as follows:

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

Where X is the position in Cartesian space, M(X) is the inertia matrix, H(X, X) is the nonlinear force, F is the input control force, and _Fe is the contact force;

In addition, the input joint torque can be obtained based on the transformation of the Jacobian matrix

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

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

Where _Md and _Bd are the inertia and damping matrices; _Fd is the command force, and _Fe 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

From the above equations, it can be seen that if _Me , Be _, and _Ke are known, then adjustments to _Md and _Bd 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-range force control, nor is it necessary to ensure full-range force control, that is, sometimes we just want to ensure the force tracking control accuracy in a certain direction;

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 get the expected hierarchical force control relationship as follows

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

和

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

and

Then the precise force control of the manipulator can be achieved; the relationship between Cartesian speed and joint speed should refer to the inverse priority control; therefore, the equation of the inverse priority force control strategy of the manipulator 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 relies on the inner speed loop control; if the inner position control effect is good, accurate force control can be achieved; since the inner speed loop control can achieve low-frequency position tracking, the outer force loop can achieve low-frequency force tracking.

As a preferred method, the joint velocity is used to solve the relationship between the external force and the joint acceleration in the reverse priority impedance control of the manipulator, so that the reverse priority impedance control of the manipulator is guaranteed to be implemented as follows:

When the manipulator implements force control, the manipulator plays the role of an 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 to the external force passively;

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

The reference speed can be expressed as

Therefore, the expression guaranteed by the reverse priority impedance control of the manipulator is:

As a preferred method, 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 manipulator speed level inverse priority impedance control as follows:

Hybrid impedance application is a combination of the above two strategies, i.e., the Cartesian task can be divided into two cases: the first is the position control subspace, in which impedance control is implemented; the second is the force control subspace, in which force control is implemented;

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

So the simplified form of the expected speed can be expressed as

Then we get 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 manipulator speed level reverse priority impedance control is as follows:

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

The present invention can achieve the following effects:

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

BRIEF DESCRIPTION OF THE DRAWINGS

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

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

FIG3 is a schematic diagram of a kinetic scheme of hybrid impedance control according to 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 DESCRIPTION

The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

Embodiment, mobile manipulator redundant manipulator reverse priority impedance control system, as shown in FIG4 and FIG5, includes a manipulator and a control console S31 for controlling the manipulator; also includes a movable mobile platform S41; the manipulator includes a manipulator 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 includes a vertical lifting and moving platform S3, arm segment one S6, arm segment two S7, arm segment three S8 and arm segment four S10;

A vertical track S24 is provided on the left surface of the vertical column, and the vertical lifting and moving platform is vertically slidably arranged 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, the cylinder seat S21 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 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 surface of the vertical lifting and moving platform; the vertical lifting and moving platform can move up and down along the vertical track driven by the telescopic rod of the vertical cylinder, forming a first degree of freedom;

The arm section 1 includes an A1 section pipe S13 and an A2 section pipe S14 which is telescopically connected to the left pipe opening of the A1 section pipe. A No. 1 cylinder S25 with a telescopic rod horizontally arranged to the left is fixedly provided at the right end of the A1 section pipe. The telescopic rod of the No. 1 cylinder is fixedly connected to the right end of the A2 section pipe.

The arm section 2 includes a B1 section pipe S16 and a B2 section pipe S17 telescopically connected to the left pipe opening of the B1 section pipe. A second cylinder 37 with a telescopic rod horizontally arranged to the left is fixedly provided at the right end of the B1 section pipe. The telescopic rod of the second cylinder is fixedly connected to the right end of the B2 section pipe.

A No. 1 horizontal rotation axis S4 driven by a No. 1 reduction motor S26 is provided at the left end of the vertical lifting and moving platform. The right end of the A1 section pipe is fixedly connected to the No. 1 horizontal rotation axis, so that the arm section 1 can rotate horizontally to form a second degree of freedom; a No. 1 electromagnetic brake S32 is also provided on the No. 1 horizontal rotation axis to control the rotation of the No. 1 horizontal rotation axis;

A second horizontal rotation axis S15 driven by a second reduction motor S27 is provided at the left end of the A2 section pipe, and the right end of the B1 section pipe is fixedly connected to the second horizontal rotation axis, so that the arm section 2 can rotate horizontally to form a third degree of freedom; a second electromagnetic brake S33 capable of controlling the rotation of the second horizontal rotation axis is also provided on the second horizontal rotation axis;

A No. 3 horizontal rotation axis S18 driven by a No. 3 reduction motor S28 is provided at the left end of the B2 section tube, and the right end of the arm section 3 is fixedly connected to the No. 3 horizontal rotation axis, so that the arm section 3 can rotate horizontally to form a fourth degree of freedom; a No. 3 electromagnetic brake S34 is also provided on the No. 3 horizontal rotation axis to control the rotation of the No. 3 horizontal rotation axis;

At the left end of arm segment 3, there is a No. 1 horizontal vertical rotation axis S9 driven by a No. 4 reduction motor S29 and capable of rotating on the left and right vertical planes. The right end of arm segment 4 is fixedly connected to the No. 1 horizontal vertical rotation axis, so that arm segment 4 can rotate vertically on the left and right vertical planes to form the fifth degree of freedom; on the No. 1 horizontal vertical rotation axis, there is also a No. 4 electromagnetic brake S35 capable of controlling the rotation of the No. 1 horizontal vertical rotation axis;

At the left end of arm section 4, there is a No. 1 longitudinal vertical rotation axis S11 driven by a No. 5 reduction motor S30 and capable of rotating on the front and rear vertical planes. The right end of the output gripper is fixedly connected to the No. 1 longitudinal vertical rotation axis, 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 rotation axis is also provided with a No. 5 electromagnetic brake S36 capable of controlling the rotation of the No. 1 longitudinal vertical rotation axis;

Driven by the telescopic rod of the No. 1 cylinder, the A2 section of pipe can move left and right in the A1 section of pipe to form the seventh degree of freedom;

Driven by the telescopic rod of the No. 2 cylinder, the B2 section of pipe can telescope left and right inside the B1 section of pipe to form the eighth degree of freedom;

The left end of a No. 1 horizontal tube S39 is horizontally fixedly connected to the right surface of the vertical column. A balance adjustment block S40 is provided in the No. 1 horizontal tube for sliding left and right. A balance adjustment cylinder S38 with a telescopic rod facing horizontally to the right is fixedly connected to the left end of the No. 1 horizontal tube. The right end of the telescopic rod of the balance adjustment cylinder is fixedly connected to the balance adjustment block.

The control end of electromagnetic brake No. 1, electromagnetic brake No. 2, electromagnetic brake No. 3, electromagnetic brake No. 4, electromagnetic brake No. 5, reduction motor No. 1, reduction motor No. 2, reduction motor No. 3, reduction motor No. 4, reduction motor No. 5, cylinder No. 1, cylinder No. 2, balance cylinder and vertical cylinder are respectively connected to the control console. The seventh degree of freedom allows the robot arm to extend freely, and the eighth degree of freedom allows the robot arm to extend freely, greatly increasing the range of operation and flexibility.

The balance adjustment cylinder controls the balance adjustment block to move left and right to control the balance of the vertical column, thereby controlling the balance of the manipulator. The manipulator is easy to move. The mobile platform includes a car.

Since the manipulator of the present invention has eight degrees of freedom, it has good flexibility, high reliability, and is easy to complete control tasks.

The reverse priority impedance control method of the redundant manipulator arm of the mobile manipulator is shown in Figure 1-3. It includes the following steps:

Step 1: Establish a kinematic model of the redundant manipulator and give the gradient direction strategy of the redundant manipulator null space vector. The implementation process is as follows:

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

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

J∈R^m×n；冗余自由度机械臂的正运动学方程可用下式描述：Define the position 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-DOF robot, where x∈R ⁿ ,

J∈R ^m×n ; The forward kinematics equation of the redundant degree of freedom manipulator can be described by the following formula:

Formula (1) is also called the robot arm kinematic velocity model;

Taking into account the solution of the least squares method, the optimal problem can be stated as:

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

to solve;

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

Where J ⁺ ——pseudo-inverse of Jacobian matrix

I——Identity Matrix

——任意零空间矢量

——Arbitrary null space vector

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

——Minimum norm solution, defining the hand movements

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

A homogeneous solution, no action is produced at the end

Formula (4) represents the position and attitude control of the end effector. By adding any residual to Formula (4), a general expression including the null space can be obtained. The above equation can be used to realize multi-task optimization on the null vector.

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

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

is the weighting coefficient and satisfies

The solution to the above equation can be expressed as:

Formula (7) is also called the redundant manipulator kinematic model;

The joint constraint function of the joint constraint gradient direction of the position-dependent scalar index of the redundant manipulator null space vector is:

Step 2: Establish the task priority solution strategy of eliminating singularity algorithm through singular robust solution as follows:

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

和

表示task1和task2in

and

Represents task1 and task2

From expression (9), the inverse kinematics equation of the redundant manipulator is obtained as follows:

是在

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

is

The final inverse kinematics expression of 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,

It is a secondary task;

If two related tasks are interdependent, the corresponding Jacobian matrix is singular; if the task Jacobian matrix is singular, the corresponding tasks are unsatisfiable; in this case, the Jacobian correlation matrix will be a singular point, defined as an algorithmic 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 and primary tasks; moreover, the redundant manipulator inverse kinematics based on task priority is designed to provide better effectiveness in controlling the primary task;

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

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

Based on the Jacobi pseudo-inverse solution, kinematic singularities will occur, which are caused by the secondary matrix; for the kinematic singularity problem, the DLS (damped least squares) solution should also be given;

The cost function of the DLS solution can be modified as follows:

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

Formula (15) is the inverse kinematics analysis model of the singular robust solution. Let λ = η ² I. The above DLS solution is equivalent to the additional regularization solution. 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∈∈B ^m×m , V∈R ^n×n , ∑∈R ^m×n , U is a one-element matrix consisting of column vectors u _i , V is a one-element matrix consisting of column vectors _vi , ∑ is a block matrix of an m×n diagonal matrix containing the singular values σ _i ≥ 0 of J in decreasing order and containing nm zero column vectors;

Where r≤n is the rank of the matrix J;

For motion singularity, refer to the singular value decomposition SVD required to calculate the pseudo-inverse solution. The large joint velocity is due to the small singular value rapidly approaching 0, as shown in the following formula:

The factor λ ₀ will affect the singularity. The higher the value of λ ₀ , the greater the damping and the closer the joint velocity is to the singularity point. In addition, the strategy for defining the variable damping factor is also different; we can get

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

Step 4: Establish the inverse priority control strategy of the multi-task redundant manipulator as follows;

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

This matrix includes the null space of the corresponding elements of the lowest priority lk-1 tasks that do not depend on the kth task, so we have

where Ji _|j is the 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 = 1, l-1, ..., 1, where l is a positive integer; the initial value

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

There you have it

表示

的行；in

express

's line;

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

The pseudo-inverse solution of can be expressed as:

and

的扩充；Among them, _TR represents the matrix

expansion;

The final reverse priority projection can be written as:

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

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

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

For a six- or seven-degree-of-freedom redundant manipulator, there are not enough degrees of freedom 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 primary task and the secondary task;

The inverse control equation for a redundant manipulator with primary and secondary tasks is as follows

是次要任务，

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

的计算；

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

It's a secondary task.

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

Calculation of

The expression of is as follows:

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

Step 6: Establish the reverse priority force control strategy of the manipulator as follows:

The dynamics of the manipulator in the force control 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 _Fe is the contact force;

In addition, the input joint torque can be obtained based on the transformation of the Jacobian matrix

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

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

Where _Md and _Bd are the inertia and damping matrices; _Fd is the command force, and _Fe is the contact force;

The kinetic 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

From the above equations, it can be seen that if _Me , Be _, and _Ke are known, then adjustments to _Md and _Bd 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-range force control, nor is it necessary to ensure full-range force control, that is, sometimes we just want to ensure the force tracking control accuracy in a certain direction;

For example, when the manipulator interacts with a planer, it only needs to maintain precise force tracking control in the vertical direction, while the other direction does not need precise force tracking control; in other cases, position direction force control is more important than attitude direction force control;

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 get the expected hierarchical force control relationship as follows

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

和

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

and

Then the precise force control of the manipulator can be achieved; the relationship between Cartesian speed and joint speed should refer to the inverse priority control; therefore, the equation of the inverse priority force control strategy of the manipulator 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 relies on the inner speed loop control; if the inner position control effect is good, accurate force control can be achieved; since the inner speed loop control can achieve low-frequency position tracking, the outer force loop can achieve low-frequency force tracking.

Step 7: Use joint velocity to solve the relationship between external force and joint acceleration in the inverse priority impedance control of the manipulator, so as to obtain the implementation 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 an initiator to a certain extent, that is, the manipulator is ready to respond to the external environment; when the manipulator

When working as an impedance-controlled model, the robot passively responds to external forces; 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 expression guaranteed by the reverse 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 manipulator speed level inverse priority impedance control. The implementation method is as follows:

Hybrid impedance application is a combination of the above two strategies, i.e., the Cartesian task can be divided into two cases: the first is the position control subspace, in which impedance control is implemented; the second is the force control subspace, in which force control is implemented;

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

So the simplified form of the expected speed can be expressed as

Then we get 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 manipulator speed level reverse priority impedance control is as follows:

Expression (52) solves the problem of extending the inverse priority calculation of the position control space to the inverse priority calculation of the force control space in the manipulator inverse priority mixed impedance control; it can enable the redundant manipulator arms of the manipulator to achieve the desired impedance control task under different hierarchical structures.

Claims (1)

1. A method for reverse priority impedance control of redundant mechanical arms of a mobile manipulator, characterized in that it comprises the following steps: Step 1: Establish a kinematic model of the redundant manipulator and provide a gradient direction strategy of the redundant manipulator null space vector; Step 2: Establish a task priority solution strategy for eliminating singularity through a singular robust solution; Step 3, establishing a singular robust solution inverse kinematics analysis model; Step 4: Establish an inverse priority control strategy for the multi-task redundant manipulator; Step 5, simplifying the inverse control equation of the redundant manipulator with the primary task and the secondary task; Step 6: Establish the inverse priority force control strategy of the manipulator; Step 7: Use joint velocity to solve the relationship between external force and joint acceleration in the inverse priority impedance control of the manipulator, so as to obtain the inverse priority impedance control guarantee 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, thereby obtaining the overall framework of the manipulator speed level inverse priority impedance control; The kinematic model of the redundant manipulator is established, and the gradient direction strategy of the redundant manipulator null space vector is given as follows: Define the position 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-DOF robot, where x∈R ⁿ ,

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

Formula (1) is also called the robot arm kinematic velocity model; Taking into account the solution of the least squares method, the optimal problem can be stated as:

The solution of formula (1) can be obtained by finding the optimal

to solve;

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

Where J ⁺ ——pseudo-inverse of Jacobian matrix I——Identity Matrix

——Arbitrary null space vector

——Minimum norm solution, defining the hand movements

——Homogeneous solution, no action is produced at the end Formula (4) represents the position and attitude control of the end effector. By adding any residual to Formula (4), a general expression including the null space can be obtained. The above equation can be used to realize multi-task optimization on the null vector. However, the above equation ignores the ill-conditioning of the Jacobian matrix; the regularization equation can be modified by adding additional regularization values,

where λ≥0 is the weighting matrix,

is the weighting coefficient and satisfies

The solution to the above equation can be expressed as:

Formula (7) is also called the redundant manipulator kinematic model; The joint constraint function of the joint constraint gradient direction of the position-dependent scalar index of the redundant manipulator null space vector is:

The task priority solution strategy for establishing the algorithm for eliminating singularities through singular robust solutions is as follows: In the redundant manipulator solution of the Jacobian matrix, the optimization task is implemented in the null space of the main task; the inverse task kinematics is established based on the forward task kinematics:

in

and

Represents task1 and task2 From expression (9), the inverse kinematics equation of the redundant manipulator is obtained as follows:

Task1 is the main task and task2 is the auxiliary task; that is, task2

It is in task1

The final inverse kinematics expression of 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,

It is a secondary task; If two related tasks are interdependent, the corresponding Jacobian matrix is singular; if the task Jacobian matrix is singular, the corresponding tasks are unsatisfiable; in this case, the Jacobian correlation matrix will be a singular point, defined as an algorithmic singularity; That is, if

where p(·) is the rank of the matrix; It is obvious that the singularity of the algorithm is caused by the task conflict between the secondary and primary tasks; moreover, the redundant manipulator inverse kinematics based on task priority is designed to provide better effectiveness in controlling the primary task; Therefore, let the position control direction as the main task, so that the position guarantees the accuracy of the control direction task; then establish the task priority solution strategy equation of the algorithm to eliminate the singularity through the singular robust solution:

The singular robust solution inverse kinematics analysis model is established as follows: Based on the Jacobi pseudo-inverse solution, kinematic singularities will occur, which are caused by the secondary matrix; for the kinematic singularity problem, the DLS (damped least squares) solution should also be given; The cost function of the DLS solution can be modified as follows:

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

Formula (15) is the inverse kinematics analysis model of the singular robust solution. Let λ = η ² I. The above DLS solution is equivalent to the additional regularization solution. 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×n , V∈R ^n×n , ∑∈R ^m×n , U is a one-element matrix consisting of column vectors u _i , V is a one-element matrix consisting of column vectors _vi , ∑ is a block matrix of an m×n diagonal matrix containing the singular values σ _i ≥ 0 of J in decreasing order and containing nm zero column vectors;

Where r≤m is the rank of the matrix J; For motion singularity, refer to the singular value decomposition SVD required to calculate the pseudo-inverse solution. The large joint velocity is due to the small singular value rapidly approaching 0, as shown in the following formula:

The factor λ ₀ will affect the singularity. The higher the value of λ ₀ , the greater the damping and the closer the joint velocity is to the singularity point. In addition, the strategy for defining the variable damping factor is also different; we can get

From the above formula, we can see that the parameter δ>0 monitors the smallest singular value; The inverse priority control strategy of the multi-task redundant manipulator is established as follows; Introducing the inverse priority projection matrix

This matrix includes the null space of the corresponding elements of the lowest priority lk-1 tasks that do not depend on the kth task, so we have

where Ji _|j is the 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, where 1 is a positive integer; the initial value

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

There you have it

in

express

's line; So,

The pseudo-inverse solution of can be expressed as:

and

Where T _k represents the matrix

expansion; The final reverse priority projection can be written as:

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

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

The inverse control equations of a redundant manipulator with primary and secondary tasks are simplified as follows: For a six- or seven-degree-of-freedom redundant manipulator, there are not enough degrees of freedom 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 primary task and the secondary task; The inverse control equation for a redundant manipulator with primary and secondary tasks is as follows

The above formula is quite different from the previous expression (11), but the algorithm framework is similar; in the above equation,

It's a secondary task.

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

Calculation of

The expression of is as follows:

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

The inverse priority force control strategy of the manipulator is established as follows: The dynamics of the manipulator in the force control space can be written as:

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 _Fe is the contact force; In addition, the input joint torque can be obtained based on the transformation of the Jacobian matrix τ＝J ^T (q)F (33) The desired equation of motion of the manipulator in the force control space can be defined as follows:

Where _Md and _Bd are the inertia and damping matrices; _Fd is the command force, and _Fe 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

From the above equations, it can be seen that if _Me , Be _, and _Ke are known, then adjustments to _Md and _Bd 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-range force control, nor is it necessary to ensure full-range force control, that is, sometimes we just want to ensure the force tracking control accuracy in a certain direction; 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 get the expected hierarchical force control relationship as follows

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

If the robot end effector can track the desired Cartesian velocity

and

Then the precise force control of the manipulator can be achieved; the relationship between Cartesian speed and joint speed should refer to the inverse priority control; therefore, the equation of the inverse priority force control strategy of the manipulator 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 speed loop control; if the inner position control effect is good, accurate force control can be achieved; since the inner speed loop control can achieve low-frequency position tracking, the outer force loop can achieve low-frequency force tracking; The joint velocity is used to solve the relationship between the external force and the joint acceleration in the inverse priority impedance control of the manipulator, so that the inverse priority impedance control of the manipulator is guaranteed to be implemented as follows: When the manipulator implements force control, the manipulator plays the role of an 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 to the external force passively; The corresponding impedance relationship between external force and joint acceleration can be expressed as

The reference speed can be expressed as

Therefore, the expression guaranteed by the reverse priority impedance control of the manipulator is:

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 manipulator speed level inverse priority impedance control as follows: Hybrid impedance application is a combination of the above two strategies, i.e., the Cartesian task can be divided into two cases: the first is the position control subspace, in which impedance control is implemented; the second is the force control subspace, in which force control is implemented; Therefore, a selection matrix is selected; the relationship between external force and position response is as follows

So the simplified form of the expected speed can be expressed as

Then we get 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 manipulator speed level reverse priority impedance control is as follows:

Expression (52) solves the problem of extending the inverse priority calculation of the position control space to the inverse priority calculation of the force control space in the manipulator inverse priority mixed impedance control; it can enable the redundant manipulator arms of the manipulator to achieve the desired impedance control task under different hierarchical structures; A mobile manipulator redundant manipulator arm reverse priority impedance control system applicable to a mobile manipulator redundant manipulator arm reverse priority impedance control method, the mobile manipulator redundant manipulator arm reverse priority impedance control system comprising a manipulator and a control console (S31) for controlling the manipulator; and also comprising a movable mobile platform (S41); the manipulator comprises a manipulator 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 and moving platform (S3), arm segment one (S6), arm segment two (S7), arm segment three (S8) and arm segment four (S10); A vertical track (S24) is provided on the left surface of the vertical column, and the vertical lifting and moving platform is vertically slidably arranged on the vertical track; 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 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 surface of the vertical lifting and moving platform; the vertical lifting and moving platform can move up and down along the vertical track driven by the telescopic rod of the vertical cylinder, forming a first degree of freedom; The arm section 1 includes an A1 section pipe (S13) and an A2 section pipe (S14) telescopically connected to the left pipe opening of the A1 section pipe, and a No. 1 cylinder (S25) with a telescopic rod horizontally arranged to the left is fixedly provided at the right end of the A1 section pipe, and the telescopic rod of the No. 1 cylinder is fixedly connected to the right end of the A2 section pipe; The arm section 2 comprises a B1 section pipe (S16) and a B2 section pipe (S17) telescopically connected to the left pipe opening of the B1 section pipe, a No. 2 air cylinder (37) with a telescopic rod horizontally arranged to the left is fixedly provided at the right end of the B1 section pipe, and the telescopic rod of the No. 2 air cylinder is fixedly connected to the right end of the B2 section pipe; A No. 1 horizontal rotating shaft (S4) driven by a No. 1 reduction motor (S26) is provided at the left end of the vertical lifting and moving platform, and the right end of the A1 section pipe is fixedly connected to the No. 1 horizontal rotating shaft, so that the arm section 1 can rotate horizontally to form a second degree of freedom; and a No. 1 electromagnetic brake (S32) capable of controlling the rotation of the No. 1 horizontal rotating shaft is also provided on the No. 1 horizontal rotating shaft; A second horizontal rotation axis (S15) driven by a second reduction motor (S27) is provided at the left end of the A2 section pipe, and the right end of the B1 section pipe is fixedly connected to the second horizontal rotation axis, so that the arm section 2 can rotate horizontally to form a third degree of freedom; a second electromagnetic brake (S33) capable of controlling the rotation of the second horizontal rotation axis is also provided on the second horizontal rotation axis; A No. 3 horizontal rotation axis (S18) driven by a No. 3 reduction motor (S28) is provided at the left end of the B2 section tube, and the right end of the arm section 3 is fixedly connected to the No. 3 horizontal rotation axis, so that the arm section 3 can rotate horizontally to form a fourth degree of freedom; a No. 3 electromagnetic brake (S34) capable of controlling the rotation of the No. 3 horizontal rotation axis is also provided on the No. 3 horizontal rotation axis; A first horizontal vertical rotation axis (S9) driven by a fourth reduction motor (S29) and capable of rotating on a left and right vertical plane is provided at the left end of the arm segment 3, and the right end of the arm segment 4 is fixedly connected to the first horizontal vertical rotation axis, so that the arm segment 4 can rotate vertically on the left and right vertical planes to form a fifth degree of freedom; a fourth electromagnetic brake (S35) capable of controlling the rotation of the first horizontal vertical rotation axis is also provided on the first horizontal vertical rotation axis; A No. 1 longitudinal vertical rotation axis (S11) driven by a No. 5 reduction motor (S30) and capable of rotating on the front and rear vertical planes is provided at the left end of the arm section 4. The right end of the output gripper is fixedly connected to the No. 1 longitudinal vertical rotation axis, so that the right end of the output gripper can rotate vertically on the front and rear vertical planes to form a sixth degree of freedom; a No. 5 electromagnetic brake (S36) capable of controlling the rotation of the No. 1 longitudinal vertical rotation axis is also provided on the No. 1 longitudinal vertical rotation axis; Driven by the telescopic rod of the No. 1 cylinder, the A2 section of pipe can move left and right in the A1 section of pipe to form the seventh degree of freedom; Driven by the telescopic rod of the No. 2 cylinder, the B2 section of pipe can move left and right in the B1 section of pipe to form the eighth degree of freedom; The left end of a No. 1 horizontal tube (S39) is horizontally fixedly connected to the right surface of the vertical column, and a balance adjustment block (S40) is provided in the No. 1 horizontal tube for sliding left and right. A balance adjustment cylinder (S38) with a telescopic rod facing horizontally to the right is fixedly connected to the left end of the No. 1 horizontal tube, and the right end of the telescopic rod of the balance adjustment cylinder is fixedly connected to the balance adjustment block; The control end of electromagnetic brake No. 1, the control end of electromagnetic brake No. 2, the control end of electromagnetic brake No. 3, the control end of electromagnetic brake No. 4, the control end of electromagnetic brake No. 5, the control end of reduction motor No. 1, the control end of reduction motor No. 2, the control end of reduction motor No. 3, the control end of reduction motor No. 4, the control end of reduction motor No. 5, the control end of cylinder No. 1, the control end of cylinder No. 2, the control end of balance adjustment cylinder and the control end of vertical cylinder are respectively controlled and connected to the control console.

Images