[發(fā)明專利]估計空間占據(jù)的方法

有效

• 申請?zhí)枺?/td>: 201611226316.8
• 申請日: 2016-12-27
• 公開（公告）號: CN108240807B
• 公開（公告）日: 2023-06-02
• 發(fā)明（設計）人: 殷鵬
• 申請（專利權）人: 法法汽車（中國）有限公司
• 主分類號: G01C21/00
• 分類號: G01C21/00
• 代理公司: 北京潤平知識產(chǎn)權代理有限公司 11283
• 代理人: 羅攀;肖冰濱

1.一種估計空間占據(jù)的方法，其特征在于，該方法包括：

在地圖坐標系下計算傳感器檢測的點云信息中各個點的方差信息，其中地圖坐標系相對于傳感器位置固定，且傳感器檢測的點云信息中姿態(tài)信息在地圖坐標系和世界坐標系中一致；

其中，所述點云信息中各個點的方差信息通過下式計算：

E＝Ds×Es×(Ds)^T+Dq×Eq×(Dq)^T

其中，E為點的方差信息，Es為激光傳感器的估計測量誤差的協(xié)方差矩陣，矩陣參數(shù)與傳感器模型相關，Eq為慣性導航傳感器的估計測量誤差的協(xié)方差矩陣；Ds為點云信息中各點的測量誤差，(Ds)^T表示Ds的轉置，Dq為點云信息中各點的姿勢誤差，(Dq)^T表示Dq的轉置，根據(jù)點云信息中位于空間柵格的各個點的方差信息計算所述空間柵格的占據(jù)概率；

取點云信息中位于空間柵格中的一個點的方差信息和空間柵格針對該點的占據(jù)概率分別作為空間柵格的初始的方差信息和占據(jù)概率進行存儲；

逐個提取點云信息中位于空間柵格中的點的方差信息和空間柵格針對該點的占據(jù)概率來迭代更新存儲的空間柵格的方差信息和占據(jù)概率，直到點云信息中位于空間柵格中的點都被使用為止；

其中，通過下式對存儲的空間柵格的占據(jù)概率和方差信息進行更新，最終估計出空間柵格在實際中的占據(jù)概率：

p+＝((E)×(p)+(E-)×(p-))/((E-)+E)

E+＝(E-)×(E)/((E-)×(E))

其中(p-，E-)表示當前存儲的空間柵格的占據(jù)概率和方差信息，(p，E)表示空間柵格對應于提取的點云信息中點的占據(jù)概率和該點的方差信息，(p+，E+)表示更新后的空間柵格的占據(jù)概率和方差信息。

2.根據(jù)權利要求1所述的方法，其特征在于，

所述在地圖坐標系下計算傳感器檢測的點云信息中各個點的方差信息包括：

將傳感器坐標系下的點云信息中各個點的位置信息轉換為地圖坐標系下的位置信息，其中所述傳感器坐標系為依據(jù)傳感器位置所建立的坐標系；

對于點云信息中各點，根據(jù)所述點在地圖坐標系下的位置信息計算該點相應的測量誤差和姿勢誤差；

對于點云信息中各點，根據(jù)所述點的測量誤差和姿勢誤差計算該點的方差信息。

3.根據(jù)權利要求1所述的方法，其特征在于，所述根據(jù)點云信息中位于空間柵格的各個點的方差信息計算所述空間柵格的占據(jù)概率包括：

對于點云信息中位于空間柵格的各點，根據(jù)所述點的位置計算空間柵格相應于該點的占據(jù)概率；

使用點云信息中位于所述空間柵格的各個點的方差信息和空間柵格相應于各點的占據(jù)概率計算所述空間柵格的占據(jù)概率。

4.根據(jù)權利要求3所述的方法，其特征在于，所述對于點云信息中位于空間柵格的各點，根據(jù)所述點的位置計算空間柵格相應于該點的占據(jù)概率包括：

對于點云信息中位于空間柵格的各點，分別計算該點與空間柵格中心間的距離的倒數(shù)，對所計算的距離的倒數(shù)進行歸一化獲得空間柵格相應于該點的占據(jù)概率。