JP2009050936A - Interference determination device and leg wheel type robot - Google Patents
Interference determination device and leg wheel type robot Download PDFInfo
- Publication number
- JP2009050936A JP2009050936A JP2007218646A JP2007218646A JP2009050936A JP 2009050936 A JP2009050936 A JP 2009050936A JP 2007218646 A JP2007218646 A JP 2007218646A JP 2007218646 A JP2007218646 A JP 2007218646A JP 2009050936 A JP2009050936 A JP 2009050936A
- Authority
- JP
- Japan
- Prior art keywords
- leg
- scanning
- feature point
- coordinate system
- distance measuring
- 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.)
- Pending
Links
Images
Landscapes
- Manipulator (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
Description
本発明は、障害物との干渉判定を行う装置および段差を乗り越えて移動できる脚車輪型ロボットに係り、特に、実際の干渉を伴うことなく干渉判定を行うことができ、部品点数および重量の増加を抑制し、干渉判定の制約を低減するのに好適な干渉判定装置および脚車輪型ロボットに関する。 The present invention relates to an apparatus for performing interference determination with an obstacle and a leg-wheel type robot that can move over a step, and in particular, can perform interference determination without actual interference, increasing the number of parts and weight. The present invention relates to an interference determination device and a leg-wheel type robot suitable for suppressing the interference and reducing the interference determination constraint.
ロボットの制御においては、ロボットに対して障害物が干渉（衝突）したか否かを判定し、その判定結果に基づいて障害物を回避する制御等の各種の制御を行っている。干渉を判定する技術としては、例えば、特許文献１、２記載の技術が知られている。
特許文献１記載の発明は、減速機を介してモータにより駆動されるロボットにおいて、モータが減速機に出力するトルクからロボットの逆動力学演算で求めた動力学トルクを差し引くことにより、衝突による外力をセンサレスで検出し、外力の検出値があらかじめ設定した所定の閾値より大きければアームが外力を受けたと判断する。
In the control of the robot, it is determined whether or not an obstacle has interfered (collised) with the robot, and various controls such as control for avoiding the obstacle are performed based on the determination result. As a technique for determining interference, for example, techniques described in
In the invention described in
特許文献２記載の発明は、脚部の足平上にマークを付けておき、基体に取り付けられた視覚センサから取得された画像データに基づいて、歩行に対する障害物と脚部の位置関係を把握しながら歩行動作を行う。
しかしながら、特許文献１記載の発明にあっては、ロボットに対して障害物が実際に干渉しないと干渉したことが判定できないため、障害物を事前に回避する制御を行う上では実用的ではないという問題があった。
また、特許文献２記載の発明にあっては、干渉判定専用のカメラを設ける必要があるため、部品点数および重量が増加するという問題があった。
However, in the invention described in
Further, the invention described in
また、干渉を判定するには、カメラで撮影した画像に脚部と障害物の両方が含まれている必要があるため、干渉の判定に制約があるという問題もあった。例えば、脚部の可動範囲が大きいため下方を広範に撮影しようとすると、ロボットの前方にある障害物を撮影できず、前方の領域について干渉の判定が行えない。逆に、前方を広範に撮影しようとすると、脚部の可動範囲をすべてカバーすることができず、下方および後方の領域について干渉の判定が行えない。 In addition, in order to determine interference, it is necessary to include both legs and obstacles in an image photographed by a camera, so that there is a problem in that interference determination is limited. For example, if the range of movement of the leg is large and an attempt is made to capture a wide area below, an obstacle in front of the robot cannot be imaged and interference cannot be determined for the area in front. Conversely, if an image of the front is to be taken extensively, the entire range of movement of the legs cannot be covered, and interference cannot be determined for the lower and rear regions.
ところで、ロボットの移動機構は、車輪型、クローラ型、脚型またはこれらを組み合わせた機構に分類される。一般に、車輪型ロボットは、平地での移動性は高いが、段差への適応性が低いという問題がある。また、クローラ型ロボットは、不整地に適し、多少の段差であれば乗り越えられるが、積極的な重心移動ができないために急な階段への適応性が低く、平地における移動性が車輪型ロボットより低いという問題があった。また、脚型ロボットは、階段への適応性が最も優れているが、平地での移動性が極端に低いという問題がある。 By the way, the movement mechanism of the robot is classified into a wheel type, a crawler type, a leg type, or a combination of these. In general, a wheel type robot has a problem of high mobility on a flat ground but low adaptability to a step. Crawler type robots are suitable for rough terrain, and can be overcome if there are a few steps. However, because they cannot actively move the center of gravity, they are less adaptable to steep stairs and are more mobile on flat ground than wheel type robots. There was a problem of being low. In addition, the legged robot has the best adaptability to the stairs, but has the problem of extremely low mobility on flat ground.
車輪型、クローラ型および脚型の問題を解決するため、脚型と車輪型を組み合わせた脚車輪型ロボットが提案されている。
脚車輪型ロボットが段差を乗り越える場合、重心を低くするために大きく開脚し、脚部の可動範囲について脚先と段差の干渉を判定しながら、段差を乗り越えるべく脚部を適切に駆動制御することが考えられる。このように、脚部の可動範囲および障害物の監視範囲が広範囲である場合に、特許文献２記載の発明を適用しようとすると、上記制約の問題がより顕著となる。
In order to solve the problems of the wheel type, the crawler type and the leg type, a leg wheel type robot combining the leg type and the wheel type has been proposed.
When a leg-wheel type robot climbs over a step, it opens wide to lower the center of gravity, and appropriately controls the leg to drive over the step while judging the interference between the tip of the leg and the step over the movable range of the leg. It is possible. In this way, when the leg movable range and the obstacle monitoring range are wide, if the invention described in
そこで、本発明は、このような従来の技術の有する未解決の課題に着目してなされたものであって、実際の干渉を伴うことなく干渉判定を行うことができ、部品点数および重量の増加を抑制し、干渉判定の制約を低減するのに好適な干渉判定装置および脚車輪型ロボットを提供することを目的としている。 Accordingly, the present invention has been made paying attention to such an unsolved problem of the conventional technology, and can perform interference determination without accompanying actual interference, increasing the number of parts and weight. It is an object of the present invention to provide an interference determination device and a leg-wheel type robot suitable for suppressing the interference and reducing the interference determination constraint.
〔発明１〕 上記目的を達成するために、発明１の干渉判定装置は、対象部位について障害物との干渉判定を行う干渉判定装置であって、障害物の特徴点を検出する特徴点検出手段と、前記特徴点および前記対象部位の座標を定める座標系において、前記対象部位またはこれと連動する部位の姿勢および前記対象部位の形状により特定される線、平面または空間であって、前記対象部位の形状に対応する線、前記対象部位の表面に対応する平面または前記対象部位に対応する空間と、前記特徴点検出手段で検出した特徴点との位置関係に基づいて、前記対象部位について障害物との干渉判定を行う干渉判定手段とを備える。
[Invention 1] In order to achieve the above object, an interference determination apparatus according to
このような構成であれば、特徴点検出手段により、障害物の特徴点が検出される。そして、干渉判定手段により、特徴点および対象部位の座標を定める座標系において、対象部位の形状に対応する線、対象部位の表面に対応する平面または対象部位に対応する空間と、検出された特徴点との位置関係に基づいて、対象部位について障害物との干渉判定が行われる。この平面等は、対象部位またはこれと連動する部位の姿勢および対象部位の形状により特定される。 With such a configuration, the feature point of the obstacle is detected by the feature point detection means. Then, in the coordinate system for determining the coordinates of the feature point and the target part by the interference determination means, a line corresponding to the shape of the target part, a plane corresponding to the surface of the target part, or a space corresponding to the target part, and the detected feature Based on the positional relationship with the point, interference determination with the obstacle is performed for the target part. This plane or the like is specified by the posture of the target part or a part linked to the target part and the shape of the target part.
したがって、所定の座標系における平面等と特徴点の位置関係に基づいて干渉判定を行うので、実際の干渉を伴う必要はない。また、特許文献２における干渉判定専用のカメラのような専用のセンサを設けなくても、他の用途に用いられるセンサ等を利用することができる。さらに、所定の座標系において対象部位に対応する平面等は姿勢や形状により特定されるため、障害物の特徴点さえ検出できればよく、障害物の監視範囲が対象部位の可動範囲に制約されない。
Therefore, since the interference determination is performed based on the positional relationship between the plane and the like in the predetermined coordinate system and the feature point, it is not necessary to accompany actual interference. Moreover, even if a dedicated sensor such as a camera dedicated to interference determination in
ここで、特徴点および対象部位の座標を定める座標系としては、特徴点の絶対座標（例えば、接地面を基準とする座標）または相対座標（例えば、干渉判定装置を基準とする座標）を定める座標系、対象部位の絶対座標（例えば、接地面を基準とする座標）または相対座標（例えば、対象部位と連動する部位を基準とする座標）を定める座標系、その他任意の座標系が含まれる。なお、特徴点の座標を定める座標系と、対象部位の座標を定める座標系が異なる場合は、一方の座標系の座標を他方の座標系の座標に変換するか、両方の座標系の座標を、特徴点および対象部位の座標を定める他の座標系の座標に変換すればよい。 Here, as a coordinate system for determining the coordinates of the feature points and the target part, absolute coordinates (for example, coordinates with reference to the ground plane) or relative coordinates (for example, coordinates with reference to the interference determination device) of the feature points are determined. A coordinate system, a coordinate system that defines absolute coordinates (for example, coordinates with reference to the ground plane) or relative coordinates (for example, coordinates with reference to a part linked to the target part), and any other coordinate system are included. . If the coordinate system that defines the coordinates of the feature points is different from the coordinate system that defines the coordinates of the target part, either convert the coordinates of one coordinate system to the coordinates of the other coordinate system, or change the coordinates of both coordinate systems What is necessary is just to convert into the coordinate of another coordinate system which determines the coordinate of a feature point and an object part.
また、対象部位と連動する部位とは、対象部位と連なって動作する部位であって、自身の姿勢から対象部位の姿勢を間接的に特定できるものをいう。
また、干渉判定とは、対象部位に対する障害物の干渉に関する判定をいい、干渉しないことを判定すること、干渉することを判定すること、干渉するか否かを判定すること、干渉の度合いを判定すること、干渉までの度合い（例えば、特徴点と対象部位の間の距離）を判定することが含まれる。以下、発明２の脚車輪型ロボットにおいて同じである。
Moreover, the site | part interlock | cooperating with a target site | part is a site | part which operate | moves in connection with a target site | part, Comprising: The thing which can specify the attitude | position of a target site | part indirectly from own attitude | position.
Interference determination refers to determination regarding interference of an obstacle with respect to a target part, determining that there is no interference, determining whether to interfere, determining whether or not to interfere, and determining the degree of interference Determining the degree of interference (for example, the distance between the feature point and the target part). Hereinafter, the same applies to the leg-wheel type robot of the second aspect.
また、線には、直線、線分、多次曲線、円、楕円その他の曲線が含まれる。以下、発明２の脚車輪型ロボットにおいて同じである。 Lines include straight lines, line segments, multi-order curves, circles, ellipses and other curves. Hereinafter, the same applies to the leg-wheel type robot of the second aspect.
〔発明２〕 一方、上記目的を達成するために、発明２の脚車輪型ロボットは、基体と、前記基体に対して自由度を有して連結された脚部と、前記脚部に回転可能に設けられた車輪と、物体センサとを備え、前記脚部の駆動および前記車輪の回転により移動する脚車輪型ロボットであって、前記物体センサの測定結果に基づいて障害物の特徴点を検出する特徴点検出手段と、前記特徴点および前記対象部位の座標を定める座標系において、当該脚車輪型ロボットの姿勢および前記対象部位の形状により特定される線、平面または空間であって、前記対象部位の形状に対応する線、前記対象部位の表面に対応する平面または前記対象部位に対応する空間と、前記特徴点検出手段で検出した特徴点との位置関係に基づいて、前記対象部位について障害物との干渉判定を行う干渉判定手段とを備える。 [Invention 2] On the other hand, in order to achieve the above object, the leg-wheel type robot of Invention 2 is a base, a leg connected to the base with a degree of freedom, and rotatable to the leg. A leg-wheel type robot having a wheel and an object sensor that are moved by driving the leg and rotating the wheel, and detecting a feature point of an obstacle based on a measurement result of the object sensor In the coordinate system for defining the feature point detection means and the coordinates of the feature point and the target part, a line, a plane, or a space specified by the posture of the leg wheel type robot and the shape of the target part, the target Based on the positional relationship between the line corresponding to the shape of the part, the plane corresponding to the surface of the target part or the space corresponding to the target part, and the feature point detected by the feature point detecting means, the target part And a collision determination unit that performs collision determination of the harm thereof.
このような構成であれば、段差等の障害物のあるところでは、物体センサにより、その測定方向について測定が行われ、特徴点検出手段により、物体センサの測定結果に基づいて障害物の特徴点が検出される。そして、干渉判定手段により、特徴点および対象部位の座標を定める座標系において、対象部位の形状に対応する線、対象部位の表面に対応する平面または対象部位に対応する空間と、検出された特徴点との位置関係に基づいて、対象部位について障害物との干渉判定が行われる。この平面等は、脚車輪型ロボットの姿勢および対象部位の形状により特定される。 With such a configuration, where there is an obstacle such as a step, measurement is performed in the measurement direction by the object sensor, and the feature point of the obstacle is detected based on the measurement result of the object sensor by the feature point detection means. Is detected. Then, in the coordinate system for determining the coordinates of the feature point and the target part by the interference determination means, a line corresponding to the shape of the target part, a plane corresponding to the surface of the target part, or a space corresponding to the target part, and the detected feature Based on the positional relationship with the point, interference determination with the obstacle is performed for the target part. This plane or the like is specified by the posture of the leg wheel type robot and the shape of the target part.
したがって、所定の座標系における平面等と特徴点の位置関係に基づいて干渉判定を行うので、実際の干渉を伴う必要はない。また、特許文献２における干渉判定専用のカメラのような専用のセンサを設けなくても、他の用途に用いられる物体センサを利用することができる。さらに、所定の座標系において対象部位に対応する平面等は姿勢や形状により特定されるため、障害物の特徴点さえ検出できればよく、障害物の監視範囲が対象部位の可動範囲に制約されない。
Therefore, since the interference determination is performed based on the positional relationship between the plane and the like in the predetermined coordinate system and the feature point, it is not necessary to accompany actual interference. In addition, an object sensor used for other purposes can be used without providing a dedicated sensor such as a camera dedicated to interference determination in
また、脚車輪型ロボットにおいては、例えば、物体センサを基体に設け、対象部位に対する障害物の干渉判定を行うことが考えられる。この場合、基体に対して自由度の範囲で脚部が駆動することから、脚車輪型ロボットの姿勢により、対象部位に対する物体センサの位置が変化することとなる。また、物体センサを走査する場合は、対象部位に対する物体センサの向きが変化することとなる。 In a leg wheel type robot, for example, it is conceivable to provide an object sensor on a base and perform interference determination of an obstacle with respect to a target part. In this case, since the leg portion is driven within the range of the degree of freedom with respect to the base body, the position of the object sensor with respect to the target portion changes depending on the posture of the leg wheel type robot. Further, when scanning the object sensor, the direction of the object sensor with respect to the target part changes.
このように、対象部位に対する物体センサの位置や向きが変化する場合は、物体センサの測定結果に基づいて検出される特徴点の座標と、対象部位の座標は、物体センサおよび対象部位の動作の系が異なることからそれぞれ異なる座標系により定められる。しかしながら、座標系が異なる場合は、対象部位に対応する平面等と特徴点の位置関係を直ちには特定することができない。そこで、座標系の変換により平面等と特徴点の座標系を一致させて（すなわち、特徴点および対象部位の座標を定める座標系において）それらの位置関係を特定する必要があるところ、本発明は、脚車輪型ロボットの姿勢および対象部位の形状に基づいてこれを実現することができる。 As described above, when the position and orientation of the object sensor with respect to the target part change, the coordinates of the feature point detected based on the measurement result of the object sensor and the coordinates of the target part are the movements of the object sensor and the target part. Since the systems are different, they are determined by different coordinate systems. However, when the coordinate systems are different, it is not possible to immediately specify the positional relationship between the plane corresponding to the target part and the feature point. Therefore, it is necessary to make the coordinate system of the feature point and the coordinate system of the feature point coincide with each other by conversion of the coordinate system (that is, in the coordinate system that determines the coordinates of the feature point and the target part) and specify the positional relationship between them. This can be realized based on the posture of the leg wheel type robot and the shape of the target part.
したがって、対象部位との位置関係によらず物体センサを設置することができる。
さらに、段差のあるところでは、自由度の範囲で脚部が可動し、段差を乗り越えることができる。したがって、脚型ロボットと同様に段差への適応性が高い。
一方、平地では、車輪走行で移動することができる。したがって、車輪型ロボットと同様に平地での移動性が高い。
Therefore, the object sensor can be installed regardless of the positional relationship with the target part.
Further, where there is a step, the leg can move within a range of degrees of freedom, and the step can be overcome. Therefore, the adaptability to a level difference is high like a legged robot.
On the other hand, on a flat ground, it can move by wheel running. Therefore, the mobility on the flat ground is high like the wheel type robot.
ここで、特徴点および対象部位の座標を定める座標系としては、特徴点の絶対座標（例えば、接地面を基準とする座標）または相対座標（例えば、脚車輪型ロボットを基準とする座標）を定める座標系、対象部位の絶対座標（例えば、接地面を基準とする座標）または相対座標（例えば、脚部を基準とする座標）を定める座標系、その他任意の座標系が含まれる。なお、特徴点の座標を定める座標系と、対象部位の座標を定める座標系が異なる場合は、一方の座標系の座標を他方の座標系の座標に変換するか、両方の座標系の座標を、特徴点および対象部位の座標を定める他の座標系の座標に変換すればよい。
また、脚車輪型ロボットの姿勢には、脚車輪型ロボットの全体の姿勢のほか、脚車輪型ロボットの一部分（例えば、脚部、物体センサ）の姿勢も含まれる。
Here, as a coordinate system for determining the coordinates of the feature point and the target part, the absolute coordinates of the feature point (for example, coordinates with reference to the ground plane) or relative coordinates (for example, coordinates with reference to the leg wheel type robot) are used. A coordinate system to be defined, a coordinate system to define absolute coordinates (for example, coordinates with reference to the ground plane) or relative coordinates (for example, coordinates with reference to the legs), and any other coordinate system are included. If the coordinate system that defines the coordinates of the feature points is different from the coordinate system that defines the coordinates of the target part, either convert the coordinates of one coordinate system to the coordinates of the other coordinate system, or change the coordinates of both coordinate systems What is necessary is just to convert into the coordinate of another coordinate system which determines the coordinate of a feature point and an object part.
Further, the posture of the leg wheel type robot includes the posture of a part of the leg wheel type robot (for example, a leg portion and an object sensor) in addition to the whole posture of the leg wheel type robot.
〔発明３〕 さらに、発明３の脚車輪型ロボットは、発明２の脚車輪型ロボットにおいて、前記干渉判定手段は、前記座標系において、前記対象部位の表面に対応する複数の平面で囲まれる空間に前記特徴点が属するか否かを判定することにより、前記対象部位に対して障害物が干渉するか否かを判定する。
[Invention 3] Furthermore, the leg-wheel robot of Invention 3 is the leg-wheel robot of
このような構成であれば、干渉判定手段により、座標系において、対象部位の表面に対応する複数の平面で囲まれる空間に特徴点が属するか否かが判定される。対象部位に対して障害物が干渉した場合は、同空間に特徴点が属し、障害物が干渉していない場合は、同空間に特徴点は属さない。したがって、この判定により、対象部位に対して障害物が干渉するか否かが判定される。 With such a configuration, the interference determination unit determines whether or not the feature point belongs to a space surrounded by a plurality of planes corresponding to the surface of the target part in the coordinate system. When the obstacle interferes with the target part, the feature point belongs to the same space, and when the obstacle does not interfere, the feature point does not belong to the same space. Therefore, this determination determines whether an obstacle interferes with the target part.
〔発明４〕 さらに、発明４の脚車輪型ロボットは、発明３の脚車輪型ロボットにおいて、前記干渉判定手段は、当該脚車輪型ロボットの姿勢および前記対象部位の形状に基づいて、前記対象部位の座標を定める第１座標系の座標を、前記特徴点の座標を定める第２座標系の座標に変換して前記第２座標系における前記複数の平面を特定し、前記第２座標系において、前記複数の平面で囲まれる空間に前記特徴点が属するか否かを判定する。
このような構成であれば、干渉判定手段により、脚車輪型ロボットの姿勢および対象部位の形状に基づいて、第１座標系から第２座標系に座標が変換されて第２座標系における複数の平面が特定され、第２座標系において、複数の平面で囲まれる空間に特徴点が属するか否かが判定される。
[Invention 4] Further, the leg wheel type robot of the
With such a configuration, the interference determination unit converts the coordinates from the first coordinate system to the second coordinate system based on the posture of the leg-wheel robot and the shape of the target part, and a plurality of coordinates in the second coordinate system are obtained. A plane is specified, and it is determined whether or not the feature point belongs to a space surrounded by a plurality of planes in the second coordinate system.
〔発明５〕 さらに、発明５の脚車輪型ロボットは、発明３および４のいずれか１の脚車輪型ロボットにおいて、前記干渉判定手段は、前記特徴点と前記平面上の任意の点を結ぶ直線と、前記平面の法線ベクトルとの内積に基づいて、前記複数の平面で囲まれる空間に前記特徴点が属するか否かを判定する。
このような構成であれば、干渉判定手段により、特徴点と平面上の任意の点を結ぶ直線と、平面の法線ベクトルとの内積に基づいて、複数の平面で囲まれる空間に特徴点が属するか否かが判定される。
[Invention 5] The leg-wheel robot according to Invention 5 is the leg-wheel robot according to any one of
In such a configuration, the feature point is found in a space surrounded by a plurality of planes based on the inner product of a straight line connecting the feature point and an arbitrary point on the plane and the normal vector of the plane by the interference determination unit. It is determined whether or not it belongs.
〔発明６〕 さらに、発明６の脚車輪型ロボットは、発明２ないし５のいずれか１の脚車輪型ロボットにおいて、前記物体センサは、物体上の測定点までの距離を測定する測距センサであり、前記測距センサの測定方向とは異なる第１走査方向に前記測距センサを走査する第１走査手段と、前記測定方向および前記第１走査方向とは異なる第２走査方向に前記測距センサを走査する第２走査手段と、前記第１走査手段および前記第２走査手段の走査範囲で測定可能な前記測定点について前記測距センサの測定結果を取得する測定結果取得手段とを備え、前記特徴点検出手段は、前記測定結果取得手段で取得した測定結果に基づいて前記特徴点を検出する。
このような構成であれば、第１走査手段により、第１走査方向に測距センサが、第２走査手段により、第２走査方向に測距センサがそれぞれ走査可能となる。したがって、障害物の立体的な形状を把握することができる。そして、測定結果取得手段により、第１走査手段および第２走査手段の走査範囲で測定可能な測定点について測距センサの測定結果が取得され、特徴点検出手段により、取得された測定結果に基づいて特徴点が検出される。
[Invention 6] Further, the leg-wheel type robot of the
With this configuration, the first scanning unit can scan the distance measuring sensor in the first scanning direction, and the second scanning unit can scan the distance measuring sensor in the second scanning direction. Therefore, the three-dimensional shape of the obstacle can be grasped. Then, the measurement result acquisition means acquires the measurement result of the distance measuring sensor for the measurement points that can be measured in the scanning range of the first scanning means and the second scanning means, and the feature point detection means based on the acquired measurement results Feature points are detected.
ここで、第１走査手段および第２走査手段としては、例えば、次の構成を採用することができる。
（１）回転機構
前記測距センサの測定方向に対して所定角度をなす第１走査軸の回りに前記測距センサを回転させる第１回転手段と、前記測定方向および前記第１走査軸に対して所定角度をなす第２走査軸の回りに前記測距センサを回転させる第２回転手段とからなる構成である。
Here, as the first scanning unit and the second scanning unit, for example, the following configuration can be adopted.
(1) Rotation mechanism First rotation means for rotating the distance measuring sensor around a first scanning axis that forms a predetermined angle with respect to the measurement direction of the distance measuring sensor, and the measurement direction and the first scanning axis. And a second rotating means for rotating the distance measuring sensor around a second scanning axis forming a predetermined angle.
（２）移動機構
前記測距センサの測定方向とは異なる第１走査方向に前記測距センサを移動させる第１移動手段と、前記測定方向および前記第１走査方向とは異なる第２走査方向に前記測距センサを移動させる第２移動手段とからなる構成である。なお、第１移動手段は、前記第１走査方向に延長する経路を含む第１経路に沿って前記測距センサを移動させてもよいし、第２移動手段は、前記第２走査方向に延長する経路を含む第２経路に沿って前記測距センサを移動させてもよい。以下、（３）、（４）においても同様である。
(2) Moving mechanism First moving means for moving the distance measuring sensor in a first scanning direction different from the measuring direction of the distance measuring sensor, and a second scanning direction different from the measuring direction and the first scanning direction. It comprises a second moving means for moving the distance measuring sensor. The first moving unit may move the distance measuring sensor along a first path including a path extending in the first scanning direction, and the second moving unit extends in the second scanning direction. The distance measuring sensor may be moved along a second route including a route to be performed. The same applies to (3) and (4) below.
（３）回転機構と移動機構の組み合わせ
前記測距センサの測定方向に対して所定角度をなす走査軸の回りに前記測距センサを回転させる回転手段と、前記走査軸の軸方向とは異なる走査方向に前記測距センサを移動させる移動手段とからなる構成である。
(3) Combination of rotating mechanism and moving mechanism Rotating means for rotating the distance measuring sensor around a scanning axis forming a predetermined angle with respect to the measuring direction of the distance measuring sensor, and scanning different from the axial direction of the scanning axis And a moving means for moving the distance measuring sensor in the direction.
（４）移動機構と回転機構の組み合わせ
前記測距センサの測定方向とは異なる走査方向に前記測距センサを移動させる移動手段と、前記走査方向に対して所定角度をなす走査軸の回りに前記測距センサを回転させる回転手段とからなる構成である。
(4) A combination of a moving mechanism and a rotating mechanism The moving means for moving the distance measuring sensor in a scanning direction different from the measuring direction of the distance measuring sensor, and the scanning axis about a predetermined angle with respect to the scanning direction It is the structure which consists of a rotation means to rotate a ranging sensor.
〔発明７〕 さらに、発明７の脚車輪型ロボットは、発明６の脚車輪型ロボットにおいて、前記第１走査手段は、前記測定方向に対して所定角度をなす第１走査軸の回りに前記測距センサを回転させる第１回転手段であり、前記第２走査手段は、前記測定方向および前記第１走査軸に対して所定角度をなす第２走査軸の回りに前記測距センサを回転させる第２回転手段であり、前記測定結果取得手段は、前記第１回転手段により前記測距センサを回転させながら前記第１回転手段の所定単位角度ごとに前記測距センサの測定結果を取得する第１走査を、前記第２回転手段により前記測距センサを回転させながら前記第２回転手段の所定単位角度ごとに行う第２走査を行うことにより、前記第１回転手段の所定単位角度ごとおよび前記第２回転手段の所定単位角度ごとの前記測定結果を取得する。
[Invention 7] Further, the leg-wheel type robot of the
このような構成であれば、第１回転手段により、第１走査軸の回りに測距センサが、第２回転手段により、第２走査軸の回りに測距センサがそれぞれ回転可能となる。したがって、障害物の立体的な形状を把握することができる。そして、測定結果取得手段により、第２走査が行われることにより第１回転手段の所定単位角度ごとおよび第２回転手段の所定単位角度ごとの測定結果が取得される。第２走査では、第２回転手段により測距センサを回転させながら第２回転手段の所定単位角度ごとに第１走査が行われる。第１走査では、第１回転手段により測距センサを回転させながら第１回転手段の所定単位角度ごとに測定結果が取得される。 With such a configuration, the distance measuring sensor can be rotated around the first scanning axis by the first rotating means, and the distance measuring sensor can be rotated around the second scanning axis by the second rotating means. Therefore, the three-dimensional shape of the obstacle can be grasped. Then, the second scanning is performed by the measurement result acquisition unit, whereby measurement results for each predetermined unit angle of the first rotation unit and for each predetermined unit angle of the second rotation unit are acquired. In the second scanning, the first scanning is performed for each predetermined unit angle of the second rotating means while the distance measuring sensor is rotated by the second rotating means. In the first scan, a measurement result is acquired for each predetermined unit angle of the first rotating means while rotating the distance measuring sensor by the first rotating means.
以上説明したように、発明１の干渉判定装置によれば、所定の座標系における平面等と特徴点の位置関係に基づいて干渉判定を行うので、実際の干渉を伴うことなく干渉判定を行うことができるという効果が得られる。また、干渉判定専用のカメラのような専用のセンサを設けなくても、他の用途に用いられるセンサ等を利用することができるので、部品点数および重量の増加を抑制することができるという効果も得られる。さらに、所定の座標系において対象部位に対応する平面等は姿勢や形状により特定されるため、障害物の特徴点さえ検出できればよく、障害物の監視範囲が対象部位の可動範囲に制約されず、干渉判定の制約を低減することができるという効果も得られる。 As described above, according to the interference determination device of the first aspect of the present invention, the interference determination is performed based on the positional relationship between the plane and the like in the predetermined coordinate system and the feature points, so that the interference determination can be performed without actual interference. The effect of being able to be obtained. In addition, even if a dedicated sensor such as a camera dedicated to interference determination is not provided, a sensor used for other purposes can be used, so that an increase in the number of parts and weight can be suppressed. can get. Furthermore, since the plane corresponding to the target part in the predetermined coordinate system is specified by the posture and shape, it is only necessary to detect the feature point of the obstacle, and the monitoring range of the obstacle is not limited to the movable range of the target part, The effect that the restriction | limiting of interference determination can be reduced is also acquired.
一方、発明２の脚車輪型ロボットによれば、所定の座標系における平面等と特徴点の位置関係に基づいて干渉判定を行うので、実際の干渉を伴うことなく干渉判定を行うことができるという効果が得られる。また、干渉判定専用のカメラのような専用のセンサを設けなくても、他の用途に用いられる物体センサを利用することができるので、部品点数および重量の増加を抑制することができるという効果も得られる。さらに、所定の座標系において対象部位に対応する平面等は姿勢や形状により特定されるため、障害物の特徴点さえ検出できればよく、障害物の監視範囲が対象部位の可動範囲に制約されず、干渉判定の制約を低減することができるという効果も得られる。さらに、対象部位との位置関係によらず物体センサを設置することができるので、干渉判定の制約をさらに低減することができるという効果も得られる。 On the other hand, according to the leg-wheel type robot of the second aspect, since the interference determination is performed based on the positional relationship between the plane and the like in the predetermined coordinate system and the feature point, the interference determination can be performed without actual interference. An effect is obtained. In addition, since an object sensor used for other purposes can be used without providing a dedicated sensor such as a dedicated camera for interference determination, there is also an effect that an increase in the number of parts and weight can be suppressed. can get. Furthermore, since the plane corresponding to the target part in the predetermined coordinate system is specified by the posture and shape, it is only necessary to detect the feature point of the obstacle, and the monitoring range of the obstacle is not limited to the movable range of the target part, The effect that the restriction | limiting of interference determination can be reduced is also acquired. Furthermore, since the object sensor can be installed regardless of the positional relationship with the target part, an effect that the restriction of interference determination can be further reduced can be obtained.
さらに、発明３の脚車輪型ロボットによれば、対象部位に対して障害物が干渉するか否かを比較的正確に判定することができるという効果が得られる。
さらに、発明５の脚車輪型ロボットによれば、複数の平面で囲まれる空間に特徴点が属するか否かを比較的容易に判定することができるという効果が得られる。
さらに、発明６の脚車輪型ロボットによれば、障害物の立体的な形状を把握することができ、これに基づいて検出された特徴点により干渉判定を行うことができるので、干渉判定の精度を向上することができるという効果が得られる。
Furthermore, according to the leg-wheel type robot of the invention 3, it is possible to determine relatively accurately whether or not an obstacle interferes with the target part.
Furthermore, according to the leg-wheel type robot of the fifth aspect, it is possible to relatively easily determine whether or not a feature point belongs to a space surrounded by a plurality of planes.
Furthermore, according to the leg-wheel type robot of the
さらに、発明７の脚車輪型ロボットによれば、測距センサを回転させる回転機構を採用したので、移動機構に比して、走査に必要なスペースが小さくてすみ、走査のための機構が簡素となり、しかも高速な走査を実現することができるという効果が得られる。
Furthermore, according to the leg-wheel type robot of the
以下、本発明の第１の実施の形態を図面を参照しながら説明する。図１ないし図１４は、本発明に係る干渉判定装置および脚車輪型ロボットの第１の実施の形態を示す図である。 Hereinafter, a first embodiment of the present invention will be described with reference to the drawings. 1 to 14 are diagrams showing a first embodiment of an interference determination device and a leg-wheel type robot according to the present invention.
まず、本発明を適用する脚車輪型ロボット１００の構成を説明する。
図１は、脚車輪型ロボット１００の正面図である。
図２は、脚車輪型ロボット１００の側面図である。
脚車輪型ロボット１００は、図１および図２に示すように、基体１０と、基体１０に連結された４つの脚部１２とを有して構成されている。
基体１０の前部には、２本の脚部１２が回転関節１４を介して左右対称の位置に連結されている。また、基体１０の後部には、２本の脚部１２が回転関節１４を介して左右対称の位置に連結されている。回転関節１４は、脚車輪型ロボット１００の底面と直交する方向を軸方向として回転する。すなわち、ヨー軸回りに回転する。
First, the configuration of a leg
FIG. 1 is a front view of a leg
FIG. 2 is a side view of the leg
As shown in FIGS. 1 and 2, the leg-
Two
各脚部１２には、２つの回転関節１６、１８が設けられている。回転関節１４は、下方を軸方向として回転し、回転関節１６、１８は、回転関節１４が図１の状態であるときは、脚車輪型ロボット１００の側面と直交する方向を軸方向として回転する。すなわち、回転関節１４が図１の状態であるときは、ピッチ軸回りに回転し、回転関節１４が図１の状態から９０度回転した状態であるときは、ロール軸回りに回転する。したがって、脚部１２は、それぞれ３自由度を有する。
Each
各脚部１２の先端には、回転関節１６、１８と軸方向を同一にして駆動輪２０が回転可能に設けられている。
各脚部１２の先端には、脚車輪型ロボット１００の移動経路上に存在する物体までの距離を測定する前方脚先センサ２２と、接地面までの距離を測定する下方脚先センサ２４とが設けられている。
A
At the tip of each
一方、基体１０の正面の上部中央には、水平面レーザ光を照射する水平レーザ２６が設けられている。また、基体１０の正面の中央左右には、垂直面レーザ光を照射する垂直レーザ２８、３０がそれぞれ設けられている。
基体１０の正面の下部中央には、水平面レーザ光および垂直面レーザ光の反射光を含む画像を撮影するカメラ３２が設けられている。
On the other hand, a
A
水平レーザ２６は、カメラ３２で水平面レーザ光の反射光を含む画像が撮影できるように下方に所定角度傾けて設けられている。同様に、垂直レーザ２８は、カメラ３２で垂直面レーザ光の反射光を含む画像が撮影できるように右方に所定角度傾けて設けられ、垂直レーザ３０は、左方に所定角度傾けて設けられている。
カメラ３２の左右には、障害物を検出する障害物センサ３４、３６がそれぞれ設けられている。
The
図３は、障害物センサ３４、３６の構成を示す図である。
障害物センサ３４、３６は、図３（ａ）に示すように、指向性の低い超音波測距センサを複数アレイ状に配列して構成することができる。また、図３（ｂ）に示すように、指向性の高い赤外線測距センサを複数アレイ状に配列して構成することもできる。アレイ状に配列する構成に限らず、単体で構成してもよい。また、超音波測距センサまたは赤外線測距センサを複数平面上に配列したエリアセンサで構成してもよい。これにより、脚車輪型ロボット１００の移動経路上に存在する物体を大まかに検出することができる。
FIG. 3 is a diagram illustrating the configuration of the
As shown in FIG. 3A, the
次に、脚車輪型ロボット１００の移動制御システムを説明する。
図４は、脚車輪型ロボット１００の移動制御システムを示すブロック図である。
各脚部１２の回転関節１４〜１８には、図４に示すように、回転関節１４〜１８を回転駆動する関節モータ４０がそれぞれ設けられている。各関節モータ４０には、関節モータ４０の回転角度位置を検出するエンコーダ４２と、モータ指令信号およびエンコーダ４２の出力信号に基づいて関節モータ４０の駆動を制御するドライバ４４が設けられている。
Next, the movement control system of the leg
FIG. 4 is a block diagram showing a movement control system of the leg
As shown in FIG. 4,
各脚部１２の駆動輪２０には、駆動輪２０を回転駆動する車輪モータ５０がそれぞれ設けられている。各車輪モータ５０には、車輪モータ５０の回転角度位置を検出するエンコーダ５２と、モータ指令信号およびエンコーダ５２の出力信号に基づいて車輪モータ５０の駆動を制御するドライバ５４が設けられている。
脚車輪型ロボット１００は、さらに、ＣＰＵ６０と、脚車輪型ロボット１００の姿勢を検出する３軸姿勢センサ７０と、カメラ３２の画像信号を処理するビジョンプロセッサ７２と、外部のＰＣ等と無線通信を行う無線通信部７４と、ビジョンプロセッサ７２および無線通信部７４とＣＰＵ６０の入出力を中継するハブ７６と、警告音等を出力するスピーカ７８とを有して構成されている。
A
The leg
３軸姿勢センサ７０は、ジャイロ若しくは加速度センサ、またはその両方を有し、地軸に対して脚車輪型ロボット１００の姿勢の傾きを検出する。
ＣＰＵ６０は、モータ指令出力Ｉ／Ｆ６１を介してドライバ４４、５４にモータ指令信号を出力し、角度取込Ｉ／Ｆ６２を介してエンコーダ４２、５２の出力信号を入力する。また、センサ入力Ｉ／Ｆ６３を介して、前方脚先センサ２２、下方脚先センサ２４、障害物センサ３４、３６および３軸姿勢センサ７０からそれぞれセンサ信号を入力する。また、通信Ｉ／Ｆ６４を介してハブ７６と信号の入出力を行い、サウンド出力Ｉ／Ｆ６５を介してスピーカ７８に音声信号を出力する。
The
The
次に、ＣＰＵ６０で実行される処理を説明する。
ＣＰＵ６０は、ＲＯＭ等の所定領域に格納されている制御プログラムを起動させ、その制御プログラムに従って、図５のフローチャートに示す昇降制御処理を実行する。
次に、昇降制御処理を説明する。
図５は、昇降制御処理を示すフローチャートである。
Next, processing executed by the
The
Next, the elevation control process will be described.
FIG. 5 is a flowchart showing the elevation control process.
昇降制御処理は、脚部１２の昇降制御を行う処理であって、ＣＰＵ６０において実行されると、まず、図５に示すように、ステップＳ１００に移行する。
ステップＳ１００では、ビジョンプロセッサ７２から画像を取り込み、ステップＳ１０２に移行する。
ステップＳ１０２では、取り込んだ画像に基づいて光切断法により階段の特徴点を検出する。
The elevation control process is a process for performing the elevation control of the
In step S100, an image is captured from the
In step S102, feature points of the staircase are detected by a light cutting method based on the captured image.
図６は、光切断法の原理を説明するための図である。
光切断法は、三角測量の原理により計測対象上の座標を求める計測法である。図６に計測座標系を示す。
計測対象上の座標Ｐ（x0、y0、z0）は、カメラ３２の撮像素子上の任意の座標をＰs（xi、yi、zi）とすると、下式（１）により求めることができる。
FIG. 6 is a diagram for explaining the principle of the light cutting method.
The light section method is a measurement method for obtaining coordinates on a measurement object based on the principle of triangulation. FIG. 6 shows the measurement coordinate system.
The coordinates P (x0, y0, z0) on the measurement target can be obtained by the following equation (1), where Ps (xi, yi, zi) is an arbitrary coordinate on the image sensor of the
次に、得られた３次元座標から、レーザ光の反射光の不連続点または屈曲点を階段の特徴点として検出する。
図７は、階段にレーザ光を照射した状態およびカメラ３２の撮像素子の画像を示す図である。
Next, from the obtained three-dimensional coordinates, a discontinuous point or a bending point of the reflected laser beam is detected as a feature point of the staircase.
FIG. 7 is a diagram illustrating a state in which the staircase is irradiated with laser light and an image of the imaging element of the
脚車輪型ロボット１００の移動経路上に階段が存在すると、図７（ａ）左側に示すように、水平レーザ２６から照射された水平面レーザ光が階段の蹴込板および床面で反射し、カメラ３２により、その反射光を含む階段の画像が撮影される。その画像に対して画像処理を行うと、図７（ａ）右側に示すように、蹴込板での反射光エッジおよび床面での反射光エッジを検出することができる。そして、そのエッジ画像および上式（１）により得られた３次元座標に基づいて、反射光エッジの不連続点に対応する実座標を算出することができる。
When a stairway is present on the movement path of the leg-
また、図７（ｂ）左側に示すように、垂直レーザ２８から照射された垂直面レーザ光が階段の蹴込板および踏板で反射し、カメラ３２により、その反射光を含む階段の画像が撮影される。その画像に対して画像処理を行うと、図７（ｂ）右側に示すように、蹴込板での反射光エッジおよび踏板での反射光エッジを検出することができる。また、垂直レーザ３０についても同様であり、図７（ｃ）右側に示すように、蹴込板での反射光エッジおよび踏板での反射光エッジを検出することができる。そして、それらエッジ画像および上式（１）により得られた３次元座標に基づいて、反射光エッジの屈曲点に対する実座標を算出することができる。
Further, as shown on the left side of FIG. 7B, the vertical plane laser light emitted from the
図８は、階段の昇降時に段鼻部の特徴点を検出する場合を示す図である。
脚車輪型ロボット１００が階段を上る場合は、図８（ａ）に示すように、階段を上る前に、反射光エッジの屈曲点として段鼻部の特徴点を検出することができる。また、階段を下る場合は、図８（ｂ）に示すように、階段を下る前に、反射光エッジの屈曲点として段鼻部の特徴点を検出することができる。
FIG. 8 is a diagram illustrating a case where feature points of the stair nose are detected when the stairs are raised and lowered.
When the leg-
図５に戻り、次いで、ステップＳ１０３に移行して、障害物との干渉の有無を判定する干渉判定処理を実行し、ステップＳ１０４に移行して、干渉判定処理の判定結果に基づいて障害物と干渉するか否かを判定し、干渉しないと判定したとき(No)は、ステップＳ１０５に移行する。
ステップＳ１０５では、検出した特徴点に基づいて階段の幅を算出し、ステップＳ１０６に移行して、検出した特徴点に基づいて階段の段鼻部の実座標を算出し、ステップＳ１０８に移行する。
Returning to FIG. 5, next, the process proceeds to step S <b> 103, where an interference determination process for determining the presence or absence of interference with the obstacle is executed. The process proceeds to step S <b> 104, and the obstacle determination It is determined whether or not interference occurs, and when it is determined that interference does not occur (No), the process proceeds to step S105.
In step S105, the width of the staircase is calculated based on the detected feature point, the process proceeds to step S106, the actual coordinates of the stair nosing part of the staircase are calculated based on the detected feature point, and the process proceeds to step S108.
ステップＳ１０８では、算出した階段の幅および段鼻部の実座標、並びに３軸姿勢センサ７０のセンサ信号に基づいて逆運動学計算および重心計算を行い、ステップＳ１１０に移行して、ステップＳ１０８の計算結果に基づいて脚先（駆動輪２０）の着地位置を決定し、ステップＳ１１２に移行する。
ステップＳ１１２では、前方脚先センサ２２および下方脚先センサ２４からそれぞれセンサ信号を入力し、ステップＳ１１４に移行して、入力した前方脚先センサ２２のセンサ信号に基づいて蹴込板までの距離を算出し、ステップＳ１１６に移行して、入力した下方脚先センサ２４のセンサ信号に基づいて脚先と踏板の位置関係を算出し、ステップＳ１１８に移行する。
In step S108, inverse kinematics calculation and centroid calculation are performed based on the calculated stair width and actual coordinates of the nose and the sensor signal of the three-
In step S112, sensor signals are input from the front
ステップＳ１１８では、決定した着地位置および算出した両距離に基づいてドライバ４４、５４へのモータ指令信号を生成し、ステップＳ１２０に移行して、生成したモータ指令信号をドライバ４４、５４に出力し、ステップＳ１２２に移行する。
ステップＳ１２２では、脚先が踏板に着地したか否かを判定し、脚先が着地したと判定したとき(Yes)は、一連の処理を終了して元の処理に復帰させる。
In step S118, a motor command signal to the
In step S122, it is determined whether or not the leg tip has landed on the tread. If it is determined that the leg tip has landed (Yes), the series of processes is terminated and the process returns to the original process.
一方、ステップＳ１２２で、脚先が着地しないと判定したとき(No)は、ステップＳ１１２に移行する。
一方、ステップＳ１０４で、障害物と干渉すると判定したとき(Yes)は、ステップＳ１２４に移行して、干渉を回避する干渉回避処理を実行し、一連の処理を終了して元の処理に復帰させる。
On the other hand, when it is determined in step S122 that the leg tip does not land (No), the process proceeds to step S112.
On the other hand, if it is determined in step S104 that the object interferes with the obstacle (Yes), the process proceeds to step S124, the interference avoidance process for avoiding the interference is executed, the series of processes are terminated, and the original process is restored. .
次に、ステップＳ１０３の干渉判定処理を説明する。
脚車輪型ロボット１００が階段を昇降する場合は、脚部１２のうち回転関節１８および駆動輪２０が連結するリンク（以下、脚すね部という。）と、階段の段鼻部とが干渉し得る。したがって、干渉判定処理では、脚すね部に対して段鼻部が干渉するか否かを判定する。この判定は、特定の脚部１２（例えば、前脚）について行ってもよいし、すべての脚部１２について行ってもよい。
Next, the interference determination process in step S103 will be described.
When the leg-
図９は、グローバル座標系において脚すね部の表面に対応する４つの平面を示す図である。同図（ａ）は、脚すね部の斜視図であり、同図（ｂ）は、脚すね部の厚さ方向の断面図であり、同図（ｃ）は、脚すね部の伸長方向の断面図である。
段鼻部の特徴点の座標は、特徴点を検出するための物体センサ（本実施の形態では、レーザ２６〜３０およびカメラ３２）からみた脚車輪型ロボット１００の外部を基準とするグローバル座標系で定められる。そこで、干渉の判定は、図９（ａ）に示すように、グローバル座標系において、矩形からなる脚すね部の正面、背面、右側面および左側面の各表面ごとにその表面に対応する平面（以下、脚すね部平面という。）を特定し、図９（ｂ）に示すように、特定した４つの脚すね部平面で囲まれる空間（すなわち、脚すね部の内部領域に対応する空間）に特徴点が属するか否かを判定することにより行う。同空間に特徴点が属すると判定したときは、脚すね部に対して段鼻部が干渉すると判定し、同空間に特徴点が属さないと判定したときは、脚すね部に対して段鼻部が干渉しないと判定する。
FIG. 9 is a diagram showing four planes corresponding to the surface of the leg shin portion in the global coordinate system. The figure (a) is a perspective view of a leg shin part, The figure (b) is sectional drawing of the thickness direction of a leg shin part, The figure (c) is the extension direction of a leg shin part. It is sectional drawing.
The coordinates of the feature points of the nose portion are a global coordinate system based on the outside of the leg-
図１０は、段鼻部の特徴点Ａ、脚すね部平面、脚すね部平面上の任意の点Ｐおよび脚すね部平面の法線ベクトルＮを示す図である。
段鼻部の特徴点をＡ、脚すね平面上の任意の点をＰ、脚すね部平面の法線ベクトルをＮとすると、図１０に示すように、特徴点Ａと点Ｐを結ぶ直線ＰＡと、法線ベクトルＮとの内積Ｄは、下式（２）により求めることができる。
FIG. 10 is a diagram showing the characteristic point A of the nose, the leg shin plane, the arbitrary point P on the leg shin plane, and the normal vector N of the leg shin plane.
Assuming that the feature point of the nose is A, an arbitrary point on the leg shin plane is P, and the normal vector of the leg shin plane is N, as shown in FIG. 10, a straight line PA connecting the feature point A and the point P The inner product D with the normal vector N can be obtained by the following equation (2).
Ｄ＞０のときは、脚すね平面の表側（脚すね部の外側）に特徴点が位置し、Ｄ＝０のときは、脚すね平面上に特徴点が位置し、Ｄ＜０のときは、脚すね平面の裏側（脚すね部の内側）に特徴点が位置する。 When D> 0, the feature point is located on the front side of the leg shin plane (outside the leg shin part), when D = 0, the feature point is located on the leg shin plane, and when D <0 The feature point is located on the back side of the leg shin plane (inside the leg shin part).
脚すね部の正面、背面、右側面および左側面に対応する脚すね平面についての内積ＤをそれぞれＤ1、Ｄ2、Ｄ3、Ｄ4とすると、Ｄ1≧０またはＤ2≧０またはＤ3≧０またはＤ4≧０が成立する場合は、４つの脚すね部平面で囲まれる空間に特徴点Ａが属さないので、脚すね部に対して段鼻部が干渉しないと判定する。
なお、４つの脚すね部平面で判定する場合は、脚すね部の底面よりも下方の領域も干渉判定の対象領域としてしまうため、脚すね部が特徴点Ａを跨ぐような場合も干渉すると判定してしまう。そこで、より好ましくは、脚すね部の底面に対応する脚すね平面も考慮することである。脚すね部の底面に対応する脚すね平面についての内積ＤをＤ5とすると、Ｄ1≧０またはＤ2≧０またはＤ3≧０またはＤ4≧０またはＤ5≧０が成立する場合は、５つの脚すね部平面で囲まれる空間に特徴点Ａが属さないので、脚すね部に対して段鼻部が干渉しないと判定することができる。
If the inner products D of the leg shin planes corresponding to the front, back, right side, and left side of the leg shin part are D1, D2, D3, and D4, respectively, D1 ≧ 0 or D2 ≧ 0 or D3 ≧ 0 or D4 ≧ 0. When is established, since the feature point A does not belong to the space surrounded by the four leg shin planes, it is determined that the nosing part does not interfere with the leg shin part.
In addition, when determining with the four leg shin part planes, since the area below the bottom surface of the leg shin part is also the target area for the interference determination, it is determined that interference occurs even when the leg shin part straddles the feature point A. Resulting in. Therefore, more preferably, a leg shin plane corresponding to the bottom surface of the leg shin part is also taken into consideration. If the inner product D for the leg shin plane corresponding to the bottom surface of the leg shin part is D5, if D1≥0 or D2≥0 or D3≥0 or D4≥0 or D5≥0, then five leg shin parts Since the feature point A does not belong to the space surrounded by the plane, it can be determined that the nose part does not interfere with the leg shin part.
同様に、４つの脚すね部平面で判定する場合は、脚すね部の上面よりも上方の領域も干渉判定の対象領域としてしまうため、脚すね部が特徴点Ａを潜るような場合も干渉すると判定してしまう。そこで、より好ましくは、脚すね部の上面に対応する脚すね平面も考慮することである。脚すね部の上面に対応する脚すね平面についての内積ＤをＤ6とすると、Ｄ1≧０またはＤ2≧０またはＤ3≧０またはＤ4≧０またはＤ6≧０が成立する場合は、５つの脚すね部平面で囲まれる空間に特徴点Ａが属さないので、脚すね部に対して段鼻部が干渉しないと判定することができる。 Similarly, when the determination is made using the four leg shin planes, the area above the upper surface of the leg shin is also the target area for the interference determination. I will judge. Therefore, more preferably, a leg shin plane corresponding to the upper surface of the leg shin part is also taken into consideration. When the inner product D for the leg shin plane corresponding to the upper surface of the leg shin part is D6, if D1≥0 or D2≥0 or D3≥0 or D4≥0 or D6≥0, five leg shin parts Since the feature point A does not belong to the space surrounded by the plane, it can be determined that the nose part does not interfere with the leg shin part.
さらに好ましくは、脚すね部の底面および上面に対応する脚すね平面も考慮することである。Ｄ1≧０またはＤ2≧０またはＤ3≧０またはＤ4≧０またはＤ5≧０またはＤ6≧０が成立する場合は、６つの脚すね部平面で囲まれる空間に特徴点Ａが属さないので、脚すね部に対して段鼻部が干渉しないと判定することができる。
次に、グローバル座標系において点Ｐの座標を求める場合を説明する。
More preferably, the leg shin plane corresponding to the bottom surface and the top surface of the leg shin portion is also taken into consideration. If D1 ≧ 0 or D2 ≧ 0 or D3 ≧ 0 or D4 ≧ 0 or D5 ≧ 0 or D6 ≧ 0, the feature point A does not belong to the space surrounded by the six leg shin planes. It can be determined that the nose part does not interfere with the part.
Next, a case where the coordinates of the point P are obtained in the global coordinate system will be described.
特徴点Ａの座標は、グローバル座標系で定められるのに対し、脚すね部の座標は、脚すね部を基準とする相対座標系で定められる。したがって、グローバル座標系における点Ｐの座標は、脚すね部の座標を定める座標系の座標をグローバル座標系の座標に変換して求める必要がある。
図１１は、グローバル座標系および脚車輪型ロボット１００の各座標系を示す図である。
The coordinates of the feature point A are determined in the global coordinate system, while the coordinates of the leg shin part are determined in a relative coordinate system based on the leg shin part. Therefore, the coordinates of the point P in the global coordinate system must be obtained by converting the coordinates of the coordinate system that determines the coordinates of the leg shin portion into the coordinates of the global coordinate system.
FIG. 11 is a diagram illustrating a global coordinate system and each coordinate system of the leg-
図１１において、Ｐgは、グローバル座標系、Ｐ0は、基体１０の底面中心を原点とする座標系、Ｐ1は、基体１０と右後ろの脚部１２との付け根を原点とする座標系、Ｐ2は、回転関節１４を原点とする座標系、Ｐ3は、回転関節１６を原点とする座標系、Ｐ4は、回転関節１８を原点とする座標系である。ここで、座標系Ｐ4が脚すね部の座標を定める座標系となる。
In FIG. 11, Pg is a global coordinate system, P0 is a coordinate system with the center of the bottom surface of the base 10 as an origin, P1 is a coordinate system with the base of the
グローバル座標系Ｐgおよび座標系Ｐ0〜Ｐ4は、３軸が互いに直交する直交座標系として定義される。
グローバル座標系Ｐgは、特徴点Ａを検出するための物体センサからみた座標系であって、その原点は、座標系Ｐ0の原点から所定距離（Ａ、Ｂ、Ｃ）離れたところに設定されている。
The global coordinate system Pg and the coordinate systems P0 to P4 are defined as orthogonal coordinate systems in which three axes are orthogonal to each other.
The global coordinate system Pg is a coordinate system viewed from the object sensor for detecting the feature point A, and its origin is set at a predetermined distance (A, B, C) from the origin of the coordinate system P0. Yes.
座標系Ｐ0は、基体１０の奥行き（前後の長さ）方向をｘ0軸、基体１０の幅（左右の長さ）方向をｙ0軸、基体１０の高さ方向をｚ0軸とし、ｘ0軸は基体１０の後方を、ｙ0軸は基体１０の右方を、ｚ0軸は基体１０の上方をそれぞれ正の方向とする。原点は、基体１０の底面中心となるので、基体１０の奥行きをＬ、基体１０の幅をＷとすると、（Ｌ／２、Ｗ／２、０）となる。
In the coordinate system P0, the depth (front / rear length) direction of the
グローバル座標系Ｐgと座標系Ｐ0の関係は、下式（３）の行列で表すことができる。したがって、下式（３）により、グローバル座標系Ｐgから座標系Ｐ0への座標変換を行うことができる。 The relationship between the global coordinate system Pg and the coordinate system P0 can be expressed by a matrix of the following equation (3). Therefore, the coordinate transformation from the global coordinate system Pg to the coordinate system P0 can be performed by the following equation (3).
座標系Ｐ1は、ｘ0軸、ｙ0軸、ｚ0軸と同一の方向をｘ1軸、ｙ2軸、ｚ3軸とし、ｘ1軸は基体１０の後方を、ｙ1軸は基体１０の右方を、ｚ1軸は基体１０の下方をそれぞれ正の方向とする。原点は、基体１０と右後ろの脚部１２との付け根となるので、（０、０、０）となる。
The coordinate system P1 has the same direction as the x0, y0, and z0 axes as the x1, y2, and z3 axes, where the x1 axis is the rear of the
座標系Ｐ0、Ｐ1の関係は、下式（４）の行列で表すことができる。したがって、下式（４）により、座標系Ｐ0から座標系Ｐ1への座標変換を行うことができる。 The relationship between the coordinate systems P0 and P1 can be expressed by a matrix of the following equation (4). Therefore, the coordinate transformation from the coordinate system P0 to the coordinate system P1 can be performed by the following equation (4).
座標系Ｐ2は、座標系Ｐ1に対して回転関節１４がθ1回転した座標系であって、ｚ1軸と同一の方向をｚ2軸、ｘ1軸に対してｚ2軸回りにθ1回転した方向をｘ2軸、ｙ1軸に対してｚ2軸回りにθ1回転した方向をｙ2軸とし、ｚ2軸は基体１０の下方を正の方向とする。
The coordinate system P2 is a coordinate system in which the rotary joint 14 rotates θ1 with respect to the coordinate system P1, and the same direction as the z1 axis is the z2 axis, and the direction rotated θ1 around the z2 axis with respect to the x1 axis is the x2 axis. The direction rotated by θ1 around the z2 axis with respect to the y1 axis is defined as the y2 axis, and the z2 axis is defined as the positive direction below the
座標系Ｐ1、Ｐ2の関係は、下式（５）の行列で表すことができる。したがって、下式（５）により、座標系Ｐ1から座標系Ｐ2への座標変換を行うことができる。 The relationship between the coordinate systems P1 and P2 can be expressed by a matrix of the following equation (5). Therefore, the coordinate transformation from the coordinate system P1 to the coordinate system P2 can be performed by the following equation (5).
座標系Ｐ3は、座標系Ｐ2に対して回転関節１６がθ2回転した座標系であって、ｙ2軸と同一の方向をｙ3軸、ｘ2軸に対してｙ3軸回りにθ2回転した方向をｘ3軸、ｚ2軸に対してｙ3軸回りにθ2回転した方向をｚ3軸とし、ｚ3軸は、脚部１２のうち回転関節１４、１６が連結するリンクの下方を正の方向とする。
The coordinate system P3 is a coordinate system in which the rotary joint 16 rotates θ2 with respect to the coordinate system P2, and the same direction as the y2 axis is the y3 axis, and the direction rotated θ2 around the y3 axis with respect to the x2 axis is the x3 axis. , The direction rotated θ2 around the y3 axis with respect to the z2 axis is defined as the z3 axis, and the z3 axis is defined as the positive direction below the link of the
座標系Ｐ2、Ｐ3の関係は、下式（６）の行列で表すことができる。したがって、下式（６）により、座標系Ｐ2から座標系Ｐ3への座標変換を行うことができる。 The relationship between the coordinate systems P2 and P3 can be expressed by a matrix of the following equation (6). Therefore, the coordinate transformation from the coordinate system P2 to the coordinate system P3 can be performed by the following equation (6).
座標系Ｐ4は、座標系Ｐ3に対して回転関節１８がθ3回転した座標系であって、ｙ3軸と同一の方向をｙ4軸、ｘ3軸に対してｙ4軸回りにθ3回転した方向をｘ4軸、ｚ3軸に対してｙ4軸回りにθ3回転した方向をｚ4軸とし、ｚ4軸は、脚すね部の下方を正の方向とする。 The coordinate system P4 is a coordinate system in which the rotary joint 18 is rotated by θ3 with respect to the coordinate system P3, and the same direction as the y3 axis is the y4 axis, and the direction rotated θ3 around the y4 axis with respect to the x3 axis is the x4 axis. , The direction rotated θ3 around the y4 axis with respect to the z3 axis is the z4 axis, and the z4 axis is the positive direction below the leg ridge.
座標系Ｐ3、Ｐ4の関係は、下式（７）の行列で表すことができる。したがって、下式（７）により、座標系Ｐ3から座標系Ｐ4への座標変換を行うことができる。 The relationship between the coordinate systems P3 and P4 can be expressed by a matrix of the following equation (7). Therefore, the coordinate transformation from the coordinate system P3 to the coordinate system P4 can be performed by the following equation (7).
図１２は、座標系Ｐ4を示す図である。
座標系Ｐ4における点Ｐの座標は、図１２に示すように、脚すね部の奥行きをｌ、幅をｍ、伸長方向の長さをｎとすると、（ｌ／２、０、０）となる。
FIG. 12 is a diagram showing the coordinate system P4.
As shown in FIG. 12, the coordinates of the point P in the coordinate system P4 is (l / 2, 0, 0), where l is the depth of the leg shin, m is the width, and n is the length in the extension direction. .
したがって、グローバル座標系Ｐgにおける点Ｐの座標は、下式（８）により求めることができる。下式（８）のなかで、θ1〜θ3は、脚車輪型ロボット１００の姿勢により、ｌは、脚すね部の形状により、Ｌ、Ｗは、基体１０の形状によりそれぞれ特定されるので、グローバル座標系Ｐgにおける点Ｐの座標は、脚車輪型ロボット１００の姿勢並びに基体１０および脚すね部の形状により特定することができる。
Therefore, the coordinates of the point P in the global coordinate system Pg can be obtained by the following equation (8). In the following formula (8), θ1 to θ3 are specified by the posture of the leg
上式（８）において、4Ｘpは、下式（９）により表すことができ、4Ｘpのうち（ｌ／２、０、０）は、座標系Ｐ4における点Ｐの座標である。また、Ｘpのうち（ｘ、ｙ、ｚ）は、グローバル座標系Ｐgにおける点Ｐの座標である。 In the above equation (8), 4 X p can be expressed by the following equation (9), and (l / 2, 0, 0) of 4 X p is the coordinates of the point P in the coordinate system P4. In addition, (x, y, z) in X p is the coordinates of the point P in the global coordinate system Pg.
次に、脚すね部平面の法線ベクトルＮを求める場合を説明する。
図１３は、脚すね部平面および法線ベクトルＮを示す図である。
法線ベクトルＮは、図１３に示すように、グローバル座標系Ｐgにおいて、脚すね部平面の４つの頂点（脚すね部の表面のうち右上、右下、左上および左下の各角部に対応する点）のうち３つをそれぞれＶ1、Ｖ2、Ｖ3とすると、点Ｖ3から点Ｖ1へのベクトルと、点Ｖ2から点Ｖ3のベクトルの外積で表すことができるので、下式（10）により求めることができる。
Next, the case of obtaining the normal vector N of the leg shin plane will be described.
FIG. 13 is a diagram showing the leg shin plane and the normal vector N. As shown in FIG.
As shown in FIG. 13, the normal vector N corresponds to the four vertices of the leg shin plane in the global coordinate system Pg (upper right, lower right, upper left and lower left corners of the surface of the leg shin part). If three of the points are V1, V2 and V3, respectively, they can be expressed by the outer product of the vector from the point V3 to the point V1 and the vector from the point V2 to the point V3. Can do.
なお、グローバル座標系Ｐgにおける点Ｖ1の座標は、点Ｐの場合と同様に、下式（11）により求めることができる。グローバル座標系Ｐgにおける点Ｖ2、Ｖ3の座標も同様に求めることができる。 Note that the coordinates of the point V1 in the global coordinate system Pg can be obtained by the following equation (11) as in the case of the point P. The coordinates of the points V2 and V3 in the global coordinate system Pg can be obtained similarly.
図１４は、干渉判定処理を示すフローチャートである。
干渉判定処理は、ステップＳ１０３において実行されると、まず、図１４に示すように、ステップＳ２００に移行する。
FIG. 14 is a flowchart showing the interference determination process.
When the interference determination process is executed in step S103, first, the process proceeds to step S200 as shown in FIG.
ステップＳ２００では、回転関節１４〜１８の角度θ1〜θ3、基体１０の寸法Ｌ、Ｗおよび脚すね部の寸法ｌに基づいて、上式（８）により、グローバル座標系Ｐgにおける点Ｐの座標を算出し、ステップＳ２０２に移行する。
ステップＳ２０２では、回転関節１４〜１８の角度θ1〜θ3、基体１０の寸法Ｌ、Ｗおよび脚すね部の寸法ｌに基づいて、上式（11）により、各脚すね部平面ごとにグローバル座標系Ｐgにおける点Ｖ1〜Ｖ3の座標を算出し、ステップＳ２０４に移行する。
In step S200, the coordinates of the point P in the global coordinate system Pg are calculated by the above equation (8) based on the angles θ1 to θ3 of the
In step S202, based on the angles θ1 to θ3 of the
ステップＳ２０４では、上式（10）により、各脚すね部平面ごとに法線ベクトルＮを算出し、ステップＳ２０６に移行して、特徴点Ａの座標を取得し、ステップＳ２０８に移行して、上式（２）により、特徴点Ａと点Ｐを結ぶ直線ＰＡと、法線ベクトルＮとの内積Ｄ1〜Ｄ4（またはＤ5、Ｄ6）を算出し、ステップＳ２１０に移行する。
ステップＳ２１０では、Ｄ1≧０またはＤ2≧０またはＤ3≧０またはＤ4≧０（またはＤ5≧０、Ｄ6≧０）が成立するか否かを判定することにより４つの脚すね部平面で囲まれる空間に特徴点Ａが属するか否かを判定し、同空間に特徴点Ａが属さないと判定したとき(No)は、ステップＳ２１２に移行する。
In step S204, the normal vector N is calculated for each leg shin plane by the above equation (10), the process proceeds to step S206, the coordinates of the feature point A are acquired, the process proceeds to step S208, and the upper The inner product D1 to D4 (or D5, D6) of the straight line PA connecting the feature point A and the point P and the normal vector N is calculated by the equation (2), and the process proceeds to step S210.
In step S210, it is determined whether D1 ≧ 0 or D2 ≧ 0 or D3 ≧ 0 or D4 ≧ 0 (or D5 ≧ 0, D6 ≧ 0) is satisfied. It is determined whether or not the feature point A belongs, and when it is determined that the feature point A does not belong to the same space (No), the process proceeds to step S212.
ステップＳ２１２では、未判定の特徴点Ａが存在するか否かを判定し、未判定の特徴点Ａが存在しないと判定したとき(No)は、ステップＳ２１４に移行して、障害物と干渉しないことを示す値（例えば、「０」）を戻り値として設定し、一連の処理を終了して元の処理に復帰させる。
一方、ステップＳ２１２で、未判定の特徴点Ａが存在すると判定したとき(Yes)は、ステップＳ２１６に移行して、次の特徴点Ａの座標を取得し、ステップＳ２０８に移行する。
一方、ステップＳ２１０で、４つの脚すね部平面で囲まれる空間に特徴点Ａが属すると判定したとき(Yes)は、ステップＳ２１８に移行して、障害物と干渉することを示す値（例えば、「１」）を戻り値として設定し、一連の処理を終了して元の処理に復帰させる。
In step S212, it is determined whether or not an undetermined feature point A exists, and when it is determined that there is no undetermined feature point A (No), the process proceeds to step S214 and does not interfere with an obstacle. A value indicating this (for example, “0”) is set as a return value, a series of processing is terminated, and the original processing is restored.
On the other hand, when it is determined in step S212 that an undetermined feature point A exists (Yes), the process proceeds to step S216, the coordinates of the next feature point A are acquired, and the process proceeds to step S208.
On the other hand, when it is determined in step S210 that the feature point A belongs to the space surrounded by the four leg shin planes (Yes), the process proceeds to step S218 to indicate a value indicating interference with an obstacle (for example, “1”) is set as a return value, and a series of processing is terminated and the original processing is restored.
次に、本実施の形態の動作を説明する。
脚車輪型ロボット１００の移動経路上に階段が存在すると、水平レーザ２６から照射された水平面レーザ光、および垂直レーザ２８、３０から照射された垂直面レーザ光がそれぞれ階段で反射し、カメラ３２により、それら反射光を含む画像が撮影される。次いで、ステップＳ１００、Ｓ１０２を経て、カメラ３２で撮影された画像が取り込まれ、取り込まれた画像から階段の特徴点が検出される。
Next, the operation of the present embodiment will be described.
If there are stairs on the moving path of the leg-
次いで、ステップＳ１０３を経て、干渉判定処理が実行される。干渉判定処理では、ステップＳ２００〜Ｓ２１０を経て、各脚すね部平面ごとに法線ベクトルＮが算出され、特徴点Ａと点Ｐを結ぶ直線ＰＡと、法線ベクトルＮとの内積Ｄ1〜Ｄ4（またはＤ5、Ｄ6）が算出され、内積Ｄ1〜Ｄ4（またはＤ5、Ｄ6）に基づいて４つの脚すね部平面で囲まれる空間に特徴点Ａが属するか否かが判定される。いずれの特徴点Ａも同空間に属しないと判定されると、ステップＳ２１４、Ｓ１０４を経て、障害物と干渉しないと判定される。非干渉が判定されると、ステップＳ１０５〜Ｓ１１０を経て、検出された特徴点に基づいて階段の幅および段鼻部の実座標が算出され、算出された階段の幅および段鼻部の実座標に基づいて脚先の着地位置が決定される。 Next, an interference determination process is executed through step S103. In the interference determination process, the normal vector N is calculated for each leg shin plane through steps S200 to S210, and the inner product D1 to D4 (the straight line PA connecting the feature point A and the point P and the normal vector N) Alternatively, D5, D6) is calculated, and it is determined whether or not the feature point A belongs to the space surrounded by the four leg shin planes based on the inner products D1-D4 (or D5, D6). If it is determined that none of the feature points A belong to the same space, it is determined through steps S214 and S104 that they do not interfere with the obstacle. When non-interference is determined, the width of the staircase and the actual coordinates of the stair nose are calculated based on the detected feature points through steps S105 to S110, and based on the calculated stair width and the actual coordinates of the stair nose. The landing position of the leg tip is determined.
さらに、ステップＳ１１２〜Ｓ１１６を経て、脚先センサ２２、２４からそれぞれセンサ信号が入力され、蹴込板までの距離および脚先と踏板の位置関係が算出される。そして、ステップＳ１１８、Ｓ１２０を経て、決定された着地位置および算出された両距離に基づいてモータ指令信号が生成され、生成されたモータ指令信号がドライバ４４、５４に出力される。これにより、駆動輪２０が回転するとともに回転関節１４〜１８が駆動し、脚車輪型ロボット１００が姿勢を適切に保ちつつ階段を乗り越える。また、状況によっては階段を回避、停止する。したがって、脚型ロボットと同様に階段への適応性が高い。
Further, through steps S112 to S116, sensor signals are input from the
一方、いずれかの特徴点Ａが同空間に属すると判定されると、ステップＳ２１８、Ｓ１０４を経て、障害物と干渉すると判定される。干渉が判定されると、ステップＳ１２４を経て、干渉回避処理が実行され、干渉が回避される。
一方、平地では、脚車輪型ロボット１００は、車輪走行で移動することができる。したがって、車輪型ロボットと同様に平地での移動性が高い。
On the other hand, if it is determined that any feature point A belongs to the same space, it is determined through steps S218 and S104 that it interferes with the obstacle. If interference is determined, an interference avoidance process is performed through step S124, and interference is avoided.
On the other hand, on a flat ground, the leg-
このようにして、本実施の形態では、レーザ２８〜３０およびカメラ３２からなる物体センサを備え、物体センサの測定結果に基づいて障害物の特徴点を検出し、グローバル座標系Ｐgにおいて、脚車輪型ロボット１００の姿勢並びに基体１０および脚すね部の形状により特定される脚すね部平面と、検出した特徴点との位置関係に基づいて、脚すね部に対して障害物が干渉するか否かを判定する。
As described above, in this embodiment, the object sensor including the
これにより、グローバル座標系Ｐgにおける脚すね部平面と特徴点の位置関係に基づいて干渉の有無を判定するので、実際の干渉を伴うことなく干渉の有無を判定することができる。また、特許文献２における干渉判定専用のカメラのような専用のセンサを設けなくても、本実施の形態のように、他の用途（脚部１２の駆動制御等）に用いられる物体センサを利用することができるので、部品点数および重量の増加を抑制することができる。さらに、グローバル座標系Ｐgにおいて脚すね部平面は姿勢や形状により特定されるため、障害物の特徴点さえ検出できればよく、障害物の監視範囲が脚すね部の可動範囲に制約されず、干渉判定の制約を低減することができる。さらに、脚すね部との位置関係によらず物体センサを設置することができるので、干渉判定の制約をさらに低減することができる。
Thereby, since the presence or absence of interference is determined based on the positional relationship between the leg shin plane and the feature point in the global coordinate system Pg, the presence or absence of interference can be determined without accompanying actual interference. Further, even if a dedicated sensor such as a dedicated camera for interference determination in
さらに、本実施の形態では、グローバル座標系Ｐgにおいて、４つの脚すね部平面で囲まれる空間に特徴点が属するか否かを判定することにより、脚すね部に対して障害物が干渉するか否かを判定する。
これにより、脚すね部に対して障害物が干渉するか否かを比較的正確に判定することができる。
Furthermore, in the present embodiment, whether or not an obstacle interferes with the leg shin portion by determining whether or not the feature point belongs to the space surrounded by the four leg shin portion planes in the global coordinate system Pg. Determine whether or not.
Thereby, it can be determined comparatively accurately whether an obstacle interferes with a leg shin part.
さらに、本実施の形態では、特徴点と脚すね部平面上の任意の点を結ぶ直線と、脚すね部平面の法線ベクトルとの内積に基づいて、４つの脚すね部平面で囲まれる空間に特徴点が属するか否かを判定する。
これにより、４つの脚すね部平面で囲まれる空間に特徴点が属するか否かを比較的容易に判定することができる。
Furthermore, in the present embodiment, a space surrounded by four leg shin part planes based on the inner product of a straight line connecting a feature point and an arbitrary point on the leg shin part plane and a normal vector of the leg shin part plane. It is determined whether or not a feature point belongs to.
Thereby, it can be determined relatively easily whether or not the feature point belongs to a space surrounded by the four shin planes.
さらに、本実施の形態では、レーザ２６〜３０およびカメラ３２からなる画像センサと、脚先センサ２２、２４とを備え、カメラ３２で撮影した画像および脚先センサ２２、２４で測定した距離に基づいて階段を認識し、その認識結果に基づいてモータ４０、５０を制御する。
これにより、画像センサおよび脚先センサ２２、２４を用いて未知の階段を認識しながら脚部１２の昇降制御を行うので、従来に比して、未知の階段に対して高い適応性を実現することができる。また、人が活動する環境での動作を行えるので、人と一緒に行動する用途に用いられるホームロボット、パーソナルロボット等に好適である。
Further, in the present embodiment, the image sensor including the
Thereby, since the raising / lowering control of the
さらに、本実施の形態では、画像センサを基体１０の正面に設け、脚先センサ２２、２４を脚部１２の先端に設けた。
これにより、脚車輪型ロボット１００の移動経路上に存在する物体を広い視野で検出することができるとともに、階段昇降時に駆動輪２０と階段の距離を精度よく測定することができる。
Further, in the present embodiment, the image sensor is provided on the front surface of the
As a result, it is possible to detect an object existing on the movement path of the leg
さらに、本実施の形態では、進行方向に対して所定角度にレーザ光を照射するレーザ２６〜３０と、レーザ２６〜３０から所定距離隔てて設けられかつレーザ光の反射光を含む画像を撮像するカメラ３２とを備え、カメラ３２で撮影した画像に基づいて光切断法により階段を認識する。
これにより、階段の特徴のうち脚部１２の昇降制御に有効な特徴を検出することができるので、未知の階段に対してさらに高い適応性を実現することができる。
Further, in the present embodiment, the
Thereby, since the characteristic effective for the raising / lowering control of the
さらに、本実施の形態では、水平レーザ２６から照射された水平面レーザ光の反射光の撮影状態に基づいて階段の幅を算出し、垂直レーザ２８、３０から照射された２つの垂直面レーザ光の反射光の撮影状態に基づいて階段の段鼻部の実座標を算出する。
これにより、階段の特徴のうち脚部１２の昇降制御にさらに有効な特徴を検出することができるので、未知の階段に対してさらに高い適応性を実現することができる。
Furthermore, in the present embodiment, the width of the staircase is calculated based on the imaging state of the reflected light of the horizontal plane laser light emitted from the
Thereby, since the characteristic more effective for the raising / lowering control of the
さらに、本実施の形態では、前方脚先センサ２２の測定結果に基づいて階段の蹴込板までの距離を算出し、下方脚先センサ２４の測定結果に基づいて駆動輪２０と階段の踏板の位置関係を算出する。
これにより、階段の特徴のうち脚部１２の昇降制御にさらに有効な特徴を検出することができるので、未知の階段に対してさらに高い適応性を実現することができる。
Further, in the present embodiment, the distance to the stair riser plate is calculated based on the measurement result of the front
Thereby, since the characteristic more effective for the raising / lowering control of the
上記第１の実施の形態において、脚すね部は、発明１ないし４の対象部位に対応し、ステップＳ１０２は、発明１または２の特徴点検出手段に対応し、ステップＳ１０３は、発明１ないし５の干渉判定手段に対応し、座標系Ｐ4は、発明４の第１座標系に対応し、グローバル座標系Ｐgは、発明４の第２座標系に対応している。
次に、本発明の第２の実施の形態を図面を参照しながら説明する。図１５ないし図２８は、本発明に係る干渉判定装置および脚車輪型ロボットの第２の実施の形態を示す図である。
In the first embodiment, the leg shanks correspond to the target portions of
Next, a second embodiment of the present invention will be described with reference to the drawings. FIGS. 15 to 28 are diagrams showing a second embodiment of the interference determination device and the leg-wheel type robot according to the present invention.
本実施の形態は、上記第１の実施の形態に対して、レーザ２８〜３０およびカメラ３２からなる物体センサに代えて、測距センサを２方向にそれぞれ走査して測定を行う３次元距離測定装置を用いて特徴点を検出する点が異なる。
まず、脚車輪型ロボット１００の構成を説明する。
図１５は、脚車輪型ロボット１００を側面図である。
The present embodiment is a three-dimensional distance measurement in which the distance measurement sensor scans in two directions in place of the object sensor composed of the
First, the configuration of the leg
FIG. 15 is a side view of the leg
基体１０の正面には、図１５に示すように、３次元距離測定装置２００が取り付けられている。３次元距離測定装置２００の座標系は、基体１０の奥行き方向をｘ軸、基体１０の幅方向をｙ軸、基体１０の高さ方向をｚ軸とし、ｘ軸は基体１０の前方を、ｙ軸は基体１０の右方を、ｚ軸は基体１０の上方をそれぞれ正の方向とする。
次に、３次元距離測定装置２００の外観構造を説明する。
As shown in FIG. 15, a three-dimensional
Next, the external structure of the three-dimensional
図１６は、３次元距離測定装置２００の外観構造を示す図である。同図（ａ）は、３次元距離測定装置２００の正面図（ｚ−ｙ平面）を示す図であり、同図（ｂ）は、３次元距離測定装置２００の上面図（ｘ−ｙ平面）である。
３次元距離測定装置２００は、図１６（ａ）に示すように、板状の支持部材２１９と、２次元距離測定装置２１２と、２次元距離測定装置２１２を回転駆動するモータ２１６と、モータ２１６の回転角度位置を検出するエンコーダ２１８と、モータ２１６の駆動力を２次元距離測定装置２１２に伝達する駆動プーリ２２０ａ、従動プーリ２２０ｂおよびベルト２２１とを有して構成されている。
FIG. 16 is a diagram showing an external structure of the three-dimensional
As shown in FIG. 16A, the three-dimensional
支持部材２１９の下面には、モータ２１６が取り付けられている。モータ２１６の回転軸（以下、駆動回転軸という。）は、支持部材２１９を下方から貫通し、支持部材２１９を挟んで反対側に配置された駆動プーリ２２０ａの軸穴に連結している。
一方、支持部材２１９の下面であってモータ２１６から水平方向に所定距離隔てた位置には、２次元距離測定装置２１２が取り付けられている。２次元距離測定装置２１２は、回転軸（以下、従動回転軸という。）を有し、従動回転軸が支持部材２１９を下方から貫通し、支持部材２１９を挟んで反対側に配置された従動プーリ２２０ｂの軸穴に連結している。
A
On the other hand, a two-dimensional
駆動プーリ２２０ａおよび従動プーリ２２０ｂには、ベルト２２１が巻き掛けられている。したがって、モータ２１６により駆動プーリ２２０ａが回転し、駆動プーリ２２０ａに巻き掛けたベルト２２１により従動プーリ２２０ｂが回転することにより、２次元距離測定装置２１２は、図１６（ｂ）に示すように、ｚ軸回りに回転する。
次に、２次元距離測定装置２１２の制御構造を説明する。
A
Next, the control structure of the two-dimensional
図１７は、２次元距離測定装置２１２の制御構造を示すブロック図である。
２次元距離測定装置２１２は、図１７に示すように、測定範囲内に存在する物体上の測定点までの距離を測定する測距センサ２１２ａと、測距センサ２１２ａを回転駆動するモータ２１２ｃと、モータ２１２ｃの回転角度位置を検出するエンコーダ２１２ｄと、指令信号およびエンコーダ２１２ｄの出力信号に基づいてモータ２１２ｃの駆動を制御するドライバ２１２ｂとを有して構成されている。
FIG. 17 is a block diagram showing a control structure of the two-dimensional
As shown in FIG. 17, the two-dimensional
ドライバ２１２ｂは、センシングプロセッサ２１０からの指令信号において設定された走査角度範囲（例えば、−４０°〜＋４０°等の所定の角度範囲）および走査単位角度（例えば、０．３６°等の所定の単位角度）に基づいて、モータ２１２ｃの回転軸を走査単位角度ずつ回転させる制御を行う。
モータ２１２ｃは、測距センサ２１２ａのレーザ出力部（不図示）および受光部（不図示）をｙ軸回りに回転駆動するように設けられており、ドライバ２１２ｂからの制御信号に応じて、自己の回転軸を走査単位角度（Δθ）ずつ回転駆動する。
The
The
図１８は、測距センサ２１２ａの走査範囲を示す図である。
測距センサ２１２ａの走査範囲は、脚車輪型ロボット１００が階段の昇降や障害物の回避を行うことを目的としているため、図１８に示すように、脚車輪型ロボット１００の下方を重点的に走査するように設定されている。なお、３次元距離測定装置２００および測距センサ２１２ａの原点位置（回転角度θおよびφが０°の位置）においては、測距センサ２１２ａの測定方向がｘ軸と一致し、測距センサ２１２ａの回転軸がｙ軸と一致する。
FIG. 18 is a diagram illustrating a scanning range of the
The scanning range of the
３次元距離測定装置２００は、２次元距離測定装置２１２を回転駆動する回転駆動機構（モータ２１６、エンコーダ２１８、駆動プーリ２２０ａ、従動プーリ２２０ｂ、ベルト２２１および支持部材２１９）が、図１８に示す走査範囲外に設けられているため、図１８に示す走査範囲であれば、３次元距離測定装置２００を構成する各機構部によって、測距センサ２１２ａのｙ軸回りの走査が阻害されない。
In the three-dimensional
また、脚車輪型ロボット１００の走行経路上の障害物を認識できればよいので、ｚ軸回りの回転駆動による走査範囲も、前方１８０°までをカバーする必要はなく、２次元距離測定装置２１２から水平方向に所定距離隔てて配置されたモータ２１６およびエンコーダ２１８を走査範囲外とする範囲でも十分である。したがって、２次元距離測定装置２１２の回転駆動範囲を、モータ２１６およびエンコーダ２１８を含まない範囲とする。
Further, since it is only necessary to be able to recognize an obstacle on the travel route of the leg-
次に、３次元距離測定装置２００の制御構造を説明する。
図１９は、３次元距離測定装置２００の制御構造を示すブロック図である。
３次元距離測定装置２００は、図１９に示すように、センシングプロセッサ２１０と、２次元距離測定装置２１２と、モータ２１６と、エンコーダ２１８と、指令信号およびエンコーダ２１８の出力信号に基づいてモータ２１６の駆動を制御するドライバ２１４とを有して構成されている。
Next, the control structure of the three-dimensional
FIG. 19 is a block diagram showing a control structure of the three-dimensional
As shown in FIG. 19, the three-dimensional
センシングプロセッサ２１０は、専用のプログラムを実行し、ドライバ２１２ｂに指令信号を与えて測距センサ２１２ａを回転させ、測距センサ２１２ａの走査範囲で測定可能な領域（以下、走査平面という。）内に存在する物体上の測定点までの距離を測定する第１走査処理を実行するとともに、１つの走査平面に対する第１走査処理が終了するごとに、ドライバ２１４に指令信号を与えて２次元距離測定装置２１２を回転させる第２走査処理を実行する。
The
さらに、センシングプロセッサ２１０は、第１走査処理および第２走査処理を経て２次元距離測定装置２１２で測定した距離の情報（以下、距離情報という。）に基づいて、各走査平面の各測定点の勾配の出現頻度を算出する処理を実行するとともに、算出した出現頻度および各測定点の座標情報に基づいて、各走査平面における物体の１次元の形状（凹凸）および測定範囲内に存在する物体上の面の形状を認識する処理を実行する。
Furthermore, the
次に、３次元距離測定装置２００の距離測定の原理を説明する。
図２０は、２次元距離測定装置２１２の距離測定の原理を説明する図である。
２次元距離測定装置２１２は、測距センサ２１２ａが、モータ２１２ｃの回転軸の回転駆動に応じて、ｙ軸回りに走査単位角度ずつ回転し、かつ、回転するごとに、図２０に示すように、レーザ出力部からレーザ光を出力するとともに、出力光に対する物体（図２０中の障害物）からの反射光を受光部で受光し、各回転角度（走査角度）に応じた距離（図２０中の測定距離Ｌ（物体と受光部との間の距離））を測定する。
Next, the principle of distance measurement of the three-dimensional
FIG. 20 is a diagram for explaining the principle of distance measurement of the two-dimensional
As shown in FIG. 20, the two-dimensional
図２１は、第１走査処理および第２走査処理により走査を行った場合を示す図である。同図（ａ）は、測距センサ２１２ａをｙ軸回りに回転させたときの測定距離と回転角度θとの関係を示す図であり、同図（ｂ）は、２次元距離測定装置２１２をｚ軸回りに回転させたときの走査平面と回転角度φとの関係を示す図である。
第１走査処理は、例えば、図２１（ａ）に示すように、測距センサ２１２ａをｙ軸回りに走査単位角度ずつ回転させながら、原点位置に対する各回転角度（図２１（ａ）中のθ1、θ2、θ3）に応じた距離情報（図２１（ａ）中のＬ（θ1）、Ｌ（θ2）、Ｌ（θ3））を測定する処理となる。
FIG. 21 is a diagram illustrating a case where scanning is performed by the first scanning process and the second scanning process. FIG. 6A is a diagram showing the relationship between the measurement distance and the rotation angle θ when the
In the first scanning process, for example, as shown in FIG. 21 (a), each rotation angle (θ in FIG. 21 (a)) is rotated with respect to the origin position while rotating the
また、第１走査処理における、モータ２１２ｃの回転軸の回転中心と、レーザの走査軌道線の両端とを結んで形成される平面が、走査平面（物体が存在しない場合は扇形の平面）となる。
ドライバ２１４は、センシングプロセッサ２１０からの指令信号において設定された走査角度範囲および走査単位角度（Δφ）に基づいて、モータ２１６の回転軸を走査単位角度ずつ回転させる制御を行う。
In the first scanning process, a plane formed by connecting the rotation center of the rotating shaft of the
The
モータ２１６は、減速機（不図示）、駆動プーリ２２０ａ、従動プーリ２２０ｂおよびベルト２２１を介して、２次元距離測定装置２１２をｚ軸回りに回転駆動するように設けられており、ドライバ２１４からの制御信号に応じて、自己の回転軸を走査単位角度ずつ回転駆動する。これにより、モータ２１６の回転軸の回転駆動に応じて、駆動プーリ２２０ａ、従動プーリ２２０ｂおよびベルト２２１を介して従動回転軸に回転駆動力が伝達され、２次元距離測定装置２１２がｚ軸回りに走査単位角度ずつ回転する。
The
すなわち、第２走査処理は、図２１（ｂ）に示すように、２次元距離測定装置２１２をｚ軸回りに走査単位角度ずつ回転させる処理となる。そして、第１走査処理と第２走査処理とを交互に連続して行うことにより、第１走査処理によって形成される走査平面をｚ軸回りに連続して形成する。
図２２は、３次元距離測定装置２００の距離の計測例を示す図である。
That is, the second scanning process is a process of rotating the two-dimensional
FIG. 22 is a diagram illustrating a distance measurement example of the three-dimensional
これにより、例えば、図２２に示すように、壁、ついたて、スタンド、棚等の物体の立体的な形状を測定することが可能である。
また、図２１（ｂ）に示すように、第２走査処理後の各測定点の距離情報をＬ（θi，φj）と表記する。ここで、ｉは、ｙ軸回りの走査角度に応じて各測定点に付与される通し番号であり、ｊは、ｚ軸回りの走査角度に応じて各測定点に付与される通し番号である。
Thereby, for example, as shown in FIG. 22, it is possible to measure the three-dimensional shape of an object such as a wall, a wall, a stand, or a shelf.
Further, as shown in FIG. 21B, the distance information of each measurement point after the second scanning process is expressed as L (θ i , φ j ). Here, i is a serial number given to each measurement point according to the scanning angle around the y axis, and j is a serial number given to each measurement point according to the scanning angle around the z axis.
次に、３次元距離測定装置２００における物体認識処理の流れを説明する。
図２３は、３次元距離測定装置２００における物体認識処理を示すフローチャートである。
物体認識処理は、ＣＰＵ６０からの指令信号に基づいて、センシングプロセッサ２１０が、ＲＯＭ（不図示）に記憶された専用のプログラムを読み出し、読み出したプログラムを実行することで実現される処理であり、処理が開始されると、図２３に示すように、まず、ステップＳ３００に移行する。
Next, the flow of object recognition processing in the three-dimensional
FIG. 23 is a flowchart showing object recognition processing in the three-dimensional
The object recognition process is a process realized by the
ステップＳ３００では、３次元距離測定装置２００において、ＣＰＵ６０からの指令信号に基づいて、測距センサ２１２ａおよび２次元距離測定装置２１２の走査角度範囲および走査単位角度を設定し、ステップＳ３０２に移行する。ここで、ＣＰＵ６０からの指令信号には、走査角度範囲および走査単位角度の情報が含まれている。
ステップＳ３０２では、２次元距離測定装置２１２において、センシングプロセッサ２１０からの指令信号に応じて、ドライバ２１２ｂ、モータ２１２ｃおよびエンコーダ２１２ｄを駆動し、測距センサ２１２ａを、ステップＳ３００で設定されたｙ軸回りの走査角度範囲内において、同じくステップＳ３００で設定された走査単位角度ずつｙ軸回りに回転させるとともに、各回転角度に応じた距離情報を測定する処理（第１走査処理）を実行し、ステップＳ３０４に移行する。
In step S300, in the three-dimensional
In step S302, in the two-dimensional
ステップＳ３０４では、センシングプロセッサ２１０において、ステップＳ３０２で測定した距離情報に対して、メディアンフィルタを用いたフィルタリング処理を実行してノイズ成分を除去し、ステップＳ３０６に移行する。
ステップＳ３０６では、センシングプロセッサ２１０において、ステップＳ３０４でノイズ除去後の回転座標系の距離情報を直交座標系の距離情報に変換し、ステップＳ３０８に移行する。
In step S304, the
In step S306, the
回転座標系から直交座標系への変換は、第１走査処理による各測定点の座標は、ｚ−ｘ平面上の座標となり、ｘ軸は、第２走査処理によって、ｚ軸回りに走査単位角度ずつ座標位置が変化する。したがって、直交座標系への変換は、ｚ軸回りの各回転角度ごとに行われる。
ステップＳ３０８では、センシングプロセッサ２１０において、ステップＳ３０６で直交座標系に変換された距離情報に基づいて、各測定点の勾配を算出してステップＳ３１０に移行する。
In the conversion from the rotational coordinate system to the orthogonal coordinate system, the coordinates of each measurement point by the first scanning process are coordinates on the z-x plane, and the x-axis is a scanning unit angle around the z-axis by the second scanning process. The coordinate position changes one by one. Therefore, the conversion to the orthogonal coordinate system is performed for each rotation angle around the z axis.
In step S308, the
図２４は、勾配の算出例を示す図である。
各測定点の勾配は、各測定点の測定距離と、その前後の所定数の測定点の距離情報とを用いて、図２４（ａ）に示すように、最小二乗法を用いて算出する。
図２４（ａ）に示すように、最小二乗法を用いて、勾配の算出対象の測定点（図２４（ａ）中の白丸）を含む前後所定数の測定点に対する回帰直線を近似し、回帰直線の勾配を、対象の測定点の勾配とする。最小二乗法は、データのばらつきによって受ける影響は少ないが、計算量が比較的多くなる。
FIG. 24 is a diagram illustrating a gradient calculation example.
The gradient of each measurement point is calculated using the least square method, as shown in FIG. 24A, using the measurement distance of each measurement point and the distance information of a predetermined number of measurement points before and after that.
As shown in FIG. 24 (a), a regression line is approximated to a predetermined number of measurement points before and after the measurement points (white circles in FIG. 24 (a)) for which the gradient is to be calculated using the least square method. The slope of the straight line is the slope of the target measurement point. The least squares method is less affected by data variations, but the calculation amount is relatively large.
また、別の方法として、図２４（ｂ）に示すように、各測定点に対して、所定数の他の測定点を挟む前後２つの測定点の距離情報の差分値を用いて勾配を算出する。
具体的に、図２４（ｂ）に示すように、前後２つの測定点のｚ軸方向の測定距離の差分値である第１差分値（例えば、注目する測定点のｚ軸方向の距離をｚiとすると、ｚi+2−ｚi-2）を算出するとともに、前後２つの測定点のｘ軸方向の測定距離の差分値である第２差分値（例えば、注目する測定点のｘ軸方向の距離をｘiとすると、ｘi+2−ｘi-2）を算出し、これら算出結果から、注目する測定点の勾配（第１差分値／第２差分値）を求める。この方法では、データのばらつきによる影響が最小二乗法よりも大きくなるが、計算が、単純な引き算と割り算のみで済むため最小二乗法よりも高速に勾配を算出することができる。
As another method, as shown in FIG. 24 (b), for each measurement point, a gradient is calculated using a difference value of distance information between two measurement points before and after a predetermined number of other measurement points. To do.
Specifically, as shown in FIG. 24B, the first difference value (for example, the distance in the z-axis direction of the measurement point of interest is z If i , z i + 2 −z i− 2 ) is calculated, and the second difference value (for example, the x axis of the measurement point of interest) that is the difference value of the measurement distance between the two measurement points in the x-axis direction is calculated. If the distance in the direction is x i , x i + 2 −x i−2 ) is calculated, and the gradient of the measurement point of interest (first difference value / second difference value) is obtained from these calculation results. In this method, the influence due to data variation is greater than that in the least square method, but since the calculation only requires simple subtraction and division, the gradient can be calculated faster than the least square method.
ステップＳ３１０では、センシングプロセッサ２１０において、ステップＳ３０８で算出した勾配に基づいて各勾配の出現頻度を算出し、ステップＳ３１２に移行する。
本実施の形態では、勾配ごとの測定点の個数を出現頻度として算出する。
ステップＳ３１２では、センシングプロセッサ２１０において、ステップＳ３１０で算出した各勾配の出現頻度に基づいて、出現頻度の判定処理が未処理の勾配の出現頻度を選択し、ステップＳ３１４に移行する。
In step S310, the
In the present embodiment, the number of measurement points for each gradient is calculated as the appearance frequency.
In step S312, in the
ステップＳ３１４では、センシングプロセッサ２１０において、ステップＳ３１２で選択した勾配の出現頻度とあらかじめ設定された閾値とを比較するとともに選択された出現頻度は、閾値以上か否かを判定し、閾値以上と判定された場合(Yes)は、ステップＳ３１６に移行し、そうでない場合(No)は、ステップＳ３３４に移行する。
ステップＳ３１６に移行した場合は、センシングプロセッサ２１０において、選択した勾配とこの勾配を有する各測定点の座標情報とを対応付けて不図示のＲＡＭ等の記録媒体に記録し、ステップＳ３１８に移行する。
In step S314, the
When the process proceeds to step S316, the
ステップＳ３１８では、センシングプロセッサ２１０において、すべての勾配についてステップＳ３１４の判定処理が終了したか否かを判定し、終了したと判定された場合(Yes)は、ステップＳ３２０に移行し、そうでない場合(No)は、ステップＳ３１２に移行する。
ステップＳ３２０に移行した場合は、センシングプロセッサ２１０において、ステップＳ３１６で記録した勾配と各測定点の座標情報とに基づいて、連続面（実際は、連続面と走査平面とが交差する線）の有無を判断し、ステップＳ３２２に移行する。
In step S318, the
When the process proceeds to step S320, the
ここで、連続面の判断は、例えば、所定の勾配範囲に含まれる勾配に対応する各測定点の座標情報に基づいて、測定点の座標が連続しているときに、これらの座標位置と交差する連続面があると判断する。所定の勾配範囲に含まれる勾配に対応する測定点の座標が連続していないときは、連続面がないと判断する。
ステップＳ３２２では、センシングプロセッサ２１０において、ステップＳ３２０の判断結果に基づいて連続面があるか否かを判定し、連続面があると判定された場合(Yes)は、ステップＳ３２４に移行し、そうでない場合(No)は、ステップＳ３２６に移行する。
Here, the determination of the continuous surface is performed by, for example, intersecting these coordinate positions when the coordinates of the measurement points are continuous based on the coordinate information of each measurement point corresponding to the gradient included in the predetermined gradient range. It is determined that there is a continuous surface to perform. When the coordinates of the measurement points corresponding to the gradient included in the predetermined gradient range are not continuous, it is determined that there is no continuous surface.
In step S322, the
ステップＳ３２４に移行した場合は、センシングプロセッサ２１０において、連続面に対応する各測定点の測定距離の座標系をセンサ座標系（３次元座標）に変換し、変換後のセンサ座標系の距離情報を勾配の情報と対応付けて、不図示のＲＡＭ等の記録媒体に記録し、ステップＳ３２６に移行する。
ステップＳ３２６では、センシングプロセッサ２１０において、第２走査処理の走査角度範囲および走査単位角度に対応する、すべての走査平面に対して、連続面の判断処理が終了したか否かを判定し、終了したと判定された場合(Yes)は、ステップＳ３２８に移行し、そうでない場合(No)は、ステップＳ３３２に移行する。
When the process proceeds to step S324, in the
In step S326, the
ステップＳ３２８に移行した場合は、センシングプロセッサ２１０において、ステップＳ３２４においてＲＡＭ等の記録媒体に記録した連続面と判断された距離情報に基づいて面データを生成し、ステップＳ３３０に移行する。
ここで、面データの生成は、例えば、各走査平面間において、連続面と判断された勾配およびその測定点の座標情報とを比較し、座標値と勾配とが近いものを面と判断し、これらの座標情報を対応付けたり、公知の補完法を用いてつなぎ合わせたりすることで行う。
When the process proceeds to step S328, the
Here, the generation of the surface data is performed by, for example, comparing the gradient determined to be a continuous surface and the coordinate information of the measurement point between the respective scanning planes, determining that the coordinate value and the gradient are close to a surface, The coordinate information is associated with each other or connected by using a known complement method.
例えば、勾配が０に近い測定点の連続する座標情報から構成される面データの面は、水平面と判断できるので、そこが歩行可能な面だと判断できる。
ステップＳ３３０では、３次元距離測定装置２００において、ハブ７６および通信Ｉ／Ｆ６４を介して、ステップＳ３２８で生成した面データをＣＰＵ６０に出力して処理を終了する。
For example, since the surface of the surface data composed of the continuous coordinate information of the measurement points whose gradient is close to 0 can be determined as a horizontal surface, it can be determined that the surface can be walked.
In step S330, in the three-dimensional
一方、ステップＳ３２６において、すべての走査平面に対して連続面の判断処理が終了しておらず、ステップＳ３３２に移行した場合は、３次元距離測定装置２００において、センシングプロセッサ２１０からの指令信号に応じて、ドライバ２１４、モータ２１６およびエンコーダ２１８を駆動し、２次元距離測定装置２１２を、ステップＳ３００で設定されたｚ軸回りの走査角度範囲内において、同じくステップＳ３００で設定された走査単位角度ずつｚ軸回りに回転させる処理（第２走査処理）を実行し、ステップＳ３０２に移行する。
On the other hand, if the determination process of the continuous plane is not completed for all the scanning planes in step S326 and the process proceeds to step S332, the three-dimensional
また、ステップＳ３１４において、出現頻度が閾値以上ではなく、ステップＳ３３４に移行した場合は、センシングプロセッサ２１０において、ステップＳ３１２で選択した勾配に対応する測定点の測定距離を測定ノイズとして除外し、ステップＳ３１８に移行する。
In step S314, if the appearance frequency is not equal to or higher than the threshold value and the process proceeds to step S334, the
次に、脚車輪型ロボット１００の移動制御システムを説明する。
ＣＰＵ６０は、センサ入力Ｉ／Ｆ６３を介して、３次元距離測定装置２００、前方脚先センサ２２、下方脚先センサ２４、障害物センサ３４、３６および３軸姿勢センサ７０からそれぞれセンサ信号を入力する。
次に、ＣＰＵ６０で実行される処理を説明する。
ＣＰＵ６０は、ステップＳ１００、Ｓ１０２に代えて、３次元距離測定装置２００を用いた特徴点検出処理を実行する。特徴点検出処理では、３次元距離測定装置２００から面データを入力し、そのセンサ座標系における各測定点の座標をグローバル座標系の座標に変換し、変換した測定点のうち面の周縁上の点（段部の境界点）を特徴点として検出する。
Next, the movement control system of the leg
The
Next, processing executed by the
The
次に、本実施の形態の動作を説明する。
図２５は、測定範囲内に階段がある場合の、第１走査処理による測定結果を示す図である。
まず、３次元距離測定装置２００は、ＣＰＵ６０からの指令信号に基づいて、センシングプロセッサ２１０において走査角度範囲および走査単位角度を設定する（ステップＳ３００）。
Next, the operation of the present embodiment will be described.
FIG. 25 is a diagram illustrating a measurement result by the first scanning process when there is a staircase in the measurement range.
First, the three-dimensional
ここで、２次元距離測定装置２１２は、測距範囲２０〜４０９５[mm]、最大走査角度範囲２４０°、角度分解能０．３６°の２次元レンジセンサであることとする。走査角度範囲が２４０°であれば、上述した図１８に示す走査平面を形成する走査角度範囲と同じとなり、２次元距離測定装置２１２の上方にある支持部材２１９、駆動プーリ２２０ａ、従動プーリ２２０ｂおよびベルト２２１が走査範囲に含まれないことになる。
Here, it is assumed that the two-dimensional
また、第１走査処理に対して、走査角度範囲２４０°および走査単位角度０．３６°が設定され、第２走査処理に対して、走査角度範囲−４０°〜＋４０°および走査単位角度１０°が設定されたとする（この場合は、走査平面が９つ形成される）。走査角度範囲−４０°〜＋４０°であれば、２次元距離測定装置２１２から水平方向に所定距離隔てて配置されたモータ２１６およびエンコーダ２１８が走査範囲に含まれないことになる。
Further, a scanning angle range of 240 ° and a scanning unit angle of 0.36 ° are set for the first scanning process, and a scanning angle range of −40 ° to + 40 ° and a scanning unit angle of 10 ° are set for the second scanning process. Is set (in this case, nine scanning planes are formed). If the scanning angle range is −40 ° to + 40 °, the
センシングプロセッサ２１０は、まず、第１走査処理に対して設定された走査角度範囲および走査単位角度に基づいて、ドライバ２１２ｂに指令信号を出力する。
２次元距離測定装置２１２は、ドライバ２１２ｂにおいて、センシングプロセッサ２１０からの指令信号およびエンコーダ２１２ｄからの出力信号に基づいて、モータ２１２ｃの回転軸を回転駆動し、測距センサ２１２ａをｙ軸回りに走査単位角度Δθ＝０．３６°ずつ回転するとともに、各回転角度に応じた距離を測定する第１走査処理を実行する（ステップＳ３０２）。最初は、ｚ軸回りに対して回転角度φが０°の位置（初期位置）に対する第１走査処理を実行する。また、各距離情報（回転座標系）は、データ列Ｌ（θi，φj）として、センシングプロセッサ２１０へと出力される。
First, the
In the
なお、ｙ軸回りの走査範囲内には、基体１０以外に、２次元距離測定装置２１２の駆動機構等の走査を阻害する物が一切存在しないため、走査範囲内に存在する物体の正確な距離情報を得ることができる。
そして、１つの走査平面に対する第１走査処理が終了すると、センシングプロセッサ２１０は、第１走査処理において測定された距離情報に対して、メディアンフィルタを用いたフィルタリング処理を行う（ステップＳ３０４）。これにより、測定情報におけるノイズ成分を除去する。
In the scanning range around the y-axis, there is no object other than the
When the first scanning process for one scanning plane is completed, the
ここで、ノイズ成分除去後の各測定点の測定距離Ｌ[mm]は、例えば、図２５に示すようになる。図２５において、横軸は各走査角度に応じた測定点の番号（第１走査角度番号）であり、縦軸は各走査角度番号の測定点に対する測定距離Ｌ[mm]である。
図２５に示す例では、脚車輪型ロボット１００の歩行経路上に、段差が一定でかつ蹴込板が連続面となっている階段が存在している。
Here, the measurement distance L [mm] of each measurement point after removal of the noise component is, for example, as shown in FIG. In FIG. 25, the horizontal axis is the number of the measurement point corresponding to each scanning angle (first scanning angle number), and the vertical axis is the measurement distance L [mm] to the measurement point of each scanning angle number.
In the example shown in FIG. 25, there are stairs on the walking path of the leg-
次に、センシングプロセッサ２１０は、フィルタリング処理後の回転座標系の距離情報を直交座標系の距離情報に変換する（ステップＳ３０６）。
図２６は、図２５の各測定点の回転座標系の測定距離を直交座標系の測定距離に変換した場合の測定結果を示す図である。
すなわち、図２５に示す各測定点の回転座標系の測定距離を直交座標系の測定距離へと変換することで、図２６に示すように、各回転角度に対応するｘ軸方向の距離[mm]とｚ軸方向の距離[mm]とで表現される２次元の座標情報となる。図２６において、横軸はｘ軸方向の距離Ｌx[mm]であり、縦軸はｚ軸方向の距離Ｌz[mm]である。
Next, the
FIG. 26 is a diagram illustrating a measurement result when the measurement distance in the rotational coordinate system of each measurement point in FIG. 25 is converted into the measurement distance in the orthogonal coordinate system.
That is, by converting the measurement distance of the rotational coordinate system of each measurement point shown in FIG. 25 into the measurement distance of the orthogonal coordinate system, as shown in FIG. 26, the distance in the x-axis direction corresponding to each rotation angle [mm] ] And z-axis direction distance [mm]. In FIG. 26, the horizontal axis represents the distance L x [mm] in the x-axis direction, and the vertical axis represents the distance L z [mm] in the z-axis direction.
次に、直交座標系に変換された距離情報に基づいて、各測定点の勾配を算出する（ステップＳ３０８）。
図２７は、最小二乗法を用いて算出したｘ方向およびｚ方向の勾配の一例を示す図である。
Next, the gradient of each measurement point is calculated based on the distance information converted into the orthogonal coordinate system (step S308).
FIG. 27 is a diagram illustrating an example of gradients in the x direction and the z direction calculated using the least square method.
ここでは、最小二乗法を用いて、各測定点の勾配を算出する。ここで、最小二乗法を用いた場合に、図２７（ａ）および（ｂ）に示すように、最小二乗法を適用する複数の測定点に対して、ｘ方向の勾配と、ｚ方向の勾配とが異なる場合がある。そこで、下式（12）および（13）に示すように、ｘ方向およびｚ方向の勾配を求める２つの直線の方程式を用いて最小二乗法計算を行う。
ｚ＝ａx・ｘ＋ｂx …（12）
ｘ＝ａz・ｚ＋ｂz …（13）
Here, the gradient of each measurement point is calculated using the least square method. Here, when the least square method is used, as shown in FIGS. 27A and 27B, the gradient in the x direction and the gradient in the z direction are applied to a plurality of measurement points to which the least square method is applied. May be different. Therefore, as shown in the following equations (12) and (13), the least square method calculation is performed using two linear equations for obtaining gradients in the x direction and the z direction.
z = a x · x + b x (12)
x = a z · z + b z (13)
そして、上式（12）におけるａxおよびｂxを下式（14）により算出し、上式（13）におけるａzおよびｂzを下式（15）により算出する。 Then, a x and b x in the above equation (12) are calculated by the following equation (14), and a z and b z in the above equation (13) are calculated by the following equation (15).
ａxおよびｂx、並びにａzおよびｂzを算出すると、これらの算出結果を代入した、上式（12）および（13）の直線と、最小二乗計算に用いた各測定点との距離を残差ｈxおよびｈzとして算出するとともに、前記各測定点に対する残差ｈxの二乗和と、残差ｈzの二乗和とを算出する（下式（16））。 When a x and b x and a z and b z are calculated, the distances between the straight lines of the above equations (12) and (13) and the respective measurement points used for the least square calculation are calculated. While calculating as residuals h x and h z , the sum of squares of residuals h x and the sum of squares of residuals h z for the respective measurement points are calculated (the following equation (16)).
残差ｈxの二乗和および残差ｈzの二乗和が算出されると、両者を比較して値の小さい方の直線を、正しい直線として採用し、この直線の勾配に基づいて、算出対象の測定点の勾配を求める。具体的に、残差ｈxの二乗和の方が小さければ、「ａ＝ａx、ｂ＝ｂx」とし、残差ｈzの二乗和の方が小さければ、「ａ＝１／ａz、ｂ＝１／ｂz」とする。 When the sum of squares of the residual h x and the square sum of the residuals h z are calculated, the straight line with the smaller value is compared as the correct straight line, and the calculation target is calculated based on the slope of this straight line. The slope of the measurement point is obtained. Specifically, if the square sum of the residual h x is smaller, “a = a x , b = b x ”. If the square sum of the residual h z is smaller, “a = 1 / az”. , B = 1 / b z ”.
上記のようにして、走査平面に対する全測定点の勾配が算出されると、次に、各勾配の出現頻度を算出する（ステップＳ３１０）。
図２８は、ある走査平面に対する勾配の出現頻度の一例を示す図である。
ここでは、図２８に示すような、出現頻度が得られたとする。
図２８に示すように、勾配が０近傍の出現頻度が比較的高くなっており、これらの勾配に対応する測定点の座標情報が連続していれば、そこに水平面があると推定することができる。
When the gradients of all the measurement points with respect to the scanning plane are calculated as described above, next, the appearance frequency of each gradient is calculated (step S310).
FIG. 28 is a diagram illustrating an example of the appearance frequency of a gradient with respect to a certain scanning plane.
Here, it is assumed that the appearance frequency as shown in FIG. 28 is obtained.
As shown in FIG. 28, if the frequency of appearance near the gradient is relatively high and the coordinate information of the measurement points corresponding to these gradients is continuous, it can be estimated that there is a horizontal plane there. it can.
出現頻度が算出されると、センシングプロセッサ２１０は、次に、未判定の勾配の出現頻度を選択し（ステップＳ３１２）、その出現頻度が、あらかじめ設定された閾値以上か否かを判定する（ステップＳ３１４）。そして、閾値以上であれば（ステップＳ３１４の「Ｙｅｓ」の分岐）、勾配と測定点の座標情報とを対応付けてＲＡＭ（不図示）に記録する（ステップＳ３１６）。一方、閾値未満であれば（ステップＳ３１４の「Ｎｏ」の分岐）、測定ノイズとして、測定点の距離情報を除外する（ステップＳ３３４）。
When the appearance frequency is calculated, the
上記の判定処理が、すべての勾配に対して終了すると（ステップＳ３１８の「Ｙｅｓ」の分岐）、次に、ＲＡＭに記録された、出現頻度が閾値以上の勾配および測定点の座標情報に基づいて、連続面の有無を判断する（ステップＳ３２０）。
脚車輪型ロボット１００の移動経路上には、段差が一定でかつ蹴込板が連続面となっている階段が存在しているため、階段部分にある水平面等が、勾配０および略０の測定点の連続する座標として現われる。このような場合に、センシングプロセッサ２１０は、連続面（実際は、面と走査平面との交差線）がある（ステップＳ３２２の「Ｙｅｓ」の分岐）と判断する。一方、座標の途中で勾配がいきなり外れた値となったときは、そこで面（実際は、面と走査平面との交差線）が途切れていると判断する。これらの判断結果から、物体の凹凸を認識することが可能である。
When the above determination processing is completed for all the gradients (“Yes” branch in step S318), next, based on the gradient recorded in the RAM and having the appearance frequency equal to or greater than the threshold and the coordinate information of the measurement points. Then, the presence / absence of a continuous surface is determined (step S320).
On the moving path of the leg-
そして、連続面に対応する直交座標系の測定距離を、センサ座標系（３次元）の測定距離に変換し、ＲＡＭに記録する（ステップＳ３２４）。
連続面の判断が終了すると、３次元距離測定装置２００は、ドライバ２１４において、センシングプロセッサ２１０からの指令信号およびエンコーダ２１８からの出力信号に基づいて、モータ２１６の回転軸を回転駆動し、２次元距離測定装置２１２をｚ軸回りに回転角度１０°ずつ回転する第２走査処理を実行する（ステップＳ３３２）。第２走査処理によって、２次元距離測定装置２１２の向きが１つ前の状態に対してｚ軸回りに１０°だけ変化する。そして、２次元距離測定装置２１２は、この状態で、再び、第１走査処理を実行する（ステップＳ３０２）。これにより、ｚ軸回りに１０°ずれた位置に新たな走査平面が形成される。
Then, the measurement distance of the orthogonal coordinate system corresponding to the continuous surface is converted into the measurement distance of the sensor coordinate system (three-dimensional) and recorded in the RAM (step S324).
When the determination of the continuous surface is completed, the three-dimensional
そして、設定された走査角度範囲（−４０°〜＋４０°）に対して、設定された走査単位角度１０°で、２次元距離測定装置２１２を回転させ、各回転角度φにおいて、第１走査処理を実行する。
ここで、２次元距離測定装置２１２が基体１０の正面に取り付けられているため、基体１０が走査範囲内に含まれてしまうが、脚車輪型ロボット１００の前方および歩行経路上を含む範囲においては阻害物が一切ないため、脚車輪型ロボット１００の歩行制御を行うのに十分な走査範囲が確保できているといえる。
Then, the two-dimensional
Here, since the two-dimensional
そして、９つの走査平面に対する距離の測定が終了すると（ステップＳ３２６の「Ｙｅｓ」の分岐）、次に、ＲＡＭに記録された連続面の情報に基づいて面データを生成する（ステップＳ３２８）。
面データは、上述したように、ｚ軸回りに連続する各隣り合う走査平面間において、勾配および座標の近い連続面をつなぎ合わせる（対応付ける）ことで生成される。生成した面データは、ハブ７６および通信Ｉ／Ｆ６４を介して、ＣＰＵ６０に出力される（ステップＳ３３０）。
When the measurement of the distances to the nine scanning planes is completed (“Yes” branch of step S326), surface data is then generated based on the information of the continuous surface recorded in the RAM (step S328).
As described above, the plane data is generated by connecting (corresponding) continuous planes having close gradients and coordinates between adjacent scanning planes that are continuous around the z axis. The generated surface data is output to the
そして、ＣＰＵ６０により、入力された面データについてセンサ座標系の座標がグローバル座標系の座標に変換され、特徴点が検出される。
また、ＣＰＵ６０により、入力された面データが解析され、特に、勾配が０に近い交差線を有する面がある場合は、それを水平面とみなし、脚車輪型ロボット１００が歩行可能な面であると判断される。
Then, the
Further, the input surface data is analyzed by the
歩行可能な面であると判断された場合は、面データのグローバル座標系への変換結果と、３軸姿勢センサ７０のセンサ信号等とに基づいて脚先の着地位置が決定される。
なお、段差が一定でかつ蹴込板が連続面となる階段を例に挙げたが、段差が一定でない階段、蹴込板が連続面とならない階段、蹴込板の無い階段等に対しても、正確にその面の形状を認識することができるので、脚車輪型ロボット１００の階段への適応性を高めることが可能である。
If it is determined that the face is a walkable face, the landing position of the leg tip is determined based on the conversion result of the face data to the global coordinate system, the sensor signal of the
In addition, although the steps are constant and the staircase where the kick plate is a continuous surface is given as an example, the staircase where the step is not constant, the staircase where the kick plate is not a continuous surface, the staircase without the kick plate, etc. Since the shape of the surface can be recognized, the adaptability of the leg
このようにして、本実施の形態では、測距センサ２１２ａをｙ軸回りに回転させるモータ２１２ｃ等からなるｙ軸回転機構と、測距センサ２１２ａをｚ軸回りに回転させるモータ２１６等からなるｚ軸回転機構とを備え、ｙ軸回転機構により測距センサ２１２ａを回転させながらｙ軸回転機構の走査単位角度ごとに測距センサ２１２ａの測定結果を取得する第１走査を、ｚ軸回転機構により測距センサ２１２ａを回転させながらｚ軸回転機構の走査単位角度ごとに行う第２走査を行うことにより、ｙ軸回転機構の走査単位角度ごとおよびｚ軸回転機構の走査単位角度ごとの測定結果を取得する。
Thus, in the present embodiment, a z-axis rotation mechanism including a
これにより、障害物の立体的な形状を把握することができ、これに基づいて検出された特徴点により干渉判定を行うことができるので、干渉判定の精度を向上することができる。また、測距センサ２１２ａを回転させる回転機構を採用したので、移動機構に比して、走査に必要なスペースが小さくてすみ、走査のための機構が簡素となり、しかも高速な走査を実現することができる。
Thereby, the three-dimensional shape of the obstacle can be grasped, and interference determination can be performed based on the feature points detected based on the three-dimensional shape, so that the accuracy of interference determination can be improved. In addition, since a rotation mechanism that rotates the
さらに、本実施の形態では、モータ２１６およびエンコーダ２１８と、２次元距離測定装置２１２とを水平方向に所定距離隔てて配置し、駆動回転軸の回転駆動力を、駆動プーリ２２０ａ、従動プーリ２２０ｂおよびベルト２２１を介して従動回転軸へと伝達し、２次元距離測定装置２１２をｚ軸回りに回転駆動する構成とした。
これにより、２次元距離測定装置２１２の走査角度範囲内には、走査を阻害するものが一切なくなるので、正確な距離情報を得ることができる。また、モータ２１６およびエンコーダ２１８と、２次元距離測定装置２１２とを水平方向に配置したので、３次元距離測定装置２００の高さ方向の占有率を低減することができる。
Further, in the present embodiment, the
As a result, there is no obstacle to scanning within the scanning angle range of the two-dimensional
さらに、本実施の形態では、測距センサ２１２ａをｙ軸回りに走査単位角度ずつ回転させるとともに、各回転角度における距離を測定する第１走査処理を実行し、距離情報に基づいて、各測定点の勾配および勾配の出現頻度を算出することで、走査平面内に存在する物体の凹凸を認識する。
これにより、連続面が存在することを推定することができる。
Further, in the present embodiment, the
Thereby, it can be estimated that a continuous surface exists.
さらに、本実施の形態では、１つの走査平面に対する第１走査処理が終了するごとに、２次元距離測定装置２１２をｚ軸回りに走査単位角度だけ回転させる第２走査処理を実行する。
これにより、複数の走査平面に対して第１走査処理を実行することができるので、測定範囲内に存在する物体上の面の形状を認識することができる。また、面の形状が認識できるので、脚車輪型ロボット１００は、未知の障害物に対して高い適応性を実現することができる。また、人が活動する環境での動作を行えるので、人と一緒に行動する用途に用いられるホームロボット、パーソナルロボット等のヒューマンアシストロボットに好適である。
Further, in the present embodiment, every time the first scanning process for one scanning plane is completed, the second scanning process for rotating the two-dimensional
As a result, the first scanning process can be executed on a plurality of scanning planes, so that the shape of the surface on the object existing within the measurement range can be recognized. Further, since the shape of the surface can be recognized, the leg
上記第２の実施の形態において、ｙ軸は、発明７の第１走査軸に対応し、ｚ軸は、発明７の第２走査軸に対応し、ｙ軸回転機構は、発明６若しくは７の第１走査手段、または発明７の第１回転手段に対応し、ｚ軸回転機構は、発明６若しくは７の第２走査手段、または発明７の第２回転手段に対応している。また、ステップＳ３０２は、発明６または７の測定結果取得手段に対応している。
In the second embodiment, the y-axis corresponds to the first scanning axis of the
なお、上記第１および第２の実施の形態においては、脚すね部平面と特徴点の位置関係に基づいて干渉の有無を判定するように構成したが、これに限らず、脚すね部の形状に対応する線（直線、線分、多次曲線、円、楕円その他の曲線）と、特徴点との位置関係に基づいて干渉の有無を判定するように構成することもできる。また、脚すね部に対応する空間と、特徴点との位置関係に基づいて干渉の有無を判定するように構成することもできる。 In the first and second embodiments, the configuration is such that the presence or absence of interference is determined based on the positional relationship between the leg shin plane and the feature points. It is also possible to determine whether or not there is interference based on the positional relationship between a line (straight line, line segment, multi-order curve, circle, ellipse, or other curve) corresponding to the feature point. Moreover, it can also comprise so that the presence or absence of interference may be determined based on the positional relationship between the space corresponding to the leg shank and the feature point.
また、上記第１および第２の実施の形態においては、レーザ２８〜３０およびカメラ３２からなる物体センサまたは３次元距離測定装置２００を用いて特徴点を検出するように構成したが、これに限らず、他の方式の物体センサを用いて特徴点を検出するように構成することもできる。
また、上記第１および第２の実施の形態においては、物体センサを用いて特徴点を検出するように構成したが、これに限らず、物体センサを用いることなく、あらかじめ測定、計算若しくは定義した特徴点に関する特徴点情報、またはシミュレーション等の計算結果に基づいて特徴点を検出するように構成することもできる。
In the first and second embodiments, the feature point is detected by using the object sensor including the
In the first and second embodiments, the feature point is detected using the object sensor. However, the present invention is not limited to this, and the measurement, calculation, or definition is performed in advance without using the object sensor. It can also be configured to detect feature points based on feature point information on feature points or calculation results such as simulation.
また、上記第１および第２の実施の形態においては、干渉の有無を判定するように構成したが、これに限らず、干渉しないことのみを判定すること、干渉することのみを判定すること、干渉の度合いを判定すること、干渉までの度合い（例えば、特徴点と脚すね部の間の距離）を判定するように構成することもできる。
また、上記第２の実施の形態においては、エンコーダ２１８を駆動回転軸に設けて構成としたが、これに限らず、従動回転軸に設けて構成とすることもできる。
Further, in the first and second embodiments, the presence / absence of interference is determined. However, the present invention is not limited to this, and it is determined only that there is no interference, it is determined only that there is interference, It can also be configured to determine the degree of interference and to determine the degree of interference (for example, the distance between the feature point and the leg shin part).
In the second embodiment, the
これにより、伝達誤差の影響を受けずに第２走査処理における走査角度を高精度に検出することができる。
また、上記第２の実施の形態においては、駆動プーリ２２０ａ、従動プーリ２２０ｂおよびベルト２２１を介して、駆動回転軸の回転駆動力を従動回転軸へと伝達するように構成としたが、これに限らず、複数の歯車を介して、駆動回転軸の回転駆動力を従動回転軸へと伝達するように構成とすることもできる。
Thereby, the scanning angle in the second scanning process can be detected with high accuracy without being affected by the transmission error.
In the second embodiment, the rotational driving force of the driving rotating shaft is transmitted to the driven rotating shaft via the driving
また、上記第２の実施の形態においては、第１走査処理における距離情報の取得を離散的（第２走査処理で回転させてから第１走査処理で距離情報を取得）に行う構成としたが、これに限らず、走査角度と測定距離との対応付けを行うことで連続的（第２走査処理で回転させつつ、第１走査処理で距離情報を取得）に行う構成とすることもできる。
また、上記第２の実施の形態においては、ＣＰＵ６０からの指令信号に基づいて、走査角度範囲および走査単位角度を設定するように構成としたが、これに限らず、３次元距離測定装置２００にあらかじめ設定しておく構成とすることもできる。
In the second embodiment, the distance information in the first scanning process is acquired discretely (the distance information is acquired in the first scanning process after being rotated in the second scanning process). However, the present invention is not limited to this, and a configuration in which scanning angles and measurement distances are associated with each other continuously (distance information is acquired by the first scanning process while being rotated by the second scanning process) may be employed.
In the second embodiment, the scanning angle range and the scanning unit angle are set based on the command signal from the
また、上記第２の実施の形態において、３次元距離測定装置２００は、測距センサ２１２ａをｙ軸回りに回転させ、２次元距離測定装置２１２をｚ軸回りに回転させる構成としたが、これに限らず、測距センサ２１２ａをｚ軸回りに回転させ、２次元距離測定装置２１２をｙ軸回りに回転させる構成とすることもできる。また、ｙ軸およびｚ軸回りに限らず、測距センサ２１２ａの測定方向に対して互いに直交する２つの軸であれば、どの方向の軸回りでもよい。さらに、このような回転機構に限らず、測距センサ２１２ａの測定方向とは異なる第１走査方向に測距センサ２１２ａを移動させ、測定方向および第１走査方向とは異なる第２走査方向に測距センサ２１２ａを移動させるように移動機構として構成することもできる。この場合、移動経路の形状としては、直線のほか円弧その他の曲線を採用することができる。回転機構と移動機構の組み合わせることもできる。
In the second embodiment, the three-dimensional
また、上記第２の実施の形態においては、図２９（ａ）に示すように、２次元距離測定装置２１２自身を回転させているが、これに限らず、光学式の測距センサを有する２次元距離測定装置であれば、図２９（ｂ）に示すように、測定方向の光軸上に挿入したミラーを回転させてもよい。
In the second embodiment, as shown in FIG. 29 (a), the two-dimensional
図２９は、測距センサの測定方向を変更する場合の構成を示す図である。
また、上記第１および第２の実施の形態においては、脚すね部を判定の対象部位として設定したが、これに限らず、脚車輪型ロボット１００の任意の部位を判定の対象部位として設定することができる。
また、上記第１および第２の実施の形態においては、本発明に係る干渉判定装置および脚車輪型ロボットを、階段を乗り越える場合について適用したが、これに限らず、階段以外の段差を乗り越える場合についても同様に適用することができる。
FIG. 29 is a diagram illustrating a configuration when the measurement direction of the distance measuring sensor is changed.
Moreover, in the said 1st and 2nd embodiment, although the leg shin part was set as an object part of judgment, it is not restricted to this but arbitrary parts of leg
In the first and second embodiments, the interference determination device and the leg-wheel type robot according to the present invention are applied to the case of going over the stairs. The same applies to.
また、上記第１および第２の実施の形態においては、本発明に係る干渉判定装置および脚車輪型ロボットを脚車輪型ロボット１００に適用したが、これに限らず、本発明の主旨を逸脱しない範囲で他の場合にも適用可能である。例えば、直動関節で脚構造を実現する脚車輪型ロボットや他の構成のロボット、車両その他の装置に適用することができる。
In the first and second embodiments, the interference determination device and the leg wheel type robot according to the present invention are applied to the leg
１００ 脚車輪型ロボット
１０ 基体
１２ 脚部
１４〜１８ 回転関節
２０ 駆動輪
２２、２４ 脚先センサ
２６ 水平レーザ
２８、３０ 垂直レーザ
３２ カメラ
３４、３６ 障害物センサ
４０、５０ モータ
４２、５２ エンコーダ
４４、５４ ドライバ
７０ ３軸姿勢センサ
２００ ３次元距離測定装置
２１０ センシングプロセッサ
２１２ ２次元距離測定装置
２１２ａ 測距センサ
２１２ｃ、２１６ モータ
２１２ｄ、２１８ エンコーダ
２１２ｂ、２１４ ドライバ
２２０ａ 駆動プーリ
２２０ｂ 従動プーリ
２２１ ベルト
DESCRIPTION OF
Claims (7)
障害物の特徴点を検出する特徴点検出手段と、
前記特徴点および前記対象部位の座標を定める座標系において、前記対象部位またはこれと連動する部位の姿勢および前記対象部位の形状により特定される線、平面または空間であって、前記対象部位の形状に対応する線、前記対象部位の表面に対応する平面または前記対象部位に対応する空間と、前記特徴点検出手段で検出した特徴点との位置関係に基づいて、前記対象部位について障害物との干渉判定を行う干渉判定手段とを備えることを特徴とする干渉判定装置。 An interference determination device that performs interference determination with an obstacle for a target part,
Feature point detection means for detecting feature points of obstacles;
In the coordinate system that defines the coordinates of the feature point and the target part, a line, a plane, or a space that is specified by the posture of the target part or a part linked to the target part and the shape of the target part, and the shape of the target part On the basis of the positional relationship between the feature point detected by the feature point detecting means and the plane corresponding to the surface of the target site or the space corresponding to the target site and the feature point detected by the feature point detecting means. An interference determination apparatus comprising: an interference determination unit that performs interference determination.
前記物体センサの測定結果に基づいて障害物の特徴点を検出する特徴点検出手段と、
前記特徴点および前記対象部位の座標を定める座標系において、当該脚車輪型ロボットの姿勢および前記対象部位の形状により特定される線、平面または空間であって、前記対象部位の形状に対応する線、前記対象部位の表面に対応する平面または前記対象部位に対応する空間と、前記特徴点検出手段で検出した特徴点との位置関係に基づいて、前記対象部位について障害物との干渉判定を行う干渉判定手段とを備えることを特徴とする脚車輪型ロボット。 A base, a leg connected to the base with a degree of freedom, a wheel rotatably provided on the leg, and an object sensor, and driving of the leg and rotation of the wheel A leg-wheel robot that moves by
Feature point detecting means for detecting a feature point of an obstacle based on the measurement result of the object sensor;
In the coordinate system that defines the coordinates of the feature point and the target part, a line, a plane, or a space that is specified by the posture of the leg-wheel robot and the shape of the target part, and corresponds to the shape of the target part Then, based on the positional relationship between the plane corresponding to the surface of the target site or the space corresponding to the target site and the feature point detected by the feature point detecting means, the interference determination of the target site with the obstacle is performed. A leg-wheel type robot comprising an interference determination unit.
前記干渉判定手段は、前記座標系において、前記対象部位の表面に対応する複数の平面で囲まれる空間に前記特徴点が属するか否かを判定することにより、前記対象部位に対して障害物が干渉するか否かを判定することを特徴とする脚車輪型ロボット。 In claim 2,
In the coordinate system, the interference determination unit determines whether the feature point belongs to a space surrounded by a plurality of planes corresponding to the surface of the target part, so that an obstacle exists for the target part. A leg-wheel robot characterized by determining whether or not to interfere.
前記干渉判定手段は、当該脚車輪型ロボットの姿勢および前記対象部位の形状に基づいて、前記対象部位の座標を定める第１座標系の座標を、前記特徴点の座標を定める第２座標系の座標に変換して前記第２座標系における前記複数の平面を特定し、前記第２座標系において、前記複数の平面で囲まれる空間に前記特徴点が属するか否かを判定することを特徴とする脚車輪型ロボット。 In claim 3,
The interference determination unit is configured to convert the coordinates of the target part based on the posture of the leg-wheel robot and the shape of the target part into the coordinates of the first coordinate system and the coordinates of the feature point. Converting into coordinates to identify the plurality of planes in the second coordinate system, and determining whether or not the feature points belong to a space surrounded by the plurality of planes in the second coordinate system. Leg-wheel type robot.
前記干渉判定手段は、前記特徴点と前記平面上の任意の点を結ぶ直線と、前記平面の法線ベクトルとの内積に基づいて、前記複数の平面で囲まれる空間に前記特徴点が属するか否かを判定することを特徴とする脚車輪型ロボット。 In any one of Claims 3 and 4,
The interference determination means determines whether the feature point belongs to a space surrounded by the plurality of planes based on an inner product of a straight line connecting the feature point and an arbitrary point on the plane and a normal vector of the plane. A leg-wheel type robot characterized by determining whether or not.
前記物体センサは、物体上の測定点までの距離を測定する測距センサであり、
前記測距センサの測定方向とは異なる第１走査方向に前記測距センサを走査する第１走査手段と、前記測定方向および前記第１走査方向とは異なる第２走査方向に前記測距センサを走査する第２走査手段と、前記第１走査手段および前記第２走査手段の走査範囲で測定可能な前記測定点について前記測距センサの測定結果を取得する測定結果取得手段とを備え、
前記特徴点検出手段は、前記測定結果取得手段で取得した測定結果に基づいて前記特徴点を検出することを特徴とする脚車輪型ロボット。 In any one of Claims 2 thru | or 5,
The object sensor is a distance measuring sensor that measures a distance to a measurement point on an object,
First scanning means for scanning the distance measuring sensor in a first scanning direction different from the measuring direction of the distance measuring sensor, and the distance measuring sensor in a second scanning direction different from the measuring direction and the first scanning direction. Second scanning means for scanning, and measurement result acquisition means for acquiring measurement results of the distance measuring sensor for the measurement points that can be measured in the scanning ranges of the first scanning means and the second scanning means,
The feature wheel detection unit detects the feature point based on the measurement result acquired by the measurement result acquisition unit.
前記第１走査手段は、前記測定方向に対して所定角度をなす第１走査軸の回りに前記測距センサを回転させる第１回転手段であり、
前記第２走査手段は、前記測定方向および前記第１走査軸に対して所定角度をなす第２走査軸の回りに前記測距センサを回転させる第２回転手段であり、
前記測定結果取得手段は、前記第１回転手段により前記測距センサを回転させながら前記第１回転手段の所定単位角度ごとに前記測距センサの測定結果を取得する第１走査を、前記第２回転手段により前記測距センサを回転させながら前記第２回転手段の所定単位角度ごとに行う第２走査を行うことにより、前記第１回転手段の所定単位角度ごとおよび前記第２回転手段の所定単位角度ごとの前記測定結果を取得することを特徴とする脚車輪型ロボット。 In claim 6,
The first scanning unit is a first rotating unit that rotates the distance measuring sensor around a first scanning axis that forms a predetermined angle with respect to the measurement direction.
The second scanning unit is a second rotating unit that rotates the distance measuring sensor around a second scanning axis that forms a predetermined angle with respect to the measurement direction and the first scanning axis;
The measurement result acquisition means performs a first scan for acquiring a measurement result of the distance measurement sensor for each predetermined unit angle of the first rotation means while rotating the distance measurement sensor by the first rotation means. By performing the second scan performed at every predetermined unit angle of the second rotating means while rotating the distance measuring sensor by the rotating means, the predetermined unit angle of the first rotating means and the predetermined unit of the second rotating means. A leg-wheel type robot characterized by acquiring the measurement result for each angle.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2007218646A JP2009050936A (en) | 2007-08-24 | 2007-08-24 | Interference determination device and leg wheel type robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2007218646A JP2009050936A (en) | 2007-08-24 | 2007-08-24 | Interference determination device and leg wheel type robot |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JP2009050936A true JP2009050936A (en) | 2009-03-12 |
Family
ID=40502488
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2007218646A Pending JP2009050936A (en) | 2007-08-24 | 2007-08-24 | Interference determination device and leg wheel type robot |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2009050936A (en) |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101090082B1 (en) * | 2009-05-29 | 2011-12-07 | 한국원자력연구원 | System and method for automatic measuring of the stair dimensions using a single camera and a laser |
| JP2012220227A (en) * | 2011-04-05 | 2012-11-12 | Toyota Motor Corp | Method and system for recognizing obstacle for moving body |
| JP2013066996A (en) * | 2011-09-07 | 2013-04-18 | Honda Motor Co Ltd | Contact state estimating apparatus, and trajectory generation apparatus |
| JP2013114330A (en) * | 2011-11-25 | 2013-06-10 | Chiba Inst Of Technology | Environment information obtaining system using unattended traveling body |
| JP2014100767A (en) * | 2012-11-20 | 2014-06-05 | Toshiba Corp | Level difference walking control device and method of multileg walking robot |
| WO2014162360A1 (en) * | 2013-04-05 | 2014-10-09 | テルモ株式会社 | Sphygmomanometer and blood pressure value computation method |
| WO2016135861A1 (en) * | 2015-02-24 | 2016-09-01 | 株式会社日立製作所 | Manipulator, motion planning method for manipulator, and control system for manipulator |
| CN107225556A (en) * | 2017-07-01 | 2017-10-03 | 徐州乐泰机电科技有限公司 | A kind of removable mineral transporter |
| JP2017198581A (en) * | 2016-04-28 | 2017-11-02 | 富士通株式会社 | Measuring apparatus and measuring system |
-
2007
- 2007-08-24 JP JP2007218646A patent/JP2009050936A/en active Pending
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101090082B1 (en) * | 2009-05-29 | 2011-12-07 | 한국원자력연구원 | System and method for automatic measuring of the stair dimensions using a single camera and a laser |
| JP2012220227A (en) * | 2011-04-05 | 2012-11-12 | Toyota Motor Corp | Method and system for recognizing obstacle for moving body |
| JP2013066996A (en) * | 2011-09-07 | 2013-04-18 | Honda Motor Co Ltd | Contact state estimating apparatus, and trajectory generation apparatus |
| JP2013114330A (en) * | 2011-11-25 | 2013-06-10 | Chiba Inst Of Technology | Environment information obtaining system using unattended traveling body |
| JP2014100767A (en) * | 2012-11-20 | 2014-06-05 | Toshiba Corp | Level difference walking control device and method of multileg walking robot |
| WO2014162360A1 (en) * | 2013-04-05 | 2014-10-09 | テルモ株式会社 | Sphygmomanometer and blood pressure value computation method |
| WO2016135861A1 (en) * | 2015-02-24 | 2016-09-01 | 株式会社日立製作所 | Manipulator, motion planning method for manipulator, and control system for manipulator |
| JPWO2016135861A1 (en) * | 2015-02-24 | 2017-06-22 | 株式会社日立製作所 | Manipulator, manipulator operation planning method, and manipulator control system |
| JP2017198581A (en) * | 2016-04-28 | 2017-11-02 | 富士通株式会社 | Measuring apparatus and measuring system |
| CN107225556A (en) * | 2017-07-01 | 2017-10-03 | 徐州乐泰机电科技有限公司 | A kind of removable mineral transporter |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP2009050936A (en) | Interference determination device and leg wheel type robot | |
| EP2888603B1 (en) | Robot positioning system | |
| JP5510081B2 (en) | Obstacle avoidance support device, obstacle avoidance support method, and moving object | |
| JP2009096335A (en) | Legged robot | |
| JP4241651B2 (en) | Mobile device | |
| JP2009008648A (en) | Three-dimensional distance measuring device and caster-type robot | |
| JP6828579B2 (en) | Environmental maintenance robot and its control program | |
| JP5310285B2 (en) | Self-position estimation apparatus and self-position estimation method | |
| JP5278283B2 (en) | Autonomous mobile device and control method thereof | |
| JP5649298B2 (en) | Object detection apparatus and object detection method | |
| JP2011209845A (en) | Autonomous mobile body, self-position estimation method and map information creation system | |
| JP2011027598A (en) | Environment recognition device and moving body with the same | |
| JP2007190654A (en) | Leg-wheel robot | |
| JP6781535B2 (en) | Obstacle determination device and obstacle determination method | |
| JP6187499B2 (en) | Self-localization method for autonomous mobile robot, autonomous mobile robot, and landmark for self-localization | |
| JP2009006984A (en) | Leg wheel type robot | |
| TW202024666A (en) | Information processing device and mobile robot | |
| KR100809379B1 (en) | Apparatus for extracting of flat region using triangle region orthogonal vector and method therefor | |
| KR101359649B1 (en) | obstacle detection sensor | |
| KR102564663B1 (en) | Coordinates recognition apparatus of automatic guided vehicle and method thereof | |
| JP2009006466A (en) | Leg wheel type robot | |
| JP2010005718A (en) | Leg type robot | |
| KR100904769B1 (en) | Detecting device of obstacle and method thereof | |
| CN113610910A (en) | Obstacle avoidance method for mobile robot | |
| JP2009042146A (en) | Apparatus and method for recognizing object |