A layered continuous inverse kinematics solving method for fiber reciprocating winding of cylindrical component

Abstract

The application discloses a layered continuous inverse kinematics solving method for fiber reciprocating winding of a cylindrical component and belongs to the technical field of robot end control. Based on a fiber winding task of a six-axis robot cylindrical component, trajectory modeling and pose generation are carried out; a kinematics model of the six-axis robot is established; a Jacobian matrix and singularity representation thereof are obtained; end pose error is defined; the fiber winding task of the six-axis robot cylindrical component is described as a continuous inverse kinematics problem; full-pose inverse kinematics iteration is carried out by using a damped least square method; initial value generation based on state prediction is carried out for the fiber winding task of the six-axis robot cylindrical component; a position rollback strategy is set; and the continuous inverse kinematics problem is solved. The application aims at solving the problems of poor continuity of the end trajectory of the six-axis robot, strong attitude constraint and unstable end reversing solving in the fiber winding process of the cylindrical component.

Description

Technical Field

[0001] This invention belongs to the field of robot end-effector control technology, specifically relating to a layered continuous inverse kinematics solution method for reciprocating fiber winding of cylindrical components. Background Technology

[0002] Fiber winding is one of the core processes for manufacturing high-strength composite components such as pressure vessels, pipelines, and rotating shells. Its advantage lies in its ability to significantly reduce structural mass while maintaining structural integrity through precise control of fiber placement path, winding angle, and interlaminar tension, thus possessing significant application value in aerospace, hydrogen energy storage and transportation, and deep-sea equipment. For cylindrical components, the winding trajectory typically manifests as a surface helix formed by the coupling of mandrel rotation and axial feed of the guide head. Its forming quality is highly dependent on the consistency of the winding angle, trajectory continuity, and the smoothness of end reversal.

[0003] Traditional winding equipment often employs dedicated multi-axis linkage mechanisms, which limit the degrees of freedom of motion and trajectory patterns, making it suitable for the mass production of standard rotating parts. Six-axis industrial robots, with their large workspace, high flexibility, and modular deployment capabilities, are gradually becoming a new platform for winding and laying complex components. However, robot winding is not a simple position tracking problem. Its end effector must simultaneously satisfy geometric constraints (guide wire point located on the mandrel surface), process constraints (target winding angle), and mechanical constraints (joint limits, singularity avoidance). Especially in the end-reversal region of reciprocating winding, sudden changes in trajectory speed and attitude can easily lead to inverse kinematics solution failure or joint motion jitter, severely restricting the system's continuous operation capability.

[0004] In fiber winding trajectory modeling, existing technologies employ winding path generation methods based on helical parameterization and improve layup consistency by modifying the surface normal using differential geometry. Furthermore, for non-circular cross-section mandrels, a variable winding angle trajectory model based on parametric splines has been constructed. These works primarily target specialized winding machines and do not consider the coupling effect between the end effector posture and joint motion of a six-axis robot.

[0005] In terms of robot winding motion planning, existing technologies transform the five-axis linkage winding task into a robot end-effector pose interpolation problem and use cubic spline smoothing trajectory, but do not address the continuity problem of the end-effector reversal zone. Existing technologies introduce a trajectory segmentation optimization strategy and jointly adjust the axial feed and spindle speed to match the target winding angle, but its inverse kinematics solution still uses the single initial value Jacobi pseudo-inverse method, which is sensitive to the initial value and makes it difficult to guarantee the continuity of the solution for the entire trajectory.

[0006] In the field of robot inverse kinematics (IK) solving algorithms, classical methods include analytical methods, the Jacobi transpose method, damped least squares (DLS), and intelligent solutions based on neural networks. DLS is widely used in continuous trajectory solving due to its numerical stability and ease of implementation, but its performance is highly dependent on the quality of the initial values. To improve initial value selection, some studies have introduced state prediction mechanisms: existing techniques use Kalman filtering for the continuous IK of a seven-DOF redundant manipulator, improving the robustness of the solution. However, most of the above work focuses on simple tasks such as welding and grinding, and there is still no systematic research on its applicability to fiber entanglement tasks with high attitude constraints and strong nonlinear reversal characteristics.

[0007] Furthermore, existing methods generally treat pose constraints as strong constraints, and once a local solution cannot satisfy the full pose solution, it is judged as a failure, lacking a mechanism for hierarchical processing of "position executability" and "pose feasibility". Summary of the Invention

[0008] This invention provides a layered continuous inverse kinematics solution method for reciprocating fiber winding of cylindrical components, which addresses the problems of poor end-effector trajectory continuity, strong attitude constraints, and unstable end-effector reversal solution during the fiber winding process of cylindrical components.

[0009] This invention is achieved through the following technical solution: A method for solving the layered continuous inverse kinematics of reciprocating fiber winding in cylindrical components, the method comprising the following steps: Step 1: Based on the fiber winding task of a cylindrical component using a six-axis robot, obtain the winding geometry of the cylindrical component; Step 2: Model the reciprocating winding trajectory based on the geometric relationships in Step 1; Step 3: Based on the reciprocating winding trajectory model from Step 2, calculate the winding angle and mark the working segments; Step 4: Based on the winding angle calculated in Step 3, output the end effector attitude; Step 5: Generate the target pose based on the end effector pose output in Step 4; Step 6: Establish a kinematic model of the six-axis robot and obtain the Jacobian matrix and its singularity representation; Differentiating the robot's forward kinematics allows us to establish a mapping relationship between joint velocities and end-effector linear and angular velocities. Step 7: Define the end-effector pose error; Step 8: Describe the task of winding the cylindrical component of the six-axis robot as a continuous inverse kinematics problem, and use the damped least squares method to perform full pose inverse kinematics iteration; Step 9: Generate initial values ​​based on state prediction for the fiber winding task of the cylindrical component of the six-axis robot; Step 10: Solve the problem of continuous inverse kinematics in Step 8.

[0010] Furthermore, step 2 specifically includes the following steps: Step 2.1: Based on the geometric relationships in Step 1, divide the cylindrical component into transition sections and working sections; Step 2.2: Define the forward half-cycle trajectory and the reverse half-cycle trajectory to form a complete cycle trajectory expression; Furthermore, step 4 specifically involves: Step 4.1: Trajectory tangential direction; Step 4.2: Normal to the cylindrical surface; Step 4.3: Reference Directions and Tools Construction of the shaft; Step 4.4: Construct an orthogonal tool coordinate system; Furthermore, step 5 includes the following steps: Step 5.1: Target pose in the core mold coordinate system; Step 5.2: Core mold installation pose transformation; A layered continuous inverse kinematics solution method for reciprocating fiber winding of cylindrical components, as described above, is applied to the end reversal region where reciprocating winding exists, which is also the region where the trajectory velocity and attitude of a six-axis robot change abruptly.

[0011] The beneficial effects of this invention are: By establishing a reciprocating winding trajectory model of a cylindrical component and determining the axial feed relationship based on the mandrel radius, mandrel length, target winding angle, and spindle speed, and by setting cosine transition sections at both ends of the trajectory, the velocity and acceleration changes at the end in the reversing region can be made more continuous, thereby reducing abrupt motion changes during the end reversing process.

[0012] By combining the trajectory tangential direction, the cylindrical surface normal direction, and the reference direction to construct the end-effector posture, the smoothness and continuity of the posture sequence can be improved while meeting the requirements of the winding process direction, thereby reducing the impact of drastic posture changes on the stability of robot joint motion and inverse kinematics solution.

[0013] By adopting a joint initial value generation method based on state prediction and combining it with damped least squares method for full pose inverse kinematics iteration, the local convergence stability of continuous trajectory point solution can be improved and the sensitivity of the solution process to a single initial value can be reduced.

[0014] By setting a position constraint backoff solution mechanism when the full pose constraint fails, the reachability of the guide wire point position can be guaranteed when the local pose constraint is difficult to satisfy, thereby improving the overall executability of the trajectory in the end-point reversal zone and near-singular region.

[0015] Through the synergistic effect of trajectory smoothing, continuous posture construction, prediction initial value generation, and hierarchical inverse kinematics solution, this invention can improve the stability and success rate of continuous robot motion in the fiber winding task of cylindrical components, and reduce the impact of local joint jumps and posture mismatches on winding quality. Attached Figure Description

[0016] Figure 1 This is a schematic diagram of the cylindrical winding geometry of the present invention.

[0017] Figure 2 This is a schematic diagram of the reciprocating winding trajectory and end transition section of the present invention.

[0018] Figure 3 This is a schematic diagram of the end-effector posture structure of the present invention.

[0019] Figure 4 This is a schematic diagram of the kinematic model of the DH six-axis robot of the present invention.

[0020] Figure 5 This is a schematic diagram of the continuous inverse kinematics solution process of the present invention. Detailed Implementation

[0021] In the following description, specific details such as particular system architectures and techniques are set forth for illustrative purposes and not for limitation, in order to provide a thorough understanding of the embodiments of this application. However, those skilled in the art will understand that this application may also be implemented in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, apparatuses, circuits, and methods are omitted so as not to obscure the description of this application with unnecessary detail.

[0022] It should be understood that, when used in this specification and the appended claims, the term "comprising" indicates the presence of the described features, integrals, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or collections thereof.

[0023] It should also be understood that the terminology used in this application specification is for the purpose of describing particular embodiments only and is not intended to limit the application. As used in this application specification and the appended claims, the singular forms “a,” “an,” and “the” are intended to include the plural forms unless the context clearly indicates otherwise.

[0024] The following is in conjunction with the appendix to this application specification. Figure 1-5The technical solutions in the embodiments of this application are clearly and completely described. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of this application.

[0025] Many specific details are set forth in the following description in order to provide a full understanding of this application. However, this application may also be implemented in other ways different from those described herein. Those skilled in the art can make similar extensions without departing from the spirit of this application. Therefore, this application is not limited to the specific embodiments disclosed below.

[0026] Implementation Method 1 This embodiment provides a method for solving the layered continuous inverse kinematics of reciprocating fiber winding in cylindrical components. The method includes the following steps: Step 1: Based on the fiber winding task of a cylindrical component using a six-axis robot, obtain the winding geometry of the cylindrical component; Specifically, let the radius of the cylindrical mandrel be R, the length be L, and the target winding angle be... The core mold rotates uniformly around its own axis at an angular velocity of . .like Figure 1 As shown, establish the core mold coordinate system. ,in The shaft is along the axial direction of the mandrel. shaft and The shaft is located within the plane of the core mold section; If the feed rate of the end guide wire point along the axial direction in the mandrel coordinate system is Then the tangential circumferential velocity of the cylindrical surface is Based on the geometric relationship of helical winding on the surface of a cylinder, we have:

[0027] Therefore, the axial feed rate that satisfies the target winding angle requirement is:

[0028] In the formula, Let be the axial speed of the ideal constant working section. As can be seen from equation (2-2), once the mandrel radius, main rotation speed and target winding angle are given, the axial feed speed can be directly determined.

[0029] Let the core mold rotation angle be:

[0030] The ideal helical trajectory of the guide wire point on the cylindrical surface can be determined by axial displacement. With core mold rotation angle This was determined jointly. Because the actual winding process requires reciprocating reversal at both ends of the mandrel, therefore... It should no longer be a monotonic function, but should be constructed as a periodic function containing a forward motion segment, a reverse motion segment, and a smooth transition segment at the end.

[0031] Step 2: Model the reciprocating winding trajectory based on the geometric relationships in Step 1; Step 2.1: Based on the geometric relationships in Step 1, divide the cylindrical component into transition sections and working sections; To reduce the sudden speed change at the reversal points of the mandrel, this invention sets a length of [missing information] at both ends. The transition section, then the length of the middle constant speed working section is

[0032] in, The length of the single-end transition section. This refers to the length of the stable winding section in the middle.

[0033] Within the middle working section, the axial velocity is taken as the constant value corresponding to equation (2-2). Therefore, the duration of the working segment is

[0034] To match the average speed of the transition section with that of the middle working section, this invention uses a cosine function to construct a smooth reversal trajectory at both ends. The time of the single-end transition section is defined as...

[0035] Therefore, the total time for a single half-cycle (from one end to the other) is:

[0036] The complete reciprocating cycle time is

[0037] Step 2.2: Define the forward half-cycle trajectory and the reverse half-cycle trajectory to form a complete cycle trajectory expression; Specifically: the positive half-cycle trajectory is defined by defining local time variables. When the guide wire point moves along the mandrel axis from the left end... To the right During motion, this is called the positive half-cycle. Its axial displacement function is defined as...

[0038] The corresponding axial velocity is:

[0039] From equations (2-9) and (2-10), it can be seen that in the positive half-cycle: 1. The left-end transition section achieves smooth acceleration from rest to constant speed; 2. Maintain a constant axial speed in the middle working section; 3. The right-end transition section achieves smooth deceleration from constant speed to standstill.

[0040] Therefore, the velocity change before and after the reversal point is continuous.

[0041] The reverse half-cycle trajectory is specifically as follows: When the guidewire point is from the right end To the left end The return period is called the reverse half-cycle. The corresponding axial displacement function is:

[0042] The corresponding axial velocity is:

[0043] Therefore, the guidewire point completes the deceleration, constant speed, and re-deceleration processes in the reverse half-cycle in a manner symmetrical to that in the forward half-cycle. The complete periodic trajectory expression is as follows: For any time The local time within the period is defined as

[0044] Then axial displacement Can be written as

[0045] The corresponding axial velocity is

[0046] Combining equation (2-3), the position of the guide wire point in the mandrel coordinate system can be expressed as follows:

[0047] Equation (2-16) is the position model of the reciprocating winding trajectory on the cylindrical surface.

[0048] Step 3: Based on the reciprocating winding trajectory model from Step 2, calculate the winding angle and mark the working segments; Specifically, to analyze the consistency between the trajectory generation result and the target winding angle, the velocity component is calculated using numerical differentiation for discrete trajectory points. Let the sampling period be... Then at discrete time At this point, it can be approximately obtained

[0049] in, For the first kdiscrete time x Directional velocity component; For the first k discrete time y Directional velocity component; For the first k discrete time z Directional velocity component; For the first k At discrete moments, the trajectory is... yz The polar angle corresponding to motion in a plane.

[0050] The actual winding angle can be determined by the resultant direction of the axial velocity and the circumferential tangential velocity, and is calculated in the following form:

[0051] The winding angle error is

[0052] In the end transition section, due to the continuous change in axial velocity, the winding angle will deviate from the target value; while in the middle working section, due to... Since it is a constant value, theoretically it should be closer to the target winding angle. Therefore, this invention defines the working segment as a local time segment that satisfies:

[0053] Based on this, working segments are marked for each discrete point within the entire cycle. This marking is used to subsequently distinguish between "overall winding angle error" and "stable working segment winding angle error".

[0054] Step 4: Based on the winding angle calculated in Step 3, output the end effector attitude; Step 4: A positional trajectory alone is insufficient to describe the winding task; the robot's end effector also needs an attitude that aligns with the fiber placement direction and continuously changes with the trajectory. This invention combines the trajectory tangent, the cylindrical surface normal, and a global reference direction to construct the end effector's attitude. Step 4.1: Trajectory tangential direction; Specifically, by differentiating equation (2-16), we obtain the trajectory velocity vector.

[0055] in The rotational angular velocity of the core mold. Differentiate the guide wire point position vector in the mandrel coordinate system; After normalization, it is defined as the tool coordinate system. Axial direction:

[0056] Equation (2-23) ensures that the main direction of the end tool is consistent with the tangential direction of the actual winding path, thereby ensuring that the guide wire direction is coordinated with the fiber laying direction.

[0057] At individual discrete points, if numerical precision issues lead to... If the value is too small, the velocity vector can be approximated by the difference between adjacent trajectory points to maintain the stability of the attitude configuration. Step 4.2: Normal to the cylindrical surface; Specifically, points on the surface of a cylinder The outward normal unit vector is

[0058] This vector represents the local surface geometry at the entanglement point. If the tool attitude is entirely determined by the surface normal, rapid attitude changes are likely to occur in the reversal zone. To reduce such fluctuations, this invention does not directly use a single normal to construct the tool attitude, but instead introduces a reference direction for weighted fusion.

[0059] Step 4.3: Reference Directions and Tools Construction of the shaft; Specifically, let the reference direction be...

[0060] Tool coordinate system The initial direction of the axis is obtained by a weighted combination of the inward normal direction of the surface and the reference direction:

[0061] After normalization, we can obtain

[0062] In equation (2-26), This causes the tool to tend towards the surface of the mandrel. This is used to enhance global attitude stability. Weights β is a weighting coefficient.

[0063] Step 4.4: Construct an orthogonal tool coordinate system; Specifically, in the known and Then, the coordinate system of the tool is constructed through the cross product. Axial direction:

[0064] To ensure that the three axes are strictly orthogonal, the calculation is repeated. axis:

[0065] Therefore, the desired end-effector attitude matrix can be expressed as:

[0066] From equation (2-30), we can see that the three axes of the tool coordinate system always maintain a unit orthogonal relationship and change continuously with the trajectory.

[0067] Step 5: Generate the target pose based on the end effector pose output in Step 4; Step 5.1: Target pose in the core mold coordinate system; Specifically, by combining the position vector obtained from equation (2-16) with the attitude matrix obtained from equation (2-30), the desired homogeneous pose matrix of the end effector in the core model coordinate system can be constructed:

[0068] Equation (2-31) gives the end target pose at each sampling time, which is the direct input for subsequent inverse kinematics solution.

[0069] Step 5.2: Core mold installation pose transformation; Specifically, in practical systems, the mandrel coordinate system and the robot base coordinate system usually do not coincide, therefore a transformation of the mandrel relative to the robot base coordinate system is required. Let the robot base coordinate system be B, then the transformation matrix from the mandrel to the robot base coordinate system is:

[0070] The core mold mounting rotation matrix is ​​taken as the identity matrix, i.e.

[0071] The translation vector is

[0072] Therefore, the target end-effector pose in the robot's base coordinate system is:

[0073] in, The desired pose of the end effector in the core mold coordinate system.

[0074] Step 6: Establish a kinematic model of the six-axis robot and obtain the Jacobian matrix and its singularity representation; To achieve continuous tracking of the winding trajectory of a cylindrical component, a kinematic model of a six-axis industrial robot is first required. This invention employs the standard Denavit-Hartenberg (DH) method to describe the geometric relationships between the robot's joints and links, such as... Figure 4 As shown. Suppose the robot has 6 rotational joints, and its joint variable vector is represented as follows:

[0075] For the The homogeneous transformation matrix between the coordinate systems of each joint and adjacent links. It can be represented as:

[0076] After unfolding, it contains:

[0077] in, To bypass z The rotation transformation matrix of the axis. To bypass z The rotation angle of the shaft; For along z The translation transformation matrix of the axis. For along the current coordinate system z Axis translation distance; For along x The translation transformation matrix of the axis. For along x The translation distance of the axis; Let be the rotation transformation matrix about the x-axis. To bypass x The rotation angle of the shaft; Therefore, the positive kinematics of the end effector relative to the robot's base coordinate system can be written as:

[0078] remember:

[0079] in, This is the end-effector attitude matrix. The end position vector, Let be the transformation matrix of the first link. Let be the transformation matrix for the second link. The transformation matrix for the third link. The transformation matrix for the fourth link. The transformation matrix for the fifth link. This is the transformation matrix for the sixth link.

[0080] The six-axis robot studied in this invention uses a fixed DH parameter set, the specific values ​​of which can be given by the robot's structural dimensions. Since all joints are revolute joints, the variables of each joint are reflected in the corresponding joint angle parameters. At the same time, to ensure that the solution results satisfy the physical constraints of the mechanism, joint limiting conditions also need to be introduced, namely:

[0081] In the formula, and These are the minimum and maximum allowable rotation angles for each joint.

[0082] The Jacobian matrix and its singularity characteristics are as follows: Differentiating the robot's forward kinematics allows us to establish a mapping relationship between joint velocities and the linear and angular velocities of the end effector. Let the linear velocity of the end effector be... angular velocity is Then we have:

[0083] in, Let be the geometric Jacobian matrix. Representing the Jacobian matrix in blocks, it can be written as:

[0084] in, Let Jacobian be the positional Jacobian matrix. Let be the pose Jacobian matrix. In solving continuous inverse kinematics, the conditional properties of the Jacobian matrix directly affect numerical stability. To characterize the singularity of the robot near different trajectory points, this invention uses the condition number and the minimum singular value as singularity indices, defined as follows:

[0085]

[0086] in, The first Jacobian matrix represents the first Jacobian matrix. A singular value, It is the maximum singular value. When Close to zero or When the value increases significantly, it indicates that the robot is approaching a singular configuration. At this point, traditional numerical iterative methods are prone to joint increment amplification and convergence performance degradation.

[0087] Step 7: Define the end-effector pose error; Specifically, let the first... The target pose of each discrete sampling point is:

[0088] The robot's actual end-effector pose with the current joint configuration is

[0089] in, Let be the target attitude matrix. Let the target position vector be... This is the actual attitude matrix. This is the actual position vector; Position error is defined as the difference between the target position and the actual position:

[0090] Its norm can be written as

[0091] Attitude error is represented using a relative rotation matrix. The attitude error matrix is ​​defined as follows:

[0092] In numerical solutions, the attitude error vector can be constructed from the antisymmetric part of the relative rotation matrix:

[0093] Its norm is defined as:

[0094] Combining the position error and attitude error, we can obtain the full pose error vector:

[0095] This error vector serves as the iterative input for solving the inverse kinematics of the full pose; when only position constraints are considered, only the position error vector is used. .

[0096] Step 8: Describe the task of winding the cylindrical component of the six-axis robot as a continuous inverse kinematics problem, and use the damped least squares method to perform full pose inverse kinematics iteration; The task of winding a cylindrical component can be discretized into a series of target pose points arranged in time sequence, i.e.

[0097] The continuous inverse kinematics problem is: for each target pose Find the corresponding joint solutions. , making

[0098] And try to meet the following requirements: 1. The position error and attitude error meet the given threshold; 2. The joint solutions between adjacent sampling points change continuously; 3. The joint angles meet the limit constraints; 4. It maintains good solution stability in the end-commutation and near-singular regions.

[0099] Unlike single-point inverse kinematics, continuous trajectory solutions exhibit significant temporal correlation. Adjacent pose points are typically very close; therefore, effectively utilizing the solution results from the previous time step and local state evolution information can significantly improve the initial value quality and solution continuity for the next time step. Conversely, performing completely independent inverse kinematics calculations for each sampling point can easily lead to convergence failures, solution jumps, and local discontinuities.

[0100] The damped least squares iteration with only position constraints in step 8 is specifically as follows: Full pose damped least squares iteration To address the numerical stability problem of a six-axis robot in near-singular regions, this invention employs a damped least squares method for iterative inverse kinematics across all poses. Let the Jacobian matrix of the current iteration step be... The full pose error vector is Then the joint increment can be expressed as:

[0101] in, The damping factor, It is an identity matrix.

[0102] Joint variables are updated using the following formula:

[0103] in, For the first j Joint variables for the next iteration; When both position error and attitude error meet the convergence threshold, the full pose is considered to be solved successfully.

[0104] in, and These represent the position and attitude error tolerances, respectively. If the maximum number of iterations is reached and equation (3-22) is still not satisfied, then the full pose solution under this initial value is considered to have failed.

[0105] Damped least squares iteration with only position constraints At certain trajectory points, especially in the end-point reversal zone or sections with rapid attitude changes, the full pose constraint may be too strong, leading to difficulties in numerical iteration convergence. To address this, this invention further introduces a damped least squares solution with only position constraints, where the joint increment can be written as:

[0106] Equation (3-23) uses only the position Jacobian matrix and position error vector for iteration, which can improve the reachability of local points while relaxing attitude requirements.

[0107] The role of the damping term Compared to the standard Jacobian pseudo-inverse method, the damped least squares method introduces a regularization term during the inverse calculation process. This alleviates the ill-conditioned amplification problem when the Jacobian matrix approaches singularity. For the reciprocating winding trajectory studied in this invention, the end reversal region is often accompanied by attitude changes and an increase in the local condition number. Therefore, the damped least squares method is more suitable as the main solver.

[0108] Step 9: Generate initial values ​​based on state prediction for the fiber winding task of the cylindrical component of the six-axis robot; Initial value problem analysis Continuous inverse kinematics is essentially a sequential problem. Since the target pose changes at adjacent sampling points are usually small, the joint solutions at adjacent time points often exhibit continuous evolution characteristics. If the joint states near the next sampling point can be predicted based on the existing solution results, it can provide initial values ​​closer to the true solution for numerical iteration, thereby improving the convergence speed and the success rate of the solution.

[0109] To address this, the present invention introduces a joint state prediction method based on Kalman filtering to construct a priority initial value for the next trajectory point.

[0110] State-space model If the robot's joint states are composed of joint angles and joint angular velocities, then the state vector is defined as follows:

[0111] During the sampling period The constant velocity model is used to describe the state evolution relationship between adjacent time steps:

[0112] in, For process noise, the state transition matrix is:

[0113] The observations are taken from the joint angles that have been identified or calculated at the current moment; therefore, the observation model can be expressed as:

[0114] in, To account for observation noise, the observation matrix is:

[0115] Kalman filter recursion At each discrete moment, a prediction is first made based on the state of the previous moment:

[0116]

[0117] in, The covariance matrix is ​​estimated for the state. Let be the process noise covariance matrix.

[0118] Then, the predicted state is corrected using the current joint observations:

[0119]

[0120]

[0121] Where R is the radius of the cylindrical mandrel; Therefore, the predicted joint angle value for the next moment can be extracted from the predicted state and used as the preferred initial value for continuous inverse kinematics.

[0122] The quantity and significance of innovation To evaluate the consistency between the predicted state and the current observation, the Kalman filter innovation vector is defined as follows:

[0123] Its norm is

[0124] The innovation quantity reflects the degree of deviation between the model prediction and the actual joint state. When the trajectory change is relatively smooth and the prediction is relatively accurate, the innovation quantity is usually small; when entering the reversal zone or the rapid attitude change zone, the innovation quantity may increase. In subsequent simulations, the trend of innovation quantity changes can be analyzed by combining the number of point-by-point iterations to discuss the relationship between the quality of the initial prediction value and the difficulty of local solution.

[0125] Step 10: Set the location rollback strategy; In the end-reversal zone or near singular regions, some target pose points may struggle to simultaneously meet position and attitude error requirements within a given number of iterations. Directly determining a solution failure at this point would disrupt the continuity of the entire winding trajectory. Considering that the continuity of the guide wire point position is typically the most fundamental requirement in fiber winding tasks, while attitude error can be relaxed to a limited extent in local sections, this invention employs a position backoff strategy. The basic process is as follows: 1. First, attempt to solve the inverse kinematics of the entire pose; 2. If the full pose solution fails, retain the target position constraint and relax the pose constraint; 3. Resolve using the damped least squares method with only position constraints; 4. If only the location is solved successfully, the point will still be considered an executable point, but its solution mode will be marked separately.

[0126] Based on this, the solution results for each trajectory point can be divided into the following three categories: 1. Full pose solution successful; 2. The full pose calculation failed, but the position calculation was successful only; 3. Both full pose and position-only solutions failed.

[0127] By introducing a position backoff strategy, the overall executability of the entire trajectory can be improved, and it can help distinguish between the two types of cases: "the process constraints are fully met" and "the geometric position is reachable but the attitude is limited".

[0128] Step 11: Solve the problem of continuous inverse kinematics in Step 8.

[0129] Specifically, let the target pose sequence be... The steps for solving the continuous inverse kinematics at each sampling point are as follows: Figure 5 As shown, it can be summarized as follows: 1. Generate candidate initial values ​​for the current target pose based on the selected method; 2. Perform the corresponding numerical inverse kinematics iteration for each set of candidate initial values; 3. If a full pose solution that satisfies the error threshold exists, then the final solution is selected according to the joint continuity criterion; 4. If a full pose solution does not exist, then perform a position constraint-only solution. 5. If there is still no feasible solution, then the solution for that point is considered unsuccessful; 6. For the state prediction method, update the filter using the currently obtained joint solution and generate the initial prediction value for the next sampling point.

[0130] By proceeding step by step through the above process, the sequence of joint solutions corresponding to the entire winding trajectory can be obtained. This sequence is not only used for subsequent trajectory back-substitution analysis, but also provides a basis for the statistics of evaluation quantities such as position error, attitude error, solution success rate, average number of iterations, total solution time, and singularity index.

[0131] Under the same trajectory and robot model, the present invention is compared with methods such as no Kalman filtering, single initial value DLS, position-only DLS and Jacobi transpose method. The performance is evaluated from indicators such as position error, attitude error, solution success rate, number of iterations, computation time, Jacobi condition number, minimum singular value and winding angle error.

[0132] This invention proposes a continuous inverse kinematics solution framework: it uses Kalman filtering to predict joint initial values ​​and combines damped least squares iteration to improve local convergence stability; when full pose constraints are not feasible, it automatically reduces the order to position constraint-only solution to improve trajectory executability.

[0133] This invention constructs an end-effector attitude sequence: by combining the trajectory tangent, surface normal, and reference direction, an end-effector attitude that satisfies the layup requirements and is smooth and continuous is generated.

[0134] This invention establishes a reciprocating winding trajectory model: based on the mandrel radius, length, target winding angle and spindle speed, the axial feed speed relationship is derived, and a cosine transition section is set at both ends to make the speed and acceleration continuous during the reversal process.

[0135] Implementation Method 2 This embodiment provides a layered continuous inverse kinematics solution method for reciprocating fiber winding of cylindrical components, as described in Embodiment 1. It is applied to the end reversal region where reciprocating winding exists, which is the region where the trajectory speed and attitude of a six-axis robot change abruptly.

Claims

1. A method for solving layered continuous inverse kinematics of reciprocating fiber winding in cylindrical components, characterized in that, The solution method includes the following steps: Step 1: Based on the fiber winding task of a cylindrical component using a six-axis robot, obtain the winding geometry of the cylindrical component; Step 2: Model the reciprocating winding trajectory based on the geometric relationships in Step 1; Step 3: Based on the reciprocating winding trajectory model from Step 2, calculate the winding angle and mark the working segments; Step 4: Based on the winding angle calculated in Step 3, output the end effector attitude; Specifically, step 4 is as follows: Step 4.1: Trajectory tangential direction; Specifically, the trajectory velocity vector is obtained by differentiating the position of the guide wire point in the mandrel coordinate system. in The rotational angular velocity of the core mold. Differentiate the guide wire point position vector in the mandrel coordinate system; After normalization, it is defined as the tool coordinate system. Axial direction: Ensure that the main direction of the end tool is consistent with the tangential direction of the actual winding path, thereby ensuring that the guide wire direction is coordinated with the fiber laying direction; At individual discrete points, if numerical precision issues lead to... If the value is too small, the velocity vector can be approximated by the difference between adjacent trajectory points to maintain the stability of the attitude configuration. Step 4.2: Normal to the cylindrical surface; Specifically, points on the surface of a cylinder The outward normal unit vector is: This vector represents the local surface geometry at the point of entanglement; Step 4.3: Reference Directions and Tools Construction of the shaft; Specifically, let the reference direction be: Tool coordinate system The initial direction of the axis is obtained by a weighted combination of the inward normal direction of the surface and the reference direction: in This causes the tool to tend towards the surface of the mandrel. This is used to enhance global attitude stability; weights β is a weighting coefficient; After normalization, we get: Step 4.4: Construct an orthogonal tool coordinate system; Specifically, in the known and Then, the coordinate system of the tool is constructed through the cross product. Axial direction: To ensure that the three axes are strictly orthogonal, the calculation is repeated. axis: The desired attitude matrix at the end point can be represented as: The three axes of the tool coordinate system always maintain a unit orthogonal relationship and change continuously with the trajectory; Step 5: Generate the target pose based on the end effector pose output in Step 4; Step 6: Establish a kinematic model of the six-axis robot and obtain the Jacobian matrix and its singularity representation; Differentiating the robot's forward kinematics allows us to establish a mapping relationship between joint velocities and end-effector linear and angular velocities. Step 7: Define the end-effector pose error; Step 8: Describe the task of winding the cylindrical component of the six-axis robot as a continuous inverse kinematics problem, and use the damped least squares method to perform full pose inverse kinematics iteration; Step 9: Generate initial values ​​based on state prediction for the fiber winding task of the cylindrical component of the six-axis robot; Step 10: Solve the problem of continuous inverse kinematics in Step 8.

2. The solution method according to claim 1, characterized in that, Step 2 specifically includes the following steps: Step 2.1: Based on the geometric relationships in Step 1, divide the cylindrical component into transition sections and working sections; Specifically: Set a length of [value] at both ends. The transition section, then the length of the middle constant speed working section is: in, The length of the single-end transition section. This refers to the length of the stable winding section in the middle. Within the middle working section, the axial velocity remains constant. Therefore, the duration of the working segment is To match the average speed of the transition section with that of the middle working section, a cosine function is used to construct a smooth reversal trajectory at both ends; the time of the single-end transition section is defined as: Therefore, the total time for a single half-cycle is: The complete cycle time is: Step 2.2: Define the forward half-cycle trajectory and the reverse half-cycle trajectory to form a complete cycle trajectory expression; Specifically: for any time... The local time within the period is defined as: Then axial displacement It can be written as: The corresponding axial velocity is: The position of the guide wire point in the mandrel coordinate system can be represented as: in The radius of the cylindrical mandrel is the position model of the reciprocating winding trajectory on the cylindrical surface.

3. The solution method according to claim 1, characterized in that, Step 3 specifically involves calculating the velocity components of the discrete trajectory points using numerical differentiation; assuming the sampling period is... Then at discrete time At this point, we can approximate the following: in, For the first k discrete time x Directional velocity component, For the first k discrete time y Directional velocity component, For the first k discrete time z Directional velocity component, The actual winding angle can be determined by the resultant direction of the axial velocity and the circumferential tangential velocity, and is calculated in the following form: The winding angle error is: in, The target winding angle is defined; the working segment is defined as satisfying the following local time conditions: Based on this, working segments are marked for each discrete point within the entire cycle.

4. The solution method according to claim 1, characterized in that, Step 5 includes the following steps: Step 5.1: Target pose in the core mold coordinate system; Specifically, by combining the position vector obtained from the position of the guide wire point in the mandrel coordinate system with the desired end-effector pose matrix, the desired homogeneous end-effector pose matrix in the mandrel coordinate system can be constructed: The end target pose at each sampling time is given, which is the direct input for subsequent inverse kinematics solution; Step 5.2: Core mold installation pose transformation; Specifically, let the robot's base coordinate system be B, then the transformation matrix from the mandrel to the robot's base coordinate system is: The core mold mounting rotation matrix is ​​taken as the identity matrix, that is: The translation vector is: Therefore, the target end-effector pose in the robot's base coordinate system is: in, The desired pose of the end effector in the core mold coordinate system.

5. The solution method according to claim 1, characterized in that, Specifically, step 6 involves assuming the robot has 6 rotational joints, and its joint variable vector is represented as follows: For the The homogeneous transformation matrix between the coordinate systems of each joint and adjacent links. It can be represented as: in, To bypass z The rotation transformation matrix of the axis. To bypass z The rotation angle of the shaft; For along z The translation transformation matrix of the axis. for; For along x The translation transformation matrix of the axis. For along x The translation distance of the axis; Let be the rotation transformation matrix about the x-axis. Let be the rotation angle about the x-axis; Therefore, the forward kinematics of the end effector relative to the robot's reference system can be written as: in, This is the end-effector attitude matrix. The end position vector; Introduce joint limiting conditions, namely: In the formula, and These are the minimum and maximum allowable rotation angles for each joint, respectively. Differentiating the robot's forward kinematics allows us to establish a mapping relationship between joint velocities and end-effector linear and angular velocities; let the end-effector linear velocity be... angular velocity is Then we have: in, Let the Jacobian matrix be a geometric Jacobian matrix; the Jacobian matrix can be represented in blocks as follows: in, Let Jacobian be the positional Jacobian matrix. Let Jacobian be the attitude Jacobian matrix; in solving the continuous inverse kinematics, the condition number and the minimum singular value are used as singularity indices, defined as follows: in, The Jacobian matrix is ​​the first... A singular value, It is the largest singular value.

6. The solution method according to claim 1, characterized in that, Specifically, step 7 involves setting the first... The target pose of each discrete sampling point is: The robot's actual end-effector pose with the current joint configuration is: in, Let be the target attitude matrix. Let the target position vector be... This is the actual attitude matrix. This is the actual position vector; Position error is defined as the difference between the target position and the actual position: Attitude error is represented in the form of a relative rotation matrix; the attitude error matrix is ​​defined as follows: Construct the attitude error vector from the antisymmetric part of the relative rotation matrix: Combining the position error and attitude error, we can obtain the full pose error vector: This error vector serves as the iterative input for solving the inverse kinematics of the full pose; when only position constraints are considered, only the position error vector is used. .

7. The solution method according to claim 5, characterized in that, The continuous inverse kinematics problem in step 8 is specifically described as follows: the task of winding the cylindrical component can be discretized into a series of target pose points arranged in time sequence, that is: The continuous inverse kinematics problem is: for each target pose Find the corresponding joint solutions. , so that: in, For the first k The actual end pose matrix at each sampling point; It must also meet the following requirements: position error and attitude error must meet a given threshold; joint solutions between adjacent sampling points must change continuously; joint angles must meet limit constraints; and good solution stability must be maintained in end-point reversal and near-singular regions. The damped least squares iteration with only position constraints in step 8 is specifically as follows: Damped least squares method is used for full pose inverse kinematics iteration; let the Jacobian matrix of the current iteration step be... The full pose error vector is Then the joint increment can be expressed as: in, The damping factor, It is the identity matrix; Joint variables are updated using the following formula: in, For the first j Joint variables for the next iteration; When both position error and attitude error meet the convergence threshold, the full pose is considered to be solved successfully. in, and These are the position and attitude error tolerances, respectively. The joint increments for damped least squares solution with only position constraints can be written as: By using only the position Jacobian matrix and the position error vector for iteration, the reachability of local points can be improved while relaxing attitude requirements.

8. The solution method according to claim 1, characterized in that, Step 9 specifically involves: using a joint state prediction method based on Kalman filtering to construct the initial value of the next trajectory point; If the robot's joint states are composed of joint angles and joint angular velocities, then the state vector is defined as follows: During the sampling period The constant velocity model is used to describe the state evolution relationship between adjacent time steps: in, For process noise, the state transition matrix is: The observations are taken from the joint angles that have been identified or calculated at the current moment; therefore, the observation model can be expressed as: in, To account for observation noise, the observation matrix is: At each discrete moment, a prediction is first made based on the state of the previous moment: in, The covariance matrix is ​​estimated for the state. The process noise covariance matrix; Then, the predicted state is corrected using the current joint observations: Where R is the radius of the cylindrical mandrel; Therefore, the predicted joint angle value for the next moment can be extracted from the predicted state and used as the preferred initial value for continuous inverse kinematics.

9. A layered continuous inverse kinematics solution method for reciprocating fiber winding of cylindrical components as described in any one of claims 1-8, which is applied to the end reversal region where reciprocating winding exists, that is, the region where the trajectory velocity and attitude of a six-axis robot change abruptly.

Images