Exoskeleton pole hanging operation dual-arm cooperative control method and system

Abstract

The application discloses a kind of exoskeleton hanging pole operation double-arm cooperative control method and system, method includes obtaining the angle information of each joint of upper limb exoskeleton and the mechanical information of operating terminal, establishes upper limb exoskeleton connecting rod model and determines D-H parameter, according to D-H parameter analysis double-arm operating terminal position variation relationship with joint angle, obtain the coordinate rotation matrix of upper limb exoskeleton, according to the coordinate rotation matrix of upper limb exoskeleton and the angle information of each joint of upper limb exoskeleton determine the jacobian matrix of upper limb exoskeleton, according to the mechanical information of operating terminal and the jacobian matrix of upper limb exoskeleton calculate the torque required for each joint of upper limb exoskeleton, with the torque required for each joint of upper limb exoskeleton and each joint drive actual measured torque as input, optimize the jacobian matrix of upper limb exoskeleton by compensation controller. Realize the technical effect of improving the cooperativity of the motion control of the multi-arm multi-joint of upper limb exoskeleton.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of exoskeleton robots, and particularly relates to an exoskeleton pole hanging operation double-arm cooperative control method and system. BACKGROUND

[0002] The exoskeleton robot system is a human-machine cooperative system, which can enhance the strength of the wearer in various environments. When the wearable upper limb exoskeleton completes the pole hanging operation, the motors of the left and right shoulder joints and the elbow joint need to be controlled to cooperatively control the left and right arms and the small arms to complete the pole hanging action. Therefore, the upper limb exoskeleton needs to adopt a double-arm cooperative pole hanging control strategy to improve the coordination of the double-arm movement, enhance the cooperative control ability of the multi-joint, and make the exoskeleton more efficient and accurate in performing the work task. In the existing exoskeleton pole hanging operation double-arm cooperative control method, the control effect of different joints has a large deviation, and it is difficult to improve the coordination of the multi-arm and multi-joint motion control of the upper limb exoskeleton. SUMMARY

[0003] The present application provides an exoskeleton pole hanging operation double-arm cooperative control method and system for improving the coordination of the multi-arm and multi-joint motion control of the upper limb exoskeleton.

[0004] Therefore, the present application provides an exoskeleton pole hanging operation double-arm cooperative control method and system for improving the coordination of the multi-arm and multi-joint motion control of the upper limb exoskeleton.

[0005] Obtain the angle information of each joint of the upper limb exoskeleton and the mechanical information of the operation terminal;

[0006] Establish a link model of the upper limb exoskeleton and determine the D-H parameters, analyze the relationship between the position of the double-arm operation terminal and the joint angle according to the D-H parameters, and obtain the coordinate rotation matrix of the upper limb exoskeleton;

[0007] Determine the Jacobian matrix of the upper limb exoskeleton according to the coordinate rotation matrix of the upper limb exoskeleton and the angle information of each joint of the upper limb exoskeleton, and calculate the required torque of each joint of the upper limb exoskeleton according to the mechanical information of the operation terminal and the Jacobian matrix of the upper limb exoskeleton;

[0008] Take the required torque of each joint of the upper limb exoskeleton and the actual measured torque of each joint drive as input, and optimize the Jacobian matrix of the upper limb exoskeleton through a compensation controller.

[0009] Optionally, the Jacobian matrix of the upper limb exoskeleton is determined according to the coordinate rotation matrix of the upper limb exoskeleton and the angle information of each joint of the upper limb exoskeleton, and the required torque of each joint of the upper limb exoskeleton is calculated according to the mechanical information of the operation terminal and the Jacobian matrix of the upper limb exoskeleton, including:

[0010] Determine the Jacobian matrix of the upper limb exoskeleton according to the coordinate rotation matrix of the upper limb exoskeleton and the angle information of each joint of the upper limb exoskeleton;

[0011] The theoretical torque required by each joint of the upper limb exoskeleton is calculated according to the mechanical information of the operating terminal and the Jacobian matrix of the upper limb exoskeleton;

[0012] The theoretical torque required by each joint of the upper limb exoskeleton is taken as the input of the PID controller, and the actual torque required by each joint of the upper limb exoskeleton is obtained through the PID controller.

[0013] Optionally, the calculation formula of the theoretical torque required by each joint of the upper limb exoskeleton is:

[0014] Δτ=J(θ) T ΔF

[0015] ΔF=F d -F h

[0016] T cal =Δτ

[0017] Wherein, Δτ is the error torque, T cal is the theoretical torque required by each joint of the upper limb exoskeleton, J(θ) T is the transpose of the Jacobian matrix J(θ) of the upper limb exoskeleton, ΔF is the error between the force acting on the operating terminal collected by the multi-dimensional force sensor of the dual-arm operating terminal and the expected force, F d is the expected force, F h is the force acting on the operating terminal collected by the multi-dimensional force sensor of the dual-arm operating terminal.

[0018] Optionally, the calculation formula of the actual torque required by each joint of the upper limb exoskeleton is:

[0019]

[0020] Wherein, τ is the actual torque required by each joint of the upper limb exoskeleton, k p is the proportional coefficient of the PID controller, k i is the integral coefficient of the PID controller, k d is the differential coefficient of the PID controller, is the derivative of T cal .

[0021] Optionally, the torque required by each joint of the upper limb exoskeleton and the actual measured torque of each joint drive are taken as the input, and the Jacobian matrix of the upper limb exoskeleton is optimized through the compensation controller, including:

[0022] The torque required by each joint of the upper limb exoskeleton and the actual measured torque of each joint drive are input into the compensation controller;

[0023] The compensation controller calculates an average required torque according to the required torques of the joints of the upper limb exoskeleton, and the calculation formula of the average required torque is:

[0024]

[0025] wherein T cal_mean is the average required torque, n is the number of joints, and T cal,i is the required theoretical torque of the ith joint.

[0026] The compensation controller calculates an average measured torque according to the actual measured torques of the joints of the upper limb exoskeleton, and the calculation formula of the average measured torque is:

[0027]

[0028] wherein T me_mean is the average measured torque, and ΔT me,i is the actual measured torque of the ith joint.

[0029] The compensation controller calculates the torque that needs to be compensated for each joint according to the average required torque and the average measured torque, and the calculation formula of the torque that needs to be compensated for each joint is:

[0030]

[0031] wherein T com,i is the torque that needs to be compensated for the ith joint.

[0032] An impedance model is established according to the torque that needs to be compensated for each joint, and the angle that needs to be compensated for each joint is obtained, and the impedance model is:

[0033]

[0034] wherein M is the desired inertia matrix, B is the desired damping matrix, K is the desired stiffness matrix, is the deviation of the angular acceleration at the joint from the desired angular acceleration, is the deviation of the angular velocity at the joint from the desired angular velocity, and e is the deviation of the angle at the joint from the desired angle.

[0035] The Jacobian matrix of the upper limb exoskeleton is optimized according to the angle that needs to be compensated for each joint, and the optimization formula is:

[0036] J (θ) = J (θ + e).

[0037] The second aspect of the present application provides an exoskeleton hanging rod operation double-arm cooperative control system, comprising:

[0038] An information acquisition module is configured to acquire angle information of each joint of the upper limb exoskeleton and mechanical information of the operation terminal.

[0039] a rotation matrix obtaining module, configured to establish an upper-limb exoskeleton linkage model and determine D-H parameters, analyze a relationship between a position of a dual-arm operating terminal and a joint angle according to the D-H parameters, and obtain a coordinate rotation matrix of the upper-limb exoskeleton;

[0040] a joint required torque calculating module, configured to determine a Jacobian matrix of the upper-limb exoskeleton according to the coordinate rotation matrix of the upper-limb exoskeleton and angle information of each joint of the upper-limb exoskeleton, and calculate a required theoretical torque of each joint of the upper-limb exoskeleton according to mechanical information of the operating terminal and the Jacobian matrix of the upper-limb exoskeleton;

[0041] an optimization module, configured to take the required torque of each joint of the upper-limb exoskeleton and an actually measured torque of each joint of the upper-limb exoskeleton as inputs, and optimize the Jacobian matrix of the upper-limb exoskeleton through a compensation controller.

[0042] Optionally, the joint required torque calculating module is specifically configured to:

[0043] determine the Jacobian matrix of the upper-limb exoskeleton according to the coordinate rotation matrix of the upper-limb exoskeleton and the angle information of each joint of the upper-limb exoskeleton;

[0044] calculate the required theoretical torque of each joint of the upper-limb exoskeleton according to the mechanical information of the operating terminal and the Jacobian matrix of the upper-limb exoskeleton;

[0045] take the required theoretical torque of each joint of the upper-limb exoskeleton as an input of a PID controller, and obtain an actually required torque of each joint of the upper-limb exoskeleton through the PID controller.

[0046] Optionally, a calculation formula of the required theoretical torque of each joint of the upper-limb exoskeleton is:

[0047] ΔF=F d -F h

[0048] T cal =Δτ

[0049] wherein Δτ is an error torque, T cal is the required theoretical torque of each joint of the upper-limb exoskeleton, J(θ) T is a transpose of the Jacobian matrix J(θ) of the upper-limb exoskeleton, ΔF is an error between an action force received by the operating terminal and an expected force, which is collected by a multi-dimensional force sensor of the dual-arm operating terminal, F d is the expected force, and F h is the action force received by the operating terminal, which is collected by the multi-dimensional force sensor of the dual-arm operating terminal.

[0050] Optionally, a calculation formula of the actually required torque of each joint of the upper-limb exoskeleton is:

[0051]

[0052] wherein τ is the actual torque required by each joint of the upper limb exoskeleton, k p is a proportional coefficient of the PID controller, k i is an integral coefficient of the PID controller, k d is a differential coefficient of the PID controller, is the derivative of T cal .

[0053] Optionally, the optimization module is specifically configured to:

[0054] input the torque required by each joint of the upper limb exoskeleton and the actual measured torque of each joint drive into the compensation controller;

[0055] the compensation controller calculates the average required torque according to the torque required by each joint of the upper limb exoskeleton, and the calculation formula of the average required torque is:

[0056]

[0057] wherein T cal_mean is the average required torque, n is the number of joints, and T cal,i is the required theoretical torque of the i-th joint;

[0058] the compensation controller calculates the average measured torque according to the actual measured torque of each joint drive of the upper limb exoskeleton, and the calculation formula of the average measured torque is:

[0059]

[0060] wherein T me_mean is the average measured torque, and ΔT me,i is the actual measured torque of the i-th joint;

[0061] the compensation controller calculates the torque required to be compensated for each joint according to the average required torque and the average measured torque, and the calculation formula of the torque required to be compensated for each joint is:

[0062]

[0063] wherein T com,i is the torque required to be compensated for the i-th joint;

[0064] an impedance model is established according to the torque required to be compensated for each joint, and the angle required to be compensated for each joint is obtained, and the impedance model is:

[0065]

[0066] wherein M is the desired inertia matrix, B is the desired damping matrix, K is the desired stiffness matrix, is the deviation of the angular acceleration at the joint from the desired angular acceleration, is the angular velocity deviation at the joint, and e is the angle deviation at the joint;

[0067] The Jacobian matrix of the upper limb exoskeleton is optimized according to the angles that need to be compensated, and the optimization formula is:

[0068] J (θ) = J (θ + e).

[0069] It can be seen from the above technical solution that the exoskeleton hanging rod operation double-arm cooperative control method provided by the application has the following advantages:

[0070] The exoskeleton hanging rod operation double-arm cooperative control method provided by the application comprises the following steps: acquiring angle information of each joint of an upper limb exoskeleton and mechanical information of an operation terminal, establishing a link model of the upper limb exoskeleton and determining D-H parameters, analyzing a change relationship between a position of the operation terminal and a joint angle according to the D-H parameters, obtaining a coordinate rotation matrix of the upper limb exoskeleton, determining a Jacobian matrix of the upper limb exoskeleton according to the coordinate rotation matrix of the upper limb exoskeleton and the angle information of each joint of the upper limb exoskeleton, calculating torques required by each joint of the upper limb exoskeleton according to the mechanical information of the operation terminal and the Jacobian matrix of the upper limb exoskeleton, and optimizing the Jacobian matrix of the upper limb exoskeleton by using a compensation controller with the torques required by each joint of the upper limb exoskeleton and actual measured torques of each joint as inputs. The exoskeleton hanging rod operation terminal load dynamic estimation method provided by the application can reduce the deviation of control effects of different joints, realize double-arm multi-joint cooperative control of the upper limb exoskeleton, and make each system more efficient and accurate in performing a work task, thereby achieving the technical effect of improving the cooperativeness of multi-arm multi-joint motion control of the upper limb exoskeleton.

[0071] The exoskeleton hanging rod operation double-arm cooperative control system provided by the application is used to execute the exoskeleton hanging rod operation double-arm cooperative control method provided by the application, and has the same principle and technical effects as the exoskeleton hanging rod operation double-arm cooperative control method provided by the application, which will not be described herein again. BRIEF DESCRIPTION OF DRAWINGS

[0072] In order to more clearly illustrate the technical solutions in the embodiments of the application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or the prior art description. Obviously, the drawings in the following description only constitute some embodiments of the application, and for those skilled in the art, other related drawings can also be obtained without creative labor.

[0073] Figure 1 It is a flowchart of the exoskeleton hanging rod operation double-arm cooperative control method provided by the application;

[0074] Figure 2A collaborative control principle block diagram of a kind of exoskeleton hanging pole operation double-arm collaborative control method provided in the application;

[0075] Figure 3 A structure schematic diagram of a kind of exoskeleton hanging pole operation double-arm collaborative control system provided in the application. DETAILED DESCRIPTION

[0076] In order to enable the personnel in the technical field to better understand the present application scheme, the technical scheme in the embodiment of the present application will be clearly and completely described below in conjunction with the drawings in the embodiment of the present application. Obviously, the described embodiment is only a part of the embodiment of the present application, not all. Based on the embodiment in the present application, all other embodiments obtained by the person skilled in the art without creative labor belong to the scope of protection of the present application.

[0077] For the convenience of understanding, please refer to Figure 1 And Figure 2 An embodiment of a kind of exoskeleton hanging pole operation double-arm collaborative control method provided in the application, comprising:

[0078] Step 101, the angle information of each joint of upper limb exoskeleton and the mechanical information of operation terminal are acquired.

[0079] It should be noted that in the embodiment of the present application, the angle information θ of each joint (shoulder joint, elbow joint, wrist joint) of upper limb exoskeleton is obtained by angle sensor, and the acting force F received by the operation terminal of upper limb exoskeleton is obtained by multi-dimensional force sensor installed at the operation terminal of upper limb exoskeleton h , the error ΔF of its and the expected force F d is calculated, that is, ΔF=F d -F h , that is, the mechanical information of the operation terminal of upper limb exoskeleton is obtained.

[0080] Step 102, a link model of upper limb exoskeleton is established and D-H parameters are determined, the relationship between the position of double-arm operation terminal and the change of joint angle is analyzed according to D-H parameters, and the coordinate rotation matrix of upper limb exoskeleton is obtained.

[0081] It should be noted that the link model of upper limb exoskeleton is established, and D-H parameters are determined, the coordinate rotation matrix of upper limb exoskeleton is calculated according to the D-H parameters of upper limb exoskeleton, and the coordinate conversion relationship between each joint, that is, the coordinate rotation matrix of upper limb exoskeleton is obtained. Iterative calculation is used to solve the coordinate rotation matrix of upper limb exoskeleton operation terminal coordinate system relative to base coordinate system, and the position coordinates and attitude coordinates of upper limb exoskeleton operation terminal relative to base coordinate system are obtained.

[0082] Step 103: Determine the Jacobian matrix of the upper limb exoskeleton based on the coordinate rotation matrix of the upper limb exoskeleton and the angle information of each joint of the upper limb exoskeleton. Calculate the required torque of each joint of the upper limb exoskeleton based on the mechanical information of the operating terminal and the Jacobian matrix of the upper limb exoskeleton.

[0083] It should be noted that the Jacobian matrix J(θ) of the upper limb exoskeleton is determined based on the coordinate rotation matrix and the angle information of each joint. Ideally, when the human operates the exoskeleton arms, the exoskeleton's driven ends lift the load, and the human can complete the task without applying additional force. In this state, the load's effect on the human is minimized. With the human-machine interaction force as the optimization objective, the desired force F is... d A value of 0 indicates that there is no operational pressure on the upper limbs or hands, and F exists. d =0. Based on the mechanical information from the operating terminal and the Jacobian matrix of the upper limb exoskeleton, the theoretical torque required for each joint of the upper limb exoskeleton is calculated. The formula for calculating the theoretical torque required for each joint of the upper limb exoskeleton is:

[0084] Δτ=J(θ) T ΔF

[0085] ΔF=F d -F h

[0086] T cal =Δτ

[0087] Where Δτ is the error torque, T cal J(θ) represents the theoretical torque required for each joint of the upper limb exoskeleton. T Let J(θ) be the transpose of the Jacobian matrix of the upper limb exoskeleton, and ΔF be the error between the force received by the multi-dimensional force sensor of the dual-arm manipulator and the desired force. d For the expectation force, F h The force on the operating terminal is collected by the multi-dimensional force sensor of the dual-arm operating terminal.

[0088] Using the theoretical torque required for each joint of the upper limb exoskeleton as input to the PID controller, the actual torque required for each joint of the upper limb exoskeleton is obtained through the PID controller. The calculation formula for the actual torque required for each joint of the upper limb exoskeleton is as follows:

[0089]

[0090] Where τ is the actual torque required by each joint of the upper limb exoskeleton, and k p k is the proportional coefficient of the PID controller. i k is the integral coefficient of the PID controller. d The derivative coefficients of the PID controller are... For T calderivative of the force.

[0091] The actual torque required by each joint of the upper limb exoskeleton is taken as the input of each joint driver, and the movement of each joint of the exoskeleton can be controlled.

[0092] In step 104, the torque required by each joint of the upper limb exoskeleton and the actual measured torque of each joint driver are taken as inputs, and the Jacobian matrix of the upper limb exoskeleton is optimized by a compensation controller.

[0093] It should be noted that the torque value directly read from the motor end can obtain the actual driving torque T me .

[0094] The torque required by each joint of the upper limb exoskeleton and the actual measured torque of each joint driver are input into the compensation controller.

[0095] The compensation controller calculates the average required torque according to the torque required by each joint of the upper limb exoskeleton, and the calculation formula of the average required torque is:

[0096]

[0097] Where T cal_mean is the average required torque, n is the number of joints, and T cal,i is the required theoretical torque of the i-th joint.

[0098] The compensation controller calculates the average measured torque according to the actual measured torque of each joint of the upper limb exoskeleton, and the calculation formula of the average measured torque is:

[0099]

[0100] Where T me_mean is the average measured torque, and ΔT me,i is the actual measured torque of the i-th joint.

[0101] The compensation controller calculates the torque required to be compensated for each joint according to the average required torque and the average measured torque, and the calculation formula of the torque required to be compensated for each joint is:

[0102]

[0103] Where T com,i is the torque required to be compensated for the i-th joint.

[0104] An impedance model is established according to the torque required to be compensated for each joint, and the angle required to be compensated for each joint is obtained, and the impedance model is:

[0105]

[0106] Where M is the desired inertia matrix, B is the desired damping matrix, and K is the desired stiffness matrix. The deviation between the angular acceleration at the joint and the desired angular acceleration. denoted as ωi, where ωi is the deviation between the angular velocity at the joint and the desired angular velocity, and ωe is the deviation between the angle at the joint and the desired angle.

[0107] The Jacobian matrix of the upper limb exoskeleton is optimized based on the angles that each joint needs to compensate for. The optimization formula is as follows:

[0108] J(θ)=J(θ+e).

[0109] The torque T generated at each joint position to balance the force at the operating end is respectively... cal The torque T measured at each joint of the exoskeleton me As the input to the collaborative compensation controller, the angle error e output by the collaborative compensation controller is used to optimize and adjust the Jacobian matrix, i.e., J(θ) = J(θ+e).

[0110] The present invention provides a dual-arm collaborative control method for exoskeleton pole operation, comprising: acquiring the angle information of each joint of the upper limb exoskeleton and the mechanical information of the operating terminal; establishing an upper limb exoskeleton linkage model and determining DH parameters; analyzing the relationship between the position of the dual-arm operating terminal and the joint angle based on the DH parameters to obtain the coordinate rotation matrix of the upper limb exoskeleton; determining the Jacobian matrix of the upper limb exoskeleton based on the coordinate rotation matrix and the angle information of each joint; calculating the required torque of each joint of the upper limb exoskeleton based on the mechanical information of the operating terminal and the Jacobian matrix of the upper limb exoskeleton; and optimizing the Jacobian matrix of the upper limb exoskeleton through a compensation controller using the required torque of each joint and the actual measured torque of each joint drive as input. The exoskeleton pole operating terminal load dynamic estimation method provided by the present invention can reduce the deviation of the control effect of different joints, realize the dual-arm multi-joint collaborative control of the upper limb exoskeleton, enable each system to perform work tasks more efficiently and accurately, and achieve the technical effect of improving the synergy of multi-arm multi-joint motion control of the upper limb exoskeleton.

[0111] In one embodiment, the upper limb exoskeleton is divided into three states: a lifting state, a holding state, and an adjustment state. The lifting state involves one arm grasping the end of the bar, while the other arm grasps the bar and gradually raises it upwards. The holding state involves both arms stopping their movement and maintaining a certain posture after the right arm has raised the bar above the head. The adjustment state involves both arm operating terminals simultaneously moving along the direction of the bar or rotating around it after the right arm has raised the bar above the head. The specific process of the motion state determination strategy is as follows:

[0112] (1) According to the D-H parameters of the upper limb exoskeleton, the coordinate rotation matrix of the upper limb exoskeleton is calculated, the coordinate conversion relationship between each joint is obtained, that is, the coordinate rotation matrix of the upper limb exoskeleton, and the coordinate rotation matrix of the upper limb operation terminal coordinate system relative to the polar coordinate system is iteratively calculated and solved, and the position coordinates and attitude coordinates of the upper limb exoskeleton operation terminal relative to the polar coordinate system are obtained.

[0113] (2) According to the position and attitude of the dual-arm operation terminal relative to the base coordinate system, the space vector composed of the dual-arm operation terminal is determined, and has:

[0114]

[0115] Wherein, P g is the space vector composed of the dual-arm operation terminal, is the right arm operation terminal position, is the left arm operation terminal position.

[0116] (3) The angle between P g and the horizontal plane is calculated as the angle θ g between the upper limb exoskeleton lifting rod and the horizontal direction, wherein when the height of the rod end exceeds the lifting position, the angle θ g is positive, otherwise, the angle θ g is negative.

[0117] (4) When the angle between the rod and the horizontal direction does not exceed a certain threshold, it can be considered that the upper limb exoskeleton is always in the rod lifting state, at this time each joint needs to be assisted by joint force to realize the lifting of the rod, each joint adopts the dual-arm cooperative control strategy, and the joint driving force is output according to the required torque.

[0118] (5) When the angle between the rod and the horizontal direction exceeds a certain threshold, the judgment of the upper limb motion state can be carried out.

[0119] (6) If the angular velocity of each joint of the dual-arm remains unchanged, and the direction of the angular velocity does not change, it can be considered that the rod lifting action is still continued.

[0120] (7) When the angle between the rod and the horizontal direction exceeds a certain angle, if the angular velocity of each joint of the dual-arm does not reach a certain threshold, it can be considered that each joint of the upper limb stops moving, at this time the upper limb exoskeleton is in a holding state, each joint needs to be kept in a fixed position, and the dual-arm cooperative control strategy is adopted to output the joint driving force by applying a constant joint torque

[0121] (8) When the elbow joint angular velocity direction of the upper arm changes, the bending angle reversely decreases, the moving directions of the two arms operating terminals are consistent, and the position change amount of the right arm operating terminal is equal to that of the left arm operating terminal (the right arm is the upper arm by default), the upper limb exoskeleton is in an adjusting state, the elbow joint of the mechanical arm on one side of the support rod adopts constant force output, the shoulder joint of the mechanical arm on one side of the support rod and the shoulder joint and the elbow joint of the mechanical arm on the other side adopt a two-arm cooperative control strategy, and joint driving force is output according to the required torque.

[0122] For ease of understanding, please refer to Figure 3 An embodiment of a two-arm cooperative control system for exoskeleton hanging rod operation is provided in the application, comprising:

[0123] An information acquisition module is configured to acquire angle information of each joint of the upper limb exoskeleton and mechanical information of the operating terminal.

[0124] A rotation matrix acquisition module is configured to establish a link model of the upper limb exoskeleton and determine D-H parameters, analyze a change relationship between the position of the two-arm operating terminal and the joint angle according to the D-H parameters, and obtain a coordinate rotation matrix of the upper limb exoskeleton.

[0125] A joint required torque calculation module is configured to determine a Jacobian matrix of the upper limb exoskeleton according to the coordinate rotation matrix of the upper limb exoskeleton and the angle information of each joint of the upper limb exoskeleton, and calculate a required theoretical torque of each joint of the upper limb exoskeleton according to the mechanical information of the operating terminal and the Jacobian matrix of the upper limb exoskeleton.

[0126] An optimization module is configured to take the required torque of each joint of the upper limb exoskeleton and the actual measured torque of each joint drive as input, and optimize the Jacobian matrix of the upper limb exoskeleton through a compensation controller.

[0127] The joint required torque calculation module is specifically configured to:

[0128] determine the Jacobian matrix of the upper limb exoskeleton according to the coordinate rotation matrix of the upper limb exoskeleton and the angle information of each joint of the upper limb exoskeleton;

[0129] calculate the required theoretical torque of each joint of the upper limb exoskeleton according to the mechanical information of the operating terminal and the Jacobian matrix of the upper limb exoskeleton.

[0130] take the required theoretical torque of each joint of the upper limb exoskeleton as input of a PID controller, and obtain the actual torque required by each joint of the upper limb exoskeleton through the PID controller.

[0131] The calculation formula of the required theoretical torque of each joint of the upper limb exoskeleton is:

[0132] ΔF=F d -F h

[0133] T cal = Δτ

[0134] Wherein, Δτ is error moment, T cal is the required theoretical moment of each joint of upper limb exoskeleton, J(θ) T is the transpose of Jacobian matrix J(θ) of upper limb exoskeleton, ΔF is the error between the force received by the operating terminal and the expected force collected by the multi-dimensional force sensor of the dual-arm operating terminal, F d is the expected force, F h is the force received by the operating terminal collected by the multi-dimensional force sensor of the dual-arm operating terminal.

[0135] The calculation formula of the actual moment required by each joint of the upper limb exoskeleton is:

[0136]

[0137] Wherein, τ is the actual moment required by each joint of the upper limb exoskeleton, k p is the proportional coefficient of the PID controller, k i is the integral coefficient of the PID controller, k d is the differential coefficient of the PID controller, is the derivative of T cal .

[0138] The optimization module is specifically used for:

[0139] inputting the required moment of each joint of the upper limb exoskeleton and the actual measured moment of each joint drive into the compensation controller;

[0140] The compensation controller calculates the average required moment according to the required moment of each joint of the upper limb exoskeleton, and the calculation formula of the average required moment is:

[0141]

[0142] Wherein, T cal_mean is the average required moment, n is the number of joints, T cal,i is the required theoretical moment of the i-th joint;

[0143] The compensation controller calculates the average measured moment according to the actual measured moment of each joint of the upper limb exoskeleton, and the calculation formula of the average measured moment is:

[0144]

[0145] Wherein, T me_mean is the average measured moment, ΔT me,i is the actual measured moment of the i-th joint;

[0146] The compensation controller calculates the torque required to be compensated for each joint according to the average required torque and the average measured torque, and the calculation formula of the torque required to be compensated for each joint is:

[0147]

[0148] Wherein, T com,i is the torque required to be compensated for the i-th joint;

[0149] An impedance model is established according to the torque required to be compensated for each joint, and the angle required to be compensated for each joint is obtained, and the impedance model is:

[0150]

[0151] Wherein, M is the desired inertia matrix, B is the desired damping matrix, K is the desired stiffness matrix, is the deviation of the angular acceleration at the joint from the desired angular acceleration, is the deviation of the angular velocity at the joint from the desired angular velocity, and e is the deviation of the angle at the joint from the desired angle;

[0152] The Jacobian matrix of the upper limb exoskeleton is optimized according to the angle required to be compensated for each joint, and the optimization formula is:

[0153] J (θ) = J (θ + e).

[0154] The exoskeleton hanging rod operation double-arm cooperative control system provided by the application is used to execute the exoskeleton hanging rod operation double-arm cooperative control method provided by the application, and the principle and the technical effects obtained are the same as those of the exoskeleton hanging rod operation double-arm cooperative control method provided by the application, and will not be repeated here.

[0155] The above-described embodiments are only used to illustrate the technical solutions of the application, and not to limit them; although the application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacements for some technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the application.

Claims

1. A method for cooperative control of a pair of arms for a pole mounting operation of an exoskeleton, characterized by, The method comprises the following steps: Obtain the angle information of each joint of the upper limb exoskeleton and the mechanical information of the operating terminal; Establish a link model of the upper limb exoskeleton and determine D-H parameters, analyze the change relationship between the position of the double-arm operating terminal and the joint angle according to the D-H parameters, and obtain the coordinate rotation matrix of the upper limb exoskeleton; Determine the Jacobian matrix of the upper limb exoskeleton according to the coordinate rotation matrix of the upper limb exoskeleton and the angle information of each joint of the upper limb exoskeleton, and calculate the required torque of each joint of the upper limb exoskeleton according to the mechanical information of the operating terminal and the Jacobian matrix of the upper limb exoskeleton; The required torque of each joint of the upper limb exoskeleton and the actual measured torque of each joint drive are input into the compensation controller to optimize the Jacobian matrix of the upper limb exoskeleton; The required torque of each joint of the upper limb exoskeleton and the actual measured torque of each joint drive are input into the compensation controller to optimize the Jacobian matrix of the upper limb exoskeleton, comprising: Determine the Jacobian matrix of the upper limb exoskeleton according to the coordinate rotation matrix of the upper limb exoskeleton and the angle information of each joint of the upper limb exoskeleton; Calculate the required theoretical torque of each joint of the upper limb exoskeleton according to the mechanical information of the operating terminal and the Jacobian matrix of the upper limb exoskeleton; The required theoretical torque of each joint of the upper limb exoskeleton is input into the PID controller to obtain the required actual torque of each joint of the upper limb exoskeleton through the PID controller; The required torque of each joint of the upper limb exoskeleton and the actual measured torque of each joint drive are input into the compensation controller to optimize the Jacobian matrix of the upper limb exoskeleton, comprising: Input the required torque of each joint of the upper limb exoskeleton and the actual measured torque of each joint drive into the compensation controller; The compensation controller calculates the average required torque according to the required torque of each joint of the upper limb exoskeleton, and the calculation formula of the average required torque is: wherein, M is the average required moment, n is the number of joints, M is the required theoretical moment of the i-th joint; The compensation controller calculates the average measured torque according to the actual measured torque of each joint drive of the upper limb exoskeleton, and the calculation formula of the average measured torque is: wherein, is the average measured torque, is the actual measured torque of the ith joint; The compensation controller calculates the torque required to be compensated for each joint according to the average required torque and the average measured torque, and the calculation formula of the torque required to be compensated for each joint is: wherein, Mcompensatedis the moment that needs to be compensated for the ith joint; An impedance model is established according to the torque required to be compensated for each joint to obtain the angle required to be compensated for each joint, and the impedance model is: where M is the desired inertia matrix, B is the desired damping matrix, and K is the desired stiffness matrix, is the joint angular acceleration error, is the joint angular velocity error, and e is the joint angle error. The Jacobian matrix of the upper limb exoskeleton is optimized according to the angle required to be compensated for each joint, and the optimization formula is: wherein, is the Jacobian matrix of the upper limb exoskeleton.

2. The exoskeleton pole hanging operation dual-arm cooperative control method according to claim 1, characterized in that, The calculation formula of the required theoretical torque of each joint of the upper limb exoskeleton is: wherein, is an error torque, is a theoretical torque required for each joint of the upper limb exoskeleton, is a Jacobian matrix of the upper limb exoskeleton is a transpose of the Jacobian matrix, is an error between an actual force and an expected force of the dual-arm operating terminal, is the expected force, is the actual force of the dual-arm operating terminal.

3. The exoskeleton pole hanging work dual-arm cooperative control method according to claim 2, characterized by, The calculation formula of the required actual torque of each joint of the upper limb exoskeleton is: wherein, is the actual torque required for each joint of the upper extremity exoskeleton, is the proportional coefficient of the PID controller, is the integral coefficient of the PID controller, is the derivative coefficient of the PID controller, is the derivative of .

4. An exoskeleton pole hanging work dual-arm cooperative control system characterized by comprising: a pole hanging work dual-arm cooperative control device according to any one of claims 1 to 3. The method comprises the following steps: The information acquisition module is used to obtain the angle information of each joint of the upper limb exoskeleton and the mechanical information of the operating terminal; The rotation matrix acquisition module is used to establish a link model of the upper limb exoskeleton and determine D-H parameters, analyze the change relationship between the position of the double-arm operating terminal and the joint angle according to the D-H parameters, and obtain the coordinate rotation matrix of the upper limb exoskeleton; The joint required torque calculation module is used to determine the Jacobian matrix of the upper limb exoskeleton according to the coordinate rotation matrix of the upper limb exoskeleton and the angle information of each joint of the upper limb exoskeleton, and calculate the required torque of each joint of the upper limb exoskeleton according to the mechanical information of the operating terminal and the Jacobian matrix of the upper limb exoskeleton; An optimization module is configured to take the required torque of each joint of the upper-limb exoskeleton and the actual measured torque of each joint of the upper-limb exoskeleton as input, and optimize the Jacobian matrix of the upper-limb exoskeleton through a compensation controller. The joint required torque calculation module is specifically configured to: determine the Jacobian matrix of the upper-limb exoskeleton according to the coordinate rotation matrix of the upper-limb exoskeleton and the angle information of each joint of the upper-limb exoskeleton; calculate the required theoretical torque of each joint of the upper-limb exoskeleton according to the mechanical information of the operation terminal and the Jacobian matrix of the upper-limb exoskeleton; take the required theoretical torque of each joint of the upper-limb exoskeleton as input of the PID controller, and obtain the required actual torque of each joint of the upper-limb exoskeleton through the PID controller; The optimization module is specifically configured to: input the required torque of each joint of the upper-limb exoskeleton and the actual measured torque of each joint of the upper-limb exoskeleton into the compensation controller; The compensation controller calculates the average required torque according to the required torque of each joint of the upper-limb exoskeleton, and the calculation formula of the average required torque is: wherein, M is the average required moment, n is the number of joints, M is the required theoretical moment of the i-th joint; The compensation controller calculates the average measured torque according to the actual measured torque of each joint of the upper-limb exoskeleton, and the calculation formula of the average measured torque is: wherein, is the average measured torque, is the actual measured torque of the i-th joint; The compensation controller calculates the torque that needs to be compensated for each joint according to the average required torque and the average measured torque, and the calculation formula of the torque that needs to be compensated for each joint is: wherein, Mcompensatedis the moment that needs to be compensated for the ith joint; establish an impedance model according to the torque that needs to be compensated for each joint to obtain the angle that needs to be compensated for each joint, and the impedance model is: where M is the desired inertia matrix, B is the desired damping matrix, and K is the desired stiffness matrix, is the joint angular acceleration error, is the joint angular velocity error, and e is the joint angle error. optimize the Jacobian matrix of the upper-limb exoskeleton according to the angle that needs to be compensated for each joint, and the optimization formula is: wherein, is the Jacobian matrix of the upper limb exoskeleton.

5. The exoskeleton pendant operation dual-arm cooperative control system according to claim 4, characterized in that, The calculation formula of the required theoretical torque of each joint of the upper-limb exoskeleton is: wherein, is an error torque, is a theoretical torque required for each joint of the upper limb exoskeleton, is a Jacobian matrix of the upper limb exoskeleton is a transpose of the Jacobian matrix, is an error between an actual force and an expected force of the dual-arm operating terminal, is the expected force, is the actual force of the dual-arm operating terminal.

6. The exoskeleton pendant operation dual-arm cooperative control system according to claim 5, characterized by, The calculation formula of the required actual torque of each joint of the upper-limb exoskeleton is: wherein, is the actual torque required for each joint of the upper extremity exoskeleton, is the proportional coefficient of the PID controller, is the integral coefficient of the PID controller, is the derivative coefficient of the PID controller, is the derivative of .

Images