[發明專利]一種基于UWB的懸掛式無人車的定位及控制方法

在審

• 申請號: 201711237360.3
• 申請日: 2017-11-30
• 公開（公告）號: CN108089576A
• 公開（公告）日: 2018-05-29
• 發明（設計）人: 莫凌飛;高正;萬玲
• 申請（專利權）人: 江蘇添仂智能科技有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 南京蘇高專利商標事務所(普通合伙) 32204
• 代理人: 王安琪

1.一種基于UWB的懸掛式無人車的定位及控制方法，其特征在于，包括如下步驟：

(1)在同一軌道的前后兩臺懸掛式無人車上分別安裝UWB測距傳感器A和UWB測距傳感器B；

(2)UWB測距傳感器A將信號發射給UWB測距傳感器B，UWB測距傳感器B收到信號后會發射一個信號給UWB測距傳感器A，UWB測距傳感器A接收到UWB測距傳感器B發送的信號后，運用UWB的TOF算法和卡爾曼濾波算法計算出UWB測距傳感器B與UWB測距傳感器A之間的距離；

(3)UWB測距傳感器B采用相同的方法獲得UWB測距傳感器B與UWB測距傳感器A之間的距離；

(4)UWB控制模塊A和UWB控制模塊B分別得到經UWB測距傳感器A和UWB測距傳感器B發送的安全距離，進行安全距離判斷和定位，實現自主駕駛、顯示和報警操作。

2.如權利要求1所述的基于UWB的懸掛式無人車的定位及控制方法，其特征在于，步驟(1)中，懸掛式無人車為運行在軌道上的懸掛式無人駕駛小車。

3.如權利要求1所述的基于UWB的懸掛式無人車的定位及控制方法，其特征在于，步驟(2)和步驟(3)中，UWB測距傳感器對外發送超寬帶電磁脈沖，并且附加自身的ID信息；接收其他UWB測距傳感器發送過來的超寬帶電磁脈沖，解析脈沖中附加的ID信息。

4.如權利要求1所述的基于UWB的懸掛式無人車的定位及控制方法，其特征在于，步驟(4)中，在一定速度下，懸掛式無人車之間有一個安全行駛距離，當懸掛式無人車A和懸掛式無人車B之間的距離小于安全距離時，懸掛式無人車A上的控制模塊A將對懸掛式無人車A的速度制動，懸掛式無人車A和懸掛式無人車B之間的距離越小制動越強，直至懸掛式無人車A和懸掛式無人車B之間的距離大于安全距離。

5.如權利要求1所述的基于UWB的懸掛式無人車的定位及控制方法，其特征在于，步驟(2)中，TOF算法具體包括如下步驟：

(21)A節點UWB測距傳感器A向遠程B節點UWB測距傳感器B發送一個數據包，當B節點收到數據包時，會發送一個確認來響應這個數據包；

(22)A節點測量出從發送數據包到接收確認的時間，這段消耗總時間記為TTOT時間；B節點記錄了B從收到數據包到B回應確認消息的這個時間段的時間，記為TTAT；用TTOT總時間減去周轉時間TTAT就是雙方的數據包在飛行中度過的往返時間，記為TRTT時間；假定在每個方向發生的飛行時間TTOF等于50％的往返時間，如下式所示：

TTOF＝TRTT/2＝(TTOT-TTAT)/2

當計算出TTOF后，根據D＝T×c，其中T代表TTOF，c代表光速，為3×108ms-1，計算出節點A與節點B之間的距離。

6.如權利要求1所述的基于UWB的懸掛式無人車的定位及控制方法，其特征在于，步驟(2)中，卡爾曼濾波算法具體為：設被估計值系統的狀態方程和測量方程為：Xk+1＝Φk+1Xk+Wk，Yk＝h|Xk|+Vk；將h|Xk|圍繞展開泰勒級數，取其一次項，得到近似表達式：

將上式寫成如下形式：

式中，

其中，X是n維狀態矢量，Φ是n乘n階狀態轉移矩陣，W是p維動態噪聲向量，Y是m維測量向量，H是m×n階測量矩陣，V是m維測量誤差向量，M是m維測量系統的系統誤差；

假設{Wk}與{Vk+1}是互不相關的零均值的高斯噪聲序列，即對所有的K、L有：

E [ X 0 W k T ] = 0 , E [ X 0 V k + 1 T ] = 0 ]]>

則有：計算預測方程：計算預測協方差：Pk/k-1＝ΦkPk-1/k-1Φt+Qk；計算預測協方差：計算濾波方程：計算濾波協方差：其中，為測量方程在預測點處計算的Jacobi矩陣。