[發(fā)明專利]基于卡爾曼濾波和粒子濾波的目標(biāo)位置計(jì)算方法及系統(tǒng)

在審

• 申請(qǐng)?zhí)枺?/td>: 202110472723.1
• 申請(qǐng)日: 2021-04-29
• 公開（公告）號(hào): CN113160281A
• 公開（公告）日: 2021-07-23
• 發(fā)明（設(shè)計(jì)）人: 于興虎;王春翔
• 申請(qǐng)（專利權(quán)）人: 寧波智能裝備研究院有限公司
• 主分類號(hào): G06T7/277
• 分類號(hào): G06T7/277;G06T7/73;G06T5/20
• 代理公司: 北京高沃律師事務(wù)所 11569
• 代理人: 杜陽(yáng)陽(yáng)

1.一種基于卡爾曼濾波和粒子濾波的目標(biāo)位置計(jì)算方法，其特征在于，包括：

獲取目標(biāo)運(yùn)動(dòng)圖像中的檢測(cè)位置；所述檢測(cè)位置由檢測(cè)算法檢測(cè)；

根據(jù)所述檢測(cè)位置利用卡爾曼濾波器計(jì)算目標(biāo)的第一初始位置；所述卡爾曼濾波器是基于勻速模型的卡爾曼濾波器；

根據(jù)所述檢測(cè)位置利用粒子濾波器計(jì)算目標(biāo)的第二初始位置；

根據(jù)所述卡爾曼濾波器檢測(cè)目標(biāo)是否出現(xiàn)連續(xù)運(yùn)動(dòng)，得到檢測(cè)結(jié)果；

根據(jù)所述檢測(cè)結(jié)果確定所述卡爾曼濾波器的權(quán)重和所述粒子濾波器的權(quán)重；

根據(jù)所述第一初始位置、所述第二初始位置、所述卡爾曼濾波器的權(quán)重和所述粒子濾波器的權(quán)重計(jì)算目標(biāo)位置。

2.根據(jù)權(quán)利要求1所述的基于卡爾曼濾波和粒子濾波的目標(biāo)位置計(jì)算方法，其特征在于，所述第一初始位置計(jì)算過程如下：

根據(jù)所述檢測(cè)位置對(duì)所述卡爾曼濾波器進(jìn)行初始化；

根據(jù)上一幀目標(biāo)運(yùn)動(dòng)圖像中的檢測(cè)位置利用初始化后的卡爾曼濾波器計(jì)算當(dāng)前幀目標(biāo)運(yùn)動(dòng)圖像中的第一計(jì)算位置；

根據(jù)所述第一計(jì)算位置和當(dāng)前幀目標(biāo)運(yùn)動(dòng)圖像中的檢測(cè)位置對(duì)初始化后的卡爾曼濾波器進(jìn)行更新；

根據(jù)更新后的卡爾曼濾波器計(jì)算第一初始位置。

3.根據(jù)權(quán)利要求1所述的基于卡爾曼濾波和粒子濾波的目標(biāo)位置計(jì)算方法，其特征在于，所述第二初始位置計(jì)算過程如下：

根據(jù)所述檢測(cè)位置對(duì)粒子濾波器進(jìn)行初始化；

根據(jù)上一幀目標(biāo)運(yùn)動(dòng)圖像中的檢測(cè)位置利用初始化后的粒子濾波器計(jì)算當(dāng)前幀目標(biāo)運(yùn)動(dòng)圖像中的第二計(jì)算位置；

根據(jù)所述第二計(jì)算位置和當(dāng)前幀目標(biāo)運(yùn)動(dòng)圖像中的檢測(cè)位置對(duì)初始化后的粒子濾波器進(jìn)行更新；

根據(jù)更新后的粒子濾波器計(jì)算第二初始位置。

4.根據(jù)權(quán)利要求2所述的基于卡爾曼濾波和粒子濾波的目標(biāo)位置計(jì)算方法，其特征在于，所述根據(jù)所述卡爾曼濾波器檢測(cè)目標(biāo)是否出現(xiàn)連續(xù)運(yùn)動(dòng)，得到檢測(cè)結(jié)果，具體包括：

根據(jù)所述第一計(jì)算位置和當(dāng)前幀目標(biāo)運(yùn)動(dòng)圖像中的檢測(cè)位置計(jì)算初始化后的卡爾曼濾波器的殘差；

根據(jù)所述初始化后的卡爾曼濾波器的殘差計(jì)算初始化后的卡爾曼濾波器歸化后的殘差；

判斷所述歸化后的殘差是否大于設(shè)定閾值；

若是，則判斷目標(biāo)沒有出現(xiàn)連續(xù)運(yùn)動(dòng)；

若否，則判斷目標(biāo)出現(xiàn)連續(xù)運(yùn)動(dòng)；

所述檢測(cè)結(jié)果包括目標(biāo)沒有出現(xiàn)連續(xù)運(yùn)動(dòng)和目標(biāo)出現(xiàn)連續(xù)運(yùn)動(dòng)。

5.根據(jù)權(quán)利要求4所述的基于卡爾曼濾波和粒子濾波的目標(biāo)位置計(jì)算方法，其特征在于，所述根據(jù)所述檢測(cè)結(jié)果確定所述卡爾曼濾波器的權(quán)重和所述粒子濾波器的權(quán)重，具體包括：

若檢測(cè)結(jié)果為目標(biāo)沒有出現(xiàn)連續(xù)運(yùn)動(dòng)，則所述卡爾曼濾波器的權(quán)重小于所述粒子濾波器的權(quán)重；

若檢測(cè)結(jié)果為目標(biāo)出現(xiàn)連續(xù)運(yùn)動(dòng)，則所述卡爾曼濾波器的權(quán)重大于所述粒子濾波器的權(quán)重；

所述卡爾曼濾波器的權(quán)重與所述粒子濾波器的權(quán)重之和為1。

6.根據(jù)權(quán)利要求1所述的基于卡爾曼濾波和粒子濾波的目標(biāo)位置計(jì)算方法，其特征在于，所述目標(biāo)位置的計(jì)算公式如下：

其中，X_k為第k幀目標(biāo)運(yùn)動(dòng)圖像中的目標(biāo)位置，為卡爾曼濾波器v在第k幀目標(biāo)運(yùn)動(dòng)圖像中的權(quán)重，H^v為卡爾曼濾波器v的觀測(cè)矩陣，為卡爾曼濾波器v在第k幀目標(biāo)運(yùn)動(dòng)圖像中的狀態(tài)向量，由第一初始位置得到，為粒子濾波器p在第k幀目標(biāo)運(yùn)動(dòng)圖像中的權(quán)重，為粒子濾波器p計(jì)算出的第k幀目標(biāo)運(yùn)動(dòng)圖像中的第二初始位置。