Hydraulic double-arm trajectory planning and control method considering multi-physical constraint restrictions

Abstract

The application discloses a closed-chain operation hydraulic double-arm trajectory planning and motion control method considering multiple physical constraint limitations. The method comprises the following steps: establishing a hydraulic double-arm trajectory planner, modeling the physical constraints of the hydraulic double-arm, and generating a new expected trajectory according to an input instruction; establishing a hydraulic double-arm motion controller, receiving the expected trajectory from the trajectory planner, adaptively updating object inertia parameters for estimation of the physical constraint boundary, and performing motion control on the double arm. The application can be used for closed-chain operation of the hydraulic double-arm object, ensures that the object motion does not exceed the multiple physical constraint limitations of the system, so that the object does not deviate from the expected path, reduces the double-arm position error and internal force, and thus avoids the occurrence of unexpected accidents.

Description

Technical Field

[0001] This invention relates to a motion planning and control method in the field of hydraulic dual-arm motion control, specifically a hydraulic dual-arm trajectory planning and control method that considers multiple physical constraints. Background Technology

[0002] Hydraulic robotic arms have long been the primary operating mechanism for engineering equipment such as excavators and cranes due to their advantages, including high power density and strong environmental adaptability. However, with the diversification of battlefield emergency response and disaster relief missions, and the increasing complexity of environments, the low efficiency and high safety risks of single-arm operation have become increasingly prominent, highlighting the necessity and superiority of dual-arm heavy-duty collaborative operation.

[0003] In practical working conditions, the motion control of hydraulic dual arms is affected by multiple physical constraints, such as flow (velocity) and dynamic (acceleration) constraints. If the object's reference trajectory is not pre-tested, situations may arise where the system's physical constraints cannot be met during actual operation. If a command signal is applied to the object at too high a speed at a certain moment, the end effector of the heavy-load hydraulic arm may not move at the desired speed, resulting in a phenomenon where the heavy-load arm moves slowly and the light-load arm moves quickly, causing relative motion between the end effectors of the two arms. Consequently, the hydraulic dual arms will not move exactly along the desired trajectory, potentially leading to accidents such as damage to the robotic arms or the object. Therefore, the main problem in closed-chain cooperation of hydraulic dual arms is planning the object's motion trajectory to meet the various physical constraints of the system. To solve the above problems, a hydraulic dual-arm trajectory planning and control method considering multiple physical constraints is proposed. This method mainly consists of two parts: trajectory planning and motion control, enabling high-precision, low-internal-force cooperative tasks where the hydraulic dual arms move along the desired path. Summary of the Invention

[0004] In view of this, in order to solve the problem of reduced control accuracy caused by multiple constraints of the hydraulic system in the motion control of the hydraulic double arm, this invention provides a hydraulic double arm trajectory planning and control method that takes into account multiple physical constraints. The method designs a trajectory planner for the manipulated object and a motion controller for the hydraulic double arm to address the multiple physical constraints of the hydraulic system, thereby improving the positional accuracy of the manipulated object moving along the desired path and reducing the internal forces on the object.

[0005] To achieve the above objectives, the specific technical solution of the present invention is as follows:

[0006] This invention includes the following steps:

[0007] 1) The trajectory planner receives the object reference trajectory and replans the reference trajectory according to the multi-physical constraints of the hydraulic double arms to generate the object's desired trajectory that meets the system constraints.

[0008] 2) The hydraulic dual-arm motion controller receives the replanned desired trajectory signal of the object, generates signals to control each servo valve of the hydraulic dual arms, and controls the hydraulic dual arms to achieve coordinated operation.

[0009] In step 1), the object's reference trajectory consists of reference path parameters and path geometry, and can be represented as: x r = p O ( r ( t )).in, x r Indicates the reference trajectory of the object. p O (·) represents a predefined path geometry. r ( t ) represents the reference time law along the path. The trajectory planner, constrained by maintaining the path geometry, only performs time law replanning on the reference path parameters.

[0010] The boundary estimator generates first-order and second-order derivative boundaries of the path parameters based on the multi-physical constraints of the hydraulic dual-arm system, real-time joint angle feedback signals, pressure feedback signals, and end-effector force feedback signals from the dual-arm bottom controller. The multi-physical constraints include at least pump flow constraints, valve flow constraints, valve flow constraints affected by load changes, dual-arm end-effector acceleration constraints, and torque constraints of each joint in the dual arms. The first-order derivative boundary of the path parameters is mainly obtained by combining the pump flow constraints, valve flow constraints, and valve flow constraints affected by load changes, and the lower boundary of the path parameters can be set to 0 to prevent object retraction. The second-order derivative boundary of the path parameters is mainly obtained by combining the dual-arm end-effector acceleration constraints and the torque constraints of each joint in the dual arms, and can be adjusted according to the estimated values ​​of the object's inertia parameters. θ aO and the need for external force at the ends of both arms F d Updated online.

[0011] The nonlinear filter performs time-law reprogramming on the reference path parameters based on the aforementioned path parameter boundaries. Preferably, the nonlinear filter includes a variable structure controller and an integrator module. The variable structure controller outputs the second derivative of the reprogrammed path parameters based on the current path parameters, the path parameter derivatives, and their boundaries. The integrator module integrates these second derivatives to obtain the reprogrammed path parameters and their first derivatives.

[0012] By combining the replanning path parameters and the predefined path geometry, the desired trajectory of the object that conforms to multi-physics constraints can be obtained, which can be expressed as: x d =p O ( s ( t )).in, x d Represents the expected trajectory of an object. s ( t ) represents the path parameters after replanning.

[0013] In step 2), a closed-chain coordination constraint relationship for the coordinated operation of the two hydraulic arms on the same object is first established. Through the position constraints, orientation constraints, and velocity constraints between the object coordinate system and the coordinate systems of the left and right arm ends, as well as the coordination relationship between the joint velocities of the two arms, the required pose and velocity of the two arm ends can be obtained according to the desired motion of the object. Combined with the joint angle signals measured by the angle encoder, the actual position and actual velocity of the object can be obtained.

[0014] Secondly, a dynamic model of the manipulated object is established in the object coordinate system and written as a linear parameterized form with respect to the object's inertial parameter vector, which can be expressed as: F O = Y O θ O .in, F O This represents the net external force acting on an object. Y O Represents the object dynamics regression matrix. θ O This represents the object's inertial parameter vector. By designing an adaptive function for the object's inertial parameters, the inertial parameter vector is updated online to obtain estimated values ​​of the object's inertial parameters. Preferably, the adaptive function can be expressed as: dθ aO =Gφ .in, G Represents the adaptive gain matrix. φ This represents the update term constructed based on the object's motion error and the regression matrix. The updated estimates of the object's inertial parameters are obtained through continuous iteration. θ aO And further obtain the combined external force required to control the motion of the object.

[0015] Then, based on the closed-chain coordination constraints, the desired trajectory of the object, the actual motion state of the object, and the required net external force, an object motion controller is established to obtain the required velocity and required external force at the ends of both arms. The object motion controller takes into account both object motion tracking and internal force control.

[0016] The actual internal force is estimated from the pressure signal of the pressure sensor. The error between the expected internal force and the actual internal force can be expressed as: ef = F id - F i .in, F id Indicates the desired internal strength. F i This represents the actual internal force estimated based on the pressure feedback signal. Based on the internal force error and a preset internal force error gain, the required external force at the ends of the two arms can be generated to reduce mutual compression or tension in closed-chain cooperation. This required external force at the ends of the two arms can also be fed back to the boundary estimator for online updating of the second derivative boundaries of the path parameters determined by the joint torque constraints.

[0017] Finally, the single-arm controller outputs control signals to each hydraulic control valve based on the required speed and external force at the ends of both arms, thereby enabling coordinated operation of the hydraulic arms on the object.

[0018] Preferably, the hydraulic dual-arm consists of a multi-joint linkage robotic arm and a hydraulic system. The robotic arm joints are driven by swing hydraulic cylinders or linear hydraulic cylinders. Angle encoders are installed at the joints of the multi-joint linkage robotic arm, and pressure sensors are installed in the hydraulic system. The pressure sensors and angle encoders are used to monitor the joint angles and pressure signals in real time, and feed them back to the trajectory planner and the hydraulic dual-arm motion controller to achieve complete hydraulic dual-arm trajectory planning and closed-loop control of motion control.

[0019] Compared with the prior art, the present invention has at least the following beneficial effects:

[0020] 1. This invention establishes a hydraulic dual-arm multi-physical constraint model and proposes a trajectory planner based on path parameter-geometric shape decomposition. While ensuring that the planned trajectory does not exceed the physical constraints of the system, it enables the object to move along the desired geometric shape.

[0021] 2. By designing an object motion controller and a single-arm controller, and introducing pressure signal internal force estimation, internal force closed-loop control and object inertial parameter adaptive mechanism, this invention can effectively reduce the position error, attitude error and internal force of the object motion.

[0022] 3. By feeding back the estimated inertial parameters of the object obtained from the control side and the required external force at the end of the two arms to the boundary estimator on the planning side, this invention can realize the integrated closed-loop coupling of "planning-control" and improve the system's adaptability to load and constraint changes. Attached Figure Description

[0023] Figure 1 This is a block diagram of the hydraulic dual-arm trajectory planning and control method considering multiple physical constraints of the present invention.

[0024] Figure 2This is a diagram showing the hydraulic circuit, joints, and coordinate system configuration of the hydraulic double-arm system of the present invention.

[0025] Figure 3 This is a structural block diagram of the trajectory planner of the present invention.

[0026] Figure 4 This is a schematic diagram illustrating the adaptive and control relationship of the object's inertial parameters according to the present invention.

[0027] Figure 5 This is a schematic diagram of the hydraulic dual-arm motion controller of the present invention.

[0028] Figure 6 This is a simulation control effect diagram of the method described in this invention. Detailed Implementation

[0029] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. The specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other. The following detailed description further illustrates these embodiments:

[0030] Now combined Figure 1 , Figure 2 , Figure 3 , Figure 4 , Figure 5 and Figure 6 The present invention will be further described below.

[0031] like Figure 1 The diagram shown is a block diagram of the hydraulic dual-arm trajectory planning and control method considering multiple physical constraints according to the present invention. This method, combined with... Figure 2 The hydraulic dual-arm system, comprising two 3-DOF hydraulic robotic arms, is illustrated in detail below. In the embodiment, as shown... Figure 2 As shown, {O} is the object coordinate system, {W} is the world coordinate system, {L} and {R} are the end coordinate systems of the left and right arms respectively, and {OL} and {OR} are the base coordinate systems of the left and right arms respectively. The joint configuration of the two arms is the same, each containing one swing hydraulic cylinder and two linear hydraulic cylinders.

[0032] In this embodiment, the method includes the following steps.

[0033] 1) The trajectory planner receives the reference motion trajectory of the object, replans the reference trajectory according to the multi-physical constraints of the hydraulic double arms, and generates the desired trajectory of the object that meets the system constraints.

[0034] The operator pre-determines a reference motion trajectory for the object being manipulated by the hydraulic dual-arm system. This reference motion trajectory is defined by reference path parameters. r ( t ) and path geometry p O (·) constitutes, satisfying x r = p O ( r ( t )).

[0035] like Figure 3 As shown, the trajectory planner includes a boundary estimator and a nonlinear filter. The boundary estimator performs online estimation of multiple physical constraints based on the real-time joint angle feedback signal, pressure feedback signal, and end-effector force feedback signal from the bottom-level controller of the hydraulic dual-arm system.

[0036] The multi-physical constraints include pump flow constraints, valve flow constraints, valve flow constraints affected by load changes, end-effector acceleration constraints, and torque constraints of each joint in the dual arms. Specifically, the pump flow constraint reflects that the total flow of the dual-arm system and the flow of each actuator must not exceed the hydraulic pump's supply capacity; the valve flow constraint reflects the maximum allowable flow capacity of the servo valve; the valve flow constraint affected by load changes reflects the impact of pressure difference changes across the valve on the actual flow rate; the end-effector acceleration constraint reflects the dynamic response capability of the end effector; and the torque constraints of each joint in the dual arms reflect the matching relationship between the joint driving torque and the load.

[0037] After establishing the above constraints, through the kinematics of the robotic arm, the Jacobian mapping, and the mapping relationship between joint velocity and hydraulic cylinder velocity, all kinds of constraints can be uniformly converted into velocity boundaries and acceleration boundaries in the path parameter space. Among them, the first derivative boundary of the path parameters is obtained by combining the flow constraints, and the second derivative boundary of the path parameters is obtained by combining the acceleration constraints and the joint torque constraints. The lower boundary of the path parameters can be set to 0 to prevent the object from retracting.

[0038] The nonlinear filter performs time-law reprogramming of the reference path parameters based on the first and second derivative boundaries of the path parameters. Preferably, the nonlinear filter consists of a variable structure controller and two integrator modules. The variable structure controller outputs the second derivative of the reprogrammed path parameters, and the two integrator modules obtain the reprogrammed path parameters respectively. s ( t ) and its first derivative. By combining the replanning path parameters with the predefined path geometry, the desired trajectory of the object that conforms to the multi-physics constraints of the hydraulic dual-arm system can be obtained. x d = pO ( s ( t )).

[0039] 2) The hydraulic dual-arm motion controller receives the desired trajectory signal of the object and generates control signals for each hydraulic control valve, thereby performing closed-loop control of the hydraulic dual-arm motion.

[0040] like Figure 4 and Figure 5 As shown, a closed-chain coordination constraint relationship for the hydraulic dual-arm collaborative operation of an object is first established. Based on the positional and orientational relationships between the object's coordinate system and the coordinate systems of the left and right arm end-effectors, the coordinated pose relationship of the arm end-effectors can be obtained. Then, based on the mapping relationship between the object's velocity and the velocities of the arm end-effectors, as well as the coordination relationship between the joint velocities of the arm end-effectors, the required pose and velocity of the arm end-effectors can be derived from the desired trajectory of the object. Combining the joint angle signals measured by the angle encoder, the actual position and actual velocity of the object can also be obtained.

[0041] Subsequently, the equations of motion for the object are established in the object coordinate system {O} and expressed as linear parameterized forms in terms of the inertial parameter vector. F O = Y O θ O .

[0042] Based on the actual and desired motion states of an object, an adaptive function for the object's inertial parameters is constructed to update the object's inertial parameter vector online, thereby obtaining estimated values ​​of the inertial parameters. θ aO By continuously iterating and updating the estimated values ​​of inertial parameters, the net external force required to control the motion of the object can be further obtained.

[0043] After obtaining the desired external force, a motion controller is established to output the required velocity and required external force at the end of the dual arms. This controller is used not only to track the desired motion of the object but also to suppress the internal forces generated during the closed-chain cooperation of the hydraulic dual arms. The actual internal force is estimated from the pressure signals of each pressure sensor, and an internal force error is formed between the desired internal force and the actual internal force. e f = F id - F i .

[0044] Based on the internal force error and the preset internal force error gain matrix, the required external force at the end of the dual arms can be generated to achieve closed-loop control of the internal force.

[0045] The estimated inertial parameters of the object θ aOThe external force required at the ends of the two arms can be further fed back to the boundary estimator in the trajectory planner, used to update the second derivative boundaries of the path parameters determined by the joint torque constraints online. Thus, when the load or system state changes, the trajectory planning side can update the constraint boundaries in real time, and the control side can also adjust the required velocity and required external force synchronously, forming a closed-loop collaborative mechanism of "control side estimation - constraint boundary update - planning side planning".

[0046] Finally, controllers for each individual arm of the hydraulic dual-arm system are established. The required velocity and external force at the end of both arms are output by the motion controller and then applied to the individual arm controllers. The individual arm controllers output control signals to the servo valves of the hydraulic dual arms to achieve position and force control at the end of the arms.

[0047] In this embodiment, a pressure sensor is installed in the hydraulic system, and an angle encoder is installed at the joints of the multi-joint robotic arm. The pressure signal and the joint angle signal are simultaneously fed back to the trajectory planner and the hydraulic dual-arm motion controller, so that both trajectory planning and motion control are in a closed-loop state.

[0048] To verify the effectiveness of the method disclosed in this invention, a MATLAB / Simulink simulation of the above-described control method was performed on a hydraulic dual-arm system, simulating the following: Figure 5 The diagram shows a circular reference trajectory with a radius of 0.25m in the XOY plane for a hydraulically operated dual-arm object. The object begins its counter-clockwise movement along the circumference from (0.55m, 2.1m). The control method is compared with that of a controller without trajectory planning to verify its effectiveness. Figure 5 As can be seen, compared with the control method without trajectory planning, this control method greatly reduces the motion error of the object along the desired path, especially in the latter half of the motion, the maximum position tracking error is reduced from 361mm to 1.2mm.

[0049] The above descriptions are merely embodiments of the present invention, and common knowledge regarding specific structures and characteristics in the solutions is not described in detail here. It should be noted that those skilled in the art can make various modifications and improvements without departing from the structure of the present invention, and these should also be considered within the scope of protection of the present invention. These modifications and improvements will not affect the effectiveness of the implementation of the present invention or its practicality.

Claims

1. A method for trajectory planning and control of a hydraulic dual-arm considering multiple physical constraints, characterized in that, Includes the following steps: 1) Establish a hydraulic dual-arm trajectory planner to receive the input object reference motion trajectory; the object reference motion trajectory is composed of the reference path parameter r(t) and the predefined path geometry p. O Composed of (·) and satisfying x r = p O (r(t)); The trajectory planner includes a boundary estimator and a nonlinear filter; wherein, the boundary estimator generates the first-order derivative boundary and the second-order derivative boundary of the path parameters based on the multi-physical constraint of the hydraulic double arm and the real-time joint angle feedback signal, pressure feedback signal and end-force feedback signal of the hydraulic double arm system and the end-force demand feedback signal of the double arm bottom controller, and inputs them to the nonlinear filter; the multi-physical constraint includes at least pump flow constraint, valve flow constraint, valve flow constraint affected by load change, end-force acceleration constraint and joint torque constraint; the nonlinear filter performs time law replanning on the reference path parameters according to the first-order derivative boundary and the second-order derivative boundary to obtain the replanned path parameters, and generates the desired trajectory of the object that conforms to the multi-physical constraint by combining the path geometry; 2) Establish a hydraulic double arm motion controller to receive the object period Based on the trajectory signal and the closed-chain coordination constraint relationship of the hydraulic dual-arm cooperative operation of the same object, the required position and velocity of the dual-arm end are obtained. On this basis, the hydraulic dual-arm motion controller adaptively updates the object's inertial parameters and generates the resultant external force required for the object's motion based on the required velocity and the actual velocity. Further, the hydraulic dual-arm motion controller forms an internal force error based on the expected internal force and the actual internal force estimated by the pressure feedback signal, and generates the required external force at the end of the dual arms through the internal force error gain matrix. The single-arm controller outputs control signals of each hydraulic control valve based on the required velocity and the required external force at the end of the dual arms, thereby performing closed-loop control of the hydraulic dual-arm motion. Among them, the object inertial parameter vector and / or the required external force at the end of the dual arms are used to estimate the second derivative boundary of the path parameters determined by the joint torque constraint and fed back to the boundary estimator.

2. The method according to claim 1, characterized in that: The hydraulic dual arms consist of a multi-joint linkage robotic arm connected to a hydraulic system. Angle encoders are installed at the joints of the multi-joint linkage robotic arm, and pressure sensors are installed in the hydraulic system. The pressure sensors and angle encoders are used to monitor the joint angles and pressure signals in real time and feed them back to the trajectory planner and the hydraulic dual arm motion controller to realize closed-loop control of hydraulic dual arm trajectory planning and motion control.

3. The method according to claim 1, characterized in that: The trajectory planner consists of a boundary estimator, a nonlinear filter, and the path geometry of the object's reference motion trajectory. The trajectory planner aims to maintain the path geometry p. O (·) remains unchanged as a constraint, and time-law reprogramming is performed only on the reference path parameter r(t) so that the output object's expected trajectory remains consistent with the path geometry while satisfying the multi-physics constraints.

4. The method according to claim 1, characterized in that: In step 1), the boundary estimator generates velocity and acceleration boundaries for path parameters based on the multi-physics constraints and real-time joint angle feedback signals, pressure feedback signals, and end-effector demand force feedback signals, and inputs the velocity and acceleration boundaries into the nonlinear filter to obtain the replanning path parameters; wherein, the nonlinear filter includes a variable structure controller and an integral module.

5. The method according to claim 1, characterized in that: The hydraulic dual-arm motion controller includes a closed-chain coordination constraint relationship for the hydraulic dual-arm operated object, an adaptive object inertial parameter, an object motion controller, and a single-arm controller; the output of the adaptive object inertial parameter is used for online estimation of the multi-physics constraint boundary.

6. The method according to claim 1, characterized in that: Step 2) includes: S1: Obtaining the actual end-effector pose from the positive kinematics of the two arms, and obtaining the required end-effector pose and the required velocity of the object from the expected trajectory of the object through the closed-chain coordination constraint relationship; S2: Designing an adaptive function for the object's inertial parameters to estimate the object's inertial parameter vector and obtain the resultant external force required for the object's motion; S3: Outputting the required velocity and required external force of the two arms' end-effectors from the object motion controller, and outputting a servo valve control signal through a single-arm controller; wherein, the actual internal force is estimated from the pressure signal of the pressure sensor, and internal force control is performed based on the internal force error formed by the expected internal force and the actual internal force.

Images