A trajectory planning and adjustment method and system based on dynamic motion primitives

Abstract

The application belongs to the technical field of robot trajectory planning, and discloses a trajectory planning and adjustment method and system based on dynamic motion primitives, which comprises the following steps: step 1, obtaining an experienced trajectory including position and attitude information in original path points; step 2, extracting independent position trajectories and attitude trajectories from the experienced trajectory, and performing speed planning preprocessing on the independent position trajectories and attitude trajectories to obtain acceleration level continuous and smooth position demonstration trajectories and attitude demonstration trajectories; and step 3, constructing dynamic motion primitive models respectively facing the position and the attitude, and optimizing the position demonstration trajectories and the attitude demonstration trajectories to obtain new position trajectories and attitude trajectories. The application provides a trajectory planning and adjustment method based on dynamic motion primitives, which has high planning efficiency and low difficulty.

Description

Technical Field

[0001] This invention belongs to the field of robot trajectory planning technology, specifically relating to a trajectory planning and adjustment method and system based on dynamic motion primitives. Background Technology

[0002] With the development of intelligent manufacturing technology, robots are widely used in the manufacturing of industrial products such as automobiles, airplanes, and furniture, replacing workers in processes such as painting, sanding, gluing, and welding. The quality of the robot's working trajectory directly affects the precision and efficiency of product processing.

[0003] Currently, the planning and programming of robot target trajectories largely relies on primitive manual teaching methods. Robot target trajectories depend on workers independently teaching and programming each product based on their processes and experience. However, with increasingly diverse products and frequent updates, the requirements for robot trajectory planning in production and processing are becoming increasingly stringent. Independent target trajectory planning for each product is time-consuming and impacts production efficiency. Once a product is updated, the original trajectory must be discarded and replanned. Coupled with the shortage of robot application talent, this undoubtedly increases the difficulty for small and medium-sized enterprises to promote and implement robot applications. Summary of the Invention

[0004] The purpose of this invention is to provide a trajectory planning and adjustment method and system based on dynamic motion primitives that is efficient and easy to plan.

[0005] To achieve the above objectives, the present invention adopts the following technical solution:

[0006] A trajectory planning and adjustment method based on dynamic motion primitives includes the following steps:

[0007] Step 1: Obtain the empirical trajectory, including position and orientation information, from the original path points. The method of this invention uses past empirical trajectories as learning samples, and performs reproduction and shape adjustment based on the empirical trajectories to meet new working conditions and obtain the target trajectory. Therefore, it is first necessary to obtain the original path points of the empirical trajectory, including the position and orientation information of the path points, as input data for the method.

[0008] Step 2 involves decoupling the original path points from their poses, extracting independent position and attitude trajectories from the empirical trajectory. The independent position trajectory is represented by the position information of the path points in the x, y, and z directions in Cartesian coordinates; the independent attitude trajectory is represented by quaternions. Velocity planning preprocessing is then performed on the independent position and attitude trajectories to obtain continuous and smooth demonstration trajectories at the acceleration level. Since the original trajectory path points used as demonstration samples for replication learning may come from manual teaching or other offline programming methods, the extracted independent position and attitude trajectories may not achieve continuous smoothness, thus affecting the fitting quality during the dynamic motion primitive learning replication process. To ensure that the trajectory planning obtains a pose trajectory (target trajectory) with quality and accuracy that meets the application requirements, velocity planning preprocessing is first performed on the acquired independent position and attitude trajectories to obtain continuous and smooth demonstration trajectories at the acceleration level.

[0009] Step 3: Construct dynamic motion primitive models for position and attitude respectively, optimize the position demonstration trajectory and attitude demonstration trajectory, and obtain new position trajectory and attitude trajectory.

[0010] Furthermore, in step 2, the method for obtaining the demonstration position trajectory is as follows: For an independent position trajectory, using the expected number of interpolation points of the target trajectory, the maximum speed, maximum acceleration, maximum jerkatonicity, and maximum spasm as constraint parameters, a 15-segment S-curve velocity planning algorithm is used to calculate the time-optimal smooth position trajectory, thus obtaining the demonstration position trajectory. The expression for the demonstration position trajectory at the nth point is:

[0011] [t n x n y n , z n v xn v yn v zn a xn a yn a zn ]

[0012] Among them, t n This indicates the time recorded for the current location trajectory point, [x n y n , z n [v] indicates the position information of the current point in three directions. xn v yn v zn [a] represents the velocity information of the current point in three directions. xn a yn a zn [] indicates the acceleration information of the current point in three directions.

[0013] Furthermore, in step 2, the method for obtaining the attitude demonstration trajectory is as follows: First, calculate the normalized velocity value of the position demonstration trajectory. Then, using the desired number of interpolation points and the normalized velocity value of the position demonstration trajectory as constraint parameters, perform spherical linear interpolation programming on the independent attitude trajectories to obtain the attitude demonstration trajectory. The expression for the attitude demonstration trajectory at the nth point is:

[0014] [t n q 0n q 1n q 2n q 3n ω xn ω yn ω zn α xn α yn α zn ]

[0015] Among them, t n Indicates the time recorded for the current attitude trajectory point, [q 0n q 1n q 2n q 3n ] represents the pose information of the current point in the quaternion expression, [ω xn ω yn ω zn ] represents the angular velocity information of the current point in three directions, [α xn α yn α zn [] indicates the angular acceleration information of the current point in three directions.

[0016] Furthermore, in step 3, a dynamic primitive coupling term is first constructed, denoted as C. t Then, construct the expression for the position-oriented dynamic motion primitive model:

[0017] In the formula, τ is a time constant; g is the target endpoint of the trajectory; s is the three-dimensional displacement of the trajectory point; v is the velocity of the trajectory point; a is the acceleration of the trajectory point; k is the damping coefficient of the model; and f is a nonlinear forcing term. This invention reuses empirical trajectories from similar working conditions and meets new work requirements by reproducing the position trajectory and adjusting its shape. To this end, coupling terms need to be added to the dynamic motion primitives. Dynamic motion primitive coupling terms are constructed based on input parameters, thereby applying perturbation forces during the reproduction of the position trajectory by the dynamic motion primitives and adjusting the shape of the reproduced position trajectory.

[0018] Furthermore, in step 3, the method for constructing the dynamic primitive coupling term is as follows:

[0019] First, construct the sigmoid system in the Gaussian radial basis functions:

[0020]

[0021] Then, a virtual disturbance force F is constructed based on the Gaussian radial basis function and the initial control parameters of the sigmoid system therein. c The expression:

[0022]

[0023] In the formula, a n b is the Gaussian distribution skewness coefficient of a set of basis functions; n c is the kurtosis coefficient of the Gaussian distribution. n r is the scaling factor for the Gaussian distribution; n d represents the slope coefficient for each sigmoid system. n The center offset coefficient corresponds to each group of sigmoid systems; μ represents the average value of the position normalization value within the selected trajectory interval; σ represents the standard deviation of the position normalization value within the selected trajectory interval.

[0024] Then construct the deflection coupling term. The expression:

[0025] In the formula, This represents a dynamic deflection term based on the angle between the virtual interactive force and the velocity direction. It generates a deflection disturbance force on the moving point of a dynamic motion primitive, achieving the effect of trajectory feature deformation. R represents a deflection matrix; v represents the velocity of the moving point; γ is a scaling variable; θ represents the angle between the virtual disturbance force vector and the velocity direction; β is a constant coefficient.

[0026] Finally, construct the dynamic primitive coupling terms:

[0027] Furthermore, in step 3, the method for optimizing the position demonstration trajectory and obtaining a new position trajectory is as follows: First, the τ, s, v, and a of the position demonstration trajectory, the constraints, and the g of the target trajectory are input into the position-oriented dynamic motion primitive model; then, the position-oriented dynamic motion primitive model extracts the motion features of the position demonstration trajectory, and then uses a nonlinear forcing term to fit the motion features to reproduce the motion features of the position demonstration trajectory, obtaining the reproduced position trajectory, and then using C... tThe perturbation is provided to the position-oriented dynamic motion primitive model to adjust the shape of the reproduced position trajectory to that of the target trajectory, resulting in the adjusted position trajectory. Then, the Gaussian radial basis function control parameters and the adjusted position trajectory are iteratively optimized to change the distribution of the virtual perturbation force in the trajectory interval until a new position trajectory that satisfies the shape of the target trajectory is obtained, and the new position trajectory is output.

[0028] Furthermore, the method for iteratively optimizing the adjusted position trajectory is as follows: Calculate the control point positions for trajectory adjustment based on the input constraints, including the starting point, ending point, target peak position, and target deviation distance of the position trajectory adjustment; compare the actual peak position of the adjusted position trajectory with the target peak position; if the actual peak position appears earlier than the target peak position, increase the skewness coefficient 'a' of the corresponding Gaussian distribution. n Or kurtosis coefficient b n If the actual peak position appears later than the target peak position, then the skewness coefficient α of the corresponding Gaussian distribution should be reduced. n Or kurtosis coefficient b n The skewness coefficient a of each Gaussian distribution n Or kurtosis coefficient b n After increasing or decreasing, new values ​​s, v, and a are obtained. This process is iterated until the actual peak position coincides with the target peak position, at which point the peak position is determined. Then, the actual deviation distance of the adjusted position trajectory is compared with the target deviation distance. When the actual deviation distance is less than the target deviation distance, the scaling factor c of the corresponding Gaussian distribution is increased. n When the actual deviation distance is greater than the target deviation distance, the scaling factor c of the corresponding Gaussian distribution is reduced. n The scaling factor c of each Gaussian distribution n After increasing or decreasing, new values ​​s, v, and a are obtained; this process is repeated until the actual offset distance equals the target offset distance, resulting in a new position trajectory.

[0029] Furthermore, in step 3, the expression for the attitude-oriented dynamic motion primitive model is:

[0030]

[0031] In the formula, τ is a time constant, and q g Let q be the quaternion representing the trajectory target attitude, ω be the quaternion attitude quantity of the trajectory motion point, α be the angular velocity of the trajectory point, k be the damping coefficient of the model, and f be a nonlinear forcing term.

[0032] Furthermore, the method for obtaining the new attitude trajectory is as follows: τ, q, ω, α of the attitude demonstration trajectory and the attitude endpoint q of the target trajectory are combined. gThe input is fed into a posture-oriented dynamic motion primitive model, which extracts the motion features of the posture demonstration trajectory. Then, a nonlinear forcing term is used to fit these motion features, reproducing the motion features of the posture demonstration trajectory to obtain the reproduced posture trajectory. Finally, based on the input target trajectory's q... g As the endpoint of the attitude trajectory, the reproduced attitude trajectory is generalized to obtain an attitude trajectory that changes the endpoint but retains the motion characteristics. The generalized attitude trajectory is the new attitude trajectory, and the new attitude trajectory is output.

[0033] A trajectory planning and adjustment system based on dynamic motion primitives includes a storage module connected to a data processing module, which in turn is connected to a position trajectory planning module and an attitude trajectory planning module. The storage module stores empirical trajectories containing position and attitude information from original path points. The data processing module includes a decoupling unit, a position trajectory processing unit, and an attitude trajectory processing unit. The decoupling unit extracts independent position and attitude trajectories from the empirical trajectories. The position trajectory processing unit performs velocity planning preprocessing on the independent position trajectories to obtain a continuous and smooth position demonstration trajectory at the acceleration level. The attitude trajectory processing unit performs velocity planning preprocessing on the independent attitude trajectories to obtain a continuous and smooth attitude demonstration trajectory at the acceleration level. The position trajectory planning module includes a position feature extraction unit, a position feature fitting unit, a perturbation unit, and a loop optimization unit. The position feature extraction unit extracts the motion features of the position demonstration trajectory, and the position feature fitting unit uses a nonlinear forcing term to fit the motion features, thereby reproducing the motion features of the position demonstration trajectory. The position trajectory is then determined. The perturbation unit uses a constructed dynamic primitive coupling term to provide perturbation, adjusting the shape of the reproduced position trajectory towards the target trajectory to obtain the adjusted position trajectory. The iterative optimization unit includes a peak optimizer and a deviation distance optimizer. The peak optimizer compares the actual peak position of the adjusted position trajectory with the target peak position, iteratively adjusting the position trajectory until the actual peak position coincides with the target peak position, thus determining the peak position. After the peak position is determined, the deviation distance optimizer compares the actual deviation distance of the adjusted position trajectory with the target deviation distance, iteratively adjusting the position trajectory until the actual deviation distance and the target deviation distance are equal, thus obtaining a new position trajectory. The attitude trajectory planning module includes an attitude feature extraction unit, an attitude feature fitting unit, and a generalization learning unit. The attitude feature extraction unit extracts the motion features of the attitude demonstration trajectory. The attitude feature fitting unit uses a nonlinear forcing term to fit the motion features, reproducing the motion features of the attitude demonstration trajectory. The generalization learning unit performs generalization learning on the reproduced attitude trajectory to obtain a new attitude trajectory.

[0034] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0035] 1. This invention uses a dynamic motion primitive algorithm for offline trajectory planning, which can reproduce previously planned experience trajectories and change the trajectory shape and endpoint according to new work requirements; it can effectively solve the problem that similar but different product processing trajectories need to repeatedly rely on manual teaching; it greatly improves the utilization rate of experience trajectories and improves the efficiency of trajectory planning programming.

[0036] 2. The method of this invention has good adaptability to both multi-dimensional position and attitude information of the trajectory. Through pose-decoupled velocity planning preprocessing, the learned and reproduced trajectory can have excellent continuity and smoothness, meeting the quality requirements of the trajectory in applications.

[0037] 3. The dynamic motion primitive algorithm used in this invention can retain the motion characteristics of the original demonstration trajectory when reproducing the trajectory, adjusting its shape, and generalizing the learning process. It only makes adjustments to the local area or the trajectory endpoint for new work requirements, ensuring that the generated trajectory can retain prior knowledge in practical applications while having good adaptability to new work objectives.

[0038] In summary, this invention enables the planning of machining trajectories for products with similar structures by reproducing and adjusting existing empirical trajectories. Compared to manual teaching, which requires confirmation and programming of each trajectory point, this invention can eliminate these steps by directly generating trajectories. This significantly reduces the workload of repetitive teaching and programming, simplifies the difficulty of robot trajectory planning, and allows for flexible offline programming that can absorb experience and improve the utilization rate of past successful trajectories. This solves the problem of repetitive reliance on manual teaching for robot trajectory planning. Attached Figure Description

[0039] Figure 1 This is a flowchart of Embodiment 1 of the present invention;

[0040] Figure 2 This is a flowchart of the trajectory velocity planning preprocessing in Embodiment 1 of the present invention;

[0041] Figure 3 This is a diagram illustrating the positional trajectory of Embodiment 1 of the present invention.

[0042] Figure 4 This is a diagram illustrating the attitude demonstration trajectory of Embodiment 1 of the present invention;

[0043] Figure 5 This is a diagram showing the position trajectory effect after reproduction of Embodiment 1 of the present invention;

[0044] Figure 6 This is a diagram illustrating the effect of reproducing and adjusting the position trajectory features in Embodiment 1 of the present invention.

[0045] Figure 7This is a diagram showing the attitude trajectory effect after reproduction of Embodiment 1 of the present invention;

[0046] Figure 8 This is a diagram showing the effect of reproducing and adjusting the attitude trajectory features in Embodiment 1 of the present invention. Detailed Implementation

[0047] Example 1

[0048] A trajectory planning and adjustment method based on dynamic motion primitives, such as Figure 1 As shown, it includes the following steps:

[0049] Step 1: Obtain the empirical trajectory, which includes position and attitude information from the original path points, as the input data for the method. In this embodiment, the empirical trajectory is obtained from a robot grinding work trajectory taught manually. The empirical trajectory includes position path point information and attitude path point information represented by a matrix. The composition of the empirical trajectory is shown in Table 1.

[0050] Table 1. Original pathpoint locations and attitude information of the empirical trajectory

[0051]

[0052] Step 2, as follows Figure 2 As shown, the original path points on the empirical trajectory are first decoupled by pose, and independent position and attitude information are extracted to obtain independent position trajectories and attitude trajectories. The independent position trajectory is represented by the position information of the path point in the x, y, and z directions in the Cartesian coordinate system; the independent attitude trajectory is represented by the attitude information of the path point in the form of quaternions, including the rotation angle of the robot end effector with respect to the x-axis, y-axis, and z-axis.

[0053] Then, using the expected number of interpolation points of the target trajectory, the maximum speed, maximum acceleration, maximum jerkness, and maximum spasm as constraint parameters, velocity planning preprocessing is performed on the independent position trajectories using 15-segment S-curve velocity planning to obtain the position demonstration trajectory as follows: Figure 3 As shown in Table 2, the normalized velocity values ​​of the position demonstration trajectories are calculated based on these parameters. The constraint parameters are shown in Table 2, and the position demonstration trajectories are shown in Table 3.

[0054] Table 2. Constraint Parameters for Velocity Planning of 15 S-Curves

[0055] Parameter value 50 60 120 200 600

[0056] Table 3. Example Position Trajectory After Velocity Planning Preprocessing

[0057] ... ... 150 [5.31,-1040.21,-844.68,-352.83,60.00,0,-0.67,0,0,-0.0000051] 151 [5.35,-1038.07,-844.68,-352.85,60.00,0,-0.67,0,0,-0.0000015] 152 [5.38,-1035.93,-844.68,-352.88,60.00,0,-0.67,0,0,-0.0000004] ... ...

[0058] Then, using the same number of interpolation points and the velocity normalization value of the demonstration trajectory, spherical linear interpolation is performed on the independent attitude trajectories to output the attitude demonstration trajectories. The output attitude demonstration trajectories after planning are shown in Table 4. The effect diagram of the attitude demonstration trajectory is shown below. Figure 4 As shown.

[0059] Table 4. Attitude demonstration trajectory after velocity planning preprocessing

[0060]

[0061] Step 3: Obtain the trajectory interval information and trajectory shape parameters of the position demonstration trajectory, and generate dynamic motion primitive coupling terms. First, construct the sigmoid system in the Gaussian radial basis function:

[0062]

[0063] Then, an expression for the virtual disturbance force Fc based on the Gaussian radial basis function and the initial control parameters of the sigmoid system is constructed:

[0064]

[0065] In the formula, a n b is the Gaussian distribution skewness coefficient of a set of basis functions; n c is the kurtosis coefficient of the Gaussian distribution. n r is the scaling factor for the Gaussian distribution; n d represents the slope coefficient for each sigmoid system. n σ represents the center offset coefficient for each sigmoid system; μ represents the average value of the position normalization value within the selected trajectory interval; and σ represents the standard deviation of the position normalization value within the selected trajectory interval.

[0066] Using the trajectory interval information of the position demonstration trajectory, the target direction of feature adjustment, deviation distance, and other parameters as constraints, the initial parameters for controlling each group of Gaussian radial basis functions are selected from the control parameter reference table. The control parameter reference table obtained using the control variable method is shown in Table 5. In this embodiment, the position demonstration trajectory is used as the object to adjust the shape of the trajectory features in the z-direction, and the constraints are shown in Table 6. Two Gaussian radial basis functions and their sigmoid system s are generated based on the input constraints. n The initial control parameters are shown in Table 7.

[0067] Table 5. Control Parameter Reference Table

[0068]

[0069] Table 6. Input Constraints for Coupling Terms

[0070] parameter [106,236] z direction 50

[0071] Table 7. Gaussian radial basis functions and initial control parameters of the sigmoid system

[0072] <![CDATA[a n ]]> 0.8 1.9 <![CDATA[b n ]]> 0.6 0.4 <![CDATA[c n ]]> 3.5 -2 <![CDATA[r n ]]> -20 -50 <![CDATA[d n ]]> 0.8 0.3

[0073] Then construct the deflection coupling term. The expression:

[0074]

[0075] In the formula, R represents a deflection matrix; v represents the velocity of the moving point; γ is a scaling variable; θ represents the angle between the virtual disturbance force vector and the velocity direction; β is a constant coefficient; in this embodiment, the values ​​of the scaling coefficient and the constant coefficient in the deflection coupling term are γ = 40 and β = 5 / Π, respectively.

[0076] Finally, construct the dynamic primitive coupling terms:

[0077] Step 4: Construct a dynamic motion primitive algorithm oriented towards position and attitude.

[0078] Construct a position-oriented dynamic motion primitive model, the expression of which is:

[0079]

[0080] In the formula, τ is a time constant, corresponding to the trajectory's motion time; g is the trajectory's target endpoint; s is the three-dimensional displacement of the trajectory's moving point; v is the trajectory point's velocity; a is the trajectory point's acceleration; k = 25 is the model's damping coefficient; and f is a nonlinear forcing term. The initial inputs s, v, and a are the s, v, and a values ​​of the position demonstration trajectory.

[0081] First, the parameters τ, s, v, and a of the demonstration trajectory, the constraints, and the target trajectory g are input into the position-oriented dynamic motion primitive model. Then, the position-oriented dynamic motion primitive model uses the demonstration trajectory as a learning sample to extract its motion features. Finally, a nonlinear forcing term is used to fit these motion features, thus reproducing the motion features of the demonstration trajectory, resulting in the reproduced position trajectory. Figure 5 , Figure 5 The black area represents the sample location trajectory, and the red area represents the reproduced location trajectory. Then, C is used. t By providing perturbations to the position-oriented dynamic motion primitive model, the shape of the reproduced position trajectory is adjusted to resemble the target trajectory, resulting in the adjusted position trajectory.

[0082] Based on the input constraints, the adjusted position trajectory is compared and evaluated. Based on the comparison results, the control parameters of each set of Gaussian radial basis functions in the virtual disturbance force are iteratively optimized, changing the distribution of the virtual disturbance force within the trajectory interval, until the obtained position trajectory satisfies the shape of the target trajectory. The iteratively optimized position trajectory is then output. The specific method is as follows:

[0083] Based on the interval information of the input positional trajectory, the target direction of feature adjustment, and the deviation distance, the control point positions for trajectory adjustment are calculated, including: the starting position, ending position, and peak position of the maximum deviation distance of the positional trajectory. The peak position of the positional trajectory with the maximum deviation distance is recorded as the target peak position. The actual peak position of the adjusted positional trajectory with the maximum deviation distance is recorded as the actual peak position. The actual peak position is compared with the target peak position. If the actual peak position appears earlier than the target peak position, the skewness coefficient 'a' of the corresponding Gaussian distribution is increased. n Or kurtosis coefficient b n If the actual peak position appears later than the target peak position, then the skewness coefficient α of the corresponding Gaussian distribution should be reduced. n Or kurtosis coefficient b n This makes the actual wave crest position closer to the target wave crest position; in this embodiment, the minimum optimization amount for the two sets of Gaussian distribution skewness coefficients a1 and a2 used is 0.0125, and the skewness coefficient a1 of the Gaussian distribution is... n Or kurtosis coefficient b n After increasing or decreasing, new values ​​s, v, and a are obtained. This process is iterated until the actual peak position coincides with the target peak position, at which point the peak position is determined. Once the peak position is determined, the actual deviation distance of the adjusted position trajectory is compared with the target deviation distance (the target deviation distance is a user-defined input). When the actual deviation distance is less than the target deviation distance, the scaling factor c of the corresponding Gaussian distribution is increased according to the ratio between the target and actual deviation distances. n (c n Multiply by the ratio of the target deviation distance to the actual deviation distance. When the actual deviation distance is greater than the target deviation distance, decrease the scaling factor c of the corresponding Gaussian distribution. n This makes the deviation distance of the position trajectory adjustment area approximate the target deviation distance in the target direction. The scaling factor c of each Gaussian distribution... n After increasing or decreasing, new values ​​s, v, and a are obtained; this process is iterated until the actual offset distance equals the target offset distance. At this point, the peak position and offset distance of the position trajectory shape adjustment area in the target direction satisfy the input constraints. The parameters after iterative optimization are shown in Table 8, and the resulting image of the new position trajectory after reproduction and feature adjustment is shown in Table 8. Figure 6 , Figure 6The image shows the position trajectory after feature shape adjustment in the z-direction. Black represents the example trajectory, while red represents the reproduced and adjusted new position trajectory. The points in the final reproduced and adjusted new position trajectory are shown in Table 9.

[0084] Table 8. Gaussian radial basis function and control parameters of sigmoid system after loop optimization

[0085]

[0086]

[0087] Table 9. Reproduced and Feature-Adjusted Location Trajectories

[0088] ... ... 150 [5.31,-1040.41,-844.68,-316.11,60.00,0,26.27,0,0,-12.87] 151 [5.35,-1038.27,-844.68,-315.19,60.00,0,25.48,0,0,-12.78] 152 [5.38,-1036.12,-844.68,-314.31,60.00,0,24.62,0,0,-12.54] ... ...

[0089] Construct a dynamic motion primitive model oriented towards attitude, its expression is:

[0090]

[0091] In the formula, τ is a time constant, corresponding to the motion time of the attitude demonstration trajectory; q g Let τ, q, ω, and α represent the trajectory target attitude in quaternion form; q is the quaternion attitude quantity of the trajectory motion point; correspondingly, ω is the angular velocity of the trajectory point; α is the angular acceleration of the trajectory point; k = 25 is the damping coefficient of the model; and f is a nonlinear forcing term. The attitude parameters τ, q, ω, and α of the demonstration trajectory and the attitude endpoint q of the target trajectory are then compared. g The input is fed into a posture-oriented dynamic motion primitive model. The model first uses the posture demonstration trajectory as a learning sample to extract the motion features of the trajectory. Then, a nonlinear forcing term is used to fit the motion features, achieving a reproduction of the motion features of the posture demonstration trajectory. The reproduction effect is as follows: Figure 7 , Figure 7 The black line represents the attitude demonstration trajectory, and the red line represents the reproduced attitude trajectory. Then, based on the target trajectory's operational requirements, the new target trajectory's q... g As the endpoint of the trajectory, the dynamic motion primitive model generalizes the attitude demonstration trajectory to obtain an attitude trajectory that changes the endpoint but retains the motion characteristics, i.e., obtains new q, ω, and α to adapt to new working conditions. In this embodiment, the endpoint attitude q of the input target trajectory... g Given a value of [0.5, 0.5, 0.5, 0.5], the obtained pose trajectory reference after reproduction and generalization learning is attached. Figure 8 The trajectory effect is shown in Table 10, where black represents the demonstration trajectory and red represents the reproduced trajectory. The points in the trajectory are shown in Table 10.

[0092] Table 10. Pose trajectories after reproduction and generalization learning

[0093]

[0094]

[0095] This embodiment first obtains the original path points of the empirical trajectory, then performs pose decoupling and velocity planning preprocessing on the empirical trajectory, and then constructs a position dynamic motion primitive coupling term according to the trajectory target. Finally, it reproduces, adjusts features, and generalizes the trajectory through a position and attitude-oriented dynamic motion primitive model. This embodiment uses a dynamic motion primitive algorithm for offline trajectory planning to reproduce the prior trajectory and change the trajectory shape and endpoint according to new work requirements. This solves the problem of repeatedly relying on manual teaching for similar but different product processing trajectories, greatly improving the utilization rate of the prior trajectory and increasing the efficiency of trajectory planning programming. This embodiment, through pose decoupling and velocity planning preprocessing, makes the learned and reproduced trajectory have excellent continuity and smoothness, and has good adaptability to multi-dimensional position and attitude information of the trajectory, which can meet the quality requirements of the trajectory in the application. The dynamic motion primitive algorithm used in this embodiment retains the motion characteristics of the original demonstration trajectory when reproducing, adjusting the shape, and generalizing the trajectory, and only adjusts the local or trajectory endpoints for new work requirements. The generated trajectory can retain prior knowledge in practical applications and has good adaptability to new work targets.

[0096] Example 2

[0097] A trajectory planning and adjustment system based on dynamic motion primitives includes a storage module connected to a data processing module, which in turn is connected to a position trajectory planning module and an attitude trajectory planning module. The storage module stores empirical trajectories containing position and attitude information from the original path points.

[0098] The data processing module includes a decoupling unit, a position trajectory processing unit, and an attitude trajectory processing unit. The decoupling unit is used to extract independent position trajectories and attitude trajectories from empirical trajectories. The position trajectory processing unit is used to perform velocity planning preprocessing on the independent position trajectories to obtain a continuous and smooth position demonstration trajectory at the acceleration level. The attitude trajectory processing unit is used to perform velocity planning preprocessing on the independent attitude trajectories to obtain a continuous and smooth attitude demonstration trajectory at the acceleration level.

[0099] The position trajectory planning module includes a position feature extraction unit, a position feature fitting unit, a perturbation unit, and a loop optimization unit. The position feature extraction unit is used to extract the motion features of the position demonstration trajectory. The position feature fitting unit is used to fit the motion features using a nonlinear forcing term to reproduce the motion features of the position demonstration trajectory, thus obtaining the reproduced position trajectory. The perturbation unit is used to provide perturbation using a constructed dynamic primitive coupling term to adjust the shape of the reproduced position trajectory towards the target trajectory, thus obtaining the adjusted position trajectory.

[0100] The iterative optimization unit includes a peak optimizer and a deviation distance optimizer. The peak optimizer compares the actual peak position of the adjusted position trajectory with the target peak position, iteratively adjusts the position trajectory until the actual peak position coincides with the target peak position, and determines the peak position. After the peak position is determined, the deviation distance optimizer compares the actual deviation distance of the adjusted position trajectory with the target deviation distance, iteratively adjusts the position trajectory until the actual deviation distance and the target deviation distance are equal, and obtains a new position trajectory.

[0101] The attitude trajectory planning module includes an attitude feature extraction unit, an attitude feature fitting unit, and a generalization learning unit. The attitude feature extraction unit is used to extract the motion features of the attitude demonstration trajectory. The attitude feature fitting unit is used to fit the motion features using a nonlinear forcing term to reproduce the motion features of the attitude demonstration trajectory. The generalization learning unit is used to perform generalization learning on the reproduced attitude trajectory to obtain a new attitude trajectory.

Claims

1. A dynamic motion primitive based trajectory planning and adjustment method, characterized in that, Includes the following steps: Step 1: Obtain the empirical trajectory from the original path points, including position and attitude information; Step 2: Extract independent position and attitude trajectories from the empirical trajectory, and perform velocity planning preprocessing on the independent position and attitude trajectories to obtain continuous and smooth position and attitude demonstration trajectories at the acceleration level. Step 3, first construct the dynamic primitive coupling term, denoted as Then, dynamic motion primitive models oriented towards position and attitude are constructed respectively, and the position demonstration trajectory and attitude demonstration trajectory are optimized to obtain new position trajectory and attitude trajectory. The method for constructing dynamic primitive coupling terms is as follows: First, construct the sigmoid system in the Gaussian radial basis functions: Then, a virtual disturbance force is constructed based on the Gaussian radial basis function and the initial control parameters of the sigmoid system therein. The expression: In the formula, The skewness coefficients of a Gaussian distribution of a set of basis functions; The kurtosis coefficient of the Gaussian distribution; This is the scaling factor for the Gaussian distribution; For each group of sigmoid systems, the slope coefficients are used. This refers to the center offset coefficient for each group of sigmoid systems; This represents the average value of the normalized position values ​​within the selected trajectory interval; This represents the standard deviation of the normalized values ​​of the positions within the selected trajectory interval; Then construct the deflection coupling term. The expression: In the formula, Represent a deflection matrix; Indicates the velocity of the moving point; For scaling variables; It is represented as the angle between the virtual disturbance force vector and the velocity direction; It is a constant coefficient; Finally, construct the dynamic primitive coupling terms: .

2. The trajectory planning and adjustment method based on dynamic motion primitives as described in claim 1, characterized in that, In step 2, the method for obtaining the position demonstration trajectory is as follows: for an independent position trajectory, using the expected number of interpolation points of the target trajectory, the maximum speed, maximum acceleration, maximum jerkness, and maximum spasm as constraint parameters, the time-optimal smooth position trajectory is calculated using a 15-segment S-curve velocity planning algorithm to obtain the position demonstration trajectory.

3. The trajectory planning and adjustment method based on dynamic motion primitives as described in claim 1 or 2, characterized in that, In step 2, the method for obtaining the attitude demonstration trajectory is as follows: First, calculate the velocity normalization value of the position demonstration trajectory, and then, using the desired number of interpolation points and the velocity normalization value of the position demonstration trajectory as constraint parameters, perform spherical linear interpolation programming on the independent attitude trajectory to obtain the attitude demonstration trajectory.

4. The trajectory planning and adjustment method based on dynamic motion primitives as described in claim 3, characterized in that, In step 3, the expression for constructing the position-oriented dynamic motion primitive model is as follows: In the formula, It is a time constant; The endpoint of the trajectory target; This represents the three-dimensional displacement of the trajectory point. The velocity of the trajectory point; The acceleration of the trajectory point; Here is the damping coefficient of the model; It is a nonlinear forcing term.

5. The trajectory planning and adjustment method based on dynamic motion primitives as described in claim 4, characterized in that, In step 3, the method for optimizing the position demonstration trajectory and obtaining a new position trajectory is as follows: First, the position demonstration trajectory is... , , and Constraints and the endpoint of the target trajectory The input is fed into a position-oriented dynamic motion primitive model; then, the position-oriented dynamic motion primitive model extracts the motion features of the position demonstration trajectory, and uses a nonlinear forcing term to fit the motion features to reproduce the motion features of the position demonstration trajectory, thus obtaining the reproduced position trajectory. Then, it is used... The model is perturbed to adjust the shape of the reproduced position trajectory to that of the target trajectory, resulting in an adjusted position trajectory. The adjusted position trajectory is then iteratively optimized to output a new position trajectory.

6. The trajectory planning and adjustment method based on dynamic motion primitives as described in claim 5, characterized in that, The method for iteratively optimizing the adjusted position trajectory is as follows: calculate the control point positions for trajectory adjustment based on the input constraints, including the starting position, ending position, target peak position, and target deviation distance of the position trajectory adjustment. Compare the actual peak position of the adjusted position trajectory with the target peak position. If the actual peak position appears earlier than the target peak position, increase the skewness coefficient of the corresponding Gaussian distribution. or kurtosis coefficient If the actual peak position appears later than the target peak position, the skewness coefficient of the corresponding Gaussian distribution should be reduced. or kurtosis coefficient The skewness coefficient of each Gaussian distribution or kurtosis coefficient After increasing or decreasing, a new result is obtained. , , The process is repeated until the actual peak position coincides with the target peak position, at which point the peak position is determined. Then, the actual deviation distance of the adjusted position trajectory is compared with the target deviation distance. When the actual deviation distance is less than the target deviation distance, the scaling factor of the corresponding Gaussian distribution is increased. When the actual deviation distance is greater than the target deviation distance, the scaling factor of the corresponding Gaussian distribution is reduced. Scaling factor of each Gaussian distribution After increasing or decreasing, a new result is obtained. , , The process is iterated until the actual offset distance equals the target offset distance, resulting in a new position trajectory.

7. The trajectory planning and adjustment method based on dynamic motion primitives as described in claim 3, characterized in that, In step 3, the expression for the attitude-oriented dynamic motion primitive model is: In the formula, It is a time constant. The trajectory target attitude is represented by a quaternion. For the quaternion attitude parameters of the trajectory movement point, the corresponding... Let ω be the angular velocity of the trajectory point. Let be the angular acceleration of the trajectory point. The damping coefficient of the model is... It is a nonlinear forcing term.

8. The trajectory planning and adjustment method based on dynamic motion primitives as described in claim 7, characterized in that, The method for obtaining a new attitude trajectory is as follows: [The text abruptly ends here, likely due to an incomplete sentence or a formatting error.] , , attitude endpoint of the target trajectory The input is fed into a posture-oriented dynamic motion primitive model. The model first extracts the motion features of the posture demonstration trajectory, fits these features using a nonlinear forcing term, and reproduces the motion features of the posture demonstration trajectory. Then, based on the input target trajectory... As the endpoint of the attitude trajectory, the reproduced attitude trajectory is generalized to obtain a new attitude trajectory.

9. A trajectory planning and adjustment system based on dynamic motion primitives, characterized in that, The system includes a storage module connected to a data processing module, which in turn is connected to a position trajectory planning module and an attitude trajectory planning module. The storage module stores empirical trajectories containing position and attitude information from original path points. The data processing module includes a decoupling unit, a position trajectory processing unit, and an attitude trajectory processing unit. The decoupling unit extracts independent position and attitude trajectories from the empirical trajectories. The position trajectory processing unit performs velocity planning preprocessing on the independent position trajectories to obtain a continuous and smooth position demonstration trajectory at the acceleration level. The attitude trajectory processing unit performs velocity planning preprocessing on the independent attitude trajectories to obtain a continuous and smooth attitude demonstration trajectory at the acceleration level. The position trajectory planning module includes a position feature extraction unit, a position feature fitting unit, a perturbation unit, and a loop optimization unit. The position feature extraction unit extracts motion features from the position demonstration trajectory. The position feature fitting unit uses a nonlinear forcing term to fit the motion features, reproducing the motion features of the position demonstration trajectory to obtain a reproduced position trajectory. The perturbation unit uses a constructed dynamic primitive coupling term to provide perturbation, adjusting the shape of the reproduced position trajectory towards the target trajectory to obtain an adjusted position trajectory. The iterative optimization unit includes a peak optimizer and a deviation distance optimizer. The peak optimizer compares the actual peak position of the adjusted position trajectory with the target peak position, iteratively adjusts the position trajectory until the actual peak position coincides with the target peak position, and determines the peak position. After the peak position is determined, the deviation distance optimizer compares the actual deviation distance of the adjusted position trajectory with the target deviation distance, iteratively adjusts the position trajectory until the actual deviation distance and the target deviation distance are equal, and obtains a new position trajectory. The attitude trajectory planning module includes an attitude feature extraction unit, an attitude feature fitting unit, and a generalization learning unit. The attitude feature extraction unit extracts the motion features of the attitude demonstration trajectory. The attitude feature fitting unit uses a nonlinear forcing term to fit the motion features and reproduce the motion features of the attitude demonstration trajectory. The generalization learning unit performs generalization learning on the reproduced attitude trajectory to obtain a new attitude trajectory.

Images