Hydraulic manipulator handle master-slave control method based on jerk-constrained trajectory planning
By combining jerk constraint trajectory planning and real-time inverse kinematics solving with PID closed-loop control, the problems of difficult mapping and insufficient smoothness of input commands in the master-slave control of hydraulic robotic arm handles are solved, achieving high-precision and stable trajectory tracking.
Patent Information
- Application Number
- CN202610647102.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-05-12
- Publication Date
- 2026-07-10
Smart Images
Figure CN122353590A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to a master-slave control method for a robotic arm handle, and specifically to a master-slave control method for a hydraulic robotic arm handle based on jerk constraint trajectory planning. Background Technology
[0002] Hydraulic robotic arms, due to their high power density, strong load capacity, and good environmental adaptability, are widely used in complex operational scenarios such as disaster relief, deep-sea exploration, and heavy industrial manufacturing. As operational modes evolve from traditional preset trajectory execution to human-machine collaboration, compliant control, and dexterous operation, the master-slave control capability based on handle input is gradually becoming a crucial indicator for evaluating the control performance of hydraulic robotic arms. In the master-slave control process of a hydraulic robotic arm handle, the operator provides the desired motion command to the end effector in the workspace via the master handle, while the hydraulic robotic arm, as the slave actuator, completes the actual control execution in the joint space. Therefore, it is essential to establish a high-precision, high-real-time mapping relationship between the desired trajectory in the workspace and the control quantities in the joint space.
[0003] However, existing technologies typically have the following shortcomings in the above process: On the one hand, multi-joint hydraulic manipulators have complex kinematic structures, and there is usually a significant nonlinear coupling relationship between their workspace position and joint space angles. Although traditional analytical inverse kinematics methods have high computational efficiency under certain configurations, they often suffer from difficulties in modeling and derivation, limited applicability, and complex handling of multiple solutions for complex hydraulic manipulator configurations. On the other hand, conventional numerical inverse kinematics methods, if lacking effective iterative strategies and stability handling mechanisms, are prone to problems such as slow convergence speed, numerical instability near singular configurations, or insufficient real-time performance, thereby affecting the continuity and accuracy of the master-slave control of the handle.
[0004] On the other hand, the desired signal output by the human-machine interface controller typically exhibits discreteness, abrupt changes, and a certain degree of jitter. If the joint angle command obtained from inverse kinematics is directly input into the underlying closed-loop controller, it can easily trigger sudden pressure changes in the hydraulic system, actuator impacts, and end-effector vibrations, thereby reducing trajectory tracking accuracy and system operational stability. While traditional PID control has the advantages of simple structure and convenient engineering implementation, without a front-end smoothing planning mechanism, relying solely on feedback adjustment is usually insufficient to effectively suppress tracking errors caused by both input discontinuities and system nonlinearities.
[0005] Therefore, how to achieve high-precision inverse kinematic mapping from the workspace to the joint space while ensuring real-time computation, and how to smoothly constrain the input commands through trajectory planning methods applicable to the dynamic characteristics of hydraulic mechanical systems, so as to improve the accuracy, stability and controllability of master-slave control of hydraulic robotic arm handles, has become a technical problem that urgently needs to be solved in this field. Summary of the Invention
[0006] To address the problems existing in the background technology, this invention provides a master-slave control method for a hydraulic robotic arm handle based on jerk-constrained trajectory planning. This method addresses the difficulties in solving the nonlinear mapping from the workspace to the joint space and the insufficient smoothness of input commands in human-machine interaction operations of hydraulic robotic arms. It establishes a mapping relationship from the desired trajectory in the workspace to the angles of each joint through real-time inverse kinematics solving, and smooths the commands through jerk-constrained trajectory planning. Combined with proportional-integral-derivative PID closed-loop control, it achieves real-time, accurate, and stable tracking of the master-end handle input action on the slave-end hydraulic robotic arm.
[0007] The technical solution adopted in this invention is: The master-slave control method for a hydraulic robotic arm handle based on jerk constraint trajectory planning of the present invention includes: Step S1: When the slave hydraulic manipulator is operated and controlled by the master handle, the operator input command of the master handle is collected and preprocessed to construct the expected trajectory model of the end effector of the slave hydraulic manipulator in the workspace.
[0008] Step S2: Establish the Jacobian matrix of the slave hydraulic manipulator, and then use the Newton-Raphson iteration method to iteratively solve the desired trajectory model to obtain the desired joint angle of the slave hydraulic manipulator in joint space.
[0009] Step S3: Replan the trajectory with jerk constraints on the desired joint angle to generate continuous desired joint trajectory commands.
[0010] Step S4: Based on the joint desired trajectory command, perform closed-loop regulation and control on the hydraulic servo valves of each joint hydraulic cylinder of the slave hydraulic robot arm to achieve precise tracking control of the slave hydraulic robot arm to the input command of the master handle.
[0011] In step S1, when preprocessing the input signal of the master handle, the input signal is first processed by the dead zone processing function to obtain the control signal vector, so as to reduce the influence of mechanical centering error and small jitter on trajectory generation. Then, after processing by the discrete integration method, the desired trajectory model of the end effector of the slave hydraulic robot in the workspace is obtained.
[0012] In step S2, the Jacobian column vectors of each joint of the slave hydraulic manipulator are obtained based on the Jacobian matrix of the slave hydraulic manipulator as follows: in, For the first end of the hydraulic robotic arm i Jacobian column vectors for each joint; and These are the first and second Jacobian components, respectively; For the first end of the hydraulic robotic arm i -1 unit vector in the direction of the rotation axis of the joint; Let be the position vector of the end effector of the hydraulic robotic arm. For the first end of the hydraulic robotic arm i -1 joint position.
[0013] Then, Newton's iteration method is used to perform real-time inverse kinematics iteration to solve the desired trajectory model. When only tracking the position of the end effector of the slave hydraulic manipulator is performed, an optimization mode is adopted, combining damped least squares method and the first Jacobian component. Optimization is performed; when simultaneously tracking the position and attitude of the end effector of the slave hydraulic manipulator, a fully constrained mode is adopted, combining the Jacobian column vectors of each joint of the slave hydraulic manipulator for constraint; both the optimized mode and the fully constrained mode obtain the joint angle update law after each iteration. To update the joint angles of the slave hydraulic robotic arm, and The first k and the k The angles of each joint of the slave-end hydraulic manipulator after +1 iteration.
[0014] Simultaneously apply the following physical constraints to the joint: in, and These represent the upper and lower limits of the joint angle, respectively.
[0015] During the iterative solution process, when Alternatively, by reaching a preset maximum number of iterations, the final updated desired joint angle of the slave-end hydraulic manipulator in joint space can be obtained. , ,in, Position error norm, Preset position error threshold.
[0016] In the optimization mode described, based on the first Jacobian component The generalized inverse is obtained using the damped least squares method. Then based on the generalized inverse Obtain the joint angle update law in the optimized mode as follows: in, and These are task space correction items and zero space optimization items, respectively. The identity matrix for dimension matching; Zero-space gain; The gradient of the cost function, This is the cost function used to make the joint angle tend towards the joint center region. For joint angle; This refers to the angle at the center of the joint, i.e., the midpoint of the hydraulic cylinder.
[0017] The joint angle update law of the fully constrained mode as follows: in, and These are the augmented Jacobian matrix and the combined error vector, respectively; It is the damping factor; The identity matrix for dimension matching; This refers to attitude error; The desired orientation of the end effector of the end-effector of the hydraulic robotic arm; n This represents the total number of joints in the end-effector hydraulic robotic arm. For the first k The 1st iteration obtained i The joint angles of each joint.
[0018] In step S3, to avoid direct input of the discrete joint angles obtained from inverse kinematics during the master-slave control process into the underlying control system, which could lead to sudden pressure changes and mechanical shocks in the hydraulic system, a real-time S-shaped trajectory generator based on jerk constraints and braking hysteresis compensation is further designed to perform real-time smoothing of the desired joint angles, as detailed below: Step S31: First, determine the state of the joint position and handle the dead zone during the joint movement of the slave hydraulic robot arm. When it is determined that the slave hydraulic robot arm has not reached the expected joint angle... If so, proceed to step S32; when it is determined that the hydraulic robotic arm at the slave end has reached the desired joint angle... If the current cycle is interrupted, the drive of the slave hydraulic robotic arm will stop and the current control cycle will be exited.
[0019] Step S32: The joint velocity, joint acceleration, and joint jerk of the slave hydraulic manipulator during the joint movement process are solved by constraint based on safety margin compensation to obtain smooth joint command jerk.
[0020] Step S33: Based on the joint command jerk and combined with the second-order integral update, obtain the joint position, joint acceleration and joint jerk of the slave hydraulic robot at the next moment, which together serve as the smooth and continuous joint desired trajectory command.
[0021] In step S31, the actual joint state of the slave-end hydraulic manipulator in the current control cycle is obtained, including the actual joint position. Joint velocity and joint acceleration To obtain the position error of the current control cycle , ,when and At that time, it is determined that the hydraulic robotic arm at the end has reached the desired joint angle. , and These are the preset position dead zone threshold and the preset speed dead zone, both of which are minimum values.
[0022] In step S32, when constraining the joint velocity of the end hydraulic manipulator, an acceleration safety margin coefficient is introduced. To obtain safe acceleration , , Limit the maximum acceleration of the joint, and then obtain the ideal tracking velocity. , , For positional error, For the sign function, with respect to the ideal tracking speed Amplitude limiting is applied to meet speed constraints. , Limit the maximum speed of the joint.
[0023] When solving for constraints on the joint acceleration of the end hydraulic robotic arm, an acceleration safety margin coefficient is introduced. To obtain safe accelerometer , , Limit the maximum jerk of the joint, and then obtain the ideal acceleration. , , , For velocity error, relative to ideal acceleration Amplitude limiting is applied to satisfy acceleration constraints. .
[0024] When solving for constraints on the joint accelerometer of the end hydraulic manipulator, the commanded accelerometer is obtained. , , , For acceleration error, The sampling period is used to accelerate the command. Amplitude limiting is applied to satisfy the jerk constraint conditions. This ensures that the generated trajectory meets the system's dynamic capability requirements.
[0025] In step S33, the acceleration is first based on the command. Update the joint acceleration for the next moment. And clamp as follows: in, For the constraint function, when Then output ,when Then output ,when Then output .
[0026] Then, the joint position at the next time step is obtained by updating using second-order integral. as follows: Then update the joint velocity for the next moment. And clamp as follows: Finally, the desired joint trajectory command is obtained. , .
[0027] In step S4, the joint desired trajectory command is input into the PID controller for processing and output control voltage, that is, the deviation between the output and the feedback value of the joint sensor, to generate servo valve control voltage, which in turn directly controls the hydraulic servo valves of each joint hydraulic cylinder of the slave hydraulic robot arm.
[0028] This invention aims to achieve high-precision mapping from the desired workspace trajectory given by the master handle to the desired angle of the hydraulic manipulator joint space by constructing a real-time inverse kinematics solution mechanism; at the same time, by introducing a trajectory planning method with jerk constraints, discrete and abrupt handle input commands are smoothed; and combined with PID closed-loop control, the slave hydraulic manipulator can achieve real-time, accurate and stable tracking of the input actions of the master handle.
[0029] The beneficial effects of this invention are: 1. This invention adopts an inverse kinematics solution method based on Newton iteration, and combines it with a forward kinematics model and Jacobian matrix for real-time calculation, thereby improving the solution accuracy and real-time performance of complex hydraulic manipulators in the process of mapping from workspace to joint space, and overcoming the problems of limited applicability of analytical methods and insufficient stability of conventional numerical methods.
[0030] 2. This invention introduces jerk constraints in the trajectory planning stage to smoothly adjust the changes in position, velocity and acceleration during joint movement, thereby effectively reducing the impact of sudden changes and jitter in the input of the master handle on the hydraulic system, improving the continuity and stability of the robotic arm movement process, and using second-order integral for state update, which helps to reduce braking lag, suppress position overshoot, and improve the accuracy and feasibility of trajectory generation under discrete control conditions.
[0031] 3. This invention combines real-time inverse kinematics solving, jerk constraint trajectory planning, and PID closed-loop control to construct a control link suitable for master-slave control of hydraulic robotic arm handles. This enables the desired action given by the master handle in the workspace to be realized in the joint space of the slave hydraulic robotic arm in a smoother and more accurate manner, thereby reducing the trajectory tracking error of the end effector and improving the quality of master-slave control. Attached Figure Description
[0032] Figure 1 This is a flowchart of the method of the present invention; Figure 2 This is a schematic diagram of a four-degree-of-freedom hydraulic robotic arm controlled in a specific implementation of the present invention; Figure 3 The tracking curves and output voltage curves of joints 1 to 4 under the three sinusoidal trajectory are shown. Figure 4 The tracking curves and output voltage curves of joints 1 to 4 under the planar circular trajectory are shown. Figure 5 The tracking curves and output voltage curves of joints 1 to 4 under the plane Lissajous figure trajectory are shown. Figure 6 Tracking error curves and output voltage curves for joints 1 to 4, which are controlled in real time by the handle; Figure 7 The motion trajectory diagram of the end effector of the hydraulic robotic arm under a three-sine trajectory; Figure 8 This is a motion trajectory diagram of the end effector of a hydraulic robotic arm under a planar circular trajectory. Figure 9 The motion trajectory diagram of the end effector of the hydraulic robotic arm under a planar Lissajous figure trajectory; Figure 10 A motion trajectory diagram of the end effector of a hydraulic robotic arm that is controlled in real time by a handle. Detailed Implementation
[0033] To make the objectives, technical solutions, and beneficial effects of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the following specific embodiments are only for explaining the invention and are not intended to limit the scope of protection of the invention. Where there is no conflict, the technical features of the various embodiments of this invention can be combined with each other.
[0034] like Figure 1 As shown, the master-slave control method for a hydraulic robotic arm handle based on jerk constraint trajectory planning of the present invention includes: Step S1: When the slave-end hydraulic manipulator is operated and controlled via the master-end handle, the operator's input commands on the master-end handle are collected and preprocessed to construct the desired trajectory model of the slave-end hydraulic manipulator's end effector in the workspace. Specifically, when preprocessing the input signal of the master-end handle, the input signal is first processed through a dead-time processing function to obtain a control signal vector, thereby reducing the influence of mechanical back-centering error and small-amplitude jitter on trajectory generation. Then, after processing using the discrete integration method, the desired trajectory model of the slave-end hydraulic manipulator's end effector in the workspace is obtained.
[0035] Considering that the master handle typically suffers from mechanical centering errors and signal jitter in practical industrial applications, the input signal of the master handle is first preprocessed. The original normalized analog signal vector output by the master handle is... ,in, and These are the X and Y axis signals for the left joystick on the main controller, respectively. and They are respectively t The left and right trigger buttons on the main controller are constantly in motion.
[0036] Dead zone handling function as follows: in, This serves as the input for the dead-time handling function; The preset dead zone threshold is 0.15 in this embodiment.
[0037] Control signal vector after dead zone processing as follows: in, , and These represent the X, Y, and Z axis components of the control signal vector, respectively.
[0038] Based on the processed control signal vector, the desired position increment of the robotic arm's end effector in the Cartesian coordinate system of the workspace is generated through integration, thereby obtaining the desired trajectory at the next moment. as follows: in, This is the speed proportional gain coefficient. Sampling time.
[0039] When the master-slave system starts, it first performs position calibration based on the preset initial position of the slave hydraulic manipulator end effector, and moves the manipulator to the corresponding initial position in the workspace to ensure the consistency of initial values in the subsequent master-slave control process of the handle.
[0040] Step S2: Establish the Jacobian matrix of the slave hydraulic manipulator, and then use the Newton-Raphson iteration method to iteratively solve the desired trajectory model to obtain the desired joint angle of the slave hydraulic manipulator in joint space.
[0041] Specifically, the Jacobian column vectors of each joint of the slave-end hydraulic manipulator are obtained based on the Jacobian matrix of the slave-end hydraulic manipulator as follows: in, For the first end of the hydraulic robotic arm i Jacobian column vectors for each joint; and These are the first and second Jacobian components, respectively; For the first end of the hydraulic robotic arm i -1 unit vector in the direction of the rotation axis of the joint; Let be the position vector of the end effector of the hydraulic robotic arm. For the first end of the hydraulic robotic arm i -1 joint position.
[0042] The inverse kinematics solution is based on the forward kinematics model and the Jacobian matrix, providing a model foundation for the inverse kinematics solution. This invention establishes a coordinate system for each joint link of a four-degree-of-freedom hydraulic manipulator. Let the length of the device link be... These are the lengths of the base fixing part and the 1st to 4th links, respectively, and the joint offset is... These represent the offsets from the base to the first joint axis and from the first joint axis to the second joint axis, respectively. The coordinate transformation relationships of each joint are established based on the DH parameter method, and the transformation matrices of each joint are used... The product of these steps yields the end effector pose matrix. Thus, the forward kinematic model is constructed as follows: in, This is a vector of joint angles, including four joint angles; This is the homogeneous transformation matrix from the fourth joint coordinate system to the actual end effector center point.
[0043] End effector position vector From the pose matrix Extract the elements from the first three rows and fourth column, as follows: in, , and These are the x-axis, y-axis, and z-axis coordinate components of the current position vector of the end effector in the base coordinate system.
[0044] Further, the differential mapping relationship between the end effector velocity and the joint angular velocity of the robotic arm is derived, namely the Jacobian matrix. : in, This represents the linear velocity vector of the end effector in the workspace. ; This represents the joint angular velocity.
[0045] In this embodiment, the focus is on the position Jacobian matrix and the Jacobian terms that reflect the relationship of the end effector's attitude angle changes, so as to be used for inverse kinematics solutions under different task constraint modes.
[0046] To address the nonlinear mapping between the desired trajectory in the workspace and the desired joint angles in the joint space, Newton's iteration method is used to perform real-time inverse kinematics iteration on the desired trajectory model. When only the end effector position of the slave hydraulic manipulator is tracked, an optimization mode is adopted, utilizing the remaining degrees of freedom of the slave hydraulic manipulator under the task constraints to optimize the joint state, combining damped least squares method and the first Jacobian component. To optimize the solution, damped least squares method is used to avoid inversion instability of the Jacobian matrix near singular configurations, thus improving the stability of the numerical solution in such areas. When simultaneously tracking the position and attitude of the end effector of the slave hydraulic manipulator, a fully constrained mode is adopted, combining constraints with the Jacobian column vectors of each joint of the slave hydraulic manipulator. Both the optimized mode and the fully constrained mode obtain the joint angle update law after each iteration. To update the joint angles of the slave hydraulic robotic arm, and The first k and the k The angles of each joint of the slave-end hydraulic manipulator after +1 iteration.
[0047] Simultaneously apply the following physical constraints to the joint: in, and These represent the upper and lower limits of the joint angle, respectively.
[0048] During the iterative solution process, when Alternatively, by reaching a preset maximum number of iterations, the final updated desired joint angle of the slave-end hydraulic manipulator in joint space can be obtained. , ,in, Position error norm, Preset position error threshold.
[0049] By using the inverse kinematics solution process based on Newton's iteration, real-time numerical mapping of the desired trajectory in the workspace can be achieved, and the solution efficiency and robustness in the master-slave control process of complex hydraulic robotic arm handles can be improved.
[0050] In the optimization mode, based on the first Jacobian component The generalized inverse is obtained using the damped least squares method. Broadly defined reverse , T For transpose, The damping factor is set to 0.01 in this embodiment; then, based on the generalized inverse... Obtain the joint angle update law in the optimized mode as follows: in, and These are task space correction items and zero space optimization items, respectively. The identity matrix for dimension matching; Zero-space gain; The gradient of the cost function, This is the cost function used to make the joint angle tend towards the joint center region. For joint angle; This refers to the angle at the center of the joint, i.e., the midpoint of the hydraulic cylinder.
[0051] By using the above methods, while meeting the position tracking requirements of the end effector, the joint angle distribution can be improved and the risk of the joint approaching its limit position can be reduced.
[0052] Joint angle update law in fully constrained mode as follows: in, and These are the augmented Jacobian matrix and the combined error vector, respectively; It is the damping factor; The identity matrix for dimension matching; This refers to attitude error; The desired orientation of the end effector of the end-effector of the hydraulic robotic arm; n This refers to the total number of joints in the end-effector hydraulic robotic arm, specifically 4. For the first k The 1st iteration obtained i The joint angles of each joint.
[0053] The joint angle update step size in the fully constrained mode is obtained by solving a system of linear equations.
[0054] Step S3: Replan the trajectory with jerk constraints on the desired joint angle to generate continuous desired joint trajectory commands.
[0055] To avoid direct input of discrete joint angles obtained from inverse kinematics solutions into the underlying control system during the master-slave control process, which could lead to sudden pressure changes and mechanical shocks in the hydraulic system, a real-time S-shaped trajectory generator based on jerk constraints and braking hysteresis compensation was further designed to smooth the desired joint angles in real time, as detailed below: First, the joint positions of the slave-end hydraulic robotic arm during joint movement are determined and dead zones are handled. If it is determined that the slave-end hydraulic robotic arm has not reached the desired joint angle... If the desired joint angle has been reached, the process continues; Then, stop driving the slave hydraulic robotic arm and directly output a stationary command vector. It then exits the current control cycle, specifically acquiring the actual joint states of the slave-end hydraulic manipulator in the current control cycle, including the actual joint positions. Joint velocity and joint acceleration To obtain the position error of the current control cycle , To prevent the robotic arm from experiencing frequent micro-oscillations near the target point due to sensor noise or minute errors, a static strong dead zone threshold is set. and At that time, it is determined that the hydraulic robotic arm at the end has reached the desired joint angle. , and These are the preset position dead zone threshold and the preset speed dead zone, both of which are minimum values.
[0056] Then, the joint velocity, joint acceleration, and joint jerk during the joint movement process of the slave hydraulic manipulator are solved using constraints based on safety margin compensation to obtain smooth joint command jerk.
[0057] Specifically, when solving for constraints on the joint velocities of the end hydraulic manipulator, an acceleration safety margin coefficient is introduced. To obtain safe acceleration , , Limit the maximum acceleration of the joint, and then obtain the ideal tracking velocity. , , For positional error, For a sign function, when The function value is 1 when it is greater than 0, -1 when it is less than 0, and 0 when it is equal to 0, for the ideal tracking speed. Amplitude limiting is applied to meet speed constraints. , Limit the maximum speed of the joint.
[0058] When solving for constraints on the joint acceleration of the end hydraulic robotic arm, an acceleration safety margin coefficient is introduced. To obtain safe accelerometer , , Limit the maximum jerk of the joint, and then obtain the ideal acceleration. , , , For velocity error, relative to ideal acceleration Amplitude limiting is applied to satisfy acceleration constraints. .
[0059] When solving for constraints on the joint accelerometer of the end hydraulic manipulator, the commanded accelerometer is obtained. , , , For acceleration error, The sampling period is used to accelerate the command. Amplitude limiting is applied to satisfy the jerk constraint conditions. This ensures that the generated trajectory meets the system's dynamic capability requirements.
[0060] To prevent braking lag and position overshoot caused by limited acceleration capabilities in traditional trajectory planning, a safety margin coefficient is introduced into the planning calculation of velocity and acceleration to reserve some control capability, thereby enhancing the feasibility and smoothness of the trajectory generation process.
[0061] Then, based on the joint command jerk and combined with the second-order integral update, the joint position, joint acceleration and joint jerk of the slave hydraulic robot at the next moment are obtained, which together serve as the joint desired trajectory command for a smooth and continuous joint.
[0062] Specifically, to improve the trajectory update accuracy under discrete control conditions, this invention employs a state update method based on second-order integrals to calculate the joint state at the next moment, instead of using the simpler Euler integral method which has lower accuracy. Firstly, based on commanded jerk... Update the joint acceleration for the next moment. And clamp as follows: in, A constraint function, its purpose is to forcibly restrict the value of a variable to a specified upper and lower limit range. Then output ,when Then output ,when Then output .
[0063] Then, the joint position at the next time step is obtained by updating using second-order integral. as follows: Then update the joint velocity for the next moment. And clamp as follows: Finally, the desired joint trajectory command is obtained. , .
[0064] Trajectory planning generates a smooth trajectory based on jerk constraints, and the velocity, acceleration, and jerk are limited to reduce the impact on the hydraulic system and improve the smoothness of the robotic arm's movement. A safety margin coefficient is set during trajectory planning to compensate for braking lag and position overshoot caused by the limited dynamic capabilities of the system. The joint state at the next moment is calculated by updating the state based on second-order integral to improve the state update accuracy in the discrete control process.
[0065] The trajectory planning method described above can effectively constrain the velocity, acceleration, and jerk during joint movement while meeting the position tracking requirements, thereby improving the continuity, smoothness, and controllability of the hydraulic robotic arm's motion.
[0066] Step S4: Based on the joint desired trajectory command, perform closed-loop regulation and control on the hydraulic servo valves of each joint hydraulic cylinder of the slave hydraulic robot arm. Specifically, the joint desired trajectory command is input into the PID controller for processing and output control voltage, that is, the deviation between the output and the feedback value of the joint sensor, to generate the servo valve control voltage, and then directly control the hydraulic servo valves of each joint hydraulic cylinder of the slave hydraulic robot arm, so as to realize the accurate tracking control of the slave hydraulic robot arm to the input command of the master handle.
[0067] This invention ultimately achieves master-slave tracking of the handle through PID closed-loop control. The desired joint trajectory, output from trajectory planning, is used as the command input to construct a PID closed-loop controller, enabling precise control of each joint of the hydraulic robotic arm. The controller outputs the servo valve control voltage. as follows: in, These are the proportional, integral, and differential gain matrices, respectively. for t The target angle value after being processed by the jerk constraint trajectory planning module at any given time. for t The feedback value from the joint angle sensor at all times. For a moment.
[0068] Finally, control voltage The hydraulic servo valve core is displaced, thereby adjusting the hydraulic cylinder flow rate to achieve corresponding joint movements, and enabling the slave hydraulic robotic arm to accurately track the action commands input by the operator via the master handle.
[0069] This invention first preprocesses the input signal from the master handle by performing dead-zone and integral operations to construct the desired trajectory of the hydraulic robotic arm's end effector in the workspace. Then, considering the nonlinear coupling relationship of the system, it uses Newton's iterative method based on the forward kinematics model and the Jacobian matrix to achieve real-time, high-precision solution of the inverse kinematics. Next, it introduces jerk constraints and safety margin compensation to smooth and limit the discrete joint desired angles and perform second-order integral state updates. Finally, based on the smooth and continuous joint desired trajectory, a PID closed-loop controller drives the hydraulic actuator to achieve real-time adjustment. This method effectively overcomes the physical impact of sudden changes and jitter in the handle input during human-machine interaction on the hydraulic system, and while ensuring high real-time mapping from the workspace to the joint space, it reduces the end-effector trajectory tracking error, significantly enhancing the stability and accuracy of master-slave control.
[0070] To verify the effectiveness of the method of this invention, a real-world test was conducted on a multi-joint hydraulic manipulator using a closed-loop control algorithm incorporating a trajectory planner. The hydraulic manipulator operated under different given desired trajectories, including a three-sine trajectory, a planar circular trajectory, and a planar Lissajous figure trajectory. Under the above test conditions, as... Figure 3 , Figure 4 and Figure 5 As shown, the control error of each joint is basically stable within 2°, and some joints can be stable within 1° under certain working conditions, indicating that the method of the present invention can effectively improve the joint tracking accuracy under the action of trajectory planning.
[0071] Furthermore, in the real-time control experiment using a master-slave handle, the operator uses the master handle to control the end effector of the slave hydraulic robotic arm in real time. For example... Figure 6 As shown, the errors of each joint can be kept within 1°, indicating that the method of the present invention can achieve good master-slave tracking effect in actual master-slave control scenarios and effectively improve the motion stability of the robotic arm in complex working environments.
[0072] like Figure 7 , Figure 8 and Figure 9 The figures show the motion trajector of the end effector in the workspace under the desired trajectories of a triple sine wave, a planar circle, and a Lissajous figure, respectively. The actual trajectories are basically consistent with the desired trajectories, indicating that the method has good tracking performance.
[0073] like Figure 10 As shown in the figure, the motion trajectory of the end effector in the workspace is shown under the remote control command input by the operator. The motion process is smooth and stable, and it can achieve master-slave following well.
[0074] In summary, by combining real-time inverse kinematics solution with jerk constraint trajectory planning, this invention can improve the accuracy and stability of master-slave control of hydraulic robotic arm handles while ensuring real-time performance, and has good engineering application value.
[0075] The above description is merely a preferred embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.
Claims
1. A master-slave control method for a hydraulic robotic arm handle based on jerk-constrained trajectory planning, characterized in that, include: Step S1: When the slave hydraulic manipulator is operated and controlled by the master handle, the input command of the master handle is collected and preprocessed to construct the expected trajectory model of the end effector of the slave hydraulic manipulator in the workspace. Step S2: Establish the Jacobian matrix of the slave hydraulic manipulator, and then use the Newton-Raphson iteration method to iteratively solve the desired trajectory model to obtain the desired joint angle of the slave hydraulic manipulator in joint space. Step S3: Replan the trajectory with acceleration constraints on the desired joint angle to generate continuous desired joint trajectory commands; Step S4: Based on the joint desired trajectory command, perform closed-loop regulation and control on the hydraulic servo valves of each joint hydraulic cylinder of the slave hydraulic robot arm to achieve precise tracking control of the slave hydraulic robot arm to the input command of the master handle.
2. The master-slave control method for a hydraulic robotic arm handle based on jerk constraint trajectory planning according to claim 1, characterized in that: In step S1, when preprocessing the input signal of the master handle, the input signal is first processed by the dead-time processing function to obtain the control signal vector, and then processed by the discrete integration method to obtain the expected trajectory model of the end effector of the slave hydraulic manipulator in the workspace.
3. The master-slave control method for a hydraulic robotic arm handle based on jerk constraint trajectory planning according to claim 1, characterized in that: In step S2, the Jacobian column vectors of each joint of the slave-end hydraulic manipulator are obtained based on the Jacobian matrix of the slave-end hydraulic manipulator as follows: in, For the first end of the hydraulic robotic arm i Jacobian column vectors for each joint; and These are the first and second Jacobian components, respectively; For the first end of the hydraulic robotic arm i -1 unit vectors in the direction of the rotation axis of the joint; Let be the position vector of the end effector of the slave hydraulic manipulator; For the first end of the hydraulic robotic arm i -1 joint position; Then, Newton's iteration method is used to perform real-time inverse kinematics iteration to solve the desired trajectory model. When only tracking the position of the end effector of the slave hydraulic manipulator is performed, an optimization mode is adopted, combining damped least squares method and the first Jacobian component. Optimization is performed; when simultaneously tracking the position and attitude of the end effector of the slave hydraulic manipulator, a fully constrained mode is adopted, combining the Jacobian column vectors of each joint of the slave hydraulic manipulator for constraint; both the optimized mode and the fully constrained mode obtain the joint angle update law after each iteration. To update the joint angles of the slave hydraulic robotic arm, and The first k and the k The angles of each joint of the slave-end hydraulic manipulator after +1 iteration; Simultaneously apply the following physical constraints to the joint: in, and These are the upper and lower limits of the joint angle, respectively; During the iterative solution process, when Alternatively, by reaching a preset maximum number of iterations, the desired joint angle of the slave-end hydraulic manipulator in joint space can be obtained in the final updated form. , ,in, Position error norm, Preset position error threshold.
4. The master-slave control method for a hydraulic robotic arm handle based on jerk constraint trajectory planning according to claim 3, characterized in that: In the optimization mode described, based on the first Jacobian component The generalized inverse is obtained using the damped least squares method. Then based on the generalized inverse Obtain the joint angle update law in the optimized mode as follows: in, and These are task space correction items and zero space optimization items, respectively. It is the identity matrix; Zero-space gain; The gradient of the cost function, For joint angle; The angle is the center angle of the joint.
5. The master-slave control method for a hydraulic robotic arm handle based on jerk constraint trajectory planning according to claim 3, characterized in that: The joint angle update law of the fully constrained mode as follows: in, and These are the augmented Jacobian matrix and the combined error vector, respectively; It is the damping factor; It is the identity matrix; This refers to attitude error; The desired orientation of the end effector of the end-effector of the hydraulic robotic arm; n This represents the total number of joints in the end-effector hydraulic robotic arm. For the first k The 1st iteration obtained i The joint angles of each joint.
6. The master-slave control method for a hydraulic robotic arm handle based on jerk constraint trajectory planning according to claim 1, characterized in that: The specific steps of S3 are as follows: Step S31: First, determine the state of the joint position and handle the dead zone during the joint movement of the slave hydraulic robot arm. When it is determined that the slave hydraulic robot arm has not reached the expected joint angle... If so, proceed to step S32; when it is determined that the hydraulic robotic arm at the slave end has reached the desired joint angle... If the current control cycle is not reached, the drive of the slave hydraulic robotic arm will stop and the current control cycle will be exited. Step S32: The joint velocity, joint acceleration, and joint jerk of the slave hydraulic manipulator during the joint movement process are solved by constraint based on safety margin compensation to obtain smooth joint command jerk. Step S33: Based on the joint command jerk and combined with the second-order integral update, obtain the joint position, joint acceleration and joint jerk of the slave hydraulic robot at the next moment, which together serve as the smooth and continuous joint desired trajectory command.
7. The master-slave control method for a hydraulic robotic arm handle based on jerk constraint trajectory planning according to claim 1, characterized in that: In step S31, the actual joint state of the slave-end hydraulic manipulator in the current control cycle is obtained, including the actual joint position. Joint velocity and joint acceleration Obtain the position error of the current control cycle. , ,when and At that time, it is determined that the hydraulic robotic arm at the end has reached the desired joint angle. , and These are the preset position dead zone threshold and the preset speed dead zone, respectively.
8. The master-slave control method for a hydraulic robotic arm handle based on jerk constraint trajectory planning according to claim 7, characterized in that: In step S32, when constraining the joint velocity of the end hydraulic manipulator, an acceleration safety margin coefficient is introduced. To obtain safe acceleration , , Limit the maximum acceleration of the joint, and then obtain the ideal tracking velocity. , , For positional error, For the sign function, with respect to the ideal tracking speed Amplitude limiting is applied to meet speed constraints. , Limit the maximum speed of the joint; When solving for constraints on the joint acceleration of the end hydraulic robotic arm, an acceleration safety margin coefficient is introduced. To obtain safe accelerometer , , Limit the maximum jerk of the joint, and then obtain the ideal acceleration. , , , For velocity error, relative to ideal acceleration Amplitude limiting is applied to satisfy acceleration constraints. ; When solving for constraints on the joint accelerometer of the end hydraulic manipulator, the commanded accelerometer is obtained. , , , For acceleration error, The sampling period is used to accelerate the command. Amplitude limiting is applied to satisfy the jerk constraint conditions. .
9. The master-slave control method for a hydraulic robotic arm handle based on jerk constraint trajectory planning according to claim 8, characterized in that: In step S33, the acceleration is first based on the command. Update the joint acceleration for the next moment. as follows: in, For the constraint function, when Then output ,when Then output ,when Then output ; Then, the joint position at the next time step is obtained by updating using second-order integral. as follows: Then update the joint velocity for the next moment. as follows: Finally, the desired joint trajectory command is obtained. , .
10. The master-slave control method for a hydraulic robotic arm handle based on jerk constraint trajectory planning according to claim 1, characterized in that: In step S4, the desired trajectory command for the joint is input into the PID controller for processing and then outputs a control voltage, which directly controls the hydraulic servo valves of each joint hydraulic cylinder of the slave hydraulic robotic arm.