KR20170068234A - Bias correcting apparatus for yaw angle estimation of mobile robots and method thereof - Google Patents

Bias correcting apparatus for yaw angle estimation of mobile robots and method thereof Download PDF

Info

Publication number
KR20170068234A
KR20170068234A KR1020150175190A KR20150175190A KR20170068234A KR 20170068234 A KR20170068234 A KR 20170068234A KR 1020150175190 A KR1020150175190 A KR 1020150175190A KR 20150175190 A KR20150175190 A KR 20150175190A KR 20170068234 A KR20170068234 A KR 20170068234A
Authority
KR
South Korea
Prior art keywords
bias
traveling
traveling robot
value
estimating
Prior art date
Application number
KR1020150175190A
Other languages
Korean (ko)
Other versions
KR101789776B1 (en
Inventor
홍성경
이상철
조윤희
Original Assignee
세종대학교산학협력단
아이에스티웍스(주)
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 세종대학교산학협력단, 아이에스티웍스(주) filed Critical 세종대학교산학협력단
Priority to KR1020150175190A priority Critical patent/KR101789776B1/en
Publication of KR20170068234A publication Critical patent/KR20170068234A/en
Application granted granted Critical
Publication of KR101789776B1 publication Critical patent/KR101789776B1/en

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • B25J9/1692—Calibration of manipulator
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J5/00—Manipulators mounted on wheels or on carriages
    • B25J5/007—Manipulators mounted on wheels or on carriages mounted on wheels
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Gyroscopes (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The present invention relates to a bias correction apparatus and method for tracking a traveling angle of a traveling robot capable of accurately estimating a traveling angle of a traveling robot by correcting a bias error generated by a gyro sensor in a traveling robot using a gyro sensor, A gyro for measuring the rotational angular velocity of the robot, an encoder for measuring a pulse according to the rotation of the traveling wheel of the traveling robot, and a controller for counting the pulses measured by the encoder to identify the traveling state of the traveling robot, A bias estimator for estimating a bias for each exercise state by expanding an interval in which a gyro bias can be estimated, and a bias corrector for removing and correcting an estimated bias calculated through a bias estimator in an output value measured through the gyro.

Description

TECHNICAL FIELD [0001] The present invention relates to a bias correcting apparatus and method for estimating a traveling angle of a traveling robot,

The present invention relates to a bias correcting apparatus and method for estimating the traveling angle of a traveling robot that can accurately estimate a traveling angle of a traveling robot by correcting a bias error generated by a gyro sensor in a traveling robot using a gyro sensor.

Recently, the robot industry has evolved from the concept of 'traditional robot' which was a substitute for labor, and has developed into the concept of 'humanoid friendly' intelligent robot.

The intelligent robot recognizes the external environment, judges the situation itself, and supports various services based on autonomous motion. Therefore, intelligent robots are expected to be used in various fields, leading to the future robot market.

One of the key technologies for realizing such an intelligent robot is location recognition technology with spatial perception ability. In particular, in order to commercialize indoor home service robot, development of location recognition technology based on low-cost yaw angle estimation It is becoming important.

The MEMS gyro sensor, which measures the angular velocity of the position recognition technology based on the low-level travel angle estimation, has advantages of low cost, low power, small size and light weight, but the integration process is inevitably required to measure the travel angle. And there is a fatal disadvantage that the error is diverged with time.

To solve this drawback, a heuristic drift reduction (HDR) based algorithm has been implemented to minimize the cumulative error. In particular, the HDR based algorithm estimates the bias drift, which is one of the main factors causing the integration error of the gyro sensor It is a technology to compensate.

However, the traveling angle estimation using the conventional HDR-based algorithm has a limitation that the traveling robot must be in a stationary state since the average value of the angular velocity output data is estimated and calculated during a certain period in a completely stopped state without movement of the traveling robot. In addition, since the gyro output that has already been contaminated by the physical bias drift generated by the gyro sensor and the small vibration generated by the traveling robot itself is used as a reference, the accuracy of the determination is also degraded.

For example, in the case of the robot cleaner, the value outputted from the gyro sensor changes as the brush rotates even in the stop state as shown in the waveform shown in FIG. If the brush is rotated even in the stationary state, the value output from the gyro sensor due to the vibration component is not accurate and the output value reflecting the vibration component is used.

In addition, the robot cleaner has a very short stop state during operation in an actual autonomous cleaning mode, so that it is difficult to estimate an accurate bias drift.

Therefore, there is a need for a method capable of estimating the bias drift not only in the stationary state of the traveling robot but also in the straight forward state where the actual angular velocity is small.

Accordingly, the applicant of the present invention has proposed the present invention based on the above-mentioned necessity. As a prior art document related thereto, there is disclosed an apparatus and a method for correcting the bias of a gyro mounted on a mobile robot in Patent Registration No. 10-0772915 , Registered Date: October 29, 2007).

SUMMARY OF THE INVENTION It is an object of the present invention to solve the above-mentioned problems, and it is an object of the present invention to provide a gyroscope that can identify a motion state of a traveling robot and extend a bias drift estimation interval generated by the gyro sensor, And a bias correcting device and method for estimating the traveling angle of a traveling robot that can estimate the traveling angle adaptively through the compensation.

Another object of the present invention is to minimize the accumulation of integration errors by identifying the motion state of the traveling robot and performing switching control so as not to perform integration calculation for the traveling angle estimation in the stationary section.

According to an aspect of the present invention, there is provided a bias correcting apparatus for estimating a traveling angle of a traveling robot, the apparatus comprising: a gyro measuring a rotational angular velocity of the traveling robot; An encoder for measuring a pulse according to the rotation of the traveling wheel of the traveling robot; A bias counting unit for counting pulses measured by the encoder to identify a motion state of the traveling robot and a bias estimation unit for estimating a bias for each motion state by expanding an interval in which the bias of the gyro can be estimated according to a motion state of the traveling robot, government; And a bias correcting unit for correcting the output value measured through the gyro by removing the estimated bias calculated through the bias estimating unit.

The bias estimator counts the pulses measured through the encoder to identify the motion state of the traveling robot according to the number of counts, and if the identified motion state is a stop region or a straight region, the bias estimator calculates a heuristic drift reduction (HDR) And estimates a bias from angular velocity values corresponding to the stationary section or the straight section of the angular velocity values measured through the gyro.

The bias estimating unit may include a motion state identifying unit that identifies a motion state of stopping, straightening, and rotating of the traveling robot in accordance with the counted number of pulses measured through the encoder and outputs an identification value according to the identified motion state, ; An attenuation gain controller for adjusting the gains of the HDR-based filters differently with respect to a stop section or a straight section of the traveling robot using the identification value; And a bias calculator for calculating a bias drift by reflecting the adjusted gain value through the attenuation gain controller on the HDR-based filter.

Wherein the motion state identification unit is configured to determine whether the number of counts of the pulses output from the two encoders is 0 or not, At the same time, it is possible to identify the straight section and the rotation section according to whether the absolute value of the difference between the counts outputted from the two encoders exceeds the threshold value.

The damping gain control unit determines whether or not the angular velocity value corresponding to the stop, straight line, and rotation interval among the angular velocity values measured through the gyro is equal to or smaller than the reference angular velocity threshold value, in addition to the identification value output through the motion state identification unit So that the gain value can be calculated for the section where both conditions are satisfied.

Also, the attenuation gain control unit may calculate a gain value for each section using the following equation.

[Mathematical Expression]

Figure pat00001

(only,

Figure pat00002
Is a gain value,
Figure pat00003
Is an identification value,
Figure pat00004
Is the angular velocity value corresponding to the stop, straight and rotation section through the gyro, and 0.7 deg / s is the reference angular velocity threshold value for determining the fine rotation occurring in the straight section.)

The bias calculator may calculate an estimated bias for each motion state of the traveling robot using the following HDR-based estimation equation.

[Estimation formula]

Figure pat00005
,

Figure pat00006

(only,

Figure pat00007
Is a bias drift estimation value, x is a gyro output value,
Figure pat00008
Constant)

The bias correcting apparatus may further include a switching unit for selectively blocking the execution of the integral calculation for the traveling angle estimation according to the motion state of the traveling robot identified through the bias estimating unit.

The switching unit may stop the integration operation and prevent integration errors from accumulating when the traveling state of the traveling robot is in the stopping period.

 According to another aspect of the present invention, there is provided a bias correcting method for estimating a traveling angle of a traveling robot, the method comprising the steps of: measuring a rotational angular velocity of a gyro of the traveling robot; ; Measuring a pulse according to rotation of the traveling wheel of an encoder mounted on a traveling wheel of the traveling robot; The bias estimating unit of the traveling robot counts the pulses measured through the encoder to identify the motion state of the traveling robot and expands the interval in which the bias of the gyro can be estimated according to the motion state of the traveling robot, Estimating a bias by each bias; And a step in which the bias correction unit of the traveling robot removes the estimated bias calculated through the bias estimation unit from the output value measured through the gyro and corrects the bias.

According to the present invention, the bias error generated by the gyro sensor is corrected by expanding the bias estimation period so that the traveling robot can estimate the bias drift not only in the stationary section but also in the rectilinear section in which the traveling robot shakes finely by identifying the motion state of the traveling robot There is an effect that can be. This bias error correction can more accurately estimate the traveling angle of the traveling robot.

In addition, according to the present invention, there is an effect of minimizing the accumulation of integration errors by identifying the motion state of the traveling robot and performing switching control so as not to perform integral calculation for estimating the traveling angle in the stop section.

FIG. 1 is a waveform diagram of a gyro output value measured in a stationary state of a general robot cleaner.
2 is a block diagram of a bias correction apparatus for estimating a traveling angle of a traveling robot according to an embodiment of the present invention.
3 is a conceptual diagram for explaining the traveling angle of the traveling robot according to the embodiment of the present invention.
4 is a detailed configuration diagram of a bias estimator applied to a bias correcting apparatus for estimating a traveling angle of a traveling robot according to an embodiment of the present invention.
5 is a table showing identification values defined according to the motion state of the traveling robot in the bias estimating unit according to the embodiment of the present invention.
6 is a flowchart illustrating a bias correction method for estimating a traveling angle of a traveling robot according to an embodiment of the present invention.
FIG. 7 is a graph comparing a progress angle estimation value output through a bias correction method according to an embodiment of the present invention with a reference device and a conventional HDR-based progress angle estimation filter.
8 is a comparative graph showing an enlarged view of a part of the graph of FIG.

BRIEF DESCRIPTION OF THE DRAWINGS The advantages and features of the present invention and the manner of achieving them will become apparent with reference to the embodiments described in detail below with reference to the accompanying drawings.

The present invention is not limited to the embodiments disclosed below but may be embodied in various forms without departing from the spirit and scope of the invention. To fully disclose the scope of the invention to a person skilled in the art, and the invention is only defined by the scope of the claims.

For reference, in the embodiment of the present invention, the low-performance low-cost encoder installed in the traveling robot is used to identify the traveling state of the traveling robot, that is, the traveling state, and the traveling state is not only the stationary period, The bias drift component included in the output of the gyro sensor is extended to a region where the bias drift factor can be estimated, thereby providing a configuration for correcting a bias drift of the gyro sensor. In this case, the bias drift is estimated using a filter based on the existing HDR (Heuristic Drift Reduction) algorithm, and the gain of the HDR filter is adaptively controlled according to the stop section or the straight section, It is possible to provide a traveling robot capable of estimating the traveling angle.

FIG. 2 is a block diagram of a bias correcting apparatus for estimating a traveling angle of a traveling robot according to an embodiment of the present invention. FIG. 3 is a conceptual diagram illustrating a traveling angle of a traveling robot according to an embodiment of the present invention.

The bias correcting apparatus 100 according to an embodiment of the present invention basically includes an encoder 120, a gyro 130, a signal filter 140, a bias estimator 150, a bias corrector 160, 170).

The traveling unit 110 and the traveling angle estimating unit 180 are basically included in a traveling robot using a gyro sensor.

The travel unit 110 provides power for the traveling robot to move. In general, the traveling unit 110 includes one or more traveling wheels 10a and 10b and a direction control device as shown in FIG. 3, but is not limited thereto. And a traveling means of the vehicle.

The travel estimating unit 180 is for grasping the current accurate position of the traveling robot 10 while traveling through the travel unit 110 and includes an angular velocity sensor 160 for detecting an angular velocity To estimate the traveling angle.

Assuming that the position coordinates of the traveling robot 10 are represented by (X (k), Y (k)) in the coordinate system shown in Fig. 3 and the left and right traveling wheels 10a and 10b move in the a direction, (10) travels in the A direction and the angle at which the traveling robot (10) rotates can be defined as? (K). Here, the rotation angle ψ (k) means the traveling angle of the traveling robot 10, which is an angle value calculated by integrating the measured angular velocity deg / s from the gyro 130 in the traveling angle estimation unit 180 (deg). < / RTI > Reference numerals 120a and 120b denote encoders.

Referring again to FIG. 2, the gyro 130 measures a rotational angular velocity (deg / s) of a traveling robot rotating around at least one axis as a type of inertial sensor. Then, the gyro 130 calculates the actual angular velocity value (

Figure pat00009
) To the gyro output value (
Figure pat00010
. The most ideal gyro 130 without noise outputs the rotational angular velocity of the measured traveling robot, but usually it is common that noise is included by the sensor and the external environment.

In the present embodiment, the gyro output value (

Figure pat00011
) Is a value in a state in which bias correction is not performed, that is, an actual rotational angular velocity value (
Figure pat00012
) To the bias element
Figure pat00013
,
Figure pat00014
). The bias is a fixed bias generated by the initial operating current when the power is applied
Figure pat00015
) And bias drift
Figure pat00016
) May be included.

The gyro 130 outputs the gyro output value (

Figure pat00017
To the bias correction unit 160, and can pass the signal filter unit 140 as needed.

The signal filter unit 140 uses the average filter to calculate the gyro output value

Figure pat00018
) To a fixed bias (
Figure pat00019
) Is removed by filtering. For reference, a fixed bias (
Figure pat00020
Can be estimated by calculating an average value of the angular velocity output data for about 5 seconds to 15 seconds under a perfect stop state in which there is no initial motion of the traveling robot after power is applied. The values output through the signal filter unit 140 are the same as those shown in (2) of FIG. Particularly, in the case of a robot cleaner, a fine vibration component and a low-band noise signal may be generated according to the rotation of the brush. The signal filter 140 filters the low- It is possible to remove unnecessary signals for estimation. In other words, the fixed bias (
Figure pat00021
) Is removed through an averaging filter, and the vibration noise generated in the signal by the rotation of the brush or the like can be removed through the low-pass filter.

The encoder 120 interlocks with the travel unit 110 to measure a pulse according to the rotation of the traveling wheel during traveling of the traveling robot. FIG. 3 shows an example in which the encoders 120a and 120b are mounted on the left and right traveling wheels 10a and 10b, respectively.

The encoder 120 is mounted in correspondence with the traveling wheels, and when two traveling wheels are provided on the right and left sides, the pulses of the respective traveling wheels are measured through the two left and right encoders 120.

The bias estimator 150 counts the pulses measured through the encoder 120 and identifies the motion state of the traveling robot according to the counting number. When the identified motion state is the stopping section or the straightening section, the gain value of the HDR-based filter is adjusted to estimate the bias from the angular velocity value corresponding to the stop section or the straight section of the angular velocity measured through the gyro 130 do.

The bias estimating unit 150 according to the present embodiment is intended to expand the bias estimation period so that the bias can be estimated even when the traveling robot is in a stationary state as well as in a stationary state. To this end, the motion state of the traveling robot is identified through the encoder 120, and the gain of the HDR-based filter is adjusted for each motion section to adaptively estimate the bias. This will be described in detail later with reference to FIG. 4 and FIG.

The bias correction unit 160 removes the estimated bias calculated through the bias estimation unit 150 from the gyro output value output from the gyro 130 via the signal filter unit 140 and corrects the bias. The corrected gyro output value is transmitted to the progress angle estimating unit 180, and it is possible to estimate the traveling angle based on more accurate data, so that the error of the traveling angle can be reduced.

The value outputted through the bias correction unit 160 is the same as the formula shown in (3) of FIG. That is, the actual rotational angular velocity value measured in the gyro 130

Figure pat00022
Is output.

The switching unit 170 is provided at the front end of the travel angle estimating unit 180 and selectively interrupts the execution of the integral calculation for estimating the traveling angle depending on the motion state of the traveling robot in conjunction with the bias estimating unit 150.

For example, when the traveling robot is stationary, there is no change in the traveling angle. Therefore, the switching unit 170 controls the switching unit 170 so as to stop the integration operation when the traveling robot is stopped to minimize the accumulation of integration errors.

Accordingly, when the traveling robot is in the stopped state ("

Figure pat00023
) Stops the integral operation and maintains the previous traveling angle, and performs the integration operation when the traveling robot is in the straight state or in the rotating state. The equation is expressed as follows.

Figure pat00024

In the above formula,

Figure pat00025
Is an identification value that identifies the motion state of the traveling robot,
Figure pat00026
Is in a stopped state,
Figure pat00027
Is a straight state,
Figure pat00028
Lt; / RTI >
Figure pat00029
Is a bias-corrected gyro output value, and? Represents a traveling angle.

FIG. 4 is a detailed configuration diagram of a bias estimating unit according to an embodiment of the present invention, and FIG. 5 is a table showing identification values defined according to a motion state of the traveling robot in the bias estimating unit according to the embodiment of the present invention.

The bias estimation unit 150 may include a motion state identification unit 152, an attenuation gain control unit 154, and a bias calculation unit 156.

The motion state identification unit 152 counts the pulses measured through the encoder, identifies the motion state of the robot stop, straight forward, androtated according to the counted number, and outputs an identification value according to the identified motion state.

Generally, as shown in FIG. 5 (a), the traveling robot can roughly classify the motions occurring outside the intermittent stop state into three types. The definition of each movement is as follows.

- swaying: The traveling robot keeps the linear motion and simultaneously makes a slight shake to the left and right

- Curving: slowly rotating according to a large radius of curvature

- Turning: Rapid rotation according to small radius of curvature.

The motion state identification unit 152 identifies these motion states as the pulse signals measured by the encoder 120. When the traveling wheels are respectively provided on the right and left sides, the motion state identification unit 152 outputs the motion signals from the left and right two encoders (120a and 120b in FIG. 3) The number of pulses to be counted (

Figure pat00030
,
Figure pat00031
) Is all 0,
Figure pat00032
). Also, the number of counts of pulses output from the two encoders (
Figure pat00033
,
Figure pat00034
) Is not 0 and the absolute value of the difference between the counts output from the two encoders at the same time
Figure pat00035
) ≪ / RTI >
Figure pat00036
), It is possible to identify the straight section and the rotation section.

The absolute value of the difference in the number of counts output from the two encoders (

Figure pat00037
) ≪ / RTI >
Figure pat00038
), It is about the period of curving or turning, and the threshold value (
Figure pat00039
), It can be defined as a section where the straight motion and the fine swaying of the left and right coexist.

5 (b), the motion state identification unit 152 outputs the motion state identification function output value

Figure pat00040
0 "in the stop state," 1 "in the straight state, and" 2 "in the rotation state.

The attenuation gain control unit 154 receives an identification value ("

Figure pat00041
), The gain values of the HDR-based filters are adjusted differently for the stop section or the straight section of the traveling robot, respectively.

Also, the attenuation gain control unit 154 outputs an identification value ("

Figure pat00042
), It is determined whether the angular velocity value corresponding to the stop, the straight line, and the rotation section among the angular velocity values measured through the gyro is less than the reference angular velocity threshold value. If both the identification value and the angular velocity value for each section are both satisfied The gain value can be calculated for the period. The gain calculation equation is shown in the following equation.

[Mathematical Expression]

Figure pat00043

here,

Figure pat00044
Is a gain value,
Figure pat00045
Is an identification value,
Figure pat00046
Is an angular velocity value corresponding to a stop, a straight line, and a rotation section through a gyro, and 0.7 deg / s is a reference angular velocity threshold value for determining a fine rotation occurring in a straight line section. That is, if the angular velocity value is larger than the reference angular velocity threshold value, the rotation is strong, and if the angular velocity value is smaller than the reference angular velocity threshold value, it can be judged as a weak rotation. Here, 0.7 deg / s corresponds to one example for convenience of understanding and can be changed according to the specifications of the traveling robot.

According to the above equation, the damping gain control unit 154 determines whether or not the motion state of the traveling robot is in the rotation period

Figure pat00047
), An angular velocity value corresponding to each section (
Figure pat00048
) Exceeds the reference angular velocity threshold of 0.7 deg / s, the gain value
Figure pat00049
) To 0 to avoid estimation of the bias drift factor. That is, it is regarded as a previous estimation bias value.

In addition, when the motion state of the traveling robot is a straight line or a stop section

Figure pat00050
,
Figure pat00051
), And at the same time, an angular velocity value (
Figure pat00052
) Is less than the reference angular velocity threshold of 0.7 deg / s, the previous gyro output value and the angular velocity value corresponding to each section (
Figure pat00053
) To the gain value (
Figure pat00054
).

The bias calculation unit 156 calculates the gain value (

Figure pat00055
) To the HDR-based filter to calculate the bias drift. The calculated bias drift is called the estimated bias or bias drift estimate. The calculation formula is the same as the following estimation formula.

 [Estimation formula]

Figure pat00056
,

Figure pat00057

(only,

Figure pat00058
Is a bias drift estimation value, x is a gyro output value,
Figure pat00059
Constant)

In the above estimation formula,

Figure pat00060
Is a fixed increment,
Figure pat00061
, Respectively,
Figure pat00062
Is determined according to the rotational direction of the gyro.
Figure pat00063
Outputs a sensor output value, that is, a gyro output value (x) is positive water surface 1, 0 is 0, and negative water surface -1.

The bias drift estimate (

Figure pat00064
Is a bias drift value included in the gyro 130 and output
Figure pat00065
), The final output value is
Figure pat00066
The bias drift can be effectively estimated through the bias calculating unit 156. [

A bias correction method for traveling angle estimation of a traveling robot according to an embodiment of the present invention will be described with reference to the above configuration.

6 is a flowchart illustrating a bias correction method for estimating a traveling angle of a traveling robot according to an embodiment of the present invention. For reference, the description will be made with reference to the constituent elements of Fig. 2 and Fig.

First, in step S100, the gyro 130 mounted on the traveling robot measures the rotational angular velocity of the traveling robot. The bias correcting apparatus 100 outputs a gyro output value (corresponding to the rotational angular velocity of the traveling robot) from the gyro 130

Figure pat00067
).

Gyro output value (

Figure pat00068
) Is the actual rotational angular velocity value (
Figure pat00069
) To the bias element
Figure pat00070
,
Figure pat00071
). That is, the gyro output value
Figure pat00072
) Is as follows.

Figure pat00073

In step S110, the bias correcting apparatus 100 calculates a gyro output value

Figure pat00074
) Is subjected to signal processing through an average filter to obtain a fixed bias (
Figure pat00075
). Therefore, the gyro output value ("
Figure pat00076
) Is as follows.

Figure pat00077

In the next step S200, the encoder 120 mounted on the traveling robot measures a pulse according to the rotation of the traveling wheel. The bias correcting apparatus 100 obtains a pulse according to the rotation of the traveling wheel from the encoder 120 in the bias estimating unit 150, counts the obtained pulse,

Figure pat00078
,
Figure pat00079
).

In the next step S210, the bias correcting apparatus 100 identifies the motion state of the traveling robot in accordance with the counting number in the motion state identifying unit 152 of the bias estimating unit 150,

Figure pat00080
). For example, the discrimination value is outputted as 0 in the stop section, 1 in the rectilinear section in which there is slight shaking, and 2 in the rotating section rotating according to the radius of curvature, depending on the motion state of the traveling robot.

At this stage, the motion state identifying unit 152 of the bias estimating unit 150 can identify the stopping period when the number of pulses output from the encoder 120 is zero. When the number of counts of the pulses output from the encoder 120 is not 0, the threshold value (

Figure pat00081
), It is possible to identify the straight section and the rotation section.

For example, when the traveling wheels are respectively provided on the left and right sides, the number of counts of pulses outputted from the two left and right encoders (120a and 120b in FIG. 3)

Figure pat00082
,
Figure pat00083
) Is all 0,
Figure pat00084
). Also, the number of counts of pulses output from the two encoders (
Figure pat00085
,
Figure pat00086
) Is not zero and at the same time the absolute value of the difference in the number of counts output from the two encoders
Figure pat00087
) ≪ / RTI >
Figure pat00088
), The rotation period, the threshold value (
Figure pat00089
), It is identified as a straight section.

In the next step S220, the bias correcting apparatus 100 corrects the bias value of the bias value by using the damping gain control unit 154 of the bias estimating unit 150,

Figure pat00090
) Is used to determine the gain value of the HDR-based filter for the stationary section or the straight section of the traveling robot
Figure pat00091
), And calculates the calculated gain value (
Figure pat00092
) To the HDR-based filter.

Gain value

Figure pat00093
) Can be calculated using the following equation.

[Mathematical Expression]

Figure pat00094

here,

Figure pat00095
Is a gain value,
Figure pat00096
Is an identification value,
Figure pat00097
Is an angular velocity value corresponding to a stop, a straight line, and a rotation section through a gyro, and 0.7 deg / s is a reference angular velocity threshold value for determining a fine rotation occurring in a straight line section.

In the next step S230, the bias correcting apparatus 100 corrects the gain value calculated in the previous step in the bias calculating unit 150

Figure pat00098
To estimate the bias. The bias estimation equation is shown in the following equation.

[Estimation formula]

Figure pat00099
,

Figure pat00100

(only,

Figure pat00101
Is a bias drift estimation value, x is a gyro output value,
Figure pat00102
Constant)

The bias drift estimate (

Figure pat00103
) Is the bias drift
Figure pat00104
) Can be calculated.

In step S240, the bias correcting apparatus 100 determines whether the gyro output value

Figure pat00105
). ≪ / RTI > That is, the bias drift estimation value (
Figure pat00106
) To the gyro output value (step < RTI ID = 0.0 >
Figure pat00107
).

At this time, the bias drift estimation value

Figure pat00108
) This bias drift (
Figure pat00109
), The final output value ("
Figure pat00110
) Is the actual rotational angular velocity value measured through the gyro (
Figure pat00111
).

Figure pat00112

Finally, in step S250, the traveling robot calculates the corrected final output value (

Figure pat00113
). ≪ / RTI >

Meanwhile, before estimating the traveling angle, the bias correcting apparatus 100 may selectively block the performing of the integration operation according to the motion state of the traveling robot in the switching unit 170. [ Particularly, when the traveling robot is in the stop section, switching is performed so as to stop the integral operation for estimating the traveling angle, thereby minimizing accumulation of integration errors.

The verification experiment was conducted based on the estimated progress angle result table.

To accomplish this, a gyro sensor, which meets the electrical interface and size specifications shown in the table below, was manufactured on the existing robot cleaner (VR20H9050UW). The MCU of the inertial sensor module is based on ATmega328 (Atmel), and the MEMS gyro adopts MPU6050 (Invensense) z-axis gyro sensor. The encoder used RB-35GM (D & J WITH) equipped with a commercial robot cleaner (VR20H9050UW). The standard equipment was Raptor-4, Motion capture equipment of MotionAnalysis.

[table]

Figure pat00114

FIG. 7 shows the result of the traveling angle estimation of the reference equipment, the traveling robot to which the bias correcting device according to the embodiment of the present invention is applied, and the conventional HDR-based traveling robot, FIG. 8 shows an enlarged view .

As shown in the graph of FIG. 7, the conventional HDR-based traveling robot P 2 exhibits a large error with respect to the reference device P 1 over time, while the proposed traveling robot P 3 of the present invention, (P1) of the reference device.

In FIG. 8, the difference is more clearly shown in the 275 to 300 seconds enlarged section. That is, the existing HDR-based traveling robot P2 shows an error of 20 degrees or more, and the proposed traveling robot P3 of the present invention shows an error of less than 1 degree.

Therefore, it can be seen that the performance of the traveling robot P3 according to the present invention is stabilized regardless of the time by adaptively estimating and correcting the bias according to the intervals through the motion state identification.

While the present invention has been described in connection with what is presently considered to be the most practical and preferred embodiment, it is to be understood that the invention is not limited to the disclosed embodiments, but, on the contrary, is intended to cover various modifications and equivalent arrangements included within the spirit and scope of the 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.

100: bias correcting device 110:
120: Encoder 130: Gyro
140: Signal filter unit 150: Bias estimation unit
160: bias correction unit 170: switch unit
180: Progress angle estimation unit 152: Motion state identification unit
154: attenuation gain control unit 156:

Claims (16)

A gyro that measures the rotational angular velocity of the traveling robot;
An encoder for measuring a pulse according to the rotation of the traveling wheel of the traveling robot;
A bias counting unit for counting pulses measured by the encoder to identify a motion state of the traveling robot and a bias estimation unit for estimating a bias for each motion state by expanding an interval in which the bias of the gyro can be estimated according to a motion state of the traveling robot, government; And
A bias correcting unit for correcting the bias value calculated by the bias estimating unit from the output value measured through the gyro,
And estimating the traveling angle of the traveling robot.
The method according to claim 1,
The bias estimator may include:
And counting the pulses measured through the encoder to identify the motion state of the traveling robot according to the counting number, and adjusting the gain value of the HDR (Heuristic Drift Reduction) -based filter according to the identified motion state as the stopping period or the straightening period And estimates a bias from an angular velocity value corresponding to the stationary section or the straight section of angular velocity values measured through the gyro.
3. The method according to claim 1 or 2,
The bias estimator may include:
A motion state identification unit for identifying a motion state of stopping, straightening, and turning of the traveling robot according to the counted number of pulses measured through the encoder and outputting an identification value according to the identified motion state;
An attenuation gain controller for adjusting the gains of the HDR-based filters differently with respect to a stop section or a straight section of the traveling robot using the identification value; And
A bias calculator for calculating a bias drift by reflecting the adjusted gain value through the attenuation gain controller on the HDR-
And estimating the traveling angle of the traveling robot.
The method of claim 3,
Wherein the motion state identification unit comprises:
When the number of counts of the pulses output from the two left and right encoders is all 0, the number of counts of the pulses output from the two encoders is not zero, Wherein the controller identifies the straight section and the rotation section according to whether the absolute value of the difference between the number of counts exceeds a threshold value.
The method of claim 3,
Wherein the attenuation gain control unit comprises:
It is determined whether or not the angular velocity value corresponding to the stop, straight line, and rotation interval among the angular velocity values measured through the gyro is equal to or less than the reference angular velocity threshold value, in addition to the identification value output through the motion state identification unit. And the gain value is calculated for a period during which the traveling robot estimates the traveling angle of the traveling robot.
The method of claim 3,
Wherein the attenuation gain control unit comprises:
And calculates a gain value for each zone using the following equation: < EMI ID = 1.0 >
[Mathematical Expression]
Figure pat00115

(only,
Figure pat00116
Is a gain value,
Figure pat00117
Is an identification value,
Figure pat00118
Is the angular velocity value corresponding to the stop, straight and rotation section through the gyro, and 0.7 deg / s is the reference angular velocity threshold value for determining the fine rotation occurring in the straight section.)
The method of claim 3,
The bias calculator
Based on the following HDR-based estimation equation, an estimated bias for each motion state of the traveling robot is calculated.
[Estimation formula]
Figure pat00119
,
Figure pat00120

(only,
Figure pat00121
Is a bias drift estimation value, x is a gyro output value,
Figure pat00122
Constant)
The method according to claim 1,
And a switching unit for selectively blocking the execution of the integration operation for the traveling angle estimation according to the motion state of the traveling robot identified through the bias estimating unit,
And estimating the traveling angle of the traveling robot.
9. The method of claim 8,
The switching unit
Wherein when the motion state of the traveling robot is in a stop section, the integration operation is stopped to prevent integration errors from accumulating, and the bias correction apparatus for estimating the traveling angle of the traveling robot.
A control method for estimating the traveling angle of a traveling robot,
Measuring a rotational angular velocity of the gyro of the traveling robot;
Measuring a pulse according to rotation of the traveling wheel of an encoder mounted on a traveling wheel of the traveling robot;
The bias estimating unit of the traveling robot counts the pulses measured through the encoder to identify the motion state of the traveling robot and expands the interval in which the bias of the gyro can be estimated according to the motion state of the traveling robot, Estimating a bias by each bias; And
Wherein the bias correction unit of the traveling robot removes the estimated bias calculated through the bias estimation unit from the output value measured through the gyro and corrects
And estimating the traveling angle of the traveling robot.
11. The method of claim 10,
The step of estimating the bias for each exercise state comprises:
The bias estimating unit of the traveling robot counts the pulses measured by the encoder and identifies the motion state of the traveling robot according to the counted number;
Adjusting a gain value of a heuristic drift reduction (HDR) -based filter by the bias estimator according to whether the identified motion state is a stop section or a straight section; And
Calculating a bias from an angular velocity value corresponding to the stationary section or the straight section of the angular velocity value measured by the bias estimating section through the gyro;
And estimating the traveling angle of the traveling robot.
12. The method of claim 11,
Wherein identifying the motion state comprises:
Wherein the bias estimating unit identifies a motion state of stopping, straightening, and turning of the traveling robot according to the counted number of pulses measured through the encoder,
When the number of counts of the pulses output from the two left and right encoders is all 0, the number of counts of the pulses output from the two encoders is not zero, Wherein the step of recognizing the straight line is based on whether or not the absolute value of the difference between the number of counting times exceeds a threshold value.
12. The method of claim 11,
The step of adjusting the gain value comprises:
The bias estimator determines whether the angular velocity value corresponding to the stop, straight line, and rotation interval of the angular velocity values measured through the gyro is equal to or less than the reference angular velocity threshold value, in addition to the identification value output through the motion state identification unit And calculates a gain value for a period in which all of the conditions are satisfied. 6. A bias correction method for estimating a traveling angle of a traveling robot, comprising:
12. The method of claim 11,
The step of adjusting the gain value comprises:
And calculating a gain for each zone using the following equation: < EMI ID = 1.0 >
[Mathematical Expression]
Figure pat00123

(only,
Figure pat00124
Is a gain value,
Figure pat00125
Is an identification value,
Figure pat00126
Is the angular velocity value corresponding to the stop, straight and rotation section through the gyro, and 0.7 deg / s is the reference angular velocity threshold value for determining the fine rotation occurring in the straight section.)
12. The method of claim 11,
The step of calculating the bias includes:
Wherein the estimated bias is calculated for each motion state of the traveling robot using the following HDR-based estimation formula.
[Estimation formula]
Figure pat00127
,
Figure pat00128

(only,
Figure pat00129
Is a bias drift estimation value, x is a gyro output value,
Figure pat00130
Constant)
11. The method of claim 10,
Before estimating the traveling angle of the traveling robot,
And stopping the integration operation for estimating the traveling angle when the traveling state of the traveling robot identified by the switching unit of the traveling robot through the bias estimating unit is in the stopping period to prevent accumulation of the integration error
And estimating the traveling angle of the traveling robot.
KR1020150175190A 2015-12-09 2015-12-09 Bias correcting apparatus for yaw angle estimation of mobile robots and method thereof KR101789776B1 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
KR1020150175190A KR101789776B1 (en) 2015-12-09 2015-12-09 Bias correcting apparatus for yaw angle estimation of mobile robots and method thereof

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
KR1020150175190A KR101789776B1 (en) 2015-12-09 2015-12-09 Bias correcting apparatus for yaw angle estimation of mobile robots and method thereof

Publications (2)

Publication Number Publication Date
KR20170068234A true KR20170068234A (en) 2017-06-19
KR101789776B1 KR101789776B1 (en) 2017-10-25

Family

ID=59279034

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020150175190A KR101789776B1 (en) 2015-12-09 2015-12-09 Bias correcting apparatus for yaw angle estimation of mobile robots and method thereof

Country Status (1)

Country Link
KR (1) KR101789776B1 (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114260890A (en) * 2020-08-26 2022-04-01 北京市商汤科技开发有限公司 Method and device for determining state of robot, robot and storage medium
CN115717884A (en) * 2022-11-18 2023-02-28 中国科学院长春光学精密机械与物理研究所 Bias flow angular velocity measurement, calculation and verification system of bias flow mechanism

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100772915B1 (en) * 2006-07-05 2007-11-05 삼성전자주식회사 Apparatus and method for correcting bias of gyroscope on a moving robot
KR101115012B1 (en) * 2009-08-28 2012-03-07 부산대학교 산학협력단 Apparatus and Method for Compenating Angular Velocity Error for Robot

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114260890A (en) * 2020-08-26 2022-04-01 北京市商汤科技开发有限公司 Method and device for determining state of robot, robot and storage medium
CN114260890B (en) * 2020-08-26 2023-11-03 北京市商汤科技开发有限公司 Method and device for determining state of robot, robot and storage medium
CN115717884A (en) * 2022-11-18 2023-02-28 中国科学院长春光学精密机械与物理研究所 Bias flow angular velocity measurement, calculation and verification system of bias flow mechanism
CN115717884B (en) * 2022-11-18 2023-08-22 中国科学院长春光学精密机械与物理研究所 Bias angular velocity measurement, calculation and verification system of bias mechanism

Also Published As

Publication number Publication date
KR101789776B1 (en) 2017-10-25

Similar Documents

Publication Publication Date Title
US8873832B2 (en) Slip detection apparatus and method for a mobile robot
US7507948B2 (en) Method of detecting object using structured light and robot using the same
CN110675435A (en) Based on Kalman filtering and chi2Vehicle track tracking method for detecting smoothing processing
KR100886340B1 (en) Apparatus and method for calibrating gyro-sensor of mobile robot
US8855819B2 (en) Method and apparatus for simultaneous localization and mapping of robot
US8000925B2 (en) Moving body with tilt angle estimating mechanism
CN109394095B (en) Robot movement carpet deviation control method, chip and cleaning robot
JP4148276B2 (en) POSITION ESTIMATION DEVICE, POSITION ESTIMATION METHOD, AND PROGRAM RECORDING MEDIUM
JP6153493B2 (en) Roll angle estimation device and transport equipment
KR101338143B1 (en) Apparatus and Method for Detecting Slip of a Mobile Robot
JP5434608B2 (en) Positioning device and positioning method
JP2015022215A5 (en)
KR101115012B1 (en) Apparatus and Method for Compenating Angular Velocity Error for Robot
KR101789776B1 (en) Bias correcting apparatus for yaw angle estimation of mobile robots and method thereof
KR100946676B1 (en) System and method for position estimation using enconders and accelerometers
JPWO2017057052A1 (en) Information processing apparatus, information processing method, and program
KR20050046298A (en) Method and apparatus for calibration of heading direction in mobile subject
KR102303230B1 (en) Apparatus and method for estimating radius of curvature in vehicle
JP2006031313A (en) Method and apparatus for measuring obstacle
EP3583021B1 (en) System for estimating the slope of a pedal-assisted bicycle
JP2014215232A (en) Yaw rate detection apparatus
WO2014171227A1 (en) Attitude angle estimation device and movement state detection device provided with same
US9862414B2 (en) Method and device for compensating a steering wheel angle signal, which is subject to an offset, in a vehicle
KR101376536B1 (en) Position Recognition Method for mobile object using convergence of sensors and Apparatus thereof
US11300426B2 (en) Encoder

Legal Events

Date Code Title Description
A201 Request for examination
E902 Notification of reason for refusal
N231 Notification of change of applicant
E701 Decision to grant or registration of patent right
GRNT Written decision to grant