CN108536163A - A kind of kinetic model/laser radar Combinated navigation method under single-sided structure environment - Google Patents

A kind of kinetic model/laser radar Combinated navigation method under single-sided structure environment Download PDF

Info

Publication number
CN108536163A
CN108536163A CN201810214097.4A CN201810214097A CN108536163A CN 108536163 A CN108536163 A CN 108536163A CN 201810214097 A CN201810214097 A CN 201810214097A CN 108536163 A CN108536163 A CN 108536163A
Authority
CN
China
Prior art keywords
moment
point
sided structure
structure environment
axis
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.)
Granted
Application number
CN201810214097.4A
Other languages
Chinese (zh)
Other versions
CN108536163B (en
Inventor
吕品
石鹏
赖际舟
邹冰倩
付相可
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN201810214097.4A priority Critical patent/CN108536163B/en
Publication of CN108536163A publication Critical patent/CN108536163A/en
Application granted granted Critical
Publication of CN108536163B publication Critical patent/CN108536163B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
    • G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft

Abstract

The invention discloses a kind of kinetic model under single-sided structure environment/laser radar Combinated navigation methods, include the following steps：(1) period reads k moment quadrotor airborne sensor information, including laser radar point cloud information S (k), gyroscope informationAccelerometer informationBarometertic altimeter information hb(k)；(2) the line feature of S (k) extraction k moment single-sided structure environment is utilized；(3) relative distance, opposite yaw angle of the k moment quadrotor relative to single-sided structure environment are calculated；(4) attitude quaternion of prediction k moment quadrotors, speed, position；(5) attitude quaternion at k moment, speed, position are corrected in conjunction with the drag characteristic of quadrotor by Kalman filter.The present invention can complete the resolving of quadrotor posture, speed, position based on laser radar under single-sided structure environment.

Description

A kind of kinetic model/laser radar Combinated navigation method under single-sided structure environment
Technical field
The present invention relates to integrated navigation field, kinetic model/laser radar under especially a kind of single-sided structure environment Combinated navigation method.
Background technology
Quadrotor have many advantages, such as it is small, simple in structure, can hover and VTOL, in recent years it is military, Civil field is widely used.Navigation system provides navigation necessary to its flight control system for quadrotor Information is that it completes the necessary guarantee of various complicated aerial missions.Currently, satellite navigation and vision guided navigation are quadrotors Common air navigation aid.However, satellite navigation signals are easily disturbed, vision guided navigation is easily interfered by light.When satellite navigation, vision When navigating unavailable, laser radar is that quadrotor commonly navigates mode.
Laser radar is to emit the radar system of the characteristic quantities such as the position of detecting laser beam target, speed.Its work is former Reason is then to be compared the reflected signal of slave target received with transmitting signal to objective emission detectable signal, After making proper treatment, so that it may obtain target for information about.When laser radar is for when navigating, generally using while positioning and ground Figure construction method (Simultaneous Localization And Mapping, abbreviation SLAM), by various on carrier Sensing data is acquired and calculates, and realizes the positioning to its own position, posture and the structure to scene map.
When using laser radar SLAM methods, it is desirable that ambient enviroment has more apparent structure feature, when feature is dilute SLAM effects can be influenced when thin.Single-sided structure environment involved in the present invention refers to the environment of an only smooth metope, Belong to feature sparse environment.For quadrotor when executing patch wall aerial mission, the environment that laser radar is detected is sometimes There is single-sided structure environment, the navigation accuracy obtained at this time by traditional laser radar SLAM methods is poor.
Invention content
Technical problem to be solved by the present invention lies in provide kinetic model/laser under a kind of single-sided structure environment Radar complex air navigation aid can complete quadrotor posture, speed based on laser radar under single-sided structure environment The resolving of degree, position.
In order to solve the above technical problems, the present invention provides kinetic model/laser radar under a kind of single-sided structure environment Combinated navigation method includes the following steps：
(1) period reads k moment quadrotor airborne sensor information, including laser radar point cloud information S (k), Gyroscope informationAccelerometer informationBarometertic altimeter information hb(k)；
(2) the line feature of S (k) extraction k moment single-sided structure environment is utilized；
(3) relative distance, opposite yaw angle of the k moment quadrotor relative to single-sided structure environment are calculated；
(4) attitude quaternion of prediction k moment quadrotors, speed, position；
(5) pass through Kalman filter, in conjunction with the drag characteristic of quadrotor, attitude quaternion, speed to the k moment Degree, position are corrected；Wherein, the X, Y, Z axis of body system be respectively the heading of aircraft, it is dextrad, lower to laser radar System is fixedly connected with body, and straight down, X-axis is perpendicularly oriented to single-sided structure to the Z axis for system of navigating, and Y-axis is determined according to the right-hand rule, is led Boat is the position that origin is initial time body system origin.
Preferably, in step (2), the line feature of extraction k moment single-sided structure environment specifically comprises the following steps：
(21) two-dimensional coordinate of horizontal plane where laser spot projection to aircraft in S (k) is calculated：
Remember piFor S (k) i-th of laser point (i=1,2 ..., N0), N0For the quantity of laser point in S (k),WithFor piCoordinate in body system, note θ (k-1) and φ (k-1) are respectively pitch angle and roll angle of the aircraft at the k-1 moment, piIt throws The two-dimensional coordinate of horizontal plane where shadow to aircraftWithIt calculates according to the following formula：
(22) the tearing point in detection S (k)：
Calculate the distance between adjacent serial number laser points of S (k)IfGreatly In threshold value Et, then piAnd pi+1To tear point, note tearing point set is combined into1≤r≤Nt, NtFor the quantity of tearing point；
(23) angle point in detection S (k), divides point cloud data to group：
If dc is more than threshold value EcThen piFor angle point, and it is not angle point to tear point, i.e.,The calculation formula of dc is as follows：
Note angle point collection is combined into1≤c≤Nc, NcFor the quantity of angle point, the mark by angle point and tearing point as minute group Will, according to serial number of the laser point in S (k) successively from p1It traversesDivide point cloud data to group, is denoted as sg, 1≤g≤Ng, Ng For the quantity of point group；
(24) screening calculates the point cloud data of the line feature of single-sided structure environment：
According to laser point in the serial number of S (k), calculate in each point group between the point of serial number minimum and the maximum point of serial number DistanceIt filters outMaximum point group is used to calculate the line feature of single-sided structure environment, which is denoted as
(25) it utilizesPoint group is fitted the linear equation of single-sided structure environment；
Remember that the linear equation of single-sided structure environment is x=a1y+a2, remember qiForI-th of laser point (i=1,2 ..., N1), N1ForThe quantity of middle laser point,WithFor qiIt projects to the two-dimensional coordinate of horizontal plane where aircraft, according to the following formula Calculate the parameter a of linear equation1And a2：
Preferably, in step (3), relative distance, phase of the k moment quadrotor relative to single-sided structure environment are calculated Yaw angle is specifically comprised the following steps：
(31) if a1=0, then quadrotor is a relative to the distance d (k) of single-sided structure environment2, yaw angle ψl(k) For：
(32) if a1≠ 0, then distance of the quadrotor relative to single-sided structure environment Yaw angle ψl(k) it is：
Wherein, it is 0 that quadrotor heading, which is perpendicularly oriented to yaw angle when single-sided structure, is clockwise turned to Just.
Preferably, in step (4), attitude quaternion, speed and the position of prediction k moment quadrotors specifically include Following steps：
(41) attitude quaternion prediction uses following formula：
Wherein, Q (k)=[qo(k) q1(k) q2(k) q3(k)]TFor the attitude quaternion at k moment, subscript T representing matrixes Transposition；；Q (k-1)=[qo(k-1) q1(k-1) q2(k-1) q3(k-1)]TFor the attitude quaternion at k-1 moment；Δ T be from Dissipate the sampling period；It is calculate by the following formula：
WhereinIt is calculate by the following formula：
Wherein,For k moment gyroscopes read aircraft body system relative to navigation Component of the angular speed of system in body system X, Y, Z axis,For k moment gyroscopes zero bias in machine Component in system X, Y, Z axis；
(42) prediction of speed uses following formula：
Wherein,Accelerate for the k moment The aircraft body system that degree meter is read is relative to component of the acceleration for being in body system X, Y, Z axis that navigate；It is k moment accelerometer bias in body system Component in X, Y, Z axis；G=[0 0 g]T, g is local gravitational acceleration value；It is k moment bodies system relative to navigation The linear velocity of system is the component in X, Y, Z axis in navigation； Relative to the linear velocity of navigation system in navigation it is X, Y, Z axis for k-1 moment body systems On component；
Attitude matrix between being for body system to navigation, is calculate by the following formula：
(43) position prediction uses following formula：
Wherein, the position at k momentRespectively In navigation it is position coordinates in X, Y, Z axis for the aircraft k moment；The position at k-1 momentThe respectively aircraft k-1 moment It is the position coordinates in X, Y, Z axis in navigation；
(44) accelerometer biasWith gyroscope zero biasPrediction uses following formula：
Wherein,It is k-1 moment accelerometer bias in body system X, Y, Z axis Component；For component of the zero bias in body system X, Y, Z axis of k-1 moment gyroscopes.
Preferably, in step (5), by Kalman filter, in conjunction with the drag characteristic of quadrotor, to the k moment Attitude quaternion, speed, position be corrected and specifically comprise the following steps：
(51) one-step prediction mean square error P is calculatedk|k-1：
P (k | k-1)=A (k, k-1) P (k-1 | k-1) A (k, k-1)T+G(k-1)W(k-1)G(k-1)T
In formula,
I3×3For 3 × 3 unit matrix, I4×4For 4 × 4 unit matrix, 03×3For 3 × 3 null matrix, 03×4It is 3 × 4 Null matrix, A (k, k-1) be filter k-1 moment to the k moment filter Matrix of shifting of a step；When P (k-1 | k-1) is k-1 The state estimation mean square deviation at quarter, P (k | k-1) are the one-step prediction mean square deviation at k-1 moment to k moment；G is the filter noise coefficient square at filter k-1 moment Battle array,W is that k-1 moment states are made an uproar Sound, εωx、εωyAnd εωzRespectivelyWithPlant noise, εfx、εfyAnd εfzRespectivelyWithModel Noise,WithRespectivelyNoise criteria it is poor, WithRespectively Noise criteria it is poor；
(52) k moment extended Kalman filter filtering gain K (k) are calculated
K (k)=P (k | k-1) H (k)T[H(k)P(k|k-1)H(k)T+R(k)]-1
In formula,Λa=[1 0 0], Λb=[0 0-1], Θa=[q0(k- 1)2+q1(k-1)2-q2(k-1)2-q3(k-1)22(q1(k-1)q2(k-1)+q0(k-1)q3(k-1))2(q1(k-1)q3(k-1)-q0(k-1)q2 (k-1))], Θb=[2 (q1(k-1)q2(k-1)-q0(k-1)q3(k-1))q0(k-1)2+q2(k-1)2-q1(k-1)2-q3(k-1)22(q2(k- 1)q3(k-1)+q0(k-1)q1(k-1))], H (k) is k moment measurement matrixes, and K (k) is k The filtering gain at moment,For the measurement noise at k moment, diag indicates square Battle array diagonalization, whereinRespectivelyNoise；kHx、kHyFor Model parameter is constant, is obtained by offline identification method；04×1For 4 × 1 null matrix, 03×1For 3 × 1 null matrix, 06×1For 6 × 1 null matrix；
(53) extended Kalman filter state estimation when calculating k
In formula,
For the estimated value of k moment quantity of states,For the state variable one-step prediction value at k-1 to k moment, step is used Suddenly the predictor formula of (4) is calculated,For the k moment Measuring value, pass through step 1 and step 3 and obtain；
(54) k moment extended Kalman filters estimation mean square error P (k | k) is calculated：
P (k | k)=[I-K (k) H (k)] P (k | k-1)
In formula, and P (k | k) it is to estimate mean square error the k moment, I is unit matrix.
Beneficial effects of the present invention are：The present invention can complete four rotations based on laser radar under single-sided structure environment The resolving of rotor aircraft posture, speed, position.
Description of the drawings
Fig. 1 is the method flow schematic diagram of the present invention.
Specific implementation mode
The flow of the method for the present invention is as shown in Figure 1, it is as follows：
Step 1：Period reads k moment quadrotor airborne sensor information, including laser radar point cloud information S (k), gyroscope informationAccelerometer informationBarometertic altimeter information hb(k)；
Step 2：The line feature of k moment single-sided structure environment is extracted using S (k), the specific method is as follows：
1) two-dimensional coordinate of horizontal plane where laser spot projection to aircraft in S (k) is calculated：
Remember piFor S (k) i-th of laser point (i=1,2 ..., N0), N0For the quantity of laser point in S (k),WithFor piCoordinate in body system, note θ (k-1) and φ (k-1) are respectively pitch angle and roll angle of the aircraft at the k-1 moment, piIt throws The two-dimensional coordinate of horizontal plane where shadow to aircraftWithIt calculates according to the following formula：
2) the tearing point in detection S (k)：
Calculate the distance between adjacent serial number laser points of S (k)IfGreatly In threshold value Et, then piAnd pi+1For tearing point.Note tearing point set is combined into1≤r≤Nt, NtFor the quantity of tearing point.
3) angle point in detection S (k), divides point cloud data to group：
If dc is more than threshold value EcThen piFor angle point, and it is not angle point to tear point, i.e.,The calculation formula of dc is as follows：
Note angle point collection is combined into1≤c≤Nc, NcFor the quantity of angle point.Mark by angle point and tearing point as minute group Will, according to serial number of the laser point in S (k) successively from p1It traversesDivide point cloud data to group, is denoted as sg, 1≤g≤Ng, Ng For the quantity of point group.
4) screening calculates the point cloud data of the line feature of single-sided structure environment：
According to laser point in the serial number of S (k), calculate in each point group between the point of serial number minimum and the maximum point of serial number DistanceIt filters outMaximum point group is used to calculate the line feature of single-sided structure environment, which is denoted as
5) it utilizesPoint group is fitted the linear equation of single-sided structure environment；
Remember that the linear equation of single-sided structure environment is x=a1y+a2, remember qiForI-th of laser point (i=1,2 ..., N1), N1ForThe quantity of middle laser point,WithFor qiIt projects to the two-dimensional coordinate of horizontal plane where aircraft, according to the following formula Calculate the parameter a of linear equation1And a2：
Step 3：Relative distance, opposite yaw angle of the k moment quadrotor relative to single-sided structure environment are calculated, The specific method is as follows：
If 1) a1=0, then quadrotor is a relative to the distance d (k) of single-sided structure environment2, yaw angle ψl(k) For：
If 2) a1≠ 0, then distance of the quadrotor relative to single-sided structure environment Yaw angle ψl(k) it is：
Wherein, it is 0 that quadrotor heading, which is perpendicularly oriented to yaw angle when single-sided structure, is clockwise turned to Just.
Step 4：The attitude quaternion of prediction k moment quadrotors, speed, position, the specific method is as follows：
1) attitude quaternion prediction uses following formula：
Wherein, Q (k)=[qo(k) q1(k) q2(k) q3(k)]TFor the attitude quaternion at k moment, subscript T representing matrixes Transposition；；Q (k-1)=[qo(k-1) q1(k-1) q2(k-1) q3(k-1)]TFor the attitude quaternion at k-1 moment；Δ T be from Dissipate the sampling period；It is calculate by the following formula：
WhereinIt is calculate by the following formula：
Wherein,For k moment gyroscopes read aircraft body system relative to navigation Component of the angular speed of system in body system X, Y, Z axis,For k moment gyroscopes zero bias in machine Component in system X, Y, Z axis；
2) prediction of speed uses following formula：
Wherein,Add for the k moment The aircraft body system that speedometer is read is relative to component of the acceleration for being in body system X, Y, Z axis that navigate；It is k moment accelerometer bias in body It is the component in X, Y, Z axis；G=[0 0 g]T, g is local gravitational acceleration value；It is k moment bodies system relative to navigation The linear velocity of system is the component in X, Y, Z axis in navigation； Relative to the linear velocity of navigation system in navigation it is X, Y, Z axis for k-1 moment body systems On component；
Attitude matrix between being for body system to navigation, is calculate by the following formula：
3) position prediction uses following formula：
Wherein, the position at k momentRespectively In navigation it is position coordinates in X, Y, Z axis for the aircraft k moment；The position at k-1 momentThe respectively aircraft k-1 moment It is the position coordinates in X, Y, Z axis in navigation；
4) accelerometer biasWith gyroscope zero biasPrediction uses following formula：
Wherein,It is k-1 moment accelerometer bias in body system X, Y, Z axis Component；For component of the zero bias in body system X, Y, Z axis of k-1 moment gyroscopes；
Step 5：By Kalman filter, in conjunction with the drag characteristic of quadrotor, to the posture quaternary at k moment Number, speed, position are corrected：
1) one-step prediction mean square error P is calculatedk|k-1：
P (k | k-1)=A (k, k-1) P (k-1 | k-1) A (k, k-1)T+G(k-1)W(k-1)G(k-1)T
In formula,
I3×3For 3 × 3 unit matrix, I4×4For 4 × 4 unit matrix, 03×3For 3 × 3 null matrix, 03×4It is 3 × 4 Null matrix, A (k, k-1) be filter k-1 moment to the k moment filter Matrix of shifting of a step；When P (k-1 | k-1) is k-1 The state estimation mean square deviation at quarter, P (k | k-1) are the one-step prediction mean square deviation at k-1 moment to k moment；G is the filter noise coefficient square at filter k-1 moment Battle array,W is that k-1 moment states are made an uproar Sound, εωx、εωyAnd εωzRespectivelyWithPlant noise, εfx、εfyAnd εfzRespectivelyWith's Plant noise,WithRespectivelyNoise criteria it is poor, WithRespectivelyNoise criteria it is poor；
2) k moment extended Kalman filter filtering gain K (k) are calculated：
K (k)=P (k | k-1) H (k)T[H(k)P(k|k-1)H(k)T+R(k)]-1
In formula,Λa=[1 0 0], Λb=[0 0-1], Θa=[q0 (k-1)2+q1(k-1)2-q2(k-1)2-q3(k-1)22(q1(k-1)q2(k-1)+q0(k-1)q3(k-1))2(q1(k-1)q3(k-1)-q0(k-1) q2(k-1))], Θb=[2 (q1(k-1)q2(k-1)-q0(k-1)q3(k-1))q0(k-1)2+q2(k-1)2-q1(k-1)2-q3(k-1)22(q2 (k-1)q3(k-1)+q0(k-1)q1(k-1))], H (k) is k moment measurement matrixes, and K (k) is k The filtering gain at moment,For the measurement noise at k moment, diag indicates square Battle array diagonalization, whereinRespectivelyNoise；kHx、kHyFor Model parameter is constant, is obtained by offline identification method；04×1For 4 × 1 null matrix, 03×1For 3 × 1 null matrix, 06×1For 6 × 1 null matrix；
3) extended Kalman filter state estimation when calculating k
In formula,
For the estimated value of k moment quantity of states,For the state variable one-step prediction value at k-1 to k moment, step 4 is used Predictor formula be calculated,For the measurement at k moment Value is obtained by step 1 and step 3；
4) k moment extended Kalman filters estimation mean square error P (k | k) is calculated：
P (k | k)=[I-K (k) H (k)] P (k | k-1)
In formula, and P (k | k) it is to estimate mean square error the k moment, I is unit matrix.
The present invention can complete quadrotor posture based on laser radar, speed, position under single-sided structure environment The resolving set.

Claims (5)

1. kinetic model/laser radar Combinated navigation method under a kind of single-sided structure environment, which is characterized in that including as follows Step：
(1) period reads k moment quadrotor airborne sensor information, including laser radar point cloud information S (k), gyro Instrument informationAccelerometer informationBarometertic altimeter information hb(k)；
(2) the line feature of S (k) extraction k moment single-sided structure environment is utilized；
(3) relative distance, opposite yaw angle of the k moment quadrotor relative to single-sided structure environment are calculated；
(4) attitude quaternion of prediction k moment quadrotors, speed, position；
(5) by Kalman filter, in conjunction with the drag characteristic of quadrotor, to the attitude quaternion at k moment, speed, Position is corrected；Wherein, the X, Y, Z axis of body system be respectively the heading of aircraft, dextrad, it is lower to, laser radar with Body system is fixedly connected with, and straight down, X-axis is perpendicularly oriented to single-sided structure to the Z axis for system of navigating, and Y-axis is determined according to the right-hand rule, navigation It is the position that origin is initial time body system origin.
2. kinetic model/laser radar Combinated navigation method under single-sided structure environment as described in claim 1, feature It is, in step (2), the line feature of extraction k moment single-sided structure environment specifically comprises the following steps：
(21) two-dimensional coordinate of horizontal plane where laser spot projection to aircraft in S (k) is calculated：
Remember piFor S (k) i-th of laser point (i=1,2 ..., N0), N0For the quantity of laser point in S (k),WithFor pi Coordinate in body system, note θ (k-1) and φ (k-1) are respectively pitch angle and roll angle of the aircraft at the k-1 moment, piProjection The two-dimensional coordinate of horizontal plane where to aircraftWithIt calculates according to the following formula：
(22) the tearing point in detection S (k)：
Calculate the distance between adjacent serial number laser points of S (k)IfMore than threshold value Et, then piAnd pi+1To tear point, note tearing point set is combined into1≤r≤Nt, NtFor the quantity of tearing point；
(23) angle point in detection S (k), divides point cloud data to group：
If dc is more than threshold value EcThen piFor angle point, and it is not angle point to tear point, i.e.,The calculation formula of dc is as follows：
Note angle point collection is combined into1≤c≤Nc, NcFor the quantity of angle point, the mark by angle point and tearing point as minute group, according to Serial number of the laser point in S (k) is successively from p1It traversesDivide point cloud data to group, is denoted as sg, 1≤g≤Ng, NgFor point group Quantity；
(24) screening calculates the point cloud data of the line feature of single-sided structure environment：
According to laser point in the serial number of S (k), the distance between the point of serial number minimum and the maximum point of serial number in each point group are calculatedIt filters outMaximum point group is used to calculate the line feature of single-sided structure environment, which is denoted as
(25) it utilizesPoint group is fitted the linear equation of single-sided structure environment；
Remember that the linear equation of single-sided structure environment is x=a1y+a2, remember qiForI-th of laser point (i=1,2 ..., N1), N1 ForThe quantity of middle laser point,WithFor qiIt projects to the two-dimensional coordinate of horizontal plane where aircraft, calculates according to the following formula straight The parameter a of line equation1And a2：
3. kinetic model/laser radar Combinated navigation method under single-sided structure environment as described in claim 1, feature It is, in step (3), the relative distance, the opposite yaw angle that calculate k moment quadrotor relative to single-sided structure environment have Body includes the following steps：
(31) if a1=0, then quadrotor is a relative to the distance d (k) of single-sided structure environment2, yaw angle ψl(k) it is：
(32) if a1≠ 0, then distance of the quadrotor relative to single-sided structure environment Yaw angle ψl(k) it is：
Wherein, it is 0 that quadrotor heading, which is perpendicularly oriented to yaw angle when single-sided structure, is clockwise turned to just.
4. kinetic model/laser radar Combinated navigation method under single-sided structure environment as described in claim 1, feature It is, in step (4), attitude quaternion, speed and the position of prediction k moment quadrotors specifically comprise the following steps：
(41) attitude quaternion prediction uses following formula：
Wherein, Q (k)=[qo(k) q1(k) q2(k) q3(k)]TFor the attitude quaternion at k moment, subscript T representing matrixes turn It sets；；Q (k-1)=[qo(k-1) q1(k-1) q2(k-1) q3(k-1)]TFor the attitude quaternion at k-1 moment；Δ T is discrete adopts The sample period；It is calculate by the following formula：
WhereinIt is calculate by the following formula：
Wherein,The angle for being relative to navigation for the aircraft body system that k moment gyroscopes are read Component of the speed in body system X, Y, Z axis,For k moment gyroscopes zero bias body system X, Y, the component on Z axis；
(42) prediction of speed uses following formula：
Wherein, It is read for k moment accelerometers The aircraft body system taken is relative to component of the acceleration for being in body system X, Y, Z axis that navigate； For k moment accelerometer bias body system X, Y, the component on Z axis；G=[0 0 g]T, g is local gravitational acceleration value； For k moment body systems relative to navigation system linear velocity navigation be in X, Y, Z axis point Amount； For the k-1 moment Body system is the component in X, Y, Z axis relative to the linear velocity for being is navigated in navigation；
Attitude matrix between being for body system to navigation, is calculate by the following formula：
(43) position prediction uses following formula：
Wherein, the position at k moment Respectively aircraft The k moment is the position coordinates in X, Y, Z axis in navigation；The position at k-1 moment Respectively the aircraft k-1 moment is the position coordinates in X, Y, Z axis in navigation；
(44) accelerometer biasWith gyroscope zero biasPrediction uses following formula：
Wherein,It is k-1 moment accelerometer bias dividing in body system X, Y, Z axis Amount；For component of the zero bias in body system X, Y, Z axis of k-1 moment gyroscopes.
5. kinetic model/laser radar Combinated navigation method under single-sided structure environment as described in claim 1, feature It is, in step (5), by Kalman filter, in conjunction with the drag characteristic of quadrotor, to the posture quaternary at k moment Number, speed, position, which are corrected, to be specifically comprised the following steps：
(51) one-step prediction mean square error P is calculatedk|k-1：
P (k | k-1)=A (k, k-1) P (k-1 | k-1) A (k, k-1)T+G(k-1)W(k-1)G(k-1)T
In formula, I3×3It is 3 × 3 Unit matrix, I4×4For 4 × 4 unit matrix, 03×3For 3 × 3 null matrix, 03×4For 3 × 4 null matrix, A (k, k-1) is filter The filter Matrix of shifting of a step at k-1 moment to k moment；P (k-1 | k-1) is the state estimation mean square deviation at k-1 moment, and P (k | k-1) it is k-1 The one-step prediction mean square deviation at moment to k moment；G is the filter k-1 moment Filter noise coefficient matrix,W For k-1 moment state-noises, εωx、εωyAnd εωzRespectivelyWithPlant noise, εfx、εfyAnd εfzRespectivelyWithPlant noise,WithRespectivelyNoise criteria it is poor, With RespectivelyNoise criteria it is poor；
(52) k moment extended Kalman filter filtering gain K (k) are calculated：
K (k)=P (k | k-1) H (k)T[H(k)P(k|k-1)H(k)T+R(k)]-1
In formula,Λa=[1 0 0], Λb=[0 0-1],
Θa=[q0(k-1)2+q1(k-1)2-q2(k-1)2-q3(k-1)22(q1(k-1)q2(k-1)+q0(k-1)q3(k-1))2(q1 (k-1)q3(k-1)-q0(k-1)q2(k-1))],
Θb=[2 (q1(k-1)q2(k-1)-q0(k-1)q3(k-1))q0(k-1)2+q2(k-1)2-q1(k-1)2-q3(k-1)22(q2(k-1)q3 (k-1)+q0(k-1)q1(k-1))], H (k) is k moment measurement matrixes, and K (k) is k The filtering gain at moment,For the measurement noise at k moment, diag indicates square Battle array diagonalization, whereinRespectivelyψl、d、hbNoise；kHx、kHyJoin for model Number, is constant, is obtained by offline identification method；04×1For 4 × 1 null matrix, 03×1For 3 × 1 null matrix, 06×1It is 6 × 1 null matrix；
(53) extended Kalman filter state estimation when calculating k
In formula,
For the estimated value of k moment quantity of states,For the state variable one-step prediction value at k-1 to k moment, use step (4) Predictor formula be calculated,For the measurement at k moment Value is obtained by step 1 and step 3；
(54) k moment extended Kalman filters estimation mean square error P (k | k) is calculated：
P (k | k)=[I-K (k) H (k)] P (k | k-1)
In formula, and P (k | k) it is to estimate mean square error the k moment, I is unit matrix.
CN201810214097.4A 2018-03-15 2018-03-15 Dynamic model/laser radar combined navigation method in single-sided structure environment Active CN108536163B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810214097.4A CN108536163B (en) 2018-03-15 2018-03-15 Dynamic model/laser radar combined navigation method in single-sided structure environment

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810214097.4A CN108536163B (en) 2018-03-15 2018-03-15 Dynamic model/laser radar combined navigation method in single-sided structure environment

Publications (2)

Publication Number Publication Date
CN108536163A true CN108536163A (en) 2018-09-14
CN108536163B CN108536163B (en) 2021-04-16

Family

ID=63484025

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810214097.4A Active CN108536163B (en) 2018-03-15 2018-03-15 Dynamic model/laser radar combined navigation method in single-sided structure environment

Country Status (1)

Country Link
CN (1) CN108536163B (en)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109708632A (en) * 2019-01-31 2019-05-03 济南大学 A kind of laser radar towards mobile robot/INS/ terrestrial reference pine combination navigation system and method
CN110749327A (en) * 2019-08-08 2020-02-04 南京航空航天大学 Vehicle navigation method in cooperation environment
CN111207734A (en) * 2020-01-16 2020-05-29 西安因诺航空科技有限公司 EKF-based unmanned aerial vehicle integrated navigation method
CN111812669A (en) * 2020-07-16 2020-10-23 南京航空航天大学 Winding inspection device, positioning method thereof and storage medium
CN113110066A (en) * 2021-05-13 2021-07-13 河北科技大学 Finite-time Super-Twisting sliding mode control method for four-rotor aircraft

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7369229B2 (en) * 2003-11-26 2008-05-06 Florida Environmental Research Institute, Inc. Spectral imaging system
CN102645933A (en) * 2012-05-02 2012-08-22 中国人民解放军海军航空工程学院 Method for implementing flexible combined overload control for aircraft in large airspace
CN103528587A (en) * 2013-10-15 2014-01-22 西北工业大学 Autonomous integrated navigation system
CN103837151A (en) * 2014-03-05 2014-06-04 南京航空航天大学 Pneumatic model-assisted navigation method for four-rotor-wing air vehicle
CN105371840A (en) * 2015-10-30 2016-03-02 北京自动化控制设备研究所 Method for combined navigation of inertia/visual odometer/laser radar
CN107063248A (en) * 2017-02-10 2017-08-18 南京航空航天大学 Kinetic model based on rotor rotating speed aids in the air navigation aid of inertial navigation

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7369229B2 (en) * 2003-11-26 2008-05-06 Florida Environmental Research Institute, Inc. Spectral imaging system
CN102645933A (en) * 2012-05-02 2012-08-22 中国人民解放军海军航空工程学院 Method for implementing flexible combined overload control for aircraft in large airspace
CN103528587A (en) * 2013-10-15 2014-01-22 西北工业大学 Autonomous integrated navigation system
CN103837151A (en) * 2014-03-05 2014-06-04 南京航空航天大学 Pneumatic model-assisted navigation method for four-rotor-wing air vehicle
CN105371840A (en) * 2015-10-30 2016-03-02 北京自动化控制设备研究所 Method for combined navigation of inertia/visual odometer/laser radar
CN107063248A (en) * 2017-02-10 2017-08-18 南京航空航天大学 Kinetic model based on rotor rotating speed aids in the air navigation aid of inertial navigation

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
WENJING CHEN 等: "An improved model aided navigation method with online dynamics parameters estimation for multirotor UAV (IEEE CGNCC)", 《PROCEEDINGS OF 2016 IEEE CHINESE GUIDANCE, NAVIGATION AND CONTROL CONFERENCE》 *
ZHANG LIANG 等: "An improved MCS/INS integrated navigation algorithm for multi-rotor UAV in indoor flight", 《PROCEEDINGS OF 2016 IEEE CHINESE GUIDANCE, NAVIGATION AND CONTROL CONFERENCE》 *
吕品 等: "飞行器气动模型辅助导航方法的研究概况与进展", 《控制与决策》 *
黄明登 等: "环境特征提取在移动机器人导航中的应用", 《控制工程》 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109708632A (en) * 2019-01-31 2019-05-03 济南大学 A kind of laser radar towards mobile robot/INS/ terrestrial reference pine combination navigation system and method
CN110749327A (en) * 2019-08-08 2020-02-04 南京航空航天大学 Vehicle navigation method in cooperation environment
CN111207734A (en) * 2020-01-16 2020-05-29 西安因诺航空科技有限公司 EKF-based unmanned aerial vehicle integrated navigation method
CN111207734B (en) * 2020-01-16 2022-01-07 西安因诺航空科技有限公司 EKF-based unmanned aerial vehicle integrated navigation method
CN111812669A (en) * 2020-07-16 2020-10-23 南京航空航天大学 Winding inspection device, positioning method thereof and storage medium
CN113110066A (en) * 2021-05-13 2021-07-13 河北科技大学 Finite-time Super-Twisting sliding mode control method for four-rotor aircraft
CN113110066B (en) * 2021-05-13 2022-04-29 河北科技大学 Finite-time Super-Twisting sliding mode control method for four-rotor aircraft

Also Published As

Publication number Publication date
CN108536163B (en) 2021-04-16

Similar Documents

Publication Publication Date Title
CN108536163A (en) A kind of kinetic model/laser radar Combinated navigation method under single-sided structure environment
CN107314718B (en) High speed rotation bullet Attitude estimation method based on magnetic survey rolling angular rate information
CN106780699B (en) Visual SLAM method based on SINS/GPS and odometer assistance
CN107727079B (en) Target positioning method of full-strapdown downward-looking camera of micro unmanned aerial vehicle
Li et al. LIDAR/MEMS IMU integrated navigation (SLAM) method for a small UAV in indoor environments
CN108362288B (en) Polarized light SLAM method based on unscented Kalman filtering
Shen et al. Optical flow sensor/INS/magnetometer integrated navigation system for MAV in GPS-denied environment
US9618344B2 (en) Digital map tracking apparatus and methods
CN109813311A (en) A kind of unmanned plane formation collaborative navigation method
CN107289930B (en) Pure inertial vehicle navigation method based on MEMS inertial measurement unit
CN108426576B (en) Aircraft path planning method and system based on identification point visual navigation and SINS
CN108873038A (en) Autonomous parking localization method and positioning system
CN111426320B (en) Vehicle autonomous navigation method based on image matching/inertial navigation/milemeter
CN108387236B (en) Polarized light SLAM method based on extended Kalman filtering
CN108562289A (en) Quadrotor laser radar air navigation aid in continuous polygon geometry environment
EP2856273A2 (en) Pose estimation
CN112835085B (en) Method and device for determining vehicle position
Yun et al. IMU/Vision/Lidar integrated navigation system in GNSS denied environments
CN110849360B (en) Distributed relative navigation method for multi-machine collaborative formation flight
CN108592911A (en) A kind of quadrotor kinetic model/airborne sensor Combinated navigation method
CN106352897B (en) It is a kind of based on the silicon MEMS gyro estimation error of monocular vision sensor and bearing calibration
CN106017460B (en) A kind of underwater hiding-machine navigation locating method of terrain aided inertial navigation tight integration
CN111189442A (en) Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF
CN107063248A (en) Kinetic model based on rotor rotating speed aids in the air navigation aid of inertial navigation
CN111337056B (en) Optimization-based LiDAR motion compensation position and attitude system alignment method

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