JP5057224B2

JP5057224B2 - Mobile robot controller

Info

Publication number: JP5057224B2
Application number: JP2007240498A
Authority: JP
Inventors: 淳千々和
Original assignee: Yaskawa Electric Corp
Current assignee: Yaskawa Electric Corp
Priority date: 2007-09-18
Filing date: 2007-09-18
Publication date: 2012-10-24
Anticipated expiration: 2027-09-18
Also published as: JP2009070315A

Description

本発明は、オフィス、工場等の施設内において、環境情報を計測して移動する移動ロボットに搭載された移動ロボット制御装置に関する。 The present invention relates to a mobile robot control device mounted on a mobile robot that moves by measuring environmental information in a facility such as an office or factory.

移動ロボットは、移動手段である車輪と床面とのすべりなどにより目標位置からの位置誤差が発生する。そのため、従来の移動ロボット制御装置では、誤差の補正のために超音波センサなどの外界センサを用いてロボット位置を計測し、目標位置とのずれを補正する方法が取られている。例えば、往復直進運動に限定して、ジャイロやエンコーダなどの内界センサと外界計測用の超音波センサとにより位置ずれを計測して、その結果から補正量を算出し、移動制御に使う方法が提案されている（例えば、特許文献１参照）。
また、超音波センサおよびデッドレコニングにより、絶対位置補正地点にロボットが到着していると判断された場合、超音波センサで周囲の壁までの距離を測定し、その距離データの値が理想値と一致していない場合に、位置、方向の補正を行うものもある（例えば、特許文献２参照）。 In a mobile robot, a position error from a target position occurs due to slippage between a wheel and a floor as a moving means. Therefore, in the conventional mobile robot control device, a method of measuring a robot position using an external sensor such as an ultrasonic sensor to correct an error and correcting a deviation from a target position is employed. For example, there is a method that is used for movement control by measuring the positional deviation by using an internal sensor such as a gyroscope and an encoder and an ultrasonic sensor for measuring the external environment, calculating a correction amount from the result, and limiting to reciprocating linear motion. It has been proposed (see, for example, Patent Document 1).
Also, when it is determined by the ultrasonic sensor and dead reckoning that the robot has arrived at the absolute position correction point, the distance to the surrounding wall is measured by the ultrasonic sensor, and the value of the distance data is the ideal value. In some cases, the position and direction are corrected when they do not match (for example, see Patent Document 2).

図７（ａ）および（ｂ）は、特許文献１における移動ロボット１０１の装置構成を示す。移動ロボット１０１を移動させる駆動手段１０２と、ジャイロおよび積分器から構成され本体の方向を計測する方向計測手段１０３と、超音波センサから構成され本体周囲の物体までの距離を計測する測距手段１０４と、前記駆動手段１０２を制御し移動ロボット１０１の走行を制御する走行制御手段１０５と、エンコーダなどから構成され移動ロボット１０１の走行距離を計測する走行距離検出手段１０６を備え、前記走行制御手段１０５は、方向計測手段１０３の出力に基づいて移動ロボット１０１を所定の目標方向に直進走行させる直進手段１０７と、走行距離検出手段１０６と測距手段１０４の情報から本体車輪の滑りを求める演算手段１０８を有する。このような構成において、方向計測手段１０３の出力に基づいて直進手段１０７にて直進方向に制御し、演算手段１０８にて走行距離検出手段１０６と測距手段１０４の情報から制御周期毎に位置ずれを計測して、補正動作を行っている。 7A and 7B show a device configuration of the mobile robot 101 in Patent Document 1. FIG. A driving means 102 for moving the mobile robot 101, a direction measuring means 103 comprising a gyro and an integrator for measuring the direction of the main body, and a distance measuring means 104 comprising an ultrasonic sensor for measuring the distance to an object around the main body. A travel control means 105 for controlling the drive means 102 to control the travel of the mobile robot 101; and a travel distance detection means 106 for measuring the travel distance of the mobile robot 101, which comprises an encoder or the like. Is based on the output of the direction measuring means 103, the straight traveling means 107 that makes the mobile robot 101 travel straight in a predetermined target direction, and the computing means 108 that obtains the slip of the main body wheel from the information of the travel distance detecting means 106 and the distance measuring means 104. Have In such a configuration, the straight traveling means 107 controls the straight traveling direction based on the output of the direction measuring means 103, and the arithmetic means 108 determines the position shift for each control cycle from the information of the travel distance detecting means 106 and the distance measuring means 104. Is measured and corrective action is performed.

図８（ａ）および（ｂ）は、特許文献２における移動ロボット１１１の装置構成を示す。
移動ロボット１１１は、モータを有し走行回転速度やステアリング角が制御されて動作する走行用車輪１１２、本体周囲の物体までの距離を計測する超音波センサ１１３、センサデータ取り込み手段１１７、絶対位置補正手段１１８、経路追従手段１１９を有し車輪回転速度・方向制御装置１１５に指令を与える演算処理装置１１４、走行用車輪１１２を制御する車輪回転速度・方向制御装置１１５、走行用車輪１１２の回転数・方向から移動ロボット１１１の位置・姿勢を求めるデッドレコニング装置１１６から構成される。
演算処理装置１１４において、センサデータ取り込み手段１１７は超音波センサ１１３を起動し、周囲の空き空間距離データを取り込む。この取り込み後センサデータ取り込み手段１１７はデッドレコニング装置１１６からロボットの位置座標及び進行方向角を読み込み、距離データと共に経路追従手段１１９に伝達する。経路追従手段１１９は距離データ、ロボットの位置座標及び進行方向角を読み込み、これらデータに基づいて現在位置が予め設定してある絶対位置補正地点であるかどうかを判断する。絶対位置補正地点にロボットが到着していると判断された場合、絶対位置補正手段１１８は、超音波センサ１１３を起動して周囲の壁までの距離を測定させる。そして、その距離データの値が理想値を示していない場合には、位置、方向の補正動作を行っている。
このように、従来の移動ロボット制御装置は、すべりなどによる位置誤差を補正するために超音波センサなどの外界センサを用いて、ロボット位置を計測し、目標位置とのずれを補正しているのである。
特開平０５−２５００３２号公報（第３−４頁、図１）特開平０６−１４９３５８号公報（第２−３頁、図１） 8A and 8B show a device configuration of the mobile robot 111 in Patent Document 2. FIG.
The mobile robot 111 has a motor 112 that has a motor and operates by controlling the traveling rotational speed and steering angle, an ultrasonic sensor 113 that measures the distance to an object around the body, a sensor data capturing unit 117, an absolute position correction. Means 118, route follower 119, arithmetic processing device 114 for giving a command to wheel rotation speed / direction control device 115, wheel rotation speed / direction control device 115 for controlling traveling wheel 112, and rotation speed of traveling wheel 112 -It is comprised from the dead reckoning apparatus 116 which calculates | requires the position and attitude | position of the mobile robot 111 from a direction.
In the arithmetic processing unit 114, the sensor data capturing unit 117 activates the ultrasonic sensor 113 and captures surrounding free space distance data. The post-capture sensor data capturing unit 117 reads the position coordinates and the traveling direction angle of the robot from the dead reckoning device 116 and transmits them to the path following unit 119 together with the distance data. The route following unit 119 reads the distance data, the robot position coordinates, and the traveling direction angle, and determines whether the current position is a preset absolute position correction point based on these data. If it is determined that the robot has arrived at the absolute position correction point, the absolute position correction unit 118 activates the ultrasonic sensor 113 to measure the distance to the surrounding wall. When the value of the distance data does not indicate the ideal value, the position and direction correction operation is performed.
As described above, the conventional mobile robot control device uses an external sensor such as an ultrasonic sensor to correct a position error due to slipping, etc., so that the robot position is measured and the deviation from the target position is corrected. is there.
JP 05-250032 A (page 3-4, FIG. 1) Japanese Patent Laid-Open No. 06-149358 (page 2-3, FIG. 1)

特許文献１の移動ロボット制御装置は、超音波センサと内界センサを用い、制御周期毎に距離誤差によりすべり量を計算し補正を行っているが、動作が直進動作に限定されカーブを曲がる動作など曲線動作には対応できず、汎用的な動作には適用できないという問題がある。
また、特許文献２の移動ロボット制御装置は、停止している状態から補正動作が実行されるため、シームレスな連続移動ができずに移動時間が長くなるという問題がある。また、超音波センサによる補正量を、１つの取り付け面にあるセンサに関し、平面までの計測距離の大小により決定しているため、正確な値を求めておらず補正量の精度が低下するという問題や、平面以外の一般的な形状の周囲物体との位置関係を指定できないという問題もある。
本発明はこのような問題点に鑑みてなされたものであり、動作パターンを限定したり一旦停止したりすることなく、シームレスな動作によって位置決め精度を向上する移動ロボット制御装置を提供することを目的とする。 The mobile robot control device of Patent Document 1 uses an ultrasonic sensor and an internal sensor, and calculates and corrects a slip amount by a distance error for each control cycle. There is a problem that it cannot be applied to a general-purpose operation.
In addition, the mobile robot control device of Patent Document 2 has a problem in that since the correction operation is executed from a stopped state, the mobile robot control device cannot perform seamless continuous movement and the movement time becomes long. Further, since the correction amount by the ultrasonic sensor is determined by the magnitude of the measurement distance to the plane for the sensor on one mounting surface, an accurate value is not obtained and the accuracy of the correction amount is reduced. There is also a problem that the positional relationship with a surrounding object having a general shape other than a plane cannot be specified.
The present invention has been made in view of such problems, and an object thereof is to provide a mobile robot control device that improves positioning accuracy by seamless operation without limiting an operation pattern or temporarily stopping the operation pattern. And

上記問題を解決するため、本発明は、次のように構成したのである。
請求項１に記載の発明は、与えられた目標位置を基に直交座標系の速度指令を生成する速度指令生成部と、前記直交座標系の速度指令を各駆動軸の速度指令に変換する逆運動学変換部とを備えた移動ロボット制御装置において、前記逆運動学変換部から出力された各駆動軸速度指令にゲインを乗じて調整した値を出力する速度ゲイン乗算部と、移動ロボットから周囲物体までの距離を計測する環境認識センサと、前記移動ロボットの駆動機構の情報を取得する内界センサと、直交座標系の速度指令および前記環境認識センサおよび前記内界センサによって取得した情報から駆動軸速度補正量を計算する駆動軸速度補正量計算部と、前記駆動軸速度補正量計算部から出力された補正量に基づき前記速度ゲイン乗算部の出力値の補正を行う駆動軸補正部とを備えたことを特徴とするものである。
また、請求項２に記載の発明は、逐次目標対象物教示モデルとのマッチングをとって目標位置に到達するようにモデル補正量を求めるモデル補正量計算部と、前記モデル補正量計算部にて求められたモデル補正量により目標値を変更する目標値変更部とを備えたことを特徴とするものである。
また、請求項３に記載の発明は、前記駆動軸速度補正量計算部は、前記環境認識センサおよび前記内界センサによって取得した情報から自己位置推定を行って求められた推定位置を微分した後に移動平均を施し、その結果得られた値と前記速度指令生成部からの出力値との偏差を計算し、前記偏差を積分したものに位置ループゲイン乗算、逆運動学変換、フィルタによる平滑化の処理を施して駆動軸速度補正量を出力することを特徴とするものである。
また、請求項４に記載の発明は、前記モデル補正量計算部は、ＩＣＰ（ＩｔｅｒａｔｉｖｅＣｌｏｓｅｓｔＰｏｉｎｔ）法により前記目標対象物教示モデルとのマッチングを行って補正量を計算することを特徴とするものである。
また、請求項５に記載の発明は、前記目標対象物教示モデルは、地図のノード情報に付帯したノード特徴データより抽出して前記モデル補正量計算部に入力することを特徴とするものである。
また、請求項６に記載の発明は、前記自己位置推定は、環境認識センサおよび内界センサの情報を用いてカルマンフィルタにより位置推定を行うことを特徴とするものである。
In order to solve the above problem, the present invention is configured as follows.
According to the first aspect of the present invention, a speed command generation unit that generates a speed command of an orthogonal coordinate system based on a given target position, and an inverse for converting the speed command of the orthogonal coordinate system into a speed command of each drive axis A mobile robot control device including a kinematic conversion unit, a speed gain multiplication unit that outputs a value obtained by multiplying each drive shaft speed command output from the inverse kinematic conversion unit by a gain, and a surrounding from the mobile robot Driven from an environment recognition sensor that measures the distance to an object, an inner world sensor that acquires information about the driving mechanism of the mobile robot, a speed command of an orthogonal coordinate system, and information acquired by the environment recognition sensor and the inner world sensor Drive shaft speed correction amount calculation unit for calculating the shaft speed correction amount, and drive shaft for correcting the output value of the speed gain multiplication unit based on the correction amount output from the drive shaft speed correction amount calculation unit It is characterized in that it comprises a Tadashibu Prefecture.
According to the second aspect of the present invention, a model correction amount calculation unit that obtains a model correction amount so as to reach the target position by sequentially matching with the target object teaching model, and the model correction amount calculation unit And a target value changing unit that changes the target value according to the obtained model correction amount.
The invention according to claim 3, wherein the drive shaft speed correction amount calculation unit, after differentiating the estimated position obtained by performing a self-position estimation from the information acquired by the environment recognition sensor and said field sensor Apply a moving average, calculate the deviation between the value obtained as a result and the output value from the speed command generator, and integrate the deviation into position loop gain multiplication, inverse kinematic transformation, and smoothing with a filter. This is characterized in that a drive shaft speed correction amount is output after processing.
According to a fourth aspect of the present invention, the model correction amount calculation unit calculates a correction amount by performing matching with the target object teaching model by an ICP (Iterative Closest Point) method. It is.
The invention according to claim 5 is characterized in that the target object teaching model is extracted from node feature data attached to node information of a map and input to the model correction amount calculation unit. .
The invention according to claim 6 is characterized in that the self-position estimation is performed by a Kalman filter using information of an environment recognition sensor and an internal sensor .

請求項１、３、６に記載の発明によると、自己位置推定値から制御周期毎に常に位置補正を行うようにしているため、動作パターンを限定したり一旦停止したりすることなく、シームレスな動作によって位置決め精度を向上することができる。
請求項２、４、５に記載の発明によると、逐次目標対象物教示モデルとのマッチングをとって目標値の補正を行うようにしているため、目標位置に不確かさを持った簡易的な地図においても一般的な形状の周囲物体との位置関係を指定して正確な補正量を求め、動作パターンを限定したり一旦停止したりすることなく、シームレスな動作によって位置決め精度を向上することができる。
According to the first, third, and sixth aspects of the present invention, since the position correction is always performed for each control period from the self-position estimation value, the operation pattern is not limited or stopped temporarily. The positioning accuracy can be improved by the operation.
According to the second, fourth, and fifth aspects of the invention, since the target value is corrected by sequentially matching with the target object teaching model, a simple map having uncertainty in the target position. In addition, it is possible to improve the positioning accuracy by specifying a positional relationship with a surrounding object of a general shape and obtaining an accurate correction amount, and without restricting the operation pattern or temporarily stopping it. .

以下、本発明の実施の形態について図を参照して説明する。 Hereinafter, embodiments of the present invention will be described with reference to the drawings.

図１は、本発明の移動ロボット制御装置の装置構成を示す図である。移動ロボット制御装置２は、例えば、ＣＰＵやメモリを内蔵した計算機であり、速度指令生成部１１、逆運動学変換部１２、速度ゲイン乗算部１３、駆動軸速度補正量計算部１４、駆動軸補正部１５から構成され、センサなどの情報を入力して駆動軸に対する速度指令を計算し出力する。
速度指令生成部１１は、上位の制御装置から与えられる目標位置の情報を基に補間・加減速パターン算出を行い、直交座標系速度指令を生成する。逆運動学変換部１２は、速度指令生成部１１から出力された直交座標系の速度指令を駆動軸の速度指令に変換する。速度ゲイン乗算部１３は、逆運動学変換部１２から出力された駆動軸速度指令を調整して駆動軸フィードフォワード速度指令を出力する。駆動軸速度補正量計算部１４は、直交座標系速度指令および環境認識センサ、内界センサによる情報から駆動軸速度補正量を計算する。
駆動軸補正部１５は、駆動軸速度補正量計算部１４から出力された駆動軸速度補正量に基づき速度ゲイン乗算部１３の出力値の補正を制御周期毎に行い、駆動軸に対する速度指令である駆動軸補正速度指令を出力する。 FIG. 1 is a diagram showing a device configuration of a mobile robot control device of the present invention. The mobile robot controller 2 is, for example, a computer with a built-in CPU and memory, and includes a speed command generation unit 11, an inverse kinematics conversion unit 12, a speed gain multiplication unit 13, a drive shaft speed correction amount calculation unit 14, and a drive axis correction. It is comprised from the part 15, inputs information, such as a sensor, calculates and outputs the speed command with respect to a drive shaft.
The speed command generation unit 11 performs interpolation / acceleration / deceleration pattern calculation based on information on the target position given from the host controller, and generates an orthogonal coordinate system speed command. The inverse kinematic conversion unit 12 converts the speed command of the orthogonal coordinate system output from the speed command generation unit 11 into the speed command of the drive axis. The speed gain multiplication unit 13 adjusts the drive shaft speed command output from the inverse kinematics conversion unit 12 and outputs a drive shaft feedforward speed command. The drive shaft speed correction amount calculation unit 14 calculates the drive shaft speed correction amount from the information from the orthogonal coordinate system speed command, the environment recognition sensor, and the internal sensor.
The drive axis correction unit 15 corrects the output value of the speed gain multiplication unit 13 for each control cycle based on the drive axis speed correction amount output from the drive axis speed correction amount calculation unit 14, and is a speed command for the drive axis. Outputs drive axis correction speed command.

図２（ａ）および（ｂ）は、図１の移動ロボット制御装置２を搭載した移動ロボットの装置構成を示す図である。移動ロボット１は、移動ロボット制御装置２、モータ制御装置３、モータ４、エンコーダ５、駆動機構６、環境認識センサ７から構成される。
モータ制御装置３は、移動ロボット制御装置２からの指令を受けモータ４の駆動制御を行う。モータ４は駆動機構６を駆動する。エンコーダ５は、モータ４の回転量を計測し、内界センサ情報を生成する。また、内界センサ情報はエンコーダだけでなくジャイロセンサを移動ロボット１に組み込んで適用するようにしても良い。駆動機構６は、モータ４の出力軸に接続された車輪あるいは操舵軸を複数組有し移動ロボット１を駆動する。
環境認識センサ７は、例えば２次元レーザレンジファインダのような２次元スキャン型距離センサであり、そのレーザ光線が、左右に傾いていない平面（スキャン平面）内において、センサ取付位置を中心にして扇状にスキャンされ、壁などの周囲物体までの距離を計測する。
このような構成において、移動ロボット制御装置２は、環境認識センサ７およびエンコーダ５からの情報を取り込み、駆動軸の速度指令を補正しながらモータ制御装置３に指令を出力してモータ４、駆動機構６を介して移動ロボット１を制御しているのである。
本発明が従来技術と異なる部分は、速度ゲイン乗算部１３、駆動軸速度補正量計算部１４、駆動軸補正部１５を備えた部分である。 2 (a) and 2 (b) are diagrams showing a device configuration of a mobile robot equipped with the mobile robot control device 2 of FIG. The mobile robot 1 includes a mobile robot control device 2, a motor control device 3, a motor 4, an encoder 5, a drive mechanism 6, and an environment recognition sensor 7.
The motor control device 3 performs drive control of the motor 4 in response to a command from the mobile robot control device 2. The motor 4 drives the drive mechanism 6. The encoder 5 measures the rotation amount of the motor 4 and generates internal sensor information. Further, the inner world sensor information may be applied by incorporating not only an encoder but also a gyro sensor in the mobile robot 1. The drive mechanism 6 has a plurality of sets of wheels or steering shafts connected to the output shaft of the motor 4 and drives the mobile robot 1.
The environment recognition sensor 7 is a two-dimensional scanning distance sensor such as a two-dimensional laser range finder, for example, and a fan-shaped laser beam centered on the sensor mounting position in a plane that is not tilted to the left or right (scan plane). To measure the distance to surrounding objects such as walls.
In such a configuration, the mobile robot control device 2 takes in information from the environment recognition sensor 7 and the encoder 5, outputs a command to the motor control device 3 while correcting the speed command of the drive shaft, and outputs the motor 4 and the drive mechanism. The mobile robot 1 is controlled via 6.
The present invention is different from the prior art in that the speed gain multiplication unit 13, the drive shaft speed correction amount calculation unit 14, and the drive shaft correction unit 15 are provided.

図３は移動ロボット制御装置２において駆動軸補正速度指令を計算する処理を示すブロック図である。この図を用いて本発明における処理を説明する。
速度指令生成部１１から出力された直交座標系速度指令（ベクトル）Ｖ_ｒｅｆをもとに、逆運動学変換部１２、速度ゲイン乗算部１３により、駆動軸フィードフォワード速度指令（ベクトル）ｗ_refを次式により計算する。 FIG. 3 is a block diagram showing processing for calculating a drive axis correction speed command in the mobile robot controller 2. The processing in the present invention will be described with reference to this figure.
Based on the orthogonal coordinate system speed command (vector) V _ref output from the speed command generator 11, the inverse kinematics converter 12 and the speed gain multiplier 13 generate a drive shaft feedforward speed command (vector) w _ref . Calculate with the following formula.

ｗ_ref ＝Ｋ_ｆＪ^−１Ｖ_ｒｅｆ（式１） w _ref = K _f J ⁻¹ V _ref (Formula 1)

ここでＪ^−１はヤコビアンの逆行列、Ｋ_ｆは速度ゲインの行列（対角行列）を表す。
次に、駆動軸速度補正量計算部１４にて、直交座標系速度指令（ベクトル）Ｖ_ｒｅｆおよびセンサ情報を基にカルマンフィルタにより自己位置推定された推定位置（ベクトル）Ｐ_ｅをもとに、位置偏差（ベクトル）ｅ_ｐｏｓを次式により計算する。 Here, J ⁻¹ represents a Jacobian inverse matrix, and K _f represents a velocity gain matrix (diagonal matrix).
Next, in the drive shaft speed correction amount calculation unit 14, the estimated position that has been self-localization by Kalman filter based on an orthogonal coordinate system speed command (vector) V _ref and the sensor information (vector) P _e based, location The deviation (vector) e _pos is calculated by the following equation.

ｅ_ｐｏｓ＝（Ｖ_ｒｅｆ−Ｆ_ｍ（ｓＰ_ｅ））／ｓ（式２） e _pos = (V _ref −F _m (sP _e )) / s (Formula 2)

ここでｓは、微分器をラプラス変換したもの、１／ｓは積分器をラプラス変換したものを表す。Ｆ_ｍ（ｘ）は、ｘの移動平均を表す。
さらに位置偏差（ベクトル）ｅ_ｐｏｓから駆動軸速度補正量（ベクトル）Δｗを次式により計算する。 Here, s represents the Laplace transform of the differentiator, and 1 / s represents the Laplace transform of the integrator. F _m (x) represents a moving average of x.
Further, a drive shaft speed correction amount (vector) Δw is calculated from the position deviation (vector) e _pos by the following equation.

Δｗ＝Ｊ^−１Ｋ_Ｐｅ_ｐｏｓ／（ｓＴ＋１）（式３） ^{_{_{Δw = J -1 K P e pos}}} / (sT + 1) ( Equation 3)

ここでＪ^−１は、ヤコビアンの逆行列、Ｋ_Ｐは、位置ループゲインの行列（対角行列）を表す。１／（ｓＴ＋１）は、平滑化フィルタを表し、ここでは、１次遅れフィルタを使用している。
最後に、駆動軸補正部１５にて、駆動軸フィードフォワード速度指令（ベクトル）ｗ_refおよび駆動軸速度補正量（ベクトル）Δｗより駆動軸補正速度指令（ベクトル）ｗ’_refを次式のように計算し、モータ制御装置３に出力する。 Here, J ⁻¹ represents a Jacobian inverse matrix, and K _P represents a position loop gain matrix (diagonal matrix). 1 / (sT + 1) represents a smoothing filter, and here, a first-order lag filter is used.
Finally, the drive axis correction unit 15 determines the drive axis correction speed command (vector) w ′ _ref from the drive axis feedforward speed command (vector) w _ref and the drive axis speed correction amount (vector) Δw as follows: Calculate and output to the motor control device 3.

ｗ’_ref ＝ｗ_ref ＋ Δｗ（式４） w ′ _ref = w _ref + Δw (Formula 4)

このように、自己位置推定値から制御周期毎に常に位置補正を行うようにしているため、動作パターンを限定したり一旦停止したりすることなく、シームレスな動作によって位置決め精度を向上することができる。 As described above, since position correction is always performed for each control cycle from the self-position estimation value, positioning accuracy can be improved by seamless operation without limiting the operation pattern or temporarily stopping the operation pattern. .

実施例２の実施の形態について図４を参照して説明する。図４は、本発明の移動ロボット制御装置の装置構成を示す図である。図４において、実施例１と異なるのは、モデル補正量計算部１６および目標値変更部１７を付加した部分である。モデル補正量計算部１６は、地図データより抽出した目標対象物教示モデルおよび環境認識センサ７からの情報から逐次マッチングをとって目標位置に到達するようにモデル補正量を計算する。目標値変更部１７は、モデル補正量計算部にて求められたモデル補正量により目標値を変更する。
図５に示すようにモデル補正量計算部１６にて、ＩＣＰ（ＩｔｅｒａｔｉｖｅＣｌｏｓｅｓｔＰｏｉｎｔ）法により逐次目標対象物教示モデルとのマッチングを行って、現在センサ情報と目標対象物教示モデルのずれ量を求め、ずれ量の符号を反転し座標変換を行って補正量を計算する。
目標対象物教示モデルの地図データからの抽出は、図６に示すように予め教示されたノード情報に付帯したノード特徴データより、目標とする移動ロボット１の姿勢およびセンサのスキャン領域から、一部を切り出すようにして行う。ノード特徴データは、移動ロボット１がそのノードに到達した時の壁などの周囲物体との位置関係を３６０°分登録したものであり、環境認識センサ７を用いて予め教示される。図６の場合、目標とする移動ロボット１の姿勢が９０°でセンサのスキャン領域−４５°から４５°までの場合の目標対象物教示モデルを切り出した様子を示している。このように地図より抽出された目標対象物教示モデルとのマッチングを逐次とり、補正量が求められた場合は目標値を変更する。補正量が閾値以下になったら目標値変更をやめ、その状態で位置偏差（ベクトル）ｅ_ｐｏｓが閾値以下になったら移動完了とする。 An embodiment of Example 2 will be described with reference to FIG. FIG. 4 is a diagram showing a device configuration of the mobile robot control device of the present invention. In FIG. 4, the difference from the first embodiment is a part to which a model correction amount calculation unit 16 and a target value change unit 17 are added. The model correction amount calculation unit 16 calculates the model correction amount so as to reach the target position by sequentially matching the target object teaching model extracted from the map data and the information from the environment recognition sensor 7. The target value changing unit 17 changes the target value according to the model correction amount obtained by the model correction amount calculating unit.
As shown in FIG. 5, the model correction amount calculation unit 16 sequentially matches the target object teaching model by the ICP (Iterative Closest Point) method to obtain the deviation amount between the current sensor information and the target object teaching model. The correction amount is calculated by inverting the sign of the deviation amount and performing coordinate conversion.
The extraction of the target object teaching model from the map data is partly based on the target mobile robot 1 posture and the sensor scan area based on the node feature data attached to the previously taught node information as shown in FIG. It is done by cutting out. The node feature data is obtained by registering the positional relationship with a surrounding object such as a wall when the mobile robot 1 reaches the node by 360 °, and is previously taught using the environment recognition sensor 7. FIG. 6 shows a state in which the target object teaching model is cut out when the posture of the target mobile robot 1 is 90 ° and the sensor scan region is −45 ° to 45 °. Thus, matching with the target object teaching model extracted from the map is sequentially performed, and when the correction amount is obtained, the target value is changed. Correction stop target value changes when equal to or less than a threshold value, the position deviation in this state (vector) e _pos is a movement completion When equal to or less than a threshold value.

このように、逐次目標対象物教示モデルとのマッチングをとって目標値の補正を行うようにしているため、目標位置に不確かさを持った簡易的な地図においても一般的な形状の周囲物体との位置関係を指定して正確な補正量を求め、動作パターンの限定や一旦停止することもなく、シームレスな動作によって位置決め精度を向上することができる。 In this way, since the target value is corrected by sequentially matching with the target object teaching model, even in a simple map with uncertainty in the target position, By specifying the positional relationship, the accurate correction amount is obtained, and positioning accuracy can be improved by seamless operation without limiting the operation pattern or temporarily stopping.

本発明の第１の実施例における移動ロボット制御装置の装置構成図1 is a diagram showing the configuration of a mobile robot control device according to a first embodiment of the present invention. 本発明の移動ロボット制御装置を移動ロボットに搭載した場合の装置構成図Device configuration diagram when the mobile robot control device of the present invention is mounted on a mobile robot 本発明の移動ロボット制御装置における処理を示すブロック図The block diagram which shows the process in the mobile robot control apparatus of this invention 本発明の第２の実施例における移動ロボット制御装置の装置構成図Device configuration diagram of mobile robot controller in second embodiment of the present invention 本発明の第２の実施例におけるモデル補正量を求める処理を説明する説明図Explanatory drawing explaining the process which calculates | requires the model correction amount in 2nd Example of this invention 本発明の第２の実施例における目標対象物教示モデル抽出処理を説明する説明図Explanatory drawing explaining the target object teaching model extraction process in 2nd Example of this invention. 従来例１の移動ロボットの装置構成図Device configuration diagram of mobile robot of conventional example 1 従来例２の移動ロボットの装置構成図Device configuration diagram of mobile robot of conventional example 2

符号の説明Explanation of symbols

１移動ロボット
２移動ロボット制御装置
３モータ制御装置
４モータ
５エンコーダ
６駆動機構
７環境認識センサ
１１速度指令生成部
１２逆運動学変換部
１３速度ゲイン乗算部
１４駆動軸速度補正量計算部
１５駆動軸補正部
１６モデル補正量計算部
１７目標値変更部
１０１移動ロボット
１０２駆動手段
１０３方向計測手段
１０４測距手段
１０５走行制御手段
１０６走行距離検出手段
１０７直進手段
１０８演算手段
１１１移動ロボット
１１２走行用車輪
１１３超音波センサ
１１４演算処理装置
１１５車輪回転速度・方向制御装置
１１６デッドレコニング装置
１１７センサデータ取り込み手段
１１８絶対位置補正手段
１１９経路追従手段 DESCRIPTION OF SYMBOLS 1 Mobile robot 2 Mobile robot control apparatus 3 Motor control apparatus 4 Motor 5 Encoder 6 Drive mechanism 7 Environment recognition sensor 11 Speed command generation part 12 Inverse kinematics conversion part 13 Speed gain multiplication part 14 Drive shaft speed correction amount calculation part 15 Drive axis Correction unit 16 Model correction amount calculation unit 17 Target value changing unit 101 Mobile robot 102 Driving unit 103 Direction measuring unit 104 Distance measuring unit 105 Traveling control unit 106 Traveling distance detecting unit 107 Straight running unit 108 Computing unit 111 Mobile robot 112 Traveling wheel 113 Ultrasonic sensor 114 Arithmetic processing device 115 Wheel rotation speed / direction control device 116 Dead reckoning device 117 Sensor data fetching means 118 Absolute position correcting means 119 Path following means

Claims

与えられた目標位置を基に直交座標系の速度指令を生成する速度指令生成部と、前記直交座標系の速度指令を各駆動軸の速度指令に変換する逆運動学変換部とを備えた移動ロボット制御装置において、
前記逆運動学変換部から出力された各駆動軸速度指令にゲインを乗じて調整した値を出力する速度ゲイン乗算部と、
移動ロボットから周囲物体までの距離を計測する環境認識センサと、
前記移動ロボットの駆動機構の情報を取得する内界センサと、
直交座標系の速度指令および前記環境認識センサおよび前記内界センサによって取得した情報から駆動軸速度補正量を計算する駆動軸速度補正量計算部と、
前記駆動軸速度補正量計算部から出力された補正量に基づき前記速度ゲイン乗算部の出力値の補正を行う駆動軸補正部と、を備えたことを特徴とする移動ロボット制御装置。 A movement provided with a speed command generation unit that generates a speed command of an orthogonal coordinate system based on a given target position, and an inverse kinematics conversion unit that converts the speed command of the orthogonal coordinate system into a speed command of each drive axis In the robot controller,
A speed gain multiplier that outputs a value adjusted by multiplying each drive shaft speed command output from the inverse kinematics converter by a gain; and
An environment recognition sensor that measures the distance from the mobile robot to surrounding objects;
An internal sensor for acquiring information on the driving mechanism of the mobile robot;
A drive shaft speed correction amount calculation unit for calculating a drive shaft speed correction amount from information acquired by the speed command of the orthogonal coordinate system and the environment recognition sensor and the internal sensor;
A mobile robot control device comprising: a drive axis correction unit that corrects an output value of the speed gain multiplication unit based on a correction amount output from the drive axis speed correction amount calculation unit.

逐次目標対象物教示モデルとのマッチングをとって目標位置に到達するようにモデル補正量を求めるモデル補正量計算部と、
前記モデル補正量計算部にて求められたモデル補正量により目標値を変更する目標値変更部と、を備えたことを特徴とする請求項１記載の移動ロボット制御装置。 A model correction amount calculation unit for obtaining a model correction amount so as to reach the target position by sequentially matching with the target object teaching model;
The mobile robot control device according to claim 1, further comprising: a target value changing unit that changes a target value according to the model correction amount obtained by the model correction amount calculating unit.

前記駆動軸速度補正量計算部は、前記環境認識センサおよび前記内界センサによって取得した情報から自己位置推定を行って求められた推定位置を微分した後に移動平均を施し、その結果得られた値と前記速度指令生成部からの出力値との偏差を計算し、前記偏差を積分したものに位置ループゲイン乗算、逆運動学変換、フィルタによる平滑化の処理を施して駆動軸速度補正量を出力することを特徴とする請求項１または２記載の移動ロボット制御装置。 The drive shaft speed correction amount calculation unit performs a moving average after differentiating an estimated position obtained by performing self-position estimation from information acquired by the environment recognition sensor and the internal sensor, and a value obtained as a result the deviation between the output value from the speed command generating portion calculates the position loop gain multiplied the integral of the deviation, the inverse kinematics transformation, the output drive shaft speed correction amount subjected to a treatment of smoothing by filter The mobile robot control device according to claim 1, wherein:

前記モデル補正量計算部は、ＩＣＰ（ＩｔｅｒａｔｉｖｅＣｌｏｓｅｓｔＰｏｉｎｔ）法により前記目標対象物教示モデルとのマッチングを行って補正量を計算することを特徴とする請求項２記載の移動ロボット制御装置。 The mobile robot controller according to claim 2, wherein the model correction amount calculation unit calculates a correction amount by performing matching with the target object teaching model by an ICP (Iterative Closest Point) method.

前記目標対象物教示モデルは、地図のノード情報に付帯したノード特徴データより抽出して前記モデル補正量計算部に入力することを特徴とする請求項２または４記載の移動ロボット制御装置。 5. The mobile robot controller according to claim 2, wherein the target object teaching model is extracted from node feature data attached to node information on a map and input to the model correction amount calculation unit.

前記自己位置推定は、環境認識センサおよび内界センサの情報を用いてカルマンフィルタにより位置推定を行うことを特徴とする請求項３記載の移動ロボット制御装置。
4. The mobile robot control device according to claim 3, wherein the self-position estimation is performed by Kalman filter using information of an environment recognition sensor and an internal sensor .