KR20120042394A - System and method for localizationing of autonomous vehicle - Google Patents

Abstract

PURPOSE: A location measurement system and a location measurement method for an autonomous vehicle are provided to accurately correct angular velocity by calculating a weighted value from the instantaneous degree change and two angular velocity values of encoders. CONSTITUTION: A location measurement system for an autonomous vehicle comprises encoders(2), gyroscopes(3), accelerometers(4), an angular velocity compensation unit, a linear velocity compensation unit, and a posture compensation unit. The encoders measure the rotation angle of drive wheels. The gyroscopes measure rotational inertia. The accelerometers measure gravitational acceleration. The angular velocity compensation unit compensates for angular velocity using the angular velocity calculated from the encoders and gyroscopes. The linear velocity compensation unit compensates for linear velocity using the compensated angular velocity. The posture compensation unit compensates for posture using the calculated angular and linear velocity.

Description

Positioning system and method for driving device {System and Method for localizationing of Autonomous Vehicle}

The present invention relates to a position measurement of a traveling device, and specifically, a position measurement for a traveling device capable of minimizing a position error by correcting a positioning error due to an unknown error such as a slip phenomenon through an encoder, a gyro and an accelerometer. System and method.

In a traveling device configured to move autonomously, accurate position estimation is required for efficient control and magnetic position recognition.

The most common method used to estimate the position of such a traveling device is to use an encoder mounted on the drive wheels of the traveling device.

The encoder mounted on the driving wheel can estimate the driving distance according to the number of revolutions by counting the number of revolutions of the wheel, and calculate the difference of each encoder value when there is more than one encoder data available using two or more wheels. It is possible to estimate the rotational angular velocity and the rotational angle.

However, the position estimation using the encoder has a disadvantage in that when the slip occurs due to the sliding of the bottom surface in contact with the driving wheel, the actual moving distance and the estimated distance by the encoder have different values. Slips occur for a variety of reasons, including slippery floors, carpets, thresholds, and collisions.

As described above, the position measuring method in the related art is basically a method of measuring a rotation angle of a wheel using an encoder and estimating a position through the measured wheel rotation angle and kinematics of the traveling device.

However, driving devices using such encoders generate cumulative errors due to driving environment or system characteristics.

In order to solve this problem, many studies have been conducted on the use of probabilistic error correction methods such as an extended Kalman filter, a global positioning device capable of measuring absolute position coordinates, and an environmental cognition method.

However, even if stochastic error correction, unknown errors such as slippage could not be corrected, and the method using the global positioning device had a problem that the price was very high. Low.

In this regard, studies have been conducted on a method of correcting errors caused by wheel slippage using an inertial sensor such as an inertial navigation system (INS).

However, the INS basically calculates the acceleration of gravity using an accelerometer and integrates the acceleration twice to find the position, which causes a very large error during two integrations, resulting in very large positioning errors. There was.

In order to solve this problem, a method of using an encoder and an INS together, a method of distinguishing and measuring the constant speed acceleration and deceleration motion of the traveling device, a method of minimizing the INS error using the idling frequency of the traveling device, etc. This research has improved the performance, but there is still a problem that a large accumulation of errors.

The present invention is to solve the problem of the position measurement method of the prior art traveling device, to minimize the position error by correcting the position measurement error due to an unknown error, such as a slip phenomenon through the encoder, gyro, accelerometer It is an object of the present invention to provide a positioning system and method for a traveling device which can be used.

It is an object of the present invention to provide a position measuring system and method for a traveling device that can accurately correct an angular velocity by calculating a weight of an encoder instantaneous change amount and two calculated angular velocity values.

The present invention obtains the accurate angular velocity and linear velocity using a genetic algorithm with the corrected angular velocity and linear velocity by the encoder as a suitable function for the traveling device to determine the linear velocity and angular velocity accuracy of the accelerometer with high error accumulation. Its purpose is to provide a positioning system and method.

The present invention can determine the sliding phenomenon or the wheel turning phenomenon, it is possible to dynamically change the measurement model of the Kalman filter by using the position measurement system and method for the traveling device to calculate the exact attitude of the traveling device The purpose is to provide.

SUMMARY OF THE INVENTION An object of the present invention is to provide a positioning system and method for a traveling device capable of solving the divergence of Gaussian error covariance of a Kalman filter by varying the error covariance.

The objects of the present invention are not limited to the above-mentioned objects, and other objects that are not mentioned will be clearly understood by those skilled in the art from the following description.

Positioning system for a traveling device according to the present invention for achieving the above object is encoders for measuring the rotation angle of the drive wheels; gyros for measuring the rotational inertia; accelerometers for measuring the acceleration of gravity; driving with the encoder An angular velocity compensator for compensating the angular velocity using the angular velocity calculated through the kinematics of the device and the gyro; a linear velocity compensator for compensating using the compensated angular velocity and the linear velocity calculated through the encoder and the kinematics of the traveling device; And a posture compensator for compensating a posture using the calculated angular velocity, linear velocity and linear velocity, and the angular velocity and linear velocity.

The posture compensator may be configured to compare the angular velocity compensated by the encoder and the gyro and the angular velocity calculated by the accelerometer, and select the linear velocity calculated by the encoder or the accelerometer.

A positioning system for a traveling device according to the present invention for achieving another object includes a drive input unit for inputting the driving unit of the traveling device; encoders for obtaining a rotation angle of the wheel; gyros for calculating the angular velocity through the rotational inertia; Accelerometer for detecting the acceleration in both directions through the acceleration of gravity; Analysis unit for determining the operation of the driving device by analyzing the state of the drive input unit, encoder, gyro; Compensating the gyro and accelerometer when the driving device is not in operation Gyroscope and accelerometer correction unit; Position measurement value calculation unit for calculating the linear velocity and angular velocity using the detection values of the encoder, gyro, accelerometer; Compensation of the angular velocity by using the calculated value of the position measurement value calculation unit And a position measurement value compensator for compensating the linear velocity using the compensating linear velocity and compensating the attitude of the traveling device. And that is characterized.

Here, the position measurement value calculation unit, a first linear velocity calculation unit for calculating the measured values of the encoders and the attitude through the kinematics of the traveling device, a first angular velocity calculator and a third angular velocity for calculating the angular velocity through the gyro And a second linear velocity calculator and a third angular velocity calculator through the gravitational acceleration of the x-axis and the y-axis compensated by the accelerometer compensator and the accelerometer compensator according to the slope of the accelerometer. do.

And the position measurement value compensator, an angular velocity compensator for compensating the angular velocity based on the angular velocity calculated by the first angular velocity calculator and the angular velocity calculated by the third angular velocity calculator, and the position measured value calculator. A linear speed compensator for correcting the linear velocity using the linear speed calculated by the first linear velocity calculator and the angular velocity compensated by the angular velocity compensator, the angular velocity and the linear velocity compensator, and the second linear velocity And a posture compensator configured to compensate the posture of the driving apparatus using the linear speed of the calculator and the angular velocity of the second angular velocity calculator.

Position measuring method for a traveling device according to the present invention for achieving another object is an encoder for detecting the rotation angle, angular velocity, gravity acceleration of the wheel, gyro, the position of the traveling device having an accelerometer, the instant of the encoder Calculating a first weight according to the magnitude of the change; calculating a second weight by comparing the angular velocity calculated through the gyro with the angular velocity calculated through the kinematics of the encoder and the traveling device; using the first and second weights Calculating an angular velocity by using a genetic algorithm; predicting at least one driving wheel rotation value; calculating a linear speed based on the predicted rotation value of the driving wheel; calculated using the first and second weights Selecting a measurement model by comparing the angular velocity calculated by the angular velocity and the accelerometer; And calculating a measurement error covariance using the angular velocity calculated by using the first and second weights and the angular velocity calculated by the accelerometer.

The estimating of the driving wheel rotation value may include predicting one or more driving wheel rotation values using the angular velocity calculated using the first and second weights and the linear speed calculated using the encoder.

In the selecting of the measurement model, the angular velocity calculated using the first and second weights and the angular velocity calculated by the accelerometer may be applied to the measurement model of the Kalman filter.

, 상기 제 1,2 가중치를 이용하여 계산된 각속도

를 통해

를 계산하고, 시간 t를 이용하여 에러 공분산을

에 의해 산출하고, 여기서,

는 엔코더의 각속도 혹은 가속도계의 가속도이고,

의 k는 시간인 것을 특징으로 한다.And in the step of calculating the error covariance, the angular velocity of the selected measurement model

, The angular velocity calculated using the first and second weights

Through the

Calculate the error covariance using time t

Calculated by

Is the angular velocity of the encoder or the acceleration of the accelerometer,

K is time.

Position measuring method for a traveling device according to the present invention for achieving another object is an encoder for detecting the rotation angle, angular velocity, gravity acceleration of the wheel, gyro, in the position measurement of the traveling device having an accelerometer, Compensating the center value of the angular speedometer and gyro when not driving; Computing the linear speed and the angular speed by measuring the encoder when the driving device is running; Using the Gyman and geomagnetic field using a Kalman filter Calculating an angular velocity according to the magnitude of the encoder instantaneous change and two calculated angular velocity values; correcting the angular velocity using the calculated weight; correcting the linear velocity through a genetic algorithm; Measure the angular velocity of the x-axis and y-axis in the accelerometer, correct the angular velocity value according to the inclination of the accelerometer, Calculating a linear velocity and an angular velocity using the corrected angular velocity value; selecting a measurement model of the Kalman filter according to the difference between the gyro and the geomagnetic field and the angular velocity using the Kalman filter; together with the gyro and the geomagnetic field Correcting the measurement error covariance model of the Kalman filter according to the difference with the angular velocity using the Kalman filter; And calculating a posture.

Such a positioning system and method for a traveling device according to the present invention has the following effects.

First, the angular velocity can be accurately corrected by calculating the weight of the encoder instantaneous change amount and the calculated two angular velocity values.

Secondly, the corrected angular velocity and linear velocity can be obtained by using a genetic algorithm with the corrected angular velocity and linear velocity by encoder.

Third, it is possible to determine the accuracy of the linear speed and the angular velocity of the accelerometer with high error accumulation.

Fourth, it is possible to determine the phenomenon of slippage or wheel rotation, and use this to dynamically change the measurement model of the Kalman filter to calculate the exact attitude of the driving device.

Fifth, the variation of the Gaussian error covariance of the Kalman filter can be solved by changing the error covariance variably.

1 is a block diagram of a position measuring system of a direction traveling device according to an embodiment of the present invention
2 is a block diagram of an omnidirectional traveling device
3 is a block diagram of a configuration of a positioning system for a traveling device according to the present invention;
Figure 4 is a block diagram showing a position measuring process of the position measuring system for a traveling device according to the present invention

Hereinafter, a preferred embodiment of a positioning system and method for a traveling device according to the present invention will be described in detail.

The features and advantages of the positioning system and method for the traveling device according to the invention will become apparent from the detailed description of each embodiment below.

1 is a configuration diagram of a position measuring system of a direction traveling device according to an embodiment of the present invention, Figure 2 is a configuration diagram of the omnidirectional traveling device.

The present invention is to minimize the position error by correcting the position measurement error due to the unknown error, such as the slip phenomenon through the encoder and gyro, accelerometer.

The position measuring system of the omnidirectional traveling device according to the embodiment of the present invention includes four driving units 1 attached to the autonomous driving device, four encoders 2, two gyros and a geomagnetism as shown in FIGS. 1 and 2. It consists of a system 3 and four accelerometers 4.

Such a detailed configuration of the position measuring system for a traveling device according to the present invention is as follows.

3 is a block diagram of a configuration of a positioning system for a traveling device according to the present invention.

First, the drive input unit 100 for inputting the drive unit of the autonomous driving device, at least one encoder (2) for obtaining the rotation angle of the wheel, at least one gyro (3) for calculating the angular velocity through the rotational inertia, gravity It includes one or more accelerometers (4) for detecting the acceleration in both directions through the acceleration.

The analyzer 101 analyzes the state of the input unit, the encoder, and the gyro of the at least one driving unit to determine whether the driving device is in operation, and the gyro and accelerometer correction unit which corrects the gyro and the accelerometer when the driving device is not in operation. 102).

Then, using the calculated values of the position measurement value calculation unit 200 and the position measurement value calculation unit 200 for calculating the linear velocity and the angular velocity using the detected values of the encoder 2, the gyro 3, and the accelerometer 4 Compensating the angular speed by using the compensated angular speed to compensate the linear speed, and the position measurement value compensation unit 300 for compensating the attitude of the traveling device.

Here, the position measurement value calculation unit 200 includes a first linear velocity calculation unit 103 and a first angular velocity calculation unit 104 for calculating a measured value of the encoder 2 and a posture through kinematics of the traveling device, and a gyro. Compensated by the third angular velocity calculation unit 105 and the accelerometer compensator 108 and the accelerometer compensator 108 for compensating the accelerometer 4 according to the inclination of the accelerometer 4 through (3). A second linear velocity calculation unit 109 and the third angular velocity calculation unit 110 through the gravitational acceleration of the x-axis and y-axis.

The position measurement value compensator 300 includes an angular velocity compensator 106 for compensating the angular velocity based on the angular velocity calculated by the first angular velocity calculator 104 and the angular velocity calculated by the third angular velocity calculator 105, and The linear velocity compensator 107 for correcting the linear velocity using the linear velocity calculated by the linear velocity calculator 103 and the angular velocity compensated by the angular velocity compensator 106 and the angular velocity of the angular velocity compensator 106. And a posture compensator 111 for compensating a posture of the traveling device using the linear speed of the linear speed compensator 107, the linear speed of the second linear speed calculator 109, and the angular velocity of the second angular velocity calculator 110. ).

In the position measuring system for the traveling device according to the present invention having such a configuration, when the driving input unit 100 inputs the driving unit 1 of the autonomous driving device, the traveling device operates.

When the traveling device is operated, the gyro 3 measures the angular velocity through the encoder 2 and the rotational inertia for obtaining the rotation angle of the wheel, and analyzes whether the traveling device is running in the analysis unit 101.

At this time, if the analysis unit 101 determines that the traveling device is not running, the gyro and accelerometer correction unit 102 calculates and resets the center value of the gyro 3 and the accelerometer 4 to solve the draft error. .

If it is determined by the analysis unit 101 that the driving of the traveling device is started, the position measurement is started through the encoder 2, the gyro, the geomagnetic field 3, and the accelerometer 4.

The first linear velocity calculating section 103 and the first angular velocity calculating section 104 calculate the linear velocity and the angular velocity through the measured encoder 2 value and the kinematics of the traveling device, respectively.

In addition, the third angular velocity calculator 105 calculates the angular velocity through sensor fusion of the gyro 3 or the gyro + earth magnetic field 3.

The angular velocity of the first angular velocity calculator 104 and the angular velocity of the third angular velocity calculator 105 compensate for the angular velocity according to the weight of the encoder instantaneous change amount and the calculated two angular velocity values in the angular velocity compensator 106. .

The angular velocity compensated by the angular velocity compensator 106 is used as the fitness function of the genetic algorithm in the linear velocity compensator 107 together with the linear velocity of the first linear velocity calculator 103. The value is predicted and the linear velocity is compensated.

The accelerometer 4 is compensated by the accelerometer compensator 108 according to its slope, and the compensated acceleration is linear speed and angular velocity in the second linear velocity calculator 109 and the second angular velocity calculator 110. Will be calculated separately.

At this time, the angular velocity of the angular velocity compensator 106 and the angular velocity of the second angular velocity calculator 110 are analyzed by the posture compensator 111 and selected as a measurement model of the Kalman filter, and the attitude of the traveling device is calculated. .

Such a driving device position measuring method of the position measuring system for a traveling device according to the present invention is as follows.

4 is a block diagram illustrating a position measuring process of a position measuring system for a traveling device according to the present invention.

As shown in FIG. 4, the driving apparatus position measurement according to the embodiment of the present invention measures the encoder 2 and the gyro 3 (S201), determines whether the vehicle is driven (S202), and if it is determined that the vehicle is not driven, the gyro ( 3) and the accelerometer 4 are corrected and reset (S203).

If driving is started at the time of determining whether the vehicle is driven, the wheel rotation value is measured by the encoder 2 of the driving unit (S204), and the kinematics of the traveling device is calculated based on the encoder speeds (S205) and the kinematics of the traveling device. The angular velocity of the traveling device is calculated through the encoder values (S206).

Since the traveling device according to the exemplary embodiment has four driving units, the linear speed calculation S205 and the angular speed calculation S206 of the encoder are obtained as shown in Equations 1 and 2, respectively.

는 바퀴의 지름을 의미하며, 수학식 2에서 과

는 각각 주행 장치의 중심으로부터 엔코더의 x축과 y축의 거리를 의미한다.In Equation 1

Denotes the diameter of the wheel, and

Denotes the distance of the encoder x-axis and y-axis from the center of the traveling device, respectively.

Next, two gyro angular velocity measurements (S207) and azimuth angle measurements (S208) of the earth's magnetic field are calculated, through which angular velocities and azimuth angles are calculated, and these are fused with angular velocity calculation sensors through Kalman filters to calculate angular velocities. S209)

When the angular velocity is calculated through the gyro or the sensor fusion of the gyro and the geomagnetic field (S207) (S209), the weight is calculated according to the magnitude of the instantaneous change amount of the encoder.

This is to determine the weight of the gyro's angular velocity (S209) and the encoder's angular velocity by using the gyro, since the probability of slippage is increased when the instantaneous change in the encoder 2 increases rapidly.

That is, since the gyro 3 accumulates an error due to a draft phenomenon, the measurement accuracy is high for a short time, but the accuracy is lowered as it accumulates. Therefore, the cumulative error is minimized through the sensor fusion step with the geomagnetic field (S209). When there is no unknown error such as slipping, the encoder 2 has a very high precision, so that the angular velocity error is reduced once more.

에 따른 가중치 계산(S210)은 주행 장치의 시스템과 연관을 가지게 되고 다음과 같이 계산되어진다.Encoder's Instantaneous Variation Size

According to the weight calculation (S210) is associated with the system of the traveling device is calculated as follows.

은 엔코더의 순간 변화량 크기에 따른 가중치(S210)을 의미하고,

는 바퀴의 회전각을 의미한다.In Equation 3

Denotes a weight (S210) according to the magnitude of the instantaneous change amount of the encoder,

Means the angle of rotation of the wheel.

The driving apparatus according to an embodiment of the present invention has four driving wheels, a response speed of 100 ms, a wheel diameter of 20 cm, and an encoder resolution of 480 pulse.

는 현재

시간에서

시간까지를 비교하여 최소 변화량

과 최대 변화량

은 각각 0.65cm/s, 1.95cm/s로 하는 것이 바람직하다.At this time, the weight according to the magnitude of the instantaneous change amount

Is currently

In time

Minimum change by comparing time

And maximum change

Is preferably 0.65 cm / s and 1.95 cm / s, respectively.

Since the minimum change amount and the maximum change amount are influenced by the mass volume of the driving device and the driving part characteristics, it is desirable to find the optimal value through the simulation of the various conditions.

In addition, the weight is obtained by the number of driving wheels and divided by the number of driving wheels.

Next, according to the calculated two angular velocity values, the weight calculation S211 compares the angular velocity S206 of the encoder and the angular velocity S209 of the gyro, so that an error due to an unknown error may be largely generated. S206).

를 구하는 것이고 이는 다음과 같이 구해진다.It obtains the difference between the angular velocity (S206) of the encoder and the angular velocity (S209) of the gyro and weights accordingly.

Is obtained as

는 계산된 2개의 각속도 값에 따라 가중치(S211)를 의미하고,

와

는 각각 엔코더의 각속도(S206)과 자이로의 각속도(S209)를 나타낸다.In Equation 4

Denotes a weight S211 according to the calculated two angular velocity values,

Wow

Denotes the angular velocity S206 of the encoder and the angular velocity S209 of the gyro, respectively.

과

는 각각 0.5°/s과 1.2°/s로 두었다.The traveling device according to the present invention has a response speed of 100 ms and an average running speed of 10 cm / s.

and

Were set at 0.5 ° / s and 1.2 ° / s, respectively.

As described above, the gyro 3 is a draft phenomenon, and the error accumulates, and the error accumulates greatly over time, but the measurement for a short time is highly accurate because there is little influence on disturbance.

This means that if the difference between the measured angular velocity (S206) and the gyro angular velocity (S209) of the encoder is small for a small time, the high-precision encoder (2) does not receive an unknown error and has higher precision. have.

However, if the difference between the angular velocity S206 of the encoder and the angular velocity S209 of the gyro is large, it can be seen that the encoder has low precision due to an unknown error.

(S210)와 계산된 2개의 각속도 값에 따른 가중치

(S211) 계산되게 되면 다음 식과 같이 구한다.The angular velocity calculation (S212) is based on the weighted weight according to the magnitude of the instantaneous change amount of the encoder.

(S210) and the weight according to the calculated two angular velocity values

(S211) Once calculated, it is obtained as follows.

Next, the step of predicting the optimal rotation value of the wheel through the genetic algorithm (GA) (S213) step is to use the angular velocity (S212) and the linear velocity (S205) of the encoder calculated by the weight.

This means that the angular velocity (S206) of the encoder is wrong, which means that the linear velocity (S205) is wrong. Therefore, the linear velocity is predicted using a genetic algorithm, which is an optimal technique.

Genetic algorithms are optimization algorithms developed by observing the course of natural evolution, consisting of populations encoded in binary strings, and optimized by reproduction using genetic operators, crossover and mutation. The best way to find the solution.

Here, in the step of predicting the optimal rotation value of the wheel through the genetic algorithm (S213), the group of genetic algorithms is the rotation angle of the driving wheel and the fitness function is as follows.

Population size and generation size can be variably selected using the weights calculated from the weights.

When the optimal rotation value of the driving wheel is predicted by the genetic algorithm (S213), the linear speed of the driving device is calculated through the predicted wheel rotation value and the kinematics of the driving device (S214).

However, even when the genetic algorithm is used, since the linear speed S205 of the encoder included in the fitting function S213 may be largely damaged by an unknown error, the information of the accelerometer is further corrected.

That is, the accelerometer is measured in the angular velocity measurement (S125) of the x and y axes of the accelerometer, and the linear velocity and the angular velocity calculation (S217) are obtained through the angular velocity value correction (S216) according to their inclination.

The angular velocity measurement (S125) of the x-axis and y-axis of the accelerometer may be integrated to obtain a speed as follows.

와

는 각각 가속도계의 x축의 가속도와 y축의 가속도를 나타낸다.In Equation 7

Wow

Denotes the acceleration of the x-axis and the acceleration of the y-axis of the accelerometer, respectively.

However, the accelerometer, which calculates the position by integrating once and calculates the position by two times, has a problem that the error accumulates continuously when the body is inclined. In order to solve this problem, the angular velocity value is corrected by correcting the angular velocity value according to the slope.

와 각속도

계산은 다음과 같이 구해진다.(S217)Linear velocity through calibrated angular velocity value

And angular velocity

The calculation is obtained as follows (S217).

Here, since the most reliable, that is, the highest precision is the angular velocity (S209) of the gyro, it is determined whether to use the linear velocity (S217) of the accelerometer or the linear velocity (S214) of the encoder. (S218)

In this case, the Kalman filter is used for more accurate position measurement.

If the difference between the gyro's angular velocity (S209) and the encoder's angular velocity (S206) is smaller than the gyro's angular velocity (S209) and the accelerometer's angular velocity (S217) (S218), the Kalman filter's process model (process model) It is selected by the angular velocity (S209) and the angular velocity (S206) of the encoder (S219), and the measurement error covariance is obtained according to the difference between the angular velocity (S209) of the gyro and the angular velocity (S206) of the encoder. It is estimated (S223).

Otherwise, the measurement model of the Kalman filter is selected as the gyro's angular velocity (S209) and the accelerometer's acceleration (S217), and the measurement error covariance (S217) depends on the difference between the gyro's angular velocity (S209) and the accelerometer's angular velocity (S217). process error covariance) to estimate posture through a Kalman filter (S223).

은 다음과 같이 구한다.(S220)(S222)At this time, measurement error covariance

Is obtained as follows. (S220) (S222)

의 k는 시간을 나타낸다.In Equation 9 May be the angular velocity (S206) of the encoder or the acceleration (S217) of the accelerometer,

K represents time.

This is because when R is less than or equal to 1, R is fixed and as R is greater than 1 and k is larger, the value of R becomes smaller.

Next, the posture estimating step using the Kalman filter estimates the attitude by using the calculated linear velocity and the angular velocity of the selected measurement model S218 as the measurement model and the observation model as the posterior linear velocity and angular velocity. )

The position measuring system and method for a traveling device according to the present invention can accurately correct the angular velocity through the weight calculation based on the magnitude of the encoder instantaneous change amount and the calculated two angular velocity values.

In addition, the corrected angular velocity and linear velocity by encoder can be used to determine the exact angular velocity and linear velocity using genetic algorithms to determine the linear velocity and angular velocity accuracy of accelerometers with high error accumulation. Therefore, it is possible to determine the phenomenon of slippage or wheel rotation and use this to dynamically change the measurement model of the Kalman filter, thereby calculating the correct posture of the traveling device.

It will be understood that the present invention is implemented in a modified form without departing from the essential features of the present invention as described above.

It is therefore to be understood that the specified embodiments are to be considered in an illustrative rather than a restrictive sense and that the scope of the invention is indicated by the appended claims rather than by the foregoing description and that all such differences falling within the scope of equivalents thereof are intended to be embraced therein It should be interpreted.

100. Drive input 101. Analysis unit
102. Gyro and accelerometer correction unit 200. Position measurement value calculation unit
300. Position measurement value compensator

Claims (10)

In the system for the positioning of the traveling device,
Encoders for measuring the rotation angle of the drive wheels;
Gyros for measuring rotational inertia;
Accelerometers that measure gravity acceleration;
An angular velocity compensation unit configured to compensate the angular velocity by using the angular velocity and the angular velocity calculated by the kinematics of the encoder and the traveling device;
A linear speed compensating unit for compensating by using the compensated angular velocity and the linear velocity calculated through the kinematics of the encoder and the traveling device; And a posture compensator for compensating a posture using the calculated angular velocity, linear velocity, and the angular velocity and linear velocity calculated by the accelerometer. The method of claim 1, wherein the posture compensator,
The angular velocity compensated by the encoder and the gyro is compared with the angular velocity calculated by the accelerometer, and the linear velocity of the encoder or the linear velocity calculated by the accelerometer is selected. In the system for the positioning of the traveling device,
A drive input unit for inputting a drive unit of the traveling device;
Encoders for obtaining a rotation angle of a wheel;
Gyros for calculating angular velocity through rotational inertia;
Accelerometers that detect bidirectional acceleration through gravity acceleration;
An analyzer configured to analyze the driving input unit, the encoder, and the gyros to determine whether the driving device is operated;
A gyro and accelerometer corrector for correcting the gyro and the accelerometer when the traveling device is not operating;
A position measurement value calculator for calculating a linear velocity and an angular velocity using the detected values of the encoder, gyro and accelerometers;
And a position measurement value compensator for compensating the angular velocity using the calculated value of the position measurement value calculator and compensating the linear speed using the compensated angular velocity, and compensating the attitude of the traveling device. Positioning system. The method of claim 3, wherein the position measurement value calculation unit,
A first linear velocity calculation unit and a first angular velocity calculation unit calculating a measured value of the encoders and a posture through kinematics of the traveling device;
A third angular velocity calculator configured to calculate an angular velocity through the gyro;
And a second linear velocity calculator and a third angular velocity calculator through gravity acceleration of the x-axis and the y-axis compensated by the accelerometer compensator and the accelerometer compensator. Positioning system. The method of claim 3, wherein the position measurement value compensation unit,
An angular velocity compensator for compensating the angular velocity through the angular velocity calculated by the first angular velocity calculator and the angular velocity calculated by the third angular velocity calculator;
A linear velocity compensator for correcting the linear velocity by using the linear velocity calculated by the first linear velocity calculator and the angular velocity compensated by the angular velocity compensator;
And a posture compensator configured to compensate a posture of the traveling device by using the angular velocity compensating unit, the linear velocity compensating unit linear speed, the second linear velocity calculating unit linear speed, and the second angular velocity calculating unit angular velocity. Positioning system. In the position measurement of a traveling device having an encoder, a gyro and an accelerometer for detecting the rotation angle, the angular velocity and the gravity acceleration of the wheel,
Calculating a first weight value according to the magnitude of the instantaneous change amount of the encoder;
Calculating a second weight by comparing the angular velocity calculated by the kinematics of the encoder and the driving device with the angular velocity calculated by the gyro;
Calculating an angular velocity using the first and second weights;
Predicting one or more drive wheel rotation values through a genetic algorithm;
Calculating a linear velocity based on the predicted rotation of the driving wheel;
Selecting a measurement model by comparing the angular velocity calculated using the first and second weights with the angular velocity calculated by an accelerometer; And calculating a measurement error covariance using the angular velocity calculated by using the first and second weights and the angular velocity calculated by the accelerometer. The method of claim 6, wherein predicting the driving wheel rotation value comprises:
And predicting at least one driving wheel rotation value using the angular velocity calculated using the first and second weights and the linear velocity calculated using the encoder. The method of claim 6, wherein selecting the metrology model comprises:
And a dynamic model for comparing the angular velocity calculated by using the first and second weights with the angular velocity calculated by the accelerometer. 7. The method of claim 6, wherein in calculating the error covariance:
Angular velocity of the selected measurement model

, The angular velocity calculated using the first and second weights

Through the

Calculate the error covariance using time t

Calculated by
here,

Is the angular velocity of the encoder or the acceleration of the accelerometer,

K is time. In the position measurement of a traveling device having an encoder, a gyro and an accelerometer for detecting the rotation angle, the angular velocity and the gravity acceleration of the wheel,
Correcting the center value of the angular speedometer and gyro when the traveling device is not running;
Calculating a linear velocity and an angular velocity by measuring an encoder when the traveling apparatus is traveling;
Calculating an angular velocity using a Kalman filter together with the gyro and a geomagnetic field;
Calculating a weight according to the magnitude of the encoder instantaneous change amount and the calculated two angular velocity values;
Correcting the angular velocity using the calculated weights;
Correcting the linear velocity through a genetic algorithm;
Measuring the angular velocities of the x and y axes in the accelerometer, correcting the angular velocity values according to the inclination of the accelerometer, and calculating linear and angular velocities using the corrected angular velocity values;
Selecting a measurement model of the Kalman filter according to the difference between the gyro and the angular velocity using the Kalman filter together with the geomagnetic field;
Modifying the measurement error covariance model of the Kalman filter according to the gyro and the angular velocity using the Kalman filter together with the geomagnetic field; And calculating a posture.

Images