Force-position hybrid interpolation method for motion control

Abstract

The invention discloses a force-position hybrid interpolation method for motion control, which relates to the technical field of robot motion control, and realizes high-precision force-position cooperative control by constructing a moving frame for parallel control of force and position, combining real-time force sensing data to predict a motion track and utilizing a hybrid interpolation algorithm. The method specifically comprises the following steps: calculating a normal vector and a tangent vector of a contact surface in real time based on feedback data of a six-axis force sensor; predicting the position increment of the next motion step length through Taylor expansion and a dynamic compensation model; designing a hybrid interpolator of force and position to realize independent decoupling and collaborative optimization of force control and position control; a differential geometry method is used for extracting surface curvature and deflection, unknown surface types are recognized, trajectory planning is optimized, the adaptive capacity of the robot to the unknown surfaces is remarkably improved, and the method is suitable for scenes such as precision assembly and flexible grabbing.

Description

Technical Field

[0001] The present invention relates to the technical field of robot motion control, and particularly relates to a force-position hybrid interpolation method for motion control. Background Art

[0002] When a robot executes complex tasks (such as assembly, surface grinding, precision inspection), it is necessary to control the position and contact force of the end effector simultaneously. Traditional methods such as hybrid control and impedance control have problems such as severe force-position coupling and insufficient dynamic response. Especially in high-speed motion, it is difficult to achieve stable contact force tracking. In addition, traditional interpolators are mostly designed based on position control and lack the ability to dynamically adjust in real-time force feedback, resulting in force overshoot or trajectory deviation easily occurring in unknown surface tracking. Therefore, it is particularly important to invent a force-position hybrid interpolation method for motion control.

[0003] The prior art also has the following defects, specifically reflected in: 1. In the prior art, traditional methods such as hybrid control and impedance control have severe force-position coupling when a robot executes complex tasks. In high-speed motion scenarios, this coupling is difficult to achieve stable contact force tracking, affecting the accuracy and stability of robot operations and unable to meet the task requirements with high force-position collaborative control requirements such as precision assembly, surface grinding, and precision inspection.

[0004] 2. In the prior art, traditional interpolators are mostly designed based on position control and lack the ability to dynamically adjust in real-time force feedback. When tracking an unknown surface, due to insufficient trajectory prediction accuracy and interpolation cycle limitations, it is difficult to ensure real-time performance and robustness during high-speed motion, and force overshoot or trajectory deviation easily occurs, making it unable to adapt to complex and changeable working environments. Summary of the Invention

[0005] The purpose of the present invention is to provide a force-position hybrid interpolation method for motion control, which solves the problems existing in the background art.

[0006] To solve the above technical problems, the present invention adopts the following technical solutions: The present invention provides a force-position hybrid interpolation method for motion control, including: Step S1: Based on the real-time feedback data of a six-axis force sensor, calculate the normal vector and tangential vector of the contact surface, and construct a moving frame (C; T, B, n), where T is the trajectory tangential vector, n is the surface normal vector, and B is the cross product of T and n.

[0007] Step S2: According to the planned moving speed v d , and the interpolation period T intp , calculate the tangential feed Δs, Δs = v d T intp , and calculate the surface normal curvature k according to the derivative of the moving frame nand geodesic torsion τ g , combined with the Taylor expansion model and the tangential feed Δs, predicts the tangential position control increment within each interpolation cycle.

[0008] Step S3: According to the normal contact force F d given by the plan, and the difference from the actual normal contact force, within each interpolation cycle, invokes the force control servo algorithm to calculate the corresponding position change amount, which is the normal force control increment ΔN m .

[0009] Step S4: Design a force-position hybrid interpolator. Inside the interpolator, perform vector superposition on the tangential position control increment and the normal force control increment ΔN m . The superposed vector generates the movement amounts of each axis of the robot through two-layer interpolation of rough and fine interpolation.

[0010] Step S5: Within each interpolation cycle, record the pose of the robot in the constraint plane H, output a high-precision motion trajectory, and complete surface reconstruction.

[0011] Preferably, in the said Step S1, the calculation formula for the surface normal vector is:

[0012] where F p is the projection of the normal force in the plane H, F YH is the feedback force of the sensor in the Y direction of H, α is the angle between the tangent vector and the horizontal axis of the plane H, and the denominator F n acts as the normalization n.

[0013] Preferably, in the said Step S2, the tangential position control increment is:

[0014] ΔsT(s0)+k(s0)(Δs) 2 N(s0) / 2, where T(s0) is the tangential direction of the motion trajectory, k(s0) is the curvature of the motion trajectory, and N(s0) is the projection of the normal vector n in the plane H.

[0015] Preferably, in the said Step S3, the normal force control increment ΔN m is:

[0016] ΔN m =(K p +K i T intp ∑(F d -F n )-K d (F d -F n ) / T intp )N(s0) / <N(s0),n>, where ΔNm is the PID compensation amount calculated based on the force error, F d represents the target contact force, F n represents the actual contact force, K p and K i and K d respectively represent the proportional coefficient, integral coefficient, and differential coefficient.

[0017] Preferably, in the step S4, the hybrid interpolator includes a two-level structure of rough interpolation and fine interpolation. The interpolation period of the rough interpolation is an integer multiple of the fine interpolation period. The rough interpolation calculates the pose increment within each interpolation period in the Cartesian space of the robot, and calculates the movement amounts of each axis of the robot through the inverse kinematics of the robot. The fine interpolation further subdivides the movement amounts of each axis within the fine interpolation period to control the movement of each axis of the robot.

[0018] Preferably, in the step S5, for the rotation plane, within the constraint plane H, by collecting the surface normal curvature k n and geodesic torsion τ g data of multiple motion trajectories, a global geometric feature dataset is constructed; based on the distribution law of k n in the dataset and the change trend of τ g , the trajectory points conforming to the rotational symmetry characteristics are screened. The screening condition is that the fluctuation of the k n value on the same trajectory is less than the preset threshold and the change trends of τ g of adjacent trajectories are consistent; the surface contour curve is fitted through an optimization algorithm, and the contour shape is determined with the goal of minimizing the error between the normal curvature and the theoretical radius; finally, the fitted two-dimensional contour is rotated around the axis of symmetry to generate a three-dimensional surface model, which is mapped to the robot motion coordinate system, and a continuous and smooth motion trajectory is output. By fusing the geometric feature data of multiple trajectories and combining statistical analysis and optimization algorithms, the global reconstruction of the unknown rotationally symmetric surface is realized.

[0019] The beneficial effects of the present invention are as follows: 1. In the present invention, a moving frame including the tangential, normal, and binormal directions is constructed to realize the parallel control of force and position. Through the hybrid interpolator designed in layers, the position increment is decomposed into the force control direction (normal direction) and the position control direction (tangential direction). The PID force compensation algorithm and the position servo control algorithm are respectively adopted, so that the force control and the position control operate independently in the orthogonal space, avoiding mutual interference, realizing the independent decoupling and collaborative optimization of force and position, and significantly improving the force-position collaborative control accuracy and stability of the robot in complex tasks.

[0020] 2. In the present invention, the normal vector and tangent vector of the contact surface are calculated in real time by using the feedback data of the six-axis force sensor. Combining the Taylor expansion model with the surface geometric characteristics to predict the position increment of the next motion step, effectively improving the trajectory prediction accuracy. At the same time, based on the distribution characteristics of the normal curvature and geodesic torsion, the interpolation parameters are dynamically adjusted to optimize the trajectory planning; it can also complete the global surface reconstruction within the constraint plane H and output a high-precision motion trajectory, enhancing the robot's adaptive ability and tracking accuracy for unknown surfaces and meeting the requirements of industrial-level precision operations. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0022] Figure 1 It is a schematic flowchart of the implementation steps of the method of the present invention.

[0023] Figure 2 It is a block diagram of the force and position parallel control strategy of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0024] The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the drawings in the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, rather than all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present invention.

[0025] Referring to Figure 1 As shown, the present invention provides a force-position hybrid interpolation method for motion control, including: Step S1: Based on the real-time feedback data of the six-axis force sensor, calculate the normal vector and tangent vector of the contact surface, and construct a moving frame (C; T, B, n), where T is the trajectory tangent vector, n is the surface normal vector, and B is the cross product of T and n.

[0026] It should be noted that the end of the robot is equipped with a high-precision six-axis force / torque sensor (such as the M4325K from Sunriseinstruments), which can capture subtle changes in contact force in real time. The sensor data is transmitted to the host computer through a data acquisition card (DAQ) at a sampling frequency of 1kHz to ensure the real-time performance of the control system. The end effector adopts a spherical contact design (such as a diameter of 10mm) or a cylindrical design (such as a flap wheel grinding head with a diameter of 22mm), and is connected to the sensor through a rigid link to minimize the impact of mechanical deformation on force transmission. The constraint plane H is defined by a virtual coordinate system, and its normal vector is aligned with the Z axis of the robot base coordinate system to ensure the geometric constraints of the motion trajectory.

[0027] In the data preprocessing stage, the sensor raw data is first processed by Kalman filtering to eliminate the interference caused by environmental noise and mechanical vibration. n The calculation of is based on the normalized resultant force of the sensor's Z-direction component and the friction force. The friction force f is determined by the vector synthesis of the X / Y-direction components within plane H. The direction of the tangent vector T is determined by the geometric relationship between the friction force direction and plane H. It is specifically achieved by calculating the angle α between the tangent vector and the horizontal axis of plane H. The formula for calculating the angle α is: Among them F p is the projection of the normal force in plane H, F XH and F ZH are the force components in the X and Z directions of the sensor.

[0028] The binormal B of the moving frame is generated by the cross product of T and n (B = T × n), forming a complete orthogonal coordinate system (C; T, B, n). During the movement, the frame is dynamically updated with the contact point C, and its derivative is related to the surface normal curvature k through the Darboux equation. n and geodesic torsion τ g , providing a geometric benchmark for trajectory prediction.

[0029] To ensure geometric consistency of the framework, the system uses dynamic calibration to eliminate the effects of arm joint errors on coordinate system transformation. For example, arm joint angle errors are compensated using an inverse kinematics model, and sensor mounting offsets are corrected through offline calibration. Furthermore, the system monitors contact point position offsets in real time. When the offset exceeds a preset threshold (e.g., 0.5 mm), a dynamic recalibration process is triggered to recalculate the initial parameters of the moving framework.

[0030] In a specific embodiment, in step S1, the surface normal vector is calculated as follows:

[0031] Among them F p is the projection of the normal force in plane H, F YHThe feedback force of the sensor in the Y direction of H, α is the angle between the tangent vector and the horizontal axis of the plane H, and F is in the denominator n The acting position is the normalized n.

[0032] In the present invention, a moving frame including a tangent, a normal, and a binormal is constructed to achieve parallel control of force and position. Through a hierarchically designed hybrid interpolator, the position increment is decomposed into a force control direction (normal) and a position control direction (tangent). The PID force compensation algorithm and the position servo control algorithm are respectively used to make the force control and the position control operate independently in the orthogonal space, avoid mutual interference, achieve independent decoupling and collaborative optimization of force and position, and significantly improve the force-position collaborative control accuracy and stability of the robot in complex tasks.

[0033] Step S2: According to the planned moving speed v d , and the interpolation period T intp , calculate the tangential feed Δs, Δs = v d T intp , calculate the surface normal curvature k n and the geodesic torsion τ g according to the derivative of the moving frame, and combine the Taylor expansion model and the tangential feed Δs to predict the tangential direction position control increment within each interpolation period.

[0034] As Figure 2 shown, based on the geometric characteristics of the moving frame, the system uses a dynamic model to predict the position increment of the next motion step and suppresses the trajectory deviation through a force compensation mechanism. The extraction of surface geometric characteristics is the core of this step, and the normal curvature k n and the geodesic torsion τ g are obtained in real time by calculating the derivative of the moving frame. The normal curvature k n characterizes the degree of bending of the surface along the tangent vector T, and the geodesic torsion τ g describes the torsional deformation characteristics of the surface. The Taylor expansion model is used to predict the next position C(s0 + Δs), and its expression is the superposition of the current position C(s0), the tangential displacement Δs, the normal curvature compensation term, and the PID force compensation amount ΔN m . The tangential displacement Δs is determined by the preset speed (such as 25 mm / s) and the interpolation period (such as 10 μs), and the normal curvature compensation term corrects the influence of the trajectory curvature through k n .

[0035] In a specific embodiment, in the step S2, the tangential direction position control increment is:

[0036] ΔsT(s0) + k(s0)(Δs) 2 N(s0) / 2, where T(s0) is the tangential direction of the motion trajectory, k(s0) is the curvature of the motion trajectory, and N(s0) is the projection of the normal vector n in the plane H.

[0037] Step S3: According to the normal contact force F given by the plan d , and the difference from the actual normal contact force, within each interpolation cycle, call the force control servo algorithm to calculate the corresponding position change amount, which is the normal force control increment ΔN m .

[0038] Based on the prediction of the position increment, the system further introduces a force compensation mechanism to suppress the trajectory deviation caused by the non-ideal surface characteristics. The PID controller is used to adjust the contact force in real time, where the proportional term quickly responds to the force deviation, the integral term eliminates the steady-state error, and the derivative term suppresses high-frequency oscillations. This compensation amount is superimposed on the prediction model to dynamically correct the control output and achieve stable control of the contact force (the fluctuation is controlled within ±1N). In addition, the system also introduces an adaptive correction mechanism for the friction coefficient μ. When the system detects that the trajectory prediction error continues to increase, an iterative algorithm will be triggered to dynamically adjust μ. The preset μ0 is used in the initial stage and is gradually optimized according to the actual contact feedback later to adapt to the changes in the surface roughness of different materials, thereby enhancing the robustness and adaptability of the system in complex environments.

[0039] In a specific embodiment, in the step S3, the normal force control increment ΔN m is:

[0040] ΔN m =(K p +K i T intp ∑(F d -F n )-K d (F d -F n ) / T intp )N(s0) / <N(s0),n>, where ΔN m is the PID compensation amount calculated based on the force error, F d represents the target contact force, F n represents the actual contact force, and K p , K i , K d represent the proportional coefficient, integral coefficient, and differential coefficient respectively.

[0041] Step S4: Design a force-position hybrid interpolator, and perform vector superposition of the tangential position control increment and the normal force control increment ΔN m inside the interpolator. The superposed vector generates the movement amounts of each axis of the robot through two-layer interpolation of coarse and fine.

[0042] In a specific embodiment, in the step S4, the hybrid interpolator includes a two - level structure of rough interpolation and fine interpolation. The interpolation period of the rough interpolation is several integer times that of the fine interpolation. The rough interpolation calculates the pose increment within each interpolation period in the Cartesian space of the robot, and calculates the movement amounts of each axis of the robot through the inverse kinematics of the robot. The fine interpolation further subdivides the movement amounts of each axis within the fine interpolation period to control the movement of each axis of the robot.

[0043] The hybrid interpolator adopts a hierarchical architecture design to achieve independent decoupling and collaborative optimization of force control and position control. The rough interpolation layer runs on a real - time system (Windows + RTX) with a period of 400 μs, and is responsible for decomposing the position increment dC = ΔsT + kΔs 2 N / 2 + ΔN m into joint motion commands. For example, the tangential displacement Δs is converted into pulse signals of the X / Y - axis servo motors, and the normal compensation amount ΔN m is mapped to the Z - axis position adjustment. In the trajectory planning stage, the rough interpolator combines a dynamic obstacle detection algorithm to adjust the path in real time to avoid collisions. For example, when the sensor detects a sudden external force interference, the position control command is immediately paused and the safety protocol is triggered to prevent mechanical damage.

[0044] The fine interpolation layer is implemented by FPGA hardware, and the period is shortened to 20 μs to perform micron - level correction on the rough interpolation commands. The motor response delay (such as the elastic deformation of the transmission chain) is predicted through feed - forward compensation, and the actual position deviation is corrected through feedback regulation (such as encoder data). The fine interpolator uses the look - up table method to pre - store the motor non - linear error model and compensates for the transmission clearance in real time. For example, under high - speed motion, the motor response lag is compensated in advance through a feed - forward gain (such as 0.95) to ensure that the end - effector tracking accuracy is better than 0.1 mm.

[0045] The force control loop and the position control loop operate independently in the orthogonal space. The force control loop focuses on normal force tracking and adopts a closed - loop control strategy; the position control loop is responsible for tangential trajectory tracking and adopts a feed - forward - feedback composite control strategy. For example, in tangential motion, the feed - forward module generates a theoretical control quantity according to the preset acceleration, and the feedback module corrects the actual position deviation through encoder data. The two loops interact data in real time through shared memory to ensure the synchronization of collaborative work.

[0046] In the present invention, the normal vector and tangential vector of the contact surface are calculated in real time by using the feedback data of the six - axis force sensor, and the position increment of the next movement step is predicted by combining the Taylor expansion model and the surface geometric characteristics, effectively improving the trajectory prediction accuracy. At the same time, based on the distribution characteristics of the normal curvature and geodesic torsion, the interpolation parameters are dynamically adjusted to optimize the trajectory planning; it can also complete the global surface reconstruction within the constraint plane H and output a high - precision motion trajectory, improving the robot's adaptive ability and tracking accuracy for unknown surfaces and meeting the requirements of industrial - level precision operations.

[0047] Step S5: Record the pose of the robot within the constraint plane H in each interpolation cycle, output a high-precision motion trajectory, and complete surface reconstruction.

[0048] In a specific embodiment, in step S5, for the rotation plane, within the constraint plane H, by collecting the surface normal curvature k n and geodesic torsion τ g data of multiple motion trajectories, a global geometric feature dataset is constructed; based on the distribution law of k n in the dataset and the change trend of τ g , the trajectory points conforming to the rotational symmetry characteristics are screened, and the screening condition is that the fluctuation of the k n value on the same trajectory is less than the preset threshold and the change trends of τ g of adjacent trajectories are consistent; the surface contour curve is fitted by an optimization algorithm, and the contour shape is determined with the goal of minimizing the error between the normal curvature and the theoretical radius; finally, the fitted two-dimensional contour is rotated around the axis of symmetry to generate a three-dimensional surface model, which is mapped to the robot motion coordinate system, and a continuous and smooth motion trajectory is output. By fusing the geometric feature data of multiple trajectories and combining statistical analysis and optimization algorithms, the global reconstruction of the unknown rotationally symmetric surface is realized.

[0049] Within the constraint plane H, the system completes the global modeling of the unknown surface through multi-trajectory data fusion. In the data acquisition stage, the normal curvature k n and geodesic torsion τ g are collected along different paths (such as helical lines, radial scans), and a high-density dataset covering the contact area is constructed. Through statistical analysis, the data points with the k n value decreasing with the radius and τ g being highly correlated are screened out and determined as rotationally symmetric surfaces. The least squares method is used for contour curve fitting, and the optimization goal is to minimize the sum of the squares of the errors between the normal curvature k n of each point and the theoretical radius 1 / R. For example, for the blade surface data, the maximum error of the fitted curve is 0.18 mm, and the standard deviation is 0.05 mm. In the three-dimensional model generation stage, the fitted two-dimensional contour curve is rotated around the axis of symmetry to generate a three-dimensional surface model, which is mapped to the robot base coordinate system {W} through coordinate transformation, and a continuous and non-mutating motion trajectory is output. Experiments show that the error between the reconstructed surface and the real contour is less than 0.2 mm, meeting the requirements of precision assembly. In actual industrial applications, the system can also combine visual sensors for multi-modal data fusion. For example, surface point cloud data is obtained through a laser scanner and used for complementary verification with the force control reconstruction model to further improve the modeling accuracy.

[0050] The above content is only an example and illustration of the concept of the present invention. Those skilled in the art of the present technology can make various modifications or supplements to the described specific embodiments or use similar ways to replace them. As long as they do not deviate from the concept of the invention or exceed the scope defined by the present invention, they should fall within the protection scope of the present invention.

Claims

1. A force-position hybrid interpolation method for motion control, characterized in that Including: Step S1: Based on the real-time feedback data of the six-axis force sensor, calculate the normal vector and tangent vector of the contact surface, and construct a moving frame (C; T, B, n), where T is the trajectory tangent vector, n is the surface normal vector, and B is the cross product of T and n; Step S2: According to the given moving speed v in the plan d , and the interpolation period T intp , calculate the tangential feed Δs, Δs = v d T intp , calculate the surface normal curvature k according to the derivative of the moving frame n and the geodesic torsion τ g , and combine the Taylor expansion model and the tangential feed Δs to predict the tangential direction position control increment in each interpolation period; Step S3: According to the normal contact force F given by the plan d , and the difference from the actual normal contact force, within each interpolation cycle, call the force control servo algorithm to calculate the corresponding position change amount, which is the normal force control increment ΔN m ; Step S4: Design a force-position hybrid interpolator, and perform vector superposition of the tangential direction position control increment and the normal direction force control increment ΔN within the interpolator. The superposed vector generates the movement amounts of each axis of the robot through two-layer interpolation of rough and fine interpolation; m ​ Step S5: In each interpolation cycle, record the pose of the robot in the constraint plane H, output a high-precision motion trajectory, and complete surface reconstruction.

2. A force-position hybrid interpolation method for motion control according to claim 1, characterized in that In the said Step S1, the calculation formula of the surface normal vector is: where F p is the projection of the normal force in the plane H, and F YH is the feedback force of the sensor in the Y direction of H, α is the angle between the tangent vector and the horizontal axis of the plane H, and the denominator F n acts at the normalized n.

3. A force-position hybrid interpolation method for motion control according to claim 1, characterized in that, In the said Step S2, the tangential position control increment is: ΔsT(s0)+k(s0)(Δs) 2 N(s0) / 2, where T(s0) is the tangent direction of the motion trajectory, k(s0) is the curvature of the motion trajectory, and N(s0) is the projection of the normal vector n in the plane H.

4. A force-position hybrid interpolation method for motion control according to claim 1, characterized in that, In the step S3, the control increment ΔN of the normal direction force m is as follows: ΔN m = (K p + K i T intp ∑(F d - F n ) - K d (F d - F n ) / T intp ) N(s0) / <N(s0), n>, where ΔN m is the PID compensation amount calculated based on the force error, F d represents the target contact force, F n represents the actual contact force, K p , K i , K d represent the proportional coefficient, integral coefficient, and differential coefficient, respectively.

5. A force-position hybrid interpolation method for motion control according to claim 1, characterized in that In the said Step S4, the hybrid interpolator includes a two-stage structure of rough interpolation and fine interpolation. The interpolation cycle of rough interpolation is several integer multiples of the interpolation cycle of fine interpolation. Rough interpolation calculates the pose increment within each interpolation cycle in the Cartesian space of the robot, and calculates the movement amount of each axis of the robot through the inverse kinematics of the robot. Fine interpolation further subdivides the movement amount of each axis within the fine interpolation cycle to control the movement of each axis of the robot.

6. The force-position hybrid interpolation method for motion control according to claim 1, wherein In the step S5, for the rotation plane, within the constraint plane H, by collecting the surface normal curvature k n and the geodesic torsion τ g data, a global geometric feature dataset is constructed; based on the distribution law of k n in the dataset and the changing trend of τ g , the trajectory points conforming to the rotational symmetry characteristics are screened, and the screening condition is that the fluctuation of the k n value on the same trajectory is less than the preset threshold and the changing trends of τ g of adjacent trajectories are consistent; Optimize the algorithm to fit the surface contour curve, determine the contour shape with the goal of minimizing the error between the normal curvature and the theoretical radius; finally, rotate the fitted two-dimensional contour around the symmetry axis to generate a three-dimensional surface model, map it to the robot motion coordinate system, output a continuous and smooth motion trajectory, and realize the global reconstruction of the unknown rotationally symmetric surface by fusing the geometric feature data of multiple trajectories and combining statistical analysis and optimization algorithms.

Images