MINIMAL-SNAP Based Passive Motion Trajectory Generation Method

Abstract

The present invention discloses a passive motion trajectory generation method based on MINIMAL-SNAP. Initial conditions are set, and a sparse trajectory point sequence is allocated; the maximum allowable number of segments is set, the sparse trajectory point sequence is segmented, and a time allocation function is configured; a target constraint matrix is determined; a process constraint matrix is determined, including configuring derivative constraints and configuring continuity constraints; the trajectory parameter matrix within the segment is solved by callback, and the end-point state values are saved; all segments between two points are processed through traversal and loop, and then all points of the trajectory sequence are processed. The generated sequence is sent to the trajectory execution mechanism for execution to generate the trajectory. By giving a sparse and rough trajectory point sequence, the present invention autonomously completes trajectory smoothing and dynamic smoothing, and efficiently outputs trajectory points while taking into account the execution speed, making up for the defect that low-cost motion controllers cannot generate complex trajectories, and can provide a good passive motion rehabilitation experience for patients.

Description

Technical Field

[0001] The present invention relates to the technical field of robot system control, and particularly to a passive motion trajectory generation method based on MINIMAL-SNAP. Background Art

[0002] In rehabilitation medicine, the exercise therapy can be divided into active type and passive type according to the source of force. The movement caused by an external force acting on a part of the human body is called passive movement. Generally, it is used to maintain the normal or increase the limited joint range of motion, prevent muscle atrophy and joint contracture; the method of relying on the patient's own muscle strength for movement is called active movement. Patients with muscle strength above level 3 can perform active movement. Pure active movement generally does not give assistance or apply resistance, and is mainly used to maintain the joint range of motion, perform training to enhance muscle strength and endurance, and enhance the coordination between muscles. Passive movement is usually required to be able to complete a certain fixed pattern of movement, which is usually a fixed trajectory, specific actions, etc.

[0003] In desktop rehabilitation systems, many enterprises and research institutions use cross slides as mechanical linkage mechanisms. When patients use this platform for rehabilitation exercises, they place their upper arms on the hand support mechanism of the rehabilitation platform and perform passive exercises according to the trajectory selected by the caregiver, realizing the passive rehabilitation of the patient's upper limbs.

[0004] Taking the U-Resmart upper limb desktop rehabilitation robot as an example, in rehabilitation systems of the cross slide type, for patients with relatively weak muscle strength in the early stage, the device often needs to be able to autonomously complete trajectory recognition and drive the patient to complete rehabilitation training. For common motor controllers, they usually can only accept primitive control instructions such as the speed instruction at a certain moment or the position instruction at a certain moment. The motor controller needs to receive a good sequence of trajectory points to complete the movement action between two points or multiple points, and multiple good action executions are required to complete the fixed trajectory (such as a circle) pre-selected by the user.

[0005] In the generation of passive motion trajectories, there are some technical prerequisites that need to be considered in the design stage. For low-cost motor controllers without complex interpolation instructions, a sufficiently convenient and efficient trajectory generation module will have a positive impact on the entire passive motion experience. For example, interpolation of the motion trajectory is required between a relatively long sequence of trajectory points. More points can make the entire trajectory resolution higher and the entire passive motion smoother and softer, but overly dense trajectory points will significantly reduce the average execution speed of the entire trajectory; sudden starts and stops are not allowed during operation, and ensuring dynamic smoothness during operation largely determines the experience of the entire rehabilitation movement.

[0006] The present invention provides a passive motion trajectory generation method based on MINIMAL-SNAP, which can significantly optimize the experience of the patient's passive motion trajectory. Summary of the Invention

[0007] To address the deficiencies in the prior art, the object of the present invention is to provide a passive motion trajectory generation method based on MINIMAL-SNAP, which provides an idea for solving trajectory smoothing and dynamic smoothing in trajectory generation and proposes a relatively fast calculation method.

[0008] To achieve the object of the present invention, the technical solution adopted by the present invention is as follows:

[0009] A passive motion trajectory generation method based on MINIMAL-SNAP, comprising the steps of:

[0010] Step 1, perform initial condition settings, and allocate a sparse trajectory point sequence, including setting the initial position, initial velocity, and initial acceleration;

[0011] Step 2, set the maximum allowable number of trajectory segments, segment the sparse trajectory point sequence, and configure the allocation time function, configure the constrained acceleration, and configure the constrained velocity;

[0012] Step 3, determine the target constraint matrix; determine the process constraint matrix, including configuring the derivative constraint and configuring the continuity constraint; call back to solve the in-segment trajectory parameter matrix, save the end-point state value, and send the calculation result;

[0013] Step 4, traverse and loop through all segments between two points, and then process all points in the trajectory sequence, and send the sequence generated each time to the trajectory execution mechanism for execution to generate a trajectory.

[0014] Further, in Step 1, a sparse trajectory point sequence is pre-allocated, and the allocated sparse trajectory point information is stored using an extensible markup language-based data sequence representation method, and the corresponding sparse trajectory point information is stored in time or space order.

[0015] Further, in Step 2, every two discrete points form a combination, and then the two-point trajectory is segmented. The spatial information and kinematic information of the in-segment trajectory sequence are polynomially expressed as:

[0016] X(t) = c5t 5 +c 54 t 4 +c3t 3 +c2t 2 +c1t + c0

[0017] The velocity and acceleration can be obtained by taking the derivative of this polynomial.

[0018] Further, in step 2, every time two points are completed, the time parameter is reset and the timing restarts. At the end of each T n moment, the end timing will be reset to 0. Then each period of time is successively T n -T n-i .

[0019] Further, in step 2, a trapezoidal acceleration and deceleration is used to be responsible for the distribution of the time matrix. The time distribution function is as follows:

[0020]

[0021] where the distributed distance is dist, and the acceleration and deceleration distance is d.

[0022] Further, in step 3, the duration of each segment is obtained, and each segment is set as a column of the target constraint matrix M, and the derivative expression of M is made to satisfy the representation form of X(t). Then the target constraint matrix satisfies the following formula:

[0023]

[0024] Further, in step 3, to ensure the continuous and smooth movement of the trajectory points, derivative constraints and continuity constraints are imposed on the trajectory matrix; at the end position of the previous segment and the starting position of the next segment, both can pass through P i , then derivative constraints are imposed here, and the derivative constraints are:

[0025]

[0026] To ensure the continuity of acceleration, jerk, and snap in the generated trajectory matrix, at least ensure that each derivative is at P i , and the continuity constraint is:

[0027]

[0028] Further, in step 3, after the trajectory point segmentation processing, derivative constraints, and continuity constraints, the matrix M obtained each time is a banded non-singular matrix, b is a specified initial and end state constraint, a linear equation system Mc = b is constructed, and the parameter matrix c is solved therein;

[0029] The callback function is polled for execution. After each callback execution, the parameter matrix is obtained according to the callback. The obtained parameter matrix is combined with the time distribution function to synthesize the position, velocity, acceleration, and jerk information of the corresponding trajectory points; then the calculation result is sent to the actuator, and the last end state value is saved each time, and two consecutive points are published to the actuator each time.

[0030] The beneficial effects of the present invention are as follows. Compared with the prior art, the present invention can autonomously complete trajectory smoothing and dynamic smoothing when given a sparse and rough sequence of trajectory points, and efficiently output trajectory points while taking into account the execution speed, making up for the defect that low-cost motion controllers cannot generate complex trajectories, and can provide a good passive motion rehabilitation experience for patients.

[0031] The present invention performs time allocation when interpolating motion points between two points. This method can significantly optimize the motion time allocation of the motor between two points during motion planning, so that the motor will not have excessive motion caused by too long time allocation or insufficient motion track distance caused by too short time allocation when performing motion between two points.

[0032] The present invention uses polynomials to represent the motion trajectory of each point, which enables us to analyze the information of position, velocity, and acceleration at each moment, providing an effective model support for ensuring trajectory smoothing and dynamic smoothing.

[0033] The present invention optimizes the trajectory based on minimal snap. The trajectory generated by this method stores the trajectory information in the matrix in the form of a matrix. The generated trajectory not only has reasonable time allocation and moderate density between points, but also has smooth connection between points, avoiding sudden start and stop of the motor as much as possible, ensuring dynamic smoothing, further optimizing the passive motion trajectory, and making the whole process more comfortable. In addition, the whole trajectory calculation process is segmented, and the time and resources are reallocated every time a segment is divided. The matrix scale is small and the calculation is fast.

[0034] The present invention isolates the initial conditions before each execution of the whole module, which can be conveniently set, enabling the whole module to be easily transplanted and set. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 is a flowchart of the passive motion trajectory generation method based on MINIMAL-SNAP according to the present invention;

[0036] Figure 2 is a schematic diagram of sparse trajectory points of a rectangular motion trajectory;

[0037] Figure 3 is a schematic diagram of sparse trajectory points of a hexagonal motion trajectory;

[0038] Figure 4 is a schematic diagram of sparse trajectory points of a spiral motion trajectory;

[0039] Figure 5 is a flowchart of the time segmentation method;

[0040] Figure 6 is a schematic diagram of the constraint process;

[0041] Figure 7 is a schematic diagram of rectangular motion trajectory generation;

[0042] Figure 8 is a schematic diagram of hexagonal motion trajectory generation;

[0043] Figure 9 is a schematic diagram of spiral motion trajectory generation. Specific implementation manners

[0044] The technical solutions of the present invention will be further described below in conjunction with the accompanying drawings and embodiments. The following embodiments are only used to more clearly illustrate the technical solutions of the present invention, and cannot be used to limit the protection scope of this application.

[0045] As Figure 1 shown, the passive motion trajectory generation method based on MINIMAL-SNAP of the present invention includes the steps:

[0046] Step 1, perform initial condition settings, and allocate a sparse trajectory point sequence, including setting the initial position, initial velocity, and initial acceleration;

[0047] Before each run, it is usually necessary to set the initial state of the system. These settings usually include a sparse trajectory point allocation, setting the initial position point information, initial velocity, and initial acceleration.

[0048] In order to complete the motion trajectories in the geometric sense such as "circular", "rectangular", "polygonal", etc., it is necessary to pre-give a rough trajectory point sequence, and then perform corresponding output processing operations on the trajectory points at each moment. This sparse trajectory point sequence is the most basic execution sequence of the motion trajectory, and can be understood as the "skeleton" of the entire motion trajectory. It indicates the most basic shape of our entire trajectory and the chronological order of time. This sparse trajectory point sequence is usually pre-specified for the user by us, as Figure 2 and Figure 3 and Figure 4 shown, which gives the general form of such sparse trajectory points.

[0049] Adopt a data sequence representation method based on Extensible Markup Language to store the above-mentioned sparse trajectory point information. This method stores the corresponding sparse trajectory point information in time or space order and stores it as a self-defined format file. This can facilitate us to read the trajectory points therein, perform operations such as adding, deleting, searching, modifying, and visualizing the trajectory points therein, which is convenient for users to use and developers to analyze.

[0050] Isolate the initial conditions before each execution of the entire module, which can facilitate setting, so that the entire module can be easily transplanted and set.

[0051] Step 2: Set the maximum allowable number of trajectory segments, segment the sparse trajectory point sequence, and configure the time allocation function, configure the constrained acceleration, and configure the constrained velocity;

[0052] Take every two discrete points as a combination, and then segment the two-point trajectory. Each segment is a polynomial or a combination of polynomials.

[0053] Define a fifth-order polynomial at a certain moment to represent the spatial information and kinematic information equation X(t) at that moment. For all spatial position points within the segment, we use this expression to represent them. When information such as velocity and acceleration needs to be calculated, we only need to take the derivative of this polynomial. Then the polynomial expression of the trajectory sequence within the segment is:

[0054] X(t) = c5t 5 + c 54 t 4 + c3t 3 + c2t 2 + c1t + c0

[0055] Using polynomials to represent the motion trajectories of each point within the segment enables us to analyze the information of position, velocity, and acceleration at each moment, which provides an effective model support for ensuring trajectory smoothness and dynamic smoothness.

[0056] Take every two discrete points as a combination. After every two points are completed, the time parameter will be reset and the timing will start again. As shown in Figure 5 , the process of resetting the time is demonstrated. At the end of each T n moment, the timing will be reset to 0. Then each time segment is successively T n - T n-i . This is done to ensure that the relative time interval is calculated each time, and the resulting parameter matrix will not be too large.

[0057] After obtaining the sparse point sequence and completing the trajectory segmentation, a reasonable time parameter needs to be assigned between the trajectory points within the segment to ensure smooth and non-lagging trajectories. In practice, we use trapezoidal acceleration and deceleration to be responsible for the allocation of the time matrix.

[0058] Trapezoidal acceleration and deceleration means that during the motor startup phase, the motor is slowly accelerated to the limited speed, and during the stop phase, the motor is slowly decelerated from the limited speed to 0. The specific time allocation method is as follows: when two points within the segment are determined, the acceleration and deceleration distances are judged according to the allocated distance dist and the limited speed vel. If the allocated distance dist does not exceed twice the acceleration and deceleration distance d during the entire movement time, the acceleration and deceleration movement time is returned. If it is greater than or equal to two acceleration and deceleration distances of the movement time, the uniform movement time and the acceleration and deceleration time are returned. The time allocation function is:

[0059]

[0060] When performing motion point interpolation between two points, time allocation is carried out. This method can significantly optimize the motion time allocation of the motor between two points during motion planning, so that the motor will not have excessive motion caused by too long time allocation or insufficient motion track distance caused by too short time allocation when performing motion between two points.

[0061] Step 3: Determine the target constraint matrix; determine the process constraint matrix, including configuration derivative constraint and configuration continuity constraint; call back and solve the trajectory parameter matrix within the segment, save the end point state value, and send the calculation result.

[0062] The two-point trajectory is segmented, and each segment is a polynomial or a combination of polynomials. Minimal snap ensures that the trajectory is as dynamically smooth as possible. After obtaining the duration of each segment, each segment is set as a column of the target matrix M. The so-called target constraint of minimal snap is to make the derivative expression of M satisfy the representation form of X(t). For example, if it is determined that the snap is minimized, that is, the jerk is minimized, then the target constraint matrix requires at least 5th-order differentiation. That is, the target constraint matrix satisfies the following formula:

[0063]

[0064] The boundary value problem (BIVP) is often adopted by robot path planners based on waypoints or trajectory points. In the research on the proof of optimal conditions, many researchers have found that the solutions to such problems usually need to meet the following two intermediate conditions:

[0065] 1. When the order S of the target constraint is 3, a piecewise quintic polynomial spline curve with continuous capture, which is called a minimal jerk trajectory.

[0066] 2. When S = 4, a piecewise septic polynomial spline curve with continuous pop, which is called a minimal snap trajectory. And researchers have found in some scientific research practices that directly constructing the optimal solution is easier and more effective than implicit or explicit optimization, and the minimal snap trajectory scheme is adopted in practice.

[0067] To ensure the continuity of trajectory points and smooth motion, some methods are used to constrain the trajectory matrix. The specific process is as Figure 6 For example, the time when the trajectory passes through R i (t) runs from P i-1 to P i , and there are n such trajectory points in the figure.

[0068] Taking P1 as an example, in order to ensure that the trajectory generated after time R1 passes through P1 at the end position of the previous segment and the trajectory generated after time R2 passes through P1 at the starting position of the next segment, a derivative constraint can be imposed here and this treatment can be adopted for each subsequent segment. The derivative constraint is:

[0069]

[0070] In order to make the generated trajectory smooth dynamically and avoid sudden starts and stops of acceleration, it is necessary to ensure that the generated trajectory matrix is continuous in acceleration, jerk, and snap, that is, at least ensure that each derivative should be at P1 and the same treatment should be done for each subsequent segment. The continuity constraint is:

[0071]

[0072] After the above trajectory point segmentation processing, derivative constraint, and continuity constraint, the matrix M obtained each time is basically a banded non-singular matrix, b is a specified initial and final state constraint, and the PLU solver of Eigen3 can be called to perform fast calculation to obtain the parameter matrix c. Given the known time allocation, the position, velocity, acceleration, etc. information of each time period of the trajectory can be obtained. Due to our segmentation processing, the matrix scale is effectively controlled within the segment trajectory and the calculation speed is also relatively ideal.

[0073] After the above processing, the solution process is actually to construct a linear equation system to solve the parameter matrix c in it. The linear equation system is:

[0074] Mc = b

[0075]

[0076]

[0077] Among them, M is the in-segment trajectory matrix, which is a banded non-singular matrix; b is the spline matrix, which is a specified initial and final state constraint.

[0078] With the continuous polling execution of the callback function, it ensures the dense generation of trajectory points within each execution time period. After each callback execution, the coefficient matrix is obtained according to the callback. The obtained coefficient matrix can be combined with the time allocation function to call the corresponding getPos(), getVel(), getAcl(), getJerk(), getSnap() to synthesize the position, velocity, acceleration, jerk, and snap information of the corresponding trajectory points. The optimal condition ensures that the matrix M is non-singular each time, and each time interval is positive. Obtaining this solution only requires linear time. When sending to the actuator, a copy of the previous end state value is saved each time, and two consecutive points are published to the actuator each time.

[0079] Step 4, traverse and loop through all segments between two points, and then process all points in the trajectory sequence. Send the sequence generated each time to the trajectory execution mechanism for execution to generate a trajectory.

[0080] In the above text Figure 2 、 Figure 3 、 Figure 4 The corresponding generated trajectories are as Figure 7 and 8 and shown in Figure 9. It should be noted that the denser the points, the better the fit to the trajectory. For example, to complete a very fitting rectangle or polygon, usually in addition to the corner points, more points should also be arranged on the corresponding sides. The trajectory shows a better fit as the density increases.

[0081] Taking the example of providing data for two points and ten segments, a comparison was made with the quadratic programming method using MATLAB, and it was found that there was a significant improvement in the calculation speed using the optimization method. The comparison of the trajectory generation speed is shown in Table 1.

[0082] Table 1

[0083] Method for generating trajectory Time consumption (ms) MATLAB quadprog.m 9.51 C++ minimal snap 0.34

[0084] The present invention can autonomously complete trajectory smoothing and dynamic smoothing in the case of a given sparse and rough trajectory point sequence, and efficiently output trajectory points while taking into account the execution speed, making up for the defect that low-cost motion controllers cannot generate complex trajectories, and can provide a good passive motion rehabilitation experience for patients.

[0085] The applicant of the present invention has made a detailed description and illustration of the embodiments of the present invention in combination with the accompanying drawings of the specification. However, those skilled in the art should understand that the above embodiments are only the preferred implementation schemes of the present invention, and the detailed description is only to help readers better understand the spirit of the present invention, rather than a limitation on the protection scope of the present invention. On the contrary, any improvement or modification made based on the spirit of the present invention should fall within the protection scope of the present invention.

Claims

1. A passive motion trajectory generation method based on MINIMAL-SNAP, characterized in that, Including the steps: Step 1, perform initial condition setting and allocate a sparse trajectory point sequence, including setting the initial position, initial velocity, and initial acceleration; Step 2, set the maximum allowable number of trajectory segments, segment the sparse trajectory point sequence, and configure the allocation time function, configure the constrained acceleration, and configure the constrained velocity; Take every two discrete points as a combination, and then segment the trajectory of the two points. The spatial information and kinematic information of the trajectory sequence within the segment are polynomially expressed as: X(t) = c5t 5 + c4t 4 + c3t 3 + c2t 2 + c1t + c0 Taking the derivative of this polynomial can obtain the velocity and acceleration; Every time two points are completed, the time parameter is reset and the timing restarts. At the end of each T n moment, the timing will be reset to 0. Then each period of time is successively T n -T n-i ; Step 3, determine the target constraint matrix; Determine the process constraint matrix, including configuring the derivative constraint and configuring the continuity constraint; call back to solve the trajectory parameter matrix within the segment, save the end point state value, and send the calculation result; Obtain the duration of each segment, set each segment as a column of the target constraint matrix M, and make the derivative expression of M satisfy the representation form of X(t). Then the target constraint matrix satisfies the following formula: Step 4, traverse and loop to process all segments between two points, and then process all points of the trajectory sequence. Send the sequence generated each time to the trajectory execution mechanism for execution to generate a trajectory.

2. The passive motion trajectory generation method based on MINIMAL-SNAP according to claim 1, wherein, In Step 1, pre-allocate the sparse trajectory point sequence, and use the data sequence representation method based on Extensible Markup Language to store the allocated sparse trajectory point information, and store the corresponding sparse trajectory point information in time or space order.

3. The passive motion trajectory generation method based on MINIMAL-SNAP according to claim 1, wherein In Step 2, use the trapezoidal acceleration and deceleration to be responsible for the allocation of the time matrix, and the time allocation function is: where the allocated distance is dist, and the acceleration and deceleration distance is d.

4. The passive motion trajectory generation method based on MINIMAL-SNAP according to claim 1, wherein In step 3, to ensure the smooth continuous movement of the trajectory points, derivative constraints and continuity constraints are imposed on the trajectory matrix; at the end position of the previous paragraph and the start position of the next paragraph, both can pass through P i , then derivative constraints are imposed here, and the derivative constraints are: Ensure that the generated trajectory matrix is continuous in terms of acceleration, jerk, and snap, which means at least ensuring the continuity of each derivative at point P i , and the continuity constraint is as follows:

5. The passive motion trajectory generation method based on MINIMAL-SNAP according to claim 4, wherein In Step 3, after the trajectory point segmentation process, derivative constraint, and continuity constraint, the matrix M obtained each time is a banded non-singular matrix, b is a specified initial and end state constraint, construct a linear equation system Mc = b, and solve the parameter matrix c therein; Poll and execute the callback function. After each callback execution, obtain the parameter matrix according to the callback. Combine the obtained parameter matrix with the time allocation function to synthesize the position, velocity, acceleration, and jerk information of the corresponding trajectory points; then send the calculation result to the actuator, save a copy of the previous end state value each time, and publish continuously to the actuator while maintaining the continuity of two points each time.

Images