KR101908534B1 - Apparatus and method for determining position and attitude of a vehicle - Google Patents

Abstract

)를 이용하여 이동체의 위치 및 자세를 추정하는 위치 및 자세 추정부와, 상기 GPS 위치(

)와 상기 추정한 위치(

)의 차이가 한계치를 초과하면 GPS 이상 상태를 검출하는 이상 검출부와, 상기 이상 검출부에 의해 GPS 이상이 검출되면 상기 자세 측정부의 자세(

) 및 상기 비컨 측정부의 거리, 방향각, 고도각을 이용하여 상기 추정한 위치 및 자세(

)를 보정하여 최종 위치 및 자세(

)를 결정하는 위치 및 자세 보정부를 포함한다. The present invention detects an anomalous state of a GPS signal without intervention of an external system and corrects the position and posture of the unmanned moving body by using the fusion sensor information excluding the GPS position information when detecting an abnormal state, And an apparatus and method for determining the position and attitude of a moving object. To this end, the apparatus for determining the position and orientation of a moving object according to the present invention comprises: a GPS position measuring unit for measuring a GPS position of a moving object by receiving a signal from a GPS satellite; and a GPS position measuring unit for receiving a sound signal from the plurality of beacons, A beacon measuring unit for measuring a distance, a direction angle and an altitude angle, an attitude measuring unit for measuring the attitude of the moving object by using acceleration, an angular velocity and a geomagnetic field of the moving object, a velocity measuring unit for measuring the linear velocity of the moving object, The linear velocity measured by the velocity measuring unit and the attitude output from the position and attitude correcting unit

A position and attitude estimating unit for estimating a position and an attitude of the moving object using the GPS position

) And the estimated position (

) Of the position measuring unit is greater than a threshold value, and an abnormality detecting unit that detects a GPS anomaly state when the difference between the position

) And a distance, a direction angle, and an altitude angle of the beacon measurement unit,

) To correct the final position and posture (

And a position and posture correcting section for determining the posture correcting section.

Description

[0001] APPARATUS AND METHOD FOR DETERMINING POSITION AND POSITION OF MOBILE VEHICLE [0002]

The present invention relates to an apparatus and method for determining a position and an attitude of a moving object, and more particularly, to an apparatus and method for determining the position and orientation of a moving object by detecting an anomalous state of a GPS signal without intervention of an external system, The present invention relates to an apparatus and method for determining a position and an attitude of a moving object that can accurately grasp a position and an attitude of an unmanned moving object by correcting a position and an attitude.

Unmanned Surface Vehicle (UNAV) is one of the main components that can be used for surveillance and reconnaissance, as well as for connecting unmanned submersible vehicles, unmanned aerial vehicles, and unmanned ground vehicles.

Unmanned awards definition navigation techniques are generally based on GPS / INS composite navigation. GPS can provide unusual information due to errors in the path from the satellite to the receiver, as well as the problem of the receiver or the effects of various jammers on the ground.

The case where the GPS receiver does not receive the radio wave of the satellite can be clearly recognized, but if the wrong signal is mistaken as the normal signal, the unmanned receiver can not provide the correct position.

GPS disturbance can be detected using spatial diversity or using a GPS receiver network that detects areas with a lower signal-to-noise ratio than a normal signal, but this method can be applied to systems that cover a wide range of areas, including networks It requires a great deal of cost and effort to build the system.

Also, since the unmanned system needs to receive GPS disturbance information from an external system, there is a problem that it is externally dependent.

Korean Patent No. 10-1540408

SUMMARY OF THE INVENTION It is an object of the present invention to provide an apparatus and method for detecting a GPS anomaly state by using only an unmanned water source without using an external system.

Another object of the present invention is to provide an apparatus and method for accurately measuring the position and attitude of an unmanned aerial vehicle even when GPS disturbance occurs.

)를 이용하여 이동체의 위치 및 자세를 추정하는 위치 및 자세 추정부와, 상기 GPS 위치(

)와 상기 추정한 위치(

)의 차이가 한계치를 초과하면 GPS 이상 상태를 검출하는 이상 검출부와, 상기 이상 검출부에 의해 GPS 이상이 검출되면 상기 자세 측정부의 자세(

) 및 상기 비컨 측정부의 거리, 방향각, 고도각을 이용하여 상기 추정한 위치 및 자세(

)를 보정하여 최종 위치 및 자세(

)를 결정하는 위치 및 자세 보정부를 포함한다. To this end, the apparatus for determining the position and orientation of a moving object according to the present invention comprises: a GPS position measuring unit for measuring a GPS position of a moving object by receiving a signal from a GPS satellite; and a GPS position measuring unit for receiving a sound signal from the plurality of beacons, A beacon measuring unit for measuring a distance, a direction angle and an altitude angle, an attitude measuring unit for measuring the attitude of the moving object by using acceleration, an angular velocity and a geomagnetic field of the moving object, a velocity measuring unit for measuring the linear velocity of the moving object, The linear velocity measured by the velocity measuring unit and the attitude output from the position and attitude correcting unit

A position and attitude estimating unit for estimating a position and an attitude of the moving object using the GPS position

) And the estimated position (

) Of the position measuring unit is greater than a threshold value, and an abnormality detecting unit that detects a GPS anomaly state when the difference between the position

) And a distance, a direction angle, and an altitude angle of the beacon measurement unit,

) To correct the final position and posture (

And a position and posture correcting section for determining the posture correcting section.

)를 이용하여 이동체의 위치 및 자세를 추정하는 위치 및 자세 추정부와, 상기 GPS 위치(

)와 상기 추정한 위치(

)의 차이가 한계치를 초과하면 GPS 이상 상태를 검출하는 이상 검출부와, 상기 이상 검출부에 의해 GPS 이상이 검출되면 상기 자세 측정부의 자세(

) 및 상기 신호 처리부의 거리, 방향각, 고도각을 이용하여 상기 추정한 위치 및 자세(

)를 보정하여 최종 위치 및 자세(

)를 결정하는 위치 및 자세 보정부를 포함한다. According to another aspect of the present invention, there is provided an apparatus for determining the position and orientation of a moving object, comprising: a GPS position measuring unit for measuring a GPS position of a moving object by receiving a signal from a GPS satellite; A signal processing section for receiving a radio wave or a signal coming back from the mobile body and measuring a distance from the mobile body to each feature point, a direction angle and an altitude angle, and a posture measuring section for measuring the posture of the mobile body using the acceleration, A velocity measuring unit for measuring a linear velocity of the moving object, a linear velocity measured by the velocity measuring unit, and a posture

A position and attitude estimating unit for estimating a position and an attitude of the moving object using the GPS position

) And the estimated position (

) Of the position measuring unit is greater than a threshold value, and an abnormality detecting unit that detects a GPS anomaly state when the difference between the position

) And the estimated position and posture (&thetas;) using the distance, the direction angle, and the altitude angle of the signal processing unit

) To correct the final position and posture (

And a position and posture correcting section for determining the posture correcting section.

) 및 k번째 시각에서 측정된 이동체로부터 비컨 또는 특징 점까지의 거리, 방향각, 고도각을 이용하여 상기 추정한 이동체의 위치 및 자세를 보정하는 단계를 포함한다. According to another aspect of the present invention, there is provided a method of determining a position and an attitude of a moving object, the method comprising: estimating a position and a posture of the moving object at a k-th time using a moving model of the moving object; Determining whether the difference of the position exceeds a threshold value, and determining whether the difference exceeds a threshold value,

And correcting the estimated position and posture of the moving body using the distance, the direction angle, and the altitude angle from the moving object to the beacon or feature point measured at the k-th time.

A system for determining a position and an attitude of a moving object according to the present invention includes a plurality of beacons for transmitting acoustic signals and a distance, a direction angle and an altitude angle from the moving object to each beacon by receiving an acoustic signal from the beacon, The position and attitude of the moving object are estimated using the linear and rotational movement models of the moving object, and the position and attitude estimated at the time of GPS abnormality are measured using the measured attitude, distance, direction angle, and elevation angle And corrects the moving object.

According to another aspect of the present invention, there is provided a system for determining a position and an attitude of a moving object, comprising: a plurality of characteristic points for reflecting a radio wave or a signal; and a radio wave or a signal for transmitting a radio wave or a signal to the plurality of characteristic points, And estimates the position and attitude of the moving object by measuring the linear velocity and posture of the moving object and measures the position and attitude of the moving object using the linear and rotational movement model of the moving object, And a moving object that corrects the estimated position and attitude using the measured attitude, distance, direction angle, and elevation angle.

INDUSTRIAL APPLICABILITY As described above, the present invention solves the problem that the unmanned mobile unit can detect the GPS disturbance alone without using the external system, thereby reducing the cost and effort required for constructing the external system and relying on the external system .

In addition, the present invention uses position information, distance, direction angle and elevation angle, which are measurement information excluding GPS position, when the GPS disturbance is used, and estimates the position and posture of the moving object estimated in the previous step The position and the posture of the mobile body can be accurately grasped.

BRIEF DESCRIPTION OF THE DRAWINGS FIG. 1 is a schematic diagram of a system for determining a position and an attitude of a moving object according to the present invention; FIG.
FIG. 2 is an internal configuration view of a position and orientation determining apparatus of a moving object according to the present invention. FIG.
3 is a flowchart illustrating a method of determining a position and an attitude of a moving object according to the present invention.
4 is a diagram illustrating a notation used in a method for determining a position and an attitude of a moving object according to the present invention.
5 is a diagram showing a pseudo code of a method for determining a position and an attitude of a moving object according to the present invention.
6 is a graph showing sensor measurement characteristics and conditions used in a simulation for verifying the effectiveness of the present invention.
FIG. 7 is a graph showing a result of simulation of a position estimation trajectory and a GPS anomaly according to a method for determining a position and an attitude of a moving object according to the present invention.
8 is a diagram showing a position estimation error in a simulation.
FIG. 9 is a graph showing a result of a GPS signal detection test and a locus estimation trajectory by a method for determining a position and an attitude of a moving object according to the present invention.
10 is a diagram showing a position estimation error in an experiment;

Hereinafter, embodiments of the present invention will be described in detail with reference to the accompanying drawings. The configuration of the present invention and the operation and effect thereof will be clearly understood through the following detailed description.

Before describing the present invention in detail, the same components are denoted by the same reference symbols as possible even if they are displayed on different drawings. In the case where it is judged that the gist of the present invention may be blurred to a known configuration, do.

1 shows a schematic configuration of a position and attitude determination system for a moving object according to the present invention.

Referring to FIG. 1, a system according to the present invention includes a manless vehicle 10, a plurality of beacons 20, a GPS satellite 30, and the like.

The unmanned moving body 10 is an unmanned floating body as a moving object without a person being boarded. Hereinafter, for convenience of explanation, the unmanned moving body 10 is simply referred to as a moving body 10.

The moving body 10 estimates the position and posture while moving, corrects the estimated position and posture, and determines the final position and posture.

For this purpose, in the first step, the moving object 10 estimates the position and attitude of the moving object at the k-th time using the velocity of the moving object and the attitude at the (k-1) -th viewpoint. In the second step, the moving object 10 corrects the estimated position and attitude using the GPS position, the attitude, the distance to the beacon at the k-th time, the directional angle to the beacon, and the altitude angle to the beacon.

The moving object 10 measures the moving speed by a DVL (Doppler Velocity Log), measures the attitude by an Attitude and Heading Reference System (AHRS), receives an acoustic signal from a plurality of beacons 20, And calculates the position by receiving GPS signals from the GPS satellites 30 and calculating the position.

The moving body 10 according to the present invention calculates the distance to the beacon 20, the direction angle and the altitude angle using the beacon 20, but a plurality of characteristic points are installed instead of the beacon 20, radar, lidar, and electronic optical infrared (EO / IR), etc., can be used to calculate the distance, direction angle and elevation angle to the feature point. These features can be installed specifically for the present invention, but natural objects or artificial structures that are distinct from the surrounding environment are also possible.

2 schematically shows an internal structure of a position and orientation determining apparatus for a moving object according to the present invention.

The apparatus for determining the position and orientation of a moving object according to the present invention includes a GPS position measurement unit 101, a beacon measurement unit 103, an orientation measurement unit 105, a velocity measurement unit 107, a position and orientation estimation unit 109, An anomaly detection unit 111, a position and orientation correction unit 113, and the like.

All components of the position and orientation determining device of the moving object may be present inside the moving object 10 or some of them may exist in the external device (not shown).

The GPS position measurement unit 101 measures the position from GPS signals, i.e., latitude, longitude, and altitude information from the GPS satellites 30. The information output from the GPS position measuring unit 101 is referred to as a GPS position.

The beacon measuring unit 103 receives acoustic signals from a plurality of beacons 20, and measures distances, direction angles, and elevation angles from the moving object 10 to each beacon. The information output from the beacon measuring unit 103 is called a distance, a direction angle, and an altitude angle.

As another embodiment, a plurality of characteristic points (not shown) may be provided in place of the beacon, or a peripheral natural or artificial structure may be used as a plurality of characteristic points, and an optical camera, a radar, a lidar, / IR), etc., a signal processing unit (not shown) may replace the beacon measuring unit 103 in calculating the distance to the feature point, the direction angle, and the altitude angle.

The signal processing unit radiates a radio wave or an infrared signal, receives a radio wave or a signal reflected from the characteristic point, and calculates the distance, direction, and elevation angle from the moving object 10 to each characteristic point.

The attitude measuring unit 105 measures the attitude of the moving object using the acceleration, the angular velocity, and the earth's magnetic field of the moving object. An acceleration vector, an angular velocity vector, and a magnetic field vector of a moving object can be measured using a MEMS-AHRS (Micro Electro Mechanical System - Attitude and Heading Reference System). The information output from the posture measurement unit 105 is referred to as a posture.

  The velocity measuring unit 107 measures the linear velocity of the moving object. The linear velocity of the moving object can be measured using DVL (Doppler Velocity Log). The information output from the speed measuring unit 107 is referred to as speed.

The GPS position of the GPS position measuring unit 101, the distance, the direction angle, the altitude angle of the beacon measuring unit 103, and the attitude of the attitude measuring unit 105 are collectively referred to as measurement information.

 The position and attitude estimating unit 109 estimates the position and attitude of the moving body using the attitude outputted from the position and attitude correcting unit 113 and the velocity output from the velocity measuring unit 107. The information output from the position and attitude estimating unit 109 is the estimated position and attitude.

The abnormality detecting unit 111 calculates the difference between the estimated position from the position and attitude estimating unit 109 and the GPS position from the GPS position measuring unit 101 and judges the GPS abnormal state if the difference exceeds the limit.

The position and orientation correcting unit 113 is configured to determine the attitude from the attitude measuring unit 105 when the GPS is steady, the GPS position from the GPS position measuring unit 101, the distance from the beacon measuring unit 103, The estimated position and posture are corrected using the altitude angle.

On the other hand, the position and orientation correcting unit 113 excludes the GPS position from the GPS position measuring unit 101 when the GPS is in an abnormal state and determines the posture from the posture measuring unit 105, the distance from the beacon measuring unit 103 , The direction angle, and the altitude angle are used to correct the estimated position and posture.

The information output from the position and orientation correcting unit 113 is finally determined position and posture.

FIG. 3 illustrates a process of determining the position and posture of a moving object according to the present invention.

3 may be directly performed by the mobile device 10 or may be performed by an external device (not shown). In FIG. 3, the mobile device 10 performs each step.

The notation required for explanation of each step is shown in Fig.

In the notation of Fig. 4, the superscript s of each vector indicates that the corresponding vector is expressed in the canonical (mobile) coordinate system. Without a superscript s, it indicates that the vector is represented in the reference coordinate system. When a vector subscript k or k-1 is added, it means the value of the corresponding vector at the k-th or k-1-th time.

First, the moving object 10 estimates the position and attitude of the moving object at the k-th time (S10), and then calculates the estimated uncertainty of the position and attitude of the moving object at the k-th time (S12).

Position and attitude estimation process of moving object

The moving body 10 according to the present invention simultaneously estimates the position and the posture. The position and attitude of the moving object are expressed by Equation (1).

The characteristics related to the linear movement of the moving object are modeled as shown in Equation (2).

에 회전행렬

을 곱하여 기준 좌표계에서의 속도

을 구한다.The velocity of the moving object measured in the moving object coordinate system

Rotation matrix

The velocity in the reference coordinate system

.

은 이동체 좌표계에서 나타낸 속도를 기준 좌표계에서의 속도로 변환하는 변환행렬이다. Rotation matrix

Is a transformation matrix for converting the velocity indicated by the moving body coordinate system into a velocity in the reference coordinate system.

은 이동체의 자세

에 관한 함수로서 수학식 3과 같이 구할 수 있다. Rotation matrix

The posture of the moving object

(3) < / RTI >

In Equation (3), C represents cos and S represents sin.

The characteristic relating to the rotational movement of the moving object is modeled as shown in Equation (4).

In the equations (2) and (4), the model relating to the straight line and the rotational movement of the moving object is expressed by the movement function f (X)

은 수학식 2의

와 수학식 4의

을 하나의 벡터로 통합한 것이다.In Equation (5)

(2)

And Equation 4

Into a single vector.

은 직선 및 회전 이동 모델의 불확실성을 나타내며, 오차 공분산이 Q 인 가우시안 분포

을 가진 것으로 가정한다.

Represents the uncertainty of the linear and rotational motion model, and the Gaussian distribution with error covariance Q

.

은 수학식 6과 같이 구해진다.Needed in the estimation process

Is obtained as shown in Equation (6).

을 각각

로 편미분하여 구한다. 여기서

의 1, 2, 3열은 영벡터가 되고, 4, 5, 6열은 수학식 6-1과 같이 구해진다.Equation (6) can be expressed by Equation (5)

Respectively

. here

1, 2, and 3 are zero vectors, and columns 4, 5, and 6 are obtained as shown in Equation 6-1.

[Equation 6-1]

In Equation (6-1), C, S, and T denote cos, sin, and tan, respectively. Thus, the estimation process is expressed by Equations (7) to (9).

로부터

을 구한다. 즉, k-1번째 시각에서의 위치 및 자세

에 Δt시간 동안 이동한 거리 및 회전 증분을 더하여 구한다. In Equation (7 ) , the position and posture at the ( k-1 )

from

. That is, the position and posture at the (k-1)

Is added to the distance traveled during Δt time and the rotation increment.

In Equation (8), A is a linearized movement model.

로부터 k번째 시각에서 추정된 위치 및 자세

의 오차 정도를 나타내는

을 구한다.In Equation (9), the position and posture error estimated at the (k-1)

The position and attitude estimated from the k < th >

Indicating the degree of error of

.

은 k-1번째 시각에서의 위치 및 자세의 오차 공분산

을 선형화된 이동 모델 A 에 의해

으로 변환하고, 이동 모델의 오차 공분산 Q 를 더하여 구한다. 즉,

은 k-1번째 시각에서의 위치 및 자세의 오차 공분산, 이동 모델, 이동 모델의 오차 공분산을 고려하여 구할 수 있다.

Is the error of the position and posture at the (k-1)

By a linearized moving model A

And adds the error covariance Q of the movement model to obtain the error covariance. In other words,

Can be obtained by considering the error covariance of position and attitude at the (k-1) th time, the movement model, and the error covariance of the movement model.

Next, the mobile 10 determines whether the difference between the GPS position and the estimated position exceeds a threshold value (S14). If the difference is less than the threshold value, that is, if the GPS signal is normal, (S16). If the difference exceeds the threshold value, that is, if the GPS signal is in an abnormal state, the estimated position, attitude and uncertainty are corrected using the measurement information excluding the GPS position (S18).

Calibration process

The measurement information to be used for correcting the estimated position and posture is represented by a measurement vector z as shown in Equation (10).

The measurement model of equation (10 ) is represented by a measurement function h (X) related to the state variable.

은 측정 모델의 불확실성을 나타내며, 오차 공분산이 R 인 가우시안 분포

을 가진 것으로 가정한다. 측정 벡터는 측정 함수 h(X) 에 의해 구해지는 측정값과 불확실성 또는 잡음

이 더해진 값으로 표현된다. h (X) is a function to obtain the measurement vector z from the position and posture.

Indicates the uncertainty of the measurement model, and Gaussian distribution with an error covariance of R

. The measurement vector is the measurement value obtained by the measurement function h (X) , the uncertainty or noise

Is represented by the added value.

을 구한다. 이렇게 구한 위치

가 측정값으로 사용되므로, 수학식 11의 h _x (X) 은 항등함수(identity function)가 된다. Location from latitude, longitude, and altitude information received from GPS

. Location

Is used as a measurement value, h _x (X) in Equation (11 ) becomes an identity function.

을 구한다. 이렇게 구한 자세

가 측정값으로 사용되므로, 수학식 11의 h _α (X) 도 항등함수(identity function)가 된다. From the acceleration a and global magnetic field m measured by the AHRS,

. This attitude

Is used as a measurement value, h _? (X) in _Equation ( 11 ) also becomes an identity function.

은 음향 비컨으로부터 수신한 신호를 이용하여 구한다.

은 이동체의 위치 및 자세와 i번째 비컨의 위치에 관한 함수이다. i번째 비컨의 위치 벡터는 수학식 12와 같이 구해진다.

Is obtained by using the signal received from the acoustic beacon.

Is a function relating to the position and attitude of the moving object and the position of the i-th beacon. The position vector of the i-th beacon is obtained as shown in Equation (12).

From equation (12), the distance r _i from the moving object to the i-th beacon is obtained as shown in equation (13).

을 모아서

이 구해진다. 따라서 수학식 13은 수학식 11의 h _r (X) 을 나타낸다.Equation 13

Collect

Is obtained. Therefore, Expression (13 ) represents h _r (X) in Expression (11 ) .

은 수학식 14와 같이 구해진다. Position of beacon in mobile coordinate system

Is obtained as shown in Equation (14).

을

로 표현하면 방향각

과 고도각

은 수학식 15 및 수학식 16과 같이 구해진다. In Equation 14,

of

Expressed in terms of direction angle

And elevation angle

Can be obtained by the following equations (15) and (16).

을 나타내고, 수학식 16은 수학식 11의

을 구성한다. 이와 같이,

,

,

,

,

가 모여서 측정 함수 h(X) 가 구성된다. Equation (15) is the equation

And Expression 16 represents Expression

. like this,

,

,

,

,

To form a measurement function h (X) .

The measurement function h ( X _k ) is linearized to obtain the measurement matrix H. The H matrix can be obtained as shown in equation (17 ) using the partial derivative of h ( X _k ) .

The calibration process is performed using the measurement matrix H.

가 구해진다. 수학식 20에서, 추정된 위치 및 자세

의 불확실 정도를 나타내는 오차 공분산

가 구해진다. 오차 공분산

의 특정 성분이 크면 추정된 위치 및 자세 중 해당하는 변수의 불확실성이 커서 신뢰도가 낮다는 것을 의미한다. The position and attitude at the k < th >

Is obtained. In Equation 20, the estimated position and posture

Error covariance indicating the degree of uncertainty of

Is obtained. Error covariance

Of the uncertainty of the variable that the specific component is the greater of the estimated position and position it means that the reliability is low cursor.

GPS error detection process

을 사용하여 GPS 신호의 이상을 검출한다. GPS 이상 검출 과정에서 사용하기 위해 추정된 오차 공분산 행렬은 수학식 21과 같이 분리하여 표현한다. Error covariance

To detect an abnormality of the GPS signal. The estimated error covariance matrix for use in the GPS anomaly detection process is expressed as shown in Equation (21).

은 추정된 위치의 불확실 정도를 나타내고,

은 추정된 자세의 불확실 정도를 나타낸다.

와

은 위치 및 자세의 불확실성의 상호 연관 정도를 나타낸다. In Equation 21,

Represents the degree of uncertainty of the estimated position,

Represents the degree of uncertainty of the estimated posture.

Wow

Represents the degree of correlation of uncertainties in position and posture.

을 사용하여 검출한다. Since GPS outputs the position, the GPS anomaly indicates the uncertainty of the position

.

GPS anomaly is detected by using the estimated covariance of position and attitude and the estimated state by fusion of multiple sensors.

If the difference between the GPS position and the estimated position deviates from the limit, it is determined that the GPS is abnormal. The threshold is set using the Mahalanobis distance.

와 추정된 위치

사이의 마할라노비스 거리

은 수학식 22와 같다. The position measured by the GPS at the k-th time

And the estimated location

Mahalán Novés street between

(22) "

가 설정된 한계치

보다 크면 GPS에 이상이 있는 것으로 판단한다. 즉, 수학식 23이 만족되는 경우 GPS 이상으로 판단한다.

Set threshold

It is judged that there is an abnormality in the GPS. That is, when Equation (23) is satisfied, it is determined that GPS is abnormal.

은 GPS의 위치 측정 방식이나 기종에 따라 다른 값으로 실험을 통해 적절히 설정할 수 있다.In Equation 23,

Can be appropriately set by experiment using different values depending on the position measurement method or the type of GPS.

Alternative Navigation in GPS Abnormality

If a GPS error is detected, the position measured by GPS is not used in the calibration process. Therefore, the measurement vector is set as shown in equation (24) instead of equation (10).

In this case, the measurement model is expressed by Equation (25) instead of Equation (11).

Here, the H matrix is obtained as shown in Equation (26) instead of Equation (17).

When the satisfaction of the expression (23) is satisfied and the expression (27) is satisfied even during the execution of the GPS replacement navigation, it is determined that the GPS returns to normal.

As described above, the estimation process and correction process by fusion navigation, the GPS anomaly detection process, and GPS replacement navigation are shown in pseudo code in FIG.

Hereinafter, the effectiveness of the method for determining the position and attitude of the moving object according to the present invention is verified through simulation and experiments.

Verification through simulation

The simulation is easy to artificially generate a GPS anomaly signal and is suitable for demonstrating the effectiveness of the present invention. The simulation conditions used in the present invention are shown in FIG.

All sensor measurements were updated every 0.1 second to generate simulation data. GPS signals that were normally measured initially and abnormally received from 40.3 seconds to 84.8 seconds were used as simulation data.

Fig. 7 shows the augmented actual trajectory, the GPS position measurement trajectory, and the position estimation result according to the present invention. An alternative GPS was used in the A-B section where the GPS position indicated by the green color in the trajectory was abnormally large and the GPS anomaly was detected.

We detected abnormalities more than 0.3 second after the occurrence of GPS anomaly, and normal state after 1 second from the point of normal GPS recovery.

8, when the GPS signal is normal, the position estimation error is 0.85 m in the x direction, 1.26 m in the root means square, 0.84 m in the y direction, and 1.23 m in the square root mean square. On the other hand, when the GPS signal is abnormal, the position estimation error is 8.764m in the x direction, 12.108m in the square root mean square, 3.85m in the y direction and 5.37m square root mean square.

The position estimation error at the time of the GPS abnormality serves as the deviation of the position estimation error at the abnormality detection time. This is because GPS navigation is dependent on AHRS, distance, direction angle and elevation angle, and uncertainty of distance, direction angle, and elevation angle is large and it can not compensate the estimated value.

Verification through experiments

In order to verify the effectiveness of the method for determining the position and attitude of the moving object according to the present invention, an experiment was conducted using a moving vehicle on the ground.

In the present invention, a traveling vehicle is equipped with a GPS receiver and an AHRS, and travels on the road. In the experiment, we did not actually use the actual GPS signal for a certain period of time because the GPS anomaly did not occur. Also, the distance to beacons, direction angles, and elevation angles were not used. In other words, fusion navigation was performed using only inertial sensor information and GPS anomaly was detected.

The experimental vehicle traveled for 170 seconds with the trajectory shown in Fig. 9 (a). GPS errors were set to occur in the A-B section between 80 and 125 seconds. 9 (b) shows the position estimation result and the GPS anomaly detection result.

10 shows an error statistic result of an estimated position between 80 sec and 125 sec. In this experiment, the error of the estimated position is calculated based on the normal GPS position information.

As a result, even if there is no measurement information of the external system, it is possible to temporarily detect GPS abnormality and normal return by the method according to the present invention.

The foregoing description is merely illustrative of the present invention, and various modifications may be made by those skilled in the art without departing from the spirit of the present invention.

Accordingly, the embodiments disclosed in the specification of the present invention are not intended to limit the present invention. The scope of the present invention should be construed according to the following claims, and all the techniques within the scope of equivalents should be construed as being included in the scope of the present invention.

10: Unmanned vehicle 20: Beacon
30: GPS satellite 101: GPS position measuring unit
103: beacon measuring unit 105: attitude measuring unit
107: speed measuring unit 109: position and attitude estimating unit
111: abnormality detecting section 113: position and attitude correcting section

Claims (15)

A GPS position measuring unit for receiving a signal from a GPS satellite and measuring a GPS position of the moving object,
A beacon measuring unit for measuring a distance, a direction angle, and an altitude angle from a moving object to each beacon which is not used for receiving acoustic signals from a plurality of beacons and estimating the position and attitude of the moving object,
An attitude measuring unit for measuring an attitude of the moving object using the acceleration, the angular velocity and the earth's magnetic field of the moving object;
A velocity measuring unit for measuring a linear velocity of the moving object,
A linear velocity measured by the velocity measuring unit and a posture of a previous time output from a position and orientation correcting unit

A position and posture estimating unit for estimating a position and an attitude of the mobile body at the current time using the position and posture estimating unit,
The GPS position (

) And the estimated position (

) Detects a GPS anomaly state when the difference between the GPS position and the GPS position exceeds a threshold value,
When the GPS abnormality is detected by the abnormality detecting unit, the attitude of the attitude measuring unit

) And a distance, a direction angle, and an altitude angle of the beacon measurement unit,

) To correct the final position and posture (

And a position and orientation correcting section for determining the position and orientation of the moving object. The method according to claim 1,
The position and attitude correction unit may calculate the position and attitude of the beacon measuring unit using the GPS position of the GPS position measuring unit, the attitude of the attitude measuring unit, the distance, the direction angle, and the altitude angle of the beacon measuring unit, ) To correct the final position and posture (

) Of the moving object. A GPS position measuring unit for receiving a signal from a GPS satellite and measuring a GPS position of the moving object,
The distance, direction angle, and altitude from the moving object to each feature point, which are not used for estimating the position and attitude of the moving object, by receiving a radio wave or a signal that is reflected and reflected from each characteristic point by transmitting a radio wave or a signal to a plurality of characteristic points A signal processing unit for measuring an angle,
An attitude measuring unit for measuring an attitude of the moving object using the acceleration, the angular velocity and the earth's magnetic field of the moving object;
A velocity measuring unit for measuring a linear velocity of the moving object,
A linear velocity measured by the velocity measuring unit and a posture of a previous time output from a position and orientation correcting unit

A position and posture estimating unit for estimating a position and an attitude of the mobile body at the current time using the position and posture estimating unit,
The GPS position (

) And the estimated position (

) Detects a GPS anomaly state when the difference between the GPS position and the GPS position exceeds a threshold value,
When the GPS abnormality is detected by the abnormality detecting unit, the attitude of the attitude measuring unit

) And the estimated position and posture (&thetas;) using the distance, the direction angle, and the altitude angle of the signal processing unit

) To correct the final position and posture (

And a position and orientation correcting section for determining the position and orientation of the moving object. The method of claim 3,
The position and posture correcting unit corrects the estimated position and posture using the GPS position of the GPS position measuring unit, the posture of the posture measuring unit, the distance, the direction angle, and the altitude angle of the signal processing unit,

) To correct the final position and posture (

) Of the moving object. A method for determining a position and an attitude in a mobile body,
Estimating a position and a posture of a moving object at a k-th time using a moving model of the moving object,
Determining whether a difference between the estimated position at the k-th time and the position by the GPS exceeds a threshold within the moving object,
When the difference exceeds the threshold value, the posture of the moving object measured at the k-th time

And correcting the estimated position and posture of the moving object by using the distance, the direction angle, and the altitude angle from the moving object to the beacon or feature point, which are not used for estimating the position and attitude of the moving object, And determining a position and an attitude of the moving object. 6. The method of claim 5,
The step of correcting the estimated position and posture of the mobile body may include calculating a GPS position at the k-th time, a posture of the mobile body measured at the k-th time

And a position and an attitude of the estimated moving object are corrected using a distance, a direction angle, and an altitude angle from the moving object to the beacon or feature point measured at the k-th time. 6. The method of claim 5,
The step of estimating the position and the posture of the moving object at the k-th time may include estimating the position and posture of the moving object at the linear velocity of the moving object and the posture of the moving object at the (k-1)

) Of the moving object. 6. The method of claim 5,
The step of determining whether the distance exceeds the limit value is characterized by determining a Mahalanobis distance between the estimated position at the k-th time and the position by GPS, and judging whether the Mahalanobis distance exceeds a threshold value A method for determining the position and attitude of a moving object. 6. The method of claim 5,
And calculating an estimated degree of uncertainty of the position and attitude of the moving object at the k-th time point. 10. The method of claim 9,
When the difference exceeds the threshold value, the posture of the moving object measured at the k-th time

And correcting the calculated uncertainty using a distance, a direction angle, and an altitude angle from the moving object measured at the k-th time to the beacon or feature point. 10. The method of claim 9,
If the difference is less than the threshold value, the GPS position at the k-th time point, the posture of the mobile object measured at the k-th time point

And correcting the calculated uncertainty using a distance, a direction angle, and an altitude angle from the moving object measured at the k-th time to the beacon or feature point. A system for determining a position and an attitude of a moving body,
A plurality of beacons for transmitting acoustic signals,
A distance, a direction angle, and an altitude angle from a moving object not used for estimating the position and posture of the moving object to the beacon are measured, the linear velocity and posture of the moving object are measured, And a rotational movement model, and estimates the position and attitude estimated at the time of GPS abnormality by the GPS anomaly detection process performed inside the mobile object by using the measured attitude, distance, direction angle, and altitude angle And determines the position and posture of the moving body including the moving body to be corrected. A system for determining a position and an attitude of a moving body,
A plurality of characteristic points for reflecting a radio wave or a signal,
A distance from the moving object to each feature point which is not used for estimating the position and attitude of the moving object, a direction angle and a direction angle of the moving object, which are not used to transmit a radio wave or a signal to the plurality of characteristic points, The position and attitude of the moving object are estimated by measuring the elevation angle, the linear velocity and posture of the moving object, the linear and rotational movement model of the moving object, the GPS anomaly detection process performed inside the moving object, And determining a position and an attitude of the moving object including the moving object which corrects the position and the attitude using the measured attitude, distance, direction angle, and elevation angle. The method according to claim 12 or 13,
Wherein the mobile body corrects the position and the posture estimated in the GPS steady state by using the GPS position, the measured posture and the distance, the direction angle, and the altitude angle. A recording medium readable and writable by a computer storing a program for executing the method according to any one of claims 5 to 11.

Images