[發明專利]一種使用單架無人機追蹤移動源的方法

有效

• 申請號: 202110703618.4
• 申請日: 2021-06-24
• 公開（公告）號: CN113359837B
• 公開（公告）日: 2022-10-04
• 發明（設計）人: 單馮;李增保;羅軍舟;楊明;吳文甲
• 申請（專利權）人: 南京逸智網絡空間技術創新研究院有限公司
• 主分類號: G05D1/10
• 分類號: G05D1/10;B64C39/02
• 代理公司: 南京經緯專利商標代理有限公司 32200
• 代理人: 陸燁

1.一種使用單架無人機追蹤移動源的方法，其特征在于，具體包括如下步驟：

步驟1：安裝在單架無人機上的UWB傳感器實時采集單架無人機與移動源之間的距離；

步驟2：當移動源按照某個速度方向勻速直線運動時，根據UWB傳感器實時采集到的距離以及相對速度變換的方法計算移動源相對于單架無人機的相對速度，從而估計移動源的速度矢量；

步驟3：根據移動源的速度矢量，計算得到單架無人機相對于移動源的相對速度，并基于多點數據融合策略，估計移動源的位置；

所述步驟1中UWB傳感器采集到的單架無人機與移動源之間的距離為：

y_t＝f(s_t,x_t)+ε

f(s_t,x_t)表示第t次采樣時無人機與移動源之間的真實距離，ε為UWB傳感器固有的測量誤差，ε服從N(0,δ²)，N(.)為高斯分布，δ為標準差；

所述步驟3具體為：

步驟3.1：將單架無人機與移動源所在的空間視為二維平面，對該二維平面進行柵格化，并為二維平面中每個柵格點賦予初始的權重值；將二維平面內所有的柵格點的集合作為后候選柵格點集合Φ；

步驟3.2：令迭代計算次數t’＝1；

步驟3.3：當t’＝1時，單架無人機以預設的速度按照預設的方向飛行，當t’1時，單架無人機以預設的速度朝著第t’-1次估計的移動源的位置飛行；

第t’次迭代計算時，在單架無人機飛行過程中UWB傳感器進行2k’+1次采樣，將2k’+1次采樣得到的數據組成第t’段數據段，k’為預設的正整數，k’其中ΔL為單架無人機在UWB傳感器的采樣時間間隔Δt內以相對速度移動的距離；

步驟3.4：計算第t’次迭代計算過程中UWB傳感器第k’次采樣時無人機與移動源之間的真實距離f(s_k’,x_k’)的估計值：

其中，y_k’+j’表示第t’次迭代計算過程中UWB傳感器第k’+j’次采集得到的數據；

步驟3.5：更新后候選柵格點集合Φ：若Φ中第q個柵格點Φ_q滿足條件：則保留該柵格點，否則在Φ中刪除該柵格點，q＝1，2，…，W，W為更新前Φ中柵格點的總個數，q_x，y為柵格點Φ_q在二維平面中的坐標，x_t’k’為第t’次迭代計算中UWB傳感器進行第k’次采樣時單架無人機在二維平面中的坐標；其中的表達式為：

其中σ＝3δ；

步驟3.6：為計算更新后的Φ中每個柵格點重新賦予權重：

其中，Φ_q’為更新后的Φ中第q’個柵格點，w_t’(Φ_q’)為第t’次迭代計算時柵格點Φ_q’的權重；y為隨機變量，exp(.)為指數函數；q’＝1，2…，Q；Q為更新后的Φ中柵格點的總個數，q’_x，y為柵格點Φ_q’在二維平面上的坐標；

步驟3.7：按照如下公式估計移動源的位置無人機朝著估計的位置飛行一段時間t_f后，若UWB傳感器采集到的距離值在預設的距離范圍之內，則將作為移動源的位置；否則迭代計算次數加1，并轉步驟3.3；

當t’＝1時，按照如下公式估計移動源的位置：

v_s為移動源的速度矢量；

當t’＞1時，按照如下公式估計移動源的位置：

其中ΔT₁為第t’次迭代計算過程中UWB傳感器第k’次采樣與第t’-1次迭代計算過程中UWB傳感器第k’次采樣的時間差；

所述步驟2具體為：

步驟2.1：令迭代次數e＝1，建立直角坐標系；

步驟2.2：第e次迭代計算時，單架無人機以第e次預設的速度v_e按照第e次預設的方向飛行，在飛行過程中UWB傳感器進行2k+1次采樣；將該2k+1次采樣得到的數據組合成第e段數據；k為預設的整數系數；

步驟2.3：根據第e段數據計算移動源的相對速度：

其中，i＝k，y_i為在第e次迭代計算中UWB傳感器第i次采集得到的數據，y_i+j表示在第e次迭代計算中UWB傳感器第i+j次采集得到的數據，Δt為UWB傳感器的采樣時間間隔；

步驟2.4：將v_e分解成水平方向上的速度矢量v_e，x和垂直方向上的速度矢量v_e，y，根據v_e，x和v_e，y得到v_e在直角坐標系上的坐標，以v_e在直角坐標系上的坐標為圓點，相對速度的矢量長度為半徑，得到第e次迭代計算時的圓形區域；

步驟2.5：若e＝1，則令迭代次數加1，重新預設k的值，并返回步驟2.2；否則轉步驟2.6；

步驟2.6：若e＝2且兩個圓形區域之間存在切點，根據該切點在直角坐標系上的位置得到移動源的速度矢量，并停止計算；若e＞2則轉步驟2.7；若e＝2且兩個圓形區域之間不存在切點，則令迭代次數加1，重新預設k的值，并返回步驟2.2；

步驟2.7：判斷e個圓形區域之間是否有共同的交點，若存在一個共同的交點，則根據該交點在直角坐標系上的位置得到移動源的速度矢量；若存在多個共同的交點，則剔除離散的交點，在剩余的交點中隨機選擇一個交點，根據選擇的交點在直角坐標系上的位置得到移動源的速度矢量；若不存在交點，則令迭代次數加1，重新預設k的值，并返回步驟2.2。