[發(fā)明專利]基于李雅普諾夫制導(dǎo)向量和粒子濾波的機(jī)動(dòng)目標(biāo)跟蹤方法

在審

• 申請(qǐng)?zhí)枺?/td>: 201711441820.4
• 申請(qǐng)日: 2017-12-27
• 公開（公告）號(hào): CN108319130A
• 公開（公告）日: 2018-07-24
• 發(fā)明（設(shè)計(jì)）人: 游科友;董斐;張家綺;宋士吉
• 申請(qǐng)（專利權(quán)）人: 清華大學(xué)
• 主分類號(hào): G05B13/04
• 分類號(hào): G05B13/04
• 代理公司: 北京清亦華知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 11201
• 代理人: 廖元秋

1.一種基于李雅普諾夫制導(dǎo)向量和粒子濾波的機(jī)動(dòng)目標(biāo)跟蹤方法，其特征在于，該方法包括以下步驟：

(1)當(dāng)無人機(jī)確定待跟蹤的機(jī)動(dòng)目標(biāo)后，對(duì)機(jī)動(dòng)目標(biāo)建立動(dòng)力學(xué)模型，表達(dá)式如下：

其中,為待跟蹤的機(jī)動(dòng)目標(biāo)在k時(shí)刻的狀態(tài)向量，上標(biāo)t表示機(jī)動(dòng)目標(biāo)，下標(biāo)k表示第k個(gè)采樣時(shí)刻，為機(jī)動(dòng)目標(biāo)k時(shí)刻在慣性坐標(biāo)系中的三維坐標(biāo)位置，為機(jī)動(dòng)目標(biāo)k時(shí)刻在慣性坐標(biāo)系下X方向和Y方向的速度；F_k為k時(shí)刻的機(jī)動(dòng)目標(biāo)系統(tǒng)矩陣，B_k為k時(shí)刻的機(jī)動(dòng)目標(biāo)輸入矩陣，Π_k為k時(shí)刻的機(jī)動(dòng)目標(biāo)噪聲矩陣，表達(dá)式分別如下：

其中，τ為采樣時(shí)間間隔，是k時(shí)刻的過程噪聲，代表機(jī)動(dòng)目標(biāo)在慣性坐標(biāo)系下X方向和Y方向的加速度不確定性和Z方向的速度不確定性，分別為機(jī)動(dòng)目標(biāo)在慣性坐標(biāo)系下X方向速度不確定性的方差、Y方向速度不確定性的方差和Z方向速度不確定性的方差；是機(jī)動(dòng)目標(biāo)k時(shí)刻分別在X方向和Y方向的控制輸入，γ_k是k時(shí)刻的三狀態(tài)的馬爾科夫鏈，分別對(duì)應(yīng)k時(shí)刻機(jī)動(dòng)目標(biāo)的三種控制輸入：直行、左轉(zhuǎn)、右轉(zhuǎn)；

令s＝{1,2,3}代表機(jī)動(dòng)目標(biāo)三種不同的控制輸入：1代表直行，2代表左轉(zhuǎn)，3代表右轉(zhuǎn)，則機(jī)動(dòng)目標(biāo)k時(shí)刻到k+1時(shí)刻三種控制輸入的概率轉(zhuǎn)移矩陣為：

(2)采用粒子濾波方法并結(jié)合光學(xué)相機(jī)提供的量測(cè)信息，對(duì)機(jī)動(dòng)目標(biāo)的位置和速度進(jìn)行估計(jì)；

定義：Γ_k＝{γ₀,...,γ_k}表示機(jī)動(dòng)目標(biāo)前k時(shí)刻的三狀態(tài)的馬爾科夫鏈集合，Z_k＝{z₀,...,z_k}表示前k時(shí)刻無人機(jī)搭載相機(jī)對(duì)機(jī)動(dòng)目標(biāo)的量測(cè)集合，U_k＝{u^t(γ₀),...,u^t(γ_k)}表示前k時(shí)刻機(jī)動(dòng)目標(biāo)的控制輸入集合，其中0時(shí)刻為無人機(jī)開始跟蹤機(jī)動(dòng)目標(biāo)的時(shí)刻；具體步驟如下：

(2-1)利用粒子濾波方法對(duì)粒子進(jìn)行初始化；

k＝0時(shí)刻，從p(γ₀|γ_-1)隨機(jī)生成N個(gè)粒子其中γ_-1＝1，并使Σ_0|0＝Σ₀，為第i個(gè)粒子-1時(shí)刻的重要性權(quán)值，N為粒子總數(shù)，和Σ_0|0分別為從0時(shí)刻到0時(shí)刻的機(jī)動(dòng)目標(biāo)狀態(tài)估計(jì)和估計(jì)方差矩陣，x₀和Σ₀分別為粒子濾波方法設(shè)定的機(jī)動(dòng)目標(biāo)初始狀態(tài)和初始方差矩陣Σ₀＝α*diag(5)，α＞0，；

(2-2)對(duì)每個(gè)粒子進(jìn)行重要性采樣；具體步驟如下：

(2-2-1)令當(dāng)前時(shí)刻為k，k＞0，更新對(duì)每個(gè)粒子的狀態(tài)預(yù)測(cè)和預(yù)測(cè)方差矩陣，表達(dá)式分別如下：

式中，和分別為粒子i從k-1時(shí)刻到k時(shí)刻的狀態(tài)預(yù)測(cè)和預(yù)測(cè)方差矩陣；

針對(duì)機(jī)動(dòng)目標(biāo)狀態(tài)向量的最小方差濾波方法表示為：

式中，表示粒子i從k時(shí)刻到k時(shí)刻的狀態(tài)預(yù)測(cè)；

采用全概率公式對(duì)進(jìn)行分解：

式中，是高斯概率密度；采用N個(gè)粒子去近似p(Γ_k-1|Z_k)，表示第i個(gè)粒子前k時(shí)刻的三狀態(tài)的馬爾科夫鏈集合：

式中，表示第i個(gè)粒子k-1時(shí)刻的重要性權(quán)值；

(2-2-2)對(duì)于每個(gè)粒子i，i＝1,...,N，更新k-1時(shí)刻的重要性權(quán)值，表達(dá)式如下：

其中，z_k為無人機(jī)中光學(xué)相機(jī)k時(shí)刻提供的量測(cè)信息：z_k＝[b_k c_k]^T，b_k和c_k為像素點(diǎn)在成像平面內(nèi)k時(shí)刻的二維坐標(biāo)位置，z_k＝h(X_k)+w_k，其中是k時(shí)刻的獨(dú)立高斯噪聲，R是量測(cè)噪聲的協(xié)方差矩陣，且R＝diag(σ_b,σ_c)，σ_b和σ_c是量測(cè)噪聲的標(biāo)準(zhǔn)差，h(X_k)是機(jī)動(dòng)目標(biāo)在成像平面內(nèi)k時(shí)刻的真實(shí)二維坐標(biāo)，其定義為：

式中，和X_k分別是無人機(jī)與機(jī)動(dòng)目標(biāo)的相對(duì)位置在相機(jī)坐標(biāo)系與慣性坐標(biāo)系中的表示，是無人機(jī)在k時(shí)刻的狀態(tài)向量，C_ci是慣性坐標(biāo)系到相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣；

量測(cè)方程的雅可比矩陣表示為：

式中，和分別為向量的第一個(gè)、第二個(gè)和第三個(gè)元素；

(2-3)對(duì)粒子進(jìn)行重采樣；具體步驟如下：

(2-3-1)計(jì)算粒子有效性并進(jìn)行判定；

粒子有效性的計(jì)算表達(dá)式如下：

如果則進(jìn)行重采樣，其中N_T是預(yù)設(shè)的粒子有效性閾值，按照概率分布從中生成N個(gè)新的粒子進(jìn)入步驟(2-3-2)；如果則進(jìn)入步驟(2-4)；

(2-3-2)用新產(chǎn)生的粒子代替舊的粒子，使得進(jìn)入步驟(2-4)；

(2-4)結(jié)合光學(xué)相機(jī)提供的量測(cè)信息，輸出對(duì)機(jī)動(dòng)目標(biāo)的位置估計(jì)和速度估計(jì)；具體步驟如下：

(2-4-1)對(duì)于每一個(gè)執(zhí)行如下過程：

式中，表示第i個(gè)粒子k時(shí)刻的卡爾曼濾波增益，和分別表示第i個(gè)粒子從k時(shí)刻到k時(shí)刻的狀態(tài)預(yù)測(cè)和預(yù)測(cè)方差矩陣；

(2-4-2)量測(cè)更新；

其中，是對(duì)的估計(jì)，即Σ_k|k是從k時(shí)刻到k時(shí)刻對(duì)的估計(jì)方差矩陣；即為基于粒子濾波方法k時(shí)刻生成的對(duì)機(jī)動(dòng)目標(biāo)位置估計(jì)的輸出結(jié)果，為基于粒子濾波方法k時(shí)刻生成的對(duì)機(jī)動(dòng)目標(biāo)慣性坐標(biāo)系下X方向和Y方向的速度估計(jì)的輸出結(jié)果；

(3)利用李雅普諾夫制導(dǎo)向量法進(jìn)行無人機(jī)跟蹤路徑規(guī)劃，得到無人機(jī)進(jìn)行機(jī)動(dòng)目標(biāo)跟蹤的控制量；具體步驟如下：

(3-1)利用李雅普諾夫制導(dǎo)向量法進(jìn)行無人機(jī)跟蹤路徑規(guī)劃；

假設(shè)待跟蹤的機(jī)動(dòng)目標(biāo)為靜止，則無人機(jī)k時(shí)刻的期望速度為：

式中，為無人機(jī)k時(shí)刻在慣性坐標(biāo)系下X方向和Y方向期望的相對(duì)速度，表示無人機(jī)k時(shí)刻在慣性坐標(biāo)系下X方向和Y方向的位置，u^d和r^d分別表示預(yù)定的盤旋速度和盤旋軌跡半徑；當(dāng)r_k＞r^d時(shí)，無人機(jī)向機(jī)動(dòng)目標(biāo)靠近直到期望的圓形軌道；當(dāng)r_k＜r^d時(shí)，無人機(jī)遠(yuǎn)離機(jī)動(dòng)目標(biāo)直到期望的圓形軌道；當(dāng)r_k＝r^d時(shí)，無人機(jī)在期望的圓形軌道上繞機(jī)動(dòng)目標(biāo)作盤旋運(yùn)動(dòng)；

將轉(zhuǎn)換到極坐標(biāo)為：

直角坐標(biāo)系與極坐標(biāo)系的關(guān)系為：

x_k＝r_kcosθ_k

y_k＝r_ksinθ_k

設(shè)計(jì)李雅普諾夫函數(shù)V為：

將V對(duì)時(shí)間求導(dǎo)：

當(dāng)且僅當(dāng)r_k＝0時(shí)，則無人機(jī)運(yùn)動(dòng)軌跡漸近收斂到期望的圓形軌道；

當(dāng)機(jī)動(dòng)目標(biāo)移動(dòng)時(shí)，無人機(jī)k時(shí)刻的期望速度和前進(jìn)方向表達(dá)式分別如下：

式中，和分別表示無人機(jī)k時(shí)刻的期望速度和前進(jìn)方向；

(3-2)計(jì)算無人機(jī)進(jìn)行機(jī)動(dòng)目標(biāo)跟蹤的控制量；

基于無人機(jī)期望的前進(jìn)速度前進(jìn)方向和無人機(jī)的當(dāng)前狀態(tài)采用比例控制方法控制無人機(jī)跟蹤控制指令，實(shí)現(xiàn)對(duì)存在未知控制輸入的地面機(jī)動(dòng)目標(biāo)的盤旋跟蹤：

式中，和為無人機(jī)k時(shí)刻的控制量，分別控制無人機(jī)的前進(jìn)速度和前進(jìn)方向，和分別表示無人機(jī)k時(shí)刻的速度和前進(jìn)方向，K₁和K₂為控制增益，和分別是和對(duì)時(shí)間的微分；

(4)令k＝k+1，對(duì)于i＝1,...,N，重新生成N個(gè)粒子重新返回步驟(2-2-1)，進(jìn)行下一時(shí)刻無人機(jī)對(duì)于地面機(jī)動(dòng)目標(biāo)的盤旋跟蹤。