JP2011008637A - Robot control apparatus, robot control program and robot control method - Google Patents
Robot control apparatus, robot control program and robot control method Download PDFInfo
- Publication number
- JP2011008637A JP2011008637A JP2009152957A JP2009152957A JP2011008637A JP 2011008637 A JP2011008637 A JP 2011008637A JP 2009152957 A JP2009152957 A JP 2009152957A JP 2009152957 A JP2009152957 A JP 2009152957A JP 2011008637 A JP2011008637 A JP 2011008637A
- Authority
- JP
- Japan
- Prior art keywords
- robot
- turning
- wheel
- turn
- error
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims description 67
- 238000005259 measurement Methods 0.000 claims abstract description 45
- 238000004364 calculation method Methods 0.000 claims description 59
- 238000012937 correction Methods 0.000 claims description 48
- 238000010586 diagram Methods 0.000 description 17
- 230000005856 abnormality Effects 0.000 description 16
- 230000008859 change Effects 0.000 description 13
- 238000012545 processing Methods 0.000 description 8
- 230000008569 process Effects 0.000 description 7
- 230000000694 effects Effects 0.000 description 5
- 239000000284 extract Substances 0.000 description 5
- 238000003384 imaging method Methods 0.000 description 5
- 230000006870 function Effects 0.000 description 4
- 239000011159 matrix material Substances 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000000691 measurement method Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 238000007796 conventional method Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
Abstract
Description
本発明は、ロボット制御装置、ロボット制御プログラムおよびロボット制御方法に関する。 The present invention relates to a robot control device, a robot control program, and a robot control method.
近年、２輪駆動の移動ロボットが自律走行等を行う際、自己の位置を車輪の回転量によって推定するオドメトリの技術が利用されている。オドメトリとは、２つの車輪の各回転速度からロボットの併進速度およびロボット本体の角速度を求め、これらを積分してロボットの位置および姿勢を推定する技術である。 In recent years, when a two-wheel drive mobile robot performs autonomous traveling or the like, an odometry technique for estimating its own position based on the amount of rotation of a wheel has been used. Odometry is a technique for estimating the robot translation speed and the robot body angular speed from the rotational speeds of two wheels, and integrating these to estimate the position and orientation of the robot.
かかるオドメトリの技術では、２つの車輪の各半径および２つの車輪間の距離が、ロボットの自己位置推定の精度を左右する重要なパラメータとなる。 In such odometry technology, the radii of two wheels and the distance between the two wheels are important parameters that determine the accuracy of the self-position estimation of the robot.
ところが、２つの車輪のどちらか一方若しくは両方の車輪の半径、または車輪間の距離が、ロボットの経年の運用または衝突等により変化する場合があり、自己位置推定の精度に影響を及ぼす。また、車輪の速度を制御するギア間の噛み合わせの隙間（バックラッシュ）も自己位置推定誤差の原因となる。 However, the radius of one or both of the two wheels or the distance between the wheels may change due to the operation or collision of the robot over time, which affects the accuracy of self-position estimation. In addition, a gap (backlash) between the gears that controls the speed of the wheels also causes a self-position estimation error.
そこで、従来では、ロボット管理者が、オドメトリに関わるパラメータの誤差を定期的に点検して、誤差に応じたパラメータの補正や部品の交換を行っていた。 Therefore, conventionally, a robot administrator regularly checks for errors in parameters related to odometry, and corrects parameters or replaces parts according to the errors.
また、２輪駆動の移動ロボットをスタート位置から特定距離にあるゴール位置に向けて走行させて、走行後に撮像した目印の撮像画像とあらかじめ撮像した同一目印の画像とを用いて、当該移動ロボットのオドメトリに関わるパラメータの誤差を調整するパラメータ調整装置が開示されている。 In addition, a two-wheel drive mobile robot is caused to travel toward a goal position at a specific distance from the start position, and an image of the mark captured after the travel and an image of the same mark captured in advance are used. A parameter adjustment device for adjusting an error of a parameter related to odometry is disclosed.
しかしながら、移動ロボットの自己位置推定に関わる誤差を調整する従来の技術では、当該誤差を正確且つ効率的に調整することができないという問題がある。 However, the conventional technique for adjusting the error related to the self-position estimation of the mobile robot has a problem that the error cannot be adjusted accurately and efficiently.
すなわち、従来のパラメータ調整装置では、走行後に目印を撮像する場合、目印までの撮像距離等の撮像条件が変わると撮像精度も変わってしまい、撮像誤差によりオドメトリに関わるパラメータの誤差を正確に特定することができない。その結果、パラメータ調整装置は、オドメトリに関わるパラメータの誤差を正確に調整することができない。 That is, in the conventional parameter adjustment device, when imaging a mark after traveling, the imaging accuracy also changes if the imaging conditions such as the imaging distance to the mark change, and the parameter error related to odometry is accurately specified by the imaging error. I can't. As a result, the parameter adjustment device cannot accurately adjust the error of the parameter related to odometry.
また、ロボット管理者による誤差の調整では、作業時間を多く要し、コスト高となってしまう。 In addition, the adjustment of the error by the robot administrator requires a lot of work time and increases the cost.
本発明は、上記に鑑みてなされたものであって、移動ロボットの自己位置推定に関わる誤差を正確且つ効率的に調整することができるロボット制御装置、ロボット制御プログラムおよびロボット制御方法を提供することを目的とする。 The present invention has been made in view of the above, and provides a robot control device, a robot control program, and a robot control method capable of accurately and efficiently adjusting an error related to self-position estimation of a mobile robot. With the goal.
上述した問題を解決し、目的を達成するために、ロボット制御装置は、ロボット本体の旋回開始位置から所定位置を中心にして、当該ロボット本体を旋回前後の位置が近似した位置となるように所定角度旋回させる旋回指示を検出すると、当該ロボット本体を移動させる２つの車輪の回転動作に応じて、当該ロボット本体の旋回を開始する移動制御部と、当該ロボット本体から離間した位置にある目印を基準にして、当該ロボット本体の旋回前後に関わるロボット位置およびロボット方位を計測する計測部と、前記計測部にて計測した前記旋回前後に関わるロボット位置間の差分および前記旋回前後に関わるロボット方位間の差分に基づき、前記車輪に関わる誤差を算出する算出部とを備える構成を採る。 In order to solve the above-described problems and achieve the object, the robot control device sets a predetermined position from the turning start position of the robot body so that the position before and after the turning of the robot body is approximated. When a turning instruction to turn the angle is detected, a movement control unit that starts turning of the robot main body according to the rotational operation of the two wheels that move the robot main body, and a mark that is located away from the robot main body And measuring the robot position and the robot orientation related to before and after turning of the robot body, the difference between the robot position related to before and after the turn measured by the measuring unit, and between the robot orientation related to before and after the turn A configuration is provided that includes a calculation unit that calculates an error related to the wheel based on the difference.
以上により、ロボット制御装置は、ロボットの車輪に関わる誤差を正確且つ効率的に算出することができ、当該誤差の調整精度を向上させることができるという効果を奏する。 As described above, the robot control apparatus can accurately and efficiently calculate an error related to the robot wheel, and has an effect of improving the adjustment accuracy of the error.
以下に、本発明に係るロボット制御装置、ロボット制御プログラムおよびロボット制御方法の実施例を図面に基づいて詳細に説明する。なお、本実施例によりこの発明が限定されるものではない。 Hereinafter, embodiments of a robot control device, a robot control program, and a robot control method according to the present invention will be described in detail with reference to the drawings. In addition, this invention is not limited by the present Example.
図１は、本実施例１に係るロボット制御装置の構成を示す機能ブロック図である。図１に示すように、ロボット制御装置１は、移動制御部１０、計測部２０および算出部３０を有している。
FIG. 1 is a functional block diagram illustrating the configuration of the robot control apparatus according to the first embodiment. As illustrated in FIG. 1, the
ロボット制御装置１は、車輪に関わるパラメータを用いてロボット本体の自己位置を推定する装置あり、特にオドメトリによる自己位置推定を行う。
The
移動制御部１０は、ロボット本体の旋回開始位置から所定位置を中心にして、当該ロボット本体を旋回前後の位置が近似した位置となるように所定角度旋回させる旋回指示を検出すると、当該ロボット本体を移動させる２つの車輪の回転動作に応じて、当該ロボット本体の旋回を開始する。ここで、所定角度とは、例えば３６０度や３６０度の複数倍であるがこれに限定されず、１度や２度等旋回前後の位置が近似した位置となるような角度であれば良い。
When the
計測部２０は、ロボット本体から離間した位置にある目印（以下、「ランドマーク」という。）を基準にして、当該ロボット本体の旋回前後に関わるロボット位置およびロボット方位を計測する。ここで、ロボット本体の旋回前後に関わるロボット位置およびロボット方位の計測について、図２を参照して説明する。図２は、実施例１に係るロボット制御装置１のロボット位置およびロボット方位の計測を説明するための図である。
The
移動ロボットは、旋回前に所定のランドマークを抽出する。そして、移動ロボットは、ロボット本体の旋回開始位置から、抽出されたランドマークに対して相対位置および相対方位を計測して、ランドマークに対する自己のロボット位置およびロボット方位とする。なお、計測方法として、例えば２台のステレオカメラをロボットに搭載してステレオ視覚を用いて計測する方法があるが、これに限定されるものではない。また、ランドマークには、目印として特徴があり計測しやすい標的であれば良く、例えばロボットの充電機がある。 The mobile robot extracts a predetermined landmark before turning. Then, the mobile robot measures the relative position and relative orientation with respect to the extracted landmark from the turning start position of the robot body, and sets it as its own robot position and robot orientation with respect to the landmark. As a measurement method, for example, there is a method in which two stereo cameras are mounted on a robot and measurement is performed using stereo vision. However, the measurement method is not limited thereto. The landmark may be any target that has a characteristic as a mark and is easy to measure, such as a robot charger.
次に、移動ロボットは、ロボット本体の旋回開始位置から所定位置を中心にして、旋回前後の位置が近似した位置となる所定角度、例えば３６０度旋回する。そして、移動ロボットは、旋回後の旋回終了位置から、旋回前と同じランドマークに対して相対位置および相対方位を計測して、ランドマークに対する自己のロボット位置およびロボット方位とする。 Next, the mobile robot turns around a predetermined position from the turning start position of the robot body at a predetermined angle, for example, 360 degrees, at which the position before and after the turn is approximate. Then, the mobile robot measures the relative position and relative azimuth with respect to the same landmark as before the turn from the turn end position after the turn, and sets it as its own robot position and robot azimuth with respect to the landmark.
ここで、移動ロボットは、例えば所定角度を３６０度とした場合、３６０度旋回指示に基づいて旋回しているため、車輪に関わる誤差がなければ、３６０度旋回後には旋回前の位置および方位に戻ることができる。ところが、移動ロボットは、車輪に関わる誤差があると、３６０度旋回後には旋回前の位置および方位に戻ることができない。すなわち、旋回後のランドマークに対する自己のロボット位置は、旋回前のランドマークに対する自己のロボット位置と同一にならない。そして、旋回後のロボット方位も同様に旋回前のロボット方位と同一にならない。 Here, for example, when the predetermined angle is set to 360 degrees, the mobile robot is turning based on a 360-degree turning instruction. Therefore, if there is no error related to the wheels, the position and orientation before turning are set after 360-degree turning. You can go back. However, if there is an error related to the wheel, the mobile robot cannot return to the position and direction before the turn after turning 360 degrees. That is, the position of the robot with respect to the landmark after the turn is not the same as the position of the robot with respect to the landmark before the turn. Similarly, the orientation of the robot after turning is not the same as the orientation of the robot before turning.
そこで、旋回前後のロボット位置およびロボット方位を用いて、車輪に関わる誤差を、後記する算出部３０によって算出する。
Therefore, using the robot position and the robot orientation before and after the turn, an error relating to the wheel is calculated by the
図１に戻って、算出部３０は、計測部２０にて計測した旋回前後に関わるロボット位置間の差分および旋回前後に関わるロボット方位間の差分に基づき、車輪に関わる誤差を算出する。
Returning to FIG. 1, the
上記実施例１によれば、ロボット制御装置１は、ロボット本体を旋回前後の位置が近似した位置となるような所定角度旋回させてロボット本体の位置等を計測しているため、旋回前後のロボット本体の位置が近似し、旋回前後の計測条件の変動も小さく済むこととなる。そのため、ロボット制御装置１は、ロボットの車輪に関わる誤差を正確に算出することができ、当該誤差の調整精度を向上させることができる。
According to the first embodiment, the
また、ロボット制御装置１は、ロボットの車輪に関わる誤差を自動的に算出することができ、その後、当該誤差を補正することにより効率的な調整を行うことができる。
In addition, the
ところで、移動ロボットのオドメトリによる自己位置推定は、運動学モデルを用いて行われる。この運動学モデルについて、図３を参照して説明する。図３は、移動ロボット運動学モデルの説明を示す図である。 By the way, self-position estimation by odometry of a mobile robot is performed using a kinematic model. This kinematic model will be described with reference to FIG. FIG. 3 is a diagram illustrating a mobile robot kinematic model.
まず、移動ロボットの運動学モデルを説明するための定義を行う。ロボットは、２つの車輪を有するものとする。そして、ロボットの位置および方位は、座標軸をＸ軸（ＸＧ）およびＹ軸（ＹＧ）とした絶対座標系と、座標軸をＸ軸（ＸＲ）およびＹ軸（ＹＲ）として且つロボットの中心を原点としたロボット座標系とから表される。ロボットの位置及び方位は、図３の例では、絶対座標系では（ｘＧ、ｙＧ、θＧ）で表され、ロボット座標系では（０、０、θ）で表される。ここで、θＧは、絶対座標系の原点からロボットに対する絶対方位であり、θは、ロボット座標系の原点からロボットが視ている相対方位、すなわちロボットの姿勢を指す。 First, a definition for explaining a kinematic model of a mobile robot is made. The robot is assumed to have two wheels. The position and orientation of the robot are determined with the absolute coordinate system having the coordinate axes as the X axis (X G ) and the Y axis (Y G ), the coordinate axes as the X axis (X R ), and the Y axis (Y R ). It is expressed from the robot coordinate system with the center as the origin. Position and orientation of the robot, in the example of FIG. 3, the absolute coordinate system is represented by (x G, y G, θ G), a robot coordinate system represented by (0,0, theta). Here, θ G is an absolute azimuth with respect to the robot from the origin of the absolute coordinate system, and θ represents a relative azimuth viewed by the robot from the origin of the robot coordinate system, that is, the posture of the robot.
また、ロボットの２つの車輪は、それぞれ右車輪の半径をｒ１（単位：ｍ、以下同じ）、左車輪の半径をｒ２とし、両車輪間の距離をl（単位：ｍ）とし、これらが運動学モデルでは重要な、車輪に関わるパラメータとなる。 The two wheels of the robot have a right wheel radius of r 1 (unit: m, the same applies hereinafter), a left wheel radius of r 2 and a distance between both wheels of l (unit: m). Is an important parameter related to wheels in kinematic models.
さらに、車輪の回転速度は、それぞれ右車輪の回転速度をφ１（単位：ｒａｄ／秒、以下同じ）、左車輪の回転速度をφ２とする。また、ロボット本体の前進速度をｖ（単位：ｍ／秒）、ロボット本体の回転速度をω（単位：ｍ／秒）とする。 Further, regarding the rotation speed of the wheel, the rotation speed of the right wheel is φ 1 (unit: rad / second, the same applies hereinafter), and the rotation speed of the left wheel is φ 2 . Further, the forward speed of the robot body is v (unit: m / second), and the rotational speed of the robot body is ω (unit: m / second).
例えば、正転方向にφ１の回転速度で右車輪を回転させ、正転方向と反対方向に−φ１の回転速度で左車輪を回転させると、ロボット本体は（ｘＧ、ｙＧ）を中心にその場で旋回して、ロボット本体の回転速度ωが発生する。また、φ１＝φ２の回転速度で右車輪および左車輪を回転させると、ロボット本体の回転速度ωは発生しないが、ロボットの前進速度ｖが発生する。つまり、左右車輪の回転速度φ１およびφ２によってロボット本体の絶対座標が決定されることになる。 For example, by rotating the right wheel in phi 1 of the rotational speed in the forward direction, to rotate the left wheel in the direction opposite to the forward direction at a rotational speed of -.phi 1, the robot main body (x G, y G) The robot body turns at the spot to generate the rotational speed ω of the robot body. Further, when the right wheel and the left wheel are rotated at a rotational speed of φ 1 = φ 2 , the rotational speed ω of the robot main body is not generated, but the robot forward speed v is generated. That is, the absolute coordinates of the robot body are determined by the rotational speeds φ 1 and φ 2 of the left and right wheels.
このような定義の下、移動ロボットの運動学モデルでは、絶対座標系で観測されるロボットの状態、すなわちロボットの絶対位置（ｘ、ｙ）および絶対方位θを式（１）および（２）のように表すことができる。なお、式（２）では、ロボットの状態をロボットの速度として表している。
式（２）により、ロボットの絶対位置および絶対方位（ｘＧ、ｙＧ、θＧ）は、左右車輪の回転速度φ１およびφ２以外に車輪に関わるパラメータとなる車輪の半径ｒ１、ｒ２および両車輪間の距離ｌによって決定される。 According to the equation (2), the absolute position and the absolute orientation (x G, y G , θ G ) of the robot are determined by the wheel radii r 1 , r which are parameters related to the wheels other than the rotational speeds φ 1 and φ 2 of the left and right wheels 2 and the distance l between the two wheels.
そして、ロボット座標系から絶対座標系へのロボットの速度に関する変換は、式（４）で表される回転変換の行列を用いて以下の式（３）のようになる。
さらに、ロボット座標系でのロボットの右車輪の回転速度ω１は、右車輪の回転速度φ１を用いて、以下の式（５）のようになる。また、ロボットの左車輪の回転速度ω２は、左車輪の回転速度φ２を用いて、以下の式（６）のようになる。
そして、車輪の回転速度から導出されロボットの自己位置を推定する際に使用される運動方程式は、回転変換の行列（８）を用いて式（７）のようになる。
上記した運動方程式によると、運動方程式に設定された車輪の半径ｒ１、ｒ２または両車輪間の距離ｌが、実際の車輪の半径または両車輪間の距離とそれぞれ異なると、移動後のロボットの絶対位置および方位（ｘＧ、ｙＧ、θＧ）が不正確となる。例えば、時間の経過による摩擦、さらには衝突等によって車輪の半径または車輪間の距離が変化することがある。その場合、運動学モデル（運動方程式）にパラメータとして設定された車輪の半径または車輪間の距離と実際の車輪の半径または車輪間の距離との間でそれぞれ誤差が生じることとなり、その結果、運動学モデルを用いた自己位置推定に誤差が生じることになる。したがって、車輪の半径または車輪間の距離が変化した場合には、運動学モデルにパラメータとして設定される車輪の半径または車輪間の距離を含む車輪に関わるパラメータを補正することが重要となる。 According to the above equation of motion, if the wheel radius r 1 , r 2 or the distance l between both wheels set in the equation of motion is different from the actual wheel radius or the distance between both wheels, the robot after movement absolute position and orientation (x G, y G, θ G) is inaccurate. For example, the radius of the wheel or the distance between the wheels may change due to friction over time, or even a collision. In that case, an error occurs between the wheel radius or the distance between the wheels set as a parameter in the kinematic model (the equation of motion) and the actual wheel radius or the distance between the wheels. An error will occur in the self-position estimation using the scientific model. Therefore, when the wheel radius or the distance between the wheels changes, it is important to correct the wheel-related parameters including the wheel radius or the distance between the wheels set as parameters in the kinematic model.
そこで、実施例２では、車輪に関わるパラメータのうち車輪の半径および車輪間の距離に関わる誤差を算出して、算出された誤差に基づき、当該車輪に関わるパラメータを補正する場合について説明する。 Therefore, in the second embodiment, a case will be described in which an error related to a wheel radius and a distance between wheels is calculated among parameters related to a wheel, and the parameter related to the wheel is corrected based on the calculated error.
図４は、本実施例２に係るロボット制御装置２の構成を示す機能ブロック図である。なお、図１に示すロボット制御装置１と同一の構成については同一符号を付すことで、その重複する構成および動作の説明については省略する。
FIG. 4 is a functional block diagram illustrating the configuration of the
図４に示すロボット制御装置２と実施例１に係るロボット制御装置１とが異なるところは、車輪に関わる誤差を算出する算出部３０に旋回後絶対座標算出部３１および誤差算出部３２を追加し、指示部４０、記憶部５０、補正部６０および報知部７０を追加した点にある。
The difference between the
指示部４０は、車輪に関わる誤差を補正するよう指示する機能部であり、パラメータ誤差補正指示部４１を有する。
The
パラメータ誤差補正指示部４１は、計測可能なランドマークを抽出後、車輪に関わるパラメータの誤差を補正部６０に補正させるために、所定位置を中心にして当該ロボット本体を３６０度旋回させる旋回指示を移動制御部１０に対して行う。なお、パラメータ誤差補正指示部４１は、計測可能なランドマークを抽出できない場合には、場所を移動して計測可能なランドマークを抽出後、当該旋回指示を行う。なお、ランドマークは、ロボット本体から離間した位置にあるものとする。
The parameter error
移動制御部１０は、所定位置を中心にしてロボット本体を３６０度旋回させる旋回指示をパラメータ誤差補正指示部４１から取得すると、ロボット本体を旋回させるために必要な両車輪の回転速度を記憶部５０から読み出す。また、移動制御部１０は、記憶部５０から読み出された両車輪の回転速度に基づいて、所定位置を中心にロボット本体を３６０度旋回させる場合に要する動作時間を算出する。この動作時間の算出は、例えばノミナル運動モデルを用いて算出される。
When the
また、移動制御部１０は、ロボット本体の旋回前に車輪内のギアによるバックラッシュを除去する動作を行う。具体的には、移動制御部１０は、ロボット本体の旋回を開始する前に、車輪内の第１ギア部および第２ギア部同士を噛合わせて第１ギア部および第２ギア部をロボット本体の旋回方向にゆっくりと回転させる。そして、移動制御部１０は、第１ギア部の旋回方向側の噛合部位と第２ギア部の噛合部位とが完全に接触した状態で、ロボット本体の旋回を開始する。
In addition, the
なお、バックラッシュとは、車輪内の噛合っている第１ギア部と第２ギア部との噛合わせの隙間（遊び部分）のことである。このように、ロボット本体の旋回前にバックラッシュを除去することにより、バックラッシュによる車輪の回転量の誤差を排除することができ、車輪に関わるパラメータの誤差をより正確に算出することができる。 The backlash is a meshing gap (play portion) between the meshed first gear portion and the second gear portion in the wheel. Thus, by removing the backlash before turning the robot body, it is possible to eliminate the error in the rotation amount of the wheel due to the backlash, and to calculate the parameter error relating to the wheel more accurately.
また、移動制御部１０は、ロボット本体の旋回前（バックラッシュ除去後）およびロボット本体の旋回後に、ロボット本体の位置および方位を計測するように計測部２０に指示する。
In addition, the
さらに、移動制御部１０は、バックラッシュ除去後の旋回開始位置から所定位置を中心にして、算出された動作時間分、ロボット本体を旋回させる。
Further, the
計測部２０は、パラメータ誤差補正指示部４１によって抽出されたランドマークを基準にして、当該ロボット本体の旋回前後に関わるロボット位置およびロボット方位を計測する。具体的には、計測部２０は、ロボット本体の旋回前に、ロボット本体の位置および方位を計測する旨の指示を移動制御部１０から取得すると、ロボット本体の旋回開始位置からランドマークに対して相対位置および相対方位を計測する。そして、計測部２０は、計測した相対位置および相対方位を、ランドマークに対する自己のロボット位置およびロボット方位として記憶部５０に格納する。
The measuring
また、計測部２０は、ロボット本体の旋回後にロボット本体の位置および方位を計測する旨の指示を移動制御部１０から取得すると、ロボット本体の旋回終了位置からランドマークに対して相対位置および相対方位を計測する。そして、計測部２０は、計測した相対位置および相対方位を、ランドマークに対する自己のロボット位置およびロボット方位として記憶部５０に格納する。
In addition, when the
記憶部５０は、車輪に関わるパラメータの誤差を算出するために必要な値を記憶する機能部である。具体的には、記憶部５０は、ロボット本体の旋回前後のロボット位置およびロボット方位を記憶する。また、記憶部５０は、車輪に関わるパラメータ内の複数の要素、すなわち、両車輪の各半径、両車輪間の距離および両車輪の各回転速度をあらかじめ記憶する。
The memory |
旋回後絶対座標算出部３１は、計測部２０によって計測された旋回前後のランドマークに対する自己のロボット位置および旋回開始位置に基づき、旋回後のロボットの絶対位置（旋回終了位置）を算出する。なお、旋回開始位置は、絶対座標系上の絶対位置を指す。
The after-turning absolute coordinate
また、旋回後絶対座標算出部３１は、計測部２０によって計測された旋回前後のランドマークに対する自己のロボット方位および旋回開始位置でのロボットの絶対方位に基づき、旋回終了位置でのロボットの絶対方位を算出する。なお、旋回終了位置でのロボット本体の絶対座標（絶対位置および絶対方位）を算出する方法は後に詳述するものとする。
The absolute coordinate
誤差算出部３２は、旋回後絶対座標算出部３１によって算出されたロボットの旋回前後の絶対座標の差分に基づき、車輪の半径が変化したことによる誤差または車輪間の距離が変化したことによる誤差を算出する。例えば、誤差算出部３２は、車輪の半径に関する誤差として、実際の左右車輪の半径の比率を算出する。また、誤差算出部３２は、車輪間の距離に関する誤差として、パラメータに設定された車輪間距離に対する実際の車輪間距離の比率を算出する。
Based on the difference between the absolute coordinates before and after the turn of the robot calculated by the absolute coordinate
ここで、車輪に関わるパラメータ内の車輪の半径または車輪間の距離に関わる誤差の算出方法について、図５および図６を参照して説明する。図５は、座標の定義を示す図であり、図６は、旋回後のロボット本体の絶対座標を算出する方法を示す図である。 Here, a method of calculating an error relating to a wheel radius or a distance between wheels in parameters relating to the wheel will be described with reference to FIGS. 5 and 6. FIG. 5 is a diagram illustrating the definition of coordinates, and FIG. 6 is a diagram illustrating a method of calculating absolute coordinates of the robot body after turning.
まず、図５では、ロボットから離間した位置にあるランドマークと、旋回中心に旋回した旋回前後のロボットが絶対座標系上に表されている。ロボット本体の旋回開始位置の絶対座標は、Ｇ０（ｘ０、ｙ０、θ０）である。 First, in FIG. 5, landmarks that are located away from the robot and the robot before and after turning that turns around the turning center are shown on the absolute coordinate system. The absolute coordinates of the turning start position of the robot body is G 0 (x 0, y 0 , θ 0 ).
また、旋回前のロボット本体のランドマークに対する相対位置および相対方位は、相対座標Ｍ０（ｘｍ０、ｙｍ０、θｍ０）として定義される。旋回中心を中心に旋回開始位置から３６０度旋回後のロボット本体のランドマークに対する相対位置および相対方位は、相対座標Ｍ１（ｘｍ１、ｙｍ１、θｍ１）として定義される。 In addition, the relative position and relative direction with respect to the landmark of the robot body before turning are defined as relative coordinates M 0 (x m0, y m0 , θ m0 ). A relative position and a relative direction with respect to the landmark of the robot body after 360 degrees of turning from the turning start position around the turning center are defined as relative coordinates M 1 (x m1, y m1 , θ m1 ).
図５における座標の定義の下、相対座標Ｍ０およびＭ１および旋回前のロボット本体の絶対座標Ｇ０（ｘ０、ｙ０、θ０）に基づいて、旋回後のロボット本体の絶対座標Ｇ１（ｘ１、ｙ１、θ１）が算出される。 Under the definition of coordinates in FIG. 5, based on the relative coordinates M 0 and M 1 and the absolute coordinates G 0 (x 0, y 0 , θ 0 ) of the robot body before turning, the absolute coordinates G of the robot body after turning 1 (x 1, y 1 , θ 1 ) is calculated.
図６は、旋回後のロボット本体の絶対座標を算出する方法を示す図である。 FIG. 6 is a diagram illustrating a method of calculating absolute coordinates of the robot body after turning.
まず、旋回前のロボット本体の絶対方位と、旋回前のロボット本体のランドマークに対する相対方位とに基づいて、式（９）に示すようにランドマークの絶対方位を算出する。
次に、ランドマークの絶対方位と、旋回後のロボット本体の絶対方位および旋回後のロボット本体のランドマークに対する相対方位との関係は、式（１０）のようになる。
そうすると、式（９）および式（１０）によって、式（１１）に示すように、旋回後のロボット本体の絶対方位は、式（１１）に示すように算出できる。
また、旋回前のロボット本体の絶対位置および旋回前のロボット本体のランドマークに対する相対位置に基づいて、式（１２）に示すようにランドマークの絶対位置を算出する。なお、相対位置を絶対座標系に変換するために、回転変換の行列（１３）を使用する。
次に、ランドマークの絶対位置と、旋回後のロボット本体の絶対位置および旋回後のロボット本体のランドマークに対する相対位置との関係は、式（１４）のようになる。
そうすると、式（１２）および式（１４）によって、式（１５）に示すように旋回後のロボット本体の絶対位置は、式（１５）に示すように算出できる。
ここで、移動ロボットの運動学モデルにより、車輪の回転速度が常数の場合、ロボットの動作時間ｔに応じてロボットの絶対方位θは、式（１６）より式（１７）に変換して表わすことができる。なお、旋回前のロボットの絶対座標は（０、０、０）であるものとする。また、φ１およびφ２はそれぞれ右車輪および左車輪の回転速度、ｒ１およびｒ２はそれぞれ右車輪および左車輪の半径、ｌは両車輪間の距離を指すものとする。
また、同様に、ロボットの絶対位置ｘは、式（１８）より式（１９）に変換して表わすことができる。
さらに、ロボットが３６０度旋回した後ではロボットの方位（姿勢）が２π付近となるため、ｓｉｎ（ｘ）をｘで代入すると、３６０度旋回後のロボットの絶対方位および絶対位置は、式（２０）および（２１）により表わすことができる。なお、Ｃは、２πを指すものとする。
ここで、両車輪の回転速度を常数にしてロボット本体を３６０度旋回させるという条件により、以下に示すように変数を定義することができる。 Here, variables can be defined as shown below under the condition that the rotational speed of both wheels is a constant and the robot body is turned 360 degrees.
例えば、車輪の半径が変化したことによる誤差ｅ１を、式（２２）のように、左右車輪の半径の比率で表わすものとする。ここでは、誤差ｅ１は、右車輪の半径ｒ１に対する左車輪の半径ｒ２の比率を指している。
また、車輪間の距離が変化したことによる誤差ｅ２を、式（２３）のように、パラメータに設定された車輪間距離Ｌに対する実際の車輪間距離ｌの比率で表わすものとする。
また、車輪の回転速度φ１、φ２および旋回時間ｔは、それぞれ式（２４）および式（２５）のように表わされる。なお、φは、あらかじめ定められた回転速度であるものとする。また、旋回時間ｔは、ロボット本体が３６０度旋回するのに要する動作時間Ｔである。
これらの変数を式（２０）および（２１）に代入し、さらにロボット旋回前後の絶対方位の差分を式（２０）の左辺に代入し、ロボット旋回前後の絶対位置の差分を式（２１）の左辺に代入して、車輪の半径に関わる誤差ｅ１および車輪間距離に関わる誤差ｅ２を算出する。すなわち、式（１１）によって算出されたロボット旋回後の絶対方位θR_new（以降、θ１とする。）を式（２０）の左辺に代入し、式（１５）によって算出されたロボット旋回後の絶対位置ＰR_new（以降、ｘ１とする。）を式（２１）の左辺に代入する。これにより、ロボット本体が３６０度旋回するのに要する動作時間Ｔだけ旋回した結果、旋回後の絶対座標が旋回前の絶対座標（０、０、０）との差分になり、差分に応じた誤差（ｅ１およびｅ２）を算出できる。 Substituting these variables into equations (20) and (21), substituting the difference in absolute azimuth before and after turning the robot into the left side of equation (20), and calculating the difference in absolute position before and after turning the robot in equation (21) Substituting into the left side, an error e 1 related to the wheel radius and an error e 2 related to the distance between the wheels are calculated. That is, the absolute orientation θ R_new (hereinafter referred to as θ 1 ) after turning the robot calculated by the equation (11) is substituted into the left side of the equation (20), and after the robot turning calculated by the equation (15). absolute position P R_new (hereinafter referred to as x 1.) is substituted into the left-hand side of equation (21). As a result, as a result of turning for the operation time T required for the robot body to turn 360 degrees, the absolute coordinates after turning become a difference from the absolute coordinates (0, 0, 0) before turning, and an error corresponding to the difference. (E 1 and e 2 ) can be calculated.
その結果、車輪の半径が変化したことによる誤差ｅ１は、式（２６）によって表され、車輪間の距離が変化したことによる誤差ｅ２は、式（２７）によって表わされる。
このようにして、車輪に関わるパラメータ内の車輪の半径に関する誤差ｅ１および車輪間の距離に関する誤差ｅ２をそれぞれ算出することができる。 In this way, the error e 1 related to the wheel radius and the error e 2 related to the distance between the wheels in the parameters related to the wheel can be calculated.
図４に戻って、補正部６０は、車輪に関わるパラメータを補正する機能部であり、パラメータ補正部６１を有する。
Returning to FIG. 4, the
パラメータ補正部６１は、誤差算出部３２にて算出された誤差に基づき、車輪に関わるパラメータを補正する。例えば、パラメータ補正部６１は、誤差算出部３２にて算出された車輪の半径に関わる誤差ｅ１および車輪間の距離に関わる誤差ｅ２を前述した運動モデルで使用される運動方程式のパラメータに反映させるように補正する。
The
具体的には、式（２６）による車輪の半径が変化したことによる誤差ｅ１を用いて、ロボットが移動する際に使用する運動方程式（式（７））の車輪の半径を補正する。すなわち、運動方程式の左車輪の半径ｒ２を、式（２２）を用いてｅ１×ｒ１に置換する。また、式（２７）による車輪間の距離が変化したことによる誤差ｅ２を用いて、ロボットが移動する際に使用する運動方程式（式（７））の車輪間の距離を補正する。すなわち、運動方程式の両車輪間の距離ｌを、式（２３）を用いてｌ×ｅ２に置換する。以下の式（２８）が補正後の運動方程式となる。
なお、パラメータ補正部６１は、移動制御部１０、計測部２０および算出部３０によって実行される誤差分析を、あらかじめ定められた回数分実行して、誤差分析結果の平均を用いて車輪に関わるパラメータを補正しても良い。
The
報知部７０は、誤差算出部３２にて算出された誤差が異常閾値を超えた場合、車輪の異常を報知する。具体的には、報知部７０は、車輪の半径に関わる誤差または車輪間の距離に関わる誤差のうちいずれか一方でも異常閾値を越えた場合、異常がある旨を、ロボットを管理する計算機システムに送信する。例えば、異常には、車輪の異常、計測機器の異常または組み立て不良による異常を含むものとする。
When the error calculated by the
次に、実施例２に係るロボット制御装置の誤差補正手順を、図７を参照して説明する。図７は、実施例２に係るロボット制御装置の誤差補正手順を示すフローチャートである。 Next, an error correction procedure of the robot control apparatus according to the second embodiment will be described with reference to FIG. FIG. 7 is a flowchart illustrating an error correction procedure of the robot control apparatus according to the second embodiment.
まず、パラメータ誤差補正指示部４１は、計測可能なランドマークを抽出して（ステップＳ１１）、所定位置を中心にしてロボット本体を３６０度旋回させるように移動制御部１０に指示する。
First, the parameter error
すると、移動制御部１０は、ロボット本体の旋回前に車輪内のギアによるバックラッシュを除去する動作を行う（ステップＳ１２）。これにより、移動制御部１０は、バックラッシュによる車輪の回転量の誤差を排除することができるため、車輪に関わるパラメータの誤差をより正確に算出することができる。
Then, the
バックラッシュ除去後、計測部２０は、ロボット本体の旋回開始位置からランドマークに対して相対位置および相対方位を計測して（ステップＳ１３）、ランドマークに対する自己のロボット位置およびロボット方位とする。
After the backlash removal, the
そして、移動制御部１０は、所定位置を中心にして、旋回開始位置からロボット本体を３６０度旋回させる場合に要する動作時間分、ロボット本体を旋回させる（ステップＳ１４）。なお、移動制御部１０は、例えばノミナル運動モデルを使って、記憶部５０に記憶された両車輪の回転速度に基づいた動作時間を算出する。
Then, the
ロボット本体の旋回後、計測部２０は、ロボット本体の旋回終了位置からランドマークに対して相対位置および相対方位を計測して（ステップＳ１５）、ランドマークに対する自己のロボット位置およびロボット方位とする。
After the turning of the robot body, the measuring
引き続き、旋回後絶対座標算出部３１は、旋回前後のランドマークに対する自己のロボット位置およびロボット方位と、旋回開始位置およびロボットの絶対方位とに基づき、旋回後のロボットの絶対位置および絶対方位を算出する。そして、誤差算出部３２は、ロボットの旋回前後の絶対座標（絶対位置、絶対方位）の差分に基づき、車輪に関わるパラメータ内の車輪の半径に関する誤差および車輪間の距離に関する誤差をそれぞれ算出する（ステップＳ１６）。
Subsequently, the absolute coordinate
また、誤差算出部３２は、車輪に関わるパラメータに誤差があるか否かを判定する（ステップＳ１７）。具体的には、誤差算出部３２は、車輪に関わるパラメータ内の車輪の半径に関する誤差および車輪間の距離に関する誤差のうちいずれか一方でも誤差があるか否かを判定する。
Further, the
そして、誤差算出部３２は、車輪に関わるパラメータ内の車輪の半径に関する誤差および車輪間の距離に関する誤差のうちいずれも誤差がない場合（ステップＳ１７Ｎｏ）、誤差補正処理を終了する。
Then, when there is no error in the error relating to the wheel radius and the error relating to the distance between the wheels in the parameters relating to the wheels, the
一方、誤差算出部３２は、車輪に関わるパラメータ内の車輪の半径に関する誤差および車輪間の距離に関する誤差のうちいずれか一方でも誤差がある場合（ステップＳ１７Ｙｅｓ）、当該誤差が異常閾値を超えているか否かを判定する（ステップＳ１８）。
On the other hand, if there is an error in any one of the error relating to the wheel radius and the error relating to the distance between the wheels in the parameter relating to the wheel (Yes in step S17), the
そして、誤差算出部３２は、誤差が異常閾値を超えていない場合（ステップＳ１８Ｎｏ）、車輪の半径に関わる誤差ｅ１および車輪間の距離に関わる誤差ｅ２を前述した運動モデルに用いられる運動方程式のパラメータに反映させるように補正する（ステップＳ１９）。
Then, when the error does not exceed the abnormal threshold (No in step S18), the
一方、誤差算出部３２は、誤差異常閾値を超えている場合（ステップＳ１８Ｙｅｓ）、異常がある旨を、ロボットを管理する計算機システムに報知する（ステップＳ２０）。
On the other hand, if the
上記実施例２によれば、ロボット制御装置２は、ロボット本体の旋回開始位置から所定位置を中心にして、当該ロボット本体を３６０度旋回させる旋回指示を検出すると、当該ロボット本体を移動させる２つの車輪の回転動作に応じて、当該ロボット本体の旋回を開始し、当該ロボット本体から離間した位置にある目印を基準にして、当該ロボット本体の旋回前後に関わるロボット位置及びロボット方位を計測し、計測した旋回前後に関わるロボット位置間の差分及び旋回前後に関わるロボット方位間の差分に基づき、車輪に関わるパラメータ内の車輪の半径に関する誤差および車輪間の距離に関する誤差を算出するようにした。さらに、ロボット制御装置２は、算出した誤差に基づき、車輪に関わるパラメータを補正するようにした。
According to the second embodiment, when the
かかる構成によれば、ロボット制御装置２は、ロボット本体を３６０度旋回させてロボット本体の位置等を計測しているため、旋回前後のロボット本体の位置が近似し、旋回前後の計測条件の変動も小さく済むこととなる。そのため、ロボット制御装置２は、ロボットの車輪に関わるパラメータの誤差を正確に算出することができ、オドメトリによる自己位置推定の調整精度を向上させることができる。
According to such a configuration, since the
また、ロボット制御装置２は、ロボットの車輪に関わるパラメータの誤差を自動的に算出することができ、その後、当該パラメータを補正することによりオドメトリによる自己位置推定の調整を効率的に行うことができる。
Further, the
さらに、ロボット制御装置２は、ロボットを３６０度旋回させる場所と目印とがあれば、車輪に関わるパラメータの誤差を調整することができるため、ロボットを直進させて当該誤差を調整するよりも場所を選ばないで容易に調整することができる。
Furthermore, since the
なお、上記実施例２においては、移動制御部１０は、ロボットから離間した位置を中心にして、ロボット本体を３６０度旋回させるようにした。しかし、移動制御部１０は、ロボット本体の２つの車輪間の中央位置を中心にして、ロボット本体を３６０度旋回させるようにしても同様の効果が得られることは言うまでもない。
In the second embodiment, the
また、上記実施例２においては、パラメータ誤差補正指示部４１は、ロボット本体を３６０度旋回させる旋回指示を行うものとした。しかし、パラメータ誤差補正指示部４１は、誤差を拡大させるために、ロボット本体を３６０度複数回旋回させる旋回指示をしても同様の効果が得られることは言うまでもない。
In the second embodiment, the parameter error
ところで、実施例２では、車輪に関わるパラメータのうち車輪の半径および車輪間の距離に関わる誤差を算出して、算出された誤差に基づき当該車輪に関わるパラメータを補正して、オドメトリによる自己位置推定の調整精度を向上させる場合について説明した。しかし、車輪に関わるパラメータを補正した後、バックラッシュによる間隙距離（以降、「ガタ量」という。）を算出して、算出されたガタ量に基づき当該車輪の回転量を補正して、オドメトリによる自己位置推定の調整精度をさらに向上させるようにしても良く、実施例３として、以下に説明する。 By the way, in the second embodiment, an error related to the wheel radius and the distance between the wheels is calculated among the parameters related to the wheel, the parameter related to the wheel is corrected based on the calculated error, and self-position estimation by odometry is performed. The case where the adjustment accuracy is improved has been described. However, after correcting the parameters related to the wheel, the gap distance due to backlash (hereinafter referred to as “backlash amount”) is calculated, and the rotation amount of the wheel is corrected based on the calculated backlash amount. The adjustment accuracy of the self-position estimation may be further improved, which will be described below as a third embodiment.
図８は、本実施例３に係るロボット制御装置３の構成を示す機能ブロック図である。なお、図４に示すロボット制御装置２と同一の構成については同一符号を付すことで、その重複する構成および動作の説明については省略する。
FIG. 8 is a functional block diagram illustrating the configuration of the robot control apparatus 3 according to the third embodiment. In addition, about the structure same as the
図８に示すロボット制御装置３と実施例２に係るロボット制御装置２とが異なるところは、指示部４０にガタ補正指示部４２、算出部３０にガタ量算出部３３および補正部６０にガタ量補正部６２を追加した点にある。
The robot control device 3 shown in FIG. 8 and the
ガタ補正指示部４２は、計測可能なランドマークを抽出後、ガタ量を補正部６０に補正させるために、所定位置を中心にしてロボット本体を所定角度旋回させる旋回指示を移動制御部１０に対して行う。なお、パラメータ誤差補正指示部４１は、計測可能なランドマークを抽出できない場合には、場所を移動して計測可能なランドマークを抽出後、当該旋回指示を行う。また、ガタ補正指示部４２は、車輪に関わるパラメータを補正した後、連続して旋回指示を行っても良いし、連続しないで改めて旋回指示を行っても良い。
The play
移動制御部１０は、旋回指示をガタ補正指示部４２から取得すると、車輪内の第１ギア部および第２ギア部同士を噛合わせて第１ギア部および第２ギア部をロボット本体の任意方向にゆっくりと回転させて、第１ギア部の任意方向側の噛合部位と第２ギア部の噛合部位とが完全に接触した状態にする。
When the
また、移動制御部１０は、旋回指示に応じてロボット本体を旋回させるために必要な両車輪の回転速度を記憶部５０から読み出す。また、移動制御部１０は、記憶部５０から読み出された両車輪の回転速度に基づいて、所定位置を中心にロボット本体を所定角度旋回させる場合に要する動作時間を算出する。この動作時間の算出は、例えばノミナル運動モデルを使って算出される。
Further, the
なお、移動制御部１０はガタ量の算出を目的としてロボット本体を旋回させるため、所定角度は所定位置を中心に例えば１度から２度程度であれば良い。また、所定位置は、ロボット本体の２つの車輪間の中央を示すロボット中心とするが、これに限定されるものではない。
Since the
また、移動制御部１０は、ロボット本体の旋回前（任意方向にゆっくり回転させた後）および旋回後にロボット本体の位置および方位を計測するように計測部２０に指示する。
In addition, the
さらに、移動制御部１０は、任意方向にゆっくり回転させた後の旋回開始位置からロボット中心を中心にして、算出された動作時間分、ロボット本体を旋回させる。
Further, the
ガタ量算出部３３は、計測部２０によって計測された旋回前後のランドマークに対する自己のロボット方位および旋回開始位置でのロボットの絶対方位に基づき、旋回後のロボットの絶対方位を算出する。
The play
また、ガタ量算出部３３は、算出されたロボットの旋回前後の絶対方位の差分に基づき、ガタ量を算出する。
Further, the backlash
ここで、ガタ量の算出方法について、図９を参照して説明する。図９は、ガタ量を算出する方法を示す図である。 Here, a method for calculating the backlash will be described with reference to FIG. FIG. 9 is a diagram illustrating a method for calculating the backlash amount.
まず、移動制御部１０は、ロボット中心を旋回中心に、ロボット本体の車輪内の第１ギア部ａおよび第２ギア部ｂをＡ１方向にゆっくり回転させて、第１ギア部ａのＡ１方向側の噛合部位と第２ギア部ｂの噛合部位とが完全に接触した状態にする。この状態でのロボット本体の絶対座標Ｇ０（ｘ０、ｙ０、θ０）を、旋回開始位置の絶対座標（０、０、０）とする。
First, the
次に、旋回開始位置から旋回前のロボット本体のランドマークに対する相対位置および相対方位が、計測部２０によって計測され、相対座標Ｍ０（ｘｍ０、ｙｍ０、θｍ０）として定義される。
Next, the relative position and relative orientation of the robot body before turning from the turning start position to the landmark are measured by the measuring
さらに、移動制御部１０は、ロボット中心を旋回中心に、旋回開始位置からＡ１方向と反対方向Ａ２に角度θｃ旋回させる場合に要する動作時間分旋回させる。そして、旋回後のロボット本体のランドマークに対する相対位置および相対方位が、計測部２０によって計測され、相対座標Ｍ１（ｘｍ１、ｙｍ１、θｍ１）として定義される。
Furthermore, the
ロボット本体の旋回前後の相対座標Ｍ０およびＭ１および旋回前のロボット本体の絶対座標Ｇ０に基づいて、旋回後のロボット本体の絶対座標Ｇ１が算出される。なお、旋回後のロボット本体の絶対座標の算出方法は、前述した「旋回後のロボット本体の絶対座標を算出する方法」と同様であるため、その説明を省略する。 Based on the relative coordinates M 0 and M 1 before and after turning of the robot body and the absolute coordinate G 0 of the robot body before turning, the absolute coordinate G 1 of the robot body after turning is calculated. The method for calculating the absolute coordinates of the robot main body after turning is the same as the above-described “method for calculating the absolute coordinates of the robot main body after turning”, and thus the description thereof is omitted.
ここで、旋回後のロボット本体の絶対座標Ｇ１の絶対方位は、旋回前の絶対座標Ｇ０の絶対方位が「０」であることにより、旋回後の相対方位θｍ１と旋回前の相対方位θｍ０との差分と一致する。つまり、この旋回前後の相対方位の差（θｍ１−θｍ０）は、ロボット本体が実際に旋回した角度と一致することになる。その結果、旋回指示された角度θｃと、ロボット本体が実際に旋回した角度との差分により、ガタ量が算出される。すなわち、ガタ量ｅ３は、以下の式（２９）によって表される。
ｅ３＝θｃ−（θｍ１−θｍ０）・・・（２９）
Here, the absolute azimuth of the absolute coordinate G 1 of post-turning robot body by absolute direction of the absolute coordinate G 0 before rotation is "0", the relative orientation of the front pivot relative orientation theta m1 after turning This agrees with the difference with θ m0 . That is, the difference between the relative azimuths before and after the turn (θ m1 −θ m0 ) matches the angle at which the robot body actually turns. As a result, the backlash amount is calculated based on a difference between the angle θ c instructed to turn and the angle at which the robot body actually turns. That is, amount of play e 3 is expressed by the following equation (29).
e 3 = θ c − (θ m1 −θ m0 ) (29)
図８に戻って、ガタ量算出部３３は、算出したガタ量を記憶部５０に格納する。
Returning to FIG. 8, the backlash
ガタ量補正部６２は、車輪の回転速度に変化があったとき、ガタ量算出部３３によって算出されたガタ量に基づき、車輪の回転量を補正する。具体的には、ガタ量補正部６２は、車輪の回転速度の回転方向が反対方向に変化したとき、記憶部５０によって記憶されたガタ量を車輪の回転量に加算してロボット本体の軌道を補正する。
When there is a change in the rotation speed of the wheel, the backlash
次に、実施例３に係るロボット制御装置のガタ量補正手順を、図１０を参照して説明する。図１０は、実施例３に係るロボット制御装置のガタ量補正手順を示すフローチャートである。なお、ステップＳ２１〜Ｓ２５は、ガタ量を算出する手順を説明し、ステップＳ２６〜Ｓ２８は、ガタ量を補正する手順を説明する。 Next, the play amount correction procedure of the robot control apparatus according to the third embodiment will be described with reference to FIG. FIG. 10 is a flowchart illustrating the play amount correction procedure of the robot control apparatus according to the third embodiment. Steps S21 to S25 describe the procedure for calculating the backlash amount, and steps S26 to S28 describe the procedure for correcting the backlash amount.
まず、ガタ補正指示部４２は、車輪に関わるパラメータを補正した後、計測可能なランドマークを抽出して（ステップＳ２１）、所定位置を中心にしてロボット本体を所定角度旋回させるように移動制御部１０に指示する。なお、所定角度は所定位置を中心に例えば１度から２度程度であれば良い。また、所定位置は、ロボット本体の２つの車輪間の中央を示すロボット中心とするが、これに限定されるものではない。
First, the backlash
すると、移動制御部１０が所定角度の旋回前にロボット本体を任意方向にゆっくり回転させる。そして、計測部２０は、回転後の旋回開始位置からランドマークに対して相対位置および相対方位を計測して（ステップＳ２２）、ランドマークに対する自己のロボット位置およびロボット方位とする。なお、所定角度の旋回前に任意方向にゆっくり回転させるのは、第１ギア部の任意方向側の噛合部位と第２ギア部の噛合部位とが完全に接触した状態にするためである。
Then, the
引き続き、移動制御部１０は、所定位置を中心にして旋回開始位置から任意方向と反対方向に所定角度旋回させる場合に要する動作時間分、ロボット本体を旋回させる（ステップＳ２３）。
Subsequently, the
ロボット本体の旋回後、計測部２０は、ロボット本体の旋回終了位置からランドマークに対して相対位置および相対方位を計測して（ステップＳ２４）、ランドマークに対する自己のロボット位置およびロボット方位とする。
After the turning of the robot body, the measuring
引き続き、ガタ量算出部３３は、旋回前後のランドマークに対する自己のロボット方位と旋回開始位置のロボットの絶対方位とに基づき、旋回後のロボットの絶対方位を算出する。さらに、ガタ量算出部３３は、ロボットの旋回前後の絶対方位の差分に基づき、ガタ量を算出して（ステップＳ２５）、記憶部５０に格納する。
Subsequently, the play
その後、車輪の回転速度に変化があったとき、ガタ量補正部６２は、車輪の回転速度の回転方向に変化があるか否かを判定する（ステップＳ２６）。そして、ガタ量補正部６２は、車輪の回転速度の回転方向に変化がある場合には（ステップＳ２６Ｙｅｓ）、記憶部５０によって記憶されたガタ量を車輪の回転量に加算して、ロボット本体の軌道を補正して（ステップＳ２７）、ステップＳ２８に遷移する。
Thereafter, when there is a change in the rotational speed of the wheel, the
一方、ガタ量補正部６２は、車輪の回転速度の回転方向に変化がない場合には（ステップＳ２６Ｎｏ）、モータ制御部（図示せず）に速度指令を指示する（ステップＳ２８）。
On the other hand, when there is no change in the rotational direction of the rotational speed of the wheel (No at Step S26), the backlash
上記実施例３によれば、ロボット制御装置３は、車輪に関わるパラメータの補正後、車輪内の第１ギア部および第２ギア部を任意方向にロボット本体を回転させて、回転後の旋回開始位置からロボット中心を中心としてロボット本体を任意方向と反対方向に所定角度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始し、当該ロボット本体から離間した位置にある目印を基準にして、当該ロボット本体の旋回前後に関わるロボット方位を計測し、計測した旋回前後に関わるロボット方位間の差分に基づき、第１ギア部の噛合部分および第２ギア部の噛合部分同士の間隙距離（ガタ量）を算出するようにした。さらに、ロボット制御装置３は、車輪の回転方向と反対方向に車輪を回転させた場合、算出されたガタ量を車輪の回転量に加算して補正するようにした。 According to the third embodiment, the robot control device 3 starts the turning after the rotation by rotating the robot body in an arbitrary direction with the first gear portion and the second gear portion in the wheel after correcting the parameters relating to the wheel. When a turn instruction for turning the robot body by a predetermined angle in a direction opposite to an arbitrary direction from the position is detected, the robot body starts turning, and a mark at a position away from the robot body is used as a reference. The robot orientation related to before and after turning of the robot body is measured, and based on the difference between the measured robot orientations before and after turning, the gap distance between the meshing portion of the first gear portion and the meshing portion of the second gear portion (backlash amount) ) Was calculated. Further, the robot controller 3 corrects the calculated play amount by adding the calculated play amount to the wheel rotation amount when the wheel is rotated in a direction opposite to the wheel rotation direction.
かかる構成によれば、ロボット制御装置３は、オドメトリによる自己位置推定に関わる誤差となるガタ量を補正することにより、さらに自己位置推定の調整精度を向上させることができる。 According to such a configuration, the robot control device 3 can further improve the adjustment accuracy of the self-position estimation by correcting the backlash amount that is an error related to the self-position estimation by odometry.
また、ロボット制御装置３は、仮に所定角度がロボット中心を中心に０度に近い角度であれば旋回前後のロボット本体の位置が近似することになり、旋回前後の計測条件の変動も小さく済むことになる。そのため、ロボット制御装置３は、自己位置推定に関わる誤差となるガタ量を正確に算出することができ、オドメトリによる自己位置推定の調整精度を向上させることができる。 In addition, if the predetermined angle is close to 0 degrees with respect to the center of the robot, the robot controller 3 approximates the position of the robot body before and after the turn, and fluctuations in measurement conditions before and after the turn can be reduced. become. Therefore, the robot control device 3 can accurately calculate the amount of backlash that is an error related to self-position estimation, and can improve the adjustment accuracy of self-position estimation by odometry.
さらに、ロボット制御装置３は、ガタ量を自動的に算出することができ、その後、算出されたガタ量を補正することによりオドメトリによる自己位置推定の調整を効率的に行うことができる。 Further, the robot control device 3 can automatically calculate the amount of play, and then can efficiently adjust the self-position estimation by odometry by correcting the calculated amount of play.
また、ロボット制御装置３は、ロボットをロボット中心に旋回させる場所と目印とがあれば、ガタ量を算出することができるため、仮にロボットを直進させてガタ量を算出するよりも場所を選ばないで容易に調整することができる。 Further, the robot control device 3 can calculate the amount of play if there is a place and a mark for turning the robot around the center of the robot. Therefore, the robot control device 3 can select a place rather than calculating the amount of play by moving the robot straight. Can be easily adjusted.
以上、上記実施例によって本発明の技術的思想の範囲が限定されるものではなく、特許請求の範囲に記載した技術的範囲の範囲を逸脱しない限り、各種様々な実施例が実施可能であることは言うまでもない。また、本実施例に記載した効果は、これに限定されるものではない。 As described above, the scope of the technical idea of the present invention is not limited by the above-described embodiments, and various embodiments can be implemented without departing from the scope of the technical scope described in the claims. Needless to say. Moreover, the effect described in the present Example is not limited to this.
また、図示したロボット制御装置１〜３の各構成要素は機能概念的に記載したものであって、必ずしも物理的に図示のように構成されるものではなく、そのロボット制御装置１〜３の具体的な態様は図示のものに限縮されるものでは到底ないことは言うまでもない。
The components of the
なお、ロボット制御装置１〜３にて行われる各処理機能は、その全部または任意の一部が、ＣＰＵ（Central Processing Unit）（またはＭＰＵ（Micro Processing Unit）、ＭＣＵ（Micro Controller Unit）などのマイクロ・コンピュータ）および当該ＣＰＵ（またはＭＰＵ、ＭＣＵなどのマイクロ・コンピュータ）にて解析実行されるプログラムにて実現され、あるいは、ワイヤードロジックによるハードウェアとして実現されても良い。
The processing functions performed by the
また、ロボット制御装置１〜３にて行われる各処理機能は、その全部または任意の一部が、ＣＰＵ（Central Processing Unit）（またはＭＰＵ（Micro Processing Unit）、ＭＣＵ（Micro Controller Unit）などのマイクロ・コンピュータ）および当該ＣＰＵ（またはＭＰＵ、ＭＣＵなどのマイクロ・コンピュータ）にて解析実行させるために、コンピュータ読み取り可能な記録媒体に記録されるものであっても良く、この場合でも、上記実施例と同様の効果が得られることは言うまでもない。
The processing functions performed by the
以上の実施例に係る実施形態に関し、さらに以下の付記を開示する。 The following additional remarks are disclosed regarding the embodiment according to the above example.
（付記１）ロボット本体の旋回開始位置から所定位置を中心にして、当該ロボット本体を旋回前後の位置が近似した位置となるように所定角度旋回させる旋回指示を検出すると、当該ロボット本体を移動させる２つの車輪の回転動作に応じて、当該ロボット本体の旋回を開始する移動制御部と、
当該ロボット本体から離間した位置にある目印を基準にして、当該ロボット本体の旋回前後に関わるロボット位置およびロボット方位を計測する計測部と、
前記計測部にて計測した前記旋回前後に関わるロボット位置間の差分および前記旋回前後に関わるロボット方位間の差分に基づき、前記車輪に関わる誤差を算出する算出部と
を有することを特徴とするロボット制御装置。
(Supplementary note 1) When a turning instruction for turning the robot main body by a predetermined angle from the turning start position of the robot main body to a position approximate to the position before and after the turning is detected, the robot main body is moved. A movement control unit for starting the turning of the robot body according to the rotational movement of the two wheels;
A measurement unit that measures a robot position and a robot orientation related to before and after turning of the robot body with reference to a mark located at a position away from the robot body;
A calculation unit that calculates an error related to the wheel based on a difference between robot positions related to before and after the turn measured by the measurement unit and a difference between robot orientations related to before and after the turn. Control device.
（付記２）前記移動制御部は、
前記旋回開始位置から所定位置を中心にして、前記ロボット本体を３６０度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始し、
前記算出部は、
前記計測部にて計測した前記旋回前後に関わるロボット位置間の差分および前記旋回前後に関わるロボット方位間の差分に基づき、前記車輪に関わるパラメータ内の複数の要素のうち、少なくとも前記車輪の半径または車輪間の距離に関わる誤差を算出することを特徴とする付記１に記載のロボット制御装置。
(Appendix 2) The movement control unit
When a turning instruction for turning the robot body 360 degrees around a predetermined position from the turning start position is detected, the robot body starts turning.
The calculation unit includes:
Based on the difference between the robot positions related before and after the turn measured by the measurement unit and the difference between the robot orientations before and after the turn, at least the radius of the wheel or the plurality of elements in the parameters related to the wheel The robot control device according to
（付記３）前記算出部にて算出した前記誤差に基づき、前記車輪に関わるパラメータを補正する補正部を有することを特徴とする付記１または付記２に記載のロボット制御装置。
(Supplementary Note 3) The robot control device according to
（付記４）前記移動制御部は、
前記旋回開始位置から前記所定位置を中心にして、前記ロボット本体を複数回３６０度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始することを特徴とする付記１から付記３のいずれか１つに記載のロボット制御装置。
(Appendix 4) The movement control unit
Any one of
（付記５）前記算出部にて算出した前記誤差が異常閾値を超えた場合、前記車輪の異常を報知する報知部を有することを特徴とする付記１から付記４のいずれか１つに記載のロボット制御装置。
(Additional remark 5) It has an alerting | reporting part which alert | reports the abnormality of the said wheel when the said error calculated in the said calculation part exceeds an abnormality threshold value, It is any one of the
（付記６）前記移動制御部は、
前記ロボット本体の旋回を開始する前に、前記車輪内の第１ギア部および第２ギア部同士を噛合して前記第１ギア部および前記第２ギア部を正転方向に回転させて、前記第１ギア部の前記正転方向側の噛合部位と当該第２ギアの噛合部位とが完全に接触した状態で、前記ロボット本体の旋回を開始することを特徴とする付記１から付記５のいずれか１つに記載のロボット制御装置。
(Appendix 6) The movement control unit
Before starting the turning of the robot body, the first gear portion and the second gear portion in the wheel are meshed with each other, the first gear portion and the second gear portion are rotated in the forward rotation direction, Any one of
（付記７）前記移動制御部は、
前記ロボット本体の旋回を開始する前に、前記第１ギア部および前記第２ギア部を正転方向と反対方向に回転させて、回転後の前記旋回開始位置から所定位置を中心として前記ロボット本体を前記正転方向に前記所定角度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始し、
前記算出部は、
前記計測部にて計測した前記所定角度の旋回前後のロボット方位に基づき、前記第１ギア部の噛合部分および前記第２ギア部の噛合部分同士の間隙距離を算出することを特徴とする付記６に記載のロボット制御装置。
(Appendix 7) The movement control unit
Before starting the turning of the robot body, the robot body rotates the first gear part and the second gear part in the direction opposite to the normal rotation direction, and the robot body is centered on a predetermined position from the turning start position after the rotation. When the turn instruction to turn the predetermined angle in the forward rotation direction is detected, the robot body starts turning.
The calculation unit includes:
The gap distance between the meshing part of the first gear part and the meshing part of the second gear part is calculated on the basis of the robot azimuth before and after turning at the predetermined angle measured by the measurement part. The robot control device described in 1.
（付記８）前記所定位置は、
前記ロボット本体の２つの車輪間の中央であることを特徴とする付記７に記載のロボット制御装置。
(Appendix 8) The predetermined position is:
The robot control device according to appendix 7, wherein the robot control device is in the center between two wheels of the robot body.
（付記９）前記正転方向と反対方向に前記車輪を回転させた場合、前記算出部にて算出した前記間隙距離を前記車輪の回転量に加算して補正する補正部を有することを特徴とする付記７または付記８に記載のロボット制御装置。 (Additional remark 9) When the said wheel is rotated in the direction opposite to the said normal rotation direction, it has a correction | amendment part which adds and correct | amends the said clearance distance calculated in the said calculation part to the rotation amount of the said wheel, It is characterized by the above-mentioned. The robot controller according to appendix 7 or appendix 8.
（付記１０）ロボット本体の旋回開始位置から所定位置を中心にして、当該ロボット本体を旋回前後の位置が近似した位置となるように所定角度旋回させる旋回指示を検出すると、当該ロボット本体を移動させる２つの車輪の回転動作に応じて、当該ロボット本体の旋回を開始する移動制御手順と、
当該ロボット本体から離間した位置にある目印を基準にして、当該ロボット本体の旋回前後に関わるロボット位置およびロボット方位を計測する計測手順と、
前記計測手順にて計測した前記旋回前後に関わるロボット位置間の差分および前記旋回前後に関わるロボット方位間の差分に基づき、前記車輪に関わる誤差を算出する算出手順と
をコンピュータに実行させることを特徴とするロボット制御プログラム。
(Supplementary Note 10) When a turning instruction for turning the robot main body by a predetermined angle from the turning start position of the robot main body to a position approximate to the position before and after the turning is detected, the robot main body is moved. A movement control procedure for starting the turning of the robot body according to the rotational movement of the two wheels;
A measurement procedure for measuring a robot position and a robot orientation related to before and after turning of the robot body with reference to a mark located at a position away from the robot body;
Causing the computer to execute a calculation procedure for calculating an error relating to the wheel based on the difference between the robot positions related to before and after the turn measured in the measurement procedure and the difference between the robot orientations related to before and after the turn. A robot control program.
（付記１１）前記移動制御手順は、
前記旋回開始位置から所定位置を中心にして、前記ロボット本体を３６０度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始し、
前記算出手順は、
前記計測手順にて計測した前記旋回前後に関わるロボット位置間の差分および前記旋回前後に関わるロボット方位間の差分に基づき、前記車輪に関わるパラメータ内の複数の要素のうち、少なくとも前記車輪の半径または車輪間の距離に関わる誤差を算出することを特徴とする付記１０に記載のロボット制御プログラム。
(Supplementary Note 11) The movement control procedure is as follows:
When a turning instruction for turning the robot body 360 degrees around a predetermined position from the turning start position is detected, the robot body starts turning.
The calculation procedure is as follows:
Based on the difference between the robot positions related to before and after the turn measured in the measurement procedure and the difference between the robot orientations before and after the turn, at least the radius of the wheel or the plurality of elements in the parameters related to the wheel The robot control program according to
（付記１２）前記算出手順にて算出した前記誤差に基づき、前記車輪に関わるパラメータを補正する補正手順を含むことを特徴とする付記１０または付記１１に記載のロボット制御プログラム。
(Additional remark 12) The robot control program according to
（付記１３）前記移動制御手順は、
前記旋回開始位置から前記所定位置を中心にして、前記ロボット本体を複数回３６０度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始することを特徴とする付記１０から付記１２のいずれか１つに記載のロボット制御プログラム。
(Supplementary Note 13) The movement control procedure is as follows.
Any one of
（付記１４）前記算出手順にて算出した前記誤差が異常閾値を超えた場合、前記車輪の異常を報知する報知手順を含むことを特徴とする付記１０から付記１３のいずれか１つに記載のロボット制御プログラム。
(Supplementary note 14) According to any one of
（付記１５）前記移動制御手順は、
前記ロボット本体の旋回を開始する前に、前記車輪内の第１ギア部および第２ギア部同士を噛合して前記第１ギア部および前記第２ギア部を正転方向に回転させて、前記第１ギア部の前記正転方向側の噛合部位と当該第２ギアの噛合部位とが完全に接触した状態で、前記ロボット本体の旋回を開始することを特徴とする付記１０から付記１４のいずれか１つに記載のロボット制御プログラム。
(Supplementary Note 15) The movement control procedure is as follows.
Before starting the turning of the robot body, the first gear portion and the second gear portion in the wheel are meshed with each other, the first gear portion and the second gear portion are rotated in the forward rotation direction, Any one of
（付記１６）前記移動制御手順は、
前記ロボット本体の旋回を開始する前に、前記第１ギア部および前記第２ギア部を正転方向と反対方向に回転させて、回転後の前記旋回開始位置から所定位置を中心として前記ロボット本体を前記正転方向に前記所定角度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始し、
前記算出手順は、
前記計測手順にて計測した前記所定角度の旋回前後のロボット方位に基づき、前記第１ギア部の噛合部分および前記第２ギア部の噛合部分同士の間隙距離を算出することを特徴とする付記１５に記載のロボット制御プログラム。
(Supplementary Note 16) The movement control procedure is as follows.
Before starting the turning of the robot body, the robot body rotates the first gear part and the second gear part in the direction opposite to the normal rotation direction, and the robot body is centered on a predetermined position from the turning start position after the rotation. When the turn instruction to turn the predetermined angle in the forward rotation direction is detected, the robot body starts turning.
The calculation procedure is as follows:
The gap distance between the meshing portion of the first gear portion and the meshing portion of the second gear portion is calculated based on the robot orientation before and after the turn of the predetermined angle measured in the measurement procedure. The robot control program described in 1.
（付記１７）前記所定位置は、
前記ロボット本体の２つの車輪間の中央であることを特徴とする付記１６に記載のロボット制御プログラム。
(Supplementary Note 17) The predetermined position is:
The robot control program according to appendix 16, wherein the program is the center between two wheels of the robot body.
（付記１８）前記正転方向と反対方向に前記車輪を回転させた場合、前記算出手順にて算出した前記間隙距離を前記車輪の回転量に加算して補正する補正手順を有することを特徴とする付記１６または付記１７に記載のロボット制御プログラム。 (Additional remark 18) When the said wheel is rotated in the direction opposite to the said normal rotation direction, it has the correction | amendment procedure which adds and correct | amends the said gap | interval distance calculated in the said calculation procedure to the rotation amount of the said wheel, It is characterized by the above-mentioned. The robot control program according to appendix 16 or appendix 17.
（付記１９）コンピュータが２輪駆動のロボットを制御するロボット制御方法であって、
ロボット本体の旋回開始位置から所定位置を中心にして、当該ロボット本体を旋回前後の位置が近似した位置となるように所定角度旋回させる旋回指示を検出すると、当該ロボット本体を移動させる２つの車輪の回転動作に応じて、当該ロボット本体の旋回を開始する移動制御工程と、
当該ロボット本体から離間した位置にある目印を基準にして、当該ロボット本体の旋回前後に関わるロボット位置およびロボット方位を計測する計測工程と、
前記計測工程にて計測した前記旋回前後に関わるロボット位置間の差分および前記旋回前後に関わるロボット方位間の差分に基づき、前記車輪に関わる誤差を算出する算出工程と
を含むことを特徴とするロボット制御方法。
(Supplementary note 19) A robot control method in which a computer controls a two-wheel drive robot,
When a turning instruction for turning the robot main body by a predetermined angle from the turning start position of the robot main body to a predetermined position so that the position before and after the turning is approximated is detected, the two wheels that move the robot main body are detected. A movement control step for starting the turning of the robot body according to the rotation operation;
A measurement process for measuring a robot position and a robot orientation related to before and after turning of the robot body, with reference to a mark located at a position away from the robot body,
A calculation step of calculating an error related to the wheel based on a difference between the robot positions related to before and after the turn measured in the measurement step and a difference between the robot orientations related to before and after the turn. Control method.
（付記２０）前記移動制御工程は、
前記旋回開始位置から所定位置を中心にして、前記ロボット本体を３６０度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始し、
前記算出工程は、
前記計測工程にて計測した前記旋回前後に関わるロボット位置間の差分および前記旋回前後に関わるロボット方位間の差分に基づき、前記車輪に関わるパラメータ内の複数の要素のうち、少なくとも前記車輪の半径または車輪間の距離に関わる誤差を算出することを特徴とする付記１９に記載のロボット制御方法。
(Supplementary note 20) The movement control step includes:
When a turning instruction for turning the robot body 360 degrees around a predetermined position from the turning start position is detected, the robot body starts turning.
The calculation step includes
Based on the difference between the robot positions related before and after the turn measured in the measurement step and the difference between the robot orientations before and after the turn, at least the radius of the wheel or the plurality of elements in the parameters related to the
（付記２１）前記算出工程にて算出した前記誤差に基づき、前記車輪に関わるパラメータを補正する補正工程を含むことを特徴とする付記１９または付記２０に記載のロボット制御方法。
(Supplementary note 21) The robot control method according to
（付記２２）前記移動制御工程は、
前記旋回開始位置から前記所定位置を中心にして、前記ロボット本体を複数回３６０度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始することを特徴とする付記１９から付記２１のいずれか１つに記載のロボット制御方法。
(Supplementary Note 22) The movement control step includes:
Any one of
（付記２３）前記算出工程にて算出した前記誤差が異常閾値を超えた場合、前記車輪の異常を報知する報知工程を含むことを特徴とする付記１９から付記２２のいずれか１つに記載のロボット制御方法。
(Additional remark 23) When the said error calculated at the said calculation process exceeds the abnormality threshold value, the alerting | reporting process of alerting | reporting the abnormality of the said wheel is included, The
（付記２４）前記移動制御工程は、
前記ロボット本体の旋回を開始する前に、前記車輪内の第１ギア部および第２ギア部同士を噛合して前記第１ギア部および前記第２ギア部を正転方向に回転させて、前記第１ギア部の前記正転方向側の噛合部位と当該第２ギアの噛合部位とが完全に接触した状態で、前記ロボット本体の旋回を開始することを特徴とする付記１９から付記２３のいずれか１つに記載のロボット制御方法。
(Supplementary Note 24) The movement control step includes:
Before starting the turning of the robot body, the first gear portion and the second gear portion in the wheel are meshed with each other, the first gear portion and the second gear portion are rotated in the forward rotation direction, Any one of
（付記２５）前記移動制御工程は、
前記ロボット本体の旋回を開始する前に、前記第１ギア部および前記第２ギア部を正転方向と反対方向に回転させて、回転後の前記旋回開始位置から所定位置を中心として前記ロボット本体を前記正転方向に前記所定角度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始し、
前記算出工程は、
前記計測工程にて計測した前記所定角度の旋回前後のロボット方位に基づき、前記第１ギア部の噛合部分および前記第２ギア部の噛合部分同士の間隙距離を算出することを特徴とする付記２４に記載のロボット制御方法。
(Supplementary Note 25) The movement control step includes:
Before starting the turning of the robot body, the robot body rotates the first gear part and the second gear part in the direction opposite to the normal rotation direction, and the robot body is centered on a predetermined position from the turning start position after the rotation. When the turn instruction to turn the predetermined angle in the forward rotation direction is detected, the robot body starts turning.
The calculation step includes
The gap distance between the meshing part of the first gear part and the meshing part of the second gear part is calculated on the basis of the robot orientation before and after turning at the predetermined angle measured in the measurement step. The robot control method described in 1.
（付記２６）前記所定位置は、
前記ロボット本体の２つの車輪間の中央であることを特徴とする付記２５に記載のロボット制御方法。
(Supplementary Note 26) The predetermined position is:
26. The robot control method according to appendix 25, wherein the robot control method is in the center between two wheels of the robot body.
（付記２７）前記正転方向と反対方向に前記車輪を回転させた場合、前記算出工程にて算出した前記間隙距離を前記車輪の回転量に加算して補正する補正工程を含むことを特徴とする付記２５または付記２６に記載のロボット制御方法。 (Additional remark 27) When the said wheel is rotated in the direction opposite to the said normal rotation direction, the correction process which adds and correct | amends the said gap | interval distance calculated at the said calculation process to the rotation amount of the said wheel is characterized by the above-mentioned. The robot control method according to appendix 25 or appendix 26.
１、２、３ ロボット制御装置
１０ 移動制御部
２０ 計測部
３０ 算出部
３１ 旋回後絶対座標算出部
３２ 誤差算出部
３３ ガタ量算出部
４０ 指示部
４１ パラメータ誤差補正指示部
４２ ガタ補正指示部
５０ 記憶部
６０ 補正部
６１ パラメータ補正部
６２ ガタ量補正部
７０ 報知部
1, 2, 3
Claims (7)
当該ロボット本体から離間した位置にある目印を基準にして、当該ロボット本体の旋回前後に関わるロボット位置およびロボット方位を計測する計測部と、
前記計測部にて計測した前記旋回前後に関わるロボット位置間の差分および前記旋回前後に関わるロボット方位間の差分に基づき、前記車輪に関わる誤差を算出する算出部と
を有することを特徴とするロボット制御装置。 When a turning instruction for turning the robot main body by a predetermined angle from the turning start position of the robot main body to a predetermined position so that the position before and after the turning is approximated is detected, the two wheels that move the robot main body are detected. A movement control unit for starting the turning of the robot body according to the rotation operation;
A measurement unit that measures a robot position and a robot orientation related to before and after turning of the robot body with reference to a mark located at a position away from the robot body;
A calculation unit that calculates an error related to the wheel based on a difference between robot positions related to before and after the turn measured by the measurement unit and a difference between robot orientations related to before and after the turn. Control device.
前記旋回開始位置から所定位置を中心にして、前記ロボット本体を３６０度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始し、
前記算出部は、
前記計測部にて計測した前記旋回前後に関わるロボット位置間の差分および前記旋回前後に関わるロボット方位間の差分に基づき、前記車輪に関わるパラメータ内の複数の要素のうち、少なくとも前記車輪の半径または車輪間の距離に関わる誤差を算出することを特徴とする請求項１に記載のロボット制御装置。 The movement control unit
When a turning instruction for turning the robot body 360 degrees around a predetermined position from the turning start position is detected, the robot body starts turning.
The calculation unit includes:
Based on the difference between the robot positions related before and after the turn measured by the measurement unit and the difference between the robot orientations before and after the turn, at least the radius of the wheel or the plurality of elements in the parameters related to the wheel The robot control apparatus according to claim 1, wherein an error related to a distance between the wheels is calculated.
前記ロボット本体の旋回を開始する前に、前記車輪内の第１ギア部および第２ギア部同士を噛合して前記第１ギア部および前記第２ギア部を正転方向に回転させて、前記第１ギア部の前記正転方向側の噛合部位と当該第２ギアの噛合部位とが完全に接触した状態で、前記ロボット本体の旋回を開始することを特徴とする請求項１から請求項３のいずれか１つに記載のロボット制御装置。 The movement control unit
Before starting the turning of the robot body, the first gear portion and the second gear portion in the wheel are meshed with each other, the first gear portion and the second gear portion are rotated in the forward rotation direction, 4. The turning of the robot main body is started in a state where the meshing portion of the first gear portion on the forward rotation direction side and the meshing portion of the second gear are completely in contact with each other. The robot control device according to any one of the above.
前記ロボット本体の旋回を開始する前に、前記第１ギア部および前記第２ギア部を正転方向と反対方向に回転させて、回転後の前記旋回開始位置から所定位置を中心として前記ロボット本体を前記正転方向に前記所定角度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始し、
前記算出部は、
前記計測部にて計測した前記所定角度の旋回前後のロボット方位に基づき、前記第１ギア部の噛合部分および前記第２ギア部の噛合部分同士の間隙距離を算出することを特徴とする請求項４に記載のロボット制御装置。 The movement control unit
Before starting the turning of the robot body, the robot body rotates the first gear part and the second gear part in the direction opposite to the normal rotation direction, and the robot body is centered on a predetermined position from the turning start position after the rotation. When the turn instruction to turn the predetermined angle in the forward rotation direction is detected, the robot body starts turning.
The calculation unit includes:
The gap distance between the meshing portion of the first gear portion and the meshing portion of the second gear portion is calculated based on the robot orientation before and after the turn of the predetermined angle measured by the measurement unit. 4. The robot control device according to 4.
当該ロボット本体から離間した位置にある目印を基準にして、当該ロボット本体の旋回前後に関わるロボット位置およびロボット方位を計測する計測手順と、
前記計測手順にて計測した前記旋回前後に関わるロボット位置間の差分および前記旋回前後に関わるロボット方位間の差分に基づき、前記車輪に関わる誤差を算出する算出手順と
をコンピュータに実行させることを特徴とするロボット制御プログラム。 When a turning instruction for turning the robot main body by a predetermined angle from the turning start position of the robot main body to a predetermined position so that the position before and after the turning is approximated is detected, the two wheels that move the robot main body are detected. A movement control procedure for starting the turning of the robot body according to the rotation operation;
A measurement procedure for measuring a robot position and a robot orientation related to before and after turning of the robot body with reference to a mark located at a position away from the robot body;
Causing the computer to execute a calculation procedure for calculating an error relating to the wheel based on the difference between the robot positions related to before and after the turn measured in the measurement procedure and the difference between the robot orientations related to before and after the turn. A robot control program.
ロボット本体の旋回開始位置から所定位置を中心にして、当該ロボット本体を旋回前後の位置が近似した位置となるように所定角度旋回させる旋回指示を検出すると、当該ロボット本体を移動させる２つの車輪の回転動作に応じて、当該ロボット本体の旋回を開始する移動制御工程と、
当該ロボット本体から離間した位置にある目印を基準にして、当該ロボット本体の旋回前後に関わるロボット位置およびロボット方位を計測する計測工程と、
前記計測工程にて計測した前記旋回前後に関わるロボット位置間の差分および前記旋回前後に関わるロボット方位間の差分に基づき、前記車輪に関わる誤差を算出する算出工程と
を含むことを特徴とするロボット制御方法。 A robot control method in which a computer controls a two-wheel drive robot,
When a turning instruction for turning the robot main body by a predetermined angle from the turning start position of the robot main body to a predetermined position so that the position before and after the turning is approximated is detected, the two wheels that move the robot main body are detected. A movement control step for starting the turning of the robot body according to the rotation operation;
A measurement step for measuring a robot position and a robot orientation related to before and after turning of the robot body, with reference to a mark located at a position away from the robot body;
A calculation step of calculating an error related to the wheel based on a difference between the robot positions related to before and after the turn measured in the measurement step and a difference between the robot orientations related to before and after the turn. Control method.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2009152957A JP5487756B2 (en) | 2009-06-26 | 2009-06-26 | Robot control apparatus, robot control program, and robot control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2009152957A JP5487756B2 (en) | 2009-06-26 | 2009-06-26 | Robot control apparatus, robot control program, and robot control method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2011008637A true JP2011008637A (en) | 2011-01-13 |
| JP5487756B2 JP5487756B2 (en) | 2014-05-07 |
Family
ID=43565188
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2009152957A Expired - Fee Related JP5487756B2 (en) | 2009-06-26 | 2009-06-26 | Robot control apparatus, robot control program, and robot control method |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP5487756B2 (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108372518A (en) * | 2018-01-08 | 2018-08-07 | 浙江立石机器人技术有限公司 | Robot turning error scaling method, apparatus and system |
| CN112230653A (en) * | 2020-09-24 | 2021-01-15 | 北京旷视机器人技术有限公司 | Method and device for determining driving wheel parameters and automatic navigation vehicle |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111515916A (en) * | 2020-04-29 | 2020-08-11 | 广州高新兴机器人有限公司 | Method and device for calibrating errors of front steering system of robot |
Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS6281166A (en) * | 1985-10-03 | 1987-04-14 | Data Iisuto Kk | Method for driving scanning head |
| JPS62140784A (en) * | 1985-12-17 | 1987-06-24 | フアナツク株式会社 | Backlash compensator for robot |
| JPS6320508A (en) * | 1986-07-14 | 1988-01-28 | Tsubakimoto Chain Co | Guiding method for autonomous running car |
| JP2000121357A (en) * | 1998-10-19 | 2000-04-28 | Sumitomo Heavy Ind Ltd | Position measuring instrument for moving body |
| WO2004103649A1 (en) * | 2003-05-20 | 2004-12-02 | Fujitsu Limited | Method for controlling backlash compensation, backlash compensation controller and program for controlling backlash compensation |
| US20050192721A1 (en) * | 2004-02-27 | 2005-09-01 | Jouppi Norman P. | Mobile device control system |
| JP2006252349A (en) * | 2005-03-11 | 2006-09-21 | Secom Co Ltd | Mobile robot |
| JP2006349399A (en) * | 2005-06-14 | 2006-12-28 | Yaskawa Electric Corp | Azimuthal angle measuring instrument and movable body |
| JP2007156576A (en) * | 2005-11-30 | 2007-06-21 | Mitsubishi Heavy Ind Ltd | Method and device for adjusting odometry(wheel range finder) parameter for traveling carrier |
| JP2007322138A (en) * | 2006-05-30 | 2007-12-13 | Toyota Motor Corp | Moving device, and own position estimation method for moving device |
| JP2008250589A (en) * | 2007-03-30 | 2008-10-16 | Equos Research Co Ltd | Wheel radius estimation device |
| JP2008290184A (en) * | 2007-05-24 | 2008-12-04 | Fujitsu Ltd | Correcting robot system and correcting method of distance sensor |
| JP2010003240A (en) * | 2008-06-23 | 2010-01-07 | Fujitsu Ltd | Rescue robot system, rescue method, and rescue robot |
-
2009
- 2009-06-26 JP JP2009152957A patent/JP5487756B2/en not_active Expired - Fee Related
Patent Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS6281166A (en) * | 1985-10-03 | 1987-04-14 | Data Iisuto Kk | Method for driving scanning head |
| JPS62140784A (en) * | 1985-12-17 | 1987-06-24 | フアナツク株式会社 | Backlash compensator for robot |
| JPS6320508A (en) * | 1986-07-14 | 1988-01-28 | Tsubakimoto Chain Co | Guiding method for autonomous running car |
| JP2000121357A (en) * | 1998-10-19 | 2000-04-28 | Sumitomo Heavy Ind Ltd | Position measuring instrument for moving body |
| WO2004103649A1 (en) * | 2003-05-20 | 2004-12-02 | Fujitsu Limited | Method for controlling backlash compensation, backlash compensation controller and program for controlling backlash compensation |
| US20050192721A1 (en) * | 2004-02-27 | 2005-09-01 | Jouppi Norman P. | Mobile device control system |
| JP2006252349A (en) * | 2005-03-11 | 2006-09-21 | Secom Co Ltd | Mobile robot |
| JP2006349399A (en) * | 2005-06-14 | 2006-12-28 | Yaskawa Electric Corp | Azimuthal angle measuring instrument and movable body |
| JP2007156576A (en) * | 2005-11-30 | 2007-06-21 | Mitsubishi Heavy Ind Ltd | Method and device for adjusting odometry(wheel range finder) parameter for traveling carrier |
| JP2007322138A (en) * | 2006-05-30 | 2007-12-13 | Toyota Motor Corp | Moving device, and own position estimation method for moving device |
| JP2008250589A (en) * | 2007-03-30 | 2008-10-16 | Equos Research Co Ltd | Wheel radius estimation device |
| JP2008290184A (en) * | 2007-05-24 | 2008-12-04 | Fujitsu Ltd | Correcting robot system and correcting method of distance sensor |
| JP2010003240A (en) * | 2008-06-23 | 2010-01-07 | Fujitsu Ltd | Rescue robot system, rescue method, and rescue robot |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108372518A (en) * | 2018-01-08 | 2018-08-07 | 浙江立石机器人技术有限公司 | Robot turning error scaling method, apparatus and system |
| CN112230653A (en) * | 2020-09-24 | 2021-01-15 | 北京旷视机器人技术有限公司 | Method and device for determining driving wheel parameters and automatic navigation vehicle |
Also Published As
| Publication number | Publication date |
|---|---|
| JP5487756B2 (en) | 2014-05-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP3078459B1 (en) | Robot controlling method, robot apparatus, program and recording medium | |
| JP5912627B2 (en) | Imaging inspection apparatus, control apparatus and control method thereof | |
| JP6493570B2 (en) | Electric power steering device | |
| JP2017124455A (en) | Robot device, robot control method, program and record medium | |
| US10814481B2 (en) | Robot system for performing learning control by using motor encoder and sensor | |
| JP2008059218A (en) | Method for restoring self-position of autonomously traveling robot | |
| JP4635971B2 (en) | External force detection method, manipulator control method, and manipulator control system | |
| JP5487756B2 (en) | Robot control apparatus, robot control program, and robot control method | |
| US11161697B2 (en) | Work robot system and work robot | |
| CN111409632A (en) | Vehicle control method and device, computer equipment and storage medium | |
| JP2008183690A (en) | Robot control device and system | |
| JP5338968B2 (en) | Steering control device | |
| EP4028311A1 (en) | Drift stabilization system and method for vehicles and trailer reverse assist systems while straight line backing of trailer | |
| CN111634331A (en) | Steering control method, device and system for automatic driving vehicle | |
| CN112388602B (en) | Calibration method, device and equipment of mobile robot | |
| US9270209B2 (en) | Servo apparatus, and controlling method of servo apparatus | |
| JP2019077026A5 (en) | ||
| JP2010110878A (en) | Articulated robot device and method for controlling the same | |
| CN110837257A (en) | AGV composite positioning navigation system based on iGPS and vision | |
| JP2020006469A (en) | Automatic machine and controller | |
| JP2007007804A (en) | Method of detecting abnormality of position measuring means and direction measuring means of autonomous moving robot, and autonomous moving robot | |
| JP2636675B2 (en) | Guidance tracking device | |
| JP5381039B2 (en) | Articulated manipulator tip position control method and articulated manipulator | |
| US20170060103A1 (en) | Robot control device for controlling motor-driven robot | |
| CN114001656B (en) | Detection error correction method and device for optical displacement detection device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20120309 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20130417 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20130423 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20130621 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20130723 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20130919 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20131210 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20131227 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20140110 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20140128 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20140210 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 5487756 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| LAPS | Cancellation because of no payment of annual fees |