A trajectory generation method and system for multi-degree-of-freedom target calibration

Abstract

The application discloses a trajectory generation method and system for multi-degree-of-freedom target calibration, belongs to the technical field of motion pose measurement, obtains a target trajectory, scales the target trajectory based on an adaptive scaling algorithm to map the target trajectory into a device workspace, and keeps the geometric features and motion characteristics of the target trajectory. Analyzing inverse kinematics, inversely mapping the scaled target trajectory to an actuator space, discretizing the continuous trajectory into a time sequence, obtaining an actuator instruction sequence, and compensating the control instructions of each actuator. Through the cooperation of trajectory scaling, inverse solution optimization and error compensation, the application realizes the guarantee of calculation accuracy while meeting the real-time control requirements, and has good practicability.

Description

Technical Field

[0001] This invention belongs to the technical field of spatial attitude measurement, specifically relating to a trajectory generation method and system for multi-degree-of-freedom target calibration. Background Technology

[0002] With the rapid development of artificial intelligence devices, strategic emerging industries such as high-end equipment manufacturing, aerospace, and semiconductors are increasingly demanding precision dynamic measurement. Among these, motion pose (position and attitude) composite parameters, as core values ​​ensuring equipment accuracy and reliability, directly determine the quality of R&D and manufacturing of high-end products through their metrological technology. A trajectory generator is a metrological instrument used in mechanical engineering, navigation simulation, and other fields. Its main function is to generate simulated motion trajectories for calibrating the dynamic performance of devices such as laser trackers, or as an important simulation tool for verifying inertial navigation and its integrated navigation system algorithms. Summary of the Invention

[0003] The purpose of this invention is to provide a trajectory generation method and system for multi-degree-of-freedom target calibration. By introducing an adaptive scaling algorithm, the adaptiveness and reliability of trajectory generation are achieved, and the control accuracy of the overall trajectory is improved through error compensation, which has good practicality.

[0004] This invention is mainly achieved through the following technical solutions:

[0005] A trajectory generation method for multi-degree-of-freedom target calibration includes the following steps:

[0006] Step S1: Obtain the target trajectory ;

[0007] Step S2: Scale the target trajectory based on the adaptive scaling algorithm to map the target trajectory into the device workspace while preserving the geometric features and motion characteristics of the target trajectory;

[0008] Step S3: Inverse kinematics analysis, the scaled target trajectory is inversely mapped to the actuator space, the continuous trajectory is discretized into a time series, and the actuator instruction sequence is obtained;

[0009] ;

[0010] in: : Executable pose and position;

[0011] X-axis position command sequence;

[0012] : Horizontal corner turning command sequence;

[0013] Vertical cornering command sequence;

[0014] Z-axis altitude command sequence;

[0015] : Slewing arm length command sequence;

[0016] Vibration parameter command sequence;

[0017] Step S4: Error compensation control commands for each actuator.

[0018] To better realize the present invention, step S2 further includes the following steps:

[0019] Step S21: Determine the workspace boundary based on the physical constraints of the actuator; then, let the original target trajectory point set be... and define the trajectory bounding box;

[0020] in: : The i-th trajectory point of the target trajectory;

[0021] Step S22: Construct the scaled target trajectory;

[0022] ;

[0023] in: : The i-th scaled trajectory point;

[0024] Workspace Center;

[0025] Scaling factor ;

[0026] Anisotropic scaling matrix;

[0027] : The center point of the bounding box of the trajectory;

[0028] Step S23: Based on the target trajectory, solve for the maximum scaling factor. This ensures that the scaled trajectory is entirely within the workspace;

[0029] ;

[0030] The maximum scaling factor is obtained by solving the problem. for:

[0031] ;

[0032] Where: W: is the set of all points that can be reached by the trajectory generator;

[0033] : The set of positive real numbers;

[0034] : All points on the trajectory are in the direction The maximum positive offset on;

[0035] : All points on the trajectory are in the direction The maximum negative offset on;

[0036] Workspace in direction The offset between the upper limit boundary and the center point;

[0037] Workspace in direction The offset between the lower boundary and the center point;

[0038] : Direction sign factor.

[0039] To better implement the present invention, further, in step S3, the discretization time... And at each moment, the length of the rotating arm is calculated. :

[0040] ;

[0041] ;

[0042] ;

[0043] ;

[0044] ;

[0045] ;

[0046] The solution yields: ;

[0047] in: , , Weighting coefficients;

[0048] Instruction cycle;

[0049] : The length of the rotary arm in the k-th instruction cycle;

[0050] : The length of the rotary arm in the (k-1)th instruction cycle;

[0051] : Length of the rotary arm in the 0th command cycle;

[0052] The command position of the air-floating platform on the air-floating guide rail during the k-th command cycle;

[0053] The position of the X-axis in the k-th instruction cycle;

[0054] The position of the Y-axis in the k-th instruction cycle;

[0055] The Z-axis position in the k-th instruction cycle;

[0056] : The length of the Z-axis telescopic arm in the k-th instruction cycle;

[0057] : Z-axis telescopic arm length in the 0th command cycle;

[0058] , These represent the nearest and farthest locations that the air flotation platform can reach, respectively.

[0059] , These represent the nearest and farthest positions that the height connector can reach, respectively.

[0060] , These represent the closest and furthest positions that the slewing arm can reach, respectively.

[0061] To better realize the present invention, step S4 further includes the following steps:

[0062] Step S41: Establish a dynamic prediction and compensation model;

[0063] Step S42: Obtain the attitude position of dynamic prediction compensation through offline calibration;

[0064] Step S43: In the online compensation stage, the actuator commands are corrected in real time based on the dynamically predicted and compensated attitude position.

[0065] The attitude position of dynamic prediction compensation includes X-axis position, horizontal rotation angle, vertical rotation angle, telescopic arm length, and slewing arm length.

[0066] Step S43: In the online compensation stage, the actuator instructions are corrected in real time based on the precise parameters of each error.

[0067] To better realize the present invention, further, in step S41, the dynamic prediction compensation model is as follows:

[0068] ;

[0069] ;

[0070]

[0071] in: The attitude position of the dynamic prediction compensation in the k-th period;

[0072] The theoretical command position given in the linear compensation stage of the k-th cycle;

[0073] The theoretical command position given in the linear compensation stage of the (k-1)th cycle;

[0074] Geometric error compensation for position;

[0075] Thermal error compensation for location;

[0076] Regularization coefficient;

[0077] : Temperature change in the kth period;

[0078] J: Error propagation Jacobian matrix;

[0079] G(): Geometric error transfer function;

[0080] T(): Temperature error transfer function;

[0081] : The dynamic adjustment coefficient for the theoretical command value, default value is 1;

[0082] Instruction cycle.

[0083] To better realize the present invention, further, in step S4, after error compensation, trajectory verification is performed to measure the high-frequency vibration amount. The final terminal trajectory is obtained by synthesizing the macroscopic trajectory at the end. :

[0084] ;

[0085] like Then the output will be:

[0086] ;

[0087] in: : The scaled trajectory point of the k-th instruction cycle;

[0088] : The X-axis position in the k-th instruction cycle;

[0089] : The horizontal rotation angle of the k-th instruction cycle;

[0090] : The vertical rotation angle of the k-th instruction cycle;

[0091] : The length of the Z-axis telescopic arm in the k-th instruction cycle;

[0092] : The length of the rotary arm in the k-th instruction cycle;

[0093] : No. The amount of vibration at any given moment;

[0094] : The amount of vibration in the k-th instruction cycle;

[0095] Deviation threshold;

[0096] : No. The executable pose and position at any given moment;

[0097] : No. The macroscopic trajectory attitude and position at any given moment.

[0098] This invention is mainly achieved through the following technical solutions:

[0099] A trajectory generation system for multi-degree-of-freedom target calibration, which implements the above-mentioned trajectory generation method for multi-degree-of-freedom target calibration, includes a target trajectory acquisition module, a trajectory scaling module, an inverse kinematics module, and an error compensation module;

[0100] The target trajectory acquisition module is used to acquire the target trajectory and perform temporal decomposition; the trajectory scaling module is used to scale the target trajectory based on an adaptive scaling algorithm; the inverse kinematics module is used to analyze the scaled target trajectory using inverse kinematics and obtain the actuator instruction sequence; the error compensation module is used to compensate for errors in the control instructions of each actuator.

[0101] The beneficial effects of this invention are as follows:

[0102] (1) This invention achieves real-time control requirements while ensuring computational accuracy through the synergy of trajectory scaling, inverse kinematics optimization, and error compensation. The final output command sequence can directly drive each servo motor, ensuring the coordinated operation of the entire kinematic chain and generating a high-fidelity spatial attitude trajectory. Secondly, the error processing and control strategy of this invention is the core technical support for a high-precision attitude calibration benchmark.

[0103] (2) By introducing an adaptive scaling algorithm, this invention can automatically detect whether the target trajectory exceeds the workspace boundary mathematically, and map it to the reachable area of ​​the device through anisotropic scaling, while preserving the geometric features of the original trajectory as much as possible. This mechanism enables this invention to handle arbitrarily complex target trajectories without requiring manual adjustment by the user, greatly improving the versatility and ease of use of the system.

[0104] (3) This invention constructs an optimization objective function (such as minimizing energy consumption, maintaining joint midpoint, and avoiding singularities) to enable the inverse kinematics algorithm to select the optimal actuator instruction sequence from an infinite number of solutions. This optimization not only ensures the smoothness and continuity of motion but also avoids the potential damage to the system caused by mechanical singular configurations, while improving dynamic response performance. The reasonable use of redundancy allows the system to balance multiple indicators such as energy consumption, speed, and stability while meeting positional accuracy requirements.

[0105] (4) This invention establishes a dynamic prediction and compensation model that includes geometric errors, thermal deformation errors, and dynamic errors, and obtains accurate parameters for each posture through offline calibration. During the online compensation stage, these parameters are used in real time to correct actuator commands, thereby effectively offsetting the inherent imperfections of the mechanical system (such as guide rail straightness errors, turntable shaft runout, and temperature-induced deformation). This strategy of combining feedforward compensation with feedback control makes it possible for the actual end trajectory to closely match the theoretical trajectory, significantly improving the dynamic and static accuracy of the system and demonstrating good practicality. Attached Figure Description

[0106] Figure 1 This is a flowchart of the trajectory generation method for multi-degree-of-freedom target calibration according to the present invention. Detailed Implementation

[0107] Example 1:

[0108] A trajectory generation method for multi-degree-of-freedom target calibration, such as Figure 1 As shown, it includes the following steps:

[0109] Step S1: Obtain the target trajectory ;

[0110] In practice, the user inputs the target attitude, which is then converted into a homogeneous transformation matrix and decomposed into time functions of basic motion parameters according to the time series, thereby obtaining the target trajectory, which specifically includes a sequence of position functions, a sequence of velocity functions, a sequence of acceleration functions, a sequence of angular motion parameters, a sequence of angular velocity, and a sequence of angular acceleration.

[0111] Step S2: Trajectory scaling; The target trajectory is scaled based on an adaptive scaling algorithm to map the target trajectory into the device workspace while preserving the geometric features and motion characteristics of the target trajectory;

[0112] Specifically, the target pose is scaled based on an adaptive scaling algorithm to obtain an executable trajectory generation model. By introducing an adaptive scaling algorithm, it is detected whether the target trajectory exceeds the workspace boundary, and it is mapped to the device reachable area through anisotropic scaling, while preserving the geometric features and motion characteristics of the original trajectory as much as possible.

[0113] Step S3: Inverse kinematics analysis; the scaled target trajectory is inversely mapped to the actuator space, and the continuous trajectory is discretized into a time series to obtain the actuator instruction sequence;

[0114] Specifically, inverse kinematics optimization can be further performed. By constructing an optimization objective function, the spatial trajectory is decomposed into temporal sequences and transformed into control instructions for each actuator. The optimal actuator instruction sequence is then selected based on the inverse kinematics algorithm.

[0115] Step S4: Error compensation; error compensation of control commands for each actuator.

[0116] Specifically, the joint space trajectory obtained by inverse kinematics is converted into control commands for each actuator, and the effects of dynamics and environmental factors are compensated.

[0117] Preferably, the specific content of step S1 is as follows:

[0118] 1.1 Degrees of freedom target attitude description;

[0119] The target pose input by the user is represented in the global coordinate system {O} as a time-varying homogeneous transformation matrix. :

[0120] ;

[0121] Wherein: the user input is a target pose sequence that changes over time t, and a complete 6-DOF motion pose can be represented by a homogeneous transformation moment. To express;

[0122] Rotation matrix ; where SO(3): group structure, forming a Lie group, whose Lie algebra SO(3) is composed of a 3×3 skew-symmetric matrix and is associated with the angular velocity vector through the "hat operation".

[0123] : Position vector; ;

[0124] , , These are the position vector components along the x, y, and z axes at time t;

[0125] It is a 1×3 zero vector, ensuring matrix dimension matching;

[0126] :time;

[0127] Rotate the matrix elements to satisfy the orthogonality condition. , ,

[0128] : Identity matrix.

[0129] 1.2 Decomposition of kinematic quantities;

[0130] 1.2.1 Sequence of positional functions;

[0131] ;

[0132] in: : The time function of the position of the end mirror;

[0133] : The instantaneous velocity of the end reflector;

[0134] : an extremely short time, which in actual engineering is the minimum resolution time of the instrument;

[0135] : The position of the end reflector at the initial moment.

[0136] 1.2.2 Velocity function sequence;

[0137] ;

[0138] in: : The time function of the velocity of the end mirror;

[0139] The first derivative of the position of the end mirror;

[0140] The instantaneous acceleration of the end reflector;

[0141] , , : These are the time components of the velocity of the end mirror in x, y, and z, respectively;

[0142] : The velocity of the end mirror at the initial moment.

[0143] 1.2.3 Acceleration function sequence;

[0144] ;

[0145] in: : The time function of the acceleration of the end-mirror;

[0146] The second derivative of the velocity position of the terminal reflecting mirror;

[0147] , , : These are the time components of the acceleration of the end mirror in x, y, and z, respectively.

[0148] 1.2.4 Angular motion parameter sequence;

[0149] Euler angles for:

[0150] (ZYX rotation sequence);

[0151] in: , , These are roll angle, pitch angle, and yaw angle, respectively, in radians (rad).

[0152] : Matrix transpose.

[0153] 1.2.5 Angular velocity sequence;

[0154] angular velocity vector ;

[0155] in: , , : These are the components of the angular velocity vector in x, y, and z, respectively;

[0156] Antisymmetric matrix of angular velocity vector With rotation matrix The derivative relationship is:

[0157] ;

[0158] .

[0159] 1.2.6 Angular acceleration sequence;

[0160] ;

[0161] in: Angular acceleration.

[0162] 1.3 Vibration quantity sequence;

[0163] ;

[0164] in: Vibration amount;

[0165] , , : These are the components of the vibration in x, y, and z, respectively;

[0166] , , : These are the components of amplitude in x, y, and z, respectively;

[0167] , , : These are the components of frequency in x, y, and z, respectively;

[0168] , , : These are the components on phases x, y, and z, respectively.

[0169] In summary, step S1 aims to decompose continuous, complex motion into time functions of basic physical quantities (position, velocity, acceleration, etc.), providing input for subsequent control of each actuator. Using a homogeneous transformation matrix (DH matrix) is a standard method for robotics and kinematic modeling because it uniformly handles rotation and translation.

[0170] Step S1 provides a complete and unambiguous mathematical description of complex spatial motion. By employing a homogeneous transformation matrix to uniformly represent the position and orientation of the rigid body, this method can simultaneously handle rotation and translation, avoiding the gimbaling problem that may be encountered in traditional Euler angle representation, and eliminating ambiguity caused by the rotation order. This unified and rigorous representation provides a reliable mathematical benchmark for all subsequent coordinate transformations, error analysis, and motion synthesis, ensuring that every step, from the complex requirements input by the user to the internal processing of the system, has clear physical meaning.

[0171] Secondly, this method decomposes complex motion trajectories into time functions of a series of fundamental motion parameters, including linear displacement, linear velocity, linear acceleration, angular displacement, angular velocity, angular acceleration, and independently controllable high-frequency vibrations. This decomposition makes each physical quantity an object that can be independently analyzed, measured, and controlled. For example, linear velocity can be directly mapped to the speed of the X-axis motor, angular velocity can be precisely controlled by the turntable speed, and vibrations provide an independent excitation source for dynamic response testing. More importantly, these fundamental parameters all have clear International System of Units (SI) traceability paths, making the metrological benchmarks of the entire calibration system reliable and traceable, fundamentally guaranteeing the scientific validity and credibility of the calibration results.

[0172] The introduction of time-series representation endows the system with powerful dynamic characteristic analysis capabilities. Representing motion as a continuously varying function over time allows us to obtain derived quantities such as velocity and acceleration through numerical differentiation or integration, thereby comprehensively evaluating the performance of the instrument under calibration at different stages of motion. Simultaneously, this representation facilitates the alignment of reference and measurement data over time, enabling point-by-point comparisons and revealing the error distribution characteristics over time, thus identifying systematic problems such as periodic errors and trend terms. Furthermore, frequency domain analysis tools such as Fourier transforms can convert the time series into frequency domain information, evaluating the instrument's response to specific frequency components, which is particularly important for dynamic calibration.

[0173] Furthermore, treating vibration as an independent parameterized component is another key innovation of this step. High-frequency vibration is represented as simple harmonic motion with programmable amplitude, frequency, and phase, and processed separately from macroscopic motion. This design allows the system to superimpose known dynamic disturbances on precisely controlled large-stroke motions, simulating complex environments under real-world conditions. The independently parameterized vibration component not only facilitates the automated execution of dynamic tests but also provides precise and controllable input conditions for evaluating the dynamic response characteristics of the calibrated instrument (such as frequency response curves and amplitude linearity), greatly expanding the application range of the calibration system.

[0174] Finally, this parametric decomposition lays a solid mathematical foundation for subsequent inverse kinematics solutions, error compensation, and full system trajectory synthesis. Once the target trajectory is clearly expressed as a time series, it can be directly used as input to the inverse kinematics algorithm to drive the coordinated motion of each actuator. The decomposability of each basic motion parameter also allows for refined error compensation targeting specific values, such as compensating for position bias, velocity scale factors, and attitude misalignment. This progressive and logically rigorous mathematical framework ensures that the entire process, from user requirements to actual motion, is controllable, computable, and optimizable.

[0175] Preferably, the specific content of step S2 is as follows:

[0176] 2.1 Theoretical basis of trajectory scaling;

[0177] 2.1.1 Establishment of the mathematical framework;

[0178] Establish a mathematical mapping framework from the target trajectory to the device-reachable trajectory to ensure that any input trajectory can be realized within the physical constraints of the device.

[0179] Basic principle: The original target trajectory is mapped into the device workspace through a scaling function, preserving the geometric features and motion characteristics of the trajectory.

[0180] The mathematical definition is as follows:

[0181] 1) Original target trajectory:

[0182] ;

[0183] in: : Target time-series trajectory containing spatial attitude;

[0184] The time series component of the target trajectory on the X-axis in the world coordinate system (Cartesian coordinate system);

[0185] The time-series component of the target trajectory on the Y-axis in the world coordinate system (Cartesian coordinate system);

[0186] The time series component of the target trajectory on the Z-axis in the world coordinate system (Cartesian coordinate system);

[0187] : Three-dimensional space based on Cartesian coordinates;

[0188] Time series;

[0189] : The total time length of the target time series trajectory, including spatial attitude.

[0190] 2) The space that the trajectory generator can reach;

[0191] ;

[0192] in: : The value of the air-floating platform on the x-axis guide rail when the equipment is at point P;

[0193] : The angle of the horizontal turntable when the equipment is at point P;

[0194] : The angle of the vertical turntable when the equipment is at point P;

[0195] : The height of the lifting mechanism (extension) when the equipment is at point P;

[0196] : The length of the slewing arm (extension) when the equipment is at point P;

[0197] The points that the trajectory generator can reach;

[0198] : The set of all points p that can be reached by the trajectory generator, located in three-dimensional real space;

[0199] : Mathematical symbols exist;

[0200] : The set of actuator constraints for the trajectory generator.

[0201] 3) Define the scaling function;

[0202] ;

[0203] ;

[0204] ;

[0205] in: : The defined scaling function;

[0206] : The scaled target trajectory;

[0207] Workspace Center;

[0208] : The set of positive real numbers;

[0209] The set of 3×3 real matrices;

[0210] Scaling factor ;

[0211] Anisotropic scaling matrix;

[0212] , , : These are the scaling factors for the x, y, and z axes, respectively.

[0213] 2.2 Mathematical expression of the adaptive scaling algorithm;

[0214] 2.2.1 Mathematical description of workspace boundaries;

[0215] The reachable workspace boundary of the device is precisely described using mathematical formulas. Specifically, based on the physical limitations of the actuator, the mathematical boundary conditions of the workspace are derived.

[0216] (1) The boundary conditions are derived as follows:

[0217] Constructing the final trajectory generation formula :

[0218] ;

[0219] The trajectory generator constraints include:

[0220] ;

[0221] ;

[0222] ;

[0223] ;

[0224] ;

[0225] in: , The air-floating platform can reach the nearest and farthest positions; the air-floating platform is used to adjust the position of the X-axis;

[0226] , The minimum and maximum values ​​of the horizontal rotating platform's rotation. , ;

[0227] , The minimum and maximum values ​​of rotation for a vertical rotating platform. , ;

[0228] , The telescopic boom can reach both the closest and furthest positions.

[0229] , The lifting mechanism (telescopic) can reach the nearest and farthest positions.

[0230] Specifically, the trajectory generator includes, from bottom to top, an X-axis moving platform, a horizontal circular surface generator, a lifting mechanism, a vertical circular surface generator, and a rotary arm. The X-axis moving platform includes a correspondingly arranged air-bearing guide rail and an air-bearing platform mounted on the guide rail, the air-bearing platform moving along the X-axis along the guide rail. A horizontal circular surface generator is located at the top of the air-bearing platform, used to rotate the end of the platform around the Z-axis. The top of the horizontal circular surface generator is connected to the vertical circular surface generator via the lifting mechanism, the vertical circular surface generator used to rotate the rotary arm around its central axis. The horizontal and vertical circular surface generators constitute an orthogonally combined two-dimensional turntable. The rotary arm is used to extend and retract the end of the platform radially.

[0231] (2) Workspace boundary function:

[0232] 1) Radial boundary;

[0233] ;

[0234] ;

[0235] 2) Axial boundary;

[0236] ;

[0237] ;

[0238] 3) X-direction boundary;

[0239] , ;

[0240] in: Radial boundary upper limit;

[0241] : Lower limit of radial boundary;

[0242] Upper limit of axial boundary;

[0243] Lower limit of axial boundary;

[0244] : Upper limit of the X-axis boundary;

[0245] : Lower limit of the X-direction boundary.

[0246] 2.2.2 Mathematical model for scaling factor calculation;

[0247] A mathematical model for calculating the optimal scaling factor is established. Specifically, by solving the optimization problem, the maximum scaling factor is found, ensuring that the scaled trajectory lies entirely within the workspace.

[0248] (1) Let the original target trajectory point set be Define the bounding box:

[0249] ;

[0250] ;

[0251] ;

[0252] in: : Lower limit of the original target trajectory point;

[0253] : Upper limit of the original target trajectory points;

[0254] : The center point of the bounding box of the trajectory.

[0255] (2) Scaled target trajectory:

[0256] ;

[0257] Constraints:

[0258] ;

[0259] in: : The i-th scaling trajectory point;

[0260] Each mathematical symbol.

[0261] (3) Optimize the formalization of the problem:

[0262] ;

[0263] in: : A symbol used to introduce constraints, indicating "restricted by" or "satisfying the following conditions". Its rules and usage methods need to be combined with the type of optimization problem (such as linear programming, nonlinear programming, etc.) and the rigor of the mathematical expression.

[0264] The analytical derivation of the above formula is as follows:

[0265] For each coordinate direction ,definition:

[0266] ;

[0267] Scaling factor constraint:

[0268] ;

[0269] ;

[0270] ;

[0271] ;

[0272] ;

[0273] ;

[0274] ;

[0275] ;

[0276]

[0277] In summary, the final optimized scaling factor for:

[0278] ;

[0279] in: : The projection of the offset of the i-th trajectory point relative to the trajectory center in the d direction;

[0280] The i-th point in the original trajectory (3D coordinates) );

[0281] : The center point of the bounding box of the trajectory;

[0282] Projection operation, indicating the projection along the coordinate direction. ( Component values ​​on )

[0283] : All points on the trajectory are in the direction The maximum positive offset on;

[0284] : All points on the trajectory are in the direction The maximum negative offset on;

[0285] Workspace in direction The offset between the upper limit boundary and the center point;

[0286] Workspace in direction The offset between the lower boundary and the center point;

[0287] : Direction sign factor; usually, the components in x, y, z satisfy: , , If the coordinate system is reversed, then the components are reduced to -1.

[0288] Preferably, the specific content of step S3 is as follows:

[0289] 3.1 Mathematical model for executable trajectory generation;

[0290] 3.1.1 Analytical solution for inverse kinematics;

[0291] The mathematical process of inversely mapping the scaled trajectory to the actuator space. Specifically, the mathematical transformation from Cartesian space to joint space.

[0292] The mathematical derivation is as follows:

[0293] Known scaled target trajectory ;

[0294] ;

[0295] Need to solve:

[0296] ;

[0297] Step 1: Define intermediate variables :

[0298] ;

[0299] but:

[0300] ;

[0301] ;

[0302] .

[0303] Step 2: Solve analytically;

[0304] (1) Horizontal turning angle The parsing expression:

[0305] ;

[0306] More generally, use the four-quadrant arctangent function:

[0307] ;

[0308] in: : Two-parameter arctangent function, where Return point The polar angle, with a range of values ​​of . .

[0309] The specific derivation is as follows:

[0310] ;

[0311] ;

[0312] Dividing the two equations above, we get:

[0313] ;

[0314] ;

[0315] in: : Extended coefficients, which are integers. To determine uniqueness Requires use The function takes quadrant information into account.

[0316] (2) Vertical turning angle The parsing expression:

[0317] ;

[0318] More generally, use the four-quadrant arctangent function:

[0319] ;

[0320] The specific derivation process is as follows:

[0321] ;

[0322] ;

[0323] Dividing the two equations above, we get:

[0324] ;

[0325] therefore:

[0326] .

[0327] (3) Solving the optimization problem and ;

[0328] System redundancy, define optimization objectives:

[0329] ;

[0330] in, ;

[0331] for The first derivative;

[0332] This is the initial value for the slewing arm;

[0333] J is the objective value of the optimization function;

[0334] The initial value for the Z-axis telescopic arm;

[0335] , , Weighting coefficients are used to balance the priorities of the three sub-objectives. .

[0336] (4) Instruction decomposition and instruction time series generation;

[0337] Detailed explanation of the principle: Discretizing a continuous trajectory into a time series satisfies the real-time requirements of the control system. Discretization is the foundation of digital control.

[0338] The instruction sequence includes:

[0339] : Command sequence for X-axis position;

[0340] : Command sequence for horizontal cornering;

[0341] : Command sequence for vertical cornering;

[0342] : Command sequence for the Z-axis telescopic arm length;

[0343] : Command sequence for slewing arm length;

[0344] : Command sequence for vibration parameters;

[0345] Time discretization:

[0346]

[0347] Instruction cycle choose:

[0348] ;

[0349] in: This represents the highest frequency component of the trajectory.

[0350] (5) Discretization solution:

[0351] Discretize time into Solve at each time step:

[0352]

[0353] in: , , Weighting coefficients are used to balance the priorities of the three sub-objectives. ;

[0354] k: Number of instruction cycles;

[0355] : The total time from the k-th instruction cycle to the initial time;

[0356] : The duration of a single instruction cycle;

[0357] : The length of the rotary arm in the k-th instruction cycle;

[0358] : The length of the rotary arm in the (k-1)th instruction cycle;

[0359] : Length of the rotary arm in the 0th command cycle;

[0360] The command position of the air-floating platform on the air-floating guide rail during the k-th command cycle;

[0361] The position of the X-axis in the k-th instruction cycle;

[0362] The position of the Y-axis in the k-th instruction cycle;

[0363] The Z-axis position in the k-th instruction cycle;

[0364] : The length of the Z-axis telescopic arm in the k-th instruction cycle;

[0365] : The length of the Z-axis telescopic arm in the 0th instruction cycle.

[0366] Preferably, the specific content of step S4 is as follows:

[0367] 4.1 Mathematical model for mechanical error compensation;

[0368] 4.1.1 Error model establishment;

[0369] Various mechanical errors are described using mathematical formulas. Specifically, this is based on multibody system theory and error modeling methods.

[0370] Error classification and modeling:

[0371] (1) Geometric error:

[0372] ;

[0373] in: : Geometric error vector caused by geometric changes;

[0374] : The error propagation Jacobian matrix (6×n dimensions) of the i-th kinematic pair, which describes the mapping relationship between the error Δθ_i of the kinematic pair and the terminal error;

[0375] : is the first The error parameter vector (n-dimensional) of a kinematic pair typically includes: angular error (such as bearing clearance, rotational error caused by assembly eccentricity), positional error (such as guide rail straightness error, assembly offset), and perpendicularity / parallelism error (unique to multi-degree-of-freedom systems).

[0376] (2) Thermal error:

[0377] ;

[0378] in: : The error vector of thermal deformation caused by temperature change;

[0379] Real-time temperature field (spatial temperature vector);

[0380] Reference temperature (usually 20℃ or design temperature);

[0381] : The length of the slewing arm;

[0382] A: Thermal expansion coefficient matrix (diagonal matrix), where the diagonal elements are the thermal expansion coefficients of anisotropic materials in different directions.

[0383] (3) Dynamic error :

[0384] ;

[0385] Where M is the system mass matrix (or inertia matrix), which reflects the inertial coupling characteristics between the system's degrees of freedom;

[0386] : is its inverse matrix, used to convert force into acceleration / displacement, usually with acceleration Related;

[0387] As an inertial force, the reaction force caused by acceleration is often related to the acceleration. Related;

[0388] Friction is a force that includes static friction, kinetic friction, and viscous friction, and is usually related to speed. Location Related to environmental factors (such as temperature and lubrication).

[0389] (4) Total error model:

[0390] ;

[0391] in: The geometric error transfer function describes the effect of joint angle error Δθ (such as machining error, assembly error) on the end-effector position. The impact, and will Map to the Jacobian matrix or error mapping matrix;

[0392] Joint angle error is caused by mechanical structural defects (such as gear backlash, transmission error), sensor noise, or control algorithm deviation.

[0393] Let be the temperature error transfer function. Mapped to the total thermal error; reflects the linear / nonlinear effect of temperature change ΔT (such as thermal expansion, material property changes) on the end position, often involving the thermoelastic coefficient;

[0394] For dynamic error transfer function, dynamic error is transferred. Mapped into the total error, its form may include location. speed acceleration Coupled terms (such as nonlinear stiffness and damping characteristics);

[0395] For position Velocity (first order) Acceleration (second order) .

[0396] 4.2.2 Error Model Establishment;

[0397] The mathematical implementation of the compensation algorithm. Specifically, feedforward compensation is combined with model prediction.

[0398] Compensation algorithm:

[0399] 1. Offline calibration stage:

[0400] ;

[0401] in: The actual (measured) trajectory (position vector) of the kth instruction cycle;

[0402] This is the theoretical trajectory (position vector) for the k-th instruction cycle.

[0403] Let be the geometric error transfer function for the k-th instruction cycle, describing the effect of joint angle error Δθ (such as machining error, assembly error) on the end-effector position. The impact, and will Map to the Jacobian matrix or error mapping matrix;

[0404] This represents the joint angle error.

[0405] 2. Online compensation phase:

[0406] Given ideal instructions Calculate the compensation instructions :

[0407] ;

[0408] ;

[0409] in: The required position compensation value is determined within a single instruction cycle.

[0410] These are the regularization coefficients obtained using the least squares method.

[0411] 3. Predictive compensation:

[0412] Consider dynamic effects:

[0413] ;

[0414] in: For the first The compensation instruction at that time;

[0415] For the first Ideal instructions at the time;

[0416] Joint variable vector;

[0417] Error parameters;

[0418] K is a dynamic adjustment coefficient, which defaults to 1.

[0419] 4.2 After error compensation, trajectory verification is performed;

[0420] A mathematical model for vibration synthesis is used. At the end point, high-frequency vibrations are synthesized into the trajectory. Specifically, the macroscopic trajectory (the end-effector attitude excluding vibrations) is separated from the high-frequency vibrations and synthesized at the end point to obtain the final end-effector trajectory. .

[0421] Let the macro trajectory be The vibration amount is Then the final terminal trajectory is obtained. :

[0422] ;

[0423] ;

[0424] Substitute specific parameters and expand the format:

[0425] ;

[0426] like Then the output will be:

[0427] .

[0428] in: The command position of the air-floating platform on the air-floating guide rail during the k-th command cycle;

[0429] : Horizontal rotation angle in the k-th instruction cycle;

[0430] Vertical rotation angle in the k-th instruction cycle;

[0431] : The length of the Z-axis telescopic arm in the k-th instruction cycle;

[0432] : The length of the rotary arm in the k-th instruction cycle;

[0433] : No. The amount of vibration at any given moment;

[0434] : The amount of vibration in the k-th instruction cycle;

[0435] Acceptable attitude position threshold.

[0436] : No. The executable pose and position at any given moment;

[0437] : No. The macroscopic trajectory attitude and position at any given moment.

[0438] Preferably, the vibration generation model is as follows:

[0439]

[0440]

[0441] ;

[0442] in: : The amplitude adjustment function for vibration quantity;

[0443] : The initial superposition of the basic amplitude;

[0444] Amplitude modulation refers to the fluctuation range of the amplitude, which controls the range of amplitude variation over time. The larger the amplitude, the more violent the amplitude fluctuations, such as the instantaneous amplitude surge caused by impact load;

[0445] : Frequency domain modulation function of vibration quantity;

[0446] The center frequency of vibration represents the natural vibration frequency of the system when it is not modulated.

[0447] The frequency modulation fluctuation range controls the amplitude of frequency change over time. The larger the f_m, the more significant the frequency shift, such as the natural frequency drift caused by changes in material stiffness due to temperature variations.

[0448] The periodic frequency of frequency modulation determines how the frequency fluctuates periodically over time.

[0449] The initial phase or phase shift of the vibration signal can be set as a constant or vary with time. The phase affects the starting point and shape of the vibration waveform. For example, in interferometry, phase shift may cause changes in the position of the peaks / troughs.

[0450] In summary, this invention transforms the joint space trajectory obtained from inverse kinematics into control commands for each actuator, and compensates for the influence of dynamics and environmental factors. This is the final step in achieving high-precision trajectory tracking.

[0451] Basic principle: Taking into account actuator dynamics, control bandwidth limitations, and environmental disturbances, a high-precision trajectory can be generated. Dynamic compensation can significantly improve tracking accuracy.

[0452] Actuator command generation and dynamic compensation are the final execution stages of the entire trajectory generator. Their technological advantage lies primarily in the guarantee of dynamic accuracy through dynamic feedforward compensation. Under high-speed motion or long-stroke conditions, the inertia, damping, and servo bandwidth limitations of each actuator can lead to dynamic tracking errors between commands and responses. This module establishes a second-order dynamic model for each motion axis (linear motor, servo motor, etc.) and calculates the feedforward compensation amount in real time, actively offsetting the effects of inertial delay and friction. This strategy, combining feedforward control and feedback correction, enables the system to maintain high-precision tracking over a wide bandwidth, avoiding the contamination of calibration results by dynamic errors, thereby significantly improving the reliability of high-speed dynamic testing.

[0453] Secondly, the real-time compensation mechanism for environmental factors is another significant advantage of this module. The measurement accuracy of the laser interferometer is directly affected by air temperature, air pressure, and humidity, while the thermal expansion of the mechanical structure can also introduce positional deviations. This module integrates environmental sensors and applies mathematical models such as the Edlén formula to perform real-time refractive index correction on the interferometer measurements, while simultaneously compensating for the thermal deformation of key structural components. This proactive suppression of environmental disturbances ensures the system's stability and traceability under all-weather and diverse environmental conditions, preventing calibration results from being distorted by environmental changes and greatly enhancing the system's practicality.

[0454] Real-time performance assurance measures are a key breakthrough in the engineering implementation of this module. By employing mathematical optimization methods such as discretized instruction generation, incremental calculation, and lookup tables, the system can complete complex dynamic compensation calculations within microsecond-level control cycles, meeting the requirements of real-time motion control. This efficient algorithm design enables high-precision dynamic calibration, avoiding control lag and accuracy degradation caused by computational delays.

[0455] Finally, by incorporating dynamic tracking error and environmental compensation residual error as independent components into the total uncertainty assessment, this invention ensures that each generated trajectory frame has a quantifiable accuracy label. This closed-loop control, from theoretical model to actual execution, enables the motion output by the entire trajectory generator to possess highly reliable dynamic performance, providing solid and reliable technical support for the comprehensive dynamic calibration of the calibrated instrument.

[0456] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Any simple modifications or equivalent changes made to the above embodiments based on the technical essence of the present invention shall fall within the protection scope of the present invention.

Claims

1. A trajectory generation method for multi-degree-of-freedom target calibration, characterized in that, Includes the following steps: Step S1: Obtain the target trajectory ; Step S2: Scale the target trajectory based on the adaptive scaling algorithm to map the target trajectory into the device workspace while preserving the geometric features and motion characteristics of the target trajectory; Step S21: Determine the workspace boundary based on the physical constraints of the actuator; then, let the original target trajectory point set be... and define the trajectory bounding box; in: : The i-th trajectory point of the target trajectory; Step S22: Construct the scaled target trajectory; ； in: : The i-th scaled trajectory point; Workspace Center; Scaling factor ; Anisotropic scaling matrix; : The center point of the bounding box of the trajectory; Step S23: Based on the target trajectory, solve for the maximum scaling factor. This ensures that the scaled trajectory is entirely within the workspace; ； in: : All points on the trajectory are in the direction The maximum positive offset on; : All points on the trajectory are in the direction The maximum negative offset on; Workspace in direction The offset between the upper limit boundary and the center point; Workspace in direction The offset between the lower boundary and the center point; Direction sign factor; Step S3: Inverse kinematics analysis, the scaled target trajectory is inversely mapped to the actuator space, the continuous trajectory is discretized into a time series, and the actuator instruction sequence is obtained; ； in: : Executable attitude and position; X-axis position command sequence; : Horizontal corner turning command sequence; Vertical cornering command sequence; Z-axis altitude command sequence; : Slewing arm length command sequence; Vibration parameter command sequence; Step S4: Error compensation control commands for each actuator.

2. The trajectory generation method for multi-degree-of-freedom target calibration according to claim 1, characterized in that, In step S3, the discretization time And at each moment, the length of the rotating arm is calculated. : ； ； ； ； ； ； The solution yields: ; in: , , Weighting coefficients; Instruction cycle; : The length of the rotary arm in the k-th instruction cycle; : The length of the rotary arm in the (k-1)th instruction cycle; : Length of the rotary arm in the 0th command cycle; : The command position of the air-floating platform on the air-floating guide rail in the k-th command cycle; The position of the X-axis in the k-th instruction cycle; The position of the Y-axis in the k-th instruction cycle; The Z-axis position in the k-th instruction cycle; : The length of the Z-axis telescopic arm in the k-th instruction cycle; : Z-axis telescopic arm length in the 0th command cycle; , These represent the nearest and farthest locations that the air-floating platform can reach, respectively. , These represent the nearest and farthest positions that the height connector can reach; , These represent the closest and furthest positions that the slewing arm can reach, respectively.

3. The trajectory generation method for multi-degree-of-freedom target calibration according to claim 1, characterized in that, Step S4 includes the following steps: Step S41: Establish a dynamic prediction and compensation model; Step S42: Obtain the attitude position of dynamic prediction compensation through offline calibration; Step S43: In the online compensation stage, the actuator commands are corrected in real time based on the dynamically predicted and compensated attitude position.

4. The trajectory generation method for multi-degree-of-freedom target calibration according to claim 3, characterized in that, In step S41, the dynamic prediction compensation model is: ； ； ； in: The attitude position of the dynamic prediction compensation in the k-th period; The theoretical command position given in the linear compensation stage of the k-th period; The theoretical command position given in the linear compensation stage of the (k-1)th cycle; Geometric error compensation for position; Thermal error compensation for location; Regularization coefficient; : Temperature change in the kth period; J: Error propagation Jacobian matrix; G(): Geometric error transfer function; T(): Temperature error transfer function; : The dynamic adjustment coefficient of the theoretical command value; Instruction cycle.

5. The trajectory generation method for multi-degree-of-freedom target calibration according to claim 1, characterized in that, In step S4, after error compensation, trajectory verification is performed; the high-frequency vibration is synthesized into the macroscopic trajectory of the end point to obtain the final end point trajectory. ;like Then the output will be: ； in: : The scaled trajectory point of the k-th instruction cycle; : No. The amount of vibration at any given moment; : The amount of vibration in the k-th instruction cycle; Acceptable attitude and position threshold; : No. The executable pose and position at any given moment; : No. The macroscopic trajectory attitude and position at any given moment.

6. A trajectory generation system for multi-degree-of-freedom target calibration, used to implement the trajectory generation method for multi-degree-of-freedom target calibration as described in any one of claims 1-5, characterized in that, It includes a target trajectory acquisition module, a trajectory scaling module, an inverse kinematics module, and an error compensation module; The target trajectory acquisition module is used to acquire the target trajectory and perform temporal decomposition; the trajectory scaling module is used to scale the target trajectory based on an adaptive scaling algorithm. The inverse kinematics module is used to analyze the scaled target trajectory using inverse kinematics and obtain the actuator instruction sequence; the error compensation module is used to compensate for the error in the control instructions of each actuator.

Images