KR101941009B1 - Attitude and heading reference system and unmaned vehicle including the attitude and heading refernce system - Google Patents

Abstract

An unattended mobile device is disclosed. An unmanned mobile device according to an embodiment of the present invention includes an acceleration sensor for measuring an angular velocity and an acceleration of the unmanned mobile device, an acceleration sensor for measuring a ground magnetic flux density of the unmanned mobile device, And a controller for estimating the attitude of the unmanned mobile device based on the measured values obtained from the acceleration sensor and the geomagnetic sensor.

Description

BACKGROUND OF THE INVENTION 1. Field of the Invention [0001] The present invention relates to an attitude estimation and attitude estimation system,

The present invention relates to an attitude estimation system mounted on an unmanned mobile device. And more particularly to an attitude estimation system robust to magnetic field change and linear acceleration.

In recent years, unmanned mobile devices such as unmanned vehicles, unmanned aerial vehicles, and unmanned robots have been actively commercializing high-precision micro-sensors based on MEMS (Micro-Electromechanical System) . In order to carry out the mission of the unmanned mobile device, accurate direction angle estimation of the mobile device is essential. In the case of the low-cost Attitude and Heading Reference System (AHRS), which is currently being developed and commercialized, it is possible to estimate the normal direction angle in a limited environment. However, if the measurement direction angle is disturbed due to the influx of an external magnetic field, Since there is no algorithm available, many related companies are trying to secure it.

According to the International Robotics Federation (IFR), the market for mobile robots, which are closely related to low-cost AHRS, is expected to grow rapidly from 2.23 billion won in 2007 to 4.65 billion won in 2012 . The size of the global robot market has grown to $ 13.3 billion by 2012 and by an average of 11% from 2007 to 2012. The World Robotics Federation (IFR) explained the world robot market divided into manufacturing robots and service robots. It is remarkable that the growth rate of service robots is 16% per year, compared to 8% of manufacturing robots. In the case of the unmanned mobile device for such a service, a low-cost AHRS for direction angle estimation is essential for accurate mission in a magnetic field disturbance environment.

The unmanned mobile device industry, a product that can be applied to low-cost AHRS, seems to have entered a period of growth. Unmanned helicopters, one of the products with low-cost AHRS, are in the stage of mass production beyond the basic concept establishment and prototype production stage.

According to an embodiment of the present invention, it is possible to reduce the unit price of the unmanned mobile device by predicting the attitude of the unmanned mobile device and correcting the value of the predicted attitude by using the error covariance about the attitude of the unmanned mobile device Discloses an unmanned mobile device.

An unmanned mobile device according to an embodiment of the present invention includes an acceleration sensor for measuring an angular velocity and an acceleration of the unmanned mobile device, an acceleration sensor for measuring a ground magnetic flux density of the unmanned mobile device, And a controller for estimating the attitude of the unmanned mobile device based on the measured values obtained from the acceleration sensor and the geomagnetic sensor.

The unmanned mobile device according to an embodiment of the present invention predicts the attitude of the unmanned mobile device and corrects the value of the predicted attitude using the error covariance of the attitude of the unmanned mobile device, The posture estimation of the high accuracy can be performed.

1 is a block diagram schematically showing a configuration of an unmanned mobile device according to an embodiment of the present invention.
2 is a flowchart illustrating a self-estimation method of an unmanned mobile device according to an exemplary embodiment of the present invention.

Hereinafter, specific embodiments of the present invention will be described in detail with reference to the drawings. It should be understood, however, that there is no intention to limit the spirit of the present invention to the specific embodiments set forth below, and that those skilled in the art, having the benefit of the teachings of the present invention, Or the like, but it will also be included in the spirit of the present invention.

1 is a block diagram schematically showing a configuration of an unmanned mobile device according to an embodiment of the present invention. The unmanned mobile device may be, for example, a unmanned aerial vehicle, an unmanned agricultural machine, an unmanned rescue robot, an unmanned military robot, an unmanned vehicle, or an unmanned surveillance robot.

Referring to FIG. 1, an unmanned mobile device 100 according to an embodiment of the present invention may include a sensor unit 110 and a control unit 120. The sensor unit 110 may include at least one of a Global Positioning System (GPS) sensor 111, an inertial sensor 112, and a magnetic field sensor 113.

The GPS sensor 111 can determine the position of the unmanned mobile device 100. Specifically, the GPS sensor 111 may receive data for coordinate the position of the UAV 100 from the satellite, and may calculate data to sense the position of the UAV 100 in the coordinate system. The GPS sensor 111 can calculate the current position through a distance from the three GPS satellites. The details of the algorithm are obvious to those skilled in the art, and a detailed description thereof will be omitted here.

The inertial sensor 112 may measure the acceleration of the unmanned mobile device 100. Specifically, the inertial sensor 112 detects the inertial force of the unmanned mobile device 100 and measures how much force the unattended mobile device 100 is receiving. At this time, the measured force can be used to measure the acceleration of the unmanned moving device.

At this time, the inertial sensor 112 may include an acceleration sensor and a gyro sensor. The acceleration sensor is a sensor that measures how much force the object is receiving based on the earth's gravitational acceleration. In other words, the acceleration sensor measures the magnitude of the gravitational acceleration acting on the object when the object is still standing, divided into three vectors in the x, y, and z axes. And the gravitational acceleration when tilted is divided by the x, y, and z axes. The gravitational acceleration can be expressed as a fixed value, and the gravitational acceleration can be expressed as the sum of x, y, z vectors. In other words, the acceleration sensor can measure the acceleration of an object by measuring a vector value obtained by dividing gravity acceleration in a specific posture by x, y, and z axes. The acceleration sensor can be used to determine the degree of inclination of an object or to grasp the vibration.

A gyro sensor is a sensor that measures the angular velocity of an object. The gyro sensor measures the angular velocity of an object by measuring the Coriolis force (bending force) generated perpendicular to the direction of rotation when the object is rotating. When using only the acceleration sensor, it is not possible to measure the angle of rotation (azimuth angle) with respect to the plane perpendicular to the ground surface among the rotation angles with respect to the three axes. Therefore, the gyro sensor provides information about the azimuth angle that can not be measured by the acceleration sensor. In addition, the gyro sensor can provide rotation angle information for all axes.

The earth magnetic sensor 113 is a sensor for measuring the intensity and direction of the earth magnetic field. Specifically, the geomagnetic sensor 113 measures the north magnetic north direction caused by the geomagnetic field and also measures the intensity of the geomagnetic field.

The control unit 120 can estimate the posture and the position of the UAV 100 using the data acquired from the sensor unit 110. [

A detailed estimation method will be described with reference to Fig. In one embodiment, the controller 120 and the sensor unit 110 may each be an independent hardware chip. In another embodiment, the controller 120 and the sensor unit 110 may be one physically integrated hardware chip.

The unmanned mobile device 100 according to an exemplary embodiment of the present invention may include the sensor unit 110 and the control unit 120 as well as the additional configuration. For example, the unattended mobile device 100 may include a moving means for moving the unattended mobile device under the control of the control unit 120. In addition, the unmanned mobile device 100 may include additional means necessary for a particular application.

2 is a flowchart illustrating a self-estimation method of an unmanned mobile device according to an exemplary embodiment of the present invention. 2a and 2b of the accompanying drawings are collectively referred to as Fig. The flow chart shown in Fig. 2 may be a flow chart that repeatedly loops, rather than ending immediately with a single execution. In other words, the unmanned mobile device 100 can continuously estimate the attitude of the unmanned mobile device 100 by repeating the flow chart of FIG. 2 according to a predetermined time interval.

The control unit 120 of the unmanned mobile device determines whether initialization of the posture estimation system is necessary (S1001). Initialization of the attitude estimation system may or may not be necessary, and in all cases initializing the system may not be efficient. Accordingly, the control unit 120 can determine whether or not the initialization of the system is required when starting the algorithm of the attitude estimation system.

If initialization of the system is required in one embodiment, the controller 120 initializes the algorithm-related variables and attitude information (S1003). At this time, the variable initialized by the controller 120 may be initial posture information, initial error covariance, error covariance of the system model, measurement error covariance of the acceleration sensor, measurement error covariance of the earth sensor, and previous linear acceleration.

For example, the initial posture information may be the reference posture information of the unmanned mobile device 100 preset in the system. In another example, the initial posture information may be posture information of the unmanned mobile device acquired in the immediately preceding loop.

In addition, the error covariance initial value, the error covariance of the system model, the measurement error covariance of the acceleration sensor, and the measurement error covariance value of the earth magnetic sensor may be specific constants. In other words, the above-described error covariance values may be a constant value specified according to the posture estimation system.

In one embodiment, the linear linear acceleration may be a linear acceleration value obtained in the immediately preceding loop. In yet another embodiment, the linear linear acceleration may be zero.

In another embodiment, when the control unit 120 determines that initialization of the posture estimation system is not necessary, the control unit 120 updates the posture estimation value in the immediately preceding loop and the error covariance value in the immediately preceding loop (S1005). In other words, the control unit 120 uses the posture estimation value and the error covariance value obtained in the immediately preceding loop as the initial value of the system.

When the system initialization is completed, the controller 120 calculates a timer initialization period and an information update period (S1007). At this time, the timer initialization is to know the unit time of the loop once.

The control unit 120 predicts the attitude of the unmanned mobile device through the angular velocity integration (S1009). Specifically, the control unit 120 measures the angular velocity of the unmanned mobile device through the inertial sensor 112. Then, the control unit 120 integrates the measured angular velocity to predict the attitude of the unmanned mobile device. The predicted value can be expressed as (roll, pitch, yaw). roll, pitch, yaw are the Euler angles for each axis. Specifically, roll is the rotation angle with respect to the x-axis, pitch is the rotation angle with respect to the y-axis, and yaw is the rotation angle with respect to the z-axis.

The control unit 120 performs first-order error covariance update on the predicted attitude value based on the angular velocity value (S1011). At this time, the first error covariance update can be performed using a Kalman filter. The concrete equation for updating the error covariance is shown in Equation 1 below.

Here, P ₁ is a first-order updated error covariance value. And F is a predictive model value, which may be a value indicating the dynamic relationship between attitude information and angular velocity. F ^T is the transpose of the prediction model. And P _prev is the error covariance value obtained in the previous loop.

When updating of the first error covariance is completed, the controller 120 predicts the linear acceleration of the unmanned mobile device through Equation (2) (S1013). Specifically, the control unit 120 can predict the linear acceleration of the unmanned mobile device based on the acceleration value acquired from the sensor unit 110.

Where A _linear is the predicted linear acceleration. C _a is the model constant value, between 0 and 1. N _a is _a constant value related to the error of the model and is greater than zero. A _{linear , prev} is the linear acceleration value obtained in the immediately preceding loop.

The controller 120 predicts the gravitational acceleration of the unmanned mobile device through Equation (3). In other words, the controller 120 predicts how much the unmanned mobile device is tilted with respect to gravity through Equation (3). As a result, the controller 120 can obtain the predicted roll and pitch through Equation (3).

Here, A _g is gravitational acceleration. And C _n ^b is a direction cosine matrix. A _sensor is the acceleration value measured by the acceleration sensor. A _linear is the predicted linear acceleration value according to equation (2).

The control unit 120 determines whether the measured acceleration is smaller than a threshold value (S1017). If the measured acceleration is too fast, the value is not reliable. Specifically, if the linear acceleration value is large, the sum vector with the gravitational acceleration can be measured horizontally. Since the linear acceleration value at this time serves as noise to estimate the posture, it can hinder accurate posture estimation, and the controller 120 can discard the acceleration value larger than the threshold value. For example, the threshold value can be set to 10.5 by adding the gravitational acceleration and the linear acceleration.

In one embodiment, if the measured acceleration is greater than the threshold value, the controller 120 determines that the measured acceleration value is meaningless and goes directly to step S1025.

In one embodiment, when the measured acceleration is smaller than the threshold value, the control unit calculates the first weight through Equation 4 (S1019).

Here, K _accel is the first weight. P ₁ is an error covariance value that is first updated in step S 1011. H _accel is a measurement model that shows the dynamic relationship between attitude information and gravitational acceleration. H _accel ^T is the transpose of the measurement model. R _accel is the measurement error covariance of the acceleration sensor.

The controller 120 corrects roll and pitch using the estimated gravity acceleration value in step S1015 and the first weight value calculated in step S1019 (S1021). Specifically, the controller 120 corrects roll and pitch using Equation (5).

Here, X is the attitude information of the unmanned mobile device. In other words, X is an attitude information value obtained by correcting the predicted attitude information value in step S1009 using the gravitational acceleration and the weight. X _prev is the attitude information obtained in the previous loop. K _accel is the first weight value. A _sensor is the acceleration of the unmanned mobile device measured by the acceleration sensor. A _g is the gravitational acceleration. A _linear is the predicted linear acceleration in step S1013.

The controller 120 performs the second-order error covariance update using the weight values (S1023). Specifically, the controller 120 performs the second-order error covariance update through Equation (6).

P ₂ is the second-order update error covariance value. The control unit 120 can update the error covariance value to more accurately estimate the attitude of the unmanned mobile device. K _accel is the first weight. P ₁ is the first-order update error covariance value. H _accel is a measurement model that shows the dynamic relationship between attitude information and gravitational acceleration.

The controller 120 calculates the earth magnetic flux density (S1025). Specifically, the control unit 120 calculates the magnetic field flux density based on the magnetic flux density value acquired from the geomagnetism sensor 113.

)와 기준 지표면과 지구 자기장이 이루는 각도(기준 경사각,

)간의 차가 경사각 임계치(y₁)보다 작은 값인지 여부를 판단한다(S1027). 구체적으로 제어부(120)는 측정 경사각이 일정 값 이상인 경우 노이즈로 판단하여 해당 값을 버린다. 일 실시 예에서 측정 경사각과 기준 경사각간의 차가 경사각 임계치를 초과하는 경우 제어부(120)는 yaw를 보정하지 않고 바로 단계 S1043으로 넘어간다.The control unit 120 determines the angle (measured inclination angle,

), The angle between the reference surface and the earth's magnetic field (reference inclination angle,

) Is smaller than the inclination angle threshold y ₁ (S1027). Specifically, when the measured inclination angle is equal to or greater than a predetermined value, the control unit 120 determines that the noise is a noise and discards the corresponding value. In one embodiment, when the difference between the measured inclination angle and the reference inclination angle exceeds the inclination angle threshold, the controller 120 proceeds directly to step S1043 without correcting yaw.

The controller 120 determines whether the ratio between the magnetic flux density B and the reference magnetic flux density B ₀ is smaller than the threshold value y ₂ of the measured magnetic flux density at operation S1029. Specifically, when the value obtained by dividing the intensity of the measured magnetic flux density by the reference magnetic flux density intensity is equal to or greater than a predetermined value, the controller 120 determines that the noise is a noise and discards the corresponding value. In one embodiment, if the ratio between the measured magnetic flux density and the reference magnetic flux density exceeds the magnetic flux density threshold, the control section 120 proceeds directly to step S1043 without correcting yaw.

)과 추정값 갱신 주기 동안 각속도 센서로 측정한 방향각의 변화량(

)간의 차가 추정값 갱신 주기 동안의 지자계 센서 및 각속도 센서로 측정한 방향각의 변화량 임계치보다 작은 값인지 여부를 판단한다(S1031). 구체적으로 제어부(120)는 일 실시 예에서, 추정값 갱신 주기 동안 지자계 센서로 측정한 방향각의 변화량(

)과 추정값 갱신 주기 동안 각속도 센서로 측정한 방향각의 변화량(

)간의 차가 추정값 갱신 주기 동안의 지자계 센서 및 각속도 센서로 측정한 방향각의 변화량 임계치를 초과하는 경우, 제어부(120)는 yaw를 보정하지 않고 바로 단계 S1043으로 넘어간다.The control unit 120 calculates the change amount of the direction angle measured by the geomagnetic sensor during the estimated value updating period

) And the amount of change in the direction angle measured by the angular velocity sensor during the estimation period updating period

) Is smaller than a threshold value of the direction angle variation measured by the geomagnetic sensor and the angular velocity sensor during the estimated value updating period (S1031). Specifically, in one embodiment, the control unit 120 calculates the amount of change in the direction angle measured by the geomagnetic sensor during the estimated value updating period

) And the amount of change in the direction angle measured by the angular velocity sensor during the estimation period updating period

Is greater than the change amount threshold of the direction angle measured by the geomagnetic sensor and the angular velocity sensor during the estimated value updating period, the controller 120 proceeds directly to step S1043 without correcting the yaw.

)을 계산한다(S1033). 여기에서 방위각이란, 지표면에 수직인 면에 대해 회전하는 각이다. 방위각은 yaw값을 보정하는데 이용될 수 있다.In one embodiment, the steps S1027, S1029, and when the respective values of S1031 are smaller than the respective threshold values (y _1, y _2, y _3), the control part 120 prophet using the values obtained from the magnetic sensor azimuth (

(S1033). Here, the azimuth angle is an angle that rotates with respect to a plane perpendicular to the ground surface. The azimuth angle can be used to calibrate the yaw value.

)과 비교 기준이 되는 방위각(

)간의 차가 지자계로 측정한 방위각의 신뢰 임계치(y₄)보다 작은지 여부를 판단한다(S1035). 일 실시 예에서, 현재 지자계 센서로 측정한 방위각(

)과 비교 기준이 되는 방위각(

)간의 차가 지자계로 측정한 방위각의 신뢰 임계치(y₄)를 초과하는 경우, 제어부(120)는 yaw를 보정하지 않고 바로 단계 S1043으로 넘어간다.The control unit 120 calculates the azimuth angle

) And the azimuth (

) Is smaller than the confidence threshold (y ₄ ) of the azimuth measured by the geomagnetism system (S1035). In one embodiment, the azimuth angle measured with the current geomagnetic sensor

) And the azimuth (

) If the difference exceeds the prophet to step trust threshold value of the measured azimuth angle (y ₄₎ between the controller 120 proceeds to directly to step S1043 without correcting the yaw.

)과 비교 기준이 되는 방위각(

)간의 차가 지자계로 측정한 방위각의 신뢰 임계치(y₄)보다 작은 경우, 제어부(120)는 제2 가중치를 계산한다. 제어부는 수학식 7을 통해 제2 가중치를 계산할 수 있다.In one embodiment, the azimuth angle measured with the current geomagnetic sensor

) And the azimuth (

) Is smaller than the confidence threshold y ₄ of the azimuth measured by the geomagnetism system, the controller 120 calculates the second weight. The control unit may calculate the second weight through Equation (7).

K _mag is the second weight. P ₂ is the second update error covariance. H _mag is a measurement model that shows the dynamic relationship between attitude information and geomagnetic sensors. H _mag ^T is the transpose of the measurement model. R _mag is the measurement error covariance of the geomagnetic sensor.

The controller 120 corrects the yaw using the second weight and the azimuth (S1039). Specifically, the controller 120 corrects yaw through Equation (8).

는 자세 정보이다. 특히 yaw를 나타낸다.

는 예측된 자세 정보를 나타낸다. 구체적으로

는 단계 S1009에서 예측된 yaw를 나타낸다. K_mag는 제2 가중치를 나타낸다.

는 지자계 센서를 이용하여 계산된 방위각이다.

Is attitude information. Especially yaw.

Represents predicted attitude information. Specifically

Represents yaw predicted in step S1009. K _mag represents a second weight.

Is an azimuth calculated using a geomagnetic sensor.

The controller 120 updates the posture estimation value and the error covariance value of the posture estimation system using the corrected posture estimation value and the error value (S1043). Specifically, the controller 120 may update the final error covariance value through Equation (9).

Where P is the last updated error covariance value. P ₂ is the second update error covariance. K _mag is the second weight. H _mag is a measurement model that shows the dynamic relationship between attitude information and gravitational acceleration. The final error covariance can be used as an initial value in the next loop.

Through the above algorithm, the unmanned mobile device 100 according to an embodiment of the present invention more accurately estimates the attitude (roll, pitch, yaw) of the unmanned mobile device 100 even in the case of using a sensor module with a relatively low accuracy can do. As a result, the algorithm according to an embodiment of the present invention can estimate the posture of the UAV 100 relatively accurately without using an expensive sensor, thereby reducing the production cost.

Up to now, an illustrative embodiment of an unmanned mobile device has been described and shown in the accompanying drawings to assist in understanding the present invention. It should be understood, however, that such embodiments are for the purpose of illustrating the invention only, and not for limitation. And it is to be understood that the invention is not limited to the details shown and described. Since various variations can occur to those of ordinary skill in the art.

Claims (7)

An unmanned mobile device comprising an attitude estimation system,
An inertial sensor for measuring an angular velocity and an acceleration of the unmanned mobile device;
A geomagnetic sensor for measuring the magnetic flux density of the unmanned moving device; And
And a control unit for estimating an attitude of the unmanned mobile device based on measured values obtained from the acceleration sensor and the geomagnetic sensor,
The attitude of the unmanned mobile device is determined by a first value, which is the rotation angle of the unmanned mobile device with respect to the x-axis, a second value which is the rotation angle of the unmanned mobile device with respect to the y- And a third value that is the rotation angle of the unmanned mobile device with respect to the z-axis,
Wherein the controller estimates a first value and a second value using a value obtained from the acceleration sensor, estimates a third value using a value obtained from the geomagnetic sensor,
Wherein the controller updates the first error covariance value based on an initial error covariance value for the estimated posture of the unmanned mobile device, obtains a first weight value based on the updated first error covariance value, Updating the second error covariance based on the covariance, obtaining a second weight value based on the second error covariance, correcting the first value and the second value based on the first weight value, Correcting the third value based on the second weight value,
The control unit
When the first comparison value, which is a difference between the measured inclination angle acquired through the inertial sensor and the reference inclination angle, is equal to or greater than the first threshold value,
If the second comparison value, which is a ratio between the intensity of the magnetic flux density acquired through the geomagnetism sensor and the reference magnetic flux density intensity, is equal to or greater than the second threshold value,
When the third comparison value, which is a difference between the direction angle change amount measured by the geomagnetic sensor during the update period and the direction angle change amount measured by the inertia sensor during the update period, is equal to or more than a third threshold value,
Calculating the azimuth using the value obtained from the geodetic sensor when the first comparison value is less than the first threshold value, the second comparison value is less than the second threshold value, and the third comparison value is less than the third comparison value If the fourth comparison value, which is a difference between the calculated azimuth and the reference azimuth, is equal to or greater than a fourth threshold value, the third value is not corrected, and if the fourth comparison value is less than the fourth threshold value, 2 weight value and the azimuth angle to correct the third value
Unmanned mobile device. delete delete delete delete delete The method according to claim 1,
The control unit estimates the attitude of the unmanned mobile device using the acceleration value only when the acceleration value of the unmanned mobile device acquired through the inertial sensor is smaller than a specific threshold value
Unmanned mobile device.

Images