Force-position fusion compliant control method and system for rope-driven space flexible manipulator

Abstract

The present invention discloses a force-position fusion compliant control method and system for a rope-driven space flexible manipulator, comprising the following steps: establishing a DH coordinate system, and obtaining a desired force / torque, a desired posture, and a desired arm angle according to a task plan; using a joint angle encoder to collect the current posture and the current arm angle; using a six-dimensional force sensor to collect the current force / torque; obtaining a force / torque error, a posture error, and an arm angle error by subtracting the desired force / torque, the desired posture, and the desired arm angle from the current force / torque, the current posture, and the current arm angle, respectively, and calculating a control rate through a force-position fusion controller; obtaining a desired angle of joint motion from the control rate through an extended Jacobian matrix; obtaining a joint motion instruction for the next control cycle through the joint controller according to the desired angle of joint motion, driving a motor in a drive box to complete force-position fusion compliant control, and realizing safe and compliant manipulation in a narrow and multi-obstacle environment.

Description

Technical Field

[0001] The present invention relates to the technical field of on-orbit compliant control of space robots, and in particular to a force-position fusion compliant control method and system for a rope-driven space flexible manipulator arm. Background Art

[0002] The rope-driven flexible space manipulator is remotely actuated using ropes, achieving electromechanical decoupling. This design, with its large aspect ratio and high redundant degrees of freedom, allows it to adapt to the harsh space environment and demonstrates unique advantages in confined, unstructured environments. This manipulator can perform delicate tasks in orbit, such as narrow gap crossing, fault repair, and component assembly.

[0003] However, traditional compliant control methods can only control the end-arm pose and / or end-operation force / torque, but cannot synchronously control the arm's shape. When a robot arm operates in a confined, obstructed environment, if the arm's shape is not controlled to match the operating environment, the manipulator will collide with the working environment, causing the robot arm to malfunction and, in severe cases, damage the robot arm or other equipment. Summary of the Invention

[0004] In order to solve the technical problems in the prior art of limited manipulation ability, low operating efficiency and low safety of flexible robotic arms in confined spaces and unstructured environments, the primary purpose of the present invention is to provide a force-position fusion compliant control method for a rope-driven spatial flexible robotic arm.

[0005] Another object of the present invention is to provide a rope-driven space flexible manipulator force-position fusion compliance control system, comprising a processor and a memory, wherein the memory stores a computer program, and the computer program can be executed by the processor to implement the above-mentioned control method.

[0006] The technical problem of the present invention is solved by the following technical solutions:

[0007] A force-position fusion compliance control method for a rope-driven space flexible manipulator, comprising a drive box, a flexible arm rod, a six-dimensional force sensor, and a center block equipped with a joint angle encoder, includes the following steps:

[0008] S1. Establish the DH coordinate system of the flexible arm lever and the central block, and obtain the expected force / torque of the flexible arm lever, and the expected posture and expected arm angle of the central block according to the task planning;

[0009] S2. Using the joint angle encoder to collect the current posture and current arm angle of the central block; using the six-dimensional force sensor to collect the current force / torque of the flexible arm arm;

[0010] S3. Subtracting the desired force / torque, desired posture, and desired arm angle from the current force / torque, current posture, and current arm angle, respectively, to obtain a force / torque error, a posture error, and an arm angle error, and calculating a control rate of the closed-loop control group using a force-position fusion controller;

[0011] S4, obtaining the desired angle of joint motion by applying the control rate of the closed-loop control group to the extended Jacobian matrix;

[0012] S5. According to the desired angle of the joint movement, the joint movement instruction of the next control cycle is obtained through the joint PID controller, and the motor in the drive box is driven to move to complete the force-position fusion compliance control of the entire cycle.

[0013] In some embodiments, it also includes: judging whether the rope-driven spatial flexible robotic arm has completed the set operating motion trajectory; if not, repeating steps S2 to S5 to continue to enter the force-position type fusion compliance control of the next control cycle; otherwise, ending the force-position type fusion compliance control.

[0014] In some embodiments, in step S1, the DH coordinate system is to establish a DH parameter table by the DH method and obtain the homogeneous transformation matrix of each point in the Cartesian space. The expression is:

[0015]

[0016] Where: is the expression of the homogeneous transformation matrix from the base coordinate system to the link i, 0 represents the base coordinate system; and are the homogeneous transformation matrices between the end of the i-1th link and the beginning of the i-th link. Each homogeneous transformation matrix includes rotation and translation information, describing how one link moves and rotates relative to the previous one. Since the center block has two degrees of freedom, the secondary transformation matrix between two links is a product of two.

[0017] In some embodiments, the expression of the arm angle ψ1 in the desired arm angle and the current arm angle is:

[0018]

[0019] Wherein, V1 is an arbitrary fixed unit vector; vector h1 is an auxiliary vector perpendicular to vector w1 and within the arm-shaped surface A; k1 is an auxiliary vector perpendicular to w1 and perpendicular to the arm-shaped surface A; and are the unit vectors of vectors w1 and V1 respectively; the upper right subscript T indicates the transposed matrix of the corresponding matrix.

[0020] In some embodiments, in step S3, the expression of the posture error is: ed S=e p ; Among them, e p is the pose error, X ed is the pose difference, S is the matrix;

[0021] The force / torque error is expressed as: Among them, e F is the force / torque error, F ed is the force / torque difference, matrix

[0022] The expression of the arm angle error is:

[0023] ψ ed =ψ d -ψ s (θ s )=[ψ 1,d -ψ 1,s (θ s )ψ 2,d -ψ 2,s (θ s )];

[0024] Among them, among them, ψ ed is the arm angle difference; ψ d Expected value of arm angle, ψ s (θ s ) is the current joint angle value θ collected by the joint angle encoder s The calculated current value of the arm angle; ψ 1,d and ψ 2,d are the expected values ​​of arm angle 1 and arm angle 2 respectively; ψ 1,s (θ s ) and ψ 2,s (θ s ) are the current values ​​of arm angle 1 and arm angle 2 calculated by the current joint angle values ​​collected by the joint angle encoder.

[0025] In some embodiments, in step S3, the closed-loop control group includes force / torque closed-loop control, position closed-loop control and arm angle closed-loop control.

[0026] In some embodiments, in step S4, the Jacobian matrix is ​​J ex (θ)=[J|J ev ]; wherein, the arm angle Jacobian matrix J ev The expression is:

[0027]

[0028] Where J is the Jacobian matrix from the end to the joint; θ is the joint angle θ i The set of θ=[θ1,θ2,…,θ i ]; n is the number of flexible arm links; ψ s (θ s ) is the current arm angle, and and The angles of arm 1 and arm 2 are θ i The partial differential of .

[0029] In some embodiments, in step S4, the joint movement desired angle θ cmd The expression is: cmd =θ d +Δθ;

[0030] Among them, θ d is the desired joint angle, θ d =[θ 1,d ,θ 2,d ,…,θ i,d ]; Δθ is the joint motion offset.

[0031] In some embodiments, in step S5, the expression of the joint motion instruction of the next control cycle is:

[0032]

[0033] Among them, θ(s) is the joint motion instruction for the next control cycle; K p is the proportional gain coefficient, K i is the integral gain coefficient and K d is the differential gain coefficient; s is the control period.

[0034] The present invention also proposes a rope-driven space flexible robotic arm compliance control system, which includes a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor. When the processor executes the computer program, the above-mentioned control method is implemented.

[0035] The beneficial effects of the present invention compared with the prior art include:

[0036] The present invention combines the structural characteristics and closed-loop control of the rope-driven space flexible manipulator itself to achieve the rope-driven space flexible manipulator's compliance with external forces / torques and environmental constraints during contact operations to ensure the safety of the entire system; at the same time, by designing a multi-target fusion control rate that integrates the three elements of the whole system's force / torque, position, and arm angle, the rope-driven space flexible manipulator's operating force, posture, and whole-arm contact force and arm angle can be changed according to desired characteristics to meet the accuracy, speed, operating force, and stiffness requirements of large-scale manipulation in a narrow space, thereby achieving safe and smooth manipulation in a narrow and multi-obstacle environment, providing important support for the on-orbit smooth manipulation of space robots.

[0037] Other beneficial effects of the embodiments of the present invention will be further described below. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 This is a flow chart of the force-position fusion compliance control method of a rope-driven space flexible manipulator in an embodiment of the present invention.

[0039] Figure 2 4 is a side view of a rope-driven space flexible robotic arm in an embodiment of the present invention.

[0040] Figure 3 Schematic diagram of the center block coordinates of the flexible robotic arm in an embodiment of the present invention.

[0041] Figure 4 Schematic diagram of arm angle calculation in an embodiment of the present invention.

[0042] Figure 5 This is a schematic diagram of the force-position control principle of a rope-driven space flexible manipulator in an embodiment of the present invention.

[0043] Figure 6 This is a flow chart of the force-position control of a rope-driven space flexible manipulator in an embodiment of the present invention.

[0044] Figure 7 4 is a dual closed-loop servo control flow chart of an embodiment of the present invention.

[0045] Figure 8 This is a physical picture of the rope-driven space flexible robotic arm according to an embodiment of the present invention.

[0046] Figure 9 This is a three-dimensional diagram of a rope-driven space flexible robotic arm according to an embodiment of the present invention.

[0047] Reference numerals:

[0048] Arm 100 , drive motor 200 , center block 300 , drive box 400 , drive rope 500 , six-dimensional force sensor 600 , joint angle encoder 700 . DETAILED DESCRIPTION

[0049] The present invention will be further described below with reference to the accompanying drawings and in combination with preferred embodiments. It should be noted that, unless there is a conflict, the embodiments and features in the embodiments of the present application can be combined with each other.

[0050] It should be noted that the directional terms such as left, right, up, down, top, and bottom in this embodiment are merely relative concepts, or are based on the normal use status of the product, and should not be considered as restrictive.

[0051] Currently, there are only a few studies on force-position hybrid control of robotic arms. Although rigid joint robotic arms use rigid joint robotic arms for force-position hybrid control, which has certain advantages in terms of accuracy and speed, their control capabilities in confined spaces and unstructured environments are limited. In addition, rigid joint robotic arms have low degrees of freedom and cannot adapt to the complex space environment. Compared with the present invention, the rigid joint robotic arm control technology solution has weaker control capabilities in confined spaces and unstructured environments and cannot achieve high redundant degrees of freedom. Specifically, it has the following disadvantages:

[0052] 1. Unable to achieve high redundant degrees of freedom, resulting in limited control capabilities in complex environments.

[0053] 2. In confined spaces and unstructured environments, the force-position hybrid control method of rigid joint robotic arms may not fully utilize the flexible characteristics of the robotic arm and cannot simultaneously control the three elements of the robotic arm's end position, operating force, and arm shape, resulting in reduced operational efficiency and safety.

[0054] 3. Traditional compliant control methods can only control the end-position and / or end-operating force / torque of a rope-driven space flexible manipulator. Rigid joint manipulators lack synchronous control of the manipulator's arm shape. When operating in a narrow, multi-obstacle environment, if the manipulator's arm shape is not controlled to match the working environment, the manipulator may contact and collide with the working environment, affecting the normal operation of the manipulator and even damaging the manipulator or other equipment.

[0055] In order to solve the arm shape control problem of a flexible manipulator in a confined environment caused by the existing compliant control technology that only considers the end posture and / or end operating force control of the manipulator, and in order to operate in a confined and multi-obstacle environment, the manipulator not only needs to move along the desired trajectory and generate the desired operating force / torque, but also needs to adjust the arm shape to adapt to the operating environment and avoid collision and impact. Therefore, an embodiment of the present invention proposes a force-position fusion compliant control method and a corresponding control system for a rope-driven space flexible manipulator.

[0056] This method combines the robot's inherent structural characteristics with closed-loop control, enabling the space robot to adapt to external forces / torques and environmental constraints during contact operations, ensuring overall system safety. This ensures the on-orbit compliant control capabilities of a tethered-driven space flexible manipulator. The force-position-shape fusion compliant control method for a tethered-driven space flexible manipulator also incorporates the operational path and task characteristics to design a multi-objective fusion control law that integrates the force, position, and shape elements of the entire system. Multiple spatial motions are decomposed with the goal of minimizing the overall arm drive cost, constructing a dual closed-loop joint control framework for the drive space and task space.

[0057] The above design achieves desired variations in the manipulator's operating force, position, contact force, and arm shape. It can simultaneously control the manipulator's end-position, operating force, and arm shape, meeting the precision, speed, operating force, and stiffness requirements for wide-area manipulation in confined spaces. Through this force-position-integrated compliant control method and system, the rope-driven flexible space manipulator can achieve safe and compliant manipulation in confined, multi-obstacle environments, ensuring its ability to adapt to the external environment and maintain safe operation, providing important support for the compliant on-orbit manipulation of space robots.

[0058] The principle of the present invention to solve the technical problems caused by the existing compliant control technology that only considers the end-arm posture and / or end-arm operating force control is as follows:

[0059] The force-position fusion compliant control method of a rope-driven space flexible manipulator proposed in an embodiment of the present invention can simultaneously control the three elements of the manipulator's end posture, operating force and manipulator arm shape, so that when the manipulator operates in a narrow, multi-obstacle environment, not only does its end move along the desired trajectory, contact the environment, and generate the desired operating force / torque, but the manipulator arm shape also adapts to the operating environment during movement without causing excessive collisions and impacts, thereby having higher safety and compliance, and providing important support for the on-orbit compliant control of space robots.

[0060] like Figure 2As shown, the rope-driven spatial flexible manipulator according to an embodiment of the present invention includes a flexible arm rod 100, a drive motor 200, a center block 300, a drive box 400, a drive rope 500, a six-dimensional force sensor 600, and a joint angle encoder 700. The drive motor 200 is selected and configured, and design parameters such as the length of the flexible arm rod 100 are adjustable. The drive box 400 includes N flexible arm rod segments connected in series. The N flexible arm rod segments include the flexible arm rods 100 connected in sequence and the center block 300 equipped with a joint angle encoder. Each flexible arm rod segment is connected to the drive box 400 via three drive ropes 500. Each center block 300 is equipped with a joint angle encoder 700 for collecting angle information of the center block joints. Each drive rope 500 is equipped with a six-dimensional force sensor 600 for real-time acquisition of the drive rope tension. In this embodiment, the six-dimensional force sensor 600 is a rope tension sensor, which is installed in the drive box 400.

[0061] The force-position fusion compliance control method of the rope-driven space flexible manipulator according to the embodiment of the present invention is as follows: Figure 1 As shown, it includes the following steps:

[0062] S1: Establish the DH coordinate system of the flexible arm rod and the central block in the rope-driven space, and obtain the expected force / torque of the flexible arm rod, the expected position and expected arm angle of the central block according to the task planning.

[0063] S2: Use the joint angle encoder to collect the current position and arm angle of the center block of the rope-driven flexible robot arm; specifically, collect the angle of the center block, and calculate the center block position, end position and arm angle parameters on the N arm segments; through the end six-dimensional force sensor, collect the current force / torque of the flexible arm arm.

[0064] S3: By subtracting the desired force / torque, desired posture and desired arm angle from the current force / torque, current posture and current arm angle respectively, the force / torque error, posture error and arm angle error are obtained respectively, and the control rate of the closed-loop control group is calculated through the force-position fusion controller; specifically, the operating force, position and arm shape are selected, planned and controlled according to the selection matrix.

[0065] S4: obtaining the desired angle of joint motion by applying the control rate of the closed-loop control group to the extended Jacobian matrix;

[0066] S5: According to the desired angle of joint movement, the joint movement instruction of the next cycle is obtained through the joint space closed-loop controller (i.e., joint PID controller), and the motor in the drive box is driven to move to complete the force-position fusion compliance control of the entire cycle.

[0067] S6: The output joint control variable is further converted into the rope velocity through mapping from joint space to rope space. The rope velocity is then converted into the motor velocity through mapping from rope space to drive space, driving the rope to complete the next cycle of manipulator operation. The process then determines whether the trajectory has been completed. If not, the process returns to step S2 and begins the next control cycle. Otherwise, the process ends.

[0068] The coordinates of the center point of the flexible manipulator center block are as follows: Figure 3 As shown, where P0 to P N+1 Each point represents the three-dimensional coordinate information of the position at the center of each central block.

[0069] In the embodiment of the present invention, step S1 further includes calculating the positions of each center block and the arm angle, and the specific operations are as follows:

[0070] The DH coordinate system is established by the DH method (a matrix method for establishing a coordinate system for each rod in the joint chain), establishing a DH parameter table (a transformation relationship table of adjacent joint coordinates), and bringing in the current joint angle θ i , get the homogeneous transformation matrix of each point in Cartesian space Its expression is as follows:

[0071]

[0072] Among them, in this formula and They are respectively "homogeneous transformation matrix from coordinate system 2i-2 to 2i-1" and "homogeneous transformation matrix from coordinate system 2i-1 to 2i". is the symbol for continuous multiplication. Therefore, the homogeneous transformation matrix And the robot base origin P0 can obtain P1 to P N+1 Cartesian coordinates of each point. In order to obtain the arm angle related calculation, two auxiliary vectors w1 = P3–P1 and e1 = P2-P1 are established in the local coordinate system. The arm angle calculation principle in the embodiment of the present invention is as follows: Figure 4 As shown, let V1 represent an arbitrary fixed unit vector, and the auxiliary vector d1 is the projection of vector e1 on vector w1. Other parameters can be derived geometrically as follows:

[0073]

[0074] Wherein, vector h1 is an auxiliary vector perpendicular to vector w1 (or vector d1) and within arm-shaped surface A; k1 is an auxiliary vector perpendicular to w1 (or d1) and perpendicular to arm-shaped surface A; I is a unit vector, i.e., a unit vector whose 3×3 diagonal is all 1; and are the unit vectors of vectors w1 and V1 respectively.

[0075] S3. By subtracting the desired force / torque, desired posture, and desired arm angle from the current force / torque, current posture, and current arm angle, respectively, a force / torque error, a posture error, and an arm angle error are obtained, and the control rate of the closed-loop control group is calculated by the force-position fusion controller; based on the above principle, the arm angle ψ1 in the desired arm angle and the current arm angle is further deduced as:

[0076]

[0077] Wherein, V1 is an arbitrary fixed unit vector; vector h1 is an auxiliary vector perpendicular to vector w1 and within the arm-shaped surface A; k1 is an auxiliary vector perpendicular to w1 and perpendicular to the arm-shaped surface A; and are the unit vectors of vectors w1 and V1 respectively.

[0078] Similarly, according to P N-3 、P N-2 and P N-1 The coordinates of the three points in Cartesian space ( Figure 3 (Exhibit N = 6) The wrist angle ψ2 can also be derived. The specific derivation and equations are the same as for ψ1, except that w2 = P4 – P3 and e4 = O5 – P3. Then, solve for d2, h2, k1, and ψ2.

[0079] Step S3 of the embodiment of the present invention includes decomposing the force / torque information obtained by feedback to obtain the current force difference F ed Similarly, combined with step S1, the difference value X of the current posture can also be obtained ed Considering that the position control of force-position hybrid control contains k position constraints and 6-k force constraints, the expected position of the next cycle is subtracted from the current position to obtain the position difference X ed , and then multiply it with the posture selection matrix S to get the position control rate. Among them, the posture error e p As shown below:

[0080] X ed S=e p

[0081] where e p is the error value of position control. Similarly, the selection matrix must be defined for the end operation force, so the force selection matrix Similarly, the force error can be obtained as:

[0082]

[0083] where e Fis the error value of force control. The expected arm angle of the flexible arm is mainly determined by the expected value of different task planning, so the expected flexible arm angle parameter ψ d =[ψ 1,d ,ψ 2,d ]. Similarly, the expected value (i.e., the expected arm angle) and the current value (i.e., the current arm angle) ψ s (θ s )=[ψ 1,s (θ s ),ψ 2,s (θ s )] is the difference between the arm angle error value ψ ed (i.e. arm angle error) is shown below:

[0084] ψ ed =ψ d -ψ s (θ s )=[ψ 1,d -ψ 1,s (θ s )ψ 2,d -ψ 2,s (θ s )];

[0085] The current arm angle ψ s (θ s ) is the current joint angle value θ retrieved by the joint angle encoder 700 s =[θ 1,s ,θ 2,s ,…,θ i,s ] The calculated arm angle ψ s (θ s The force-position control block diagram of the rope-driven space flexible manipulator is as follows: Figure 5 As shown:

[0086] The expected force / torque, expected posture, and expected arm angle can be derived from the planned values, and the current force / torque, posture, and arm angle can be derived from the values ​​of the six-dimensional force sensor and joint angle encoder. By subtracting the expected value from the current value, the force / torque error, position error, and arm error can be obtained. This error is then calculated using a force-position fusion controller (a PID—proportional-integral-differential controller) to determine the control rate of the closed-loop control group. This control rate is then sent to the servo drive to drive the motor, completing the entire cycle of force-position fusion compliance control.

[0087] The closed-loop control group includes three closed-loop controls: force / torque, position, and arm shape.

[0088] In closed-loop force / torque control, the real-time force information at the end of the manipulator must first be obtained from the six-dimensional force sensor and compared with the desired value. The error between the desired and actual forces is used as input. A force-position fusion controller calculates the force / torque control law for the next cycle and uses this as the output of the end effector, ensuring that the output force / torque at the end of the manipulator reaches the desired value.

[0089] In position closed-loop control, the planned posture needs to be taken as the expected value first, and the error between the expected posture and the actual posture is taken as the input. The posture control law of the next cycle is calculated through the force-position fusion controller to achieve tracking of the end motion, so as to further improve the characteristics and accuracy of motion control.

[0090] In closed-loop arm control, the main focus is on controlling the arm's parameters to maintain a consistent shape during motion, adapting to specific work scenarios. The arm angle is used as the desired value for the arm tracker, and a PID controller for the arm angle is set to track the end-of-the-arm motion, further improving its control characteristics.

[0091] S4. The control rate of the closed-loop control group is obtained by extending the Jacobian matrix to obtain the desired angle of joint motion. The specific operation is as follows:

[0092] The control rate of the force-position fusion controller output is multiplied by the extended Jacobian matrix (i.e., the arm angle Jacobian matrix) J ex (θ)=[J|J ev The purpose is to decompose the force-position space into the joint space, where J is the Jacobian matrix from the end to the joint; θ is the joint angle θ i The set of θ=[θ1,θ2,…,θ i ]; and J ev is the arm angle Jacobian matrix.

[0093]

[0094] Where J is the Jacobian matrix from the end to the joint; θ is the joint angle θ i The set of θ=[θ1,θ2,…,θ i ];ψ s (θ s ) is the current arm angle, and and The angles of arm 1 and arm 2 are θ i The expected value of each joint angle can be obtained through the above calculation process.

[0095] Combined with the extended Jacobian matrix of the arm angle, it is an expansion task of the end-of-arm manipulation force-posture control task, which can then fill the redundant degrees of freedom of the manipulator's joints, so that when the manipulator's extended task (arm angle + position + posture + force) is specific, its joint angle has a unique increment, namely the joint motion offset Δθ. It should be pointed out that from a physical point of view, in extreme cases, when the manipulator is about to straighten, the arm angle degenerates into a straight line, which cannot effectively represent the configuration of the robot. Therefore, the arm angle has a certain range, otherwise it is easy to appear singular.

[0096] Therefore, combined Figure 5 The force-position control block diagram of the rope-driven space flexible manipulator is obtained as follows: Figure 6 The force-position flow chart of the rope-driven spatial flexible robotic arm shown in the figure illustrates the process relationship of the entire control, and some of the contents are further explained: the joint angle command of the current cycle is obtained by task planning, mainly including the expected posture, expected force / torque and expected arm angle; the expected value must be subtracted from the current value obtained by the six-dimensional force sensor and joint encoder before it can be further sent to the controller; the three controllers are integrated together as a force-position fusion controller; the role of the (extended) Jacobian matrix is ​​to convert the control rate output by the controller into the joint angle increment Δθ; the mapping from joint space to motor space is mainly to convert the joint angle and the movement of the motor, and finally convert it into the offset of the motor, thereby realizing the control of the entire closed loop.

[0097] Step S4 in the embodiment of the present invention includes converting the force-position offset of the end of the manipulator into the joint motion offset Δθ of the joint, adding the current cycle joint motion instruction (the instruction of the motion layer planning) to the joint motion offset to obtain the expected joint motion angle θ cmd . Where θ d is the set of desired joint angles: θ d =[θ 1,d ,θ 2,d ,…,θ i,d ]. Further explanation: θ d It is mainly determined by the expected joint angle of task planning, that is, the expected joint angle calculated by the expected posture, and the expected angle of joint motion θ cmd It is the desired angle of motion after considering the force / torque and arm angle, which is the input command of the next layer of position control inner loop. cmd The calculation is as follows:

[0098] θ cmd =θ d +Δθ;

[0099] Where θ d Denoted as the desired joint angle, θ d =[θ1,d ,θ 2,d ,…,θ i,d ]; Δθ represents the joint motion offset.

[0100] The specific operations in step S5 of the embodiment of the present invention are as follows:

[0101] The desired angle of joint motion is used as the desired angle of joint motion tracker, and a joint PID controller of joint position is set to track the desired joint motion to further improve its control characteristics. Specifically, the desired angle of joint motion θ is used as the desired angle of joint motion tracker. cmd And the current joint angle information θ fed back by the joint encoder s The difference is used as the input of the controller, and a PID controller is used to output the joint motion feedback control planning value θ(s) (i.e., the joint motion instruction) for the next control cycle. The input and output of the controller calculate the joint motion instruction for the next control cycle, and its expression is as follows:

[0102]

[0103] Where, θ(s) represents the joint motion instruction for the next cycle; K p is the proportional gain coefficient, K i Integral gain coefficient and K d Differential gain coefficient; s is the control period.

[0104] In step S6 of this embodiment, after obtaining the joint motion instruction for the next cycle, the desired motion θ of the manipulator driving space is obtained through the conversion relationship between the flexible manipulator joint space motion and the driving space motion, and the driving motor motion is controlled to complete a planning cycle of force-position control. In the process of driving the motor, the feedback value θ of the motor encoder can be used to calculate the desired motion θ of the manipulator driving space. d Closed-loop control is performed to further ensure the follow-up performance of the drive motor to the desired motion.

[0105] The force-position fusion compliance control method of the rope-driven space flexible manipulator in the embodiment of the present invention mainly includes dual closed-loop servo control. Therefore, the control block diagram of the system is drawn as follows: Figure 7 As shown. Its dual closed-loop control mainly includes two servo controls: "force-position closed-loop control" and "joint closed-loop control". According to the planned value, the expected force / torque, expected posture and expected arm angle can be obtained, and the current force / torque, current posture and current arm angle can be obtained by using the values ​​of the six-dimensional force sensor and joint angle encoder. By taking the difference between the expected value and the current value, we can get: force / torque error, position error and arm error, and calculate them through the force-position fusion controller to get the control rate of the closed-loop control group, and calculate the joint motion offset Δθ and the expected angle θ of the joint motion by the extended Jacobian matrix. cmd, and the joint angle control rate is obtained through the joint PID controller, which is further converted into the expected value of the motor to complete a whole cycle control.

[0106] After the above steps S1 to S6, a force-position type control cycle is completed. If the rope-driven space flexible robotic arm completes the set operation motion instruction (i.e., the operation motion trajectory) at this time, that is, the motion instruction given to the rope-driven space flexible robotic arm has not stopped (i.e., not completed), then steps S2 to S6 are repeated to continue the planning and movement of another control cycle (i.e., the next control cycle) (i.e., force-position type fusion compliant control); otherwise, the force-position type fusion compliant control is ended.

[0107] An embodiment of the present invention proposes a compliant control system for a rope-driven space flexible robotic arm, comprising a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor. When the processor executes the computer program, a force-position fusion compliant control method for a rope-driven space flexible robotic arm is implemented.

[0108] The force-position-shape fusion compliant control system for a rope-driven space flexible manipulator proposed in an embodiment of the present invention fully utilizes the high redundant degrees of freedom of a flexible joint manipulator, enabling more flexible manipulation in complex space environments. This helps address the limited manipulation capabilities of existing rigid joint manipulators in confined and unstructured environments. Combining the robot's inherent structural characteristics with closed-loop control, the system enables the space robot to comply with external forces / torques and environmental constraints during contact manipulation, ensuring overall system safety. This helps address the existing issues with rigid joint manipulators, which can lead to reduced operational efficiency and safety in confined and unstructured environments. The present embodiment also designs a multi-objective fusion control law that integrates the three elements of force, position, and shape across the entire system, achieving desired variations in the manipulator force, posture, contact force, and arm shape. This helps address the existing issues with rigid joint manipulators, which lack synchronized control of the manipulator's arm shape, potentially leading to contact and collision with the operating environment in confined and obstructed environments.

[0109] In summary, the embodiments of the present invention effectively solve the technical problems in the prior art of limited manipulation capabilities, reduced operating efficiency and reduced safety of rigid joint robotic arms in confined spaces and unstructured environments through technical features such as high redundant degrees of freedom, flexible control and synchronous control, and provide strong guarantees for the requirements of accuracy, speed, operating force and rigidity for large-scale manipulation in confined spaces.

[0110] In different embodiments, a flexible arm similar to the above embodiment is designed and equipped with a six-dimensional force sensor and a joint encoder, etc.; the PID parameters in the dual closed-loop control are adjusted according to the specific performance. In other embodiments, the proportional coefficient of the force-position fusion controller is 80, the integral coefficient is 0.2, and the differential coefficient is 100. The proportional coefficient of the joint closed-loop control is 30, the integral coefficient is 0.05, and the differential coefficient is 0.1. Using this embodiment, a force-position control model is established, and a set of trajectories are designed for specific movements. The following parameters are verified through experiments: position accuracy, force / torque accuracy, and arm angle accuracy. The actual picture of the rope-driven space flexible robotic arm of this embodiment is as follows. Figure 8 As shown, the three-dimensional diagram is Figure 9 shown.

[0111] The above is a further detailed description of the present invention in conjunction with specific preferred embodiments, and the specific implementation of the present invention should not be considered to be limited to these descriptions. Those skilled in the art will recognize that several equivalent substitutions or obvious variations can be made without departing from the scope of the present invention, and that any equivalent performance or application should be considered to fall within the scope of protection of the present invention.

Claims

1. A force-position fusion compliance control method for a rope-driven space flexible manipulator, characterized in that: The rope-driven space flexible robotic arm includes a drive box, a flexible arm rod, a six-dimensional force sensor, and a center block equipped with a joint angle encoder, and includes the following steps: S1. Establish the DH coordinate system of the flexible arm lever and the central block, and obtain the expected force / torque of the flexible arm lever, and the expected posture and expected arm angle of the central block according to the task planning; S2. Using the joint angle encoder to collect the current posture and current arm angle of the central block; using the six-dimensional force sensor to collect the current force / torque of the flexible arm arm; S3. Subtracting the desired force / torque, desired posture, and desired arm angle from the current force / torque, current posture, and current arm angle, respectively, to obtain a force / torque error, a posture error, and an arm angle error, and calculating a control rate of the closed-loop control group using a force-position fusion controller; S4, obtaining the desired angle of joint motion by applying the control rate of the closed-loop control group to the extended Jacobian matrix; S5. According to the desired angle of the joint movement, the joint PID controller is used to obtain the joint movement instruction for the next control cycle, and the motor in the drive box is driven to move to complete the force-position fusion compliance control of the entire cycle; The expression of the arm angle ψ1 in the desired arm angle and the current arm angle is: Wherein, V1 is an arbitrary fixed unit vector; vector h1 is an auxiliary vector perpendicular to vector w1 and within the arm-shaped surface A; k1 is an auxiliary vector perpendicular to w1 and perpendicular to the arm-shaped surface A; and are the unit vectors of vectors w1 and V1 respectively; the upper right subscript T indicates the transposed matrix of the corresponding matrix.

2. The force-position fusion compliance control method for a rope-driven space flexible manipulator according to claim 1 is characterized in that: Also includes: Determine whether the rope-driven spatial flexible manipulator has completed the set operating motion trajectory. If not, repeat steps S2 to S5 to continue to enter the force-position fusion compliance control of the next control cycle; otherwise, end the force-position fusion compliance control.

3. The force-position fusion compliance control method for a rope-driven space flexible manipulator according to claim 1 is characterized in that: In step S1, the DH coordinate system is to establish a DH parameter table by the DH method and obtain the homogeneous transformation matrix of each point in the Cartesian space. The expression is: in: is the expression of the homogeneous transformation matrix from the base coordinate system to the link i, 0 represents the base coordinate system; and are the homogeneous transformation matrices of the tail end of the i-1th connecting rod and the head end of the i-th connecting rod respectively.

4. The force-position fusion compliance control method for a rope-driven space flexible manipulator according to claim 1 is characterized in that: In step S3, the expression of the posture error is: ed S=e p ; Among them, e p is the pose error, X ed is the pose difference, S is the matrix; The force / torque error is expressed as: Among them, e F is the force / torque error, F ed is the force / torque difference, the matrix The expression of the arm angle error is: ψ ed =ψ d -ψ s (i s )=[ψ 1,d -ψ 1,s (i s )ψ 2,d -ψ 2,s (i s )]; Among them, ψ ed is the arm angle difference; ψ d Expected value of arm angle, ψ s (θ s ) is the current joint angle value θ collected by the joint angle encoder s The calculated current value of the arm angle; ψ 1,d and ψ 2,d are the expected values ​​of arm angle 1 and arm angle 2 respectively; ψ 1,s (θ s ) and ψ 2,s (θ s ) are the current values ​​of arm angle 1 and arm angle 2 calculated by the current joint angle values ​​collected by the joint angle encoder.

5. The force-position fusion compliance control method for a rope-driven space flexible manipulator according to claim 1 is characterized in that: In step S3, the closed-loop control group includes force / torque closed-loop control, position closed-loop control and arm angle closed-loop control.

6. The force-position fusion compliance control method for a rope-driven space flexible manipulator according to claim 1 is characterized in that: In step S4, the Jacobian matrix is ​​J ex (θ)=[J|J ev ]; wherein, the arm angle Jacobian matrix J ev The expression is: Where J is the Jacobian matrix from the end to the joint; θ is the joint angle θ i The set of θ=[θ1,θ2,…,θ i ]; n is the number of flexible arm links; ψ s (θ s ) is the current arm angle, and and The angles of arm 1 and arm 2 are θ i The partial differential of .

7. The force-position fusion compliance control method for a rope-driven space flexible manipulator according to claim 1, characterized in that: In step S4, the joint motion expected angle θ cmd The expression is: cmd =θ d +Δθ; Among them, θ d is the desired joint angle, θ d =[θ 1,d ,θ 2,d ,…,θ i,d ]; Δθ is the joint motion offset.

8. The force-position fusion compliance control method for a rope-driven space flexible manipulator according to claim 1 is characterized in that: In step S5, the expression of the joint motion instruction of the next control cycle is: Among them, θ(s) is the joint motion instruction for the next control cycle; K p is the proportional gain coefficient, K i is the integral gain coefficient and K d is the differential gain coefficient; s is the control period; θ cmd (s) is the motion instruction for the desired angle of joint motion; θ s (s) is the motion instruction of the current joint angle.

9. A force-position fusion compliance control system for a rope-driven space flexible manipulator, characterized in that: The method comprises a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, wherein the processor implements the control method according to any one of claims 1 to 8 when executing the computer program.

Images