[發(fā)明專利]一種基于航位推算的激光導(dǎo)引AGV自動(dòng)建圖和路徑匹配方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201910110998.3
• 申請(qǐng)日: 2019-02-12
• 公開（公告）號(hào): CN109885049B
• 公開（公告）日: 2021-06-04
• 發(fā)明（設(shè)計(jì)）人: 許鴻飛;夏繼強(qiáng);耿春明;邢春香;李興;潘越
• 申請(qǐng)（專利權(quán)）人: 北京航空航天大學(xué)
• 主分類號(hào): G05D1/02
• 分類號(hào): G05D1/02
• 代理公司: 北京辰權(quán)知識(shí)產(chǎn)權(quán)代理有限公司 11619
• 代理人: 劉廣達(dá)

1.一種基于航位推算的激光導(dǎo)引AGV自動(dòng)建圖和路徑匹配方法，其特征在于，包括：

建立AGV工作區(qū)域的世界坐標(biāo)系以及AGV的車體坐標(biāo)系；

通過(guò)激光雷達(dá)采集附近激光反射物在所述車體坐標(biāo)系下的第一距離信息和第一角度信息；

根據(jù)AGV位姿推測(cè)模型預(yù)測(cè)下一時(shí)刻的AGV位姿，進(jìn)而獲得下一時(shí)刻激光反射物的第二距離信息和第二角度信息；其中，所述根據(jù)AGV位姿推測(cè)模型預(yù)測(cè)下一時(shí)刻的AGV位姿，進(jìn)而獲得下一時(shí)刻激光反射物的第二距離信息和第二角度信息，包括：

構(gòu)建世界坐標(biāo)系下所述反射物的距離信息和角度信息隨車體位姿的變換關(guān)系；

獲得當(dāng)前時(shí)刻所述AGV在世界坐標(biāo)系下的坐標(biāo)和車體航向角；

根據(jù)所述下一時(shí)刻的AGV位姿、所述變換關(guān)系以及所述當(dāng)前時(shí)刻AGV在世界坐標(biāo)系下的坐標(biāo)和車體航向角，獲得所述第二距離信息和所述第二角度信息；

所述位姿推測(cè)模型包括：

其中，所述W(k+1)為AGV在K+1時(shí)刻的位姿，所述Xp(k)和Yp(k)分別為AGV在初始位置的x坐標(biāo)和y坐標(biāo)；所述ψ(k)為AGV初始角度，所述Δx(k)、Δy(k)、Δψ(k)分別為x坐標(biāo)變化量、y坐標(biāo)變化量以及角度變化量，Vl(k)、Vr(k)分別為AGV左右輪控制量，Δt為時(shí)刻k到時(shí)刻k+1的時(shí)間間隔，B為AGV左右輪的軸距；

AVG移動(dòng)到下一位置，通過(guò)激光雷達(dá)采集該時(shí)刻激光反射物在所述車體坐標(biāo)系下的第三距離信息和第三角度信息；

分別將所述第三距離信息和所述第二距離信息以及所述第三角度信息和所述第二角度信息進(jìn)行對(duì)比，根據(jù)對(duì)比結(jié)果完成AGV的自動(dòng)建圖和路徑匹配；

其中，所述分別將所述第三距離信息和所述第二距離信息以及所述第三角度信息和所述第二角度信息進(jìn)行對(duì)比，根據(jù)對(duì)比結(jié)果完成AGV的自動(dòng)建圖，包括：

將所述第三距離信息和所述第二距離信息以及所述第三角度信息和所述第二角度信息進(jìn)行對(duì)比；

若對(duì)應(yīng)的距離信息或?qū)?yīng)的角度信息的兩個(gè)差值中，至少有一個(gè)所述差值大于等于預(yù)先設(shè)定閾值，則認(rèn)為發(fā)現(xiàn)新的激光反射物；

待所有激光反射物匹配成功后，將所述新的激光反射物在世界坐標(biāo)系下的位置坐標(biāo)更新入激光反射物地圖列表；

在AGV的行進(jìn)過(guò)程中重復(fù)上述過(guò)程，從而完成AGV的自動(dòng)建圖；

其中，所述分別將所述第三距離信息和所述第二距離信息以及所述第三角度信息和所述第二角度信息進(jìn)行對(duì)比，根據(jù)對(duì)比結(jié)果完成AGV的路徑匹配，包括：

將所述第三距離信息和所述第二距離信息以及所述第三角度信息和所述第二角度信息進(jìn)行對(duì)比；

若對(duì)應(yīng)的距離信息及角度信息的差值均小于設(shè)定閾值，則認(rèn)為是同一激光反射物；

待所有激光反射物匹配成功后，將當(dāng)前各激光反射物的實(shí)際距離信息及角度信息更新至激光反射物實(shí)際信息列表；

在AGV的行進(jìn)過(guò)程中重復(fù)上述過(guò)程，從而完成AGV的路徑匹配；其中，在所述分別將所述第三距離信息和所述第二距離信息以及所述第三角度信息和所述第二角度信息進(jìn)行對(duì)比前，還包括：

建立所述世界坐標(biāo)系與所述車體坐標(biāo)系之間的變換關(guān)系；

根據(jù)所述變換關(guān)系將所述第一距離信息以及第一角度信息變換到所述世界坐標(biāo)系下；其中，

所述第一距離信息經(jīng)過(guò)所述變換關(guān)系的變換后，獲得對(duì)應(yīng)激光反射物在世界坐標(biāo)系下的位置坐標(biāo)；

其中，在所述分別將所述第三距離信息和所述第二距離信息以及所述第三角度信息和所述第二角度信息進(jìn)行對(duì)比前，還包括：

將獲得的在所述車體坐標(biāo)系下的第一距離信息和第一角度信息按掃描順序存入激光反射物實(shí)際信息列表中；

將獲得的所述激光反射物在世界坐標(biāo)系下的位置坐標(biāo)按掃描順序存入激光反射物地圖列表；

將通過(guò)所述AGV位姿推測(cè)模型獲得的下一時(shí)刻激光反射物的第二距離信息和第二角度信息存入激光反射物的期望信息列表；

其中，所述激光反射物包括：激光反射板或激光反射柱。

2.根據(jù)權(quán)利要求1所述的激光導(dǎo)引AGV自動(dòng)建圖和路徑匹配方法，其特征在于，在通過(guò)激光雷達(dá)采集附近激光反射物在所述車體坐標(biāo)系下的第一距離信息和第一角度信息前，還包括：

在AGV場(chǎng)地中布置多個(gè)非對(duì)稱激光反射物，用于輔助AGV定位從而規(guī)劃AGV的行駛路徑；其中，

所述激光反射物包括：激光反射板或激光反射柱。

3.根據(jù)權(quán)利要求1所述的激光導(dǎo)引AGV自動(dòng)建圖和路徑匹配方法，其特征在于，

所述通過(guò)激光雷達(dá)采集附近激光反射物在所述車體坐標(biāo)系下的第一距離信息和第一角度信息均為所述車體坐標(biāo)系下的坐標(biāo)；

所述根據(jù)AGV位姿推測(cè)模型預(yù)測(cè)下一時(shí)刻的AGV位姿，進(jìn)而獲得下一時(shí)刻激光反射物的第二距離信息和第二角度信息均為所述世界坐標(biāo)系下的坐標(biāo)。