本發(fā)明屬于叉車定位，更具體地，本發(fā)明涉及一種基于雙雷達(dá)的叉車自動(dòng)定位方法及系統(tǒng)。

背景技術(shù)：

1、隨著自動(dòng)化技術(shù)的發(fā)展，自動(dòng)化倉儲(chǔ)物流系統(tǒng)也越來越廣泛的出現(xiàn)在生產(chǎn)物流中，而自動(dòng)化裝車系統(tǒng)是自動(dòng)化倉儲(chǔ)物流系統(tǒng)的關(guān)鍵一環(huán)，是廠區(qū)自動(dòng)化流程的最后一步。由于生產(chǎn)設(shè)備和場地的限制，現(xiàn)有的自動(dòng)化裝車方案大都比較復(fù)雜，不但需要安裝大型的機(jī)電設(shè)備，可能還需要復(fù)雜的多次轉(zhuǎn)運(yùn)，這些方案應(yīng)用起來對空間有較大的要求、成本也比較高，無法滿足越來越多的物流裝車需求。

2、基于自動(dòng)叉車的柔性裝車系統(tǒng)能直接將貨物從廠房轉(zhuǎn)運(yùn)至物流貨車中，解決了多次轉(zhuǎn)運(yùn)帶來的麻煩或大型機(jī)電設(shè)備帶來的成本及安裝問題。但是，自動(dòng)裝車系統(tǒng)需要面對人工?？康呢涇?，貨車?？康恼`差很大，需要進(jìn)行實(shí)時(shí)測量和定位，對算法精度和效率的要求很高。

3、公開號：cn114200476a，專利名稱：激光叉車定位方法及激光叉車，在倉庫內(nèi)采用激光反光柱進(jìn)行定位，在貨車車廂內(nèi)進(jìn)行裝貨時(shí)使用車載2d雷達(dá)掃描車廂環(huán)境進(jìn)行定位。該技術(shù)方案存在如下缺點(diǎn)：(1)使用一臺安裝在叉車頂端的激光雷達(dá)進(jìn)行定位，當(dāng)叉車裝載的貨物較高時(shí)會(huì)遮擋雷達(dá)的視野，導(dǎo)致定位失敗或者偏差較大；(2)在車廂內(nèi)是通過掃描貨車車廂提取貨車在雷達(dá)中的位姿來進(jìn)行定位的，這種方法在車廂內(nèi)堆滿貨物時(shí)，由于車廂特征提取存在偏差或無法提取，導(dǎo)致存在定位精度不高或無法定位的問題。

技術(shù)實(shí)現(xiàn)思路

1、本發(fā)明提供一種基于雙雷達(dá)的自動(dòng)叉車定位方法，旨在改善上述問題。

2、本發(fā)明是這樣實(shí)現(xiàn)的，一種基于雙雷達(dá)的叉車自動(dòng)定位方法，所述方法具體如下：

3、(1)貨車停穩(wěn)后打開貨車門，將貨車車門鎖緊至貨車車廂側(cè)面，通知激光叉車從倉庫中取貨；

4、(2)啟動(dòng)反光柱定位模式，控制第一激光雷達(dá)lidar1采集環(huán)境中的點(diǎn)云數(shù)據(jù)，基于反光柱匹配確定激光叉車在倉庫中的位姿；

5、(3)在激光叉車行駛至距貨車尾部設(shè)定距離時(shí)，控制第一激光雷達(dá)lidar1進(jìn)行掃描，構(gòu)建點(diǎn)云均值地圖map_pcd1，

6、(4)控制激光叉車反向，以倒車的方式駛?cè)胴涇嚨能噹麅?nèi)，在激光叉車進(jìn)入貨車車廂且距貨車尾部設(shè)定距離時(shí)，控制第二激光雷達(dá)lidar2進(jìn)行掃描，構(gòu)建點(diǎn)云地圖map_pcd2；

7、(5)將云均值地圖map_pcd1與點(diǎn)云地圖map_pcd2合并成點(diǎn)云地圖map_pcd；

8、(6)關(guān)閉反光柱定位模式，啟動(dòng)車廂定位內(nèi)模式，將第一激光雷達(dá)lidar1及第二激光雷達(dá)lidar2掃描到的點(diǎn)云與點(diǎn)云地圖map_pcd進(jìn)行匹配，匹配出激光叉車當(dāng)前的位姿。

9、進(jìn)一步的，點(diǎn)云均值地圖map_pcd1的構(gòu)建方法具體如下：

10、(31)第一激光雷達(dá)lidar1掃描環(huán)境得到雷達(dá)掃描數(shù)據(jù)scan，基于反光柱匹配確定到第一激光雷達(dá)lidar1在地圖中的位姿posem_1；

11、(32)將第一激光雷達(dá)lidar1的位姿posem_1加入雷達(dá)位姿集合pose_listm_l中，將雷達(dá)掃描數(shù)據(jù)scan中的距離di存入掃描角度θi對應(yīng)的距離集合d_listi，得到scan_list{{(θ0,d_list0),(θ1,d_list1),...,(θn-1,d_listn-1)}；

12、(33)檢測第一激光雷達(dá)lidar1當(dāng)前掃描次數(shù)是否已經(jīng)達(dá)到次數(shù)閾值，若檢測結(jié)果為是，則執(zhí)行步驟(34)，若檢測結(jié)果為否，則執(zhí)行步驟(31)；

13、(34)基于雷達(dá)位姿集合pose_listm_l計(jì)算第一激光雷達(dá)lidar1當(dāng)前在地圖中的平均位姿和位姿標(biāo)準(zhǔn)差σm_1，若位姿標(biāo)準(zhǔn)差σm_1小于設(shè)定的位姿標(biāo)準(zhǔn)差閾值，則執(zhí)行步驟(35)；

14、(35)基于各個(gè)掃描角度θi對應(yīng)的距離集合d_listi計(jì)算距離平均值和距離標(biāo)準(zhǔn)差σi，若距離標(biāo)準(zhǔn)差σi小于距離標(biāo)準(zhǔn)差閾值，基于掃描角度θi和距離平均值計(jì)算對應(yīng)點(diǎn)在第一激光雷達(dá)坐標(biāo)系下的位置(xi,yi)，并使用平均位姿轉(zhuǎn)換至地圖坐標(biāo)系后，存入均值點(diǎn)云地圖map_pcd1。

15、進(jìn)一步的，點(diǎn)云地圖map_pcd2的構(gòu)建方法具體如下：

16、(41)使用第一激光雷達(dá)lidar1掃描環(huán)境，利用反光柱匹配確定第一激光雷達(dá)lidar1當(dāng)前在地圖中的位姿posem_1，基于第二激光雷達(dá)lidar2相對于第一激光雷達(dá)lidar1的位姿變換關(guān)系t12確定第二激光雷達(dá)lidar2當(dāng)前在地圖中的位姿posem_2＝posem_1*t12；

17、(42)使用第二激光雷達(dá)lidar2掃描環(huán)境得到點(diǎn)云數(shù)據(jù)lidar_pcd2，根據(jù)第二激光雷達(dá)lidar2的當(dāng)前位姿posem_2將點(diǎn)云數(shù)據(jù)lidar_pcd2轉(zhuǎn)換到地圖坐標(biāo)系，得到點(diǎn)云地圖map_pcd2。

18、進(jìn)一步的，激光叉車在貨車車廂內(nèi)的位姿獲取方法具體如下：

19、(61)使用第一激光雷達(dá)lidar1和第二激光雷達(dá)lidar2同時(shí)進(jìn)行掃描，得到當(dāng)前幀的點(diǎn)云數(shù)據(jù)curr_pcd1和curr_pcd2，根據(jù)位姿變換關(guān)系t12將點(diǎn)云數(shù)據(jù)curr_pcd2轉(zhuǎn)換到第一激光雷達(dá)lidar1坐標(biāo)系下，并與點(diǎn)云數(shù)據(jù)curr_pcd1進(jìn)行合并得到點(diǎn)云數(shù)據(jù)curr_pcd；

20、(62)將上一時(shí)刻的位姿poset-1作為當(dāng)前時(shí)刻的位姿poset′；

21、(63)基于當(dāng)前時(shí)刻位姿poset′將點(diǎn)云數(shù)據(jù)curr_pcd的點(diǎn)ptl轉(zhuǎn)換至地圖坐標(biāo)系后形成點(diǎn)ptm，在點(diǎn)云地圖map_pcd中查找距點(diǎn)ptm最近的點(diǎn)ptm′，檢測點(diǎn)ptm與點(diǎn)ptm′的距離是否大于距離閾值，若檢測結(jié)果為否，則將點(diǎn)ptm′放入匹配點(diǎn)集合中，若檢測結(jié)果為是，則點(diǎn)ptm′不參與下面的位姿匹配過程；

22、(63)基于匹配點(diǎn)集合中的所有點(diǎn)ptm′與在地圖中的對應(yīng)點(diǎn)poset*ptl的距離構(gòu)建殘差模型，殘差模型具體如下：

23、e＝∑||poset*ptl-pt′m||

24、(64)基于高斯牛頓法求解最小殘差e對應(yīng)的位姿poset，檢測當(dāng)前迭代次數(shù)是否達(dá)到次數(shù)閾值，若檢測結(jié)果為否，則將該位姿poset作為當(dāng)前位姿poset′，執(zhí)行步驟(62)，若檢測結(jié)果為是，則位姿poset作為激光叉車當(dāng)前時(shí)刻在地圖中的位姿。

25、進(jìn)一步的，反光柱定位模式下基于反光板匹配的第一激光雷達(dá)lidar1定位方法具體如下

26、(71)遍歷第一激光雷達(dá)lidar1的當(dāng)前掃描幀中的點(diǎn)(θi,di)，根據(jù)強(qiáng)度與距離的關(guān)系提取點(diǎn)云中反光柱的反射點(diǎn)，按照距離關(guān)系對反光柱的反射點(diǎn)進(jìn)行聚類，并擬合反光柱中心(θ,d)，遍歷完所有點(diǎn)后，得到反光柱集合ref_points；

27、(72)根據(jù)提取到的反光柱之間的兩兩距離，與地圖中的反光柱間的距離進(jìn)行對比，在地圖中找到距離誤差最小的反光板集合map_points：

28、(73)基于反光柱集合ref_points以及與其匹配的反光板集合map_points匹配出第一激光雷達(dá)lidar1當(dāng)前在地圖中的位姿。

29、進(jìn)一步的，第一激光雷達(dá)lidar1安裝在叉車門架頂端正前方，高度不超過叉車門架，第二激光雷達(dá)lidar2安裝在叉車插齒下部。

30、進(jìn)一步的，貨車為四面全封閉的箱式貨車，貨車停穩(wěn)后與月臺之間距離不超過10cm，夾角不超過5°。

31、本發(fā)明是這樣實(shí)現(xiàn)的，一種基于雙雷達(dá)的叉車自動(dòng)定位系統(tǒng)，，所述系統(tǒng)包括：

32、設(shè)于叉車的第一激光雷達(dá)lidar1及第二激光雷達(dá)lidar2；

33、以及與第一激光雷達(dá)lidar1、第二激光雷達(dá)lidar2通訊連接的處理器，叉車在從倉庫取貨點(diǎn)行駛至貨車車廂的放貨點(diǎn)的過程中，處理器基于上述基于雙雷達(dá)的叉車自動(dòng)定位方法激光叉車進(jìn)行定位。

34、進(jìn)一步的，第一激光雷達(dá)lidar1安裝在叉車門架頂端正前方，高度不超過叉車門架，第二激光雷達(dá)lidar2安裝在叉車插齒下部。

35、本發(fā)明提出一種基于雙雷達(dá)的激光叉車定位方法，貨物從廠房轉(zhuǎn)運(yùn)至貨車車廂的全過程中，只依賴車載的雙激光雷達(dá)即可自動(dòng)獲取出激光叉車的位姿，且定位精度高，不但改善了貨物遮擋雷達(dá)視野的問題，也改善貨物堆滿導(dǎo)致的定位精度降低的問題；此外，系統(tǒng)簡單成且本低，環(huán)境改造較少。