CN104070525B - For the method for space manipulator continuous trajectory tracking - Google Patents

Abstract

The invention discloses a kind of method for space manipulator continuous trajectory tracking.Comprise the following steps: first, a known track followed the tracks of by use inverse broad sense Jacobi method control mechanical arm tail end device.But because the accuracy of the method result and numerical integration step-length are inversely proportional to, so larger amount of calculation is needed to obtain separating more accurately.For fixed step size improves computational accuracy, Milne-Hamming linear multi step predicted correction method combines with described method by the present invention.Then, front four values before Runge-Kutta method calculating Milne-Hamming algorithm steps five are utilized.Apply the tracking that this continuous trajectory tracking method can complete continuous path in space manipulator task space more accurately.

Description

Method for Continuous Trajectory Tracking of Space Manipulator

technical field

The invention belongs to the field of path planning of a space manipulator, in particular to a method for continuous track tracking of a space manipulator.

Background technique

The path planning of the space manipulator can be divided into point-to-point planning and continuous trajectory planning. For the problem of continuous path planning, the current research mainly focuses on the problem of continuous path planning of ground robots and the research of linear continuous path tracking. In space missions, the continuous trajectory planning of space manipulators is a very important theoretical and practical problem. The problem is transformed into a solution of coupled differential equations based on the velocity-level kinematics equation and the conservation equation of angular momentum, and a predictive correction method for end-effector tracking is proposed, and the calculation accuracy is improved from the first order to a higher order.

For the control of the space manipulator, due to the conservation of momentum in the space environment, the space manipulator has dynamic redundancy characteristics. The pose of the end device of the space manipulator is not only related to the joint angle but also related to the history of the manipulator's motion. Therefore, the present invention uses the generalized Jacobian matrix to describe the system state of the space manipulator. Since the simple calculation of the inverse generalized Jacobian will inevitably bring singularity, resulting in infinite joint angular velocity near the singularity point, so this paper solves the joint angle by calculating the damped minimum variance inverse (DLS) of the generalized Jacobian. Space manipulator for motion control. Due to the use of DLS to avoid ambiguity, the error caused by the end device of the space manipulator reaching the desired pose will increase. In this paper, SVD decomposition is added, and the ambiguity avoidance calculation is only performed on the joints where ambiguity occurs to reduce the error. However, the accuracy of this method is low, and the paper introduces the linear multi-step prediction and correction method in the numerical analysis method to further reduce the error.

In the continuous trajectory tracking of the space manipulator in the task space, the main problem is to solve the inverse generalized Jacobian matrix. The basic decomposition motion control adopts the single-step method, and the Runge-Kutta method is more commonly used, which only uses The joint information of the previous point is used to calculate the next point, that is, only the joint angle Θ(t) at a certain moment is used to calculate the joint angle Θ(t+Δt) at the next moment. In this paper, the improved Milne-Hamming linear multi-step prediction and correction method is used to solve the continuous trajectory tracking problem of the space manipulator. It is expected to obtain higher accuracy under the large solution step size, so that the tracking path of the end effector is more accurate. Its advantage is that by making more use of the known joint information of the previous steps to determine the joint information of the next point, the local truncation error can be determined, and a correction item is included to improve the accuracy of each step, thereby completing the construction calculation. A generalized Jacobian matrix update algorithm for small and high precision space manipulators.

Contents of the invention

The purpose of the present invention is to: determine the joint information of the next point to improve the accuracy of each step by making more use of the known joint information of the previous steps; complete the generalized Jacobian matrix algorithm of the space manipulator with a small amount of calculation and high precision , so that the tracking of the continuous trajectory in the task space of the space manipulator is completed more accurately.

A method for continuous trajectory tracking of a space manipulator of the present invention is characterized in that the steps are as follows:

1) Using the inverse generalized Jacobian method and solving the inverse solution of kinematic velocity level, control the end device of the space manipulator to track a known trajectory;

2), combining the Milne-Hamming linear multi-step prediction correction method with the method;

3) Runge-Kutta method to solve the first four initial values required by the Milne-Hamming linear multi-step prediction and correction method;

4) Bring the joint angles of the manipulator obtained in steps 1)-3) into the kinematic equation of the space manipulator to obtain a corrected continuous path.

According to the method for continuous trajectory tracking of the space manipulator described above, preferably, there is a problem that the joint velocity of the space manipulator is infinite or cannot be calculated due to the singularity of the dynamics during the motion planning process, so when using the inverse generalized Jacques Before calculating the angles of each joint of the manipulator than the matrix, we must avoid singularities, and the damped minimum variance method DLS is used. Due to the use of DLS to avoid singularities, the error caused by the end device of the space manipulator reaching the desired pose will increase. Add SVD decomposition, and only perform singularity avoidance calculations for joints where singularities occur to reduce errors. The SVD decomposition formula is:

$\mathrm{<span\; class="notranslate">\; J</span>}<span\; class="notranslate">\; =</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; u</span>}}{\mathrm{<span\; class="notranslate">\; SV</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}$

The meaning of this equation is: for any matrix J can be decomposed into The dot product of the three matrices S, V ^T. in: V is an orthogonal matrix (n×n matrix), S is a diagonal matrix, and the elements s ₁ , s ₂ , ..., s ₆ on the diagonal are the singular values of the generalized Jacobian matrix, and satisfy S ₁ ≥S ₂ ≥…≥S ₆ ≥0

$\mathrm{<span\; class="notranslate">\; S</span>}<span\; class="notranslate">\; =</span>\left[\begin{array}{cccccc}{\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 5</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 6</span>}}\end{array}\right]<span\; class="notranslate">\; .</span>$

According to the above-mentioned method for the continuous trajectory tracking of the space manipulator, preferably, in combination with the improved Milne-Hamming linear multi-step prediction and correction method:

forecast: ${\mathrm{\&Theta;}}^m\left(t_{\mathrm{no}+1}\right)=\mathrm{\&Theta;}\left(t_{\mathrm{no}-3}\right)+\frac{4}{3}\mathrm{\&Delta;}t(2\stackrel{\&CenterDot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}}\right)-\stackrel{\&CenterDot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}-1}\right)+2\stackrel{\&Center\; Dot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}-2}\right))$

fix: ${\mathrm{\&Theta;}}^{mm}\left(t_{\mathrm{no}+1}\right)={\mathrm{\&Theta;}}^m\left(t_{\mathrm{no}+1}\right)+\frac{112}{121}({\mathrm{\&Theta;}}^h\left(t_{\mathrm{no}}\right)-{\mathrm{\&Theta;}}^m\left(t_{\mathrm{no}}\right))$

Correction: ${\mathrm{\&Theta;}}^h\left(t_{\mathrm{no}+1}\right)=\frac{1}{8}\left(9\mathrm{\&Theta;}\left(t_{\mathrm{no}}\right)-\mathrm{\&Theta;}\left(t_{\mathrm{no}-2}\right)\right)+\frac{3}{8}\mathrm{\&Delta;}t\left(\stackrel{\&Center\; Dot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}+1},{\mathrm{\&Theta;}}^{mm}\left(t_{\mathrm{no}+1}\right)\right)-2\stackrel{\&Center\; Dot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}}\right)-\stackrel{\&CenterDot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}-1}\right)\right)$

fix: $\mathrm{\&Theta;}\left(t_{\mathrm{no}+1}\right)={\mathrm{\&Theta;}}^h\left(t_{\mathrm{no}+1}\right)-\frac{9}{121}({\mathrm{\&Theta;}}^h\left(t_{\mathrm{no}+1}\right)-{\mathrm{\&Theta;}}^m\left(t_{\mathrm{no}+1}\right))$

Θ ^H is the correction item. When calculating Θ(t ₄ ), take Θ ^H (t ₃ )=Θ ^M (t ₃ ), Θ(t ₀ )=[000000] ^T .

According to the method for continuous trajectory tracking of the space manipulator described above, preferably, the first four initial values in claim 3 are obtained by using the Runge-Kutta single-step method. Using the fourth-order Milne-Hamming linear multi-step prediction and correction method, the values of the first four steps are known, and the joint angle value at the initial moment is Θ(t ₀ )=[000000] ^T , t ₁ , t ₂ , t The value at time ₃ is solved by the fourth-order classical Runge-Kutta method; the fourth-order classical Runge-Kutta formula is:

$\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 6</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; )</span>$

k ₁ =Δt*Θ(t _n ,Θ(t _n ))

${\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>$

${\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>$

k ₄ =Δt*Θ(t _n +Δt,Θ(t _n )+k ₃ ).

Due to the problem that the joint velocity is infinite or cannot be calculated due to the singularity of the dynamics in the motion planning process of the space robot, it is necessary to avoid the singularity before using the inverse generalized Jacobian matrix to find the joint angles of the manipulator. In this paper, the damped least variance method (DLS) is used:

J ^* ＝(J _m ^T J _m +λ ² I _M ) ^-1 J _m ^T

${\mathrm{<span\; class="notranslate">\; \&lambda;</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; ,</span>& \mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; f</span>}{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; the\; s</span>}}}_{\mathrm{<span\; class="notranslate">\; 6</span>}}<span\; class="notranslate">\; \&Greater\; Equal;</span>\mathrm{<span\; class="notranslate">\; \&epsiv;</span>}\\ <span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>\frac{{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; the\; s</span>}}}_{\mathrm{<span\; class="notranslate">\; 6</span>}}}{\mathrm{<span\; class="notranslate">\; \&epsiv;</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>{{\mathrm{<span\; class="notranslate">\; \&lambda;</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; ,</span>& \mathrm{<span\; class="notranslate">\; o</span>}\mathrm{<span\; class="notranslate">\; t</span>}\mathrm{<span\; class="notranslate">\; h</span>}\mathrm{<span\; class="notranslate">\; e</span>}\mathrm{<span\; class="notranslate">\; r</span>}\mathrm{<span\; class="notranslate">\; w</span>}\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; the\; s</span>}\mathrm{<span\; class="notranslate">\; e</span>}\end{array}\right.$

is the estimated value of the minimum singular value, ε is the selected threshold value for judging whether the manipulator is singular, and λ _m is the maximum damping value of the singular region set by the user. and It is obtained by SVD decomposition of the generalized Jacobian matrix.

$\mathrm{<span\; class="notranslate">\; J</span>}<span\; class="notranslate">\; =</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; u</span>}}{\mathrm{<span\; class="notranslate">\; SV</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}$

The meaning of this equation is: for any matrix J can be decomposed into The dot product of the three matrices S, V ^T. in: V is an orthogonal matrix (n×n matrix), S is a diagonal matrix, and the elements s ₁ , s ₂ , ..., s ₆ on the diagonal are the singular values of the generalized Jacobian matrix, and satisfy s ₁ ≥s ₁ ≥…≥s ₆ ≥0

$\mathrm{<span\; class="notranslate">\; S</span>}<span\; class="notranslate">\; =</span>\left[\begin{array}{cccccc}{\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 5</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 6</span>}}\end{array}\right]$

Using the fourth-order Milne-Hamming linear multi-step prediction and correction method, the values of the first four steps are known, the value at the initial moment is Θ(t ₀ )=[000000] ^T , and the values at the first three time points are calculated by four The first-order classical Runge-Kutta method is used to solve it.

The Runge-Kutta method is a commonly used and effective method. Its advantages are: the method is concise and easy to program; it can be extrapolated step by step with a fixed step length only by knowing the starting point; it has good numerical stability, that is, with the calculation With the increase of the step size, the error of the numerical solution will not increase or the error is bounded due to the influence of the initial error or rounding error.

The following is the fourth-order classical Runge-Kutta formula:

$\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 6</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; )</span>$

k ₁ =Δt*Θ(t _n ,Θ(t _n ))

${\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>$

${\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>$

k ₄ =Δt*Θ(t _n +Δt,Θ(t _n )+k ₃ )

Improve the basic Milne-Hamming and use the extrapolation principle, namely

$\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>\frac{\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 40</span>}}{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; 14</span>}}{\mathrm{<span\; class="notranslate">\; 45</span>}}{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>}{\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 40</span>}}<span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; 14</span>}}{\mathrm{<span\; class="notranslate">\; 45</span>}}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; o</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&Delta;t</span>}}^{\mathrm{<span\; class="notranslate">\; 6</span>}}<span\; class="notranslate">\; )</span>$

Among them, Θ ^M (t _n+1 ) is the explicit Milne method, and Θ ^H (t _n+1 ) is the implicit Hamming method.

It can be seen from the above formula that the accuracy of the algorithm has been improved by one order, ignoring the error term and rewriting as:

$\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&ap;</span>\frac{\mathrm{<span\; class="notranslate">\; 112</span>}}{\mathrm{<span\; class="notranslate">\; 121</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span>$

$\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&ap;</span><span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 9</span>}}{\mathrm{<span\; class="notranslate">\; 121</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span>$

Since Θ ^M (t _n+1 ) and Θ ^H (t _n+1 ) are the data obtained during the calculation, the above two formulas can be used to adjust the solution step size Δt, that is, to choose an appropriate step size so that

$<span\; class="notranslate">\; |</span><span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 9</span>}}{\mathrm{<span\; class="notranslate">\; 121</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; |</span><span\; class="notranslate">\; <</span>\mathrm{<span\; class="notranslate">\; \&epsiv;</span>}$

Where ε is the calculation accuracy required to be achieved.

Then the final revised Milne formula is:

${\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; 112</span>}}{\mathrm{<span\; class="notranslate">\; 121</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span>$

${\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 9</span>}}{\mathrm{<span\; class="notranslate">\; 121</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span>$

Combined with the Milne-Hamming predictive correction algorithm, then the revised Milne-Hamming predictive correction algorithm:

forecast: ${\mathrm{\&Theta;}}^m\left(t_{\mathrm{no}+1}\right)=\mathrm{\&Theta;}\left(t_{\mathrm{no}-3}\right)+\frac{4}{3}\mathrm{\&Delta;}t(2\stackrel{\&CenterDot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}}\right)-\stackrel{\&Center\; Dot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}-1}\right)+2\stackrel{\&CenterDot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}-2}\right))$

fix: ${\mathrm{\&Theta;}}^{mm}\left(t_{\mathrm{no}+1}\right)={\mathrm{\&Theta;}}^m\left(t_{\mathrm{no}+1}\right)+\frac{112}{121}({\mathrm{\&Theta;}}^h\left(t_{\mathrm{no}}\right)-{\mathrm{\&Theta;}}^m\left(t_{\mathrm{no}}\right))$

Correction: ${\mathrm{\&Theta;}}^h\left(t_{\mathrm{no}+1}\right)=\frac{1}{8}\left(9\mathrm{\&Theta;}\left(t_{\mathrm{no}}\right)-\mathrm{\&Theta;}\left(t_{\mathrm{no}-2}\right)\right)-\frac{3}{8}\mathrm{\&Delta;}t\left(\stackrel{\&CenterDot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}+1},{\mathrm{\&Theta;}}^{mm}\left(t_{\mathrm{no}+1}\right)\right)-2\stackrel{\&Center\; Dot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}}\right)-\stackrel{\&Center\; Dot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}-1}\right)\right)$

fix: $\mathrm{\&Theta;}\left(t_{\mathrm{no}+1}\right)={\mathrm{\&Theta;}}^h\left(t_{\mathrm{no}+1}\right)-\frac{9}{121}({\mathrm{\&Theta;}}^h\left(t_{\mathrm{no}+1}\right)-{\mathrm{\&Theta;}}^m\left(t_{\mathrm{no}+1}\right))$

Θ ^H is the correction item. At the beginning of the algorithm, the initial value Θ(t ₁ )Θ(t ₂ )Θ(t ₃ ) is provided by the same-order single-step method. See the next section for the specific algorithm. When calculating Θ(t ₄ ), take Θ ^H (t ₃ )=Θ ^M (t ₃ ). Θ(t ₀ )=[000000] ^T in the present invention. Θ(t+Δt) has since been calculated.

Compared with the prior art, the present invention has the following advantages: 1. By making more use of the known joint information of the previous steps, the joint information of the next point is determined to improve the accuracy of each step; 2. The calculation amount of the structure is completed with small precision High generalized Jacobian matrix algorithm of the space manipulator, thus completing the tracking of the continuous trajectory in the task space of the space manipulator more accurately.

Description of drawings

The present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

Fig. 1 is a flow chart of a prediction correction method for continuous trajectory tracking provided by an embodiment of the present invention;

Fig. 2 is a structural schematic diagram of a 6-DOF mechanical arm used in the present invention;

Fig. 3 is the continuous trajectory of passing obstacles used in the present invention;

Fig. 4 is a trajectory tracking diagram of an end device provided by an embodiment of the present invention without using the Milne-Hamming method;

Fig. 5 is a trajectory tracking diagram of an end device using the Milne-Hamming method provided by an embodiment of the present invention;

Fig. 6 is a three-axis change diagram of the trajectory of the end device and the continuous path provided by the embodiment of the present invention;

Fig. 7 is a three-axis change diagram of the trajectory of the end device and the continuous path using the improved Milne-Hamming method provided by the embodiment of the present invention;

detailed description

The technical scheme of the invention is: firstly, use the pseudo-inverse generalized Jacobian method to control the terminator to track a known track. However, since the accuracy of the result of this method is inversely proportional to the step size of the numerical integration, a larger amount of calculation is required to obtain a more accurate solution. In order to improve the calculation accuracy by fixing the step size, the present invention combines the Milne-Hamming linear multi-step prediction and correction method with the above method. Then, use the Runge-Kutta method to calculate the first four values before Step 5 of the Milne-Hamming algorithm. It specifically includes the following links:

1. Establish the kinematics model of the space manipulator

A schematic diagram of the structure of the 6-DOF manipulator is shown in Figure 2. The D-H parameters of the robotic arm are shown in Table 1:

Table 1: D-H parameters of the 6-DOF space manipulator

Symbol means:

p _i ∈ R ³ (i=1,...,n) the position vector of the barycenter of link i in the inertial coordinate system;

The position vector of the end effector in the inertial coordinate system;

Linear speed of end finisher at a certain moment;

ω ₀ ∈ R ³ Angular velocity of base at a certain moment;

θi joint angle _i ;

Θ∈R ⁿ joint angular velocity;

joint angular acceleration;

m _i the mass of connecting rod i;

M system quality;

r _i the radius of connecting rod i (note: do not confuse r _i with r _i );

l the length of connecting rod _i ;

η=cos(ψ/2) The scalar part represented by the quaternion;

The vector part represented by q=ksin(ψ/2) quaternion;

Crossover operator (equivalent to "'");

if $r=\left[\begin{array}{c}{r}_x\\ r_{\mathrm{the\; y}}\\ r_z\end{array}\right],$ So $\tilde{r}=\left[\begin{array}{ccc}0& -r_z& r_{\mathrm{the\; y}}\\ r_z& 0& -r_x\\ -r_{\mathrm{the\; y}}& r_x& 0\end{array}\right]$

I _i ∈ R ^3×3 inertia matrix of connecting rod i (all connecting rods are regarded as cylinders):

${\mathrm{<span\; class="notranslate">\; I</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 12</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 3</span>}{\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 12</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 3</span>}{\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 12</span>}}{\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\end{array}\right]$

Ψ _b ∈ R ³ The attitude of the base represented by Euler angles;

ε selected threshold value for judging whether the manipulator is singular;

λ _m is the maximum damping value of the singular region set by the user;

The inverse solution formula of the generalized Jacobian matrix of the manipulator is:

$\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}<span\; class="notranslate">\; =</span>{<span\; class="notranslate">\; \&lsqb;</span>{\mathrm{<span\; class="notranslate">\; J</span>}}^{<span\; class="notranslate">\; *</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&Psi;</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; I</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&rsqb;</span>}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; \&nu;</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}\\ {\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}\end{array}\right]$

Due to the problem that the joint velocity is infinite or cannot be calculated due to the dynamic singularity in the motion planning process of the space robot, it is necessary to avoid the singularity before using the inverse generalized Jacobian matrix to find the joint angles of the manipulator. In this paper, the damped least variance method (DLS) is used:

J ^* ＝(J _m ^T J _m +λ ² I _M ) ^-1 J _m ^T

${\mathrm{<span\; class="notranslate">\; \&lambda;</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; ,</span>& \mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; f</span>}{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; the\; s</span>}}}_{\mathrm{<span\; class="notranslate">\; 6</span>}}<span\; class="notranslate">\; \&Greater\; Equal;</span>\mathrm{<span\; class="notranslate">\; \&epsiv;</span>}\\ <span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>\frac{{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; the\; s</span>}}}_{\mathrm{<span\; class="notranslate">\; 6</span>}}}{\mathrm{<span\; class="notranslate">\; \&epsiv;</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>{{\mathrm{<span\; class="notranslate">\; \&lambda;</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; ,</span>& \mathrm{<span\; class="notranslate">\; o</span>}\mathrm{<span\; class="notranslate">\; t</span>}\mathrm{<span\; class="notranslate">\; h</span>}\mathrm{<span\; class="notranslate">\; e</span>}\mathrm{<span\; class="notranslate">\; r</span>}\mathrm{<span\; class="notranslate">\; w</span>}\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; the\; s</span>}\mathrm{<span\; class="notranslate">\; e</span>}\end{array}\right.$

is the estimated value of the minimum singular value, ε is the selected threshold value for judging whether the manipulator is singular,

λ _m is the maximum damping value of the singular region set by the user. and is obtained by SVD decomposition of the generalized Jacobian matrix

$\mathrm{<span\; class="notranslate">\; J</span>}<span\; class="notranslate">\; =</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; u</span>}}{\mathrm{<span\; class="notranslate">\; SV</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}$

The meaning of this equation is: for any matrix J can be decomposed into The dot product of the three matrices S, V ^T. in: V is an orthogonal matrix (n×n matrix), S is a diagonal matrix, and the elements s ₁ , s ₂ , ..., s ₆ on the diagonal are the singular values of the generalized Jacobian matrix, and satisfy s ₁ ≥s ₁ ≥…≥s ₆ ≥0

$\mathrm{<span\; class="notranslate">\; S</span>}<span\; class="notranslate">\; =</span>\left[\begin{array}{cccccc}{\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 5</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 6</span>}}\end{array}\right]$

2. A known continuous path through obstacles

The known continuous path is shown in Figure 3.

3. Based on the improved Milne-Hamming linear multi-step prediction and correction method

Construct a linear multi-step method in the following form, where α _k and β _k are undetermined parameters, through Taylor expansion, determining α _k and β _k can make it reach p-order accuracy:

$\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\underset{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}}{\overset{\mathrm{<span\; class="notranslate">\; r</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\underset{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; r</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span>$

From the above formula, the Milne4-step 4-order explicit formula can be formed:

$\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; 4</span>}}{\mathrm{<span\; class="notranslate">\; 3</span>}}\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 2</span>}\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span>$

There are also Hamming 3-step 4-order implicit formulas:

$\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 8</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 9</span>}\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; 3</span>}}{\mathrm{<span\; class="notranslate">\; 8</span>}}\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span>$

Combining the above two formulas, the following Milne-Hamming prediction and correction algorithm can be obtained:

forecast: $\mathrm{\&Theta;}\left(t_{\mathrm{no}+1}\right)=\mathrm{\&Theta;}\left(t_{\mathrm{no}-3}\right)+\frac{4}{3}\mathrm{\&Delta;}t(2\stackrel{\&Center\; Dot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}}\right)-\stackrel{\&Center\; Dot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}-1}\right)+2\stackrel{\&CenterDot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}-2}\right))$

Correction: $\mathrm{\&Theta;}\left(t_{\mathrm{no}+1}\right)=\frac{1}{8}(9\mathrm{\&Theta;}\left(t_{\mathrm{no}}\right)-\mathrm{\&Theta;}\left(t_{\mathrm{no}-2}\right))+\frac{3}{8}\mathrm{\&Delta;}t(\stackrel{\&CenterDot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}+1}\right)-2\stackrel{\&Center\; Dot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}}\right)-\stackrel{\&Center\; Dot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}-1}\right))$

The following will improve the basic Milne-Hamming, using the extrapolation principle, that is

$\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>\frac{\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 40</span>}}{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; 14</span>}}{\mathrm{<span\; class="notranslate">\; 45</span>}}{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>}{\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 40</span>}}<span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; 14</span>}}{\mathrm{<span\; class="notranslate">\; 45</span>}}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; o</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&Delta;t</span>}}^{\mathrm{<span\; class="notranslate">\; 6</span>}}<span\; class="notranslate">\; )</span>$

O(Δt ⁶ ) is the error, where Θ ^M (t _n+1 ) is the explicit Milne method, and Θ ^H (t _n+1 ) is the implicit Hamming method. It can be seen from the above formula that the accuracy of the algorithm has been improved by one order, ignoring the error term and rewriting as:

$\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&ap;</span>\frac{\mathrm{<span\; class="notranslate">\; 112</span>}}{\mathrm{<span\; class="notranslate">\; 121</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span>$

$\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&ap;</span><span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 9</span>}}{\mathrm{<span\; class="notranslate">\; 121</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span>$

because and is the data obtained in the calculation process, and the above two formulas can be used to adjust the solution step size Δt, that is, to choose an appropriate step size so that

$<span\; class="notranslate">\; |</span><span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 9</span>}}{\mathrm{<span\; class="notranslate">\; 121</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; |</span><span\; class="notranslate">\; <</span>\mathrm{<span\; class="notranslate">\; \&epsiv;</span>}$

Where ε is the calculation accuracy required to be achieved.

Then the final revised Milne formula is:

${\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; 112</span>}}{\mathrm{<span\; class="notranslate">\; 121</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span>$

${\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 9</span>}}{\mathrm{<span\; class="notranslate">\; 121</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span>$

Combined with the Milne-Hamming predictive correction algorithm, then the revised Milne-Hamming predictive correction algorithm:

forecast: ${\mathrm{\&Theta;}}^m\left(t_{\mathrm{no}+1}\right)=\mathrm{\&Theta;}\left(t_{\mathrm{no}-3}\right)+\frac{4}{3}\mathrm{\&Delta;}t(2\stackrel{\&Center\; Dot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}}\right)-\stackrel{\&Center\; Dot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}-1}\right)+2\stackrel{\&Center\; Dot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}-2}\right))$

fix: ${\mathrm{\&Theta;}}^{mm}\left(t_{\mathrm{no}+1}\right)={\mathrm{\&Theta;}}^m\left(t_{\mathrm{no}+1}\right)+\frac{112}{121}({\mathrm{\&Theta;}}^h\left(t_{\mathrm{no}}\right)-{\mathrm{\&Theta;}}^m\left(t_{\mathrm{no}}\right))$

Correction: ${\mathrm{\&Theta;}}^h\left(t_{\mathrm{no}+1}\right)=\frac{1}{8}\left(9\mathrm{\&Theta;}\left(t_{\mathrm{no}}\right)-\mathrm{\&Theta;}\left(t_{\mathrm{no}-2}\right)\right)+\frac{3}{8}\mathrm{\&Delta;}t\left(\stackrel{\&Center\; Dot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}+1},{\mathrm{\&Theta;}}^{mm}\left(t_{\mathrm{no}+1}\right)\right)-2\stackrel{\&Center\; Dot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}}\right)-\stackrel{\&Center\; Dot;}{\mathrm{\&Theta;}}\left(t_{\mathrm{no}-1}\right)\right)$

fix: $\mathrm{\&Theta;}\left(t_{\mathrm{no}+1}\right)={\mathrm{\&Theta;}}^h\left(t_{\mathrm{no}+1}\right)-\frac{9}{121}({\mathrm{\&Theta;}}^h\left(t_{\mathrm{no}+1}\right)-{\mathrm{\&Theta;}}^m\left(t_{\mathrm{no}+1}\right))$

Θ ^H is the correction item. At the beginning of the algorithm, the initial value Θ(t ₁ )Θ(t ₂ )Θ(t ₃ ) is provided by the same-order single-step method. See the next section for the specific algorithm. When calculating Θ(t ₄ ), take Θ ^H (t ₃ )=Θ ^M (t ₃ ). Θ(t ₀ )=[000000] ^T in the present invention.

Since then, Θ(t+Δt) has been calculated, and the basic flow chart of the predictive correction continuous trajectory control algorithm in the task space of the space manipulator is shown in Figure 1.

4. Runge-Kutta single step

Using the fourth-order Milne-Hamming linear multi-step prediction and correction method, the values of the first four steps are known, and the joint angle value at the initial moment is Θ(t ₀ )=[000000] ^T , t ₁ , t ₂ , t The value at time ₃ is solved using the fourth-order classical Runge-Kutta method.

The Runge-Kutta method is a commonly used and effective method. Its advantages are: the method is concise and easy to program; it only needs to know the starting point value and can be extrapolated step by step with a fixed step length; it has good numerical stability, that is, with the calculation As the step size increases, the error of the numerical solution will not increase or the error will be bounded due to the influence of the initial error or rounding error.

The following is the fourth-order classical Runge-Kutta formula:

$\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 6</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; )</span>$

k ₁ =Δt*Θ(t _n ,Θ(t _n ))

${\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>$

${\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; \&Theta;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>$

k ₄ =Δt*Θ(t _n +Δt,Θ(t _n )+k ₃ )

The embodiments of the present invention are implemented on the premise of the technical solutions of the present invention, and detailed implementation methods and specific operation processes are given, but the protection scope of the present invention is not limited to the following embodiments. The mass characteristics and simulation parameters of each connecting rod are shown in Tables 2 and 3.

Table 2: Mass characteristics of each link of the space manipulator

Table 3: Simulation parameter table

It can be calculated by simulation that when the Milne-Hamming method is not used, the end point actually reached by the end device is not the set target point, and there is a certain error between the two. The position of the point where the end device actually arrives is as follows:

p _ef =[0.19640.9996-0.3055] ^T

Compared with the set position p _ed of the target point of the end device of the manipulator, the error between the two is: F=0.0066.

Now, when the step size is 0.1, 0.05, 0.01 when the Milne-Hamming method is not used and the step size is 0.1 when the Milne-Hamming method is used, the following results are obtained through simulation:

Δt 0.1 0.05 0.01 0.1 when using M-H f 0.0066 0.0027 0.0018 0.0013

The specific implementation steps are:

Step 1: SVD decomposition

Since the space robot has the problem that the joint speed is infinite or cannot be calculated due to the singularity of the dynamics in the motion planning process, it is necessary to avoid the singularity before using the inverse generalized Jacobian matrix to find the joint angles of the manipulator. This paper uses Damped least variance method (DLS). Due to the use of DLS to avoid ambiguity, the error caused by the end device of the space manipulator reaching the desired pose will increase. In this paper, SVD decomposition is added, and the ambiguity avoidance calculation is only performed on the joints where ambiguity occurs to reduce the error. However, the accuracy of this method is low, and the paper introduces the linear multi-step prediction and correction method in the numerical analysis method to further reduce the error.

Step 2: Runge-Kutta single-step method

Using the fourth-order Milne-Hamming linear multi-step prediction and correction method, the values of the first four steps are known, and the values at the initial moment are Θ(t ₀ )=[000000] ^T , at t=0.1,0.2,0.3 The values are solved using the fourth-order classical Runge-Kutta method.

The Runge-Kutta method is a commonly used and effective method. Its advantages are: the method is concise and easy to program; it can be extrapolated step by step with a fixed step length only by knowing the starting point; it has good numerical stability, that is, with the calculation With the increase of the step size, the error of the numerical solution will not increase or the error is bounded due to the influence of the initial error or rounding error.

Step 3: Based on the improved Milne-Hamming linear multi-step prediction and correction method

Accompanying drawing 4 is the trajectory tracking path diagram of the terminal device without using the Milne-Hamming linear multi-step prediction correction method; accompanying drawing 5 is the trajectory tracking path diagram of the end device using the Milne-Hamming linear multi-step prediction correction method. The green dotted line trajectory in the figure is a known continuous trajectory, and the red solid line is the trajectory tracking path of the 6-DOF space manipulator end device. It can be seen from Fig. 5 that the two traces basically overlap. It shows that the Milne-Hamming linear multi-step prediction and correction method of the present invention makes the end effector track the continuous trajectory more accurately.

Accompanying drawings 6 and 7 are three-axis change diagrams of end-effector tracking trajectory and continuous path before and after using the Milne-Hamming linear multi-step prediction correction method.

Claims (2)

1. for the method for space manipulator continuous trajectory tracking, it is characterized in that, step is as follows:

1), with inverse broad sense Jacobi method solve that kinematics velocity stage is inverse separates, control space manipulator end device and follow the tracks of a known track;

2), by Milne-Hamming linear multi step predicted correction Methods and steps 1) described in inverse broad sense Jacobi method combine;

3), utilization Runge-Kutta methods solves front four initial values needed for Milne-Hamming linear multi step predicted correction method;

4), by step 1)-3) each joint angle of mechanical arm that obtains brings space manipulator kinematical equation into and obtains the continuous path after correcting.

2. as claimed in claim 1 for the method for space manipulator continuous trajectory tracking, it is characterized in that, the joint velocity infinity or imponderable problem that cause because of dynamic singularity is there is in space manipulator in motion planning process, therefore to first keep away unusual before the inverse broad sense Jacobian matrix of utilization asks each joint angle of mechanical arm, employing be damping least variance method DLS; Owing to adopting DLS to keep away unusual, the error that space manipulator end device arrives expected pose generation can increase, and adds SVD and decomposes, and only carries out keeping away unusual calculating to reduce error for the unusual joint of generation; SVD decomposition formula is:

$J=\stackrel{\&OverBar;}{U}{\mathrm{SV}}^T$

The meaning of this equation is: can be analyzed to for Arbitrary Matrix J s, V ^tthe dot product of three matrixes; Wherein: v is orthogonal matrix (being n × n matrix), and S is diagonal matrix, the element s on diagonal ₁, s ₂... .., s ₆be the singular value of broad sense Jacobian matrix, and meet S ₁>=S ₂>=...>=S ₆>=0

$S=\left[\begin{array}{cccccc}{s}_1& 0& 0& 0& 0& 0\\ 0& s_2& 0& 0& 0& 0\\ 0& 0& s_3& 0& 0& 0\\ 0& 0& 0& s_4& 0& 0\\ 0& 0& 0& 0& s_5& 0\\ 0& 0& 0& 0& 0& s_6\end{array}\right].$