[發(fā)明專利]位置推定裝置、移動體控制系統(tǒng)、位置推定方法以及程序

在審

• 申請?zhí)枺?/td>: 202010115140.9
• 申請日: 2020-02-25
• 公開（公告）號: CN112486158A
• 公開（公告）日: 2021-03-12
• 發(fā)明（設(shè)計）人: 中島諒;關(guān)晃仁
• 申請（專利權(quán)）人: 株式會社東芝;東芝電子元件及存儲裝置株式會社
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 北京市中咨律師事務(wù)所 11247
• 代理人: 劉靜;段承恩

1.一種位置推定裝置，對設(shè)置了一個或者多個傳感器的移動體裝置的自身位置進(jìn)行推定，所述一個或者多個傳感器觀測與移動有關(guān)的信息，所述位置推定裝置具備：

狀態(tài)量推定部，其基于所述一個或者多個傳感器各自的觀測值，推定表示所述一個或者多個傳感器各自的狀態(tài)的狀態(tài)量；

可靠度算出部，其算出表示所述一個或者多個傳感器各自的所述狀態(tài)量的可靠程度的可靠度；以及

自身位置推定部，其基于所述一個或者多個傳感器各自的所述狀態(tài)量的所述可靠度，選擇所述一個或者多個傳感器中的一個或者多個對象傳感器，基于所選擇的所述一個或者多個對象傳感器各自的所述觀測值以及所述狀態(tài)量，推定所述自身位置。

2.根據(jù)權(quán)利要求1所述的位置推定裝置，

所述移動體裝置還設(shè)置有對所述移動體裝置的周圍進(jìn)行拍攝的拍攝裝置，

所述自身位置推定部，

在所述一個或者多個傳感器中存在所述可靠度為預(yù)先確定的閾值以上的所述一個或者多個對象傳感器的情況下，選擇所述一個或者多個傳感器中的所述狀態(tài)量的所述可靠度為所述閾值以上的一個或者多個傳感器來作為所述一個或者多個對象傳感器，

基于在對象時刻通過所述拍攝裝置拍攝到的拍攝圖像、所述對象時刻之前的過去時刻的所述拍攝圖像、所述過去時刻的所述自身位置、和所選擇的所述一個或者多個對象傳感器各自的所述觀測值以及所述狀態(tài)量，推定所述對象時刻的所述自身位置。

3.根據(jù)權(quán)利要求2所述的位置推定裝置，

所述自身位置推定部，在所述一個或者多個傳感器中不存在所述可靠度為所述閾值以上的所述一個或者多個對象傳感器的情況下，基于所述對象時刻的所述拍攝圖像、所述過去時刻的所述拍攝圖像以及所述過去時刻的所述自身位置，推定所述對象時刻的所述自身位置。

4.根據(jù)權(quán)利要求2或者3所述的位置推定裝置，

關(guān)于所述一個或者多個傳感器的各個傳感器，所述狀態(tài)量推定部推定使對預(yù)定的關(guān)系式代入了一個或者多個所述過去時刻各自的所述自身位置的情況下的殘差為最小的所述狀態(tài)量，

所述關(guān)系式是基于所述過去時刻的所述自身位置、對應(yīng)傳感器的所述觀測值及所述對應(yīng)傳感器的所述狀態(tài)量、以及所述過去時刻與所述對象時刻的時間差來算出所述對象時刻的所述自身位置的方程式。

5.根據(jù)權(quán)利要求4所述的位置推定裝置，

所述狀態(tài)量推定部通過非線性最小二乘法來推定所述狀態(tài)量，

所述可靠度算出部，

取得所推定的所述狀態(tài)量的不同的多個分量的協(xié)方差矩陣，

算出所取得的所述協(xié)方差矩陣的對角分量之和的倒數(shù)來作為所述可靠度。

6.根據(jù)權(quán)利要求4所述的位置推定裝置，

所述移動體裝置還具備對與所述狀態(tài)量相當(dāng)?shù)挠^測值進(jìn)行觀測的第2傳感器，

所述可靠度算出部對所述第2傳感器的觀測值與所推定的所述狀態(tài)量進(jìn)行比較，算出所述可靠度。

7.根據(jù)權(quán)利要求3所述的位置推定裝置，

所述自身位置包括平動分量以及旋轉(zhuǎn)分量，

所述一個或者多個傳感器包括角速度傳感器和加速度傳感器，

所述角速度傳感器測定所述對象時刻的所述移動體裝置的角速度，

所述加速度傳感器測定所述對象時刻的所述移動體裝置的加速度。

8.根據(jù)權(quán)利要求7所述的位置推定裝置，

所述狀態(tài)量推定部，

推定所述移動體裝置的角速度偏置來作為所述角速度傳感器的所述狀態(tài)量，

推定所述移動體裝置的速度、重力加速度以及所述移動體裝置的加速度偏置來作為所述加速度傳感器的所述狀態(tài)量。