JP2022027072A

JP2022027072A - Moving body

Info

Publication number: JP2022027072A
Application number: JP2020130861A
Authority: JP
Inventors: 義文郡; Yoshifumi Kori; ジュイヒングエン; Duyhinh Nguyen
Original assignee: Panasonic Intellectual Property Management Co Ltd
Current assignee: Panasonic Intellectual Property Management Co Ltd
Priority date: 2020-07-31
Filing date: 2020-07-31
Publication date: 2022-02-10

Abstract

To provide a technique for increasing estimation precision of a self position as much as possible.SOLUTION: A moving body traveling on a first plane within a three-dimensional space includes: a measuring device; a travel distance measurement part; a conversion part; a position estimation part; and an optical element. The measuring device includes a second plane as a scanning surface, acquires reflection light by successively emitting light while changing an angle on the scanning surface, and measures a distance to a reflection point for each angle. The travel distance measurement part measures a travel distance of the moving body. The conversion part converts each output value from the measuring device expressed in a two-dimensional coordinate system to a three-dimensional coordinate value by using a predetermined conversion expression. The position estimation part collates each three-dimensional coordinate value after conversion with map data, and successively outputs position information for showing a position and posture of the moving body on the first plane on the basis of a collation result. The optical element changes an advance direction of partial light emitted into a predetermined angle range θref from the measuring device from among light on the second plane emitted from the measuring device.SELECTED DRAWING: Figure 9A

Description

本開示は、移動体に備わる２次元測距装置を用いた、自己位置推定の技術に関する。 The present disclosure relates to a technique for self-position estimation using a two-dimensional distance measuring device provided in a moving body.

現在、自動運転車のニーズが高まり、自動運転に関連する様々な技術開発が盛んに行われている。自動運転に関連する特に重要な技術として、自己位置推定技術がある。自己位置推定技術は、例えばＧＰＳを用いる方法、ジャイロセンサなどの慣性計測装置（Inertial Measurement Unit；ＩＭＵ）を用いて算出する方法など様々な方法が存在する。中でも特に、測距が可能なセンサを用いて周囲の距離情報を取得し、事前に用意した地図情報とのマッチング結果により位置を推定する方法は、屋内や屋外環境問わずによく用いられる。この方法に用いられるセンサとして代表的なものとしては、レーザセンサ（ＬＩＤＡＲ）といった、レーザ光を用いて２次元平面上乃至３次元空間内の物体までの距離を測定するセンサが用いられる。３次元空間の測距が可能なセンサを用いるほうが、取得できる情報が多いため、外部環境からの影響が受けづらくなりロバスト性が向上することがわかっている。しかしながら、そのようなセンサは高価であるため、実用上は、より安価な、２次元平面上の測距が可能なセンサ（２次元センサ）を用いることが多い。しかしながら２次元センサでは、例えば、人混み内を走るときなど移動体の経路上に予期せぬ障害物が多い場合は、自己位置推定の精度が著しく低下してしまうという問題がある。 Currently, the needs for autonomous vehicles are increasing, and various technological developments related to autonomous driving are being actively carried out. Self-position estimation technology is a particularly important technology related to autonomous driving. As the self-position estimation technique, there are various methods such as a method using GPS and a method of calculating using an inertial measurement unit (IMU) such as a gyro sensor. Above all, a method of acquiring surrounding distance information using a sensor capable of distance measurement and estimating a position based on a matching result with map information prepared in advance is often used regardless of indoor or outdoor environment. As a typical sensor used in this method, a sensor such as a laser sensor (LIDAR) that measures a distance to an object on a two-dimensional plane or in a three-dimensional space using a laser beam is used. It has been found that using a sensor capable of measuring a distance in a three-dimensional space makes it less susceptible to the influence of the external environment and improves robustness because more information can be acquired. However, since such a sensor is expensive, in practice, a cheaper sensor (two-dimensional sensor) capable of measuring a distance on a two-dimensional plane is often used. However, the two-dimensional sensor has a problem that the accuracy of self-position estimation is significantly lowered when there are many unexpected obstacles on the path of a moving body, for example, when running in a crowd.

そこで例えば特許文献１は、走行路面に対してスキャン面が斜めになるよう、２次元センサまたは２次元計測装置を移動体に配置し、移動体の移動量と組み合わせることで、２次元の計測データを３次元に変換する技術を開示する。 Therefore, for example, in Patent Document 1, a two-dimensional sensor or a two-dimensional measuring device is arranged on a moving body so that the scanning surface is slanted with respect to the traveling road surface, and the two-dimensional measurement data is combined with the moving amount of the moving body. Discloses a technique for converting the above into three dimensions.

国際公開第２０１６／１５７４２８号International Publication No. 2016/157428

移動体が２次元センサを用いて自己位置を推定しながら移動する際には、自己位置の推定精度を可能な限り高くすることが求められる。 When a moving body moves while estimating its own position using a two-dimensional sensor, it is required to make the estimation accuracy of the self-position as high as possible.

本開示の目的の一つは、自己位置の推定精度をできるだけ高くするための技術を提供することである。 One of the objects of the present disclosure is to provide a technique for making the estimation accuracy of the self-position as high as possible.

本開示にかかる例示的な移動体は、３次元空間内の平面（第１平面）上を移動する。移動体は、計測装置と、移動量計測部と、変換部と、位置推定部と、駆動装置とを有している。計測装置は、第１平面と異なる第２平面をスキャン面として有し、スキャン面上で角度を変えながら次々と光を出射して反射光を取得し、角度ごとに反射点までの距離を測定する。移動量計測部は、移動体の移動量に応じた物理量を計測する内界センサの出力から、移動体の移動量を計測する。変換部は、第２平面上の角度及び距離による２次元座標系で表現された計測装置からの各出力値を、所定の変換式を用いて、３次元空間の座標値である３次元座標値に変換する。位置推定部は、変換後の各３次元座標値と３次元空間の座標値を用いて予め作成された地図データとを照合して、照合結果に基づき第１平面上の移動体の位置および姿勢を示す位置情報を順次出力する。駆動装置は、移動体を移動させるための駆動力を発生させる。移動体はさらに光学素子を備えている。光学素子は、計測装置から角度を変えながら出射される第２平面上の光のうち、計測装置から所定の角度範囲θｒｅｆ内に出射される一部の光の進む方向を変化させる。 An exemplary mobile body according to the present disclosure moves on a plane (first plane) in a three-dimensional space. The moving body has a measuring device, a moving amount measuring unit, a conversion unit, a position estimation unit, and a driving device. The measuring device has a second plane different from the first plane as a scanning plane, emits light one after another while changing the angle on the scanning plane to acquire reflected light, and measures the distance to the reflection point for each angle. do. The movement amount measuring unit measures the movement amount of the moving body from the output of the internal sensor that measures the physical quantity according to the moving amount of the moving body. The conversion unit uses a predetermined conversion formula to convert each output value from the measuring device represented by the two-dimensional coordinate system based on the angle and distance on the second plane into the three-dimensional coordinate value which is the coordinate value in the three-dimensional space. Convert to. The position estimation unit collates each of the converted three-dimensional coordinate values with the map data created in advance using the coordinate values of the three-dimensional space, and based on the collation result, the position and orientation of the moving body on the first plane. The position information indicating is output in sequence. The drive device generates a driving force for moving the moving body. The mobile body is further equipped with an optical element. The optical element changes the traveling direction of a part of the light emitted from the measuring device on the second plane while changing the angle, which is emitted from the measuring device within a predetermined angle range θref.

本開示の例示的な実施形態にかかる移動体によれば、自己位置の推定精度をできるだけ高くすることが可能になる。 According to the moving body according to the exemplary embodiment of the present disclosure, it is possible to make the estimation accuracy of the self-position as high as possible.

図１Ａは、本実施形態にかかる移動体１の外観を示す後方からの斜視図である。FIG. 1A is a rear perspective view showing the appearance of the moving body 1 according to the present embodiment. 図１Ｂは、移動体１の側面図である。FIG. 1B is a side view of the moving body 1. 図１Ｃは、移動体１の上面図である。FIG. 1C is a top view of the moving body 1. 図２は、３つの座標系の関係を示す図である。FIG. 2 is a diagram showing the relationship between the three coordinate systems. 図３は、制御ユニット５のハードウェア構成及び他の構成要素との接続関係の一例を示す図である。FIG. 3 is a diagram showing an example of the hardware configuration of the control unit 5 and the connection relationship with other components. 図４は、ある空間に測距センサ３を置いたときの各反射点の位置を模式的に示す図である。FIG. 4 is a diagram schematically showing the position of each reflection point when the distance measuring sensor 3 is placed in a certain space. 図５は、移動体１が移動しながら、測距センサ３から順次放射された、複数の光線３Ａの各反射点の位置を示す図である。FIG. 5 is a diagram showing the positions of the reflection points of the plurality of light rays 3A sequentially emitted from the distance measuring sensor 3 while the moving body 1 is moving. 図６は、制御ユニット５の演算回路５ａによる世界座標系の点群データを取得する手順を示すフローチャートである。FIG. 6 is a flowchart showing a procedure for acquiring point cloud data in the world coordinate system by the arithmetic circuit 5a of the control unit 5. 図７は、移動体１の位置推定前の状態を示す図である。FIG. 7 is a diagram showing a state before the position estimation of the moving body 1. 図８は、位置推定部５０３の処理の手順を示すフローチャートである。FIG. 8 is a flowchart showing the processing procedure of the position estimation unit 503. 図９Ａは、移動体１に搭乗者Ｈが乗車したときの側面図である。FIG. 9A is a side view when the passenger H gets on the moving body 1. 図９Ｂは、移動体１に搭乗者Ｈが乗車したときの上面図である。FIG. 9B is a top view when the passenger H gets on the moving body 1. 図１０は、移動体１がある空間内を移動している様子を示す斜視図である。FIG. 10 is a perspective view showing how the moving body 1 is moving in a certain space. 図１１は、複数の凸部９０１、９０２及び９０３が存在する空間を模式的に示す図である。FIG. 11 is a diagram schematically showing a space in which a plurality of convex portions 901, 902 and 903 exist. 図１２は、点群データＰｇｒｏｕｐ＿Ａ及びＰｇｒｏｕｐ＿Ｂが異なる凸部９０１及び９０２をカバーする様子を示す図である。FIG. 12 is a diagram showing how the point cloud data Pgroup_A and Pgroup_B cover different convex portions 901 and 902. 図１３は、変形例にかかる光学素子により、各凸部の点群データが得られる様子を示す図である。FIG. 13 is a diagram showing how the point cloud data of each convex portion can be obtained by the optical element according to the modified example. 図１４は、測距センサ３の光線が光学素子４Ｂによって反射され、ステップ角がより小さくなっていることを模式的に示す図である。FIG. 14 is a diagram schematically showing that the light beam of the distance measuring sensor 3 is reflected by the optical element 4B and the step angle is smaller.

（本開示に至った経緯）
自己位置を推定しながら移動する移動体の一例として、自動車や電動車いすのような、人が乗車する移動体が挙げられる。特許文献１では、自動車のルーフに２次元センサが設置されている。 (Background to this disclosure)
An example of a moving body that moves while estimating its own position is a moving body on which a person rides, such as a car or an electric wheelchair. In Patent Document 1, a two-dimensional sensor is installed on the roof of an automobile.

一方、電動車いすのような移動体に２次元センサを設置しようとすると、自動車と比較して、２次元センサを設置可能な位置が制約される。さらに、乗車した人自体が固定的に２次元計測装置の検知範囲に干渉するため、有効な計測データが削減されてしまう。 On the other hand, when an attempt is made to install a two-dimensional sensor on a moving body such as an electric wheelchair, the position where the two-dimensional sensor can be installed is restricted as compared with an automobile. Further, since the occupant itself fixedly interferes with the detection range of the two-dimensional measuring device, effective measurement data is reduced.

本発明者は、上述の課題を見出し、当該課題を解決するための構成を検討した。そして、人体等の固定的な障害物が２次元センサの検知範囲に干渉することを回避しつつ、２次元センサによって取得されるデータのデータ量が削減されない構成を有する移動体をなすに至った。具体的には、以下の通りである。 The present inventor has found the above-mentioned problem and examined the configuration for solving the problem. Then, while avoiding a fixed obstacle such as a human body from interfering with the detection range of the two-dimensional sensor, the moving body has a configuration in which the amount of data acquired by the two-dimensional sensor is not reduced. .. Specifically, it is as follows.

ある実施形態において、移動体は人または荷物を運搬可能である。所定の角度範囲θｒｅｆは、第２平面上で、第２平面と交差する人の体または荷物が存在する範囲を含む。 In certain embodiments, the mobile is capable of carrying a person or luggage. The predetermined angular range θref includes a range on the second plane in which a person's body or luggage intersecting the second plane.

ある実施形態において、所定の角度範囲θｒｅｆは、第２平面上で、第２平面と交差する人の体または荷物が存在すると推定される最大範囲を含む。 In certain embodiments, the predetermined angular range θref includes the maximum range on which it is presumed that a person's body or luggage intersecting the second plane.

ある実施形態において、上述の一部の光は複数の光線を含む。光学素子は、各光線が進む方向を第２平面と非平行な方向に変化させる。 In certain embodiments, some of the above-mentioned lights include a plurality of rays. The optical element changes the direction in which each light ray travels in a direction non-parallel to the second plane.

ある実施形態において、第２平面は第１平面に対して角度α［ｄｅｇ］だけ傾いている。光学素子は光を反射させる反射面を有し、反射面の法線と第２平面とがなす角度はβ［ｄｅｇ］（０＜β＜９０）である。光学素子は、各光線が進む方向を、α―２β＋１８０［ｄｅｇ］の方向に変化させる。 In one embodiment, the second plane is tilted by an angle α [deg] with respect to the first plane. The optical element has a reflecting surface that reflects light, and the angle formed by the normal of the reflecting surface and the second plane is β [deg] (0 <β <90). The optical element changes the direction in which each light ray travels in the direction of α-2β + 180 [deg].

ある実施形態において、計測装置が出射する光は、第１光線と、第１光線の直後に出射される第２光線とを含む。光学素子によって進む方向が変更される前の、第１光線と第２光線とがなす角度をθ１［ｄｅｇ］とし、光学素子によって進む方向が変更された後の、第１光線と第２光線とがなす角度をθ２［ｄｅｇ］とすると、θ１＞θ２が成り立つ。 In certain embodiments, the light emitted by the measuring device includes a first ray and a second ray emitted immediately after the first ray. The angle formed by the first ray and the second ray before the traveling direction is changed by the optical element is set to θ1 [deg], and the first ray and the second ray after the traveling direction is changed by the optical element. If the angle formed by is θ2 [deg], then θ1> θ2 holds.

ある実施形態において、計測装置から出射される、上述の一部の光は第１光線群を含み、計測装置から出射される、一部の光以外の光は第２光線群を含む。第１光線群及び第２光線群は、３次元空間の互いに異なる第１領域及び第２領域に入射し、反射される。 In certain embodiments, some of the above-mentioned light emitted from the measuring device includes a first group of light rays, and some non-light emitted from the measuring device includes a second group of light rays. The first ray group and the second ray group are incident on and reflected in the first and second regions different from each other in the three-dimensional space.

ある実施形態において、光学素子は、ミラー、反射型偏光子またはプリズムである。 In certain embodiments, the optical element is a mirror, a reflective modulator or a prism.

ある実施形態において、移動体は、１または複数のコンピュータプログラムを格納した記憶装置と、１または複数のコンピュータプログラムを実行する１または複数の演算回路とをさらに備えていてもよい。１または複数の演算回路は、１または複数のコンピュータプログラムに記述された命令群にしたがって、移動量計測部、変換部、及び、位置推定部として動作する。 In certain embodiments, the mobile may further include a storage device that stores one or more computer programs and one or more arithmetic circuits that execute the one or more computer programs. The one or more arithmetic circuits operate as a movement amount measuring unit, a conversion unit, and a position estimation unit according to a group of instructions described in one or a plurality of computer programs.

ある実施形態において、計測装置は、光の出射時刻と反射光を取得した時刻とを利用して距離を測定するレーザセンサであってもよい。 In certain embodiments, the measuring device may be a laser sensor that measures the distance using the time when the light is emitted and the time when the reflected light is acquired.

以下、適宜図面を参照しながら、本開示にかかる実施形態を詳細に説明する。但し、必要以上に詳細な説明は省略する場合がある。例えば、既によく知られた事項の詳細説明や実質的に同一の構成に対する重複説明を省略する場合がある。これは、以下の説明が不必要に冗長になるのを避け、当業者の理解を容易にするためである。なお、発明者は、当業者が本開示を十分に理解するために添付図面及び以下の説明を提供するのであって、これらによって特許請求の範囲に記載の主題を限定することを意図するものではない。 Hereinafter, embodiments according to the present disclosure will be described in detail with reference to the drawings as appropriate. However, more detailed explanation than necessary may be omitted. For example, detailed explanations of already well-known matters and duplicate explanations for substantially the same configuration may be omitted. This is to avoid unnecessary redundancy of the following description and to facilitate the understanding of those skilled in the art. It should be noted that the inventor intends to limit the subject matter described in the claims by those skilled in the art by providing the accompanying drawings and the following description in order to fully understand the present disclosure. do not have.

本実施形態では、「移動体」を以下のように定義する。すなわち「移動体」とは、人または荷物を載せて、自己位置を推定しながら移動する装置であり、移動のための駆動力(traction)を発生させる電動モータ、二足または多足歩行装置、プロペラなどの駆動装置を備える。本開示における「移動体」の用語は、自動走行する自動車、電動車いす、モバイルロボット、サービスロボット、及びドローンを含み得る。 In this embodiment, the "moving body" is defined as follows. That is, a "moving body" is a device that carries a person or luggage and moves while estimating its own position, and is an electric motor, a two-legged or multi-legged walking device that generates a driving force (traction) for movement. Equipped with a drive device such as a propeller. The term "moving body" in the present disclosure may include autonomous vehicles, electric wheelchairs, mobile robots, service robots, and drones.

移動体は、自己位置を推定するに当たって、二次元平面（以下、単に「平面」と記述する。）をスキャン面として有する測距装置を利用する。本実施形態では、測距装置はレーザセンサまたは測距センサ（ＬＩＤＡＲ）である。測距センサは、スキャン面上で角度を変えながら次々と光を出射して反射光を取得し、角度ごとに反射点までの距離を計測する。上述の測距装置、その具体例であるレーザセンサまたは測距センサは、計測装置の一例である。 In estimating the self-position, the moving body uses a distance measuring device having a two-dimensional plane (hereinafter, simply referred to as “plane”) as a scanning surface. In this embodiment, the ranging device is a laser sensor or a ranging sensor (LIDAR). The distance measuring sensor emits light one after another while changing the angle on the scanning surface to acquire the reflected light, and measures the distance to the reflection point for each angle. The above-mentioned ranging device, a laser sensor or a ranging sensor as a specific example thereof, is an example of a measuring device.

本実施形態にかかる移動体は、人または荷物を運搬可能である。そして、当該人または荷物が搭載された際、当該人または荷物が測距センサのスキャン面の一部に固定的に干渉する、換言すれば、当該人または荷物が測距センサのスキャン面の一部を恒常的に遮る場合に、本開示にかかる技術が好適に利用され得る。 The moving object according to the present embodiment can carry a person or luggage. Then, when the person or luggage is loaded, the person or luggage fixedly interferes with a part of the scanning surface of the ranging sensor, in other words, the person or luggage is one of the scanning surfaces of the ranging sensor. The technique according to the present disclosure can be suitably used when the portion is permanently shielded.

以下、図１Ａ～図１Ｃを参照しながら、本開示にかかる実施形態を説明する。 Hereinafter, embodiments according to the present disclosure will be described with reference to FIGS. 1A to 1C.

図１Ａは、本実施形態にかかる移動体１の外観を示す後方からの斜視図である。「後方」とは、移動体１の進行方向を「前方」としたときの反対側を示している。図１Ｂは移動体１の側面図であり、図１Ｃは移動体１の上面図である。 FIG. 1A is a rear perspective view showing the appearance of the moving body 1 according to the present embodiment. The "rear" indicates the opposite side when the traveling direction of the moving body 1 is "forward". 1B is a side view of the moving body 1, and FIG. 1C is a top view of the moving body 1.

移動体１は一般的に電動車いすと呼ばれ、通常は操縦桿を用いて人が移動する方向を指示する移動装置である。本実施形態では、人を運搬可能で、かつ、自己位置を推定しながら自立的に移動する、車いす型の自動搬送車であってもよい。なお、移動体１が運搬する対象は人に限られず、荷物であってもよい。 The moving body 1 is generally called an electric wheelchair, and is usually a moving device that indicates a direction in which a person moves by using a control stick. In the present embodiment, it may be a wheelchair-type automatic guided vehicle that can carry a person and moves independently while estimating its own position. The target to be carried by the moving body 1 is not limited to a person, but may be a baggage.

移動体１が配置された空間の位置を定義する３次元の世界座標系をΣｗとおく。世界座標系Σｗは、互いに直交する３本の軸、すなわちＸｗ軸、Ｙｗ軸及びＺｗ軸によって張られる。説明の便宜のため、図１Ａ～図１Ｃでは、移動体１の進行方向がＸｗ軸と平行であるとする。移動体１は、Ｘｗ－Ｙｗ平面上を移動することができる。なお、移動体１が移動する「平面」は数学的な意味で厳密な平面でなくてもよい。移動体１が移動する「平面」には、微視的に見たときに凹凸が存在してもよいし、曲面が含まれていてもよい。 Let Σw be the three-dimensional world coordinate system that defines the position of the space where the moving body 1 is placed. The world coordinate system Σw is stretched by three axes orthogonal to each other, that is, the Xw axis, the Yw axis, and the Zw axis. For convenience of explanation, in FIGS. 1A to 1C, it is assumed that the traveling direction of the moving body 1 is parallel to the Xw axis. The moving body 1 can move on the Xw-Yw plane. The "plane" on which the moving body 1 moves does not have to be a strict plane in a mathematical sense. The "plane" on which the moving body 1 moves may have irregularities when viewed microscopically, or may include curved surfaces.

図１Ａを参照する。移動体１は、４つの車輪２と、測距センサ３と、光学素子４と、制御ユニット５と、駆動装置１０とを有する。 See FIG. 1A. The mobile body 1 has four wheels 2, a distance measuring sensor 3, an optical element 4, a control unit 5, and a driving device 10.

測距センサ３は、外界の情報（距離、角度）を、光、より具体的にはレーザ光、を用いて２次元的に測定する。本明細書では、測距センサ３が測定を行う平面を「スキャン面」と呼ぶことがある。測距センサ３は、スキャン面上で角度を変えながら次々と光を出射して反射光を取得し、角度ごとに反射点までの距離を測定する。測距センサ３の詳細は後述する。 The distance measuring sensor 3 two-dimensionally measures information (distance, angle) in the outside world using light, more specifically, laser light. In the present specification, the plane on which the distance measuring sensor 3 makes a measurement may be referred to as a “scanning surface”. The distance measuring sensor 3 emits light one after another while changing the angle on the scanning surface to acquire the reflected light, and measures the distance to the reflection point for each angle. The details of the distance measuring sensor 3 will be described later.

測距センサ３は、Ｘｗ－Ｙｗ平面に対して任意の角度α（０≦α＜９０［ｄｅｇ］）だけ傾けて取り付けられている。これにより、測距センサ３のスキャン面はＸｗ－Ｙｗ平面に対して角度αだけ傾く。 The distance measuring sensor 3 is attached at an angle α (0 ≦ α <90 [deg]) with respect to the Xw-Yw plane. As a result, the scan surface of the distance measuring sensor 3 is tilted by an angle α with respect to the Xw-Yw plane.

光学素子４は、測距センサ３の所定の角度範囲内に設けられ、測距センサ３が照射する光線の一部の進む向きを、予め定められた方向へ反射させる。このような光学素子４として、例えばミラー、反射型偏光子またはプリズムを採用し得る。光学素子４が設定される範囲の詳細は後述する。 The optical element 4 is provided within a predetermined angle range of the distance measuring sensor 3 and reflects a part of the light rays emitted by the distance measuring sensor 3 in a predetermined direction. As such an optical element 4, for example, a mirror, a reflective polarizing element, or a prism can be adopted. The details of the range in which the optical element 4 is set will be described later.

制御ユニット５は、移動体１に備えられ、移動体１の移動量や、測距センサ３からのデータを処理する。移動体１の移動量は、例えば、後述するロータリエンコーダから出力されるパルス信号のパルス数に基づいて決定される。また制御ユニット５は、測距センサ３からのデータを利用して現在の自己位置を推定する。制御ユニット５は、推定の結果に従い、駆動装置１０を動作させるための信号を生成し、所望の目的地に向かうよう、駆動装置１０に送信する。 The control unit 5 is provided in the moving body 1 and processes the moving amount of the moving body 1 and the data from the distance measuring sensor 3. The amount of movement of the moving body 1 is determined, for example, based on the number of pulses of the pulse signal output from the rotary encoder described later. Further, the control unit 5 estimates the current self-position using the data from the distance measuring sensor 3. The control unit 5 generates a signal for operating the drive device 10 according to the estimation result, and transmits the signal to the drive device 10 so as to go to a desired destination.

駆動装置１０は、例えば２台の電気モータ及び各電気モータに電力を供給するモータ駆動回路を含む。駆動装置１０は、制御ユニット５からＰＷＭ信号を受け取り、モータ駆動回路はＰＷＭ信号に従って各電気モータに電力を供給する。これにより、各電気モータは回転して駆動力を発生させ、２つの車輪２の各々を回転させる。 The drive device 10 includes, for example, two electric motors and a motor drive circuit that supplies electric power to each electric motor. The drive device 10 receives a PWM signal from the control unit 5, and the motor drive circuit supplies electric power to each electric motor according to the PWM signal. As a result, each electric motor rotates to generate a driving force to rotate each of the two wheels 2.

本実施形態では説明を一般化し、よりわかりやすくするため、３つの座標系を設定する。図２は、３つの座標系の関係を示している。３つの座標系はいずれも、互いに直交する３本の軸で張られる直交座標系であり、具体的には、世界座標系Σｗ（Ｘｗ、Ｙｗ、Ｚｗ）、ロボット座標系Σｒ（Ｘｒ、Ｙｒ、Ｚｒ）、センサ座標系Σｌ（Ｘｌ、Ｙｌ、Ｚｌ）とする。 In this embodiment, three coordinate systems are set in order to generalize the explanation and make it easier to understand. FIG. 2 shows the relationship between the three coordinate systems. All three coordinate systems are orthogonal coordinate systems stretched by three axes orthogonal to each other. Specifically, the world coordinate system Σw (Xw, Yw, Zw) and the robot coordinate system Σr (Xr, Yr, Zr), sensor coordinate system Σl (Xl, Yl, Zl).

世界座標系Σｗは任意の座標を原点とし、移動や回転は行われない。また移動体１はＸｗ－Ｙｗ面に対して平行に移動する。世界座標系Σｗが建造物等の屋内の場合、Ｚｗ軸は天井面の法線に平行である。 The world coordinate system Σw has an arbitrary coordinate as the origin and is not moved or rotated. Further, the moving body 1 moves in parallel with the Xw-Yw plane. When the world coordinate system Σw is indoors such as a building, the Zw axis is parallel to the normal of the ceiling surface.

ロボット座標系Σｒは移動体１の中心（例えば回転中心）を原点にした座標系となり、移動体１の進行方向をＸｒ軸、進行方向に対して横方向をＹｒ軸、天井方向をＺｒ軸とする。 The robot coordinate system Σr is a coordinate system whose origin is the center of the moving body 1 (for example, the center of rotation). do.

センサ座標系Σｌは測距センサ３の中心を原点にした座標系であり、ロボット座標系Σｒと一定の距離と角度を有し、ロボット座標系Σｒと共に移動する。 The sensor coordinate system Σl is a coordinate system with the center of the ranging sensor 3 as the origin, has a constant distance and angle from the robot coordinate system Σr, and moves together with the robot coordinate system Σr.

レーザ光は、測距センサ３からＸｌ－Ｙｌ面内に出射される。Ｘｌ－Ｙｌ面に対して垂直方向をＺｌ軸とする。 The laser beam is emitted from the ranging sensor 3 into the Xl-Yl plane. The direction perpendicular to the Xl-Yl plane is defined as the Zl axis.

本明細書ではＹｒ軸とＹｌ軸は平行であるとする。このとき、図１に示すようにＸｌ軸とＸｒ軸とは角度αのオフセットを有している。本明細書では、ロボット座標系Σｒとセンサ座標系Σｌの相対的な位置関係は固定されている。 In this specification, it is assumed that the Yr axis and the Yl axis are parallel. At this time, as shown in FIG. 1, the Xl axis and the Xr axis have an offset of an angle α. In this specification, the relative positional relationship between the robot coordinate system Σr and the sensor coordinate system Σl is fixed.

（制御ユニット５の構成）
図３は、制御ユニット５のハードウェア構成及び他の構成要素との接続関係の一例を示している。制御ユニット５は演算回路５ａと、記憶装置５ｂとを有している。 (Configuration of control unit 5)
FIG. 3 shows an example of the hardware configuration of the control unit 5 and the connection relationship with other components. The control unit 5 has an arithmetic circuit 5a and a storage device 5b.

演算回路５ａは、例えばＣＰＵ（Central Processing Unit）と呼ばれる半導体集積回路である。以下では便宜上、演算回路５ａは１つの半導体集積回路であるとして説明するが、演算回路５ａは複数の半導体集積回路を含んでもよい。 The arithmetic circuit 5a is, for example, a semiconductor integrated circuit called a CPU (Central Processing Unit). Hereinafter, for convenience, the arithmetic circuit 5a will be described as one semiconductor integrated circuit, but the arithmetic circuit 5a may include a plurality of semiconductor integrated circuits.

記憶装置５ｂは、半導体記憶媒体、磁気記憶媒体、光学式記憶媒体などの記憶媒体にデータを格納する。データは、演算回路５ａが処理に利用する３次元データＰｇｒｏｕｐ及び地図データＭであり、さらに、演算回路５ａが実行するコンピュータプログラム５０６も含まれる。 The storage device 5b stores data in a storage medium such as a semiconductor storage medium, a magnetic storage medium, or an optical storage medium. The data is the three-dimensional data Pgroup and the map data M used for processing by the arithmetic circuit 5a, and further includes a computer program 506 executed by the arithmetic circuit 5a.

記憶装置５ｂが半導体記憶媒体である場合、本明細書では、記憶装置５ｂはＲＡＭおよびＲＯＭを包括する。ＲＯＭに記憶されているコンピュータプログラム５０６は、演算回路５ａによって読み出され、ＲＡＭに展開される。これにより、演算回路５ａは、コンピュータプログラム５０６を実行することができる。 When the storage device 5b is a semiconductor storage medium, the storage device 5b includes RAM and ROM in the present specification. The computer program 506 stored in the ROM is read out by the arithmetic circuit 5a and expanded in the RAM. As a result, the arithmetic circuit 5a can execute the computer program 506.

ソフトウェア的には、制御ユニット５は移動量計測部５０１、変換部５０２、及び、位置推定部５０３を含む。演算回路５ａは、実行するコンピュータプログラム５０６に記述された命令群にしたがって、ある時刻では移動量計測部５０１として動作し、他の時刻では変換部５０２として動作し、さらに他の時刻では位置推定部５０３として動作する。 In terms of software, the control unit 5 includes a movement amount measuring unit 501, a conversion unit 502, and a position estimation unit 503. The arithmetic circuit 5a operates as a movement amount measuring unit 501 at a certain time, as a conversion unit 502 at another time, and a position estimation unit at another time according to a group of instructions described in the computer program 506 to be executed. Operates as 503.

移動量計測部５０１は、移動体１の内部状態を計測する内界センサの計測結果を利用して移動体１の移動量（移動方向および／または移動距離）を計測する。内界センサを例示すると、駆動装置１０が車輪２を駆動する電気モータの場合には、当該電気モータの出力軸に取り付けられ、電気モータの回転速度を計測するロータリエンコーダ１２である。内界センサの他の例は、少なくとも２軸の角度または角速度、及び、加速度を検出するジャイロセンサなどの慣性計測装置である。 The movement amount measuring unit 501 measures the movement amount (movement direction and / or movement distance) of the moving body 1 by using the measurement result of the internal sensor that measures the internal state of the moving body 1. For example, in the case of an electric motor in which the drive device 10 drives the wheels 2, the rotary encoder 12 is attached to the output shaft of the electric motor and measures the rotation speed of the electric motor. Another example of an internal sensor is an inertial measurement unit such as a gyro sensor that detects at least two axes of angle or angular velocity, and acceleration.

内界センサがロータリエンコーダ１２である場合、ロータリエンコーダ１２の出力を積分して回転回数を求め、さらに車輪２の外周長との積を求めることにより、車輪２が回転した距離、すなわち移動体１の移動距離を算出することができる。車輪２の外周長は、２π・（車輪２の外周円の半径）によって計算される。 When the internal sensor is the rotary encoder 12, the output of the rotary encoder 12 is integrated to obtain the number of rotations, and the product with the outer peripheral length of the wheel 2 is obtained to obtain the distance that the wheel 2 has rotated, that is, the moving body 1. The moving distance of can be calculated. The outer peripheral length of the wheel 2 is calculated by 2π · (radius of the outer peripheral circle of the wheel 2).

なお、２つの車輪２の各々に電気モータ及びロータリエンコーダ１２が取り付けられている場合、各車輪２が回転した距離に差が生じる場合があり得る。このとき、各車輪２が回転した距離を利用して、回転中心の位置を求め、その後回転中心周りの旋回角を求めればよい。旋回前の移動体１の進行方向を基準として、旋回後の移動体１の進行方向は（９０度－旋回角）によって算出し得る。 When the electric motor and the rotary encoder 12 are attached to each of the two wheels 2, there may be a difference in the distance rotated by each wheel 2. At this time, the position of the center of rotation may be obtained by using the distance rotated by each wheel 2, and then the turning angle around the center of rotation may be obtained. The traveling direction of the moving body 1 after turning can be calculated by (90 degrees-turning angle) with reference to the traveling direction of the moving body 1 before turning.

また、内界センサが慣性計測装置である場合、移動量計測部５０１は、慣性計測装置の出力を積分することにより、初期状態から変位した角度及び移動距離を算出することができる。 When the internal sensor is an inertial measurement unit, the movement amount measuring unit 501 can calculate the angle and the moving distance displaced from the initial state by integrating the output of the inertial measurement unit.

変換部５０２は、測距センサ３と接続されており、測距センサ３から得られた点群データＰｒａｗを、移動量計測部５０１の移動量を用いて、センサ座標系Σｌから世界座標系Σｗ基準のデータＰｗｏｒｌｄへと変換する。 The conversion unit 502 is connected to the distance measurement sensor 3, and uses the movement amount of the movement amount measurement unit 501 to convert the point cloud data Praw obtained from the distance measurement sensor 3 from the sensor coordinate system Σl to the world coordinate system Σw. Convert to reference data World.

記憶装置５ｂは、変換部５０２が変換した３次元の点群データＰｗｏｒｌｄを、任意の数または予め定められた数だけ点群データＰｇｒｏｕｐとして保存する。 The storage device 5b stores the three-dimensional point cloud data World converted by the conversion unit 502 as an arbitrary number or a predetermined number of point cloud data Pgroups.

また記憶装置５ｂは、予め取得された、移動体１が走行する環境の地図データＭを保存している。この地図データＭは世界座標系Σｗで表される３次元データであり、建物の壁面や建造物を点群で示したデータである。地図データＭは移動体１を用いて予め取得されてもよいし、移動体１と同等の性能を有する測距センサ３を搭載した他の移動体を用いて予め取得されてもよい。なお「同等の性能」とは、例えば同じ製造元から入手可能な同じモデルの測距センサである。 Further, the storage device 5b stores the map data M of the environment in which the moving body 1 travels, which has been acquired in advance. This map data M is three-dimensional data represented by the world coordinate system Σw, and is data showing the wall surface of a building or a building as a point cloud. The map data M may be acquired in advance using the moving body 1 or may be acquired in advance using another moving body equipped with a distance measuring sensor 3 having the same performance as the moving body 1. Note that "equivalent performance" is, for example, a distance measuring sensor of the same model available from the same manufacturer.

位置推定部５０３は、記憶装置５ｂに保存された３次元データＰｇｒｏｕｐと、地図データＭとして保存された３次元点群データとの比較を行い、移動体１の世界座標系Σｗにおける位置を推定する。 The position estimation unit 503 compares the 3D data Pgroup stored in the storage device 5b with the 3D point cloud data stored as the map data M, and estimates the position of the moving body 1 in the world coordinate system Σw. ..

（測距センサ３の説明）
次に図４を用いて、測距センサ３から得られる点群データＰｒａｗを説明する。 (Explanation of ranging sensor 3)
Next, the point cloud data Praw obtained from the ranging sensor 3 will be described with reference to FIG.

図４は、ある空間に測距センサ３を置いたときの各反射点の位置を模式的に示している。なお、図４はセンサ座標系Σｌの＋Ｚｌ側から－Ｚｌ側を見た俯瞰図である。 FIG. 4 schematically shows the position of each reflection point when the distance measuring sensor 3 is placed in a certain space. Note that FIG. 4 is a bird's-eye view of the sensor coordinate system Σl as viewed from the + Zl side to the −Zl side.

本実施形態では、測距センサ３は一般的にはＬＩＤＡＲ（Light Detection and Ranging，Laser Imaging Detection and Ranging）と呼ばれるセンサである。測距センサ３は、光を用いて、つまり光学的手段により、測定可能な距離範囲内に存在する物体までの距離を計測する。より具体的には、測距センサ３は、周期的にたとえば約９００ｎｍの波長、あるいは１４００ｎｍ以上（例えば１５００ｎｍ）の波長のレーザ光を周囲に放射して周囲の環境をスキャンする。 In the present embodiment, the ranging sensor 3 is a sensor generally called LIDAR (Light Detection and Ranging, Laser Imaging Detection and Ranging). The distance measuring sensor 3 measures the distance to an object existing within the measurable distance range by using light, that is, by optical means. More specifically, the ranging sensor 3 periodically emits a laser beam having a wavelength of, for example, about 900 nm, or a wavelength of 1400 nm or more (for example, 1500 nm) to the surroundings to scan the surrounding environment.

測距センサ３が、いわゆるＴｏＦ（Time Of Flight）方式で測距する場合、測距センサ３は、測定可能な所定の角度範囲の空間にステップ角Δθ毎に方向を変化させながらパルス状に光線３Ａを放射して、物体表面で反射された光線３Ａの各反射光を検出する。これにより、ステップ角Δθ毎ごとの方向に存在する反射点までの距離Ｒのデータ（点群データＰｒａｗ）を得ることができる。例えば測距センサ３の測定可能な角度を３６０［ｄｅｇ］とし、測距センサ３が同一平面上に光線３Ａを出射すると仮定する。すると、１回のスキャンで３６０／Δθ個の点群データが得られる。Δθの一例は０．５度である。 When the distance measuring sensor 3 measures a distance by a so-called ToF (Time Of Flight) method, the distance measuring sensor 3 emits light rays in a pulse shape while changing the direction for each step angle Δθ in a space within a predetermined measurable angle range. It radiates 3A and detects each reflected light of the light ray 3A reflected on the surface of the object. As a result, it is possible to obtain data (point cloud data Praw) of the distance R to the reflection point existing in each direction of the step angle Δθ. For example, it is assumed that the measurable angle of the distance measuring sensor 3 is 360 [deg], and the distance measuring sensor 3 emits a light ray 3A on the same plane. Then, 360 / Δθ point cloud data can be obtained in one scan. An example of Δθ is 0.5 degrees.

測距センサ３が、ＦＭＣＷ（Frequency Modulated Continuous Wave）方式で測距する場合、測距センサ３はコヒーレントレーザ送信器（図示せず）を有している。コヒーレントレーザ送信器から出射される光は、反復的に周波数チャープされた信号である。具体的には、出射される光の周波数は、時間の経過に応じて直線的に上昇し、一定の時間後は再び最初の周波数から直線的に上昇するよう反復的に変調される。または、光の周波数は、時間の経過に応じて直線的に上昇し、一定の時間経過後には時間の経過に応じて直線的に下降する。 When the distance measuring sensor 3 measures the distance by the FMCW (Frequency Modulated Continuous Wave) method, the distance measuring sensor 3 has a coherent laser transmitter (not shown). The light emitted from the coherent laser transmitter is an iteratively frequency-chirped signal. Specifically, the frequency of the emitted light rises linearly with the passage of time, and after a certain period of time, it is repeatedly modulated so as to rise linearly from the first frequency again. Alternatively, the frequency of light rises linearly with the passage of time, and after a certain period of time, falls linearly with the passage of time.

視野内に存在する物体が測距センサ３に近付くよう、あるいは測距センサ３から遠ざかるように移動すると、物体からの反射光の周波数にドップラシフトが生じる。測距センサ３は、反射光を表す信号（反射信号）とコヒーレントレーザ送信器からの光を表す信号（出力信号）とを混合する。出力信号と反射信号とは干渉を生じるため、両者を混合した信号（ビート信号）にはうなりが含まれる。ビート信号の周波数は、反射までの時間、すなわち距離に対応する。ビート信号の周波数から、物体までの距離を計測できる。また、光の出射方向及び反射光の入射方向は、物体が存在する方向を表す。ＴｏＦ方式と同様、光の出射方向をステップ角Δθ毎ごとに変化させながら反射光を取得することにより、ステップ角Δθ毎ごとの方向に存在する反射点までの距離Ｒのデータ（点群データＰｒａｗ）を得ることができる。 When an object existing in the field of view moves toward the distance measuring sensor 3 or away from the distance measuring sensor 3, a Doppler shift occurs in the frequency of the reflected light from the object. The ranging sensor 3 mixes a signal representing reflected light (reflected signal) and a signal representing light from a coherent laser transmitter (output signal). Since the output signal and the reflected signal cause interference, the signal (beat signal) in which both are mixed includes a beat signal. The frequency of the beat signal corresponds to the time to reflection, that is, the distance. The distance to the object can be measured from the frequency of the beat signal. Further, the light emitting direction and the reflected light incident direction represent the direction in which the object exists. Similar to the ToF method, by acquiring the reflected light while changing the light emission direction for each step angle Δθ, the data of the distance R to the reflection point existing in the direction for each step angle Δθ (point group data Praw). ) Can be obtained.

点群データＰｒａｗを構成する１個の点のデータは、測距センサ３からある反射点までの距離Ｒ、及び、センサ座標系Σｌの所定の基準角度（０度）から計測された当該反射点の方向を表す角度θの各データを含む。すなわち、１個の点は極座標系の点（Ｒ，θ）として表現され得る。図４には、連続する３つの反射点Ｐ１，Ｐ２及びＰ３が例示されている。このうち、測距センサ３から距離Ｒ１、角度θ１の位置に存在する反射点Ｐ１の座標はＰ１（Ｒ１，θ１）と表現される。 The data of one point constituting the point cloud data Praw is the distance R from the distance measuring sensor 3 to a certain reflection point, and the reflection point measured from a predetermined reference angle (0 degree) of the sensor coordinate system Σl. Includes each data of the angle θ representing the direction of. That is, one point can be represented as a point (R, θ) in a polar coordinate system. FIG. 4 illustrates three consecutive reflection points P1, P2 and P3. Of these, the coordinates of the reflection point P1 existing at the position of the distance R1 and the angle θ1 from the distance measuring sensor 3 are expressed as P1 (R1, θ1).

極座標系で表現された点群データを直交座標系であるセンサ座標系Σｌで扱うためには、データの表現形式を極座標系から直交座標系に変換する必要がある。例えば、上述の極座標系で表現された点Ｐ１（Ｒ１，θ１）の場合、センサ座標系Σｌによる表現に変換すると（Ｒ１・ｃｏｓθ１，Ｒ１・ｓｉｎθ１，０）である。なお、本開示で使用される測距センサは２次元測距のため、Ｚｌ軸の値は常に０となる。 In order to handle the point cloud data expressed in the polar coordinate system in the sensor coordinate system Σl which is the orthogonal coordinate system, it is necessary to convert the data expression format from the polar coordinate system to the orthogonal coordinate system. For example, in the case of the point P1 (R1, θ1) expressed in the above-mentioned polar coordinate system, it is (R1 · cos θ1, R1 · sin θ1, 0) when converted into the representation by the sensor coordinate system Σl. Since the distance measuring sensor used in the present disclosure is two-dimensional distance measuring, the value of the Zl axis is always 0.

（３次元点群データ作成の詳細説明）
次に図５、図６を用いて、３次元点群データの作成方法を説明する。 (Detailed explanation of 3D point cloud data creation)
Next, a method of creating three-dimensional point cloud data will be described with reference to FIGS. 5 and 6.

図５は、移動体１を移動させながら測距センサ３に順次放射させた、複数の光線３Ａの各反射点の位置を示している。説明を簡素化するため、図５は移動体１から光学素子４のみを除いて記載している。上述のとおり、測距センサ３はＸｗ－Ｙｗ平面に対して所定の角度αだけ傾斜して取り付けられているため、図示されるように複数の光線３Ａは移動体１の進行方向に対して、Ｘｗ－Ｚｗ平面上で角度αだけ傾斜した方向（斜め上、斜め後ろ）に放射される。 FIG. 5 shows the positions of the reflection points of the plurality of light rays 3A that are sequentially radiated to the distance measuring sensor 3 while moving the moving body 1. In order to simplify the explanation, FIG. 5 shows the moving body 1 excluding only the optical element 4. As described above, since the distance measuring sensor 3 is attached at an angle α with respect to the Xw-Yw plane, a plurality of light rays 3A are attached to the traveling direction of the moving body 1 as shown in the figure. It is radiated in a direction (obliquely upward, diagonally backward) inclined by an angle α on the Xw-Zw plane.

移動体１は、地点Ａを初期位置（Ｘｗ１，Ｙｗ１，Ｚｗ１）として、Ｘｗ方向（Ｙｗ＝０、Ｚｗ＝０で固定）に移動する。ここで言う「初期位置」は、世界座標系Σｗで表現されている。移動体１は距離ΔＸ１だけ移動する毎に測距センサ３に光線３Ａを放射させて点群データＰｒａｗを取得する。なお、図５には、天井の複数の反射点に関する点群データＰｒａｗが記載されている。これらは一度のスキャンで得られたデータではなく、移動体１が距離ΔＸ１進むごとに測距センサ３によってスキャンが行われて取得されたデータである。 The moving body 1 moves in the Xw direction (fixed at Yw = 0, Zw = 0) with the point A as the initial position (Xw1, Yw1, Zw1). The "initial position" referred to here is expressed in the world coordinate system Σw. Every time the moving body 1 moves by the distance ΔX1, the distance measuring sensor 3 emits a light beam 3A to acquire the point cloud data Praw. In addition, in FIG. 5, the point cloud data Praw regarding a plurality of reflection points on the ceiling is shown. These are not the data obtained by one scan, but the data obtained by scanning by the distance measuring sensor 3 every time the moving body 1 advances the distance ΔX1.

測距センサ３から取得した点群データＰｒａｗを世界座標系Σｗ基準の点群データＰｗｏｒｌｄに変換するためには、点群データＰｒａｗを取得した時点の、世界座標系Σｗにおけるロボット座標系Σｒ（移動体１）の位置及び傾きを知る必要がある。なお上述のとおり、ロボット座標系Σｒとセンサ座標系Σｌの相対的な位置関係は固定である。 In order to convert the point cloud data Praw acquired from the distance measuring sensor 3 into the point cloud data World based on the world coordinate system Σw, the robot coordinate system Σr (movement) in the world coordinate system Σw at the time when the point cloud data Praw is acquired. It is necessary to know the position and inclination of the body 1). As described above, the relative positional relationship between the robot coordinate system Σr and the sensor coordinate system Σl is fixed.

移動体１の移動量は、移動量計測部５０１を用いて計測され得る。移動量計測部５０１は、例えば初期位置よりＸｗ方向にΔＸ１進んだことを計測することができる。すると、移動体１の位置は（Ｘｗ１＋ΔＸ１，Ｙｗ１，Ｚｗ１）と算出されるため、変換部５０２は、移動後の点群データＰｒａｗを点群データＰｗｏｒｌｄに変換することができる。 The movement amount of the moving body 1 can be measured by using the movement amount measuring unit 501. The movement amount measuring unit 501 can measure, for example, that it has advanced by ΔX1 in the Xw direction from the initial position. Then, since the position of the moving body 1 is calculated as (Xw1 + ΔX1, Yw1, Zw1), the conversion unit 502 can convert the point cloud data Prow after the movement into the point cloud data World.

例えば、移動体１の位置（Ｘｗ１＋ΔＸ１，Ｙｗ１，Ｚｗ１）における点群データＰｒａｗの一つとして、先に例示した点Ｐ１（Ｒ１，θ１）が得られたとする。点Ｐ１は、測距センサ３のスキャン面を規定するセンサ座標系Σｌ上の点である。図２を参照しながら説明したように、当該スキャン面（センサ座標系Σｌ）はロボット座標系Σｒの平面Ｘｒ－Ｙｒ（及び世界座標系Σｗの平面Ｘｗ－Ｙｗ）から角度αだけ傾斜している。すると、世界座標系Σｗの座標（Ｘｗ，Ｙｗ，Ｚｗ）は、以下のように表現される。
Ｘｗ＝（Ｒ１・ｃｏｓθ１・ｃｏｓα）＋（Ｘｗ１＋ΔＸ１）
Ｙｗ＝（Ｒ１・ｓｉｎθ１）＋Ｙｗ１
Ｚｗ＝（Ｒ１・ｃｏｓθ１・ｓｉｎα）＋（Ｚｒ＋Ｚｗ１）
なお、Ｚｗの右辺第２項に含まれる「Ｚｒ」は、ロボット座標系Σｒ（移動体１）から見た、センサ座標系Σｌの原点位置の高さ方向の座標値である。 For example, it is assumed that the point P1 (R1, θ1) exemplified above is obtained as one of the point cloud data Prow at the position of the moving body 1 (Xw1 + ΔX1, Yw1, Zw1). The point P1 is a point on the sensor coordinate system Σl that defines the scan surface of the distance measuring sensor 3. As described with reference to FIG. 2, the scan surface (sensor coordinate system Σl) is inclined by an angle α from the plane Xr-Yr (and the plane Xw-Yw of the world coordinate system Σw) of the robot coordinate system Σr. .. Then, the coordinates (Xw, Yw, Zw) of the world coordinate system Σw are expressed as follows.
Xw = (R1, cosθ1, cosα) + (Xw1 + ΔX1)
Yw = (R1 · sinθ1) + Yw1
Zw = (R1, cosθ1, sinα) + (Zr + Zw1)
Note that "Zr" included in the second term on the right side of Zw is a coordinate value in the height direction of the origin position of the sensor coordinate system Σl as seen from the robot coordinate system Σr (moving body 1).

図６は、制御ユニット５の演算回路５ａによる世界座標系の点群データを取得する手順を示すフローチャートである。説明の便宜のため、図３に示す演算回路５ａ内の移動量計測部５０１、変換部５０２及び位置推定部５０３が動作主体であるとして説明する。 FIG. 6 is a flowchart showing a procedure for acquiring point cloud data in the world coordinate system by the arithmetic circuit 5a of the control unit 5. For convenience of explanation, it is assumed that the movement amount measuring unit 501, the conversion unit 502, and the position estimation unit 503 in the arithmetic circuit 5a shown in FIG. 3 are the main operating units.

移動体１の移動が開始すると、ステップＳ１０１において、移動量計測部５０１は移動量の計測を開始する。 When the movement of the moving body 1 starts, in step S101, the moving amount measuring unit 501 starts measuring the moving amount.

ステップＳ１０２において、変換部５０２は、その移動量がΔＸ１以上か未満かを判断する。移動量がΔＸ１未満の場合は、処理はステップＳ１０１へ戻り、移動量計測部５０１は引き続き移動量の計測を継続する。移動量がΔＸ１以上の場合は、処理はステップＳ１０３に移行し、変換部５０２は測距センサ３からの点群データＰｒａｗを受信する。 In step S102, the conversion unit 502 determines whether the movement amount is equal to or less than ΔX1. If the movement amount is less than ΔX1, the process returns to step S101, and the movement amount measurement unit 501 continues to measure the movement amount. When the movement amount is ΔX1 or more, the process proceeds to step S103, and the conversion unit 502 receives the point cloud data Praw from the distance measuring sensor 3.

ステップＳ１０４において、変換部５０２は、受信した移動体１の移動量と、移動前の位置とを用いて、移動体１の移動後の位置（世界座標系Σｗ基準）を算出する。移動前の位置の算出は、初期位置、または、位置推定部５０３により算出された値を利用する。 In step S104, the conversion unit 502 calculates the position after the movement of the moving body 1 (based on the world coordinate system Σw) by using the received movement amount of the moving body 1 and the position before the movement. For the calculation of the position before the movement, the initial position or the value calculated by the position estimation unit 503 is used.

その後、ステップＳ１０５にて、変換部５０２は、測距センサ３のデータＰｒａｗを世界座標系Σｗ基準の点群データＰｗｏｒｌｄへと変換する。変換後のデータは３ＤデータＰｇｒｏｕｐとして記憶装置５ｂに記憶される。 After that, in step S105, the conversion unit 502 converts the data Praw of the ranging sensor 3 into the point cloud data Field based on the world coordinate system Σw. The converted data is stored in the storage device 5b as 3D data Pgroup.

ここで変換部５０２は、現在の３ＤデータＰｇｒｏｕｐ内のデータ数が上限Ｎ未満か否かを判定する。データ数がＮ未満の場合は、ステップＳ１０７に移行し、データＰｇｒｏｕｐを保存して処理を終了する。データ数が上限Ｎである場合は、処理はステップＳ１０８に移行する。ステップＳ１０８において、変換部５０２は一番古いデータを一つ消去する。その後処理はステップＳ１０７に進む。 Here, the conversion unit 502 determines whether or not the number of data in the current 3D data Pgroup is less than the upper limit N. If the number of data is less than N, the process proceeds to step S107, the data Pgroup is saved, and the process ends. If the number of data is the upper limit N, the process proceeds to step S108. In step S108, the conversion unit 502 erases one of the oldest data. After that, the process proceeds to step S107.

なお、上述の例では、距離ΔＸ１を基準として点群データＰｒａｗを取得したが、点群データを取得する基準として距離ΔＸ１に対応する時間ΔＴを採用することもできる。このとき、距離ΔＸ１と時間ΔＴとの関係は、移動体１の移動速度を一定値ＶとしてΔＴ＝ΔＸ１／Ｖと表すことができる。 In the above example, the point cloud data Praw is acquired with the distance ΔX1 as a reference, but the time ΔT corresponding to the distance ΔX1 can also be adopted as the reference for acquiring the point cloud data. At this time, the relationship between the distance ΔX1 and the time ΔT can be expressed as ΔT = ΔX1 / V with the moving speed of the moving body 1 as a constant value V.

（３次元点群データを用いた位置推定の説明）
図７、図８を参照しながら、記憶装置５ｂに３次元データＰｇｒｏｕｐとして保存された点群データと、地図データＭとを用いて、移動体の位置推定を行う方法を説明する。 (Explanation of position estimation using 3D point cloud data)
With reference to FIGS. 7 and 8, a method of estimating the position of the moving body will be described using the point cloud data stored as the three-dimensional data Pgroup in the storage device 5b and the map data M.

図７は、移動体１の位置推定前の状態を示している。図７では、地図データＭの一部のデータ（部分地図データ）Ｍと、３Ｄデータとして保存されている点群データＰｇｒｏｕｐとが可視化されている。 FIG. 7 shows the state of the moving body 1 before the position estimation. In FIG. 7, a part of the map data M (partial map data) M and the point cloud data Pgroup stored as 3D data are visualized.

移動体１は、位置推定前には地点Ｂ（Ｘｗ２、Ｙｗ２、Ｚｗ２）に存在すると認識している。しかしながら、図７で示されるように点群データＰｇｒｏｕｐと部分地図データＭとの間には、Ｘｗ方向にずれＤｉｆｆが生じている。点群データＰｇｒｏｕｐは移動体１の測定結果である。位置推定部５０５は、この測定結果と部分地図データＭとを比較・照合して、測定結果と部分地図データＭとのずれＤｉｆｆが最も小さくなったときの部分地図データＭ上の位置を、移動体１の現在の位置として推定する。例えは、位置推定部５０５は、点群データＰｇｒｏｕｐと部分地図データＭの一部との二乗平均誤差が最も小さくなる部分地図データＭ上の位置を自己位置として推定する。 The mobile body 1 recognizes that it exists at the point B (Xw2, Yw2, Zw2) before the position estimation. However, as shown in FIG. 7, a shift Diff occurs in the Xw direction between the point cloud data Pgroup and the partial map data M. The point cloud data Pgroup is the measurement result of the mobile body 1. The position estimation unit 505 compares and collates the measurement result with the partial map data M, and moves the position on the partial map data M when the deviation Diff between the measurement result and the partial map data M is the smallest. Estimated as the current position of body 1. For example, the position estimation unit 505 estimates the position on the partial map data M where the root mean square error between the point cloud data Pgroup and a part of the partial map data M is the smallest as the self-position.

以下、図８を参照しながらより詳細に説明する。図８は、位置推定部５０３の処理の手順を示すフローチャートである。 Hereinafter, a more detailed description will be given with reference to FIG. FIG. 8 is a flowchart showing the processing procedure of the position estimation unit 503.

ステップＳ１１０において、位置推定部５０３は、記憶装置５ｂから３次元データである点群データＰｇｒｏｕｐを読み出して取得する。続くステップＳ１１１において、位置推定部５０３は、記憶装置５ｂから予め用意された地図データＭを読み出して取得する。
ステップＳ１１２において、位置推定部５０３は、点群データＰｇｒｏｕｐと地図データＭとの比較・照合を行う。点群データＰｇｒｏｕｐと、地図データＭとの比較は、ＩＣＰ（ＩｔｅｒａｔｉｖｅＣｌｏｓｅｓｔＰｏｉｎｔ）マッチングや、ＮＤＴ（ＮｏｒｍａｌＤｉｓｔｒｉｂｕｔｉｏｎＴｒａｎｓｆｏｒｍ）スキャンマッチングなどの周知の手法を採用し得る。 In step S110, the position estimation unit 503 reads and acquires the point cloud data Pgroup, which is three-dimensional data, from the storage device 5b. In the following step S111, the position estimation unit 503 reads and acquires the map data M prepared in advance from the storage device 5b.
In step S112, the position estimation unit 503 compares / collates the point cloud data Pgroup with the map data M. For the comparison between the point cloud data Pgroup and the map data M, a well-known method such as ICP (Iterative Closest Point) matching or NDT (Normal Distribution Transfer) scan matching can be adopted.

ステップＳ１１３において、位置推定部５０３は、点群データＰｇｒｏｕｐが最もよく整合する地図データＭ上の位置を、現在位置を示す位置情報として出力する。これにより、自己位置が推定され、処理が終了する。なお、位置情報は、移動体１の姿勢（orientation）を示す情報も含み得る。移動体の位置および姿勢は、ポーズ（pose）と呼ばれることがある。 In step S113, the position estimation unit 503 outputs the position on the map data M where the point cloud data Pgroup best matches, as position information indicating the current position. As a result, the self-position is estimated and the process ends. The position information may also include information indicating the orientation of the moving body 1. The position and posture of the moving body is sometimes referred to as a pose.

（光学素子４による光線の進行方向の変更の説明）
次に、光学素子４を詳細に説明する。以下の説明では、光学素子４はミラーであるとする。 (Explanation of change in the traveling direction of light rays by the optical element 4)
Next, the optical element 4 will be described in detail. In the following description, it is assumed that the optical element 4 is a mirror.

図９Ａ及び図９Ｂは、それぞれ、移動体１に搭乗者Ｈが乗車したときの側面図及び上面図である。 9A and 9B are side views and top views when the passenger H gets on the moving body 1, respectively.

光学素子４は、測距センサ３の所定の角度範囲内に設けられている。「所定の角度範囲内」とは、移動体１が運搬する人または荷物が存在する範囲内である。より正確には、「所定の角度範囲内」は、測距センサ３のスキャン面上であって、かつ、スキャン面と交差する人体または荷物が存在する範囲を含む角度範囲内である。人体または荷物が存在する「範囲」は、移動体１が運搬することを設計上想定された人または荷物のうち、スキャン面内に存在すると推定される最大範囲であり得る。 The optical element 4 is provided within a predetermined angle range of the distance measuring sensor 3. "Within a predetermined angle range" is a range in which a person or luggage carried by the moving body 1 exists. More precisely, "within a predetermined angle range" is an angle range on the scan surface of the ranging sensor 3 and including a range in which a human body or a load intersecting the scan surface exists. The "range" in which the human body or luggage exists may be the maximum range of the person or luggage that is designed to be carried by the moving body 1 and is estimated to be present in the scanning surface.

図９Ａを参照する。光線３Ａは、Ｘｗ－Ｚｗ平面での測距センサ３から照射され、光学素子４により反射されてその進行方向を変えられる。その結果、図の実線のように搭乗者Ｈに干渉しない軌道をとる。 See FIG. 9A. The light ray 3A is emitted from the distance measuring sensor 3 in the Xw-Zw plane, reflected by the optical element 4, and its traveling direction can be changed. As a result, it takes a trajectory that does not interfere with passenger H as shown by the solid line in the figure.

また図９Ｂを参照すると、測距センサ３の一部範囲θｒｅｆ内の光線が反射される。すなわち、測距センサ３から照射される光線の一部の進行方向が光学素子４によって変更される。 Further, referring to FIG. 9B, the light rays in the partial range θref of the distance measuring sensor 3 are reflected. That is, the traveling direction of a part of the light rays emitted from the distance measuring sensor 3 is changed by the optical element 4.

光学素子４が無い場合は、光線は点線３Ｂのような軌道をとるため、搭乗者Ｈと光線が干渉する。その結果、地図データＭと比較するための点群データＰｇｒｏｕｐの量が少なくなり、有効な計測データが減ってしまう。 In the absence of the optical element 4, the light rays take an orbit like the dotted line 3B, so that the light rays interfere with the passenger H. As a result, the amount of point cloud data Pgroup for comparison with the map data M is reduced, and the effective measurement data is reduced.

光学素子４を設けることによって、移動体１に搭乗者Ｈが搭乗していても、点群データのデータ量が減少しない。つまり点群データの精度を維持できるため、位置推定部５０３による位置推定精度を損なうことがない。これにより、前述した問題が起こることを抑制することができる。 By providing the optical element 4, even if the passenger H is on board the moving body 1, the amount of point cloud data does not decrease. That is, since the accuracy of the point cloud data can be maintained, the position estimation accuracy by the position estimation unit 503 is not impaired. This makes it possible to prevent the above-mentioned problems from occurring.

なお、本実施形態では、点群データＰｇｒｏｕｐは世界座標系Σｗで表現された３次元データであるとした。本来であれば、測距センサ３を用いて予め取得された点群データを地図データとしてそのまま用いる。しかしながら本実施形態では、光学素子４を用いて測距センサ３から出射された光線の進行方向を変更したため、得られた点群データを世界座標系Σｗに統一して処理を行うこととした。ただし、光学素子４によって光線の一部の進行方向が変更されたとしても、任意の位置で取得される点群データには、その位置に固有の反射点の位置情報が含まれていると言える。従って、地図データＭが、測距センサによって予め取得された点群データと世界座標系Σｗ上の位置とを適切に対応付けている場合には、移動体１の自己位置推定処理時に、位置推定部５０３は、測距センサ３によって得られた点群データを世界座標系Σｗのデータに変換することなく、地図データと比較・照合してもよい。 In the present embodiment, the point cloud data Pgroup is assumed to be three-dimensional data represented by the world coordinate system Σw. Originally, the point cloud data acquired in advance using the distance measuring sensor 3 is used as it is as the map data. However, in the present embodiment, since the traveling direction of the light beam emitted from the distance measuring sensor 3 is changed by using the optical element 4, the obtained point cloud data is unified into the world coordinate system Σw and processed. However, even if the traveling direction of a part of the light beam is changed by the optical element 4, it can be said that the point cloud data acquired at an arbitrary position includes the position information of the reflection point peculiar to that position. .. Therefore, when the map data M appropriately associates the point cloud data acquired in advance by the distance measuring sensor with the position on the world coordinate system Σw, the position is estimated at the time of the self-position estimation process of the moving body 1. The unit 503 may compare and collate the point cloud data obtained by the distance measuring sensor 3 with the map data without converting it into the data of the world coordinate system Σw.

測距センサ３の一部範囲θｒｅｆの範囲は、本発明の実施例で示したように固定的な値ではなく、搭乗者や、光学素子４の位置に応じて、変更してもよい。 The range of the partial range θref of the distance measuring sensor 3 is not a fixed value as shown in the embodiment of the present invention, but may be changed according to the passenger and the position of the optical element 4.

上記構成によれば、電動車いすで搭乗者のような固定的な障害物や、測距センサの取付けの制約により、測距センサの照射範囲に制約がかかる場合でも、地図データと比較するための点群データの量を減らさずに取得することが可能となる。 According to the above configuration, even if the irradiation range of the distance measuring sensor is restricted due to a fixed obstacle such as a passenger in an electric wheelchair or the restriction of mounting the distance measuring sensor, the data can be compared with the map data. It is possible to acquire point cloud data without reducing the amount.

以下、これまで説明した実施形態の変形例を説明する。 Hereinafter, variations of the embodiments described so far will be described.

上述の実施形態の説明では、光学素子４は１つのみ設けたが、取り付ける位置、光学素子４の大きさ等の物理的な制約、搭乗者の体格、搭載する荷物の大きさ等に応じて、複数の光学素子を設けてもよい。このようにすることでより幅広い制約に対応することが可能となる。 In the above description of the embodiment, only one optical element 4 is provided, but it depends on the physical restrictions such as the mounting position, the size of the optical element 4, the physique of the passenger, the size of the load to be loaded, and the like. , A plurality of optical elements may be provided. By doing so, it becomes possible to deal with a wider range of restrictions.

また、光学素子４によって光線の進行方向を調整してもよい。 Further, the traveling direction of the light beam may be adjusted by the optical element 4.

図１０は、移動体１がある空間内を移動している様子を示す斜視図である。斜線で示されているエリアは、点群データＰｇｒｏｕｐが存在する範囲を模式的に示している。この範囲は、上述の実施形態のように光学素子４が一つ存在する場合は、図のように２つのエリアＰｇｒｏｕｐ＿Ａ及びＰｇｒｏｕｐ＿Ｂに分けることができる。エリアＰｇｒｏｕｐ＿Ａは、光学素子４によって進行方向が変更された光線が入射する範囲であり、エリアＰｇｒｏｕｐ＿Ｂは、光学素子４を介しない光線が入射する範囲である。光学素子４は、測距センサ３の取付角度α方向に対して、法線Ｎが角度β、距離Ｄとなるように取り付けられている（図９参照）。そうすると反射後の光線３ＡはＸｗ－Ｙｗ面に対して、＋Ｘｗ方向を０度とし、＋Ｙｗ方向から－Ｙｗ方向を見たとき、角度がα―２β＋１８０［ｄｅｇ］をなす方向へと反射する。例えば真上に反射させたい場合は、α―２β＋１８０＝９０［ｄｅｇ］を満たすように、角度αやβを設定すればよい。さらにそこからＸｗ－Ｙｗ面に対して、光線３Ａを平行移動させたい場合は、距離Ｄを適切な値に変更すればよい。本実施形態では真上に反射させているため、Ｐｇｒｏｕｐ＿ＡやＰｇｒｏｕｐ＿Ｂは、図１０で示されるように、重ならないような点群データとなる。 FIG. 10 is a perspective view showing how the moving body 1 is moving in a certain space. The area shown by the diagonal line schematically shows the range in which the point cloud data Pgroup exists. This range can be divided into two areas Pgroup_A and Pgroup_B as shown in the figure when one optical element 4 is present as in the above-described embodiment. The area Pgroup_A is a range in which a light ray whose traveling direction is changed by the optical element 4 is incident, and the area Pgroup_B is a range in which a light ray that does not pass through the optical element 4 is incident. The optical element 4 is mounted so that the normal line N is at an angle β and a distance D with respect to the mounting angle α direction of the distance measuring sensor 3 (see FIG. 9). Then, the reflected light ray 3A is reflected in the direction in which the + Xw direction is 0 degrees with respect to the Xw-Yw plane and the angle is α-2β + 180 [deg] when viewed from the + Yw direction to the −Yw direction. For example, when it is desired to reflect directly above, the angles α and β may be set so as to satisfy α-2β + 180 = 90 [deg]. Further, when it is desired to translate the light ray 3A with respect to the Xw-Yw plane from there, the distance D may be changed to an appropriate value. In the present embodiment, since the reflection is performed directly above, Pgroup_A and Pgroup_B are point cloud data that do not overlap as shown in FIG.

このようにエリアＰｇｒｏｕｐ＿ＡとエリアＰｇｒｏｕｐ＿Ｂの範囲が互いに重ならないようにすることは自己位置推定処理において好適である。なぜなら、位置推定部５０３により移動体１の自己位置を推定する際に、点群データＰｇｒｏｕｐは極力建物の壁面に存在する特徴を広範囲に渡り、取得するほうが好ましいからである。 It is preferable in the self-position estimation process that the ranges of the area Pgroup_A and the area Pgroup_B do not overlap each other in this way. This is because when the position estimation unit 503 estimates the self-position of the moving body 1, it is preferable that the point cloud data Pgroup covers and acquires the features existing on the wall surface of the building as much as possible.

図１１及び図１２を参照しながら、より具体的説明する。 A more specific description will be given with reference to FIGS. 11 and 12.

図１１は、複数の凸部９０１、９０２及び９０３が存在する空間を模式的に示している。複数の凸部９０１、９０２及び９０３はいずれも、空間の内側に向かっているとする。 FIG. 11 schematically shows a space in which a plurality of convex portions 901, 902 and 903 exist. It is assumed that the plurality of protrusions 901, 902 and 903 are all directed toward the inside of the space.

いま、移動体１がこのような空間内を走行することを想定する。自己位置推定を行う際に、点群データＰｇｒｏｕｐの範囲が、このような凸部を含むことが望ましい。凸部に応じた点群は車外環境の特徴となり得るからである。点群データＰｇｒｏｕｐが凸部に応じた点群を含むことで、計測時の位置に応じた車外環境の特徴を認識しやすく、位置推定部５０３による位置推定精度を向上することができる。 Now, it is assumed that the moving body 1 travels in such a space. When performing self-position estimation, it is desirable that the range of the point cloud data Pgroup includes such a convex portion. This is because the point cloud corresponding to the convex portion can be a feature of the environment outside the vehicle. By including the point cloud corresponding to the convex portion in the point cloud data Pgroup, it is easy to recognize the characteristics of the external environment according to the position at the time of measurement, and the position estimation accuracy by the position estimation unit 503 can be improved.

図１２は、点群データＰｇｒｏｕｐ＿Ａ及びＰｇｒｏｕｐ＿Ｂが異なる凸部９０１及び９０２をカバーする様子を示している。図１１で示されるように、点群データＰｇｒｏｕｐ＿Ａ、Ｐｇｒｏｕｐ＿Ｂが凸部９０１にて重なっているよりも、図１２のように凸部９０１、凸部９０２の両方をカバーするように配置されることが望ましい。なお、路面が必ずしも平坦であるとは限られず、加えて移動体１が移動することにより、ピッチ方向（Ｙ軸周り）の揺れ、ロール方向（Ｘ軸周り）の揺れ、および／またはヨー方向（Ｚ軸周り）の揺れがランダムに発生し得る。そのような場合に、点群データＰｇｒｏｕｐ＿Ａ及びＰｇｒｏｕｐ＿Ｂが凸部９０１で重なる場合があり得る。 FIG. 12 shows how the point cloud data Pgroup_A and Pgroup_B cover different convex portions 901 and 902. As shown in FIG. 11, the point cloud data Pgroup_A and Pgroup_B may be arranged so as to cover both the convex portion 901 and the convex portion 902 as shown in FIG. 12 rather than overlapping at the convex portion 901. desirable. It should be noted that the road surface is not always flat, and in addition, due to the movement of the moving body 1, the sway in the pitch direction (around the Y axis), the sway in the roll direction (around the X axis), and / or the yaw direction ( Shaking around the Z axis) can occur randomly. In such a case, the point cloud data Pgroup_A and Pgroup_B may overlap at the convex portion 901.

また図１３のように、一部の光線の向きを変更することにより、凸部９０３をカバーする点群データＰｇｒｏｕｐ＿Ｃが得られるような光学素子４Ａを新たに設けてもよい。 Further, as shown in FIG. 13, an optical element 4A may be newly provided so that the point cloud data Pgroup_C covering the convex portion 903 can be obtained by changing the direction of a part of the light rays.

さらに、光学素子４による反射角度を調整して位置推定精度を向上させることも可能である。具体的には、光学素子４を用いて測距センサ３から出射される光線の進行方向を変更する際に、変更後のステップ角が、測距センサ３から出射される光線のステップ角よりも小さくする。 Further, it is also possible to improve the position estimation accuracy by adjusting the reflection angle by the optical element 4. Specifically, when the traveling direction of the light ray emitted from the distance measuring sensor 3 is changed by using the optical element 4, the changed step angle is larger than the step angle of the light ray emitted from the distance measuring sensor 3. Make it smaller.

図１４は、測距センサ３の光線が、光学素子４Ｂによって反射され、ステップ角がより小さくなっていることを模式的に示している。光学素子４へ入射する前のステップ角、つまり測距センサ３のステップ角をΔθｂｅｆｏｒｅとし、光学素子４から出射した後のステップ角をΔθａｆｔｅｒとすると、Δθｂｅｆｏｒｅ＞Δθａｆｔｅｒの関係性を満たす光学素子４Ｂを採用する。このことで、光学素子４Ｂによる偏光後の光線のステップ角を、偏光前の光線のステップ角より小さくすることができる。 FIG. 14 schematically shows that the light beam of the distance measuring sensor 3 is reflected by the optical element 4B and the step angle becomes smaller. Assuming that the step angle before incident on the optical element 4, that is, the step angle of the distance measuring sensor 3 is Δθbefore, and the step angle after exiting from the optical element 4 is Δθafter, the optical element 4B satisfying the relationship of Δθbefore> Δθafter. adopt. As a result, the step angle of the light beam after polarization by the optical element 4B can be made smaller than the step angle of the light ray before polarization.

これにより、小さな突起物などの特徴も捉えることができる。すなわち測距センサ３の角度分解能を向上させることができる。その結果、位置推定部５０３による位置推定精度を向上させることができる。なお、上述のように光学素子４によってステップ角をより小さくする場合には、地図データＭの作成時にも同じ光学素子４を用いて点群データを取得する。 This makes it possible to capture features such as small protrusions. That is, the angular resolution of the ranging sensor 3 can be improved. As a result, the position estimation accuracy by the position estimation unit 503 can be improved. When the step angle is made smaller by the optical element 4 as described above, the point cloud data is acquired by using the same optical element 4 when creating the map data M.

さらに他の変形例として、移動体１は、人または荷物が存在するか否かを判定してもよい。例えば、人が乗車し、または荷物が積載される位置に、近接センサあるいは重量センサを設け、センサの出力から、人または荷物が存在するか否かを判定できる。人または荷物が存在する場合には上述した光学素子４を測距センサ３から出射された光の光路上に配置させ、存在しない場合には光学素子４を測距センサ３のスキャン面から取り除く。前者の場合、制御ユニット５は上述した本実施形態にかかる手順に従って点群データを取得する。一方の後者の場合、測距センサ３の通常の処理、すなわち測距センサ３による光の出射及び反射光の検出に基づいて反射点が存在する角度（方向）及び反射点までの距離の算出処理、を行えばよい。前者の場合と後者の場合とでは、自己位置推定のための照合に利用する地図データＭは異なっていてもよい。例えば前者の処理で採用される地図データＭは３次元データＰｇｒｏｕｐであり、後者の処理で採用される地図データＭは測距センサ３のスキャン面で得られた２次元データであり得る。 As yet another modification, the moving body 1 may determine whether or not a person or luggage is present. For example, a proximity sensor or a weight sensor can be provided at a position where a person gets on or a load is loaded, and it can be determined from the output of the sensor whether or not the person or the baggage is present. When a person or luggage is present, the above-mentioned optical element 4 is arranged on the optical path of the light emitted from the ranging sensor 3, and when not present, the optical element 4 is removed from the scanning surface of the ranging sensor 3. In the former case, the control unit 5 acquires the point cloud data according to the procedure according to the present embodiment described above. On the other hand, in the latter case, the normal processing of the distance measuring sensor 3, that is, the calculation processing of the angle (direction) where the reflection point exists and the distance to the reflection point based on the light emission and the detection of the reflected light by the distance measurement sensor 3. , Should be done. The map data M used for collation for self-position estimation may be different between the former case and the latter case. For example, the map data M adopted in the former process may be three-dimensional data Pgroup, and the map data M adopted in the latter process may be two-dimensional data obtained on the scan surface of the distance measuring sensor 3.

移動体１はバッテリ（図示せず）を搭載し、バッテリから制御ユニット５に電力を供給する。制御ユニット５の処理負荷、つまり消費電力、は、光学素子４を光路上に挿入するときよりも挿入しないときの方が小さい。そのため、人または荷物が存在しないときには消費電力が小さい処理を実行することにより、バッテリの消耗を抑えることができる。 The mobile body 1 is equipped with a battery (not shown), and power is supplied from the battery to the control unit 5. The processing load of the control unit 5, that is, the power consumption, is smaller when the optical element 4 is not inserted than when it is inserted on the optical path. Therefore, when there is no person or luggage, the battery consumption can be suppressed by executing the process with low power consumption.

本開示にかかる例示的な移動体は、工場、倉庫、建設現場、物流、病院などで、人、荷物の移動時および／または搬送時に、自己位置を推定しながら自律的に移動するロボット、車両等として好適に利用され得る。 Exemplary moving objects according to the present disclosure are robots and vehicles that autonomously move while estimating their own positions when moving and / or transporting people and luggage in factories, warehouses, construction sites, physical distribution, hospitals, and the like. It can be suitably used as such.

１移動体
２車輪
３測距センサ
４光学素子
５制御ユニット
５ａ演算回路
５ｂ記憶装置
５０１移動量計測部
５０２変換部
５０３位置推定部 1 Moving body 2 Wheels 3 Distance measuring sensor 4 Optical element 5 Control unit 5a Calculation circuit 5b Storage device 501 Movement amount measurement unit 502 Conversion unit 503 Position estimation unit

Claims

３次元空間内の第１平面上を移動する移動体であって、
前記第１平面と異なる第２平面をスキャン面として有し、前記スキャン面上で角度を変えながら次々と光を出射して反射光を取得し、前記角度ごとに反射点までの距離を測定する計測装置と、
前記移動体の移動量に応じた物理量を計測する内界センサの出力から、前記移動体の移動量を計測する移動量計測部と、
前記第２平面上の角度及び距離による２次元座標系で表現された前記計測装置からの各出力値を、所定の変換式を用いて、前記３次元空間の座標値である３次元座標値に変換する変換部と、
変換後の各３次元座標値と前記３次元空間の座標値を用いて予め作成された地図データとを照合して、照合結果に基づき前記第１平面上の前記移動体の位置および姿勢を示す位置情報を順次出力する位置推定部と、
前記移動体を移動させるための駆動力を発生させる駆動装置と
を備え、さらに、
前記計測装置から角度を変えながら出射される前記第２平面上の光のうち、前記計測装置から所定の角度範囲θｒｅｆ内に出射される一部の光の進む方向を変化させる光学素子を備える、移動体。 A moving body that moves on a first plane in a three-dimensional space.
It has a second plane different from the first plane as a scan surface, emits light one after another while changing the angle on the scan surface to acquire reflected light, and measures the distance to the reflection point for each angle. Measuring device and
From the output of the internal sensor that measures the physical quantity according to the movement amount of the moving body, the movement amount measuring unit that measures the movement amount of the moving body, and
Each output value from the measuring device expressed in a two-dimensional coordinate system based on an angle and a distance on the second plane is converted into a three-dimensional coordinate value which is a coordinate value in the three-dimensional space by using a predetermined conversion formula. The conversion unit to convert and
Each of the converted three-dimensional coordinate values is collated with the map data created in advance using the coordinate values of the three-dimensional space, and the position and orientation of the moving body on the first plane are shown based on the collation result. A position estimation unit that sequentially outputs position information, and a position estimation unit
It is provided with a driving device for generating a driving force for moving the moving body, and further.
It is provided with an optical element that changes the traveling direction of a part of the light emitted from the measuring device on the second plane while changing the angle, which is emitted from the measuring device within a predetermined angle range θref. Mobile body.

前記移動体は、人または荷物を運搬可能であり、
前記所定の角度範囲θｒｅｆは、前記第２平面上で、前記第２平面と交差する前記人の体または前記荷物が存在する範囲を含む、請求項１に記載の移動体。 The mobile is capable of carrying people or luggage and is capable of carrying people or luggage.
The moving body according to claim 1, wherein the predetermined angle range θref includes a range in which the person's body or the luggage intersecting the second plane on the second plane.

前記所定の角度範囲θｒｅｆは、前記第２平面上で、前記第２平面と交差する前記人の体または前記荷物が存在すると推定される最大範囲を含む、請求項２に記載の移動体。 The moving body according to claim 2, wherein the predetermined angle range θref includes a maximum range on the second plane in which the person's body or the baggage that intersects the second plane is presumed to be present.

前記一部の光は複数の光線を含み、
前記光学素子は、各光線が進む方向を前記第２平面と非平行な方向に変化させる、請求項１から３のいずれかに記載の移動体。 The part of the light contains a plurality of light rays and contains a plurality of rays.
The moving body according to any one of claims 1 to 3, wherein the optical element changes the direction in which each light ray travels in a direction non-parallel to the second plane.

前記第２平面は前記第１平面に対して角度α［ｄｅｇ］だけ傾いており、
前記光学素子は前記光を反射させる反射面を有し、
前記反射面の法線と前記第２平面とがなす角度はβ［ｄｅｇ］（０＜β＜９０）であり、
前記光学素子は、前記各光線が進む方向を、α―２β＋１８０［ｄｅｇ］の方向に変化させる、請求項４に記載の移動体。 The second plane is tilted by an angle α [deg] with respect to the first plane.
The optical element has a reflecting surface that reflects the light.
The angle formed by the normal of the reflecting surface and the second plane is β [deg] (0 <β <90).
The moving body according to claim 4, wherein the optical element changes the direction in which each of the light rays travels in the direction of α-2β + 180 [deg].

前記計測装置が出射する光は、第１光線と、前記第１光線の直後に出射される第２光線とを含み、
前記光学素子によって進む方向が変更される前の、前記第１光線と前記第２光線とがなす角度をθ１［ｄｅｇ］とし、
前記光学素子によって進む方向が変更された後の、前記第１光線と前記第２光線とがなす角度をθ２［ｄｅｇ］とすると、
θ１＞θ２が成り立つ、請求項１から５のいずれかに記載の移動体。 The light emitted by the measuring device includes a first ray and a second ray emitted immediately after the first ray.
The angle formed by the first ray and the second ray before the traveling direction is changed by the optical element is set to θ1 [deg].
Assuming that the angle formed by the first light ray and the second light ray after the traveling direction is changed by the optical element is θ2 [deg], it is assumed.
The moving body according to any one of claims 1 to 5, wherein θ1> θ2 holds.

前記計測装置から出射される前記一部の光は第１光線群を含み、
前記計測装置から出射される、前記一部の光以外の光は第２光線群を含み、
前記第１光線群及び前記第２光線群は、前記３次元空間の互いに異なる第１領域及び第２領域に入射し、反射される、請求項１から６のいずれかに記載の移動体。 The part of the light emitted from the measuring device includes the first ray group.
Light other than the part of the light emitted from the measuring device includes a second ray group.
The moving body according to any one of claims 1 to 6, wherein the first ray group and the second ray group are incident on and reflected in a first region and a second region different from each other in the three-dimensional space.

前記光学素子は、ミラー、反射型偏光子またはプリズムである、請求項１から７のいずれかに記載の移動体。 The moving body according to any one of claims 1 to 7, wherein the optical element is a mirror, a reflective polarizing element, or a prism.

１または複数のコンピュータプログラムを格納した記憶装置と、
前記１または複数のコンピュータプログラムを実行する１または複数の演算回路と
をさらに備え、
前記１または複数の演算回路は、前記１または複数のコンピュータプログラムに記述された命令群にしたがって、前記移動量計測部、前記変換部、及び、前記位置推定部として動作する、請求項１から８のいずれかに記載の移動体。 A storage device that stores one or more computer programs,
Further comprising one or more arithmetic circuits for executing the one or more computer programs.
The one or a plurality of arithmetic circuits operate as the movement amount measuring unit, the conversion unit, and the position estimation unit according to the instruction group described in the one or the plurality of computer programs, according to claims 1 to 8. A moving object described in any of.

前記計測装置は、前記光の出射時刻と前記反射光を取得した時刻とを利用して前記距離を測定するレーザセンサである、請求項１から９のいずれかに記載の移動体。 The moving body according to any one of claims 1 to 9, wherein the measuring device is a laser sensor that measures the distance by using the time when the light is emitted and the time when the reflected light is acquired.