Unhooking robot arc interpolation path planning control system

Abstract

The invention belongs to the technical field of image recognition and industrial vision, and particularly discloses an unhooking robot arc interpolation path planning control system. The system comprises a path parameterization preprocessing module, a high-order continuous spline interpolation module, a real-time track feedforward compensation module and a multi-mode switching execution module. Through generation of a high-order continuous track, feed-forward torque compensation based on an extended state observer and sub-mode fine control, smooth motion in the whole process from high-speed approaching to stable contact is achieved, mechanical resonance is restrained, and the precision and reliability of unhooking operation are ensured.

Description

Technical Field

[0001] This invention belongs to the field of image recognition and industrial vision technology, specifically relating to a circular interpolation path planning and control system for a hook-unhooking robot. Background Technology

[0002] In the field of industrial robot automation, path planning and control are core technologies for achieving precise and efficient robot operations. Robots complete complex tasks such as welding, assembly, and handling by executing preset trajectories. The smoothness of the path and the control precision directly determine the quality of the work and the stability of the equipment operation.

[0003] As an automated equipment that performs specific precision operations, the core task of a hook-removing robot is to control the hook at the end of the robotic arm to approach and remove a target object in a specific posture and trajectory. This process requires the robot's end effector, i.e., the hook, to move smoothly along the planned path and maintain extremely high positional stability at the moment of contact with the target object, so as to avoid operation failure or damage to the workpiece due to vibration or deviation.

[0004] In existing technologies, path planning for unhooking robots often employs segmented straight-line or simple circular interpolation algorithms. However, these methods are prone to causing abrupt acceleration changes, especially at path junctions or velocity change points, when planning circular paths. This discontinuity in acceleration introduces high-frequency excitation into the robotic arm's drive system, exciting the mechanical structure's natural frequencies and causing the robotic arm to resonate.

[0005] At the critical contact moment during the unhooking operation, this resonance directly manifests as continuous oscillation of the hook tip, causing the actual pose to deviate significantly from the theoretical path. This results in out-of-tolerance unhooking actions, failing to meet the stringent requirements for stability and repeatability in precision unhooking operations. Therefore, effectively suppressing mechanical resonance caused by improper path planning and ensuring an extremely smooth unhooking contact process has become a pressing technical challenge restricting the performance improvement of this type of robot. Summary of the Invention

[0006] The present invention aims to provide a circular interpolation path planning and control system for a hook-unhooking robot, in order to solve the technical contradiction in the prior art where discontinuous acceleration caused by the circular path planning algorithm leads to mechanical resonance at the moment of hook-unhooking contact, resulting in instability of the robot's end-effector posture and a decrease in operational accuracy.

[0007] This invention provides a circular interpolation path planning and control system for a hook-unhooking robot. By constructing a closed-loop control architecture from high-order continuous path generation to feedforward resonance suppression, the system fundamentally eliminates discontinuous components in trajectory excitation, ensuring absolute smoothness and stability of the robotic arm's motion throughout the hook-unhooking operation, especially in the critical contact phase.

[0008] The system includes a path parameterization preprocessing module, a high-order continuous spline interpolation module, a real-time trajectory feedforward compensation module, and a multi-modal switching execution module.

[0009] The path parameterization preprocessing module is used to receive the raw path point sequence and job constraints from the upstream task planning layer.

[0010] This module first removes redundant points and identifies key feature points from the original path point sequence, extracting the key path points necessary to describe the hook removal action, including the starting point, the center point of the arc passed through, the ending point, and the contact point.

[0011] Furthermore, the module calculates and binds kinematic constraint parameters for each path segment, especially the circular arc segment. These parameters include the maximum speed allowed for the path segment, the maximum acceleration, and an extremely low stable speed threshold specifically set for the contact point.

[0012] All processed path key points and their bound constraint parameter sets constitute a normalized path description, which is then output to the high-order continuous spline interpolation module.

[0013] The high-order continuous spline interpolation module is used to generate a smooth trajectory that is continuous in terms of position, velocity, acceleration, and even jerk, based on a normalized path description.

[0014] The core of this module lies in its use of a global optimization interpolation algorithm based on seventh-order polynomial splines. Specifically, this module models the motion between key points on adjacent paths as a polynomial function with time as the parameter.

[0015] By constructing a large-scale sparse linear system of equations, the boundary conditions of position, velocity, acceleration, and jerk at key points of the path, as well as the velocity and acceleration constraints within the path segments.

[0016] The solution to this system of equations is the coefficients of a seventh-degree polynomial that satisfy all boundary conditions and constraints.

[0017] Solving this system of equations will yield a complete trajectory curve that runs through all the key points of the path.

[0018] The first three derivatives of this curve at any point, namely velocity, acceleration, and jerk, are continuous without abrupt changes, thus avoiding the acceleration step caused by traditional circular interpolation at the connection point from the source.

[0019] The real-time trajectory feedforward compensation module is used for online operation. It receives the planned trajectory data from the high-order continuous spline interpolation module and combines it with the actual state data fed back from the robot joints to generate a feedforward compensation torque that directly acts on the servo drive.

[0020] This module contains a resonant dynamics inverse model based on an extended state observer. This inverse model uses the acceleration and jerk of the planned trajectory as primary inputs. It first simulates the rigid body dynamics of the robotic arm using a second-order mass-spring-damped system model to calculate the basic feedforward torque.

[0021] Meanwhile, the extended state observer continuously monitors the current and position feedback signals of the joint servo motor, and estimates and separates the lumped disturbances caused by factors such as mechanical structure flexibility and gear backlash in real time. These disturbances include potential resonant mode excitations.

[0022] Furthermore, this module negatively superimposes the estimated lumped disturbances to dynamically correct the basic feedforward torque. The final output feedforward compensation torque is designed to precisely offset the theoretical torque required for the robotic arm to track a smooth trajectory from the actual disturbance torque, thereby actively suppressing the excitation of resonance at the torque command level.

[0023] The multimodal switching execution module manages the entire operation process of the robot from high-speed approach to low-speed stable contact, and switches control strategies according to the characteristics of different stages.

[0024] This module defines and manages three operating modes: high-speed motion mode, contact transition mode, and stable holding mode.

[0025] In high-speed motion mode, the system mainly relies on the trajectory generated by the high-order continuous spline interpolation module and the output of the real-time trajectory feedforward compensation module for control.

[0026] When the robot's end effector, i.e. the hook, enters a spherical neighborhood with the target contact point as the center and a radius of a preset threshold, the module automatically switches to the contact transition mode.

[0027] In this mode, while maintaining feedforward compensation, the system significantly enhances the proportional gain of the position closed-loop control and sets the integral gain to zero to avoid overshoot caused by integral saturation.

[0028] At the same time, the speed of the planned trajectory is forcibly set to the contact point stable speed threshold, which is a minimum value much smaller than the normal motion speed.

[0029] When the contact force sensor between the hook and the target object detects that the force value has reached the preset contact success threshold, the module immediately switches to the stable holding mode.

[0030] In the stable holding mode, the system temporarily freezes the position command and switches to pure force control mode. It precisely controls the contact force according to the preset hook removal force curve and maintains it for a fixed time period to ensure that the hook removal action is reliably completed.

[0031] As one embodiment of the present invention, the algorithm for redundant point removal in the path parameterization preprocessing module is as follows. This module calculates the direction vector of the line connecting adjacent path points and the angle between the direction vectors of two line segments formed by three consecutive path points.

[0032] If the included angle is within the specified range, i.e., greater than 175 degrees and less than 185 degrees, the intermediate path point is determined to be a redundant point and is removed. Key feature point identification is based on the rate of change of path curvature. By calculating the discrete curvature of the path point sequence and finding the zero-crossing points of the first derivative of the curvature, these points are marked as feature points where the path direction has undergone a fundamental change and are retained.

[0033] As one embodiment of the present invention, the boundary conditions of the linear equation system constructed in the high-order continuous spline interpolation module are set as follows.

[0034] At the start and end points of the path, the position boundary conditions are given by the task, and the velocity, acceleration, and jerk boundary conditions are all set to 0, representing a stationary start and a stationary stop.

[0035] At key points within the path, the position boundary conditions must pass precisely through that point, while the boundary conditions for velocity, acceleration, and jerk are obtained by solving a local optimization subproblem. The goal of this subproblem is to minimize the sum of squares of the changes in jerk between the two polynomial trajectories before and after passing through the key point, thereby ensuring the smoothness of the global trajectory.

[0036] As one embodiment of the present invention, the design of the extended state observer in the real-time trajectory feedforward compensation module is as follows.

[0037] For each robot joint, a state-space equation is established that includes both rigid body state and extended perturbation state.

[0038] The observer takes the planned position, planned speed and actual motor current of the joint as input, and outputs the estimated values ​​of the actual position, actual speed and lumped disturbance of the joint in real time through the dynamic system determined by the observer gain matrix.

[0039] The observer gain matrix is ​​designed using the pole placement method, placing all observer poles in positions where the negative real part of the complex plane is much larger than the origin of the system, to ensure that the disturbance estimation has a fast response capability one order of magnitude higher than the mechanical resonant frequency.

[0040] As one embodiment of the present invention, the triggering condition for the contact transition mode in the multimodal switching execution module, namely the radius threshold of the spherical neighborhood, is set based on the maximum braking distance of the robot end effector at the current speed.

[0041] This distance is calculated using a formula, where the deceleration is taken as 80% of the maximum safe deceleration allowed by the robot system. The calculated braking distance, plus a fixed safety margin of 50 mm, together constitute the radius threshold of the spherical neighborhood.

[0042] This design ensures that the robot has ample space and time to smoothly decelerate to contact speed, avoiding emergency braking near the contact point.

[0043] In one embodiment of the present invention, the system further includes a trajectory monitoring and self-healing module. This module continuously compares the deviation between the planned trajectory position and the actual feedback position of the joint.

[0044] The module is considered to be in an abnormal state when the position tracking error of any joint exceeds 95% of its allowable error threshold, or when the activation time of the contact transition mode exceeds twice the preset nominal time and no successful contact signal is detected.

[0045] In abnormal conditions, the module immediately sends an interrupt command to the multimodal switching execution module, forcing the robot to retreat at a safe speed along the tangent direction of the current trajectory to the previous path key point, and restart the operation process from the high-speed motion mode.

[0046] In one embodiment of the present invention, the entire control system operates on a deterministic real-time computing platform. Path parameterization preprocessing and high-order continuous spline interpolation are non-real-time tasks, calculated all at once before the operation begins.

[0047] The real-time trajectory feedforward compensation module, the multi-modal switching execution module, and the trajectory monitoring and self-healing module are executed synchronously as real-time tasks with a fixed period of 1 millisecond to ensure the timeliness and determinism of control commands.

[0048] Compared with the prior art, the beneficial effects of the present invention are as follows: 1. This invention employs seventh-order polynomial splines for global trajectory planning, forcibly ensuring the continuity of fourth-order quantities (position, velocity, acceleration, and jerk), thus eliminating the inherent acceleration step at path junctions in traditional circular interpolation. This fundamental improvement ensures that the input excitation signal of the drive system itself does not contain high-frequency discontinuous components, eliminating the possibility of triggering mechanical structure resonance and laying a theoretical foundation for the smooth motion of the end effector.

[0049] 2. This invention innovatively introduces a feedforward compensation mechanism based on an extended state observer. This mechanism not only compensates for rigid body dynamics, but more importantly, it can estimate and compensate for lumped disturbances, including unmodeled resonant dynamics, in real time. This proactive feedforward suppression strategy can cancel out resonant energy before it accumulates, exhibiting faster response speed and stronger suppression depth compared to traditional passive feedback suppression methods, thus improving the system's robustness to changes in internal parameters and external disturbances.

[0050] 3. The multimodal switching execution logic designed in this invention finely divides the unhooking operation into different stages and applies targeted control strategies. Especially near the contact point, the system smoothly transitions to a low-speed, high-stiffness mode and ultimately switches to force-controlled holding, achieving a natural connection from trajectory tracking to force-position hybrid control. This design ensures that at the most critical contact moment of the unhooking action, the robot's end effector maintains extreme pose stability and force control accuracy, solving the technical problem of unhooking failure due to vibration and improving the success rate and reliability of the operation. Attached Figure Description

[0051] Figure 1 This is a schematic diagram of the overall technical architecture of the circular interpolation path planning and control system for the unhooking robot proposed in this invention; Figure 2 This is a schematic diagram of the core principle framework for generating high-order continuous trajectories based on seventh-order polynomial splines in this invention. Figure 3 This is a flowchart illustrating the main stages of the logical flow from path preprocessing to trajectory interpolation in this invention. Figure 4 This is a schematic diagram illustrating the principle of observation and suppression of resonant disturbances by the real-time feedforward compensation module in this invention. Figure 5 This is a flowchart of the workflow and state transition framework of the multimodal switching execution module in this invention. Detailed Implementation

[0052] To further illustrate the technical means and effects of the present invention in achieving its intended purpose, the following detailed description of the specific implementation methods, structures, features, and effects of the present invention, in conjunction with the accompanying drawings and preferred embodiments, is provided below.

[0053] Example 1: This invention provides a circular interpolation path planning and control system for a hook-unhooking robot. Please refer to the appendix. Figures 1 to 5 The system constructs a closed-loop control architecture from high-order continuous path generation to feedforward resonance suppression, aiming to eliminate discontinuous components in trajectory excitation and ensure absolute smoothness and stability of the robotic arm's motion throughout the unhooking operation, especially in the critical contact phase.

[0054] The system includes a path parameterization preprocessing module, a high-order continuous spline interpolation module, a real-time trajectory feedforward compensation module, and a multi-modal switching execution module.

[0055] The entire control system operates on a deterministic real-time computing platform.

[0056] The path parameterization preprocessing module and the high-order continuous spline interpolation module are non-real-time tasks, and are calculated all at once before the job starts.

[0057] The real-time trajectory feedforward compensation module, the multi-modal switching execution module, and the trajectory monitoring and self-healing module are executed synchronously as real-time tasks with a fixed period of 1 millisecond to ensure the timeliness and determinism of control commands.

[0058] The path parameterization preprocessing module is used to receive the raw path point sequence and job constraints from the upstream task planning layer.

[0059] The original path point sequence is usually provided by offline programming software or vision system, and includes a series of discrete spatial coordinate points traversed by the robot end effector, i.e. the hook, as it moves from the starting position to the target unhooking position and completes the unhooking action.

[0060] Operational constraints include global kinematic limitations, such as maximum speed limits, maximum acceleration limits, and the maximum safe deceleration allowed by the system for each joint of the robotic arm.

[0061] The primary function of this module is to remove redundant points and identify key feature points in the original path point sequence.

[0062] The algorithm for redundant point removal is as follows: This module sequentially reads the path point sequence and calculates the direction vector of the line connecting adjacent path points.

[0063] For three consecutive path points in the sequence, calculate the direction vector of the line segment formed by the first two points and the direction vector of the line segment formed by the last two points.

[0064] Calculate the angle between the two direction vectors. If the angle is within the specified range, i.e., greater than 175 degrees and less than 185 degrees, the intermediate path point is determined to be a redundant point and is removed.

[0065] This judgment logic is based on geometric principles, namely, when three points are nearly collinear, the contribution of the intermediate point to the description of the path shape is minimal, and removing it simplifies subsequent calculations. Key feature point identification is based on the rate of change of path curvature.

[0066] This module calculates the discrete curvature of the path point sequence after redundancy has been removed. The discrete curvature is calculated using the circular arc approximation method formed by three adjacent points, and the reciprocal of the instantaneous radius of curvature is calculated using the coordinates of the three points.

[0067] Next, the first difference of the discrete curvature sequence is calculated, which is the change in curvature between adjacent points.

[0068] Locate the points where the sign of the curvature difference value crosses zero, and mark these points as characteristic points where the path direction undergoes a fundamental change, such as the tangent points of a straight line segment and an arc segment, or the vertices of an arc. These characteristic points must be preserved.

[0069] After the above processing, the module extracts the set of key path points necessary to describe the hook removal action. This set includes at least the starting point, the center point of the arc along the path, the ending point, and the most important contact point.

[0070] The contact point is the spatial location where the hook makes its first physical contact with the target object to be unhooked. Furthermore, this module calculates and binds kinematic constraint parameters to the path segment between every two adjacent path key points.

[0071] For straight segments, the constraint parameters include the maximum allowable speed and maximum acceleration for that segment. These values ​​are typically taken as 90% of the system's global limits to allow for margin. For circular segments, in addition to the maximum speed and maximum acceleration, an extremely low stable speed threshold must be specifically set for the contact point.

[0072] This threshold is a very small value that is much smaller than the normal movement speed, typically ranging from 5 mm per second to 20 mm per second. Its specific value is preset by the process database based on the material and weight of the target to be unhooked and the required contact force accuracy.

[0073] All processed path key points and their associated constraint parameters constitute a standardized path description document.

[0074] This document outputs to the high-order continuous spline interpolation module in a structured data format, such as an array or object containing fields like point order, coordinates, segment type, and constraint parameters.

[0075] Please refer to the attached document. Figure 3 The diagram illustrates the main stages of the logical flow from path preprocessing to trajectory interpolation.

[0076] The high-order continuous spline interpolation module is used to generate a smooth trajectory that is continuous in terms of position, velocity, acceleration, and even jerk, based on a normalized path description.

[0077] The core of this module lies in the use of a global optimization interpolation algorithm based on seventh-order polynomial splines.

[0078] Please refer to the attached document. Figure 2 The figure illustrates the core principle framework for generating high-order continuous trajectories based on seventh-order polynomial splines.

[0079] Specifically, this module models each segment of motion between adjacent path key points as a time segment. Let be a seventh-degree polynomial function with respect to the independent variable. For spatial coordinates, such as the X-axis component, its motion can be expressed as: ; , , , , , , , These are all the coefficients to be determined for this polynomial segment. For a trajectory in three-dimensional space, the above modeling needs to be performed independently for the three coordinate components of X, Y, and Z, with each component having its own independent set of seventh-degree polynomial coefficients.

[0080] The goal of the module is to solve for the polynomial coefficients of all path segments and all coordinate components, so that the entire trajectory satisfies all boundary conditions and internal constraints of the path segments.

[0081] The boundary conditions are set as follows.

[0082] The positional boundary conditions at the start and end points of the entire path are determined by the coordinates of the start and end points given by the task.

[0083] The boundary conditions for velocity, acceleration, and jerk are all set to zero, which means that the robot starts moving from a stationary state and eventually stops at a stationary state.

[0084] At key points within the path, the location boundary conditions must force the polynomial function value at that point to be exactly equal to the coordinate value of that key point.

[0085] The boundary condition values ​​for velocity, acceleration, and jerk at the internal key points are obtained by solving local optimization subproblems.

[0086] The goal of this subproblem is to minimize the sum of the squares of the changes in jerk at the junction of the two polynomial trajectories before and after the key point.

[0087] Specifically, let the acceleration at the end of the first segment of the trajectory be... The acceleration at the starting point of the latter segment of the trajectory is The optimization goal is to make Minimum.

[0088] By solving this quadratic optimization problem, a set of boundary values ​​for velocity, acceleration, and jerk that make the trajectory smoothest at the internal points can be obtained. In addition to the boundary conditions at the points, each path segment must also satisfy the kinematic constraints within the segment, that is, the absolute value of the velocity at any time on the entire trajectory cannot exceed the maximum velocity bound to that segment, and the absolute value of the acceleration cannot exceed the maximum acceleration bound to that segment.

[0089] These constraints are achieved by discretizing the time variable and applying inequality constraints at multiple sampling points.

[0090] The boundary condition equations for the position, velocity, acceleration, and jerk of all path segments and all coordinate components, along with the velocity and acceleration inequality constraints within the path segments, are collectively constructed into a large-scale sparse linear system of equations and inequalities. The unknowns in this system of equations are all polynomial coefficients.

[0091] By using numerical optimization algorithms, such as sequential quadratic programming, to solve this system of equations and inequalities, a complete trajectory curve that runs through all the key points of the path can be obtained.

[0092] The first three derivatives of this curve at any point, namely velocity, acceleration, and jerk, are continuous without abrupt changes.

[0093] In particular, at the endpoints of the circular arc segment where acceleration steps inevitably occur in traditional circular interpolation algorithms, the trajectory generated by this invention has a smooth transition in acceleration curve and limited jerk, thus avoiding the high-frequency impact components generated at the connection point by traditional methods from the source of excitation.

[0094] The real-time trajectory feedforward compensation module is used for online operation. It receives the planned trajectory data from the high-order continuous spline interpolation module and combines it with the actual state data fed back from the robot joints to generate a feedforward compensation torque that directly acts on the servo drive.

[0095] The core of this module is the inverse model of resonant dynamics based on an extended state observer.

[0096] Please refer to the attached document. Figure 4 The figure illustrates the principle of observation and suppression of resonant disturbances by the real-time feedforward compensation module.

[0097] The planned trajectory data is sent to this module every control cycle, i.e., 1 millisecond. The data includes the planned position, planned velocity, planned acceleration, and planned jerk for the current cycle and several future cycles.

[0098] The actual state data of the robot joint feedback is acquired in real time through the fieldbus, mainly including the encoder position feedback value of each joint servo motor and the actual current value detected by the current sensor.

[0099] The inverse model of this module takes the acceleration and jerk of the planned trajectory as the main input.

[0100] First, the module maintains a second-order mass-spring-damping system model to simulate the rigid body dynamics of the robotic arm.

[0101] For each joint, the model calculates the basic feedforward torque required to drive the rigid body part of the joint to move, based on known parameters such as its moment of inertia and coefficient of viscous friction, combined with the current planned acceleration.

[0102] The calculations follow the Newton-Euler equations. Simultaneously, the extended state observer continues to run. For each robot joint, a state-space equation is established that includes both the rigid body state and the extended perturbation state.

[0103] State variables typically include joint angles, joint angular velocities, and extended states, which are used to characterize the sum of all unmodeled dynamics and external disturbances, i.e., lumped disturbances.

[0104] The observer takes the planned position, planned speed and actual motor current of the joint as inputs.

[0105] The dynamic system, defined by the observer gain matrix, outputs real-time estimates of the actual joint positions, velocities, and lumped disturbances. The observer gain matrix is ​​designed using the pole placement method.

[0106] In the specific design, all observer poles are configured to positions in the complex plane where the negative real part is much larger than the origin of the system. For example, the real part of the pole is set to -10 times the main resonant frequency of the robotic arm.

[0107] This configuration ensures that the disturbance estimation loop has a fast response capability one order of magnitude higher than the mechanical resonant frequency, enabling the observer to track and separate the lumped disturbances caused by factors such as mechanical structure flexibility, gear backlash, and nonlinear friction in near real time. These disturbances contain potential resonant mode excitations.

[0108] Furthermore, the module performs a negative superposition of the estimated lumped disturbance values ​​to dynamically correct the previously calculated basic feedforward torque.

[0109] The corrected formula is: ; For the final feedforward torque, Based on the feedforward torque, This is the estimated value of the lumped disturbance. This is the feedforward gain.

[0110] The final feedforward torque is equal to the basic feedforward torque minus the product of the lumped disturbance estimate and the feedforward gain.

[0111] The final output feedforward compensation torque is designed to precisely offset the theoretical torque and actual disturbance torque required for the robotic arm to track a smooth trajectory, thereby actively suppressing the excitation of resonance at the torque command level.

[0112] The torque command is superimposed with the feedback torque output by the downstream position closed-loop controller to form the total torque command sent to the servo drive.

[0113] The multimodal switching execution module manages the entire operation process of the robot from high-speed approach to low-speed stable contact, and switches control strategies according to the characteristics of different stages.

[0114] Please refer to the attached document. Figure 5 The diagram illustrates the workflow and state transition framework of the multimodal switching execution module.

[0115] This module defines and manages three operating modes: high-speed motion mode, contact transition mode, and stable holding mode.

[0116] After the system is powered on and initialized, it is in an idle state by default. When a job start command is received, the module first loads the complete trajectory calculated by the high-order continuous spline interpolation module and switches to the high-speed motion mode.

[0117] In high-speed motion mode, the system mainly relies on the trajectory generated by the high-order continuous spline interpolation module as the position command and the feedforward torque output by the real-time trajectory feedforward compensation module for control.

[0118] The position closed-loop controller uses a standard proportional-integral-derivative (PID) control algorithm, with its proportional gain, integral gain, and derivative gain set to conventional values ​​suitable for high-speed tracking.

[0119] The robot's end effector, or hook, rapidly approaches the target contact point along a smooth trajectory.

[0120] The module continuously calculates the Euclidean distance between the current position of the robot's end effector and the target contact point.

[0121] When the distance enters a spherical neighborhood with the target contact point as the center and a radius of a preset threshold, the module automatically switches to the contact transition mode.

[0122] The radius threshold of the spherical neighborhood is set based on the maximum braking distance of the robot's end effector at the current speed.

[0123] This distance is calculated using a formula. First, the maximum safe deceleration allowed by the robot system is obtained, denoted as... .

[0124] When calculating the braking distance, the deceleration value used is... 80%, recorded as .

[0125] Let the current velocity of the robot's end effector at the moment of switching be... Then the theoretical braking distance .

[0126] Based on this theoretical braking distance, plus a fixed safety margin of 50 mm, the radius threshold of the spherical neighborhood is formed together. .

[0127] This design ensures that the robot has ample space and time to smoothly decelerate to contact speed, avoiding emergency braking near the contact point.

[0128] After entering the contact transition mode, the system makes significant adjustments to the position closed-loop control strategy while maintaining feedforward compensation.

[0129] The proportional gain is increased to 2 to 3 times in high-speed motion modes to improve the positional stiffness of the system at low speeds.

[0130] At the same time, the integral gain is set to zero to avoid overshoot or oscillation caused by integral term saturation when approaching a stationary target.

[0131] The most important adjustment is that the speed of the planned trajectory is forced to be set to the contact point stable speed threshold, which is the extremely low speed value that the path parameterization preprocessing module binds to the contact point.

[0132] This means that regardless of the speed at which the mode is entered, the trajectory generator will generate a smooth speed ramp so that the end velocity decays to the stable speed threshold just before reaching the contact point.

[0133] When the contact force sensor between the hook and the target object detects that the force value has reached the preset contact success threshold, the module immediately switches to the stable holding mode.

[0134] The successful contact threshold is set according to the requirements of the unhooking process, with a typical value of 5 N to 20 N.

[0135] In the stable hold mode, the system temporarily freezes position commands, meaning it no longer tracks the preset trajectory.

[0136] It then switches to pure force control mode. The force controller precisely controls the contact force according to the preset hook-off force curve.

[0137] The unhooking force curve typically includes a rapid build-up phase, which allows the contact force to reach the target unhooking force in a short time; a holding phase, which maintains the target unhooking force for a fixed period of time, such as 300 to 500 milliseconds, to ensure that the unhooking action is reliably completed; and finally, a release phase, which smoothly reduces the contact force to zero.

[0138] During force control holding, the robot's position loop is in a relaxed state and is completely dominated by the force loop.

[0139] When the holding time reaches the preset cycle and the force controller confirms that the unhooking action is completed, the module sends a work completion signal to the system and controls the robot to move away according to the return segment of the planned trajectory or a new instruction.

[0140] The trajectory monitoring and self-healing module serves as a guarantee for system safety and reliability, and operates continuously.

[0141] This module compares the deviation between the planned trajectory position and the actual feedback position of the joint in each control cycle, and calculates the real-time position tracking error of each joint.

[0142] The permissible error threshold for each joint is preset based on its range of motion and accuracy requirements. For example, for a rotary joint, the threshold might be 0.01 radians; for a linear joint, the threshold might be 0.5 millimeters. The module has two levels of monitoring conditions.

[0143] In the first level, when the position tracking error of any joint exceeds 95% of its allowable error threshold, the module determines that the tracking is abnormal.

[0144] The second level is also considered an abnormal state if the activation time of the contact transition mode is longer than the preset nominal time and no contact success signal is detected.

[0145] The nominal time of the contact transition mode is calculated based on the radius of the spherical neighborhood and the stable velocity threshold of the contact point, with a margin of 2 times the value as the timeout judgment threshold.

[0146] In abnormal conditions, the module immediately sends a high-level interrupt command to the multi-modal switching execution module.

[0147] After receiving an interrupt command, the multimodal switching execution module forces the robot to stop any actions in the current mode.

[0148] Then, control the robot to move backward along the tangent of the current trajectory at a preset safe speed, such as 100 millimeters per second.

[0149] The target point for backtracking is the critical point of the previous path, which is clearly defined in the normalized path description.

[0150] Once the robot's end effector has retreated to that point and stabilized, the system clears the abnormality flag and restarts the entire operation process from the high-speed motion mode.

[0151] This self-healing mechanism can effectively cope with operation interruptions caused by unexpected environmental interference, momentary sensor failures, etc., and improve the robustness of the system.

[0152] The data flow and execution flow of the entire control system are strictly synchronized.

[0153] Before the job begins, the path parameterization preprocessing module and the high-order continuous spline interpolation module complete all calculations, and the generated time-position-velocity-acceleration-jerk trajectory table is loaded into the shared memory area.

[0154] Real-time tasks are triggered precisely at 1-millisecond intervals.

[0155] Within each cycle, the real-time trajectory feedforward compensation module reads current and future trajectory data from shared memory, and simultaneously reads actual joint feedback from the bus to calculate the feedforward compensation torque.

[0156] The multimodal switching execution module determines the source of the output position command based on the current mode and conditions, whether it comes from the trajectory table or the frozen position, and whether to switch force control.

[0157] The trajectory monitoring and self-healing modules execute the monitoring logic in parallel. Finally, the feedback torque calculated by the feedforward compensation torque, position loop, or force loop is superimposed and limited, and then sent to the servo drivers of each joint via the real-time bus to drive the motors to perform precise movements.

[0158] Through the coordinated operation of the above modules, this invention achieves a complete high-precision and high-stability unhooking robot control system, from ensuring smoothness at the source of trajectory planning to active resonance suppression during execution, and adaptive switching and safety monitoring during the operation phase.

[0159] Example 2: Based on the system architecture described in Example 1, this example provides an optimized implementation method for ultra-long strokes and complex paths with multiple circular arcs.

[0160] This approach primarily involves adaptively adjusting the solution strategy of the high-order continuous spline interpolation module and the observer parameters of the real-time trajectory feedforward compensation module to cope with more complex operational scenarios.

[0161] In the high-order continuous spline interpolation module, when there are many key points on the path, especially when there are multiple continuous arc segments, directly constructing a global system of equations for solving may result in an excessively large optimization problem and excessively long computation time.

[0162] Therefore, this embodiment adopts a hierarchical and segmented solution strategy.

[0163] First, the module divides the entire path into several independent path segments according to key feature points, such as the tangent point between a straight line and an arc, and the center point of the arc.

[0164] Each segment contains two boundary points, a start point and an end point, as well as a few key points that may exist within it.

[0165] For each sub-segment, construct its own system of seventh-degree polynomial spline equations and solve them independently.

[0166] At the connection points between sub-segments, in addition to forcing positional continuity, constraints for velocity continuity and acceleration continuity are also applied.

[0167] Specifically, let the final velocity of the previous segment be... acceleration is The starting velocity of the next segment is acceleration is .

[0168] The constraints are and .

[0169] At the junction, no mandatory continuity constraint is imposed on the jerk, allowing for finite jumps, because the finite jumps of the jerk have a much smaller excitation on the system than the acceleration jumps.

[0170] This relaxation decomposes large-scale, globally coupled optimization problems into multiple smaller, parallel-solvable subproblems, reducing the computation time for offline trajectory planning.

[0171] After all the sub-segments of the trajectory are solved, they are then pieced together in chronological order to form the complete global trajectory.

[0172] To ensure the smoothness of the overall trajectory after splicing, when dividing the segments, priority is given to dividing the segments into points that naturally exhibit low or zero speed in the velocity planning, such as the highest point of the arc segment or points where the path direction changes abruptly.

[0173] For the real-time trajectory feedforward compensation module, when the robotic arm performs ultra-long stroke motion, its dynamic parameters, such as the equivalent moment of inertia of the joints, may change with the change of the boom posture.

[0174] A fixed, rigid body dynamics inverse model will produce a large error.

[0175] Therefore, this embodiment employs a parameter-adaptive extended state observer.

[0176] The rigid body model parameters in the observer, mainly the moment of inertia, are no longer set as constants, but are identified online as slowly varying states.

[0177] In the state-space equations, we add states that represent changes in the moment of inertia.

[0178] While estimating position, velocity, and lumped disturbance, the observer also outputs an estimate of the current moment of inertia.

[0179] This estimate is used to update the parameters in the inverse dynamic model in real time, thereby making the calculation of the basic feedforward torque more accurate.

[0180] The observer gain matrix is ​​designed using a time-varying strategy, with its pole configuration associated with the currently estimated moment of inertia value, ensuring that the required fast perturbation estimation performance is maintained under different load inertia.

[0181] In practice, several sets of fixed gain matrices corresponding to typical inertia ranges are pre-calculated. During online operation, linear interpolation is performed based on the estimated inertia value to obtain the gain matrix used in the current cycle.

[0182] In the multimodal switching execution module, this embodiment extends the logic of the contact transition mode to address the multiple potential contact points that may exist along a complex path. The system maintains an ordered list of potential contact points, which is generated during the path parameterization preprocessing stage. Each potential contact point is bound to its own stable velocity threshold and contact force threshold. When the robot end effector enters a neighborhood centered at a potential contact point, the system does not immediately switch to the final contact transition mode, but instead first enters the pre-contact reconnaissance sub-mode.

[0183] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus.

[0184] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Although the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make some modifications or alterations to the above-disclosed technical content to create equivalent embodiments without departing from the scope of the present invention. Any simple modifications, equivalent changes and alterations made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the scope of the present invention.

Claims

1. A circular interpolation path planning and control system for a hook-unhooking robot, characterized in that, include: The path parameterization preprocessing module is used to receive the original path point sequence and job constraints, and to remove redundant points and identify key feature points in the original path point sequence to extract the path key points, including the starting point, the center point of the arc passed through, the ending point, and the contact point. The high-order continuous spline interpolation module is used to generate a smooth trajectory that is continuous in terms of position, velocity, acceleration, and jerk based on a normalized path description. The real-time trajectory feedforward compensation module is used for online operation. It receives the planned trajectory data from the high-order continuous spline interpolation module and combines it with the actual state data fed back from the robot joints to generate a feedforward compensation torque that directly acts on the servo drive. The multimodal switching execution module is used to manage the robot's operation process from high-speed approach to low-speed stable contact, and to switch control strategies according to different stages.

2. The circular interpolation path planning and control system for the unhooking robot according to claim 1, characterized in that, The path parameterization preprocessing module is also used to calculate and bind kinematic constraint parameters for each path segment. The kinematic constraint parameters include the maximum speed allowed for the path segment, the maximum acceleration, and the extremely low stable speed threshold set for the contact point. All processed path key points and their bound constraint parameter sets constitute a normalized path description.

3. The circular interpolation path planning and control system for the unhooking robot according to claim 2, characterized in that, The high-order continuous spline interpolation module adopts a global optimization interpolation algorithm based on seventh-order polynomial splines to model the motion between key points of adjacent paths as a polynomial function with time as the parameter. By constructing a large-scale sparse linear system of equations, the boundary conditions of position, velocity, acceleration, and jerk at key points of the path, as well as the velocity and acceleration constraints within the path segments, the equations are formed. Solving this system of equations yields the coefficients of a seventh-order polynomial that satisfy all boundary conditions and constraints, thus providing a complete trajectory curve that runs through all key points of the path.

4. The circular interpolation path planning and control system for the unhooking robot according to claim 3, characterized in that, The real-time trajectory feedforward compensation module includes a resonant dynamics inverse model based on an extended state observer. The inverse model takes the acceleration and jerk of the planned trajectory as input, and simulates the rigid body dynamics of the robotic arm through a second-order mass-spring-damped system model to calculate the basic feedforward torque; at the same time, the extended state observer continuously observes the current and position feedback signals of the joint servo motor, and estimates and separates the lumped disturbances caused by the flexibility of the mechanical structure and gear backlash in real time. The module negatively superimposes the estimated lumped disturbances and dynamically corrects the basic feedforward torque to output a feedforward compensation torque designed to accurately offset the theoretical torque and the actual disturbance torque.

5. The circular interpolation path planning and control system for the unhooking robot according to claim 4, characterized in that, The multimodal switching execution module defines and manages high-speed motion mode, contact transition mode, and stable holding mode; In high-speed motion mode, the system relies on the trajectory generated by the high-order continuous spline interpolation module and the output of the real-time trajectory feedforward compensation module for control; When the robot's end effector enters a spherical neighborhood with the target contact point as the center and a radius of a preset threshold, the module automatically switches to the contact transition mode. In the contact transition mode, while maintaining feedforward compensation, the system enhances the proportional gain of the position closed-loop control and sets the integral gain to zero, while forcibly setting the speed of the planned trajectory to the stable speed threshold of the contact point. When the contact force sensor between the end effector and the target object detects that the force value has reached the preset contact success threshold, the module immediately switches to the stable holding mode; In the stable holding mode, the system freezes the position command and switches to pure force control mode, precisely controlling the contact force according to the preset hook-off force curve and maintaining it for a fixed time period.

6. The circular interpolation path planning and control system for the unhooking robot according to claim 5, characterized in that, The algorithm for redundant point removal in the path parameterization preprocessing module is as follows: Calculate the direction vector of the line connecting adjacent path points, and calculate the angle between the direction vectors of two line segments formed by three consecutive path points; If the included angle is within the specified range, the intermediate path point is determined to be a redundant point and is removed.

7. The circular interpolation path planning and control system for the unhooking robot according to claim 6, characterized in that, The boundary conditions for the linear equation system constructed in the higher-order continuous spline interpolation module are as follows: At the start and end points of the path, the position boundary conditions are given by the task, and the velocity, acceleration, and jerk boundary conditions are all set to 0. At key points within the path, the position boundary conditions must pass precisely through that point, while the boundary conditions for velocity, acceleration, and jerk are obtained by solving a local optimization subproblem. The goal of this subproblem is to minimize the sum of squares of the changes in jerk between the two polynomial trajectories before and after passing through the key point.

8. The circular interpolation path planning and control system for the unhooking robot according to claim 7, characterized in that, The design of the extended state observer in the real-time trajectory feedforward compensation module is as follows: For each robot joint, establish a state-space equation that includes both rigid body state and extended perturbation state; The observer takes the planned position, planned speed and actual motor current of the joint as inputs, and outputs the estimated values ​​of the actual position, actual speed and lumped disturbance of the joint in real time through the dynamic system determined by the observer gain matrix. The observer gain matrix is ​​designed using a pole placement method, which places all observer poles in positions where the negative real part of the complex plane is much larger than the origin of the system.

9. The circular interpolation path planning and control system for the unhooking robot according to claim 8, characterized in that, The triggering condition for the contact transition mode in the multimodal switching execution module, namely the radius threshold of the spherical neighborhood, is set based on the maximum braking distance of the robot end effector at the current speed. The distance is calculated using a formula, where the deceleration is taken as 80% of the maximum safe deceleration allowed by the robot system; the calculated braking distance, plus a safety margin of 50 mm, together constitute the radius threshold of the spherical neighborhood.

10. The circular interpolation path planning and control system for the unhooking robot according to claim 9, characterized in that, It also includes a trajectory monitoring and self-healing module; the trajectory monitoring and self-healing module continuously compares the deviation between the planned trajectory position and the actual feedback position of the joint. When the position tracking error of any joint exceeds 95% of its allowable error threshold, or when the activation time of the contact transition mode exceeds twice the preset nominal time and no successful contact signal is detected, the module is determined to be in an abnormal state. In an abnormal state, the module immediately sends an interrupt command to the multimodal switching execution module, forcing the robot to retreat at a safe speed along the tangent direction of the current trajectory to the previous path key point, and restart the operation process from the high-speed motion mode.

Images