基于超聲波傳感器陣列的物體跟蹤方法
【專利摘要】本發明提供一種基于超聲波傳感器陣列的物體跟蹤方法，包括：步驟1、建立l階的系統狀態變量擬合目標物的系統狀態；步驟2、根據目標物運動特點建立運動狀態方程，根據傳感器測距方法建立觀測方程；步驟3、利用跟蹤濾波算法對目標物進行跟蹤，對于運動或觀測方程存在非線性化部分的系統，可利用能夠處理非線性系統的濾波器如擴展卡爾曼濾波器、無跡卡爾曼濾波器、粒子濾波器等進行目標物跟蹤。
【專利說明】
基于超聲波傳感器陣列的物體跟蹤方法
技術領域
[0001] 本發明涉及傳感器技術領域，特別是指一種基于超聲波傳感器陣列的物體跟蹤方 法。
【背景技術】
[0002] 傳感器技術從出現以來已經成為了一切控制機械的"眼睛"，而一切智能控制都需 要依靠傳感器技術，是先進駕駛輔助系統(Advanced Automotive Driving System，ADAS)、 智能汽車和無人駕駛汽車的基礎。目前，環境感知的主要方法是通過頂置旋轉激光雷達、激 光雷達、毫米波雷達、攝像頭、超聲波傳感器等傳感器系統對環境進行感知，尤其是對環境 中的靜態和動態目標物進行定位和跟蹤。
[0003] 頂置旋轉激光雷達、激光雷達、毫米波雷達探測范圍大，普遍使用在目前各公司開 發的無人駕駛系統中，但缺點是成本非常高，難以實現在普通乘用車中的大規模應用。尤其 是旋轉激光雷達，谷歌無人駕駛汽車使用的激光雷達由Velodyne生產，早期原型車中使用 的版本價格高達7萬美元，而目前使用版本的價格也在8000-30000美元。攝像頭價格便宜， 探測范圍較大，主要的技術難點在于后期算法，由于圖像處理算法復雜，對運行算法的計算 機性能要求非常高。除此之外通過攝像頭進行環境感知容易受到光照等環境因素的影響。
[0004] 超聲波傳感器成本非常低，具備在乘用車中進行大規模推廣的潛力。缺點是探測 距離較短，目前使用的長距離超聲波傳感器能夠達到5m的探測距離，已經可以滿足汽車周 圍近距離的環境感知。目前市場中已有的基于超聲波傳感器的車輛周圍環境感知主要應用 集中于泊車輔助系統、自動泊車系統和盲區預警系統。已有的算法存在以下兩個主要問題：
[0005] 1)多數系統采用三角定位法進行目標物的定位，傳感器誤差的存在會對三角定位 法的定位造成距離和角度上的偏差，其中間距為50cm的雙傳感器三角定位法定位中，0.5 % 的傳感器誤差可能造成0.65%的距離誤差和高達25%的角度誤差。
[0006] 2)多數系統僅根據目標物最近點的距離信號是否到達預警閾值判斷是否提供預 警。系統僅獲得目標物最近點的距離信號，無法獲得目標物的速度信息，無法區分靜態目標 物和動態目標物，進而導致盲區例如預警系統中的誤警情況的出現。

【發明內容】

[0007] 針對現有技術中存在的基于超聲波傳感器陣列進行物體跟蹤存在的問題，本發明 要解決的技術問題是提供一種效果更好的基于超聲波傳感器陣列的物體跟蹤方法。
[0008] 為了解決上述問題，本發明實施例提出了一種基于η個傳感器的超聲波傳感器陣 列的物體跟蹤方法，包括：
[0009] 步驟1、建立1階的系統狀態變量擬合目標物的系統狀態；
[0010]步驟2、根據目標物運動特點建立運動狀態方程，根據傳感器測距方程建立觀測方 程；
[0011] x(k)=f(x(k-l))+ff(k-l)
[0012] Z(k)=h(X(k))+V(k)
[0013] 其中，X(k)為系統狀態變量X(k) eR^WU)為系統過程噪聲，過程噪聲協方差矩陣 為tZU)為觀測量Z(k)eRn，V(k)為觀測噪聲，觀測噪聲協方差矩陣為R。函數f( ·)為系統 轉移方程，函數h( ·)為觀測方程。
[0014] 步驟3、利用跟蹤濾波算法對目標物進行跟蹤，對于運動或觀測方程存在非線性化 部分的系統，可利用能夠處理非線性系統的濾波器如擴展卡爾曼濾波器、無跡卡爾曼濾波 器、粒子濾波器等進行目標物跟蹤。其中，所述步驟1具體包括：
[0015] 步驟11、分析應用場景中目標物在二維平面內的在傳感器陣列坐標系中的系統狀 態特點，進行系統狀態歸類。可歸類為相對靜止狀態、勻速狀態、勻加速或勻減速狀態等。
[0016] 步驟12、根據系統狀態類型，建立合適的系統狀態變量擬合目標物系統狀態。應用 場景中，若目標物系統狀態可近似認為是靜止狀態，則可利用四階（1 = 4)的系統狀態變量 擬合目標物系統狀態，系統狀態變量如下：
[0017] Xk= [x,y ,vx,vy]T
[0018] 其中，x，y為目標物在x，y坐標系中位置的橫縱坐標值，Vx，Vy為目標物在x方向與y 方向的速度，由于目標物系統狀態可近似認為是靜止狀態，Vx，Vy可認為是高斯分布的隨機 誤差值。T表示矩陣的轉置。
[0019] 若目標物系統狀態可近似認為是勻速運動狀態，則可利用六階（1=6)的系統狀態 變量擬合目標物系統狀態，系統狀態變量如下：
[0020] Xk= [x,y ,Vx,vy,ax,ay]T
[0021] 其中，x，y為目標物在x，y坐標系中位置的橫縱坐標值，Vx，Vy為目標物在x方向與y 方向的速度，ax，ay為目標物在X方向與y方向的加速度，由于目標物系統狀態可近似認為是 勻速運動狀態，a x，ay可認為是高斯分布的隨機誤差值。
[0022] 若目標物系統狀態可近似認為是勻加速或勻減速運動狀態，則可利用八階（1 = 8) 的系統狀態變量擬合目標物系統狀態，系統狀態變量如下：
[0023] Xk = [x, y, Vx, Wy, ax, ay, ay]T
[0024] 其中，x，y為目標物在x，y坐標系中位置的橫縱坐標值，Vx，Vy為目標物在x方向與y 方向的速度，ax，ay為目標物在X方向與y方向的加速度，α'χ) ay為目標物在X方向與y方向的 加加速度，由于目標物系統狀態可近似認為是勻加速或勻減速運動狀態，βρ?ν可認為是 高斯分布的隨機誤差值。
[0025]其中，所述步驟2具體包括：
[0026]步驟21、物體系統的狀態轉移過程可以描述為一個離散時間的隨機過程。根據目 標物運動特點建立運動狀態方程，對于二維平面內做線性運動物體，首先獲取離散運動轉 移矩陣。為1階系統狀態變量建立1X1階運動狀態轉移矩陣，具體如下：

[0030] 其中T為目標物跟蹤的跟蹤周期，即超聲波傳感器陣列系統的信號采集周期；
[0031] 步驟22、基于運動狀態轉移矩陣建立運動狀態方程。對于二維平面內做線性運動 物體，可建立線性運動狀態方程：
[0032] X(k)=AX(k-l)+ff(k-l)
[0033] 其中X(k)為系統狀態變量，為系統過程噪聲，A為狀態轉移矩陣。
[0034] 步驟23、根據傳感器測距方程建立觀測矩陣。根據超聲波傳感器的測距原理，超聲 波探測傳感器到目標點的距離。對于同步信號系統及異步信號系統中自發自收的傳感器， 傳感器i的測量值為：
[0036] 其中（Xl，yi)，i = l，…，η為傳感器i位置，（x，y)為目標物超聲波反射點位置，即為 目標物跟蹤預測位置X(k)的前兩項。
[0037] 對于異步信號系統中，若存在配對檢測，及某一傳感器收到的是另一傳感器發射 的超聲波，檢測距離為超聲波從另一傳感器到目標物反射點到該傳感器的距離。則傳感器i 的測量方程為：
[0039] 其中（xi，yi)，i = 1，…，n為傳感器i位置，（xj，yj)，j = l，…，n, j乒i為傳感器j位 置，（x，y)為目標物超聲波反射點位置，即為目標物跟蹤預測位置X(k)的前兩項。γ為參數， 由系統設計決定，若記錄超聲波傳播距離長度，則γ =1，若記錄超聲波傳播距離長度的一 半，則 γ =0.5〇
[0040] 基于每個傳感器的測量方程，結合傳感器的發射時序設計，可建立觀測矩陣，觀測 矩陣為：
[0041] h(X) = [hi(X),---,hi(X),---,hn(X)]T
[0042] 如果傳感器處于未激活狀態或無讀數，則1η(Χ)為零矩陣。
[0043] 步驟24、基于觀測矩陣建立觀測方程。觀測方程具體為：
[0044] Z(k)=h(X(k))+V(k)
[0045] 其中，Z(k)為觀測量，Z(k)eRn，V(k)為觀測噪聲，函數h( ·)為觀測方程。對于每 一個傳感器，觀測方程具體為：
[0046] Zi(k)=hi(X(k))+Vi(k)
[0047] 其中，Zdk)為傳感器i的觀測量，Vdk)為傳感器i的觀測噪聲，函數lu( ·)為傳感 器i的觀測方程。
[0048]其中，所述步驟3具體包括：
[0049]步驟31、觀測矩陣線性化。二維平面內超聲波傳感器對目標物距離的測量，即觀測 矩陣具有非線性成分。部分濾波器如粒子濾波器等具有直接處理非線性系統的能力，可跳 過次步驟直接進入步驟32。對于無法直接處理非線性系統的濾波器，如擴展卡爾曼濾波器、 無跡卡爾曼濾波器等，需要對觀測矩陣進行線性化處理。擴展卡爾曼濾波器，采用的是高階 項采取忽略或逼近措施對非線性環節線性化。利用泰勒二次截斷對觀測方程進行線性化處 理。具體公式如下：
[0051] 其中，Hi(k)為線性化后的觀測矩陣。
[0052] 無跡卡爾曼濾波器采用采用近似方法，對非線性系統進行線性化。常用的采樣策 略包括對稱采樣、單形采樣、三階矩偏度采樣以及高斯分布4階矩對稱采樣等。
[0053]步驟32、目標物進入跟蹤區域判斷與狀態初始化。根據傳統陣列傳感器的讀數判 斷目標物進入跟蹤區域，如最靠近陣列端點的兩個傳感器先后獲得有效的距離信息，則可 判斷目標物進行陣列傳感器的跟蹤區域。初始狀態的計算基于判斷目標物進入跟蹤區域時 傳感器的讀數計算獲得，其他參數的初始值可根據傳感器的特性相應設計。
[0054]步驟33、對具有有效數據的傳感器信息進行多傳感器信息融合。其中，擴展卡爾曼 濾波器多傳感器信息融合的方式由多種選擇，包括集中式、順序式和分布式。三種方法的算 法原理如圖1所示。濾波器如無跡卡爾曼濾波器、粒子濾波器等可采用集中式數據融合方法 進行。有效數據集中式信息融合算法選取具有有效讀數的傳感器的讀數值、相應的觀測矩 陣與線性化后的觀測轉移矩陣組成新的測量值矩陣和觀測轉移矩陣。
[0055] Zs(k) - [ZSi(k)ZSm(k)]T
[0056] hs(k) = [hS] (k),..., hSm (k)]T
[0057] Hs(k) = [HSi(k) HSm(k)]T
[0058] 其中，S1，…，SmSm個有有效讀數輸出的傳感器。
[0059] 步驟34、目標物系統狀態預測與更新。根據濾波器的跟蹤原理對利用實時傳感器 數據對目標物進行系統狀態的預測與更新。根據運動轉移方程目標物系統狀態的先驗概率 估計，根據傳感器的測量值對目標物的系統狀態進行更新，獲得后驗概率估計。應用步驟33 中獲得的新的測量值矩陣和觀測轉移矩陣：
[0000] a.預測
[0061 ] X(k+ l|k) = AX(kjk)
[0062] P(k+1 |k)=AP(k|k)AT+Q
[0063] b.更新
[0064] K(k+l)=P(k+l |k)Hs(k+l)T[Hs(k+l)P(k+l |k)Hs(k+l)T+Rs]T
[0066] P(k+1 |k+l) = [I-K(k+l)Hs(k+l)T]P(k+l|k)
[0067] 其中，父Ck|k - 1)表示k時刻的先驗概率估計，又Ck|k)表示k時刻的后驗概率估 計。匕為!!!階矩陣，對角線值為I2,其余值為0。對于UKF算法，預測過程為對每一個采樣點位置 的預測，誤差協方差矩陣為每個采樣點誤差協方差的加權。更新過程中，誤差協方差均由采 樣點協方差加權獲得。
[0068] 本發明的上述技術方案的有益效果如下：
[0069] 1)采用陣列式的設計和布置，擴展單個超聲波傳感器的探測范圍。
[0070] 2)通過陣列中多傳感器信息融合的方式，減小單個傳感器誤差引起的目標物的定 位誤差。
[0071 ] 3)可以實現對目標物動態的跟蹤，跟蹤的系統狀態信息可包括位置、速度、加速度 等。獲得目標物的速度信息，不但能夠有效的區分靜態目標物和動態目標物，減少駕駛輔助 系統中誤警的產生，也可以提高自車感知周圍目標物的能力。
【附圖說明】
[0072]圖1為三種多傳感器信息融合跟蹤算法原理；
[0073]圖la為集中式數據融合跟蹤原理；
[0074]圖lb為順序式數據融合跟蹤原理；
[0075]圖lc為分布式數據融合跟蹤原理；
[0076]圖2為一種8傳感器等距線性陣列設計，及其車輛上的安裝布置形式與坐標系設 計；
[0077]圖3a和圖3b為車輛側向目標物跟蹤實例的兩種典型跟蹤場景；
[0078]圖4a和圖4b為針對圖3a、圖3b中兩種典型場景的有效信號篩選方法。
【具體實施方式】
[0079]為使本發明要解決的技術問題、技術方案和優點更加清楚，下面將結合附圖及具 體實施例進行詳細描述。
[0080] 本發明采用跟蹤濾波器，如擴展卡爾曼濾波器(Extended Karman Filter，EKF)、 無跡卡爾曼濾波器(Unscented Karman Filter，UKF)、粒子濾波器(Particle Filter，PF) 等，通過多傳感器的信息融合對車輛周圍的目標物進行跟蹤以獲得目標物的位置和速度信 號等，實現車輛周圍超聲波傳感器可探測距離內的環境感知，為汽車的先進駕駛輔助系統 和自動駕駛的環境感知提供數據基礎。
[0081 ]為了實現上述目的，本發明實施例具有以下改進點：
[0082] 1、重新設計了采用陣列式超聲波傳感器布置；
[0083] 2、重新設計了陣列式傳感器系統的收發時序，可采用同步發射時序，亦可以采用 異步發射時序設計；
[0084] 3、針對陣列式超聲波傳感器的布置設計，和收發時序設計，設計相對應的跟蹤濾 波器跟蹤算法。
[0085] 下面結合附圖和某一場景應用實例對本發明進行詳細的描述。實例中采用如圖2 所示的，8個傳感器等距直線型陣列設計，布置在車輛側面，實例中傳感器間距s = 50cm，傳 感器坐標系如圖2所示。實例中傳感器陣列采用同步時序控制，即每一發射周期，所有傳感 器同時發射。實例跟蹤場景選取車輛側向目標物跟蹤，選取車輛側向環境中典型的兩種目 標物進行跟蹤，跟蹤場景如圖3a和圖3b所示。在兩種跟蹤場景中，實例參數選取自車速度 5km/h，柱狀目標物靜止，車輛目標物10km/h，目標物在y軸方向距離汽車2m。
[0086] 針對上述特定的陣列設計與應用場景實例，可以采用如下方法進行物體跟蹤，以 擴展卡爾曼濾波器為例，選取集中式數據融合方法：
[0087]步驟1、在跟蹤場景1中，桿狀目標物處于靜止狀態，考慮汽車在正常行駛過程中的 行駛狀態，一般分為勻速行駛，加速行駛與減速行駛狀態，從較長的行駛時間來看，可認為 正常行駛的車輛在進行加速度為恒值，即加加速度為〇的運動。在跟蹤場景2中，場景主要代 表正常行駛過程中，其他車輛相對于自車的超車過程。超車過程中，一般來說相對于勻速行 駛的被超車車輛，主動超車車輛采取加速超車或勻速超車的超車策略。因此，主動超車車輛 相對于被超車車輛的運動方程可認為是勻速或勻加速運動。加速度的變化率即加加速度可 認為是恒值，及加加速度為〇的運動。考慮加加速度受到一個系統隨機干擾量，故對于跟蹤 場景1與跟蹤場景2，在二維運動平面內，均可采用八階系統狀態變量表示目標物系統狀態， 系統狀態變量具體如下：
[0088] X(k) = [x, y, %> py, CLXJ %, o'x，dy]1
[0089] 步驟2、建立目標物運動轉移矩陣，具體如下：
[0091] 其中T為目標物跟蹤的跟蹤周期，即超聲波傳感器陣列系統的信號采集周期；
[0092] 基于運動狀態轉移矩陣建立運動狀態方程。對于二維平面內做線性運動物體，可 建立線性運動狀態方程：
[0093] X(k)=AX(k-l)+ff(k-l)
[0094] 其中X(k)為系統狀態變量，W(k)為系統過程噪聲，A為狀態轉移矩陣。
[0095]根據傳感器測距方程建立觀測矩陣。傳感器i的測量值為：
[0097]其中&1，71)4 = 1，~，」為傳感器位置，（^7)為目標物超聲波反射點位置，即為目 標物跟蹤預測位置Xk的前兩項。建立每個傳感器的觀測方為：
[0098] Zi(k)=hi(X(k))+Vi(k)
[0099] 其中，Zdk)為傳感器i的觀測量，Vdk)為傳感器i的觀測噪聲，函數lu( ·)為傳感 器i的觀測方程。
[0100] 步驟3、利用泰勒二次截斷對觀測方程進行線性化處理。具體公式如下：
[0102] 其中，Hi(k)為線性化后的觀測矩陣。
[0103] 基于最靠近陣列端點的兩個傳感器先后獲得有效的距離信息，判斷目標物進行陣 列傳感器的跟蹤區域，并開始實施跟蹤。初始系統狀態中，目標物初始位置利用該時刻兩個 傳感器的讀數利用三角定位法計算獲得，初始速度傳感器間距與這兩個傳感器第一次獲得 有效讀數的時間差坐商獲得，其他狀態參數初始值設為〇。其他參數的初始值根據傳感器的 特性相應設計，根據本實例采用的傳感器特性，不同傳感器之間讀數無相關性，R值如下：
[0105] Q為反應系統受到干擾的大小，考慮到目標物相對于自車做近似與自車平行的運 動，故其在X方向的誤差大于在y方向的誤差。本實例Q值如下：
[0107] 初始狀態估計協方差P(0)=Q。
[0108] 有效數據的篩選方法為:在對柱狀物的跟蹤中，由于柱狀物尺寸相對于傳感器陣 列較小，可近似認為所有傳感器的測量值形成的反射點為同一點，即所有傳感器的信號為 有效信號。在對車輛目標物的跟蹤中，由于車輛的尺寸相對于傳感器陣列量級上具有可比 性，隨著目標物逐漸進入傳感器的探測范圍，傳感器的測量值形成的反射點不再是同一個 點，采取一種簡單有效的方法對有效信號進行篩選。即選取每一跟蹤時刻車輛目標物估計 位置前方的傳感器信號為有效信號。有效信號篩選方法如圖4a和圖4b所示。
[0109] 采用集中式方法對具有有效數據的傳感器信息進行多傳感器信息融合。具體的系 統狀態預測與更新方法如下。線性化后的每個傳感器觀測方程為：
[0110] Zi(k)=Hi(k)X(k)+Vi(k)
[0111] 對于選取具有有效讀數的傳感器的讀數值與相應的線性化后的觀測轉移矩陣組 成新的測量值矩陣和觀測轉移矩陣。
[0112] Z.?(k) = [ZS：1 (/c),; z，m(k.)]T
[0113] Hs(k) = \HSi(k),..,,HSm(k)]T
[0114] 代入預測和更新方程進行預測與更新。其中- 1)表示k時刻的先驗概率估 計，戈以| 表示k時刻的后驗概率估計。
[0115] a.預測
[0116] X(k + l\k) = AX{k\k)
[0117] P(k+1 |k)=AP(k|k)AT+Q
[0118] b.更新
[0119] K(k+l)=P(k+l |k)Hs(k+l)T[Hs(k+l)P(k+l |k)Hs(k+l)T+Rs]T
[0120] X(k + l|fe + 1) - X(Jc + l|fe) + K(k + l)(Zs(/c + 1) ·~ hs(X{k -I-
[0121] P(k+1 |k+l) = [I-K(k+l)Hs(k+l)T]P(k+l|k)
[0122] 其中，Rs為m階矩陣，對角線值為l2,其余值為0。
[0123] 經過實驗驗證，本發明提出的算法針案例中的跟蹤場景1和2能夠有效實現對目標 物的跟蹤，跟蹤精度優于采用三角定位法。
[0124] 由上述分析可以看出，本發明實施例具有以下優點：
[0125] 1、采用陣列式的設計和布置，擴展單個超聲波傳感器的探測范圍。
[0126] 2、通過陣列中多傳感器信息融合的方式，減小單個傳感器誤差引起的目標物的定 位誤差。
[0127] 3、可以實現對目標物動態的跟蹤，跟蹤的系統狀態信息可包括位置、速度、加速度 等。獲得目標物的速度信息，不但能夠有效的區分靜態目標物和動態目標物，減少駕駛輔助 系統中誤警的產生，也可以提高自車感知周圍目標物的能力。
[0128] 以上所述是本發明的優選實施方式，應當指出，對于本技術領域的普通技術人員 來說，在不脫離本發明所述原理的前提下，還可以作出若干改進和潤飾，這些改進和潤飾也 應視為本發明的保護范圍。
【主權項】
1. 一種基于超聲波傳感器陣列的物體跟蹤方法，其特征在于，包括： 步驟1、建立1階的系統狀態變量擬合目標物的系統狀態； 步驟2、根據目標物運動特點建立運動狀態方程，根據傳感器測距方程建立觀測方程； X(k)=f(X(k-l))+W 化-1) Z(k)=h(X(k))+V化） 其中，X化)為系統狀態變量X化）eRi，W化)為系統過程噪聲，過程噪聲協方差矩陣為Q;Z (k)為觀測量Z化）er，v(k)為觀測噪聲，觀測噪聲協方差矩陣為R;函數f(.)為系統轉移 方程，函數Μ ·)為觀測方程； 步驟3、利用跟蹤濾波算法對目標物進行跟蹤，對于運動或觀測方程存在非線性化部分 的系統，可利用能夠處理非線性系統的濾波器如擴展卡爾曼濾波器、無跡卡爾曼濾波器、粒 子濾波器等進行目標物跟蹤。2. 根據權利要求1所述的基于超聲波傳感器陣列的物體跟蹤方法，其特征在于，所述步 驟1具體包括： 步驟11、分析應用場景中目標物在二維平面內的在傳感器陣列坐標系中的系統狀態特 點，進行系統狀態歸類； 步驟12、根據系統狀態類型，建立合適的系統狀態變量擬合目標物系統狀態； 若目標物系統狀態是靜止狀態，則可利用四階的系統狀態變量擬合目標物系統狀態， 系統狀態變量如下： Xk=[x，y，Vx，Vy]T 其中，X，y為目標物在X，y坐標系中位置的橫縱坐標值，vx，vy為目標物在X方向與y方向 的速度，由于目標物系統狀態可近似認為是靜止狀態，Vx，Vy可認為是高斯分布的隨機誤差 值;T表示矩陣的轉置;四階是指1 = 4; 若目標物系統狀態是勻速運動狀態，則可利用六階的系統狀態變量擬合目標物系統狀 態，系統狀態變量如下： Xk=[x，y，Vx，Vy，ax，ay]T 其中，X，y為目標物在X，y坐標系中位置的橫縱坐標值，vx，vy為目標物在X方向與y方向 的速度，ax，ay為目標物在X方向與y方向的加速度，由于目標物系統狀態可近似認為是勻速 運動狀態，ax, ay可認為是高斯分布的隨機誤差值;六階是指1 = 6; 若目標物系統狀態是勻加速或勻減速運動狀態，則可利用八階的系統狀態變量擬合目 標物系統狀態，其中系統狀態變量如下：其中，X，y為目標物在X，y坐標系中位置的橫縱坐標值，vx，vy為目標物在X方向與y方向 的速度，ax，ay為目標物在X方向與y方向的加速度，3x1 為目標物在X方向與y方向的加加 速度，由于目標物系統狀態可近似認為是勻加速或勻減速運動狀態，3x，a：y為是高斯分布 的隨機誤差值;八階是指1 = 8。3. 根據權利要求1所述的基于超聲波傳感器陣列的物體跟蹤方法，其特征在于，所述步 驟2具體包括： 步驟21、將物體系統的狀態轉移過程描述為一個離散時間的隨機過程;根據目標物運 動特點建立運動狀態方程，對于二維平面內做線性運動物體，首先獲取離散運動轉移矩陣； 為1階系統狀態變量建立1X1階運動狀態轉移矩陣，具體如下：其中T為目標物跟蹤的跟蹤周期，即超聲波傳感器陣列系統的信號采集周期； 步驟22、基于運動狀態轉移矩陣建立運動狀態方程對于二維平面內做線性運動物體， 可建立線性運動狀態方程： X(k)=AX 化-1)+W 化-1) 其中X化)為系統狀態變量，X化）eRi，W化)為系統過程噪聲，A為狀態轉移矩陣； 步驟23、根據傳感器測距方程建立觀測矩陣;根據超聲波傳感器的測距原理，超聲波探 測傳感器到目標點的距離;傳感器i的測量值為：其中（xi，yi) 4 = 1，…，η為傳感器位置，（x，y)為目標物超聲波反射點位置，即為目標物 跟蹤預測位置X化)的前兩項;基于每個傳感器的測量值方程，結合傳感器的發射時序設計， 可建立觀測矩陣，例如對于同步發射的傳感器時序設計，即每一發射周期所有傳感器都是 同時發射，觀測矩陣為： h(X) =比 l(X)，...，hi(X)，...，hn(X)]T; 對于異步信號系統中，若存在配對檢測，及某一傳感器收到的是另一傳感器發射的超 聲波，檢測距離為超聲波從另一傳感器到目標物反射點到該傳感器的距離;則傳感器i的測 量方程為：其中（xi，yi)，i = l，…，n為傳感器i位置，（xj，yj)，j = l，…，η, j聲i為傳感器j位置，（X， y)為目標物超聲波反射點位置，即為目標物跟蹤預測位置X化)的前兩項。丫為參數，由系統 設計決定，若記錄超聲波傳播距離長度，則丫 = 1，若記錄超聲波傳播距離長度的一半，則丫 = 0.5； 基于每個傳感器的測量方程，結合傳感器的發射時序設計，可建立觀測矩陣，觀測矩陣 為： h(X) =比 l(X)，...，hi(X)，...，hn(X)]T 如果傳感器處于未激活狀態或無讀數，則hi (X)為零矩陣； 步驟24、基于觀測矩陣建立觀測方程;觀測方程具體為： Z(k)=h(X(k))+V化） 其中，Z化)為觀測量，Z化）er，Wk)為觀測噪聲，函數K .)為觀測方程;對于每一個傳 感器，觀測方程具體為： Zi(k)=hi(X 化））+Vi(k) 其中，Zi化)為傳感器i的觀測量，Vi化)為傳感器i的觀測噪聲，函數hi( ·)為傳感器i的 觀測方程。4.根據權利要求1所述的基于超聲波傳感器陣列的物體跟蹤方法，其特征在于，所述步 驟3具體包括： 步驟31、觀測矩陣線性化;二維平面內超聲波傳感器對目標物距離的測量，即觀測矩陣 具有非線性成分;部分濾波器如粒子濾波器等具有直接處理非線性系統的能力，可跳過次 步驟直接進入步驟32;對于無法直接處理非線性系統的濾波器，如擴展卡爾曼濾波器、無跡 卡爾曼濾波器等，需要對觀測矩陣進行線性化處理;擴展卡爾曼濾波器，采用的是高階項采 取忽略或逼近措施對非線性環節線性化;利用泰勒二次截斷對觀測方程進行線性化處理； 具體公式如下：其中，出化)為線性化后的觀測矩陣； 無跡卡爾曼濾波器采用采用近似方法，對非線性系統進行線性化;常用的采樣策略包 括對稱采樣、單形采樣、Ξ階矩偏度采樣W及高斯分布4階矩對稱采樣等； 步驟32、目標物進入跟蹤區域判斷與狀態初始化;根據傳統陣列傳感器的讀數判斷目 標物進入跟蹤區域，如最靠近陣列端點的兩個傳感器先后獲得有效的距離信息，則可判斷 目標物進行陣列傳感器的跟蹤區域;初始狀態的計算基于判斷目標物進入跟蹤區域時傳感 器的讀數計算獲得，其他參數的初始值可根據傳感器的特性相應設計； 步驟33、對具有有效數據的傳感器信息進行多傳感器信息融合;其中所述擴展卡爾曼 濾波器多傳感器信息融合的方式可W為集中式或順序式或分布式； 其中濾波器為無跡卡爾曼濾波器或粒子濾波器，用集中式數據融合方法進行;有效數 據集中式信息融合算法選取具有有效讀數的傳感器的讀數值、相應的觀測矩陣與線性化后 的觀測轉移矩陣組成新的測量值矩陣和觀測轉移矩陣；其中，SI，…，Sm為m個有有效讀數輸出的傳感器； 步驟34、根據濾波器的跟蹤原理對利用實時傳感器數據對目標物進行系統狀態的預測 與更新，具體包括:根據運動轉移方程目標物系統狀態的先驗概率估計，根據傳感器的測量 值對目標物的系統狀態進行更新，獲得后驗概率估計。5.根據權利要求4所述的基于超聲波傳感器陣列的物體跟蹤方法，其特征在于，其中所 述步驟34具體包括:應用步驟33中獲得的測量值矩陣和觀測轉移矩陣對目標物進行系統狀 態的預測與更新，具體包括： a.預測其中，k -巧表示k時刻的先驗概率估計，X(k|k)表示k時刻的后驗概率估計;Rs 為m階矩陣，對角線值為I2,其余值為0; 對于UKF算法，預測過程為對每一個采樣點位置的預測，誤差協方差矩陣為每個采樣點 誤差協方差的加權;更新過程中，誤差協方差均由采樣點協方差加權獲得。
【文檔編號】G01S7/521GK106093951SQ201610390557
【公開日】2016年11月9日
【申請日】2016年6月6日 公開號201610390557.X, CN 106093951 A, CN 106093951A, CN 201610390557, CN-A-106093951, CN106093951 A, CN106093951A, CN201610390557, CN201610390557.X
【發明人】李升波, 成波, 俞佳瑩, 李克強, 羅禹貢, 鄒若冰, 李國法, 趙國偉
【申請人】清華大學