CN103941592A - Online modeling method of flying robot dynamics model - Google Patents

Abstract

The invention relates to an online modeling method of a flying robot dynamics model. The method comprises steps: a flying robot dynamics reference model is built in a flying robot dynamics module simulation computer; the flying robot flies and flying state information is sent in real time to the flying robot dynamics module simulation computer; and the flying robot dynamics module simulation computer updates in real time the flying state information contained in the dynamics reference model, the value of modeling parameters inside the dynamics reference model is obtained according to modeling noise, the value of the modeling parameter is in real time used in the flying robot dynamics reference model, and online modeling of the flying robot dynamics model is realized. According to the online modeling method of a flying robot dynamics model, true dynamics of the flying robot can be reflected in real time, the method is convenient and simple, computer programming and realization are facilitated, and verification of control algorithm on a simulation platform is facilitated.

Description

The line modeling method of flying machine human occupant dynamic model

Technical field

The present invention relates to the poor on-line analysis Active Modeling of a kind of flying machine human occupant dynamic model semi-physical simulation platform, specifically a kind of online Active Modeling method based on flying robot's model difference combining based on software, hardware configuration.

Background technology

In order to meet the demand of flying robot in Control System Software design and gordian technique checking in early stage, Chinese scholars has built the various flying robots' for different application software emulation (Software in the loop simulation, SILS) platform.And can not meeting flying robot, single software emulation platform works in online model analysis under different modalities and Active Modeling, the demand of control system to control algolithm, systemic-function and coupling system hardware debug and research.

Semi-physical simulation, also claims half material object, hardware at loop simulation (Hardware in the loop simulation, HILS), in the design and R&D process of flying machine robot system, has extremely important meaning.Especially for flying robot's kinetic model, there is high system complexity, multi-control variable and the feature such as the Unpredictability that brought by the factor such as environment, weather.Therefore the simulated environment of, setting up for flying machine human occupant dynamic model is design complex control system, studies its gordian technique, realizes Stable Control Strategy and the indispensable important means of the each function of system.

The poor on-line analysis Active Modeling of flying machine human occupant dynamic model semi-physical system is distinctive a kind of emulation mode in research and development flying robot control system process, its real-time flight data based on flying robot is carried out on-line identification and the analysis of model at the poor on-line analysis Active Modeling of flying machine human occupant dynamic model computing machine, according to the poor kinetic model of setting up of the model under different modalities, and in conjunction with flying robot's on-board controller system made semi-physical simulation in kind platform.

Different from software emulation system is, semi-physical system is poor by analyze online model between different model of flights according to real flying quality, the active that completes kinetic model is set up, and the soft and hardware system of flying robot's on-board controller is placed in to emulation closed loop, full flight course, full state of flight to flying robot are carried out emulation, thereby find in time and revise the leak that flying robot's control system soft and hardware exists in real work environment, effectively improve the reliability of control system.

Existing flying machine human model adopts off-line modeling method, and system model parameter is fixed, and can not reflect in real time that the model of system in whole operational processs changes, and causes control strategy can not be applicable to all sidedly each duty of system.

Summary of the invention

Exist and cannot realize for flying robot's line modeling, this technological gap based on the poor systematic parameter correction of model, control algolithm, systemic-function and coupling system hardware debug for existing flying robot's method for establishing model and system software emulation platform, proposed flying machine human occupant dynamic model line modeling method.

The technical solution used in the present invention is: the line modeling method of flying machine human occupant dynamic model, comprises the following steps:

In flying machine human occupant dynamic model simulation computer, set up flying robot's dynamics reference model; Flying robot takes off and state of flight information is sent in flying machine human occupant dynamic model simulation computer in real time; The state of flight information comprising in flying machine human occupant dynamic model simulation computer real-time update dynamics reference model, and obtain the value of modeling parameters in dynamics reference model according to modeling noise, the value of modeling parameters is updated in flying robot's dynamics reference model in real time, realizes the line modeling of flying machine human occupant dynamic model.

Described state of flight information comprises u, v, w, p, q, r, φ, θ, δ _lat, δ _lon, δ _ped, δ _col; U, v and w are respectively forward speed, side velocity and vertical velocity; P, q and r are roll angle speed, pitch rate and the course angle speed under flying robot's reference frame; φ and θ are respectively rolling and the angle of pitch; δ _latfor side direction control inputs; δ _lonfor forward direction control inputs; δ _pedfor course control inputs; δ _colfor vertical control inputs.

The described value that obtains modeling parameters in dynamics reference model according to modeling noise comprises the following steps:

First, obtain the estimated value of current state variable according to modeling noise

$\left\{\begin{array}{c}{\mathrm{\ρ}}_k=\frac{\sqrt{r_{m,k}}}{\sqrt{r_{m,k}}+\sqrt{p_{m,k}}}\\ W_k=\frac{C_d^a{P}_{k|k-1}{C}_d^{\mathrm{aT}}}{1-{\mathrm{\ρ}}_k}+\frac{R_0}{{\mathrm{\ρ}}_k}\\ K_k^e=\frac{P_{k|k-1}{C}_d^{\mathrm{aT}}{W}_k^{-1}}{1-{\mathrm{\ρ}}_k}\\ {\mathrm{\δ}}_k=1-{(Y_k-C_d^a{\hat{X}}_{k|k-1}^a)}^T{W}_k^{-1}(Y_k-C_d^a{\hat{X}}_{k|k-1}^a)\\ {\hat{X}}_{k|k}^a={\hat{X}}_{k|k-1}^a+K_k^e(Y_k-C_d^a{\hat{X}}_{k|k-1}^a)\\ P_{k|k}={\mathrm{\δ}}_k(\frac{P_{k|k-1}}{1-{\mathrm{\ρ}}_k}-\frac{P_{k|k-1}}{1-{\mathrm{\ρ}}_k}{C}_d^{\mathrm{aT}}{W}_k^{-1}{C}_d^a\frac{P_{k|k-1}}{1-{\mathrm{\ρ}}_k})\\ {\hat{X}}_{k+1|k}^a=A_d^a{\hat{X}}_{k|k}^a+B_d^a{U}_k^a\\ {\mathrm{\β}}_k=\frac{\sqrt{\mathrm{Tr}\left(Q\right)}}{\sqrt{\mathrm{Tr}\left(Q\right)}+\sqrt{\mathrm{Tr}\left(A_d^a{P}_{k|k}{A}_d^{\mathrm{aT}}\right)}}\\ P_{k+1|k}=\frac{A_d^a{P}_{k|k}{A}_d^{\mathrm{aT}}}{1-{\mathrm{\β}}_k}+\frac{Q^a}{{\mathrm{\β}}_k}\end{array}\right.$

ρ _k, W _k, δ _k, P _k|k, P _k+1|k, β _kbe intermediate variable, R ₀for measuring the Matrix of envelope of noise, Q is h _kmatrix of envelope, Q ^afor process noise matrix of envelope, r _m,kfor R ₀maximum characteristic root, p _{m, k}for Q ^amaximum characteristic root;

Flying robot's dynamics reference model discrete equation

$\left\{\begin{array}{c}{X}_{k+1}^a=A_d^a{X}_k^a+B_d^a{U}_k^a+W_k^a\\ Y_k=C_d^a{X}_k^a+V_k\end{array}\right.$

Wherein, $A_d^a=\left(\begin{array}{cc}{A}_d& 0_{38\×38}\\ 0_{38\×38}& I_{38\×38}\end{array}\right),$ $B_d^a=\left(\begin{array}{c}{B}_d\\ 0_{38\×4}\end{array}\right),$ $C_d^a=\left(\begin{array}{cc}{C}_d& 0_{8\×38}\end{array}\right),$ $W_k^a=\left(\begin{array}{cc}{0}_{1\×38}& T_s{h}_k^T\end{array}\right);$ Y _kfor the sampled value of system output y (t), h _kfor the sampled value of process noise h (t), V _kfor measuring the sampled value of noise V (t), T _sfor the sampling period, { A _d, B _d, C _dbe system { A ₀, B ₀, the discrete expression of C}, that is:

A ₀＝diag{A _lon,A _lat,A _y-h}， $B_0=\mathrm{diag}\{{\left(\begin{array}{cc}{B}_{\mathrm{lon}}^T& B_{\mathrm{lat}}^T\end{array}\right)}^T,B_{y-h}\}$

$C=\left(\begin{array}{cccccc}{I}_{3\×3}& 0_{3\×2}& 0_{3\×3}& 0_{3\×2}& 0_{3\×2}& 0_{3\×1}\\ 0_{3\×3}& 0_{3\×2}& I_{3\×3}& 0_{3\×2}& 0_{3\×2}& 0_{3\×1}\\ 0_{2\×3}& 0_{2\×2}& 0_{2\×3}& 0_{2\×2}& I_{2\×2}& 0_{2\×1}\end{array}\right)$

And

$A_{\mathrm{lon}}=\left(\begin{array}{ccccc}{X}_u& 0& -g& X_a& 0\\ M_u& 0& 0& M_a& 0\\ 0& 1& 0& 0& 0\\ 0& -1& 0& -1/{\mathrm{\τ}}_f& A_c/{\mathrm{\τ}}_f\\ 0& -1& 0& 0& -1/{\mathrm{\τ}}_f\end{array}\right),$ $B_{\mathrm{lon}}=\left(\begin{array}{cc}{X}_{\mathrm{lon}}& X_{\mathrm{lat}}\\ M_{\mathrm{lon}}& M_{\mathrm{lat}}\\ 0& 0\\ A_{\mathrm{lon}}& A_{\mathrm{lat}}\\ C_{\mathrm{lon}}& C_{\mathrm{lat}}\end{array}\right)$

$A_{\mathrm{lat}}=\left(\begin{array}{ccccc}{Y}_u& 0& g& Y_a& 0\\ L_u& 0& 0& L_a& 0\\ 0& 1& 0& 0& 0\\ 0& -1& 0& -1/{\mathrm{\τ}}_f& B_d/{\mathrm{\τ}}_f\\ 0& -1& 0& 0& -1/{\mathrm{\τ}}_f\end{array}\right),$ $B_{\mathrm{lat}}=\left(\begin{array}{cc}{Y}_{\mathrm{lon}}& Y_{\mathrm{lat}}\\ L_{\mathrm{lon}}& L_{\mathrm{lat}}\\ 0& 0\\ B_{\mathrm{lon}}& B_{\mathrm{lat}}\\ D_{\mathrm{lon}}& D_{\mathrm{lat}}\end{array}\right)$

$A_{y-h}=\left(\begin{array}{ccc}{Z}_w& Z_r& 0\\ N_w& N_r& -N_{\mathrm{ped}}\\ 0& K_r& -K_{\mathrm{rfb}}\end{array}\right),$ $B_{y-h}=\left(\begin{array}{cc}{Z}_{\mathrm{ped}}& Z_{\mathrm{col}}\\ N_{\mathrm{ped}}& N_{\mathrm{col}}\\ 0& 0\end{array}\right)$

I _{i × i}represent the unit matrix of i × i, i={2,3}, 0 _{i × j}represent 0 matrix of i × j, wherein j={1,2,3};

Then, make the estimated value of current state variable for state vector just obtain the value of the interior modeling parameters of modeling vector f (t); Wherein, f _kfor the sampled value of k moment modeling vector f (t), X _kfor the sampled value of flying robot's dynamics reference model state vector X (t) in k moment.

The present invention has following beneficial effect and advantage:

1. the present invention can reflect flying robot's real kinetic in real time, because its kinetic model is according to flying robot's state of flight real-time update, its online model of setting up and the error of the model of flying robot own are reduced, be very beneficial for flying robot's accurate control.

2. method of the present invention is easy, is convenient to computer programming and realization.

3. the present invention contributes to the checking of control algolithm on emulation platform, and can be applicable to according to its online Design of Mathematical Model of setting up the control algolithm of its model, realizes flying robot's accurate control.

4. in line modeling method of the present invention, consider to measure noise and process noise, carry out uncertain mathematical expression and On-line Estimation based on noise envelope matrix, can overcome the uncertainty that measurement links is brought to system modelling, reduce modeling error, make the model of setting up reflect more exactly real system dynamics.

5. the present invention has stronger versatility and stronger systematicness, and operability of the present invention, show property all stronger, be applied to easily in various flying robots' semi-physical simulation experiment, avoid the design iterations of system, greatly simplified the test job of the new Function Extension of system simultaneously.

Brief description of the drawings

Fig. 1 is structured flowchart of the present invention;

Fig. 2 is flying robot's aircraft mounted control system structured flowchart in the present invention;

Fig. 3 is flying robot's on-board controller structured flowchart in the present invention;

Fig. 4 is flying robot's ground monitoring computing machine and flying robot's aircraft mounted control system wireless telecommunications block diagram in the present invention;

Fig. 5 is flying robot's control system semi-physical simulation method schematic diagram of the present invention;

Fig. 6 is flying robot's of the present invention line modeling method schematic diagram.

Embodiment

Below in conjunction with accompanying drawing, the specific embodiment of the present invention is illustrated.

By the simulation to the sensor information such as accelerometer, gyro and form Packet Generation to flying robot's on-board controller 3-1, after control algolithm, produce practical function, complete the controlled quentity controlled variable of aerial mission, form Packet Generation to flying machine human occupant dynamic model, produce corresponding control effect, and use flying robot's flight attitude demonstration/visual display computing machine 1 to realize the 3-D display of flying robot and flight environment of vehicle thereof, and then complete control algorithm design and the policy checking of flying robot's flight control system; Flying robot can be carried out to steering wheel 3-3 and be connected with flying robot's on-board controller 3-1 simultaneously, thus the correctness of the control signal that access control algorithm produces intuitively and execution steering wheel response action.

As shown in Figure 1, flying robot's control system semi-physical simulation platform of the present invention comprises: flying robot's flight attitude demonstration/visual display computing machine 1, flying machine human occupant dynamic model simulation computer 2, flying robot's aircraft mounted control system 3, radio remote controller 4, flying robot's ground monitoring computing machine 5, flying robot 6.

Flying machine human occupant dynamic model simulation computer 2 is connected with flying robot's flight attitude demonstration/visual display computing machine by network adapter; Flying machine human occupant dynamic model simulation computer 2 is connected with flying robot's aircraft mounted control system 3RS-232 serial ports by RS-232 serial ports, sends the sensor information calculating by flying machine human occupant dynamic model; Flying machine human occupant dynamic model simulation computer 2 is connected with flying robot's aircraft mounted control system 3RS-232 serial ports by RS-232 serial ports, receives the execution steering wheel 3-3 control signal of being sent by flying robot's aircraft mounted control system 3; Flying robot's aircraft mounted control system 3 is communicated by letter with radio remote controller 4 by 2.4GHz radio signal; Flying robot's aircraft mounted control system 3 is connected by RS-232 serial ports with flying robot's ground monitoring computing machine 5.Flying robot 6 and flying machine human occupant dynamic model simulation computer 2 radio communications.

Flying robot's flight attitude demonstration/visual display computing machine 1, adopts (Lenovo) Qi Tian M7150 of association type computing machine, carries out three-dimensional and shows in real time.Flying robot's flight attitude demonstration/visual display calculates and carries out network communication by TCP/IP network communication protocol and flying machine human occupant dynamic model simulation computer 2, gps coordinate and flight attitude angle (roll angle, the angle of pitch and the crab angle) of obtaining the flying robot who calculates send Flightgear simulation software to, realize flying robot's state of flight and the 3-D display of flight environment of vehicle.

Flying machine human occupant dynamic model simulation computer 2, adopts (Lenovo) Qi Tian M7150 of association type computing machine, and the additional special PCI high speed serial communication of the Supreme Being card of installing.In Matlab/Simulink environment by the iterative computation of flying machine human occupant dynamic model being realized to the simulation of sensor information (gps coordinate of row robot, flight attitude angle, three axles overall situation Flight Acceleration, three axles overall situation flying speeds, flight attitude angular rate of change), and form data communication message, by serial ports, the sensor information of generation is sent to flying robot's on-board controller 3-1; Simultaneously, flying machine human occupant dynamic model simulation computer 2 receives the control signal message of the execution steering wheel 3-3 that flying robot's on-board controller 3-1 sends by serial ports, after parsing, enter flying robot's kinetic model and calculate, complete the closed-loop control to flying robot.

As shown in Figure 2, flying robot's aircraft mounted control system 3, also comprises flying robot's on-board controller 3-1, flying robot's duty indicating module 3-2 and carries out steering wheel 3-3,2.4GHz wireless receiving module 3-4.Flying robot's on-board controller 3-1 adopts two ARM core processors, realizes A/D, the D/A conversion of navigation sensor signal, radio remote controller 4 signals, sampling in conjunction with complex programmable order logic processor (CPLD).Cooperation can be expanded external interface, as serial communication interface, SD card access hole, complete the execution of filtering, the control algolithm of original navigation data, with the serial communication of flying machine human occupant dynamic model simulation computer 2 and the storage of crucial flying quality.Flying robot's duty indicating module 3-2 is made up of the high-power highlighted LED of three pieces of different colours, according to the residing different operating state of flying robot and flying machine human body's degree of stability, flying robot's duty indicating module 3-2 can light different LED combination and flashing mode, indicates and distinguish the residing different work of flying robot and state of flight with this.Carry out the control signal that steering wheel 3-3 receives flying robot's on-board controller 3-1, drive steering wheel to realize flying robot's action.

As shown in Figure 3, described flying robot's on-board controller 3-1 comprises: navigation sensor navigation information is processed unit 3-1-1, control algolithm processing unit 3-1-2, flying quality storage unit 3-1-3, serial ports expansion unit 3-1-4, and system power supply and switch element 3-1-5.Navigation sensor navigation information is processed unit 3-1-1, by the primary data information (pdi) of virtual sensor output, it is carried out, after filtering calculating, navigation information after treatment is sent to control algolithm processing unit 3-1-2 and flying quality storage unit 3-1-3; Control algolithm processing unit, receive through navigation sensor navigation information and process unit 3-1-1 navigation information after treatment, after control algolithm is calculated, control algolithm processing unit 3-1-2 draws and reaches the control signal of controlling object, and it is sent to execution steering wheel by serial ports expansion unit; Flying quality storage unit 3-1-3, receive the data of processing unit 3-1-1 and control algolithm processing unit 3-1-2 from navigation sensor navigation information, for storing original navigation data, filtering navigation data after treatment, control signal, flying robot's duty and the required data of post analysis; Serial ports expansion unit 3-1-4, carries out real-time information exchange by RS-232 serial communication bus and flying robot's ground monitoring computing machine and flying machine human occupant dynamic model simulation computer, and with the write-in functions of ISP Pattern completion control algolithm; System power supply and switch element 3-1-5, give flying robot's on-board controller 3-1 power supply.

Radio remote controller 4, adopts professional model plane radio remote controller JR PROPO DSX12X, with 2.4GHz channel communication, is aided with 12 tunnel control channel and function buttons, can realize the complicated flying robot control of load equipment in addition.Simultaneously can store 50 remote configuration models, facilitate telepilot and receiver to frequency.

Flying robot's ground monitoring computing machine 5, adopts and grinds magnificent ARK-3420 industrial computer, and upper and lower two displays are housed; are furnished with two the technical grade four axle triple bond remote-control levers in left and right; built-in power UPS design, has in use breakpoint of power-off and overload protection, does not affect user's operation.In use occur overload, restart power supply after system normally work.Realize the monitoring of flying robot's flight attitude, flying speed, Flight Acceleration and flying robot's health status by serial communication interface and the airborne control of flying robot.Realize the online management to flying robot's aerial mission, the switching of flying robot's offline mode by flying robot's ground monitoring computing machine 5 simultaneously, and in conjunction with plane map, flying robot's geographic position is shown in real time.Flying robot's ground monitoring computing machine 5 can also adopt wireless mode with the communication of flying robot's aircraft mounted control system 3.

As shown in Figure 4, adopt the 900MHz wireless data transfer module that Freewave is produced to replace serial communication.This mode provides the method for test flight robot flying robot's ground monitoring computing machine 5 and ability to communicate of flying robot's aircraft mounted control system 3 in over the horizon aerial mission, has ensured completing completely of aerial mission.Be convenient to test and on-line monitoring the early stage in the time completing over the horizon task to flying robot.

As shown in Figure 5, the simulation flow of flying machine human occupant dynamic model semi-physical simulation platform of the present invention is as follows:

1. initialization flight environment of vehicle and display view angle;

2. flying robot takes off, and carries out multi-modal flight;

3. flying machine human occupant dynamic model simulation computer obtains state of flight information, according to dynamics reference model and noise envelope matrix, completes at line model and sets up;

4. flying robot's on-board controller control algolithm loads;

5. start emulation;

6. flying machine human occupant dynamic model simulation computer resolves sensor information, forms message, and serial ports outputs to flying robot's aircraft mounted control system;

7. flying robot's on-board controller calculates and carries out steering wheel control signal, forms message, and serial ports output, sends to flying machine human occupant dynamic model simulation computer, carries out next step iterative computation; And show in real time by flying robot's flight attitude demonstration/visual display computing machine; Meanwhile, the execution steering wheel control signal of calculating gained will send to execution steering wheel to control with analog quantity form;

8. after flying machine human occupant dynamic model simulation computer iterative computation, carry out step 4.

As shown in Figure 6, flying machine human occupant dynamic model simulation calculation is obtained flying robot's state of flight information, according to the output valve of reference model 2-1, in line model difference analysis Active Modeling algorithm computing system model parameter, and modeling parameters renewal value is sent to reference model, for next step calculating.

The online updating modeling method of flying robot's kinetic model is specific as follows:

One. in flying machine human occupant dynamic model simulation computer, set up flying robot's reference model, flying robot's dynamics reference model structure is as follows:

$\left(\begin{array}{c}\stackrel{\·}{u}\\ \stackrel{\·}{q}\\ \stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\stackrel{\‾}{a}}\\ \stackrel{\·}{\stackrel{\‾}{c}}\end{array}\right)=\begin{array}{}\end{array}\left(\begin{array}{ccccc}{X}_u& 0& -g& X_a& 0\\ M_u& 0& 0& M_0& 0\\ 0& 1& 0& 0& 0\\ 0& -1& 0& -1/{\mathrm{\τ}}_f& A_c/{\mathrm{\τ}}_f\\ 0& -1& 0& 0& -1/{\mathrm{\τ}}_f\end{array}\right)\left(\begin{array}{c}u\\ q\\ \mathrm{\θ}\\ \stackrel{\‾}{a}\\ \stackrel{\‾}{c}\end{array}\right)+\left(\begin{array}{cc}{X}_{\mathrm{lon}}& X_{\mathrm{lat}}\\ M_{\mathrm{lon}}& M_{\mathrm{lat}}\\ 0& 0\\ A_{\mathrm{lon}}& A_{\mathrm{lat}}\\ C_{\mathrm{lon}}& C_{\mathrm{lat}}\end{array}\right)\left(\begin{array}{c}{\mathrm{\δ}}_{\mathrm{lon}}\\ {\mathrm{\δ}}_{\mathrm{lat}}\end{array}\right)---\left(1\right)$

$\left(\begin{array}{c}\stackrel{\·}{w}\\ \stackrel{\·}{r}\\ {\stackrel{\·}{r}}_{\mathrm{fb}}\end{array}\right)=\left(\begin{array}{ccc}{Z}_w& Z_r& 0\\ N_w& N_r& -N_{\mathrm{ped}}\\ 0& K_r& -K_{\mathrm{rfb}}\end{array}\right)\left(\begin{array}{c}w\\ r\\ r_{\mathrm{fb}}\end{array}\right)+\left(\begin{array}{cc}{Z}_{\mathrm{ped}}& Z_{\mathrm{col}}\\ N_{\mathrm{ped}}& N_{\mathrm{col}}\\ 0& 0\end{array}\right)\left(\begin{array}{c}{\mathrm{\δ}}_{\mathrm{ped}}\\ {\mathrm{\δ}}_{\mathrm{col}}\end{array}\right)---\left(3\right)$

Wherein state variable is defined as follows: u, and v and w are respectively forward speed (along x axle), side velocity (along y axle) and vertical velocity (along z axle); P, q and r are roll angle speed, pitch rate and the course angle speed under helicopter reference frame; φ and θ are respectively rolling and the angle of pitch; with be side direction and the forward direction propeller pitch angle of main oar, with for side direction and the forward direction propeller pitch angle of aileron; r _fbfor the FEEDBACK CONTROL input of constant speed instrument; δ _latfor side direction control inputs; δ _lonfor forward direction control inputs; δ _pedfor course control inputs; δ _colfor vertical control inputs.And definition:

U(t)＝(δ _lon δ _lat δ _ped δ _col) ^T

Based on hovering models equation (1)-(3), consider the time delay of control inputs simultaneously, model can be written as:

$\left\{\begin{array}{c}\stackrel{\·}{X}\left(t\right)=A_{0}X\left(t\right)+B_{0}U(t-k_t)\\ y\left(t\right)=\mathrm{CX}\left(t\right)\end{array}\right.---\left(4\right)$

Wherein,

A ₀＝diag{A _lon,A _lat,A _y-h}

$B_0=\mathrm{diag}\{{\left(\begin{array}{cc}{B}_{\mathrm{lon}}^T& B_{\mathrm{lat}}^T\end{array}\right)}^T,B_{y-h}\}$

$C=\left(\begin{array}{cccccc}{I}_{3\×3}& 0_{3\×2}& 0_{3\×3}& 0_{3\×2}& 0_{3\×2}& 0_{3\×1}\\ 0_{3\×3}& 0_{3\×2}& I_{3\×3}& 0_{3\×2}& 0_{3\×2}& 0_{3\×1}\\ 0_{2\×3}& 0_{2\×2}& 0_{2\×3}& 0_{2\×2}& I_{2\×2}& 0_{2\×1}\end{array}\right)$

And

$A_{\mathrm{lon}}=\left(\begin{array}{ccccc}{X}_u& 0& -g& X_a& 0\\ M_u& 0& 0& M_a& 0\\ 0& 1& 0& 0& 0\\ 0& -1& 0& -1/{\mathrm{\τ}}_f& A_c/{\mathrm{\τ}}_f\\ 0& -1& 0& 0& -1/{\mathrm{\τ}}_f\end{array}\right),$ $B_{\mathrm{lon}}=\left(\begin{array}{cc}{X}_{\mathrm{lon}}& X_{\mathrm{lat}}\\ M_{\mathrm{lon}}& M_{\mathrm{lat}}\\ 0& 0\\ A_{\mathrm{lon}}& A_{\mathrm{lat}}\\ C_{\mathrm{lon}}& C_{\mathrm{lat}}\end{array}\right)$

$A_{\mathrm{lat}}=\left(\begin{array}{ccccc}{Y}_u& 0& g& Y_a& 0\\ L_u& 0& 0& L_a& 0\\ 0& 1& 0& 0& 0\\ 0& -1& 0& -1/{\mathrm{\τ}}_f& B_d/{\mathrm{\τ}}_f\\ 0& -1& 0& 0& -1/{\mathrm{\τ}}_f\end{array}\right),$ $B_{\mathrm{lat}}=\left(\begin{array}{cc}{Y}_{\mathrm{lon}}& Y_{\mathrm{lat}}\\ L_{\mathrm{lon}}& L_{\mathrm{lat}}\\ 0& 0\\ B_{\mathrm{lon}}& B_{\mathrm{lat}}\\ D_{\mathrm{lon}}& D_{\mathrm{lat}}\end{array}\right)$

$A_{y-h}=\left(\begin{array}{ccc}{Z}_w& Z_r& 0\\ N_w& N_r& -N_{\mathrm{ped}}\\ 0& K_r& -K_{\mathrm{rfb}}\end{array}\right),$ $B_{y-h}=\left(\begin{array}{cc}{Z}_{\mathrm{ped}}& Z_{\mathrm{col}}\\ N_{\mathrm{ped}}& N_{\mathrm{col}}\\ 0& 0\end{array}\right)$

I _{i × i}represent the unit matrix of i × i, i={2,3}, 0 _{i × j}represent 0 matrix of i × j, wherein j={1,2,3}, k _t∈ R is the time delay of executive system.

Wherein A ₀and B ₀for treating the parameter matrix of modeling, wherein treat that modeling parameters is the variable in reference model, comprises

X _u,g,X _a,M _u,M _a,τ _f,A _c,X _lon,X _lat,M _lon,M _lat,A _lon,A _lat,C _lon,C _lat

,Y _u,Y _a,L _u,L _a,B _d,Y _lon,Y _lat,L _lon,L _lat,B _lon,B _lat，D _lon,D _lat，Z _w,N _w,Z _r

,N _r,K _r,N _ped,K _rfb,Z _ped,Z _col,N _col

Above-mentioned parameter is without physical meaning, and each parameter has its initial value, obtains by frequency domain identification, amounts to 38 and treats modeling parameters; Modeling vector is treated in definition again

$f\left(t\right)=\left(\begin{array}{c}{X}_u,g,X_a,M_u,M_a,{\mathrm{\τ}}_f,A_c,X_{\mathrm{lon}},X_{\mathrm{lat}},M_{\mathrm{lon}},M_{\mathrm{lat}},A_{\mathrm{lon}},A_{\mathrm{lat}},C_{\mathrm{lon}},C_{\mathrm{lat}}\\ ,Y_u,Y_a,L_u,L_a,B_d,Y_{\mathrm{lon}},Y_{\mathrm{lat}},L_{\mathrm{lon}},L_{\mathrm{lat}},B_{\mathrm{lon}},B_{\mathrm{lat}},D_{\mathrm{lon}},D_{\mathrm{lat}},Z_w,N_w,Z_r\\ ,N_r,K_r,N_{\mathrm{ped}},K_{\mathrm{rfb}},Z_{\mathrm{ped}},Z_{\mathrm{col}},N_{\mathrm{col}}\end{array}\right)$

Two. upgrade the state vector in flying robot's reference model:

Flying robot takes off, and state of flight information is sent in flying machine human occupant dynamic model simulation computer in real time; State of flight information comprises: u, v, w, p, q, r, φ, θ, δ _lat, δ _lon, δ _ped, δ _col, output quantity and the controlled quentity controlled variable of composition flying robot reference model,

U(t)＝(δ _lon δ _lat δ _ped δ _col) ^T；

Flying robot's reference model is according to sampling period T _sits output quantity of real-time update and controlled quentity controlled variable.

Three. be the state matrix upgrading according to dynamics reference model and noise envelope matrix in flying robot's reference model at line model difference analysis Active Modeling algorithm, and then complete at line model and set up, comprise the steps:

First, expand with reference to model, add until modeling parameters: the state vector after definition expansion and control inputs it is as follows,

$X_k^a={(X_k^T,f_k^T)}^T$

$U_k^a=U_{k-d}$

Wherein k is the sampling time, f _kfor the sampled value of k moment f (t), X _kfor the sampled value of k moment X (t), U _kfor the sampled value of k moment U (t), time delay d=k _t/ T _s, T _sfor sampling period (can be specified arbitrarily by user, the present embodiment is 0.01s).

Then, obtain discrete equation from (4) as follows:

$\left\{\begin{array}{c}{X}_{k+1}^a=A_d^a{X}_k^a+B_d^a{U}_k^a+W_k^a\\ Y_k=C_d^a{X}_k^a+V_k\end{array}\right.---\left(7\right)$

Wherein

$A_d^a=\left(\begin{array}{cc}{A}_d& 0_{38\×38}\\ 0_{38\×38}& I_{38\×38}\end{array}\right),$ $B_d^a=\left(\begin{array}{c}{B}_d\\ 0_{38\×4}\end{array}\right)$

$C_d^a=\left(\begin{array}{cc}{C}_d& 0_{8\×38}\end{array}\right),$ $W_k^a=\left(\begin{array}{cc}{0}_{1\×38}& T_s{h}_k^T\end{array}\right)$

Y _kfor the sampled value of system output y (t), h _kfor the sampled value of process noise h (t), V _kfor measuring the sampled value of noise V (t), for process noise, { A _d, B _d, C _dbe system { A ₀, B ₀, the discrete expression of C}.

Finally, just can adopt following equation to carry out online updating simulation parameters

$\left\{\begin{array}{c}{\mathrm{\ρ}}_k=\frac{\sqrt{r_{m,k}}}{\sqrt{r_{m,k}}+\sqrt{p_{m,k}}}\\ W_k=\frac{C_d^a{P}_{k|k-1}{C}_d^{\mathrm{aT}}}{1-{\mathrm{\ρ}}_k}+\frac{R_0}{{\mathrm{\ρ}}_k}\\ K_k^e=\frac{P_{k|k-1}{C}_d^{\mathrm{aT}}{W}_k^{-1}}{1-{\mathrm{\ρ}}_k}\\ {\mathrm{\δ}}_k=1-{(Y_k-C_d^a{\hat{X}}_{k|k-1}^a)}^T{W}_k^{-1}(Y_k-C_d^a{\hat{X}}_{k|k-1}^a)\\ {\hat{X}}_{k|k}^a={\hat{X}}_{k|k-1}^a+K_k^e(Y_k-C_d^a{\hat{X}}_{k|k-1}^a)\\ P_{k|k}={\mathrm{\δ}}_k(\frac{P_{k|k-1}}{1-{\mathrm{\ρ}}_k}-\frac{P_{k|k-1}}{1-{\mathrm{\ρ}}_k}{C}_d^{\mathrm{aT}}{W}_k^{-1}{C}_d^a\frac{P_{k|k-1}}{1-{\mathrm{\ρ}}_k})\\ {\hat{X}}_{k+1|k}^a=A_d^a{\hat{X}}_{k|k}^a+B_d^a{U}_k^a\\ {\mathrm{\β}}_k=\frac{\sqrt{\mathrm{Tr}\left(Q\right)}}{\sqrt{\mathrm{Tr}\left(Q\right)}+\sqrt{\mathrm{Tr}\left(A_d^a{P}_{k|k}{A}_d^{\mathrm{aT}}\right)}}\\ P_{k+1|k}=\frac{A_d^a{P}_{k|k}{A}_d^{\mathrm{aT}}}{1-{\mathrm{\β}}_k}+\frac{Q^a}{{\mathrm{\β}}_k}\end{array}\right.---\left(8\right)$

Wherein, for the estimated value of current state variable, for next moment state variable estimated value, ρ _k, W _k, δ _k, P _k|k, P _k+1|k, β _kbe variable, R ₀for measuring the Matrix of envelope of noise, if V _kfor the square formation of n × n, so R ₀it is exactly the vector of n × 1; Q is h _kmatrix of envelope, Q ^afor process noise matrix of envelope, the diagonal entry sum of Tr (Q) representing matrix Q, r _m,kfor R ₀maximum characteristic root, p _{m, k}for Q ^amaximum characteristic root.

Measure noise V _kmatrix of envelope R ₀for probabilistic mathematical description of sensor measurement, embody the error of sensor (comprising acceleration transducer, angular-rate sensor, GPS, barometer); Process noise matrix of envelope Q ^afor the uncertain mathematical description of kinetic model, embody the error of kinetic model one-step prediction.

R _{m, k}, p _{m, k}by calculating R ₀and Q ^amaximum characteristic root obtains, and obtains the estimated value of current state variable according to formula (8) by this value substitution 's can obtain f _k, obtain following real-time modeling parameters value

$\left(\begin{array}{c}{X}_u,g,X_a,M_u,M_a,{\mathrm{\τ}}_f,A_c,X_{\mathrm{lon}},X_{\mathrm{lat}},M_{\mathrm{lon}},M_{\mathrm{lat}},A_{\mathrm{lon}},A_{\mathrm{lat}},C_{\mathrm{lon}},C_{\mathrm{lat}}\\ ,Y_u,Y_a,L_u,L_a,B_d,Y_{\mathrm{lon}},Y_{\mathrm{lat}},L_{\mathrm{lon}},L_{\mathrm{lat}},B_{\mathrm{lon}},B_{\mathrm{lat}},D_{\mathrm{lon}},D_{\mathrm{lat}},Z_w,N_w,Z_r\\ ,N_r,K_r,N_{\mathrm{ped}},K_{\mathrm{rfb}},Z_{\mathrm{ped}},Z_{\mathrm{col}},N_{\mathrm{col}}\end{array}\right)$

By the reference model in real-time modeling parameters value substitution flying machine human occupant dynamic model simulation computer,, in formula (1)-(3), realize the line modeling of flying machine human occupant dynamic model by above-mentioned steps; Then the y (t) feeding back according to online dynamics reference model and U (t) calculate each controlled quentity controlled variable of flying robot in real time, carry out the checking of flying robot's control algolithm.

Claims (3)

1. the line modeling method of flying machine human occupant dynamic model, is characterized in that comprising the following steps:

In flying machine human occupant dynamic model simulation computer, set up flying robot's dynamics reference model; Flying robot takes off and state of flight information is sent in flying machine human occupant dynamic model simulation computer in real time; The state of flight information comprising in flying machine human occupant dynamic model simulation computer real-time update dynamics reference model, and obtain the value of modeling parameters in dynamics reference model according to modeling noise, the value of modeling parameters is updated in flying robot's dynamics reference model in real time, realizes the line modeling of flying machine human occupant dynamic model.

2. the line modeling method of flying machine human occupant dynamic model according to claim 1, is characterized in that: described state of flight information comprises u, v, w, p, q, r, φ, θ, δ _lat, δ _lon, δ _ped, δ _col; U, v and w are respectively forward speed, side velocity and vertical velocity; P, q and r are roll angle speed, pitch rate and the course angle speed under flying robot's reference frame; φ and θ are respectively rolling and the angle of pitch; δ _latfor side direction control inputs; δ _lonfor forward direction control inputs; δ _pedfor course control inputs; δ _colfor vertical control inputs.

3. the line modeling method of flying machine human occupant dynamic model according to claim 1, is characterized in that: the described value that obtains modeling parameters in dynamics reference model according to modeling noise comprises the following steps:

First, obtain the estimated value of current state variable according to modeling noise

$\left\{\begin{array}{c}{\mathrm{\ρ}}_k=\frac{\sqrt{r_{m,k}}}{\sqrt{r_{m,k}}+\sqrt{p_{m,k}}}\\ W_k=\frac{C_d^a{P}_{k|k-1}{C}_d^{\mathrm{aT}}}{1-{\mathrm{\ρ}}_k}+\frac{R_0}{{\mathrm{\ρ}}_k}\\ K_k^e=\frac{P_{k|k-1}{C}_d^{\mathrm{aT}}{W}_k^{-1}}{1-{\mathrm{\ρ}}_k}\\ {\mathrm{\δ}}_k=1-{(Y_k-C_d^a{\hat{X}}_{k|k-1}^a)}^T{W}_k^{-1}(Y_k-C_d^a{\hat{X}}_{k|k-1}^a)\\ {\hat{X}}_{k|k}^a={\hat{X}}_{k|k-1}^a+K_k^e(Y_k-C_d^a{\hat{X}}_{k|k-1}^a)\\ P_{k|k}={\mathrm{\δ}}_k(\frac{P_{k|k-1}}{1-{\mathrm{\ρ}}_k}-\frac{P_{k|k-1}}{1-{\mathrm{\ρ}}_k}{C}_d^{\mathrm{aT}}{W}_k^{-1}{C}_d^a\frac{P_{k|k-1}}{1-{\mathrm{\ρ}}_k})\\ {\hat{X}}_{k+1|k}^a=A_d^a{\hat{X}}_{k|k}^a+B_d^a{U}_k^a\\ {\mathrm{\β}}_k=\frac{\sqrt{\mathrm{Tr}\left(Q\right)}}{\sqrt{\mathrm{Tr}\left(Q\right)}+\sqrt{\mathrm{Tr}\left(A_d^a{P}_{k|k}{A}_d^{\mathrm{aT}}\right)}}\\ P_{k+1|k}=\frac{A_d^a{P}_{k|k}{A}_d^{\mathrm{aT}}}{1-{\mathrm{\β}}_k}+\frac{Q^a}{{\mathrm{\β}}_k}\end{array}\right.$

ρ _k, W _k, δ _k, P _k|k, P _k+1|k, β _kbe intermediate variable, R ₀for measuring the Matrix of envelope of noise, Q is h _kmatrix of envelope, Q ^afor process noise matrix of envelope, r _m,kfor R ₀maximum characteristic root, p _{m, k}for Q ^amaximum characteristic root;

Flying robot's dynamics reference model discrete equation

$\left\{\begin{array}{c}{X}_{k+1}^a=A_d^a{X}_k^a+B_d^a{U}_k^a+W_k^a\\ Y_k=C_d^a{X}_k^a+V_k\end{array}\right.$

Wherein, $A_d^a=\left(\begin{array}{cc}{A}_d& 0_{38\×38}\\ 0_{38\×38}& I_{38\×38}\end{array}\right),$ $B_d^a=\left(\begin{array}{c}{B}_d\\ 0_{38\×4}\end{array}\right),$ $C_d^a=\left(\begin{array}{cc}{C}_d& 0_{8\×38}\end{array}\right),$ $W_k^a=\left(\begin{array}{cc}{0}_{1\×38}& T_s{h}_k^T\end{array}\right);$ Y _kfor the sampled value of system output y (t), h _kfor the sampled value of process noise h (t), V _kfor measuring the sampled value of noise V (t), T _sfor the sampling period, { A _d, B _d, C _dbe system { A ₀, B ₀, the discrete expression of C}, that is:

A ₀＝diag{A _lon,A _lat,A _y-h}， $B_0=\mathrm{diag}\{{\left(\begin{array}{cc}{B}_{\mathrm{lon}}^T& B_{\mathrm{lat}}^T\end{array}\right)}^T,B_{y-h}\}$

$C=\left(\begin{array}{cccccc}{I}_{3\×3}& 0_{3\×2}& 0_{3\×3}& 0_{3\×2}& 0_{3\×2}& 0_{3\×1}\\ 0_{3\×3}& 0_{3\×2}& I_{3\×3}& 0_{3\×2}& 0_{3\×2}& 0_{3\×1}\\ 0_{2\×3}& 0_{2\×2}& 0_{2\×3}& 0_{2\×2}& I_{2\×2}& 0_{2\×1}\end{array}\right)$

And

$A_{\mathrm{lon}}=\left(\begin{array}{ccccc}{X}_u& 0& -g& X_a& 0\\ M_u& 0& 0& M_a& 0\\ 0& 1& 0& 0& 0\\ 0& -1& 0& -1/{\mathrm{\τ}}_f& A_c/{\mathrm{\τ}}_f\\ 0& -1& 0& 0& -1/{\mathrm{\τ}}_f\end{array}\right),$ $B_{\mathrm{lon}}=\left(\begin{array}{cc}{X}_{\mathrm{lon}}& X_{\mathrm{lat}}\\ M_{\mathrm{lon}}& M_{\mathrm{lat}}\\ 0& 0\\ A_{\mathrm{lon}}& A_{\mathrm{lat}}\\ C_{\mathrm{lon}}& C_{\mathrm{lat}}\end{array}\right)$

$A_{\mathrm{lat}}=\left(\begin{array}{ccccc}{Y}_u& 0& g& Y_a& 0\\ L_u& 0& 0& L_a& 0\\ 0& 1& 0& 0& 0\\ 0& -1& 0& -1/{\mathrm{\τ}}_f& B_d/{\mathrm{\τ}}_f\\ 0& -1& 0& 0& -1/{\mathrm{\τ}}_f\end{array}\right),$ $B_{\mathrm{lat}}=\left(\begin{array}{cc}{Y}_{\mathrm{lon}}& Y_{\mathrm{lat}}\\ L_{\mathrm{lon}}& L_{\mathrm{lat}}\\ 0& 0\\ B_{\mathrm{lon}}& B_{\mathrm{lat}}\\ D_{\mathrm{lon}}& D_{\mathrm{lat}}\end{array}\right)$

$A_{y-h}=\left(\begin{array}{ccc}{Z}_w& Z_r& 0\\ N_w& N_r& -N_{\mathrm{ped}}\\ 0& K_r& -K_{\mathrm{rfb}}\end{array}\right),$ $B_{y-h}=\left(\begin{array}{cc}{Z}_{\mathrm{ped}}& Z_{\mathrm{col}}\\ N_{\mathrm{ped}}& N_{\mathrm{col}}\\ 0& 0\end{array}\right)$

I _{i × i}represent the unit matrix of i × i, i={2,3}, 0 _{i × j}represent 0 matrix of i × j, wherein j={1,2,3};

Then, make the estimated value of current state variable for state vector just obtain the value of the interior modeling parameters of modeling vector f (t); Wherein, f _kfor the sampled value of k moment modeling vector f (t), X _kfor the sampled value of flying robot's dynamics reference model state vector X (t) in k moment.