JP7809278B2

JP7809278B2 - Arithmetic device, arithmetic method, and program

Info

Publication number: JP7809278B2
Application number: JP2022047072A
Authority: JP
Inventors: 裕介木内; 克将北島; 孝司坪内
Original assignee: Mitsubishi Heavy Industries Ltd; University of Tsukuba NUC
Current assignee: Mitsubishi Heavy Industries Ltd; University of Tsukuba NUC
Priority date: 2022-03-23
Filing date: 2022-03-23
Publication date: 2026-02-02
Anticipated expiration: 2042-03-23
Also published as: JP2023140976A

Description

本開示は、演算装置、演算方法及びプログラムに関する。 This disclosure relates to a computing device, a computing method, and a program.

移動体には、周囲を検出するセンサが備えられる場合がある。特許文献１には、車両に設けられた複数のセンサ同士の位置ずれ量を検出するキャリブレーション装置が記載されている。 Mobile vehicles may be equipped with sensors that detect their surroundings. Patent Document 1 describes a calibration device that detects the amount of positional misalignment between multiple sensors installed in a vehicle.

特開２０１６－２２３９６３号公報JP 2016-223963 A

このような移動体においては、例えば移動体の基準座標系におけるセンサの位置などの、移動体の移動量を推定するためのパラメータを高精度に算出することで、移動体のキャリブレーションを適切に実行することが求められている。 For such moving bodies, it is necessary to properly calibrate the moving body by calculating with high accuracy parameters for estimating the amount of movement of the moving body, such as the position of the sensor in the reference coordinate system of the moving body.

本開示は、上述した課題を解決するものであり、移動体の移動量を推定するためのパラメータを高精度に算出可能な演算装置、演算方法及びプログラムを提供することを目的とする。 The present disclosure aims to solve the above-mentioned problems by providing a calculation device, calculation method, and program that can accurately calculate parameters for estimating the movement amount of a moving object.

上述した課題を解決し、目的を達成するために、本開示に係る演算装置は、移動体に搭載されて前記移動体の周囲を検出する外界センサの検出結果に基づいて、前記移動体の第１推定位置を算出する第１位置算出部と、前記移動体に搭載されて前記移動体の制御出力を示す出力パラメータを検出する内界センサの検出結果に基づいて、前記移動体の第２推定位置を算出する第２位置算出部と、第１推定位置及び前記第２推定位置に基づいて、前記移動体の移動量を算出するための移動パラメータの推定値を算出するパラメータ算出部と、前記移動体の停止時における前記外界センサの検出結果に基づいて、前記推定値の妥当性を判定する判定部と、妥当であると判断された前記推定値を前記移動体の移動パラメータとして設定するパラメータ設定部と、を含む。 In order to solve the above-mentioned problems and achieve the objectives, the computing device disclosed herein includes a first position calculation unit that calculates a first estimated position of the moving body based on the detection results of an external sensor that is mounted on the moving body and detects the surroundings of the moving body; a second position calculation unit that calculates a second estimated position of the moving body based on the detection results of an internal sensor that is mounted on the moving body and detects an output parameter that indicates the control output of the moving body; a parameter calculation unit that calculates estimated values of movement parameters for calculating the amount of movement of the moving body based on the first estimated position and the second estimated position; a determination unit that determines the validity of the estimated values based on the detection results of the external sensor when the moving body is stopped; and a parameter setting unit that sets the estimated values that are determined to be valid as movement parameters of the moving body.

上述した課題を解決し、目的を達成するために、本開示に係る演算方法は、移動体に搭載されて前記移動体の周囲を検出する外界センサの検出結果に基づいて、前記移動体の第１推定位置を算出するステップと、前記移動体に搭載されて前記移動体の制御出力を示す出力パラメータを検出する内界センサの検出結果に基づいて、前記移動体の第２推定位置を算出するステップと、第１推定位置及び前記第２推定位置に基づいて、前記移動体の移動量を算出するための移動パラメータの推定値を算出するステップと、前記移動体の停止時における複数の前記外界センサの検出結果に基づいて、前記推定値の妥当性を判定するステップと、妥当であると判断された前記推定値を前記移動体の移動パラメータとして設定するステップと、を含む。 In order to solve the above-mentioned problems and achieve the objectives, the calculation method disclosed herein includes the steps of: calculating a first estimated position of a moving body based on the detection results of an external sensor mounted on the moving body that detects the surroundings of the moving body; calculating a second estimated position of the moving body based on the detection results of an internal sensor mounted on the moving body that detects an output parameter indicating the control output of the moving body; calculating estimated values of movement parameters for calculating the amount of movement of the moving body based on the first estimated position and the second estimated position; determining the validity of the estimated values based on the detection results of the external sensors when the moving body is stopped; and setting the estimated values determined to be valid as movement parameters of the moving body.

上述した課題を解決し、目的を達成するために、本開示に係るプログラムは、移動体に搭載されて前記移動体の周囲を検出する外界センサの検出結果に基づいて、前記移動体の第１推定位置を算出するステップと、前記移動体に搭載されて前記移動体の制御出力を示す出力パラメータを検出する内界センサの検出結果に基づいて、前記移動体の第２推定位置を算出するステップと、第１推定位置及び前記第２推定位置に基づいて、前記移動体の移動量を算出するための移動パラメータの推定値を算出するステップと、前記移動体の停止時における複数の前記外界センサの検出結果に基づいて、前記推定値の妥当性を判定するステップと、妥当であると判断された前記推定値を前記移動体の移動パラメータとして設定するステップと、を、コンピュータに実行させる。 To solve the above-mentioned problems and achieve the objectives, the program disclosed herein causes a computer to execute the following steps: calculating a first estimated position of a moving body based on detection results from external sensors mounted on the moving body that detect the surroundings of the moving body; calculating a second estimated position of the moving body based on detection results from internal sensors mounted on the moving body that detect output parameters indicating the control output of the moving body; calculating estimated values of movement parameters for calculating the amount of movement of the moving body based on the first estimated position and the second estimated position; determining the validity of the estimated values based on detection results from the multiple external sensors when the moving body is stopped; and setting the estimated values determined to be valid as movement parameters of the moving body.

本開示によれば、移動体の移動量を推定するためのパラメータを高精度に算出することができる。 This disclosure makes it possible to calculate parameters for estimating the movement amount of a moving object with high accuracy.

図１は、本実施形態に係る移動体の模式図である。FIG. 1 is a schematic diagram of a moving body according to this embodiment. 図２は、制御装置の模式的なブロック図である。FIG. 2 is a schematic block diagram of the control device. 図３は、推定位置の算出の一例を説明する模式図である。FIG. 3 is a schematic diagram illustrating an example of calculation of an estimated position. 図４は、移動パラメータの推定値の妥当性の判定方法の例を説明する模式図である。FIG. 4 is a schematic diagram illustrating an example of a method for determining the validity of estimated values of movement parameters. 図５は、移動パラメータの推定値の妥当性の判定方法の例を説明する模式図である。FIG. 5 is a schematic diagram illustrating an example of a method for determining the validity of estimated values of movement parameters. 図６は、本実施形態に係る移動パラメータの設定の処理フローを説明するフローチャートである。FIG. 6 is a flowchart illustrating the processing flow for setting movement parameters according to this embodiment. 図７は、第１実施形態の他の例に係る演算装置の模式的なブロック図である。FIG. 7 is a schematic block diagram of a calculation device according to another example of the first embodiment. 図８は、バイアス値の一例を示すグラフである。FIG. 8 is a graph showing an example of the bias value. 図９は、第３実施形態における移動パラメータの設定の処理フローを説明するフローチャートである。FIG. 9 is a flowchart illustrating the processing flow for setting movement parameters in the third embodiment.

以下に添付図面を参照して、本開示の好適な実施形態を詳細に説明する。なお、この実施形態により本開示が限定されるものではなく、また、実施形態が複数ある場合には、各実施形態を組み合わせて構成するものも含むものである。 Preferred embodiments of the present disclosure are described in detail below with reference to the accompanying drawings. Note that the present disclosure is not limited to these embodiments, and when there are multiple embodiments, they also include configurations that combine the respective embodiments.

（第１実施形態）
（移動体）
図１は、本実施形態に係る移動体の模式図である。本実施形態においては、移動体１０は、自動で移動可能な装置である。より詳しくは、移動体１０は、フォークリフトであり、より詳しくはいわゆるＡＧＶ（ＡｕｔｏｍａｔｅｄＧｕｉｄｅｄＶｅｈｉｃｌｅ）やＡＧＦ（ＡｕｔｏｍａｔｅｄＧｕｉｄｅｄＦｏｒｋｌｉｆｔ）である。ただし、移動体１０は、移動可能な任意の装置であってよく、フォークリフトであることに限られない。また、移動体１０は、自動で移動する装置に限られず、ユーザの運転によって移動する装置であってもよい。 (First embodiment)
(Mobile)
FIG. 1 is a schematic diagram of a mobile object according to this embodiment. In this embodiment, the mobile object 10 is an automatically movable device. More specifically, the mobile object 10 is a forklift, and more specifically, a so-called AGV (Automated Guided Vehicle) or AGF (Automated Guided Forklift). However, the mobile object 10 may be any mobile device and is not limited to a forklift. Furthermore, the mobile object 10 is not limited to an automatically movable device, and may be a device that is driven by a user.

（移動体の構成）
図１に示すように、移動体１０は、車体２０と、車輪２０Ａと、ストラドルレッグ２１と、マスト２２と、フォーク２４と、外界センサ２６Ａと、内界センサ２６Ｂと、制御装置２８とを備えている。ストラドルレッグ２１は、車体２０の前後方向における一方の端部に設けられて、車体２０から突出する一対の軸状の部材である。車輪２０Ａは、それぞれのストラドルレッグ２１の先端と、車体２０とに設けられている。すなわち、車輪２０Ａは、合計３個設けられているが、車輪２０Ａの設けられる位置や個数は任意であってよい。マスト２２は、ストラドルレッグ２１に移動可能に取り付けられ、車体２０の前後方向に移動する。マスト２２は、前後方向に直交する上下方向に沿って延在する。フォーク２４は、マスト２２に方向Ｚに移動可能に取付けられている。フォーク２４は、マスト２２に対して、車体２０の横方向（上下方向及び前後方向に交差する方向）にも移動可能であってよい。フォーク２４は、一対のツメ２４Ａ、２４Ｂを有している。ツメ２４Ａ、２４Ｂは、マスト２２から車体２０の前方向に向けて延在している。ツメ２４Ａとツメ２４Ｂとは、マスト２２の横方向に、互いに離れて配置されている。以下、前後方向のうち、移動体１０においてフォーク２４が設けられている側の方向を、前方向とし、フォーク２４が設けられていない側の方向を、後方向とする。 (Configuration of moving body)
As shown in FIG. 1 , the mobile body 10 includes a vehicle body 20, wheels 20A, straddle legs 21, a mast 22, a fork 24, an external sensor 26A, an internal sensor 26B, and a control device 28. The straddle legs 21 are a pair of shaft-shaped members provided at one end of the vehicle body 20 in the longitudinal direction and protruding from the vehicle body 20. The wheels 20A are provided at the tip of each straddle leg 21 and on the vehicle body 20. That is, a total of three wheels 20A are provided, but the positions and number of the wheels 20A may be arbitrary. The mast 22 is movably attached to the straddle legs 21 and moves in the longitudinal direction of the vehicle body 20. The mast 22 extends in an up-down direction perpendicular to the longitudinal direction. The fork 24 is movably attached to the mast 22 in direction Z. The fork 24 may also be movable in the lateral direction of the vehicle body 20 (a direction intersecting the up-down direction and the fore-and-aft direction) relative to the mast 22. The fork 24 has a pair of claws 24A, 24B. The claws 24A, 24B extend from the mast 22 toward the front of the vehicle body 20. The claws 24A and 24B are arranged apart from each other in the lateral direction of the mast 22. Hereinafter, in the fore-and-aft direction, the direction toward the side of the vehicle 10 where the fork 24 is provided is referred to as the forward direction, and the direction toward the side where the fork 24 is not provided is referred to as the rearward direction.

（外界センサ）
外界センサ２６Ａは、移動体１０に搭載されて、移動体１０の周囲を検出するセンサである。外界センサ２６Ａは、移動体１０の周辺に存在する対象物の位置を検出する。本実施形態では、外界センサ２６Ａは、移動体１０に対する対象物の位置及び姿勢を検出することが好ましい。外界センサ２６Ａは、例えばレーザ光を照射するセンサである。外界センサ２６Ａは、一方向（ここでは横方向）に走査しつつレーザ光を照射し、照射したレーザ光の反射光から、対象物の位置及び向きを検出する。すなわち、外界センサ２６Ａは、いわゆる２次元（２Ｄ）－ＬｉＤＡＲ（ＬｉｇｈｔＤｅｔｅｃｔｉｏｎＡｎｄＲａｎｇｉｎｇ）であるともいえる。ただし、外界センサ２６Ａは、以上のものに限られず任意の方法で対象物を検出するセンサであってよく、例えば、複数の方向に走査されるいわゆる３次元（３Ｄ）－ＬｉＤＡＲであってもよいし、走査されない、いわゆる１次元（１Ｄ）－ＬｉＤＡＲであってもよいし、周囲を撮像するカメラであってもよい。 (external sensor)
The external sensor 26A is a sensor mounted on the mobile body 10 and detects the surroundings of the mobile body 10. The external sensor 26A detects the position of an object present in the surroundings of the mobile body 10. In this embodiment, it is preferable that the external sensor 26A detects the position and orientation of the object relative to the mobile body 10. The external sensor 26A is, for example, a sensor that emits laser light. The external sensor 26A emits laser light while scanning in one direction (here, the horizontal direction), and detects the position and orientation of the object from the reflected light of the irradiated laser light. In other words, the external sensor 26A can also be said to be a so-called two-dimensional (2D)-LiDAR (Light Detection and Ranging). However, the external sensor 26A is not limited to the above and may be a sensor that detects an object in any manner, for example, a so-called three-dimensional (3D)-LiDAR that scans in multiple directions, a so-called one-dimensional (1D)-LiDAR that does not scan, or a camera that captures images of the surroundings.

図１の例では、外界センサ２６Ａは、車体２０の後方向側（フォーク２４と反対側）において、横方向に並んで２つ設けられている。ただし、外界センサ２６Ａの設けられる位置はこれに限られず、任意の位置に設けられてもよいし、設けられる数も任意であってよい。ただし、外界センサ２６Ａは、複数設けられることが好ましい。 In the example shown in Figure 1, two external sensors 26A are provided side by side on the rear side of the vehicle body 20 (opposite the fork 24). However, the locations at which the external sensors 26A are provided are not limited to this, and the external sensors 26A may be provided in any location, and the number of external sensors provided may also be arbitrary. However, it is preferable to provide multiple external sensors 26A.

（内界センサ）
内界センサ２６Ｂは、移動体１０に搭載されて、移動体１０の出力パラメータを検出するセンサである。出力パラメータとは、移動体の制御出力を示すパラメータである。さらに言えば、出力パラメータとは、移動体１０が制御によって動作した度合いを指しており、移動体１０に制御入力がなされた際の、移動体１０からの出力値であるといえる。すなわち、移動体１０の駆動機構（アクチュエータなど）には、制御装置２８により、所望の進行方向及び移動速度となるように、制御入力がなされる。移動体１０の駆動機構は、制御入力に応じて作動して、制御入力に応じた出力パラメータが、内界センサ２６Ｂにより検出される。出力パラメータは、移動体の制御出力を示す任意のパラメータであってよいが、本実施形態では、移動体１０の車輪２０Ａの回転数と移動体１０の操舵角である。この場合、内界センサ２６Ｂは、車輪２０Ａの回転数（単位時間当たりの回転数）と、移動体１０の操舵角（車輪２０Ａの向きの変化量）とを、検出する。内界センサ２６として、回転数を検出するセンサと操舵角を検出するセンサとが設けられていてもよいし、１つのセンサで回転数と操舵角とを検出してもよい。内界センサ２６Ｂは、例えば、ＩＭＵ（ＩｎｅｒｔｉａｌＭｅａｓｕｒｅｍｅｎｔＵｎｉｔ）であってもよい。 (Internal sensor)
The internal sensor 26B is mounted on the mobile body 10 and detects output parameters of the mobile body 10. The output parameters are parameters that indicate the control output of the mobile body 10. More specifically, the output parameters indicate the degree to which the mobile body 10 operates under control, and can be considered to be output values from the mobile body 10 when control input is provided to the mobile body 10. That is, the control device 28 provides control input to the drive mechanism (such as an actuator) of the mobile body 10 so that the mobile body 10 moves in a desired direction and at a desired speed. The drive mechanism of the mobile body 10 operates in response to the control input, and the output parameters corresponding to the control input are detected by the internal sensor 26B. The output parameters may be any parameters that indicate the control output of the mobile body, but in this embodiment, they are the rotation speed of the wheels 20A of the mobile body 10 and the steering angle of the mobile body 10. In this case, the internal sensor 26B detects the rotation speed of the wheels 20A (the number of rotations per unit time) and the steering angle of the mobile body 10 (the amount of change in the direction of the wheels 20A). The internal sensor 26B may include a sensor for detecting the rotation speed and a sensor for detecting the steering angle, or a single sensor may be used to detect both the rotation speed and the steering angle. The internal sensor 26B may be, for example, an IMU (Inertial Measurement Unit).

（制御装置）
図２は、制御装置の模式的なブロック図である。本実施形態では、演算装置としての制御装置２８は、移動体１０の移動パラメータを設定するキャリブレーション処理を実行し、移動体１０の移動を制御する装置である。すなわち、本実施形態では、制御装置２８が、移動パラメータを設定する演算装置と、移動体１０の移動を制御する制御装置との両方の機能を兼ね備えている。ただし、後述のように、移動パラメータを設定する演算装置と、移動体１０の移動を制御する制御装置とが別体であってよい。この場合、移動パラメータを設定する演算装置は、移動体１０に搭載されず、移動体１０とは別の場所に配置されていてよい。なお、移動パラメータについては後述する。 (Control device)
FIG. 2 is a schematic block diagram of the control device. In this embodiment, the control device 28 as a calculation device executes a calibration process to set movement parameters of the moving body 10 and controls the movement of the moving body 10. That is, in this embodiment, the control device 28 combines the functions of both a calculation device that sets movement parameters and a control device that controls the movement of the moving body 10. However, as will be described later, the calculation device that sets the movement parameters and the control device that controls the movement of the moving body 10 may be separate entities. In this case, the calculation device that sets the movement parameters may not be mounted on the moving body 10 but may be located in a location separate from the moving body 10. Movement parameters will be described later.

制御装置２８は、コンピュータであり、図２に示すように、通信部３０と記憶部３２と制御部３４とを含む。通信部３０は、制御部３４に用いられて、外部の装置と通信するモジュールであり、例えばＷｉＦｉ（登録商標）モジュールやアンテナなどを含んでよい。通信部３０による通信方式は、本実施形態では無線通信であるが、通信方式は任意であってよい。記憶部３２は、制御部３４の演算内容やプログラムなどの各種情報を記憶するメモリであり、例えば、ＲＡＭ（ＲａｎｄｏｍＡｃｃｅｓｓＭｅｍｏｒｙ）と、ＲＯＭ（ＲｅａｄＯｎｌｙＭｅｍｏｒｙ）のような主記憶装置と、ＨＤＤ（ＨａｒｄＤｉｓｋＤｒｉｖｅ）などの外部記憶装置とのうち、少なくとも１つ含む。 The control device 28 is a computer and, as shown in FIG. 2, includes a communication unit 30, a memory unit 32, and a control unit 34. The communication unit 30 is a module used by the control unit 34 to communicate with external devices and may include, for example, a Wi-Fi (registered trademark) module or an antenna. In this embodiment, the communication method used by the communication unit 30 is wireless communication, but any communication method may be used. The memory unit 32 is a memory that stores various information such as the calculation contents and programs of the control unit 34, and may include, for example, at least one of a main memory device such as RAM (Random Access Memory), a ROM (Read Only Memory), and an external memory device such as an HDD (Hard Disk Drive).

制御部３４は、演算装置であり、例えばＣＰＵ（ＣｅｎｔｒａｌＰｒｏｃｅｓｓｉｎｇＵｎｉｔ）などの演算回路を含む。制御部３４は、移動制御部４０と、第１位置算出部４２と、第２位置算出部４４と、パラメータ算出部４６と、判定部４８と、パラメータ設定部５０とを含む。制御部３４は、記憶部３２からプログラム（ソフトウェア）を読み出して実行することで、移動制御部４０と第１位置算出部４２と第２位置算出部４４とパラメータ算出部４６と判定部４８とパラメータ設定部５０とを実現して、それらの処理を実行する。なお、制御部３４は、１つのＣＰＵによってこれらの処理を実行してもよいし、複数のＣＰＵを備えて、それらの複数のＣＰＵで、処理を実行してもよい。また、移動制御部４０と第１位置算出部４２と第２位置算出部４４とパラメータ算出部４６と判定部４８とパラメータ設定部５０との少なくとも一部を、ハードウェア回路で実現してもよい。また、記憶部３２が保存する制御部３４用のプログラムは、制御装置２８（演算装置）が読み取り可能な記録媒体に記憶されていてもよい。 The control unit 34 is a computing device and includes a computing circuit such as a CPU (Central Processing Unit). The control unit 34 includes a movement control unit 40, a first position calculation unit 42, a second position calculation unit 44, a parameter calculation unit 46, a determination unit 48, and a parameter setting unit 50. The control unit 34 implements the movement control unit 40, the first position calculation unit 42, the second position calculation unit 44, the parameter calculation unit 46, the determination unit 48, and the parameter setting unit 50 by reading and executing a program (software) from the storage unit 32. The control unit 34 may implement these processes using a single CPU, or may be equipped with multiple CPUs and execute the processes using the multiple CPUs. Furthermore, at least a portion of the movement control unit 40, the first position calculation unit 42, the second position calculation unit 44, the parameter calculation unit 46, the determination unit 48, and the parameter setting unit 50 may be implemented using hardware circuits. In addition, the program for the control unit 34 stored in the memory unit 32 may be stored on a recording medium readable by the control device 28 (computing device).

（移動制御部）
移動制御部４０は、移動体１０の移動を制御する。移動制御部４０は、移動体１０が移動する経路を設定する。移動制御部４０は、移動体１０の移動経路を任意の方法で設定してよく、例えば、移動体１０の移動先などを設定するシステム（装置）から、移動経路の情報を取得してもよいし、そのシステムから取得した移動先の情報に基づき、自身で移動経路を設定してもよい。移動制御部４０は、移動体１０の駆動機構に対して制御入力を行うことで駆動機構を駆動させ、設定した移動経路に沿って移動体１０を移動させる。また、移動制御部４０は、外界センサ２６Ａ及び内界センサ２６Ｂを制御して、外界センサ２６Ａに周囲を検出させつつ、内界センサ２６Ｂに出力パラメータを検出させる。移動制御部４０は、外界センサ２６Ａに所定時間毎に周囲を検出させて、内界センサ２６Ｂに所定時間毎に出力パラメータを検出させる。 (Movement control unit)
The movement control unit 40 controls the movement of the mobile object 10. The movement control unit 40 sets a route along which the mobile object 10 will move. The movement control unit 40 may set the movement route of the mobile object 10 in any manner. For example, the movement control unit 40 may acquire information about the movement route from a system (device) that sets the destination of the mobile object 10, or may set the movement route itself based on information about the destination acquired from the system. The movement control unit 40 drives the drive mechanism of the mobile object 10 by providing control input to the drive mechanism, causing the mobile object 10 to move along the set movement route. The movement control unit 40 also controls the external sensor 26A and the internal sensor 26B to cause the external sensor 26A to detect the surroundings and the internal sensor 26B to detect output parameters. The movement control unit 40 causes the external sensor 26A to detect the surroundings at predetermined time intervals and the internal sensor 26B to detect output parameters at predetermined time intervals.

（第１位置算出部）
図３は、推定位置の算出の一例を説明する模式図である。第１位置算出部４２は、外界センサ２６Ａの検出結果に基づいて、移動体１０の第１推定位置を算出する。さらに言えば、第１位置算出部４２は、外界センサ２６Ａの検出結果と、後述の移動パラメータΦの設定値（本例では移動体１０の基準座標系における外界センサ２６Ａの位置）とに基づいて、移動体１０の第１推定位置を算出する。第１推定位置とは、外界センサ２６Ａの検出結果により推定された、移動体１０の位置を指す。第１位置算出部４２は、移動体１０の移動中における外界センサ２６Ａの所定時間毎の検出結果を取得して、所定時間毎の検出結果から、所定時間毎の第１推定位置を算出する。以下、図３に基づいてより具体的に説明する。 (First position calculation unit)
FIG. 3 is a schematic diagram illustrating an example of calculation of an estimated position. The first position calculation unit 42 calculates a first estimated position of the mobile object 10 based on the detection results of the external sensor 26A. Furthermore, the first position calculation unit 42 calculates the first estimated position of the mobile object 10 based on the detection results of the external sensor 26A and the set value of a movement parameter Φ (described below, in this example, the position of the external sensor 26A in the reference coordinate system of the mobile object 10). The first estimated position refers to the position of the mobile object 10 estimated based on the detection results of the external sensor 26A. The first position calculation unit 42 acquires detection results of the external sensor 26A at predetermined time intervals while the mobile object 10 is moving, and calculates the first estimated position for each predetermined time interval from the detection results for each predetermined time interval. A more detailed description will be given below with reference to FIG. 3.

ここで、ある時刻（図３の例では時刻ｉ－１）からその後の時刻（図３の例では時刻ｉ）までの外界センサ２６Ａの移動方向及び移動量（図３の例では、移動ベクトルｆ（ｚ_ｉ））は、時刻ｉにおいて外界センサ２６Ａに検出された対象物の位置と、時刻ｉ－１において外界センサ２６Ａに検出された同じ対象物の位置との差分に相当する。従って、第１位置算出部４２は、時刻ｉにおいて外界センサ２６Ａに検出された対象物の位置と、時刻ｉ－１において外界センサ２６Ａに検出された同じ対象物の位置とに基づいて、時刻ｉ－１から時刻ｉまでの外界センサ２６Ａの移動方向及び移動量を算出できる。例えば、第１位置算出部４２は、時刻ｉ－１での対象物の位置から、時刻ｉでの同じ対象物の位置までの変位量及び変位方向を、時刻ｉ－１から時刻ｉまでの外界センサ２６Ａの移動方向及び移動量として算出する。なお、同じ対象物であるかは任意の方法で判定できるが、例えば外界センサ２６ＡがＬｉＤＡＲの場合、第１位置算出部４２は、スキャンマッチングを行うことで、時刻ｉ－１における点群と時刻ｉにおける点群のうちから、同一対象物を検出した点群（検出点）を抽出する。そして、第１位置算出部４２は、抽出した時刻ｉ－１における点群の位置と、抽出した時刻ｉにおける点群の位置とに基づいて、時刻ｉ－１から時刻ｉまでの外界センサ２６Ａの移動方向及び移動量を算出する。また例えば、外界センサ２６Ａがカメラの場合、第１位置算出部４２は、画像認識によって、時刻ｉ－１に撮像された画像と時刻ｉに撮像された画像のうちから、同じ対象物が写っている領域（画素群）を特定する。そして、第１位置算出部４２は、時刻ｉ－１において特定した領域の位置と、時刻ｉにおいて特定した領域の位置とに基づいて、時刻ｉ－１から時刻ｉまでの外界センサ２６Ａの移動方向及び移動量を算出する。 Here, the direction and amount of movement (movement vector f(z i ) in the example of FIG. 3 ) of external sensor 26A from a certain time (time i-1 in the example of FIG. 3 ) to a subsequent time (time i in the example of FIG. ₃ ) corresponds to the difference between the position of an object detected by external sensor 26A at time i and the position of the same object detected by external sensor 26A at time i-1. Therefore, the first position calculation unit 42 can calculate the direction and amount of movement of external sensor 26A from time i-1 to time i based on the position of the object detected by external sensor 26A at time i and the position of the same object detected by external sensor 26A at time i-1. For example, the first position calculation unit 42 calculates the amount and direction of displacement from the position of the object at time i-1 to the position of the same object at time i as the direction and amount of movement of external sensor 26A from time i-1 to time i. Whether the objects are the same can be determined by any method. For example, if the external sensor 26A is a LiDAR, the first position calculation unit 42 performs scan matching to extract point clouds (detected points) that detect the same object from the point clouds at time i-1 and the point clouds at time i. The first position calculation unit 42 then calculates the direction and amount of movement of the external sensor 26A from time i-1 to time i based on the positions of the extracted point clouds at time i-1 and the extracted point clouds at time i. Furthermore, for example, if the external sensor 26A is a camera, the first position calculation unit 42 uses image recognition to identify areas (pixel groups) that depict the same object from the images captured at time i-1 and the images captured at time i. The first position calculation unit 42 then calculates the direction and amount of movement of the external sensor 26A from time i-1 to time i based on the positions of the areas identified at time i-1 and the positions of the areas identified at time i.

第１位置算出部４２は、このようにして算出した時刻ｉ－１から時刻ｉまでの外界センサ２６Ａの移動方向及び移動量（移動ベクトルｆ（ｚ_ｉ））と、移動体１０の基準座標系における外界センサ２６Ａの位置（図３の例では外界センサ２６Ａから基準点ＰまでのベクトルΦ’）に基づいて、時刻ｉ－１から時刻ｉまでの移動体１０の基準点Ｐの移動方向及び移動量（図３の例では、移動ベクトルｆ_Φ’（ｚ_ｉ））を算出する。言い換えれば、第１位置算出部４２は、移動ベクトルｆ（ｚ_ｉ）から、ベクトルΦ’だけずらしたベクトルを、時刻ｉ－１から時刻ｉまでの基準点Ｐの移動方向及び移動量（移動ベクトルｆ_Φ’（ｚ_ｉ））として算出する。なお、移動体１０の基準座標系とは、移動体１０の基準点Ｐを基準とした座標系であり、基準点Ｐは任意の位置に設定されてよい。移動体１０の基準座標系における外界センサ２６Ａの位置は、移動体１０の基準点Ｐに対する外界センサ２６Ａの位置ともいえ、例えば設計情報などから、予め設定されてよい。 The first position calculation unit 42 calculates the direction of movement and amount of movement of the reference point P of the moving body 10 from time i-1 to time i (movement vector f(z _i )) from time i-1 to time i based on the direction of movement and amount of movement of the external sensor 26A from time i-1 to time i calculated in this manner (movement vector f(z _i )) and the position of the external sensor 26A in the reference coordinate system of the moving body 10 (vector Φ _' from the external sensor 26A to the reference point P in the example of FIG. 3). In other words, the first position calculation unit 42 calculates a vector shifted by vector Φ' from the movement vector f(z _i ) as the direction of movement and amount of movement of the reference point P from time i-1 to time i (movement vector f _Φ' (z _i )). Note that the reference coordinate system of the moving body 10 is a coordinate system based on the reference point P of the moving body 10, and the reference point P may be set at an arbitrary position. The position of the external sensor 26A in the reference coordinate system of the moving body 10 can also be said to be the position of the external sensor 26A relative to the reference point P of the moving body 10, and may be set in advance based on, for example, design information.

第１位置算出部４２は、このようにして算出した、時刻ｉ－１から時刻ｉまでの基準点Ｐの移動方向及び移動量（移動ベクトルｆ_Φ’（ｚ_ｉ））を、時刻ｉにおける移動体１０の第１推定位置とする。ただしそれに限られず、第１位置算出部４２は、所定位置（図３の例では位置Ｐ_ｉ－１）から移動ベクトルｆ_Φ’（ｚ_ｉ）だけ離れた座標（図３の例では位置ＰＡｉ）そのものを、第１推定位置としてもよい。 The first position calculation unit 42 determines the direction and amount of movement of the reference point P from time i-1 to time i (movement vector f _Φ' (z _i )) calculated in this manner as the first estimated position of the moving object 10 at time i. However, without being limited thereto, the first position calculation unit 42 may also determine the coordinates (position PAi in the example of FIG. 3) itself that are separated from a predetermined position (position P _i-1 in the example of FIG. 3) by the movement vector f _Φ' (z _i ) as the first estimated position.

第１位置算出部４２は、所定時間毎に検出された外界センサ２６Ａの検出結果（対象物の位置）に基づき、所定時間毎の、第１推定位置を算出する。すなわち例えば、第１位置算出部４２は、時刻ｉにおける外界センサ２６Ａの検出結果と、時刻ｉ＋１における外界センサ２６Ａの検出結果とから、上記と同様の方法で、時刻ｉ＋１における第１推定位置を算出する。このように、本実施形態では、移動体１０を移動させつつ、移動中の時刻を一定の規則に従い離散的にサンプリングして、各サンプル時刻における、外界センサ２６Ａの検出により求めた移動体１０の位置及び姿勢（第１推定位置）をノードとし、ノード間をエッジで結んだグラフ（ポーズグラフ）を構築するといえる。 The first position calculation unit 42 calculates a first estimated position for each predetermined time based on the detection results (position of the object) of the external sensor 26A detected at each predetermined time. That is, for example, the first position calculation unit 42 calculates the first estimated position at time i+1 using a method similar to that described above from the detection results of the external sensor 26A at time i and the detection results of the external sensor 26A at time i+1. In this way, in this embodiment, while the mobile object 10 is moving, the time during movement is discretely sampled according to a certain rule, and the position and attitude (first estimated position) of the mobile object 10 determined by detection by the external sensor 26A at each sample time is used as a node, and a graph (pose graph) is constructed in which the nodes are connected by edges.

（第２位置算出部）
第２位置算出部４４は、内界センサ２６Ｂの検出結果に基づいて、移動体１０の第２推定位置を算出する。第２推定位置とは、内界センサ２６Ｂの検出結果により推定された、移動体１０の位置を指す。第２位置算出部４４は、移動体１０の移動中における内界センサ２６Ｂの所定時間毎の検出結果を取得する。第２位置算出部４４は、移動体１０の移動中における内界センサ２６Ｂの所定時間毎の検出結果から、所定時間毎の第２推定位置を算出する。以下、図３に基づいてより具体的に説明する。 (Second position calculation unit)
The second position calculation unit 44 calculates a second estimated position of the mobile object 10 based on the detection result of the internal sensor 26B. The second estimated position refers to the position of the mobile object 10 estimated based on the detection result of the internal sensor 26B. The second position calculation unit 44 acquires the detection result of the internal sensor 26B at each predetermined time while the mobile object 10 is moving. The second position calculation unit 44 calculates the second estimated position at each predetermined time from the detection result of the internal sensor 26B at each predetermined time while the mobile object 10 is moving. This will be described in more detail below with reference to FIG. 3.

第２位置算出部４４は、内界センサ２６Ｂが検出した出力パラメータに基づいて、移動体１０の移動量及び移動方向を算出する。例えば、第２位置算出部４４は、時刻ｉ－１において内界センサ２６Ｂに検出された出力パラメータに基づき、時刻ｉ－１から時刻ｉまでの移動体１０の基準点Ｐの移動方向及び移動量（図３の例では、移動ベクトルαｕ_ｉ）を算出する。第２位置算出部４４は、出力パラメータを積分することにより、オドメトリの手法で、基準点Ｐの移動方向及び移動量を算出する。すなわち例えば、第２位置算出部４４は、時刻ｉ－１で検出された車輪２０Ａの回転数を積分して、時刻ｉ－１から時刻ｉまでの基準点Ｐの移動量を算出し、時刻ｉ－１で検出された操舵角（車輪２０Ａの向きの変化量）から、基準点Ｐの移動方向を算出して、時刻ｉ－１から時刻ｉまでの移動体１０の基準点Ｐの移動方向及び移動量とする。第２位置算出部４４は、このようにして算出した、時刻ｉ－１から時刻ｉまでの基準点Ｐの移動方向及び移動量（移動ベクトルαｕ_ｉ）を、時刻ｉにおける移動体１０の第２推定位置とする。ただしそれに限られず、第２位置算出部４４は、所定位置（図３の例では位置Ｐ_ｉ－１）から移動ベクトルαｕ_ｉだけ離れた座標（図３の例では位置ＰＢｉ）そのものを、第２推定位置としてもよい。 The second position calculation unit 44 calculates the amount and direction of movement of the moving object 10 based on the output parameters detected by the internal sensor 26B. For example, the second position calculation unit 44 calculates the direction and amount of movement of the reference point P of the moving object 10 from time i-1 to time i (in the example of FIG. 3 ) based on the output parameters detected by the internal sensor 26B at time _i -1. The second position calculation unit 44 calculates the direction and amount of movement of the reference point P using an odometry technique by integrating the output parameters. That is, for example, the second position calculation unit 44 integrates the number of rotations of the wheel 20A detected at time i-1 to calculate the amount of movement of the reference point P from time i-1 to time i, and calculates the direction of movement of the reference point P from the steering angle (amount of change in the orientation of the wheel 20A) detected at time i-1, and sets this as the direction and amount of movement of the reference point P of the moving object 10 from time i-1 to time i. The second position calculation unit 44 determines the direction and amount of movement of the reference point P from time i-1 to time i (movement vector αu _i ) calculated in this manner as the second estimated position of the moving object 10 at time i. However, without being limited thereto, the second position calculation unit 44 may also determine the coordinates (position PBi in the example of FIG. 3 ) that are separated from a predetermined position (position P _i-1 in the example of FIG. 3 ) by the movement vector αu _i as the second estimated position.

なお、第２位置算出部４４は、後述の移動パラメータΦの設定値にも基づいて、第２推定位置を算出してよい。例えば、第２位置算出部４４は、移動体１０の車輪２０Ａの径の設定値と、内界センサ２６Ｂのバイアス値（正常値からのずれ量）の設定値との少なくとも１つ（好ましくは両方）も用いて、第２推定位置を算出してよい。 The second position calculation unit 44 may also calculate the second estimated position based on the set value of the movement parameter Φ, which will be described later. For example, the second position calculation unit 44 may also calculate the second estimated position using at least one (preferably both) of the set value of the diameter of the wheels 20A of the mobile object 10 and the set value of the bias value (deviation from the normal value) of the internal sensor 26B.

第２位置算出部４４は、所定時間毎に検出された内界センサ２６Ｂの検出結果（出力パラメータ）に基づき、所定時間毎の、第２推定位置を算出する。すなわち例えば、第２位置算出部４４は、時刻ｉにおける内界センサ２６Ｂの検出結果から、上記と同様の方法で、時刻ｉ＋１における第２推定位置を算出する。このように、本実施形態では、移動体１０を移動させつつ、移動中の時刻を一定の規則に従い離散的にサンプリングして、各サンプル時刻における、内界センサ２６Ｂの検出により求めた移動体１０の位置及び姿勢（第１推定位置）をノードとし、ノード間をエッジで結んだグラフ（ポーズグラフ）を構築するといえる。 The second position calculation unit 44 calculates a second estimated position for each predetermined time based on the detection results (output parameters) of the internal sensor 26B detected at each predetermined time. That is, for example, the second position calculation unit 44 calculates a second estimated position at time i+1 using the detection results of the internal sensor 26B at time i in a manner similar to that described above. In this way, in this embodiment, while the mobile object 10 is moving, the time during movement is discretely sampled according to a certain rule, and the position and attitude (first estimated position) of the mobile object 10 determined by detection by the internal sensor 26B at each sample time is used as a node, and a graph (pose graph) is constructed in which the nodes are connected by edges.

（パラメータ算出部）
パラメータ算出部４６は、第１推定位置及び第２推定位置に基づいて、移動体１０の移動量を算出するための移動パラメータΦの、推定値を算出する。移動パラメータΦとは、移動体１０の移動量に影響を及ぼすパラメータであるが、上述の出力パラメータとは異なり制御出力を示すものではなく、制御入力によって変動させることができないパラメータといえる。また、移動パラメータΦは、経時により変化する可能性があるパラメータともいえる。本実施形態の例では、移動パラメータΦとは、移動体１０の基準座標系における外界センサ２６Ａの位置（基準点Ｐに対する外界センサ２６Ａの位置）と、移動体１０の車輪径（車輪２０Ａの径）と、内界センサ２６Ｂの検出結果の正常値からのずれ量を示すバイアス値との、少なくとも１つであってよく、これら全てであってもよい。ただしそれに限られず、移動パラメータΦは、経時により変動する可能性があり、移動体１０の移動量に影響を及ぼすが制御入力によっては変動されない、任意のパラメータであってよい。 (Parameter calculation unit)
The parameter calculation unit 46 calculates an estimated value of a movement parameter Φ for calculating the movement amount of the mobile object 10 based on the first estimated position and the second estimated position. The movement parameter Φ is a parameter that affects the movement amount of the mobile object 10. However, unlike the output parameters described above, it does not indicate a control output and can be considered a parameter that cannot be varied by a control input. The movement parameter Φ can also be considered a parameter that can change over time. In this embodiment, the movement parameter Φ may be at least one of the following: the position of the external sensor 26A in the reference coordinate system of the mobile object 10 (the position of the external sensor 26A relative to the reference point P), the wheel diameter of the mobile object 10 (the diameter of the wheel 20A), and a bias value indicating the deviation of the detection result of the internal sensor 26B from a normal value, or all of these. However, the movement parameter Φ is not limited to these, and may be any parameter that can change over time, affects the movement amount of the mobile object 10, but is not varied by a control input.

ここで、同じ時刻における第１推定位置と第２推定位置との差分（誤差）は、理想的な条件ではゼロである。しかしながら、第１推定位置や第２推定位置の算出時に用いたパラメータの誤差などにより、第１推定位置と第２推定位置との差分がゼロにならない場合がある。それに対して、本実施形態では、その差分が移動パラメータΦの誤差により生じていると仮定し、パラメータ算出部４６が、第１推定位置と第２推定位置との差分に基づいて、例えばその差分ができるだけ小さくなるような移動パラメータΦの値を、移動パラメータΦの推定値として算出する。すなわち、第１推定位置と第２推定位置とは、移動パラメータΦに基づいて算出されるため、言い換えれば移動パラメータΦの関数であるため、第１推定位置と第２推定位置との差分が小さくなるような移動パラメータΦの値を、移動パラメータΦの推定値として算出するといえる。 Here, under ideal conditions, the difference (error) between the first estimated position and the second estimated position at the same time is zero. However, due to errors in the parameters used when calculating the first and second estimated positions, the difference between the first and second estimated positions may not be zero. In contrast, in this embodiment, it is assumed that this difference is caused by an error in the movement parameter Φ, and the parameter calculation unit 46 calculates, as an estimated value of the movement parameter Φ, a value of the movement parameter Φ that minimizes the difference based on the difference between the first and second estimated positions. In other words, since the first estimated position and the second estimated position are calculated based on the movement parameter Φ, or in other words, are functions of the movement parameter Φ, it can be said that a value of the movement parameter Φ that minimizes the difference between the first estimated position and the second estimated position is calculated as an estimated value of the movement parameter Φ.

具体的には、パラメータ算出部４６は、所定時間毎の第１推定位置と第２推定位置との差分に基づいて、移動パラメータΦの推定値を算出する。さらに言えば、パラメータ算出部４６は、所定時間毎の第１推定位置と第２推定位置との差分の合計値が最小となるように、言い換えれば、所定時間毎の第１推定位置と第２推定位置との差分の合計値が最小となるような最適化問題を解くことで、移動パラメータΦの推定値を算出する。具体的には、時刻ｉにおける第１推定位置と第２推定位置との差分をｅ_ｉ、時刻ｉにおける定数をΩ_ｉとすると、パラメータ算出部４６は、次の式（１）に示す移動パラメータΦの評価関数をＪ_Ｏ（Φ）が最小となるような移動パラメータΦを、移動パラメータΦの推定値として算出する。 Specifically, the parameter calculation unit 46 calculates an estimate of the movement parameter Φ based on the difference between the first estimated position and the second estimated position for each predetermined time period. Furthermore, the parameter calculation unit 46 calculates the estimate of the movement parameter Φ by solving an optimization problem that minimizes the total value of the differences between the first estimated position and the second estimated position for each predetermined time period, in other words, that minimizes the total value of the differences between the first estimated position and the second estimated position for each predetermined time period. Specifically, where e _i is the difference between the first estimated position and the second estimated position at time i and Ω _i is a constant at time i, the parameter calculation unit 46 calculates, as the estimate of the movement parameter Φ, the movement parameter Φ that minimizes the evaluation function of the movement parameter Φ shown in the following equation (1), J _O (Φ).

なお、Ｔは転置を指す。また、Ω_ｉは、任意に設定される重み係数であり、例えば、差分ｅ_ｉの分散共分散行列の逆行列である。 Note that T indicates transposition. Ω _i is an arbitrarily set weighting coefficient, for example, the inverse matrix of the variance-covariance matrix of the difference e _i .

（判定部）
判定部４８は、パラメータ算出部４６によって算出された移動パラメータΦの推定値が妥当であるかを判定する。移動パラメータΦの値は、例えば自己位置検出などの移動体１０の移動制御において用いられるため、高精度に推定されることが好ましい。それに対して、本実施形態においては、判定部４８により、移動パラメータΦの推定値が妥当であるかを判定することで、移動パラメータΦを高精度に推定することを可能としている。 (Judgment Department)
The determination unit 48 determines whether the estimated value of the movement parameter Φ calculated by the parameter calculation unit 46 is valid. The value of the movement parameter Φ is preferably estimated with high accuracy because it is used in movement control of the moving body 10, such as self-position detection. In contrast, in this embodiment, the determination unit 48 determines whether the estimated value of the movement parameter Φ is valid, thereby making it possible to estimate the movement parameter Φ with high accuracy.

判定部４８は、移動体１０の停止時における外界センサ２６Ａの検出結果に基づいて、移動パラメータΦの推定値が妥当であるかを判定する。すなわち、判定部４８は、移動体１０の停止時において検出された外界センサ２６Ａの検出結果を取得して、その検出結果に基づき、移動パラメータΦの推定値が妥当であるかを判定する。判定部４８は、停止時における外界センサ２６Ａの検出結果に基づいた任意の方法で、移動パラメータΦの推定値の妥当性を判定してよいが、以下に判定方法の具体例を説明する。 The determination unit 48 determines whether the estimated value of the movement parameter Φ is valid based on the detection results of the external sensor 26A when the moving body 10 is stopped. That is, the determination unit 48 acquires the detection results of the external sensor 26A when the moving body 10 is stopped, and determines whether the estimated value of the movement parameter Φ is valid based on the detection results. The determination unit 48 may determine the validity of the estimated value of the movement parameter Φ using any method based on the detection results of the external sensor 26A when the moving body 10 is stopped, but specific examples of determination methods are described below.

判定部４８は、移動体１０の停止時における複数の外界センサ２６Ａの検出結果から、移動パラメータΦの推定値の妥当性を判定することが好ましい。すなわち、判定部４８は、移動体１０の停止時において、複数の外界センサ２６Ａに、移動体１０の周囲の検出体Ａを検出させて、それらの検出体Ａの検出結果に基づいて、推定値の妥当性を判断する。以降では、複数の外界センサ２６Ａとして、第１外界センサ２６Ａ１及び第２外界センサ２６Ａ２を用いる場合を例として説明するが、用いる外界センサ２６Ａの数は２つに限られず３つ以上であってもよい。検出体Ａは、外界センサ２６Ａが検出可能な任意の物体であってよく、例えばレーザ光を反射可能な反射体であってもよいし、停止している移動体１０の周囲に位置している構造物であってもよい。外界センサ２６Ａは、推定値の妥当性を判断するために設けられた検出体Ａを検出してもよいし、停止している移動体１０の周囲に位置している任意の物体を検出体Ａとして利用してもよい。 The determination unit 48 preferably determines the validity of the estimated value of the movement parameter Φ based on the detection results of the multiple external sensors 26A when the mobile body 10 is stopped. That is, when the mobile body 10 is stopped, the determination unit 48 causes the multiple external sensors 26A to detect detected objects A around the mobile body 10, and determines the validity of the estimated value based on the detection results of these detected objects A. Hereinafter, an example will be described in which a first external sensor 26A1 and a second external sensor 26A2 are used as the multiple external sensors 26A, but the number of external sensors 26A used is not limited to two and may be three or more. The detected object A may be any object that the external sensor 26A can detect, such as a reflector that can reflect laser light or a structure located around the stopped mobile body 10. The external sensor 26A may detect a detected object A provided to determine the validity of the estimated value, or any object located around the stopped mobile body 10 may be used as the detected object A.

図４は、移動パラメータの推定値の妥当性の判定方法の例を説明する模式図である。図４に示すように、判定部４８は、移動体１０の停止時において第１外界センサ２６Ａ１により検出された検出体Ａの検出結果を取得して、検出体Ａの検出結果と、移動パラメータΦの推定値とに基づいて、移動体１０の基準座標系における検出体Ａの位置を算出する。すなわち、判定部４８は、検出体Ａの検出結果が示す、第１外界センサ２６Ａ１の座標系における検出体Ａの位置（第１外界センサ２６Ａ１に対する検出体Ａの位置）と、移動パラメータΦの推定値が示す、移動体１０の基準座標系における第１外界センサ２６Ａ１の位置（移動体１０の基準点Ｐに対する第１外界センサ２６Ａ１の位置）とから、移動体１０の基準座標系における検出体Ａの位置（移動体１０の基準点Ｐに対する検出体Ａの位置）を算出する。このようにして第１外界センサ２６Ａ１の検出結果から算出された移動体１０の基準座標系における検出体Ａの位置を、以下適宜、第１検出位置とする。同様に、判定部４８は、移動体１０の停止時において第２外界センサ２６Ａ２により検出された同じ検出体Ａの検出結果と、移動パラメータΦの推定値とに基づいて、移動体１０の基準座標系における検出体Ａの位置を算出する。すなわち、判定部４８は、検出体Ａの検出結果が示す、第２外界センサ２６Ａ２の座標系における検出体Ａの位置（第２外界センサ２６Ａ２に対する検出体Ａの位置）と、移動パラメータΦの推定値が示す、移動体１０の基準座標系における第２外界センサ２６Ａ２の位置（移動体１０の基準点Ｐに対する第２外界センサ２６Ａ２の位置）とから、移動体１０の基準座標系における検出体Ａの位置を算出する。このようにして第２外界センサ２６Ａ２の検出結果から算出された移動体１０の基準座標系における検出体Ａの位置を、以下適宜、第２検出位置とする。 Figure 4 is a schematic diagram illustrating an example of a method for determining the validity of an estimated value of a movement parameter. As shown in Figure 4, the determination unit 48 acquires the detection result of the detected object A detected by the first external sensor 26A1 when the mobile body 10 is stopped, and calculates the position of the detected object A in the reference coordinate system of the mobile body 10 based on the detection result of the detected object A and the estimated value of the movement parameter Φ. That is, the determination unit 48 calculates the position of the detected object A in the reference coordinate system of the mobile body 10 (the position of the detected object A relative to the reference point P of the mobile body 10) from the position of the detected object A in the coordinate system of the first external sensor 26A1 indicated by the detection result of the detected object A (the position of the detected object A relative to the first external sensor 26A1) and the position of the first external sensor 26A1 in the reference coordinate system of the mobile body 10 indicated by the estimated value of the movement parameter Φ (the position of the first external sensor 26A1 relative to the reference point P of the mobile body 10). The position of detected object A in the reference coordinate system of the mobile body 10 calculated in this manner from the detection result of the first external sensor 26A1 will hereinafter be referred to as the first detected position, as appropriate. Similarly, the determination unit 48 calculates the position of detected object A in the reference coordinate system of the mobile body 10 based on the detection result of the same detected object A detected by the second external sensor 26A2 when the mobile body 10 is stopped and the estimated value of the movement parameter Φ. That is, the determination unit 48 calculates the position of detected object A in the reference coordinate system of the mobile body 10 from the position of detected object A in the coordinate system of the second external sensor 26A2 indicated by the detection result of detected object A (the position of detected object A relative to the second external sensor 26A2) and the position of the second external sensor 26A2 in the reference coordinate system of the mobile body 10 indicated by the estimated value of the movement parameter Φ (the position of the second external sensor 26A2 relative to the reference point P of the mobile body 10). The position of the detected object A in the reference coordinate system of the mobile object 10 calculated in this way from the detection results of the second external sensor 26A2 will hereinafter be referred to as the second detected position, as appropriate.

このように、第１検出位置と第２検出位置とは、異なる外界センサ２６Ａにより検出された同じ検出体Ａの位置を指す。同じ検出体Ａであるかは任意の方法で判定できるが、例えば外界センサ２６ＡがＬｉＤＡＲの場合、判定部４８は、スキャンマッチングを行うことで、第１外界センサ２６Ａ１が検出した点群と第２外界センサ２６Ａ２が検出した点群のうちから、同一の対象を検出した点群（検出点）を抽出する。そして、判定部４８は、抽出した第１外界センサ２６Ａ１の点群の位置を、第１外界センサ２６Ａ１の座標系における検出体Ａの位置とし、抽出した第２外界センサ２６Ａ２の点群の位置を、第２外界センサ２６Ａ２の座標系における検出体Ａの位置とする。また例えば、外界センサ２６Ａがカメラの場合、判定部４８は、画像認識によって、第１外界センサ２６Ａ１が撮像した画像と第２外界センサ２６Ａ２が撮像した画像のうちから、同じ対象物が写っている領域（画素群）を特定する。そして、判定部４８は、第１外界センサ２６Ａ１の撮像画像から特定した領域の位置を、第１外界センサ２６Ａ１の座標系における検出体Ａの位置とし、第２外界センサ２６Ａ２の撮像画像から特定した領域の位置を、第２外界センサ２６Ａ２の座標系における検出体Ａの位置とする。 As such, the first detection position and the second detection position refer to the position of the same detected object A detected by different external sensors 26A. Whether the detected object A is the same can be determined using any method. For example, if external sensor 26A is LiDAR, the determination unit 48 performs scan matching to extract point clouds (detected points) that detect the same object from the point clouds detected by first external sensor 26A1 and second external sensor 26A2. The determination unit 48 then determines the position of the extracted point cloud of first external sensor 26A1 as the position of detected object A in the coordinate system of first external sensor 26A1, and the position of the extracted point cloud of second external sensor 26A2 as the position of detected object A in the coordinate system of second external sensor 26A2. For example, if the external sensor 26A is a camera, the determination unit 48 uses image recognition to identify areas (pixel groups) in which the same object is captured from the images captured by the first external sensor 26A1 and the second external sensor 26A2. The determination unit 48 then determines the position of the area identified from the image captured by the first external sensor 26A1 as the position of detected object A in the coordinate system of the first external sensor 26A1, and the position of the area identified from the image captured by the second external sensor 26A2 as the position of detected object A in the coordinate system of the second external sensor 26A2.

判定部４８は、第１検出位置と第２検出位置とに基づいて、移動パラメータΦの推定値の妥当性を判断する。すなわち、理想的には、第１検出位置と第２検出位置との差分（誤差）はゼロであるが、移動パラメータΦの推定値の精度が低い場合には、第１検出位置と第２検出位置との差分が大きくなる。従って、判定部４８は、第１検出位置と第２検出位置とのずれ量が所定値以下である場合には、言い換えれば、第１検出位置と第２検出位置との間の距離が所定距離以下である場合には、移動パラメータΦの推定値が妥当であると判断する。一方、判定部４８は、第１検出位置と第２検出位置とのずれ量が所定値より大きい場合には、移動パラメータΦの推定値が妥当でないと判断する。 The determination unit 48 determines the validity of the estimated value of the movement parameter Φ based on the first and second detected positions. Ideally, the difference (error) between the first and second detected positions is zero, but if the accuracy of the estimated value of the movement parameter Φ is low, the difference between the first and second detected positions becomes large. Therefore, if the deviation between the first and second detected positions is less than a predetermined value, in other words, if the distance between the first and second detected positions is less than a predetermined distance, the determination unit 48 determines that the estimated value of the movement parameter Φ is valid. On the other hand, if the deviation between the first and second detected positions is greater than the predetermined value, the determination unit 48 determines that the estimated value of the movement parameter Φ is invalid.

次に、妥当性の判定方法の他の例を説明する図５は、移動パラメータの推定値の妥当性の判定方法の例を説明する模式図である。本例の場合、図５に示すように、判定部４８は、移動体１０の停止時において、第１外界センサ２６Ａ１に、異なる位置にある複数の検出体Ａを検出させる。なお、検出体Ａの数は複数であれば任意であってよい。判定部４８は、第１外界センサ２６Ａ１による、複数の検出体Ａの検出結果を取得して、複数の検出体Ａの検出結果に基づいて、第１外界センサ２６Ａ１の位置を算出する。複数の検出体Ａの検出結果からの第１外界センサ２６Ａ１の位置の算出方法は任意であるが、例えば、判定部４８は、複数の検出体Ａの位置から、三角測量の手法を用いて、第１外界センサ２６Ａ１の位置を算出してよい。同様に、判定部４８は、移動体１０の停止時において、第２外界センサ２６Ａ２に、異なる位置にある複数の検出体Ａを検出させる。判定部４８は、第２外界センサ２６Ａ２による、複数の検出体Ａの検出結果を取得して、複数の検出体Ａの検出結果に基づいて、第２外界センサ２６Ａ２の位置を算出する。なお、本例では、第１外界センサ２６Ａ１と第２外界センサ２６Ａ２とは、同じ検出体Ａを検出することが好ましいが、それに限られず、異なる検出体Ａを検出してもよい。 Next, FIG. 5, which illustrates another example of a validity determination method, is a schematic diagram illustrating an example of a method for determining the validity of estimated values of movement parameters. In this example, as shown in FIG. 5, the determination unit 48 causes the first external sensor 26A1 to detect multiple detected objects A at different positions when the mobile object 10 is stopped. Note that any number of detected objects A may be used. The determination unit 48 acquires the detection results of the multiple detected objects A by the first external sensor 26A1 and calculates the position of the first external sensor 26A1 based on the detection results of the multiple detected objects A. Any method may be used to calculate the position of the first external sensor 26A1 from the detection results of the multiple detected objects A. For example, the determination unit 48 may calculate the position of the first external sensor 26A1 from the positions of the multiple detected objects A using a triangulation technique. Similarly, the determination unit 48 causes the second external sensor 26A2 to detect multiple detected objects A at different positions when the mobile object 10 is stopped. The determination unit 48 acquires the detection results of multiple detected objects A by the second external sensor 26A2 and calculates the position of the second external sensor 26A2 based on the detection results of the multiple detected objects A. In this example, it is preferable that the first external sensor 26A1 and the second external sensor 26A2 detect the same detected object A, but this is not limited thereto and they may detect different detected objects A.

判定部４８は、複数の検出体Ａの検出結果から算出した第１外界センサ２６Ａ１の位置と、複数の検出体Ａの検出結果から算出した第２外界センサ２６Ａ２の位置とに基づき、第１外界センサ２６Ａ１に対する第２外界センサ２６Ａ２の位置を算出する。以下、このようにして算出した第１外界センサ２６Ａ１に対する第２外界センサ２６Ａ２の位置を、適宜、第１センサ位置と記載する。また、判定部４８は、移動パラメータΦの推定値から、第１外界センサ２６Ａ１に対する第２外界センサ２６Ａ２の位置を算出する。具体的には、判定部４８は、移動パラメータΦの推定値で示される移動体１０の基準座標系における第１外界センサ２６Ａ１の位置と、移動パラメータΦの推定値で示される移動体１０の基準座標系における第２外界センサ２６Ａ２の位置とから、第１外界センサ２６Ａ１に対する第２外界センサ２６Ａ２の位置を算出する。以下、このようにして算出した第１外界センサ２６Ａ１に対する第２外界センサ２６Ａ２の位置を、適宜、第２センサ位置と記載する。 The determination unit 48 calculates the position of the second external sensor 26A2 relative to the first external sensor 26A1 based on the position of the first external sensor 26A1 calculated from the detection results of multiple detected objects A and the position of the second external sensor 26A2 calculated from the detection results of multiple detected objects A. Hereinafter, the position of the second external sensor 26A2 relative to the first external sensor 26A1 calculated in this manner will be appropriately referred to as the "first sensor position." The determination unit 48 also calculates the position of the second external sensor 26A2 relative to the first external sensor 26A1 from the estimated value of the movement parameter Φ. Specifically, the determination unit 48 calculates the position of the second external sensor 26A2 relative to the first external sensor 26A1 from the position of the first external sensor 26A1 in the reference coordinate system of the mobile object 10 indicated by the estimated value of the movement parameter Φ and the position of the second external sensor 26A2 in the reference coordinate system of the mobile object 10 indicated by the estimated value of the movement parameter Φ. Hereinafter, the position of the second external sensor 26A2 relative to the first external sensor 26A1 calculated in this manner will be referred to as the second sensor position, where appropriate.

判定部４８は、第１センサ位置と第２センサ位置とに基づいて、移動パラメータΦの推定値の妥当性を判断する。すなわち、理想的には、第１センサ位置と第２センサ位置の差分（誤差）はゼロであるが、移動パラメータΦの推定値の精度が低い場合には、第１センサ位置と第２センサ位置との差分が大きくなる。従って、判定部４８は、第１センサ位置と第２センサ位置とのずれ量が所定値以下である場合には、言い換えれば、第１センサ位置と第２センサ位置との間の距離が所定距離以下である場合には、移動パラメータΦの推定値が妥当であると判断する。一方、判定部４８は、第１センサ位置と第２センサ位置とのずれ量が所定値より大きい場合には、移動パラメータΦの推定値が妥当でないと判断する。 The determination unit 48 determines the validity of the estimated value of the movement parameter Φ based on the first sensor position and the second sensor position. That is, ideally, the difference (error) between the first sensor position and the second sensor position is zero, but if the accuracy of the estimated value of the movement parameter Φ is low, the difference between the first sensor position and the second sensor position becomes large. Therefore, if the deviation between the first sensor position and the second sensor position is less than a predetermined value, in other words, if the distance between the first sensor position and the second sensor position is less than a predetermined distance, the determination unit 48 determines that the estimated value of the movement parameter Φ is valid. On the other hand, if the deviation between the first sensor position and the second sensor position is greater than the predetermined value, the determination unit 48 determines that the estimated value of the movement parameter Φ is invalid.

（パラメータ設定部）
パラメータ設定部５０は、判定部４８によって妥当であると判断された移動パラメータΦの推定値を、移動パラメータΦの値として設定する。すなわち、パラメータ設定部５０は、判定部４８によって妥当であると判断された移動パラメータΦの推定値を、今後の移動体１０の制御に用いる正式な移動パラメータΦとして設定する。言い換えれば、パラメータ設定部５０は、移動パラメータΦを更新するキャリブレーション処理を行うといえる。移動制御部４０は、パラメータ設定部５０によって設定された移動パラメータΦ（妥当とされた移動パラメータΦの推定値）に基づいて、移動体１０の移動を制御する。すなわち、移動制御部４０は、パラメータ設定部５０によって設定された移動パラメータΦを入力値として演算を実行することで、移動体１０の移動を制御する。 (Parameter setting section)
The parameter setting unit 50 sets the estimated value of the movement parameter Φ determined to be valid by the determination unit 48 as the value of the movement parameter Φ. That is, the parameter setting unit 50 sets the estimated value of the movement parameter Φ determined to be valid by the determination unit 48 as the official movement parameter Φ to be used for future control of the moving body 10. In other words, the parameter setting unit 50 performs a calibration process to update the movement parameter Φ. The movement control unit 40 controls the movement of the moving body 10 based on the movement parameter Φ set by the parameter setting unit 50 (the estimated value of the movement parameter Φ determined to be valid). That is, the movement control unit 40 controls the movement of the moving body 10 by performing a calculation using the movement parameter Φ set by the parameter setting unit 50 as an input value.

（処理フロー）
次に、以上説明した移動パラメータΦの設定の処理フローを説明する。図６は、本実施形態に係る移動パラメータの設定の処理フローを説明するフローチャートである。図６に示すように、制御装置２８は、第１位置算出部４２により、所定時間毎の外界センサ２６Ａの検出結果に基づき、所定時間毎の移動体１０の第１推定位置を算出し（ステップＳ１０）、第２位置算出部４４により、所定時間毎の内界センサ２６Ｂの検出結果に基づき、所定時間毎の移動体１０の第２推定位置を算出する（ステップＳ１２）。制御装置２８は、パラメータ算出部４６により、第１推定位置及び第２推定位置に基づいて、移動パラメータΦの推定値を算出する（ステップＳ１４）。本実施形態では、パラメータ算出部４６は、所定時間毎の第１推定位置と第２推定位置との差分が最小になるように、移動パラメータΦの推定値を算出する。制御装置２８は、判定部４８により、移動体１０の停止時における外界センサ２６Ａの検出結果に基づいて、移動パラメータΦの推定値が妥当であるかを判断し（ステップＳ１６）、妥当である場合（ステップＳ１６；Ｙｅｓ）、パラメータ設定部５０により、その移動パラメータΦの推定値を、正式な移動パラメータΦとして設定する（ステップＳ１８）。一方、移動パラメータΦの推定値が妥当でない場合（ステップＳ１６；Ｎｏ）、その移動パラメータΦの推定値を正式な移動パラメータΦとして設定しない（ステップＳ２０）。なお、移動パラメータΦの推定値を採用しない場合には、以降の移動においては、これまでに設定されていた移動パラメータΦの値を用いてよい。その後、外界センサ２６Ａや内界センサ２６Ｂから、以降の移動における検出結果を取得したら、同様の処理を実行して、移動パラメータΦの推定値の算出と妥当性判断とを行ってよい。 (Processing flow)
Next, a process flow for setting the movement parameter Φ described above will be described. FIG. 6 is a flowchart illustrating a process flow for setting the movement parameter according to this embodiment. As shown in FIG. 6 , the control device 28 uses the first position calculation unit 42 to calculate a first estimated position of the moving object 10 for each predetermined time period based on the detection results of the external sensor 26A for each predetermined time period (step S10), and the second position calculation unit 44 to calculate a second estimated position of the moving object 10 for each predetermined time period based on the detection results of the internal sensor 26B for each predetermined time period (step S12). The control device 28 uses the parameter calculation unit 46 to calculate an estimated value of the movement parameter Φ based on the first estimated position and the second estimated position (step S14). In this embodiment, the parameter calculation unit 46 calculates the estimated value of the movement parameter Φ so as to minimize the difference between the first estimated position and the second estimated position for each predetermined time period. The control device 28 determines whether the estimated value of the movement parameter Φ is valid based on the detection results of the external sensor 26A when the moving object 10 is stopped (step S16) using the determination unit 48. If the estimated value of the movement parameter Φ is valid (step S16; Yes), the parameter setting unit 50 sets the estimated value of the movement parameter Φ as the official movement parameter Φ (step S18). On the other hand, if the estimated value of the movement parameter Φ is invalid (step S16; No), the estimated value of the movement parameter Φ is not set as the official movement parameter Φ (step S20). Note that if the estimated value of the movement parameter Φ is not adopted, the previously set value of the movement parameter Φ may be used for subsequent movements. Thereafter, when detection results for subsequent movements are obtained from the external sensor 26A or the internal sensor 26B, similar processing may be performed to calculate the estimated value of the movement parameter Φ and determine its validity.

（効果）
ここで、移動体１０を適切に制御するために、移動体１０の移動量を推定するための移動パラメータΦを高精度に算出することが求められている。移動パラメータΦは、定期メンテナンス時に計測されて更新される場合もあるが、経時的に値が変化する場合もあるため、定期メンテナンスまでの間に値が変わってしまう場合がある。それに対して、本実施形態においては、移動体１０が移動中の外界センサ２６Ａと内界センサ２６Ｂとの検出結果から、移動パラメータΦの推定値を算出するため、定期メンテナンスを待たずに移動パラメータΦを更新することができ、移動パラメータΦを高精度に算出できるといえる。さらに言えば、このようにして算出した移動パラメータΦは、局所最適解に陥ったり、検出タイミング（タイムスタンプ）の誤差が生じたりすることなどにより、精度が低下することも懸念される。それに対して、本実施形態においては、停止時における外界センサ２６Ａの検出結果から、移動パラメータΦの妥当性を判断して、妥当であるものを採用する。従って、局所最適解への収束を検知し、かつ、タイムスタンプの誤差を抑制できるので、移動パラメータΦをより高精度に算出でき、キャリブレーションを適切に実行することが可能となる。 (effect)
In order to appropriately control the mobile object 10, it is necessary to accurately calculate the movement parameter Φ for estimating the movement amount of the mobile object 10. The movement parameter Φ may be measured and updated during scheduled maintenance, but its value may also change over time, resulting in a change in its value between scheduled maintenance and scheduled maintenance. In contrast, in this embodiment, an estimated value of the movement parameter Φ is calculated from the detection results of the external sensor 26A and the internal sensor 26B while the mobile object 10 is moving. This allows the movement parameter Φ to be updated without waiting for scheduled maintenance, and the movement parameter Φ can be calculated with high accuracy. Furthermore, there is a concern that the accuracy of the movement parameter Φ calculated in this manner may decrease due to factors such as falling into a local optimum solution or errors in the detection timing (timestamp). In contrast, in this embodiment, the validity of the movement parameter Φ is determined based on the detection results of the external sensor 26A when the mobile object 10 is stopped, and the valid one is adopted. Therefore, by detecting convergence to a local optimum solution and suppressing timestamp errors, the movement parameter Φ can be calculated with higher accuracy, enabling appropriate calibration.

（他の例）
図７は、第１実施形態の他の例に係る演算装置の模式的なブロック図である。以上の説明では、制御装置２８が、移動パラメータを設定する演算装置と、移動体１０の移動を制御する制御装置との両方の機能を兼ね備えていた。ただし、移動パラメータを設定する演算装置と、移動体１０の移動を制御する制御装置とが別体であってよい。この場合、図７に示すように、移動体１０に搭載されない演算装置２８Ａが、第１位置算出部４２と第２位置算出部４４とパラメータ算出部４６と判定部４８とパラメータ設定部５０とを含み、それらの処理を実行する。この場合、演算装置２８Ａは、通信部３０を介して、移動体１０から、外界センサ２６Ａ及び内界センサ２６Ｂの検出結果を取得して、それらの検出結果から、移動パラメータΦの推定値の設定と妥当性の判断を行う。そして、演算装置２８Ａは、妥当であると判断された移動パラメータΦの推定値を、正式な移動パラメータΦとして移動体１０に送信する。 (Other examples)
FIG. 7 is a schematic block diagram of a calculation device according to another example of the first embodiment. In the above description, the control device 28 functions as both a calculation device that sets movement parameters and a control device that controls the movement of the mobile body 10. However, the calculation device that sets movement parameters and the control device that controls the movement of the mobile body 10 may be separate entities. In this case, as shown in FIG. 7 , a calculation device 28A not mounted on the mobile body 10 includes a first position calculation unit 42, a second position calculation unit 44, a parameter calculation unit 46, a determination unit 48, and a parameter setting unit 50, and executes these processes. In this case, the calculation device 28A acquires detection results from the external sensor 26A and the internal sensor 26B from the mobile body 10 via the communication unit 30, and sets an estimated value of the movement parameter Φ and determines its validity based on these detection results. The calculation device 28A then transmits the estimated value of the movement parameter Φ that is determined to be valid to the mobile body 10 as the official movement parameter Φ.

（第２実施形態）
次に、第２実施形態について説明する。第２実施形態においては、内界センサ２６Ｂのバイアス値を算出して、それに基づいて移動パラメータΦを算出する点で、第１実施形態と異なる。第２実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。 Second Embodiment
Next, a second embodiment will be described. The second embodiment differs from the first embodiment in that the bias value of the internal sensor 26B is calculated and the movement parameter Φ is calculated based on the calculated bias value. In the second embodiment, the description of the parts of the configuration common to the first embodiment will be omitted.

図８は、バイアス値の一例を示すグラフである。第２実施形態においては、パラメータ算出部４６は、移動体１０の停止時に、内界センサ２６Ｂに出力パラメータを検出させる。そして、パラメータ算出部４６は、移動体１０の停止時における内界センサ２６Ｂの検出結果（出力パラメータ）に基づいて、内界センサ２６Ｂの検出結果のバイアス値を算出する。バイアス値とは、内界センサ２６Ｂの検出結果の、正常値からのずれ量を示す。パラメータ算出部４６は、移動体１０の停止時における内界センサ２６Ｂの検出結果に基づいた任意の方法でバイアス値を算出してよいが、例えば、移動体１０の停止時における正常値を予め設定しておき、停止時における内界センサ２６Ｂの検出結果（出力パラメータ）と正常値との差分から、バイアス値を算出してよい。この場合例えば、図８の例では、時間帯Δｔが、移動体１０が停止している時間帯を示している。この場合、パラメータ算出部４６は、時間帯Δｔにおける内界センサ２６Ｂによる出力パラメータの検出値と、時間帯Δｔにおける出力パラメータの正常値（例えばゼロ）との差分ΔＢを算出し、その差分ΔＢを、バイアス値としてよい。 Figure 8 is a graph showing an example of a bias value. In the second embodiment, the parameter calculation unit 46 causes the internal sensor 26B to detect an output parameter when the mobile body 10 is stopped. The parameter calculation unit 46 then calculates a bias value for the detection result of the internal sensor 26B based on the detection result (output parameter) of the internal sensor 26B when the mobile body 10 is stopped. The bias value indicates the amount of deviation of the detection result of the internal sensor 26B from a normal value. The parameter calculation unit 46 may calculate the bias value using any method based on the detection result of the internal sensor 26B when the mobile body 10 is stopped. For example, the parameter calculation unit 46 may set a normal value when the mobile body 10 is stopped in advance and calculate the bias value from the difference between the detection result (output parameter) of the internal sensor 26B when the mobile body 10 is stopped and the normal value. In this case, for example, in the example of Figure 8, the time period Δt indicates the time period when the mobile body 10 is stopped. In this case, the parameter calculation unit 46 may calculate the difference ΔB between the output parameter detected by the internal sensor 26B in the time period Δt and the normal value of the output parameter (e.g., zero) in the time period Δt, and use this difference ΔB as the bias value.

パラメータ算出部４６は、第１推定位置及び第２推定位置に加えて、このようにして算出したバイアス値にも基づいて、移動パラメータΦの推定値を算出する。例えば第１実施形態においては、パラメータ算出部４６は、内界センサ２６Ｂのバイアス値を、算出対象となる移動パラメータΦに含めて、第１推定位置及び第２推定位置により、内界センサ２６Ｂのバイアス値も、移動パラメータΦとして算出することが可能である。それに対して、第２実施形態においては、移動パラメータΦの推定値の算出の前に、予め内界センサ２６Ｂのバイアス値を固定値として設定しておき、第１推定位置及び第２推定位置と、固定値として設定した内界センサ２６Ｂのバイアス値とに基づいて、第１実施形態と同様の方法で、内界センサ２６Ｂのバイアス値以外の移動パラメータΦについての、推定値を算出する。これにより、算出対象となる変数を減らすことができるので、移動パラメータΦの推定値の演算負荷を低減できる。 The parameter calculation unit 46 calculates an estimated value of the movement parameter Φ based on the bias value calculated in this manner, in addition to the first and second estimated positions. For example, in the first embodiment, the parameter calculation unit 46 includes the bias value of the internal sensor 26B in the movement parameter Φ to be calculated, and is able to calculate the bias value of the internal sensor 26B as a movement parameter Φ using the first and second estimated positions. In contrast, in the second embodiment, before calculating the estimated value of the movement parameter Φ, the bias value of the internal sensor 26B is set as a fixed value in advance, and estimates of the movement parameters Φ other than the bias value of the internal sensor 26B are calculated using a method similar to that of the first embodiment based on the first and second estimated positions and the bias value of the internal sensor 26B set as a fixed value. This reduces the number of variables to be calculated, thereby reducing the computational load of the estimated value of the movement parameter Φ.

なお、内界センサ２６Ｂのバイアス値は、例えば、ＩＭＵの測定結果のバイアス値を指してよい。 Note that the bias value of the internal sensor 26B may refer to, for example, the bias value of the IMU measurement results.

（第３実施形態）
次に、第３実施形態について説明する。第３実施形態においては、停止時における外界センサ２６Ａの検出結果から妥当であると判断され、かつ、過去における移動パラメータΦの推定値に対して所定条件を満たす場合に、移動パラメータΦの推定値が妥当であるかを判断する点で、第１実施形態とは異なる。第２実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。なお、第３実施形態は、第２実施形態にも適用可能である。 (Third embodiment)
Next, a third embodiment will be described. The third embodiment differs from the first embodiment in that it determines whether the estimated value of the movement parameter Φ is valid when it is determined to be valid from the detection result of the external sensor 26A when the vehicle is stopped and when a predetermined condition is satisfied for the estimated value of the movement parameter Φ in the past. In the second embodiment, a description of parts that are common to the first embodiment will be omitted. Note that the third embodiment can also be applied to the second embodiment.

第３実施形態においては、第１実施形態などで説明した移動パラメータΦの推定値の算出処理と、推定値の妥当性の判断とを、繰り返す。以降においては、第１時間帯におけるセンサの検出結果から、第１時間帯における移動パラメータΦの推定値を算出し、第１時間帯よりも後の第２時間帯におけるセンサの検出結果から、第２時間帯における移動パラメータΦの推定値を算出する場合を例に説明する。 In the third embodiment, the process of calculating the estimated value of the movement parameter Φ and determining the validity of the estimated value, as described in the first embodiment and other embodiments, are repeated. Hereinafter, an example will be described in which an estimated value of the movement parameter Φ for a first time period is calculated from the sensor detection results for that time period, and an estimated value of the movement parameter Φ for a second time period that follows the first time period is calculated from the sensor detection results for that time period.

第３実施形態においては、第１位置算出部４２は、移動体１０が移動している第１時間帯において検出された、外界センサ２６Ａの検出結果を取得して、第１時間帯での外界センサ２６Ａの検出結果に基づいて、第１時間帯での第１推定位置を算出する。また、第２位置算出部４４は、第１時間帯において検出された内界センサ２６Ｂの検出結果を取得して、第１時間帯での内界センサ２６Ｂの検出結果に基づいて、第１時間帯での第２推定位置を算出する。そして、パラメータ算出部４６は、第１時間帯での第１推定位置と、第１時間帯での第２推定位置とに基づき、第１時間帯での移動パラメータΦの推定値を算出する。判定部４８は、停止時の外界センサ２６Ａの検出結果に基づき、第１時間帯での移動パラメータΦの推定値の仮の妥当性を判断する。仮の妥当性の判定方法は、第１実施形態の妥当性の判断方法と同様である。 In the third embodiment, the first position calculation unit 42 acquires the detection results of the external sensor 26A detected during a first time period when the mobile object 10 is moving, and calculates a first estimated position for the first time period based on the detection results of the external sensor 26A during the first time period. The second position calculation unit 44 acquires the detection results of the internal sensor 26B detected during the first time period, and calculates a second estimated position for the first time period based on the detection results of the internal sensor 26B during the first time period. The parameter calculation unit 46 then calculates an estimated value of the movement parameter Φ for the first time period based on the first estimated position for the first time period and the second estimated position for the first time period. The determination unit 48 determines the provisional validity of the estimated value of the movement parameter Φ for the first time period based on the detection results of the external sensor 26A when the mobile object 10 is stopped. The method for determining the provisional validity is the same as the method for determining the validity in the first embodiment.

その後、第３実施形態においては、第１位置算出部４２は、第２時間帯において検出された外界センサ２６Ａの検出結果を取得して、第２時間帯での外界センサ２６Ａの検出結果に基づいて、第２時間帯での第１推定位置を算出する。なお、第２時間帯は、第１時間帯より後であって移動体１０が移動した時間帯である。第２位置算出部４４は、第２時間帯において検出された内界センサ２６Ｂの検出結果を取得して、第２時間帯での内界センサ２６Ｂの検出結果に基づいて、第２時間帯での第２推定位置を算出する。そして、パラメータ算出部４６は、第２時間帯での第１推定位置と、第２時間帯での第２推定位置とに基づき、第２時間帯での移動パラメータΦの推定値を算出する。判定部４８は、停止時の外界センサ２６Ａの検出結果に基づき、第２時間帯での移動パラメータΦの推定値の仮の妥当性を判断する。 In the third embodiment, the first position calculation unit 42 then acquires the detection results of the external sensor 26A detected during the second time period and calculates a first estimated position for the second time period based on the detection results of the external sensor 26A during the second time period. The second time period is a time period that follows the first time period and during which the mobile object 10 moves. The second position calculation unit 44 acquires the detection results of the internal sensor 26B detected during the second time period and calculates a second estimated position for the second time period based on the detection results of the internal sensor 26B during the second time period. The parameter calculation unit 46 then calculates an estimated value of the movement parameter Φ for the second time period based on the first estimated position for the second time period and the second estimated position for the second time period. The determination unit 48 determines the validity of the provisional estimated value of the movement parameter Φ for the second time period based on the detection results of the external sensor 26A when the mobile object 10 was stopped.

さらに、判定部４８は、複数の外界センサ２６Ａの検出結果に基づき、第２時間帯での移動パラメータΦの推定値が仮の妥当ありと判断された場合には、その第２時間帯での移動パラメータΦの推定値と、仮の妥当ありと判断された第１時間帯での移動パラメータΦの推定値とを比較する。具体的には、判定部４８は、第２時間帯での移動パラメータΦの推定値が、第１時間帯での移動パラメータΦの推定値に対して所定条件を満たすかを判断する。例えば、移動パラメータΦは経時により変化するため、判定部４８は、移動パラメータΦの単位時間当たりの許容される変化量の範囲を予め設定しておく。そして、判定部４８は、単位時間当たりの許容される変化量の範囲から、第１時間帯から第２時間帯における許容変化量の範囲を算出して、第１時間帯での移動パラメータΦの推定値に対する、第２時間帯での移動パラメータΦの推定値の変化量が、許容変化量の範囲内にあるかを判断する。判定部４８は、第２時間帯での移動パラメータΦの推定値の変化量が、許容変化量の範囲内にある場合に、所定条件を満たすと判断し、第２時間帯での移動パラメータΦの推定値の変化量が、許容変化量の範囲内にない場合に、所定条件を満たさないと判断する。 Furthermore, if the determination unit 48 determines, based on the detection results of the multiple external sensors 26A, that the estimated value of the movement parameter Φ for the second time period is provisionally valid, it compares the estimated value of the movement parameter Φ for the second time period with the estimated value of the movement parameter Φ for the first time period that was provisionally determined to be valid. Specifically, the determination unit 48 determines whether the estimated value of the movement parameter Φ for the second time period satisfies a predetermined condition with respect to the estimated value of the movement parameter Φ for the first time period. For example, because the movement parameter Φ changes over time, the determination unit 48 pre-sets a range of allowable change per unit time for the movement parameter Φ. Then, the determination unit 48 calculates the range of allowable change from the first time period to the second time period from the range of allowable change per unit time, and determines whether the change in the estimated value of the movement parameter Φ for the second time period relative to the estimated value of the movement parameter Φ for the first time period is within the range of allowable change. The determination unit 48 determines that the predetermined condition is met if the amount of change in the estimated value of the movement parameter Φ during the second time period is within the range of the allowable change amount, and determines that the predetermined condition is not met if the amount of change in the estimated value of the movement parameter Φ during the second time period is not within the range of the allowable change amount.

判定部４８は、第２時間帯での移動パラメータΦの推定値が、第１時間帯での移動パラメータΦの推定値に対して所定条件を満たすと判断した場合に、第２時間帯での移動パラメータΦの推定値が妥当であると最終判定する。パラメータ設定部５０は、妥当であると判断された第２時間帯での移動パラメータΦの推定値を、移動パラメータΦとして正式に設定する。一方、判定部４８は、第２時間帯での移動パラメータΦの推定値が、第１時間帯での移動パラメータΦの推定値に対して所定条件を満たさないと判断した場合には、第２時間帯での移動パラメータΦの推定値が妥当ではないと判定する。この場合、パラメータ設定部５０は、第２時間帯での移動パラメータΦを採用しない。 If the determination unit 48 determines that the estimated value of the movement parameter Φ in the second time period satisfies a predetermined condition with respect to the estimated value of the movement parameter Φ in the first time period, it makes a final determination that the estimated value of the movement parameter Φ in the second time period is valid. The parameter setting unit 50 officially sets the estimated value of the movement parameter Φ in the second time period that is determined to be valid as the movement parameter Φ. On the other hand, if the determination unit 48 determines that the estimated value of the movement parameter Φ in the second time period does not satisfy a predetermined condition with respect to the estimated value of the movement parameter Φ in the first time period, it determines that the estimated value of the movement parameter Φ in the second time period is invalid. In this case, the parameter setting unit 50 does not adopt the movement parameter Φ in the second time period.

以上説明した第３実施形態における移動パラメータΦの設定の処理フローを説明する。図９は、第３実施形態における移動パラメータの設定の処理フローを説明するフローチャートである。なお、図９では、第１時間帯での移動パラメータΦの推定値の算出及び仮の妥当性の処理については省略している。図９に示すように、第３実施形態においては、第１位置算出部４２及び第２位置算出部４４により、第２時間帯における第１推定位置及び第２推定位置が算出され、パラメータ算出部４６は、第２時間帯における第１推定位置及び第２推定位置に基づき、第２時間帯での移動パラメータΦの推定値を算出する（ステップＳ１４Ａ）。そして、判定部４８は、移動体１０の停止時における外界センサ２６Ａの検出結果に基づいて、第２時間帯での移動パラメータΦの推定値の仮の妥当性を判断し（ステップＳ１６Ａ）、仮に妥当である場合（ステップＳ１６Ａ；Ｙｅｓ）、第２時間帯での移動パラメータΦの推定値が、第１時間帯での移動パラメータΦの推定値に対して所定条件を満たすかを判定する（ステップＳ１７Ａ）。所定条件を満たす場合（ステップＳ１７Ａ；Ｙｅｓ）、判定部４８は、第２時間帯での移動パラメータΦの推定値が妥当と判断して、パラメータ設定部５０により、第２時間帯での移動パラメータΦの推定値を、正式な移動パラメータΦとして設定する（ステップＳ１８Ａ）。一方、第２時間帯での移動パラメータΦの推定値に仮妥当性がない場合（ステップＳ１６Ａ；Ｎｏ）、及び第２時間帯での移動パラメータΦの推定値が所定条件を満たさない場合（ステップＳ１７Ａ；Ｎｏ）、第２時間帯での移動パラメータΦの推定値を正式な移動パラメータΦとして設定しない（ステップＳ２０Ａ）。 The process flow for setting the movement parameter Φ in the third embodiment described above will now be described. Figure 9 is a flowchart illustrating the process flow for setting the movement parameter Φ in the third embodiment. Note that Figure 9 omits the calculation of the estimated value of the movement parameter Φ for the first time period and the process for determining the provisional validity. As shown in Figure 9 , in the third embodiment, the first position calculation unit 42 and the second position calculation unit 44 calculate the first and second estimated positions for the second time period, and the parameter calculation unit 46 calculates the estimated value of the movement parameter Φ for the second time period based on the first and second estimated positions for the second time period (step S14A). Then, the determination unit 48 determines the provisional validity of the estimated value of the movement parameter Φ for the second time period based on the detection results of the external sensor 26A when the mobile object 10 is stopped (step S16A). If the provisional validity is found (step S16A; Yes), the determination unit 48 determines whether the estimated value of the movement parameter Φ for the second time period satisfies a predetermined condition with respect to the estimated value of the movement parameter Φ for the first time period (step S17A). If the predetermined condition is met (Step S17A; Yes), the determination unit 48 determines that the estimated value of the movement parameter Φ for the second time period is valid, and the parameter setting unit 50 sets the estimated value of the movement parameter Φ for the second time period as the official movement parameter Φ (Step S18A). On the other hand, if the provisional estimated value of the movement parameter Φ for the second time period is not valid (Step S16A; No), or if the estimated value of the movement parameter Φ for the second time period does not meet the predetermined condition (Step S17A; No), the estimated value of the movement parameter Φ for the second time period is not set as the official movement parameter Φ (Step S20A).

このように、第３実施形態においては、過去の移動パラメータΦの推定値と比較して妥当でないとされた場合には、移動パラメータΦの推定値を正式な値として採用しない。そのため、移動パラメータΦの算出精度をより好適に向上させることができる。 In this way, in the third embodiment, if the estimated value of the movement parameter Φ is determined to be inappropriate when compared with past estimated values of the movement parameter Φ, the estimated value is not adopted as the official value. This makes it possible to more effectively improve the calculation accuracy of the movement parameter Φ.

なお、以上の説明では、第３実施形態においては、停止時の外界センサ２６Ａの検出結果による仮の妥当性の判定と、過去の移動パラメータΦの推定値との比較による判定との、２段階の判定を行っていた。ただしそれに限られず、第３実施形態においては、停止時の外界センサ２６Ａの検出結果による仮の妥当性の判定を行わなくてよい。すなわちこの場合、判定部４８は、停止時の外界センサ２６Ａの検出結果による仮の妥当性を行わずに、第２時間帯での移動パラメータΦの推定値と第１時間帯での移動パラメータΦの推定値とを比較して、第２時間帯での移動パラメータΦの妥当性を判定してよい。 In the above description, in the third embodiment, a two-stage determination is made: determining the provisional validity based on the detection results of the external sensor 26A when stopped, and determining the validity based on a comparison with past estimated values of the movement parameter Φ. However, this is not limited to this, and in the third embodiment, it is not necessary to determine the provisional validity based on the detection results of the external sensor 26A when stopped. That is, in this case, the determination unit 48 may compare the estimated value of the movement parameter Φ in the second time period with the estimated value of the movement parameter Φ in the first time period to determine the validity of the movement parameter Φ in the second time period, without determining the provisional validity based on the detection results of the external sensor 26A when stopped.

（効果）
以上説明したように、本開示に係る演算装置は、第１位置算出部４２と、第２位置算出部４４と、パラメータ算出部４６と、判定部４８と、パラメータ設定部５０とを含む。第１位置算出部４２は、移動体１０に搭載されて移動体１０の周囲を検出する外界センサ２６Ａの検出結果に基づいて、移動体の第１推定位置を算出する。第２位置算出部４４は、移動体１０に搭載されて移動体１０の制御出力を示す出力パラメータを検出する内界センサ２６Ｂの検出結果に基づいて、移動体１０の第２推定位置を算出する。パラメータ算出部４６は、第１推定位置及び第２推定位置に基づいて、移動体１０の移動量を算出するための移動パラメータΦの推定値を算出する。判定部４８は、移動体１０の停止時における外界センサ２６Ａの検出結果に基づいて、移動パラメータΦの推定値の妥当性を判定する。パラメータ設定部５０は、妥当であると判断された移動パラメータΦの推定値を、移動体１０の移動パラメータとして設定する。 (effect)
As described above, the computing device according to the present disclosure includes a first position calculation unit 42, a second position calculation unit 44, a parameter calculation unit 46, a determination unit 48, and a parameter setting unit 50. The first position calculation unit 42 calculates a first estimated position of the moving body 10 based on the detection result of the external sensor 26A mounted on the moving body 10 and detecting the surroundings of the moving body 10. The second position calculation unit 44 calculates a second estimated position of the moving body 10 based on the detection result of the internal sensor 26B mounted on the moving body 10 and detecting an output parameter indicating the control output of the moving body 10. The parameter calculation unit 46 calculates an estimated value of a movement parameter Φ for calculating the movement amount of the moving body 10 based on the first estimated position and the second estimated position. The determination unit 48 determines the validity of the estimated value of the movement parameter Φ based on the detection result of the external sensor 26A when the moving body 10 is stopped. The parameter setting unit 50 sets the estimated value of the movement parameter Φ determined to be valid as the movement parameter of the moving body 10.

本開示に係る演算装置は、外界センサ２６Ａと内界センサ２６Ｂとの検出結果から、移動パラメータΦの推定値を算出するため、移動パラメータΦを高精度に算出できるといえる。また、停止時における外界センサ２６Ａの検出結果から、移動パラメータΦの妥当性を判断するため、局所最適解への収束を検知し、かつ、タイムスタンプの誤差を抑制できるので、移動パラメータΦをより高精度に算出できる。 The computational device disclosed herein calculates an estimated value of the movement parameter Φ from the detection results of the external sensor 26A and the internal sensor 26B, and therefore can calculate the movement parameter Φ with high accuracy. Furthermore, because the validity of the movement parameter Φ is determined from the detection results of the external sensor 26A when the vehicle is stopped, convergence to a local optimum solution can be detected and timestamp errors can be suppressed, allowing the movement parameter Φ to be calculated with even higher accuracy.

第１位置算出部４２は、所定時間毎に検出された外界センサ２６Ａの検出結果から、所定時間毎の第１推定位置を算出し、第２位置算出部４４は、所定時間毎に検出された内界センサ２６Ｂの検出結果から、所定時間毎の第２推定位置を算出する。パラメータ算出部４６は、所定時間毎の第１推定位置と第２推定位置との差分に基づいて、移動パラメータΦの推定値を算出する。本開示に係る演算装置は、このようにして移動パラメータΦの推定値を算出することで、移動パラメータΦをより高精度に算出できる。 The first position calculation unit 42 calculates a first estimated position for each predetermined time period from the detection results of the external sensor 26A detected at each predetermined time period, and the second position calculation unit 44 calculates a second estimated position for each predetermined time period from the detection results of the internal sensor 26B detected at each predetermined time period. The parameter calculation unit 46 calculates an estimated value of the movement parameter Φ based on the difference between the first estimated position and the second estimated position for each predetermined time period. By calculating an estimated value of the movement parameter Φ in this manner, the calculation device according to the present disclosure can calculate the movement parameter Φ with higher accuracy.

パラメータ算出部４６は、所定時間毎の第１推定位置と第２推定位置との差分の合計値が、最小となるように、移動パラメータΦの推定値を算出する。本開示に係る演算装置は、このようにして移動パラメータΦの推定値を算出することで、移動パラメータΦをより高精度に算出できる。 The parameter calculation unit 46 calculates an estimated value of the movement parameter Φ so that the sum of the differences between the first estimated position and the second estimated position for each predetermined time period is minimized. By calculating the estimated value of the movement parameter Φ in this manner, the calculation device according to the present disclosure can calculate the movement parameter Φ with higher accuracy.

判定部４８は、移動体１０の停止時において第１外界センサ２６Ａ１により検出された、移動体１０の周囲の検出体Ａの検出結果と、移動パラメータΦの推定値とに基づいて、移動体１０の基準座標系における検出体Ａの位置を算出する。判定部４８は、移動体１０の停止時において第２外界センサ２６Ａ２により検出された、移動体１０の周囲の検出体Ａの検出結果と、移動パラメータΦの推定値とに基づいて、移動体１０の基準座標系における検出体Ａの位置を算出する。そして、判定部４８は、第１外界センサ２６Ａ１の検出結果に基づいて算出された検出体Ａの位置と、第２外界センサ２６Ａ２の検出結果に基づいて算出された検出体Ａの位置とのずれ量が、所定値以下である場合に、移動パラメータΦの推定値が妥当であると判断する。本開示に係る演算装置は、このようにして移動パラメータΦの妥当性を判断することで、移動パラメータΦをより高精度に算出できる。 The determination unit 48 calculates the position of the detected object A in the reference coordinate system of the mobile body 10 based on the detection results of the detected object A around the mobile body 10 detected by the first external sensor 26A1 when the mobile body 10 is stopped and the estimated value of the movement parameter Φ. The determination unit 48 calculates the position of the detected object A in the reference coordinate system of the mobile body 10 based on the detection results of the detected object A around the mobile body 10 detected by the second external sensor 26A2 when the mobile body 10 is stopped and the estimated value of the movement parameter Φ. The determination unit 48 then determines that the estimated value of the movement parameter Φ is valid when the deviation between the position of the detected object A calculated based on the detection results of the first external sensor 26A1 and the position of the detected object A calculated based on the detection results of the second external sensor 26A2 is equal to or less than a predetermined value. By determining the validity of the movement parameter Φ in this manner, the computing device according to the present disclosure can calculate the movement parameter Φ with higher accuracy.

判定部４８は、移動体１０の停止時において第１外界センサ２６Ａ１により検出された、移動体１０の周囲の複数の検出体Ａの検出結果から、第１外界センサ２６Ａ１の位置を算出する。判定部４８は、移動体１０の停止時において第２外界センサ２６Ａ２により検出された、移動体１０の周囲の複数の検出体Ａの検出結果から、第２外界センサ２６Ａ２の位置を算出する。そして、判定部４８は、第１外界センサ２６Ａ１の位置と第２外界センサ２６Ａ２の位置とから算出された、第１外界センサ２６Ａ１に対する第２外界センサ２６Ａ２の相対位置と、移動パラメータΦの推定値から算出された、第１外界センサ２６Ａ１に対する第２外界センサ２６Ａ２の相対位置とのずれ量が、所定値以下である場合に、移動パラメータΦの推定値が妥当であると判断する。本開示に係る演算装置は、このようにして移動パラメータΦの妥当性を判断することで、移動パラメータΦをより高精度に算出できる。 The determination unit 48 calculates the position of the first external sensor 26A1 based on the detection results of multiple detected objects A around the mobile object 10 detected by the first external sensor 26A1 when the mobile object 10 is stopped. The determination unit 48 calculates the position of the second external sensor 26A2 based on the detection results of multiple detected objects A around the mobile object 10 detected by the second external sensor 26A2 when the mobile object 10 is stopped. The determination unit 48 then determines that the estimated value of the movement parameter Φ is valid if the deviation between the relative position of the second external sensor 26A2 with respect to the first external sensor 26A1, calculated from the positions of the first external sensor 26A1 and the second external sensor 26A2, and the relative position of the second external sensor 26A2 with respect to the first external sensor 26A1, calculated from the estimated value of the movement parameter Φ, is less than a predetermined value. By determining the validity of the movement parameter Φ in this manner, the computing device disclosed herein can calculate the movement parameter Φ with higher accuracy.

パラメータ算出部４６は、移動体１０の停止時における内界センサ２６Ｂの検出結果に基づいて、内界センサ２６Ｂの検出結果の正常値からのずれ量を示すバイアス値を算出し、バイアス値にも基づき、移動パラメータΦの推定値を算出する。これにより、算出対象となる変数を減らすことができるので、移動パラメータΦの推定値の演算負荷を低減できる。 The parameter calculation unit 46 calculates a bias value indicating the deviation of the detection result of the internal sensor 26B from a normal value based on the detection result of the internal sensor 26B when the moving body 10 is stopped, and calculates an estimated value of the movement parameter Φ based on the bias value as well. This reduces the number of variables to be calculated, thereby reducing the computational load of the estimated value of the movement parameter Φ.

パラメータ算出部４６は、第１時間帯での外界センサ２６Ａ及び内界センサ２６Ｂの検出結果に基づいて算出された第１推定位置と第２推定位置とに基づいて、第１時間帯における移動パラメータΦの推定値を算出し、第１時間帯より後の第２時間帯での外界センサ２６Ａ及び内界センサ２６Ｂの検出結果に基づいて算出された第１推定位置と第２推定位置とに基づいて、第２時間帯における移動パラメータΦの推定値を算出する。判定部４８は、第２時間帯における移動パラメータΦの推定値が、第１時間帯における移動パラメータΦの推定値に対して所定条件を満たす場合に、第２時間帯における移動パラメータΦの推定値が妥当であると判断する。これにより、移動パラメータΦをより高精度に算出できる。 The parameter calculation unit 46 calculates an estimate of the movement parameter Φ for the first time period based on the first and second estimated positions calculated based on the detection results of the external sensor 26A and the internal sensor 26B for the first time period, and calculates an estimate of the movement parameter Φ for the second time period based on the first and second estimated positions calculated based on the detection results of the external sensor 26A and the internal sensor 26B for the second time period after the first time period. The determination unit 48 determines that the estimate of the movement parameter Φ for the second time period is valid if the estimate of the movement parameter Φ for the second time period satisfies a predetermined condition with respect to the estimate of the movement parameter Φ for the first time period. This allows the movement parameter Φ to be calculated with higher accuracy.

移動パラメータΦは、移動体１０の基準座標系における外界センサ２６Ａの位置と、移動体１０の車輪径と、内界センサ２６Ｂの検出結果の正常値からのずれ量を示すバイアス値との、少なくとも１つである。本開示によれば、このような移動パラメータΦを高精度に算出できる。 The movement parameter Φ is at least one of the position of the external sensor 26A in the reference coordinate system of the mobile body 10, the wheel diameter of the mobile body 10, and a bias value indicating the amount of deviation from the normal value of the detection result of the internal sensor 26B. According to the present disclosure, such movement parameter Φ can be calculated with high accuracy.

以上、本開示の実施形態を説明したが、この実施形態の内容により実施形態が限定されるものではない。また、前述した構成要素には、当業者が容易に想定できるもの、実質的に同一のもの、いわゆる均等の範囲のものが含まれる。さらに、前述した構成要素は適宜組み合わせることが可能である。さらに、前述した実施形態の要旨を逸脱しない範囲で構成要素の種々の省略、置換又は変更を行うことができる。 Although the embodiments of the present disclosure have been described above, the embodiments are not limited to the content of these embodiments. Furthermore, the aforementioned components include those that would be easily conceivable to a person skilled in the art, those that are substantially identical, and those that are within the scope of what is known as equivalents. Furthermore, the aforementioned components can be combined as appropriate. Furthermore, various omissions, substitutions, or modifications of the components can be made without departing from the spirit of the aforementioned embodiments.

１０移動体
２６Ａ外界センサ
２６Ｂ内界センサ
４０移動制御部
４２第１位置算出部
４４第２位置算出部
４６パラメータ算出部
４８判定部
５０パラメータ設定部 10 Mobile object 26A External sensor 26B Internal sensor 40 Movement control unit 42 First position calculation unit 44 Second position calculation unit 46 Parameter calculation unit 48 Determination unit 50 Parameter setting unit

Claims

a first position calculation unit that calculates a first estimated position of the moving object based on a detection result of an external sensor that is mounted on the moving object and detects the surroundings of the moving object;
a second position calculation unit that calculates a second estimated position of the moving body based on a detection result of an internal sensor that is mounted on the moving body and detects an output parameter that indicates a control output of the moving body;
a parameter calculation unit that calculates an estimated value of a movement parameter for calculating a movement amount of the moving object based on the first estimated position and the second estimated position;
a determination unit that determines the validity of the estimated value based on information related to detected objects around the moving object detected by the external sensor when the moving object is stopped and information related to the detected objects obtained from the estimated value and the position of the external sensor ;
a parameter setting unit that sets the estimated value that is determined to be valid as a movement parameter of the moving object;
Including,
Computing device.

the first position calculation unit calculates the first estimated position for each predetermined time period from a detection result of the external sensor detected for each predetermined time period;
the second position calculation unit calculates the second estimated position for each predetermined time period from a detection result of the internal sensor detected for each predetermined time period;
The computing device according to claim 1 , wherein the parameter calculation unit calculates the estimated value of the movement parameter based on a difference between the first estimated position and the second estimated position for each predetermined time period.

The computing device described in claim 2, wherein the parameter calculation unit calculates the estimated values of the movement parameters so that the sum of the differences between the first estimated position and the second estimated position for each predetermined time period is minimized.

The determination unit
calculating a position of the detected object in a reference coordinate system of the moving object based on a detection result of the detected object around the moving object detected by a first external sensor when the moving object is stopped and the estimated value of the movement parameter;
calculating a position of the detected object in a reference coordinate system of the moving object based on a detection result of the detected object detected by a second external sensor when the moving object is stopped and the estimated value of the movement parameter;
4. The computing device according to claim 1, wherein the estimated value of the movement parameter is determined to be valid when the deviation between the position of the detected object calculated based on the detection result of the first external sensor and the position of the detected object calculated based on the detection result of the second external sensor is less than a predetermined value.

The determination unit
calculating a position of the first external sensor from detection results of a plurality of detection objects around the moving object detected by the first external sensor when the moving object is stopped;
calculating a position of the second external sensor from detection results of the plurality of detection objects detected by the second external sensor when the moving object is stopped;
4. The computing device according to claim 1, wherein the estimated value of the movement parameter is determined to be valid when the deviation between the relative position of the second external sensor with respect to the first external sensor calculated from the position of the first external sensor and the position of the second external sensor and the relative position of the second external sensor with respect to the first external sensor calculated from the estimated value of the movement parameter is less than a predetermined value.

The computing device described in any one of claims 1 to 5, wherein the parameter calculation unit calculates a bias value indicating the deviation of the detection result of the internal sensor from a normal value based on the detection result of the internal sensor when the moving object is stopped, and calculates an estimated value of the movement parameter based also on the bias value.

The parameter calculation unit
calculating an estimated value of the movement parameter for a first time period based on the first estimated position and the second estimated position calculated based on detection results of the external sensor and the internal sensor for the first time period;
calculating an estimated value of the movement parameter in a second time period after the first time period based on the first estimated position and the second estimated position calculated based on detection results of the external sensor and the internal sensor in the second time period;
7. The computing device according to claim 1, wherein the determination unit determines that the estimated value of the movement parameter in the second time period is valid when the estimated value of the movement parameter in the second time period satisfies a predetermined condition with respect to the estimated value of the movement parameter in the first time period.

The computing device of any one of claims 1 to 7, wherein the movement parameters are at least one of the position of the external sensor in the reference coordinate system of the moving body, the wheel diameter of the moving body, and a bias value indicating the amount of deviation of the detection result of the internal sensor from a normal value.

calculating a first estimated position of the moving body based on a detection result of an external sensor mounted on the moving body and detecting the surroundings of the moving body;
calculating a second estimated position of the moving body based on a detection result of an internal sensor mounted on the moving body and detecting an output parameter indicating a control output of the moving body;
calculating an estimated value of a movement parameter for calculating a movement amount of the moving object based on the first estimated position and the second estimated position;
determining the validity of the estimated value based on information related to detected objects around the moving object detected by the plurality of external sensors when the moving object is stopped, and information related to the detected objects obtained from the estimated value and the positions of the external sensors ;
setting the estimated value determined to be valid as a movement parameter of the moving object;
Including,
Calculation method.

calculating a first estimated position of the moving body based on a detection result of an external sensor mounted on the moving body and detecting the surroundings of the moving body;
calculating a second estimated position of the moving body based on a detection result of an internal sensor mounted on the moving body and detecting an output parameter indicating a control output of the moving body;
calculating an estimated value of a movement parameter for calculating a movement amount of the moving object based on the first estimated position and the second estimated position;
determining the validity of the estimated value based on information related to detected objects around the moving object detected by the plurality of external sensors when the moving object is stopped, and information related to the detected objects obtained from the estimated value and the positions of the external sensors ;
setting the estimated value determined to be valid as a movement parameter of the moving object;
The computer executes the
program.