[發明專利]一種基于UWB與激光雷達的室內行人跟隨與避障方法

在審

• 申請號: 202010847107.5
• 申請日: 2020-08-21
• 公開（公告）號: CN112130559A
• 公開（公告）日: 2020-12-25
• 發明（設計）人: 劉成菊;袁家遙;陳啟軍;石文博;杜家源
• 申請（專利權）人: 同濟大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 上海科盛知識產權代理有限公司 31225
• 代理人: 楊宏泰

1.一種基于UWB與激光雷達的室內行人跟隨與避障方法，其特征在于，包括以下步驟：

1)利用AGV車載UWB模塊獲取行人坐標信息，利用激光雷達獲取障礙物信息，并實時發送至上位機；

2)根據行人坐標信息搜索行人跟隨路徑，根據障礙物信息規劃避障路徑，獲取AGV當前位姿信息(x，y，θ)，并分別發送至路徑跟蹤控制模塊；

3)路徑跟蹤控制模塊計算AGV當前位姿與行人跟隨路徑之間的關系，并通過模糊PID算法輸出線速度v和角速度w；

4)將線速度v和角速度w發送至底層控制板，實現AGV對行人的穩定跟蹤。

2.根據權利要求1所述的一種基于UWB與激光雷達的室內行人跟隨與避障方法，其特征在于，所述的行人手持信標，所述的UWB模塊通過雙邊雙向測距算法獲取信標的距離和角度，并將該距離投影于AGV自身坐標軸，得到行人坐標信息。

3.根據權利要求1所述的一種基于UWB與激光雷達的室內行人跟隨與避障方法，其特征在于，所述的步驟2)具體包括：

21)將行人坐標信息作為目標點，利用A*算法搜索行人跟隨路徑；

22)利用動態避障算法實時預判碰撞，并規劃避障路徑；

23)利用IMU傳感器和里程計提供的位姿信息進行擴展卡爾曼濾波融合，獲取AGV當前的位姿信息(x，y，θ)；

24)將行人跟隨路徑、避障路徑和位姿信息(x，y，θ)分別發送至路徑跟蹤控制模塊。

4.根據權利要求3所述的一種基于UWB與激光雷達的室內行人跟隨與避障方法，其特征在于，所述的步驟21)中，初始化時利用激光雷達點云數據和Karto_Slam算法建立一張周圍障礙物地圖，采用A*算法搜索出地圖中到達行人坐標的全局路徑，并在AGV跟蹤路徑的過程中不斷完善地圖信息；

所述的步驟22)中，當預判到和障礙物發生碰撞時，首先原地轉設定角度，判斷前方90度視角內激光雷達點云分布狀態，若激光雷達點云分布小于設定閾值，則向側前方繞行，否則后退設定距離后重新規劃避障路徑。

5.根據權利要求3或4所述的一種基于UWB與激光雷達的室內行人跟隨與避障方法，其特征在于，所述的步驟23)具體包括：

231)使用擴展卡爾曼濾波融合IMU傳感器與電機編碼器的數據，得到AGV的里程計信息；

232)結合激光雷達點云數據，使用自適應蒙特卡洛定位算法，得到AGV在地圖坐標系上的位姿信息。

6.根據權利要求3或4所述的一種基于UWB與激光雷達的室內行人跟隨與避障方法，其特征在于，所述的步驟21)中，當行人坐標信息丟失或者發生大于設定閾值的位移改變時，仍將上一時刻行人坐標信息作為目標點，所述的行人跟隨路徑的更新頻率低于行人坐標信息的更新頻率。

7.根據權利要求1所述的一種基于UWB與激光雷達的室內行人跟隨與避障方法，其特征在于，所述的步驟3)具體包括：

31)路徑跟蹤控制模塊計算AGV當前位姿與行人跟隨路徑之間的位置偏差、方向偏差、位置偏差變化率和方向偏差變化率；

32)利用偏差變化率進行模糊推理整定PID參數；

33)利用整定完成的PID參數對位置和方向偏差進行PID控制；

34)輸出線速度v和角速度w。

8.根據權利要求7所述的一種基于UWB與激光雷達的室內行人跟隨與避障方法，其特征在于，所述的模糊推理整定PID參數中，所述的方向偏差e_th、位置偏差變化率de_d/dt和方向偏差變化率de_th/dt分別采用{負大，負小，零，正小，正大}5個量化等級進行模糊化處理，所述的位置偏差e_d采用{零，正小，正大}3個量化等級進行模糊化處理，整定后輸出的PID參數取值范圍分為{小，中，大}3個模糊子集。