CN-122008184-A - Arc interpolation path planning control system of unhooking robot

CN122008184ACN 122008184 ACN122008184 ACN 122008184ACN-122008184-A

Abstract

The invention belongs to the technical field of image recognition and industrial vision, and particularly discloses a circular arc interpolation path planning control system of an unhooking robot. 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. By generating a high-order continuous track, carrying out feedforward torque compensation and modal splitting refined control based on the extended state observer, smooth motion in the whole process from high-speed approach to stable contact is realized, mechanical resonance is restrained, and accuracy and reliability of unhooking operation are ensured.

Inventors

YANG FEI
XU JINHAO
WEI LONG

Assignees

山东省科创集团有限公司

Dates

Publication Date: 20260512
Application Date: 20251224

Claims (10)

1. Arc interpolation path planning control system of unhooking robot, its characterized in that includes: The path parameterization preprocessing module is used for receiving an original path point sequence and operation constraint, and carrying out redundant point elimination and key characteristic point identification on the original path point sequence so as to extract path key points comprising a starting point, a circle center point passing through an arc, an ending point and a contact point; the high-order continuous spline interpolation module is used for generating a continuous smooth track on the position, speed, acceleration and jerk layers based on the normalized path description; the real-time track feedforward compensation module is used for running on line, receiving planning track data from the high-order continuous spline interpolation module, and generating feedforward compensation moment directly acting on the servo driver by combining actual state data fed back from the robot joint; The multi-mode switching execution module is used for managing the operation flow of the robot from high-speed approaching to low-speed stable contact and switching the control strategy according to different stages.
2. The arc interpolation path planning control system of the unhooking robot according to claim 1, wherein the path parameterization preprocessing module is further configured to calculate and bind a kinematic constraint parameter for each path segment, where the kinematic constraint parameter includes a maximum speed and a maximum acceleration allowed by the path segment and an extremely low steady speed threshold set for a contact point, and all the processed path keypoints and bound constraint parameter sets thereof form a normalized path description.
3. The arc interpolation path planning control system of the unhooking robot according to claim 2, wherein the high-order continuous spline interpolation module adopts a global optimization interpolation algorithm based on seven-degree polynomial splines, and models the motion between adjacent path key points as a polynomial function taking time as a parameter; The method comprises the steps of constructing a large-scale sparse linear equation set through constraint of positions, speeds, accelerations and boundary conditions of jerk at key points of a path and speeds and accelerations inside a path section; solving the equation set to obtain seven-degree polynomial coefficients meeting all boundary conditions and constraints, thereby obtaining a complete track curve penetrating all path key points.
4. The arc interpolation path planning control system of the unhooking robot according to claim 3, wherein the real-time track feedforward compensation module includes a harmonic dynamic inverse model based on an extended state observer; The inverse model takes the acceleration and jerk of a planned track as input, and simulates the rigid body dynamics of the mechanical arm through a second-order mass-spring-damping system model to calculate and obtain a basic feedforward moment; The module carries out negative superposition on the estimated lumped disturbance and carries out dynamic correction on the basic feedforward torque so as to output feedforward compensation torque aiming at accurately counteracting the theoretical torque and the actual disturbance torque.
5. The arc interpolation path planning control system of the unhooking robot of claim 4, wherein the multi-mode switching execution module defines and manages a high-speed motion mode, a contact transition mode, and a steady-state holding mode; Under a high-speed motion mode, the system controls depending on the track generated by the high-order continuous spline interpolation module and the output of the real-time track feedforward compensation module; When the robot end effector enters a spherical neighborhood with a target contact point as a circle center and a radius as a preset threshold, the module is automatically switched to a contact transition mode; In the contact transition mode, the system enhances the proportional gain of the position closed-loop control and sets the integral gain to zero while maintaining feedforward compensation, and meanwhile, the speed of a planned track is forcedly set as the contact point stable speed threshold; When the contact force sensor of the end effector and the target object detects that the force value reaches a preset contact success threshold value, the module is immediately switched to a stable holding mode; Under the stable holding mode, the system freezes the position command and switches to a pure force control mode, and the contact force is precisely controlled according to a preset unhooking force curve and maintained for a fixed time period.
6. The arc interpolation path planning control system of the unhooking robot according to claim 5, wherein the algorithm process of redundant point elimination in the path parameterized preprocessing module is as follows: Calculating the direction vectors of the connecting lines of the adjacent path points, and calculating the included angle of the direction vectors of two sections of line segments formed by the continuous 3 path points; if the included angle is within the specified range, the intermediate path point is judged as a redundant point and is eliminated.
7. The arc interpolation path planning control system of the unhooking robot according to claim 6, wherein the linear equation set constructed in the high-order continuous spline interpolation module has the boundary conditions set as follows: at the starting point and the ending point of the path, the boundary conditions of the position are given by the task, and the boundary conditions of the speed, the acceleration and the jerk are all set to 0; at a critical point inside the path, the position boundary conditions must pass precisely through the point, while the boundary conditions of velocity, acceleration, jerk are obtained by solving a local optimization sub-problem whose objective is to minimize the sum of the squares of jerk changes of the polynomial trajectories passing through the critical point before and after.
8. The arc interpolation path planning control system of the unhooking robot of claim 7, wherein the design of the extended state observer in the real-time trajectory feedforward compensation module is as follows: establishing a state space equation comprising a rigid body state and an expansion disturbance state for each robot joint; The observer takes the planned position, the planned speed and the actual motor current of the joint as inputs, and the estimated values of the actual position, the actual speed and the lumped disturbance of the joint are output in real time through a dynamic system determined by an observer gain matrix; The observer gain matrix is designed by a pole allocation method, and all observer poles are allocated to a position of which the negative real part of the complex plane is far greater than the origin of the system.
9. The arc interpolation path planning control system of the unhooking robot according to claim 8, wherein a trigger condition of a contact transition mode in the multi-mode switching execution module, namely a radius threshold of a spherical neighborhood, is set according to a maximum braking distance of the tail end of the robot at a current speed; the distance is calculated by a formula, wherein the deceleration takes 80% of the maximum safe deceleration allowed by the robot system, and the calculated braking distance is added with a safe allowance fixed to be 50mm to jointly form the radius threshold value of the spherical neighborhood.
10. The arc interpolation path planning control system of the unhooking robot according to claim 9, further comprising a track monitoring and self-healing module, wherein the track monitoring and self-healing module continuously compares the deviation between the planned track position and the actual feedback position of the joint; The module determines an abnormal condition when the position tracking error of any joint is greater than 95% of its allowable error threshold, or when the contact transition mode activation time is greater than 2 times the preset nominal time, and no contact success signal is detected yet; And under an abnormal state, the module immediately sends an interrupt instruction to the multi-mode switching execution module, forces the robot to return to the key point of the previous path at a safe speed along the tangential direction of the current track, and re-starts executing the operation flow from the high-speed motion mode.

Description

Arc interpolation path planning control system of unhooking robot Technical Field The invention belongs to the technical field of image recognition and industrial vision, and particularly relates to a circular arc interpolation path planning control system of an unhooking robot. Background In the field of industrial robot automation, path planning and control are core technologies for realizing precise and efficient operation of robots. The robot performs complex tasks such as welding, assembling, handling, etc. by executing a preset trajectory path, and the smoothness and control accuracy of the path directly determine the quality of the operation and the stability of the operation of the equipment. The unhooking robot is used as an automatic device for performing specific precise operations, and the task core of the unhooking robot is to control a hook body at the tail end of a mechanical arm to approach and pick up a target object in a specific posture and track. This process requires that the robotic end effector, i.e., the hook, be able to move smoothly along the planned path and maintain very high pose stability at the moment of contact with the target to avoid operational failure or damage to the workpiece due to vibration or misalignment. In the prior art, a piecewise straight line or a simple circular interpolation algorithm is often adopted for path planning of the unhooking robot. However, such methods are prone to step-wise abrupt changes in acceleration when planning circular paths, particularly at the points of attachment or speed change of the path. Such discontinuities in acceleration may introduce high frequency excitation in the mechanical arm drive system, exciting the natural frequency of the mechanical structure, causing the mechanical arm to resonate. At the critical contact moment of the unhooking operation, the resonance can be directly expressed as continuous oscillation of the hook tip, so that the actual pose deviates from the theoretical path seriously, the unhooking operation is out of tolerance, and the severe requirements of the precision unhooking operation on stability and repeated precision cannot be met. Therefore, how to effectively inhibit mechanical resonance caused by improper path planning and ensure the extremely stable unhooking contact process has become a technical problem to be solved in order to restrict the improvement of the humanization of the machine. Disclosure of Invention The invention aims to provide a circular arc interpolation path planning control system of an unhooking robot, which solves the technical contradiction that in the prior art, acceleration is discontinuous due to a circular arc path planning algorithm, so that mechanical resonance is excited at the moment of unhooking contact, and the tail end pose of the robot is unstable and the operation precision is reduced. The invention provides a planning control system for an arc interpolation path of a unhooking robot, which radically eliminates discontinuous components in track excitation by constructing a closed-loop control framework from high-order continuous path generation to feedforward resonance suppression, and ensures absolute smoothness and stability of motion of a mechanical arm in the whole unhooking operation process, particularly in a critical contact stage. 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. And the path parameterization preprocessing module is used for receiving the original path point sequence and the job constraint from the upstream task planning layer. The module firstly performs redundant point elimination and key feature point identification on an original path point sequence, and extracts path key points which are necessary for describing the unhooking action and comprise a starting point, a circle center point passing through an arc, an ending point and a contact point. Further, the module calculates and binds for each path segment, in particular an arc segment, kinematic constraint parameters including the maximum speed allowed by the path segment, the maximum acceleration and an extremely low steady speed threshold specifically set for the contact point. All the processed path key points and the binding constraint parameter sets thereof form normalized path description, and the normalized path description is output to a high-order continuous spline interpolation module. And the high-order continuous spline interpolation module is used for generating a smooth track which is continuous in the layers of position, speed, acceleration and even jerk based on the normalized path description. The core of the module is to adopt a global optimization interpolation algorithm based on seven-degree polynomial spline. Specifically, the module models the motion between neighboring path keypoints as a polynomi