Disclosure of Invention
In view of the above, the invention provides a method for designing a trajectory generator of a long-endurance inertial navigation system based on trajectory planning, which can effectively overcome the difficulty of artificially designing a motion state in a traditional trajectory generator design method, reduce the time and cost consumed in research and test, and improve the research efficiency of a gravity-assisted inertial navigation system.
The invention relates to a design method of a long-endurance inertial navigation system track generator based on track planning, which comprises the following steps:
step 1, planning the track of an aircraft by adopting a track planning algorithm, and considering gravity field information during track planning;
step 2, marking out specific speed, angular speed and course information of the aircraft in the navigation process under the flight path planned and obtained in the step 1 by adopting a dynamic window rule;
step 3, converting the speed, the angular speed and the course information of the underwater vehicle obtained in the step 2 into centripetal acceleration, tangential acceleration and angular speed information;
and 4, generating gyroscope data, accelerometer data and navigation data according to the centripetal acceleration, the tangential acceleration and the angular velocity of the underwater vehicle obtained in the step 3, and finishing the design of the track generator.
Preferably, in the step 1, the track planning algorithm adopts classical a and its improved algorithm, genetic algorithm or artificial potential field method.
Preferably, in the step 1, when the classical a-x algorithm is adopted, the cost function F isnComprises the following steps:
Fn=p1gn+p2cn+p3hn
wherein, gnIs the known path distance from the starting point to point n; c. CnIs a point n gravity field characteristic parameter; h isnAn estimate of the minimum distance from point n to the target point; p is a radical of1,p2,p3Are weights.
Preferably, when the dynamic window method is used for planning in step 2, the following evaluation function is used for evaluation to obtain the optimal speed and the optimal angular velocity:
G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)+η·velocity(v,ω))
wherein G (v, omega) is an evaluation function, v is a speed, omega is an angular speed, heading (v, omega) represents an angle difference value between the direction of the underwater vehicle when the underwater vehicle reaches the tail end of the track simulation at the current sampling speed and the direction of a connecting line between the tail end and a target point, dist (v, omega) represents the distance between the underwater vehicle and the nearest obstacle on the current track, velocity (v, omega) represents the speed of the current track, α is a weight, and sigma is a smoothing parameter for smoothing the three weights.
Has the advantages that:
(1) the invention combines the flight path planning with the generation of the gyroscope, the accelerometer and the navigation data to form the long-endurance aircraft track generator, thereby facilitating the verification of theories and methods and reducing the time and cost of actual tests.
(2) According to the invention, the speed, the angular velocity and the course information are converted into centripetal acceleration, tangential acceleration and angular velocity information according to the planned flight path and are used for designing the trajectory generator, the state switching process is natural and continuous, the problem of over-ideal data when a motion state is artificially designed is avoided, the method is closer to the actual state of the motion of an aircraft, and the effectiveness of gravity-assisted inertial navigation research is improved.
Detailed Description
The invention is described in detail below by way of example with reference to the accompanying drawings.
The invention provides a method for designing a track generator of an inertial navigation system based on track planning, which is different from the traditional method for obtaining a track and corresponding data of a gyroscope and an accelerometer by designing a motion state; the method is characterized in that the method directly starts from track planning, takes gravity field information into consideration in the process of track planning, extracts specific speed, angular speed and course information of the aircraft in the process of navigation by combining condition constraints suffered by the aircraft in underwater motion, and converts the speed and course information into centripetal acceleration, tangential acceleration and angular speed so as to obtain gyroscope and accelerometer data. The flow chart of the method of the invention is shown in figure 1, and the method comprises the following steps:
step 1, planning the track of an aircraft by adopting a track planning algorithm, and considering gravity field information during track planning, wherein the conventional track planning algorithms such as classic A and an improved algorithm thereof, a genetic algorithm, an artificial potential field method and the like can be adopted, and for example, gravity field characteristic parameters can be considered in a cost function when planning the track by adopting the classic A and the improved algorithm thereof; the artificial potential field method can set the area which is not suitable for navigation in the gravity field as an obstacle to carry out track planning; the gravity field characteristic parameters and the like can be considered in the adaptive value when the genetic algorithm and the improved algorithm plan the track; other track planning algorithms can add gravity field information to the algorithm by referring to the above-mentioned idea.
The present embodiment takes the classic a algorithm as an example to illustrate: according to the characteristic of gravity matching, gravity field characteristic parameters are added into a traditional cost function in a classical A-x algorithm to represent the adaptability of the point, and the cost function of the improved point n is
Fn=p1gn+p2cn+p3hn(1)
Wherein, gnIs the known path distance from the starting point to point n; c. CnIs the gravity field characteristic parameter of point n; h isnIs an estimate of the minimum distance from point n to the target point. p is a radical of1,p2,p3The weight is set according to the requirements of the length of the flight path, the passing position of the flight path, the running time of the algorithm and the like. In general, the Euclidean distance is used to calculate g in the cost function according to the characteristics of the underwater vehiclenAnd hnCharacteristic parameter c of gravitational fieldnAnd determining according to the intensity of the change of the gravity abnormal value in the gravity matching area. The larger the value of the gravity field characteristic parameter is, the better the gravity matching effect is. So for three weights there are
p2<0 (2)
p1,p3>0 (3)
The distance is considered, the adaptability of a local area is also considered, and the purpose that the planned flight path passes through an area with obvious gravity field change is achieved.
And 2, planning specific speed, angular speed and course information of the aircraft in the navigation process of the flight path planned and obtained in the step 1 by a Dynamic Window Algorithm (DWA).
A common coordinate system is first defined: the system comprises a geocentric inertial coordinate system (i system), a terrestrial coordinate system (e system), a geographic coordinate system, a carrier coordinate system (b system), a track coordinate system (t system) and a navigation coordinate system (n system). The origin of a track coordinate system (t system) is positioned at the center of gravity of the carrier and fixedly connected with the carrier, the y axis is forward along a track, the x axis is horizontally rightward, a right-hand rectangular coordinate system is formed by the z axis and the xy axis, and the roll angle is not considered in the track coordinate system. The navigation coordinate system (n system) is selected as the geographical coordinate system and the heading angle is defined as being positive when rotating counterclockwise. For underwater vehicles, only advancing and steering are generally carried out, so that the invention does not consider the movement in the direction of the sky, but only the movement in the plane formed by the x and y axes of the navigation coordinate system. Because the distance of motion is short in the adjacent moments of the vehicles, the motion between two adjacent points can be considered as a straight line. At the moment, the speed v and the angular speed omega are planned, and the track can be calculated according to the initial position, the speed and the posture. In general, the motion of an underwater vehicle is constrained by the following conditions:
(1) maximum (angular) velocity (v) of underwater vehiclemax,ωmax) With minimum (angular) velocity (v)min,ωmin) The limit of (2);
(2) limiting the acceleration and deceleration of the underwater vehicle;
(3) the underwater vehicle is limited in (angular) speed under maximum deceleration conditions.
Under the condition of meeting the constraint conditions, sampling is carried out on the velocity v and the angular velocity omega to obtain the velocity v of a sampling pointi(i ═ 1,2,3, …) and ωi(i 1,2,3, …), calculating the trace at time Δ t as
x=x+viΔt cosψt(4)
y=y+viΔt sinψt(5)
ψt=ψt+ωiΔt (6)
Wherein x and y are the position of the underwater vehicle in the navigation system, and delta tFor the time length of the track simulation, psitIs the heading angle of the underwater vehicle. By evaluating the function
G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)+η·velocity(v,ω)) (7)
The method comprises the steps of evaluating a track, wherein the heading (v, omega) represents the angle difference between the orientation of the underwater vehicle when the underwater vehicle reaches the tail end of a track simulation at a current sampling speed and the direction of a connecting line between the tail end and a target point, dist (v, omega) represents the distance between the underwater vehicle and the nearest obstacle on the current track, if no obstacle exists, the dist is set to be a constant, velocity (v, omega) represents the speed of the current track, α represents the proportion occupied by the three evaluation indexes (the angle difference, the distance and the speed), and sigma smoothes the three proportionsoptAnd angular velocity ωopt. In each sampling period, passing through an optimum speed voptAnd angular velocity ωoptAnd (4) traveling, namely, enabling the traveling track to navigate along the track planned in the step 1.
Step 3, obtaining the optimal speed v in the step 2optAnd angular velocity ωoptThe information is converted into centripetal acceleration, tangential acceleration and angular velocity information, and the conversion relationship is as follows:
an=voptωopt(8)
wherein, anFor centripetal acceleration, atIs the tangential acceleration.
Step 4, obtaining the centripetal acceleration a of the underwater vehicle in the step 3nTangential acceleration atAnd angular velocity ωoptThe gyroscope data, accelerometer data, and navigation data may be generated as inputs.
When the underwater vehicle only has forward motion and steering, the projection a of the acceleration under the carrier system at the moment is
a=[-anat0]T(10)
An attitude angular rate omega of
ω=[0 0 ωopt]T(11)
From the coordinate system definition, a transformation matrix between coordinate systems can be obtained as
Wherein
Is a transformation matrix of the navigation coordinate system to the carrier coordinate system,
is a transformation matrix from the trajectory coordinate system to the navigation coordinate system,
and transforming the attitude angular rate into a transformation matrix under the carrier system. Psi, theta and gamma are the heading angle, pitch angle and roll angle of the carrier.
The navigation data can be obtained as follows
Wherein v is
x,v
yAnd v
zThe east, north and sky velocities of the underwater vehicle,
λ and h are latitude, longitude and altitude.
The gyroscope and accelerometer outputs may then be calculated as
Wherein
Is the projection of the angular velocity of the carrier system relative to the inertial system on the carrier system,
for the projection of the angular velocity of the earth rotation on the navigation system,
for the projection of the angular velocity of the navigation system relative to the earth system on the navigation system,
for the projection of specific forces on the carrier system, v
n=[v
xv
yv
z]
TVelocity of the underwater vehicle under the navigation system, g
n=[0 0 g]
TAnd g is the acceleration of gravity.
Finally, proper noise is added to the output of the gyroscope and the accelerometer, so that real data can be better simulated.
The invention can effectively overcome the difficulty of artificially designing the motion state during the generation of the traditional flight path and improve the research efficiency of the gravity-assisted inertial navigation system. The invention combines the flight path planning with the generation of the gyroscope, the accelerometer and the navigation data to form the long-endurance aircraft track generator, thereby facilitating the verification of theories and methods such as subsequent gravity matching and the like, and reducing the time and cost of actual tests. In addition, the speed, the angular speed and the course information are converted into centripetal acceleration, tangential acceleration and angular speed information according to the planned flight path and are used for designing the track generator, the state switching process is natural and continuous, the problem that the data is too ideal when the motion state is artificially designed is solved, the problem is closer to the actual state of the motion of the aircraft, and the effectiveness of gravity-assisted inertial navigation research is improved.
The method comprises the steps of selecting a map with 116 multiplied by 116 (unit is a grid) and gravity field parameters, setting the starting point of the underwater vehicle to be [106,99], setting the end point to be [40,30], setting errors of a gyroscope and an accelerometer in a track generator to be 0, and obtaining a track graph of each step through the method, wherein the track graph is shown in figure 2. Wherein, the dot-dash line represents the trajectory planned by the a-x algorithm; the solid line represents the underwater vehicle trajectory generated by the DWA algorithm; the dashed lines represent traces that are resolved from gyroscope and accelerometer data generated by the trace generator. It can be seen that the track obtained by performing inertial navigation calculation on gyroscope and accelerometer data obtained by the design method provided by the invention is almost the same as the track obtained by planning the flight path.
In summary, the above description is only a preferred embodiment of the present invention, and is not intended to limit the scope of the present invention. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.