CN111006694B - Design method of long-endurance inertial navigation system track generator based on track planning - Google Patents
Design method of long-endurance inertial navigation system track generator based on track planning Download PDFInfo
- Publication number
- CN111006694B CN111006694B CN201911385476.0A CN201911385476A CN111006694B CN 111006694 B CN111006694 B CN 111006694B CN 201911385476 A CN201911385476 A CN 201911385476A CN 111006694 B CN111006694 B CN 111006694B
- Authority
- CN
- China
- Prior art keywords
- track
- planning
- speed
- algorithm
- inertial navigation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
Abstract
The invention discloses a method for designing a track generator of an inertial navigation system based on track planning. The invention can effectively overcome the difficulty of artificially designing the motion state of the traditional track generator design method, reduce the time and cost consumed in research and test 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 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.
Description
Technical Field
The invention relates to the technical field of navigation, guidance and control, in particular to a design method of a long-endurance inertial navigation system track generator based on track planning.
Background
The inertial navigation system can provide navigation positioning information such as position, speed, attitude and the like for the carrier in real time, and is widely applied to various carriers such as land, sea, air and sky. For the long-endurance carriers of the underwater vehicle, the error of the inertial navigation system is dispersed along with the time, and the navigation requirement can not be met. The marine gravity measurement is passivity and stable in gravity information, and the gravity-assisted inertial navigation becomes a research hotspot of underwater autonomous navigation. With the research on the gravity-assisted inertial navigation system, a great deal of research and test work is required. The practical test has the problems of long period, high cost, incapability of separating various error sources to carry out single-item test and the like. To solve this problem, it is an effective way to design a trajectory generator to generate gyroscope and accelerometer data and corresponding navigation data. The conventional track generator adopts a mode that tracks are obtained by artificially designing motion states of all stages of an underwater vehicle, and meanwhile, the motion states of all stages in the tracks are input into the track generator to generate navigation data, and then subsequent researches such as gravity matching and the like are carried out. However, the long-endurance inertial navigation system has the characteristics of long driving time, long driving distance and complex driving track, and the problem of difficult design of manually setting different motion states in each stage of the traditional method exists, and the gravity field characteristic parameters cannot be included. In addition, in the process of the artificially designed motion state, all states are directly combined without considering transition during state switching, so that data generated by the traditional method is too ideal during motion state switching and is not in line with the situation of continuous change in the actual process.
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, the method adoptsClassic a algorithm, cost function FnComprises 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, ω) is an evaluation function, v is a velocity, and ω is an angular velocity; heading (v, ω) represents the angular difference between the heading of the underwater vehicle when it reaches the end of the trajectory simulation at the current sampling speed and the direction of the line connecting the end to the target point; dist (v, ω) represents the distance between the underwater vehicle and the nearest obstacle on the current trajectory; velocity (v, ω) represents the velocity of the current track; α, β, η are weights, and σ 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.
Drawings
FIG. 1 is a flow chart of a method for designing a trajectory generator of a long endurance inertial navigation system based on trajectory planning according to an embodiment of the present invention;
FIG. 2 is a trace diagram generated at various stages in accordance with an embodiment of the present invention.
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, gnAs a starting pointA known path distance 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 a navigation system, delta t is the time length of track simulation, psitIs the heading angle of the underwater vehicle. By evaluating the function
G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)+η·velocity(v,ω)) (7)
The trajectory is evaluated. Wherein, heading (v, ω) represents an angular difference between an orientation of the underwater vehicle when the underwater vehicle reaches the terminal of the trajectory simulation at the current sampling speed and a direction of a line connecting the terminal to the target point; dist (v, ω) represents the distance between the underwater vehicle on the current trajectory and the nearest obstacle, which is set to a constant if there is no obstacle; velocity (v, ω) represents the velocity of the current track; α, β, η represent the specific gravities of the above three evaluation indexes (angle difference, distance, and speed), and σ smoothes the three specific gravities. Selecting the optimal speed v according to the evaluation function G (v, omega) under each (v, omega)optAnd angular velocity ωopt. In each sampling period, passing through an optimum speed voptAnd angular velocity ωoptMoving, i.e. making the moving track along the rail planned in step 1And (5) sailing.
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=[-an at 0]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
WhereinIs 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 isx,vyAnd vzThe 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
WhereinIs 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, vn=[vx vy vz]TVelocity of the underwater vehicle under the navigation system, gn=[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.
Claims (3)
1. A design method of a long-endurance inertial navigation system track generator based on track planning is characterized by comprising 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; when planning by the dynamic window method, the following evaluation functions are adopted for evaluation to obtain the optimal speed and the optimal angular speed:
G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)+η·velocity(v,ω))
wherein G (v, ω) is an evaluation function, v is a velocity, and ω is an angular velocity; heading (v, ω) represents the angular difference between the heading of the underwater vehicle when it reaches the end of the trajectory simulation at the current sampling speed and the direction of the line connecting the end to the target point; dist (v, ω) represents the distance between the underwater vehicle and the nearest obstacle on the current trajectory; velocity (v, ω) represents the velocity of the current track; alpha, beta, eta are weights, and sigma is a smoothing parameter for smoothing the three weights;
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.
2. The method for designing the trajectory generator of the long endurance inertial navigation system based on the track planning as claimed in claim 1, wherein in the step 1, the track planning algorithm adopts any one of the following algorithms: a classical a algorithm, an a modified algorithm, a genetic algorithm or an artificial potential field algorithm.
3. The method for designing a trajectory generator of a long-endurance inertial navigation system based on trajectory planning as claimed in claim 2, wherein in step 1, when the classic 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.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911385476.0A CN111006694B (en) | 2019-12-29 | 2019-12-29 | Design method of long-endurance inertial navigation system track generator based on track planning |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911385476.0A CN111006694B (en) | 2019-12-29 | 2019-12-29 | Design method of long-endurance inertial navigation system track generator based on track planning |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111006694A CN111006694A (en) | 2020-04-14 |
CN111006694B true CN111006694B (en) | 2022-03-18 |
Family
ID=70118172
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911385476.0A Active CN111006694B (en) | 2019-12-29 | 2019-12-29 | Design method of long-endurance inertial navigation system track generator based on track planning |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111006694B (en) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117251684B (en) * | 2023-11-14 | 2024-02-02 | 中国船舶集团有限公司第七〇七研究所 | Unmanned platform gravity meter data processing method based on multisource information fusion |
Family Cites Families (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101424534B (en) * | 2008-12-09 | 2011-10-05 | 东南大学 | Inertia/gravity combined navigation semi-physical object simulating device |
US8843340B2 (en) * | 2010-06-23 | 2014-09-23 | Aisin Aw Co., Ltd. | Track information generating device, track information generating method, and computer-readable storage medium |
CN102508954A (en) * | 2011-10-21 | 2012-06-20 | 天津大学 | Full-digital simulation method and device for global positioning system (GPS)/strapdown inertial navigation system (SINS) combined navigation |
WO2015105597A2 (en) * | 2013-11-27 | 2015-07-16 | The Trustees Of The University Of Pennsylvania | Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft micro-aerial vehicle (mav) |
CN106885569A (en) * | 2017-02-24 | 2017-06-23 | 南京理工大学 | A kind of missile-borne deep combination ARCKF filtering methods under strong maneuvering condition |
CN108225310B (en) * | 2017-12-22 | 2020-08-25 | 中国船舶重工集团公司第七0七研究所 | Gravity-assisted navigation track planning method |
CN109489687A (en) * | 2018-11-16 | 2019-03-19 | 北京电子工程总体研究所 | A kind of emulation verification method and simulation and verification platform for navigation algorithm |
-
2019
- 2019-12-29 CN CN201911385476.0A patent/CN111006694B/en active Active
Also Published As
Publication number | Publication date |
---|---|
CN111006694A (en) | 2020-04-14 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107462243B (en) | A kind of cloud control automatic Pilot task creating method based on high-precision map | |
CN110471450B (en) | Method for directly planning reentry trajectory in altitude velocity profile | |
Lugo-Cárdenas et al. | Dubins path generation for a fixed wing UAV | |
CN102393200B (en) | General inertial navigation test method based on flight simulation | |
CN107024863A (en) | A kind of UUV Trajectory Tracking Control methods for avoiding differential from exploding | |
Fowler et al. | Bézier curve path planning for parafoil terminal guidance | |
Tian et al. | Wind sensing and estimation using small fixed-wing unmanned aerial vehicles: A survey | |
CN109657256A (en) | A kind of high-precision ballistic reenters nominal return trajectory emulation mode | |
CN111444603A (en) | Method for rapidly planning shortest time off-orbit trajectory of recoverable spacecraft | |
CN113900448A (en) | Aircraft prediction correction composite guidance method based on sliding mode disturbance observer | |
De Ridder et al. | Optimal longitudinal trajectories for reusable space vehicles in the terminal area | |
CN111006694B (en) | Design method of long-endurance inertial navigation system track generator based on track planning | |
Ayhan et al. | Path planning for UAVs with engine failure in the presence of winds | |
CN113310496B (en) | Method and device for determining moon-earth transfer track | |
Elkaim et al. | Station keeping and segmented trajectory control of a wind-propelled autonomous catamaran | |
Recoskie | Autonomous hybrid powered long ranged airship for surveillance and guidance | |
CN109445283B (en) | Control method for fixed-point tracking of under-actuated aerostat on plane | |
Stolle et al. | A vision-based flight guidance and navigation system for autonomous cross-country soaring UAVs | |
CN115963855A (en) | Unpowered reentry aircraft landing area prediction method based on deep learning | |
CN105973237A (en) | Actual flight data interpolation-based simulated dynamic locus analyzing and generating method | |
Zakharin et al. | Concept of navigation system design of UAV | |
CN111651860B (en) | Predictive correction robust guidance method for re-entry section of reusable carrier | |
Li et al. | Re-entry guidance method based on decoupling control variables and waypoint | |
CN114740882A (en) | Trajectory generation method for ensuring visibility of elastic target tracking by unmanned aerial vehicle | |
Geiger et al. | Flight testing a real-time direct collocation path planner |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |