CN103713654B - The rolling window non-causal inverse control method that flexible arm terminal end path is followed the tracks of - Google Patents

Abstract

The invention discloses the rolling window non-causal inverse control method that a kind of flexible arm terminal end path is followed the tracks of, mainly comprise the steps that 1) based on the assumption that modal method sets up the model of flexible arm, it is determined that the window width of rolling window non-causal inverse control method；2) in first window width, according to the non-causal for non-minimum phase system against the requirement of technology, design flexible arm joint trajectories, and adopt non-causal inverse approach to obtain control input corresponding to system initial time and reference state, then start system；3) with fixing step-length rolls forward window, i.e. the task according to path trace, provide the desired trajectory obtained after planning with fixing step-length in real time, obtain follow-up control by calculating and input and act on system, until system end of run。The present invention relaxes the track following requirement to flexible arm end, completes the task of flexible arm end on-line tracing expected path in the way of the control of planning limit, limit。

Description

Rolling window non-causal inverse control method for flexible arm tail end path tracking

Technical Field

The invention relates to a control method in the field of flexible arm tail end tracking control, in particular to a rolling window-based non-causal inverse control method.

Background

The flexible arm has the advantages of light weight, energy saving, quick action response and the like, and has wide application prospect in practice. The conventional application of the flexible arm tail end tracking device is to realize tail end track tracking, and the application of the flexible arm tail end tracking device in the occasions with no requirement on tracks, such as automatic welding, automatic drawing and carrying, highlights the importance of the high-precision tracking problem of the tail end path of the flexible arm. However, the system from the joint moment to the tail end has non-minimum phase characteristics due to the flexibility of the connecting rod of the flexible arm, so that the effect of adopting the conventional control method is poor.

The common causal inverse tracking algorithm cannot be used when applied to the tracking of the end path of the flexible arm because the control input and reference state obtained by the method are unbounded. The output redefinition method and the output planning method can solve the output tracking problem of the non-minimum phase system to a certain extent, and have the common characteristic that the expected track needs to be redefined and the influence of unstable zero points is eliminated. However, the two methods have extremely high requirements on the accuracy of the model and poor robustness, and are difficult to apply in practice.

Disclosure of Invention

In order to overcome the defects of the prior art, the invention provides a rolling window-based non-causal inverse control method, which introduces a rolling window, designs a reasonable flexible arm joint track according to the requirements of a non-causal stable inverse technology of a non-minimum phase system and an expected terminal path in each window period, and then adopts the non-causal inverse method to provide the instantaneous input of the flexible arm system. The step length of window rolling is the size of sampling time, and the task of online tracking of the expected path at the tail end of the flexible arm is realized in a mode of planning and controlling.

A rolling window non-causal inverse control method for flexible arm tail end path tracking is used for realizing on-line tracking of a flexible arm tail end path, and is characterized in that requirements for track tracking of the flexible arm tail end are relaxed, a rolling window is introduced, a tracking task is adjusted in real time in the window moving process, the on-line tracking task of the tail end path is completed in a mode of planning and controlling at the same time, and the method comprises three steps of modeling, window width determination, window initialization and system starting, and on-line rolling window, and specifically comprises the following steps:

1) modeling and window width determination: and (3) using a hypothetical modality method to enable the dynamic equation of the infinite dimension of the flexible arm to be a finite dimension, obtaining a dynamic model of the flexible arm through Lagrange equation, and rewriting the dynamic model into a state equation form. Obtaining a corresponding inverse system by using a non-causal inverse method; determining the window width T of the rolling window non-causal inverse control method according to the non-minimum phase characteristics of the model, the given terminal expected track and the expected tracking precision_wind；

2) Initializing the window and starting the system: in the initial segment of the plan t₀,t₁]The internal requirement keeps the output to be 0, so that the condition of using the non-causal inverse theory is met, and the state of the system reaches a proper state in advance under the condition of zero output; then combining the targets tracked according to the path to be reached₀,t₁]0 output within a segment is programmed to get [ t ]₀,T_wind]A desired trajectory within; t here₁Can be compared with T_windThe output is large, and the corresponding expected output of the whole initial window is 0 at the moment; obtaining an initial time according to a rolling window non-causal inverse methodCorresponding control inputs and reference states;

3) and (3) rolling the window on line: scrolling the window forward by a fixed step Δ T; according to the path tracking task, an expected track obtained after planning is given in real time, and subsequent control input is obtained through calculation and acts on the system; move forward to T in a mode of planning and controlling_end-T_pEnding the moment; the real-time window moving control mode enables [ T_wind, + ∞) in real time.

The dynamic model in the step 1) is defined by using a formula (1), wherein the formula (1) is as follows:

$D\left(q\right)\stackrel{\·\·}{q}+N(q,\stackrel{\·}{q})+Kq=T\mathrm{\τ}$

wherein q is [ theta, lambda ]]^T＝[θ₁,θ₂,λ₁,...,λ_n]，θ＝[θ₁,θ₂]^TFor joint angles corresponding to two joints, λ ═ λ₁,...,λ_n]^TFor n predefined modes phi ═ phi [ ]₁,...,φ_n]^TD (q) is the inertia matrix of the flexible arm system,for coriolis force and centrifugal force vectors, K is the intensity matrix, T is the input matrix associated with the mode shape, τ ═ τ₁,τ₂]^TIs the moment applied to both joints.

The equation of state described in step 1) is defined using equation (2), where equation (2) is:

$\left[\begin{array}{cc}{D}_{\mathrm{\θ}\mathrm{\θ}}& D_{\mathrm{\θ}\mathrm{\λ}}\\ D_{\mathrm{\θ}\mathrm{\λ}}^T& I\end{array}\right]\left[\begin{array}{c}\stackrel{\·\·}{\mathrm{\θ}}\\ \stackrel{\·\·}{\mathrm{\λ}}\end{array}\right]+\left[\begin{array}{c}{N}_{\mathrm{\θ}}\\ N_{\mathrm{\λ}}\end{array}\right]+\left[\begin{array}{c}0\\ K_{\mathrm{\λ}}\mathrm{\λ}\end{array}\right]=\left[\begin{array}{c}\mathrm{\τ}\\ {\mathrm{\φ}}^{,}\left(0\right){\mathrm{\τ}}_2\end{array}\right]$

where D is referred to as the inertial matrix,a matrix obtained by blocking D according to the dimensions of theta and lambda.

The inverse system equation described in step 1) is defined by equation (4), where equation (4) is:

$\left\{\begin{array}{c}\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{x_{ref}}\end{array}\right]=\left[\begin{array}{cc}A-{\mathrm{BK}}_{fb},& {\mathrm{BK}}_{fb}\\ 0,& A\end{array}\right]\left[\begin{array}{c}x\\ x_{ref}\end{array}\right]+\left[\begin{array}{c}B\\ B\end{array}\right]u_{ff}\\ y\left(t\right)=\left[\begin{array}{cc}C& 0\end{array}\right]\left[\begin{array}{c}x\\ x_{ref}\end{array}\right]\end{array}\right.$

the corresponding original system is defined by formula (3), and formula (3) is:

$\left\{\begin{array}{c}\stackrel{\·}{x}=Ax+Bu\\ y=Cx\end{array}\right.$

wherein,is input;is an output;is the system state;is a constant column vector; u. of_ff(t)，x_ref(t) are the control inputs and reference states by a non-causal inverse method.

The control input corresponding to the initial time in the step 2) is defined by a formula (5), wherein the formula (5) is as follows:

wherein, t₀Is the initial time; r ═ r₁,...,r_m]^T，r_iTo make it possible toIs the smallest integer (called relative order) and i is more than or equal to 1 and less than or equal to m;T₁、T₂ξ and ζ are respectively the first coordinate transformation T of the state equation of the original system₁Then obtaining external dynamic state and internal dynamic state; a. the_ξAnd A_ζRespectively carrying out first coordinate transformation T on the state equation of the original system₁Obtaining an A matrix corresponding to the external dynamic equation and the internal dynamic equation; sigma_ssAnd σ_usPerforming second coordinate transformation T for the internal dynamic equations respectively₂The stable internal dynamic state and the unstable internal dynamic state are obtained;andare respectively T₂T after transformation corresponding to stable internal dynamics and unstable internal dynamics₂A block matrix of the matrix.

The reference state corresponding to the initial time in the step 2) is defined by a formula (7), wherein the formula (7) is as follows:

$x_{ref}\left(t_0\right)=T_1^{-1}\left[\begin{array}{c}{\mathrm{\ξ}}_{des}\left(t_0\right)\\ {\mathrm{\ζ}}_{des}\left(t_0\right)\end{array}\right]=T_1^{-1}\left[\begin{array}{c}{\mathrm{\ξ}}_{des}\left(t_0\right)\\ \widehat{T_{2ss}}{\mathrm{\σ}}_{ss}\left(t_0\right)+\widehat{T_{2us}}\widetilde{{\mathrm{\σ}}_{us}}\left(t_0\right)\end{array}\right]$

wherein, T₁、T₂，ξ、ζ，σ_ss、σ_us，Andthe definition of (A) is as above.

Compared with the existing technology, the invention has the beneficial effects that: the invention is a mode of planning and controlling simultaneously, can realize high-precision tracking of the path at the tail end of the flexible arm, and has good effect on the control of other non-minimum phase systems. According to the method, a rolling window is introduced, in each window period, a reasonable flexible arm joint track is designed according to the requirements of the non-causal inverse technology of the non-minimum phase system and an expected tail end path, and then the non-causal inverse method is adopted to give the instantaneous input of the flexible arm system. The step length of window rolling is the size of sampling time, so the method can adjust the tracking task in real time in the window moving process, and has wide application prospect in occasions with higher requirements on flexibility.

Drawings

FIG. 1 is a schematic view of a planar dual link flexible arm;

FIG. 2 is a state diagram of the system after feedforward and feedback are added;

FIG. 3 is a flowchart illustrating the operation of the rolling window based non-causal inverse method of the present invention;

FIG. 4 is a schematic diagram of a cooperative design of two traces;

FIG. 5 is T_windThe first axial output tracking effect graph at 25;

FIG. 6 is T_windA second axial output tracking effect graph at 25 f;

FIG. 7 is four internal dynamic diagrams of the system in this operation;

FIG. 8 is T_windThe circular path tracking effect map at 25.

Detailed Description

The present invention will be described in detail with reference to the accompanying drawings.

For the flexible arm shown in fig. 1, the invention adopts a rolling window-based non-causal inverse control method to realize the tracking of the tail end path of the flexible arm. The method has the advantages that the track tracking requirement on the tail end of the flexible arm is prevented, the tracking task is adjusted in real time in the window rolling process by introducing the rolling window, and the task of tracking the expected path on line at the tail end of the flexible arm is realized in a mode of planning and controlling. The step length of window rolling is the sampling time, which is specifically as follows:

step 1, modeling and window width determination, comprising the following steps:

1) the dynamic of the infinite dimension of the flexible arm is formulated into a finite dimension by using a hypothetical modality method, and a dynamic model of the finite dimension is obtained by Lagrange's equation, as shown in formula (1):

$D\left(q\right)\stackrel{\·\·}{q}+N(q,\stackrel{\·}{q})+Kq=T\mathrm{\τ}---\left(1\right)$

wherein q is [ theta, lambda ]]^T＝[θ₁,θ₂,λ₁,...,λ_n]，θ＝[θ₁,θ₂]^TFor joint angles corresponding to two joints, λ ═ λ₁,...,λ_n]^TFor n predefined modes phi ═ phi [ ]₁,...,φ_n]^TD (q) is the inertia matrix of the flexible arm system,for coriolis force and centrifugal force vectors, K is the intensity matrix, T is the input matrix associated with the mode shape, τ ═ τ₁,τ₂]^TIs the moment applied to both joints.

2) Rewriting the dynamic model described in equation (1) into the form of a state equation, as shown in equation (2):

$\left[\begin{array}{cc}{D}_{\mathrm{\θ}\mathrm{\θ}}& D_{\mathrm{\θ}\mathrm{\λ}}\\ D_{\mathrm{\θ}\mathrm{\λ}}^T& I\end{array}\right]\left[\begin{array}{c}\stackrel{\·\·}{\mathrm{\θ}}\\ \stackrel{\·\·}{\mathrm{\λ}}\end{array}\right]+\left[\begin{array}{c}{N}_{\mathrm{\θ}}\\ N_{\mathrm{\λ}}\end{array}\right]+\left[\begin{array}{c}0\\ K_{\mathrm{\λ}}\mathrm{\λ}\end{array}\right]=\left[\begin{array}{c}\mathrm{\τ}\\ {\mathrm{\φ}}^{,}\left(0\right){\mathrm{\τ}}_2\end{array}\right]---\left(2\right)$

where D is referred to as the inertial matrix,a matrix obtained by blocking D according to the dimensions of theta and lambda.

3) After the output of the system is defined, a corresponding output equation is obtained, and a canonical representation of the original system can be obtained by combining the state equation corresponding to the formula (2), as shown in the formula (3):

$\left\{\begin{array}{c}\stackrel{\·}{x}\left(t\right)=Ax\left(t\right)+Bu\left(t\right)\\ y\left(t\right)=Cx\left(t\right)\end{array}\right.---\left(3\right)$

wherein,is input;is an output;is the system state.

4) And (3) obtaining a corresponding inverse system by using a non-causal inverse method, calculating to obtain a control input and a reference state, and applying the control input and the reference state to the original system. The system equation after the control action is added is defined by formula (4), and formula (4) is:

$\left\{\begin{array}{c}\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{x_{ref}}\end{array}\right]=\left[\begin{array}{cc}A-{\mathrm{BK}}_{fb},& {\mathrm{BK}}_{fb}\\ 0,& A\end{array}\right]\left[\begin{array}{c}x\\ x_{ref}\end{array}\right]+\left[\begin{array}{c}B\\ B\end{array}\right]u_{ff}\\ y\left(t\right)=\left[\begin{array}{cc}C& 0\end{array}\right]\left[\begin{array}{c}x\\ x_{ref}\end{array}\right]\end{array}\right.---\left(4\right)$

wherein u (t), y (t), x (t) are as defined above;is a constant column vector; u. of_ff(t)，x_ref(t) is the control input and reference state by a non-causal inverse method; the state diagram of the feedforward-feedback system corresponding to the formula (4) is shown in fig. 2.

5) Calculating the zero point of the system, and determining the window width T according to the distance from the zero point to the virtual axis and the requirement of the expected tracking precision_wind。

Step 2, initializing a window and starting a system, comprising the following steps:

1) in the initial segment of the plan t₀,t₁]The internal requirement keeps the output to be 0, so that the condition of using the non-causal inverse theory is met, and the state of the system reaches a proper state in advance under the condition of zero output;

2) combining [ t ] according to the target of the path to be traced₀,t₁]0 output within a segment is programmed to get [ t ]₀,T_wind]A desired trajectory within; t here₁Can be compared with T_windThe output is large, and the corresponding expected output of the whole initial window is 0 at the moment;

3) obtaining a control input and a reference state corresponding to an initial moment according to a rolling window non-causal inverse method; the control input corresponding to the initial time is defined by equation (5), where equation (5) is:

$u_{ff}\left(t_0\right)=B_{in}^{-1}\[y_{des}^{\left(r\right)}\left(t_0\right)-A_{\mathrm{\ξ}}{\mathrm{\ξ}}_{des}\left(t_0\right)-A_{\mathrm{\ζ}}{\hat{T}}_{2ss}{\mathrm{\σ}}_{ss}\left(t_0\right)-A_{\mathrm{\ζ}}{\hat{T}}_{2us}\widetilde{{\mathrm{\σ}}_{us}}\left(t_0\right)\]---\left(5\right)$

wherein, t₀Is the initial time; r ═ r₁,...,r_m]^T，r_iTo make it possible toIs the smallest integer (called relative order) and i is more than or equal to 1 and less than or equal to m;T₁、T₂ξ and ζ are respectively the first coordinate transformation T of the state equation of the original system₁Then obtaining external dynamic state and internal dynamic state; a. the_ξAnd A_ζRespectively carrying out first coordinate transformation T on the state equation of the original system₁Obtaining an A matrix corresponding to the external dynamic equation and the internal dynamic equation; sigma_ssAnd σ_usPerforming second coordinate transformation T for the internal dynamic equations respectively₂The stable internal dynamic state and the unstable internal dynamic state are obtained;andare respectively T₂T after transformation corresponding to stable internal dynamics and unstable internal dynamics₂A block matrix of the matrix.

σ_ssAnd σ_usIs defined by equation (6), equation (6) being:

wherein phi is_ssAnd phi_usState transition matrixes of a stable internal dynamic equation and an unstable internal dynamic equation are respectively used; b is_ssAnd B_usB matrices corresponding to the stable internal dynamic equations and the unstable internal dynamic equations, respectively.

The reference state corresponding to the initial time is defined by equation (7), where equation (7) is:

$x_{ref}\left(t_0\right)=T_1^{-1}\left[\begin{array}{c}{\mathrm{\ξ}}_{des}\left(t_0\right)\\ {\mathrm{\ζ}}_{des}\left(t_0\right)\end{array}\right]=T_1^{-1}\left[\begin{array}{c}{\mathrm{\ξ}}_{des}\left(t_0\right)\\ \widehat{T_{2ss}}{\mathrm{\σ}}_{ss}\left(t_0\right)+\widehat{T_{2us}}\widetilde{{\mathrm{\σ}}_{us}}\left(t_0\right)\end{array}\right]---\left(7\right)$

wherein, T₁、T₂，ξ、ζ，σ_ss、σ_us，Andthe definition of (A) is as above.

Step 3, rolling the window on line, comprising the following steps:

1) scrolling the window forward by a fixed step Δ T;

2) planning and controlling simultaneously: according to the path tracking task, an expected track obtained after planning is given in real time, and subsequent control input is obtained through calculation and acts on the system;

3) adjusting [ T ] in real time_wind, + ∞) in forward run until T in a control-while-planning manner_end-T_pEnding the moment; the resulting actual trajectory is truncated with respect to the desired trajectory by the window width T_wind(ii) a The process of system operation when scrolling on line is shown in fig. 3.

Examples

Assuming a circular path is to be traced, two tracks can be constructed that fit into each other as shown in FIG. 4. Firstly, obtaining a dynamic model of the flexible arm by using the method described above, converting the dynamic model into a state equation form, and determining the width of a window to be used; then, designing an initialization window according to the requirements and the expected path of the non-causal inverse theory, and starting the system after obtaining initial control input and reference state through calculation; and finally, rolling the window online by a fixed step length until the system operation is finished. FIG. 5 is a graph of the tracking effect of the output of the first axis direction obtained by the method used in the present invention, and FIG. 6 is a graph of the tracking effect of the output of the second axis direction obtained by the method used in the present invention; FIG. 7 is a graph of the four bounded internal dynamics of the system during operation, resulting in a graph of the tracking effect on a circular path as shown in FIG. 8. From the results, the method provided by the invention has high tracking precision on the end circular path, so that the effectiveness of the method is verified.

Claims (7)

1. A rolling window non-causal inverse control method for flexible arm tail end path tracking is used for realizing online tracking of a flexible arm tail end path, and is characterized in that requirements for track tracking of the flexible arm tail end are relaxed, a rolling window is introduced, a tracking task is adjusted in real time in the window moving process, and the online tracking task of the tail end path is completed in a mode of planning and controlling at the same time, wherein the method comprises three steps of modeling, window width determination, window initialization, system starting and online rolling window, and specifically comprises the following steps:

1) modeling and window width determination: using a hypothetical modality methodThe method comprises the following steps of (1) enabling a dynamic equation of an infinite dimension of a flexible arm to be a finite dimension, obtaining a dynamic model of the flexible arm through a Lagrange equation, and rewriting the dynamic model into a state equation; obtaining a corresponding inverse system by using a non-causal inverse method; determining the window width T of the rolling window non-causal inverse control method according to the non-minimum phase characteristics of the model, the given terminal expected track and the expected tracking precision_wind；

2) Initializing the window and starting the system: in the initial segment of the plan t₀,t₁]The internal holding output is 0, so that the condition of using a non-causal inverse theory is met, and the state of the system is enabled to reach a proper state in advance under the condition of zero output; then combining the targets tracked according to the path to be reached₀,t₁]0 output within a segment is programmed to get [ t ]₀,T_wind]A desired trajectory within; obtaining a control input and a reference state corresponding to an initial moment according to a rolling window non-causal inverse method;

3) and (3) rolling the window on line: scrolling the window forward by a fixed step Δ T; according to the path tracking task, an expected track obtained after planning is given in real time, and subsequent control input is obtained through calculation and acts on the system; move forward to T in a mode of planning and controlling_end-T_pEnding the moment; the real-time window moving control mode enables [ T_wind, + ∞) in real time.

2. The method of claim 1, wherein: the dynamic model in the step 1) is defined by using a formula (1), wherein the formula (1) is as follows:

wherein q is [ theta, lambda ]]^T＝[θ₁,θ₂,λ₁,...,λ_n]，θ＝[θ₁,θ₂]^TFor joint angles corresponding to two joints, λ ═ λ₁,...,λ_n]^TFor n predefined modes phi ═ phi [ ]₁,...,φ_n]^TD (q) is the inertia matrix of the flexible arm system,for coriolis force and centrifugal force vectors, K is the intensity matrix, T is the input matrix associated with the mode shape, τ ═ τ₁,τ₂]^TIs the moment applied to both joints.

3. The method of claim 2, wherein: the equation of state described in step 1) is defined using equation (2), where equation (2) is:

where D is referred to as the inertial matrix,a matrix obtained by blocking D according to the dimensions of theta and lambda.

4. The method of claim 3, wherein: the inverse system equation described in step 1) is defined by equation (4), where equation (4) is:

the corresponding original system is defined by formula (3), and formula (3) is:

wherein,is input;is an output;is the system state;is a constant column vector; u. of_ff(t)，x_ref(t) are the control inputs and reference states by a non-causal inverse method.

5. The method of claim 4, wherein: the control input corresponding to the initial time in the step 2) is defined by a formula (5), wherein the formula (5) is as follows:

wherein, t₀Is the initial time; r ═ r₁,...,r_m]^T，r_iTo make it possible toI is more than or equal to 1 and less than or equal to m;T₁、T₂ξ and ζ are respectively the first coordinate transformation T of the state equation of the original system₁Then obtaining external dynamic state and internal dynamic state; a. the_ξAnd A_ζRespectively carrying out first coordinate transformation T on the state equation of the original system₁Obtaining an A matrix corresponding to the external dynamic equation and the internal dynamic equation; sigma_ssAnd σ_usPerforming second coordinate transformation T for the internal dynamic equations respectively₂The stable internal dynamic state and the unstable internal dynamic state are obtained;andare respectively T₂T after transformation corresponding to stable internal dynamics and unstable internal dynamics₂A block matrix of the matrix.

6. The method of claim 5, wherein: the reference state corresponding to the initial time in the step 2) is defined by a formula (7), wherein the formula (7) is as follows:

wherein, T₁、T₂ξ and ζ are respectively the first coordinate transformation T of the state equation of the original system₁Then obtaining external dynamic state and internal dynamic state;andare respectively T₂T after transformation corresponding to stable internal dynamics and unstable internal dynamics₂A block matrix of the matrix; sigma_ssAnd σ_usPerforming second coordinate transformation T for the internal dynamic equations respectively₂The stable internal dynamic state and the unstable internal dynamic state are obtained.

7. The method of claim 1, wherein: in step 3), the window is rolled forward by a fixed step length delta T; according to the path tracking task, an expected track obtained after planning is given in real time, and subsequent control input is obtained through calculation and acts on the system; real-time adjustmentWhole [ T ]_wind, + ∞) in forward run until T in a control-while-planning manner_end-T_pThe time is over.