[發(fā)明專利]一種用于轉(zhuǎn)彎機(jī)動(dòng)目標(biāo)的軌跡起始方法及系統(tǒng)

有效

• 申請?zhí)枺?/td>: 201710573188.2
• 申請日: 2017-07-14
• 公開（公告）號(hào): CN107390631B
• 公開（公告）日: 2019-08-09
• 發(fā)明（設(shè)計(jì)）人: 劉宗香;朱小平;李良群;謝維信
• 申請（專利權(quán)）人: 深圳大學(xué)
• 主分類號(hào): G05B19/19
• 分類號(hào): G05B19/19
• 代理公司: 深圳市恒申知識(shí)產(chǎn)權(quán)事務(wù)所(普通合伙) 44312
• 代理人: 袁文英

1.一種用于轉(zhuǎn)彎機(jī)動(dòng)目標(biāo)的軌跡起始方法，其特征在于，所述方法包括：

步驟1、分別從三個(gè)不同時(shí)刻的測量集中取出一個(gè)測量，用取出的三個(gè)測量形成一測量組，測試所述測量組中的三個(gè)測量是否同時(shí)滿足三個(gè)預(yù)設(shè)條件，并用同時(shí)滿足三個(gè)預(yù)設(shè)條件的測量組中的三個(gè)測量形成試探性軌跡；

步驟2、利用已形成試探性軌跡的所述測量組中的三個(gè)測量得到目標(biāo)的轉(zhuǎn)彎率估計(jì)；

步驟3、利用所述已形成試探性軌跡的所述測量組中的三個(gè)測量和所述目標(biāo)的轉(zhuǎn)彎率估計(jì)，得到目標(biāo)在三個(gè)不同時(shí)刻的位置和速度估計(jì)；

步驟4、利用所述目標(biāo)的轉(zhuǎn)彎率估計(jì)和目標(biāo)在三個(gè)不同時(shí)刻的位置和速度估計(jì)，得到目標(biāo)在所述三個(gè)不同時(shí)刻的狀態(tài)估計(jì)；

其中，所述步驟1包括：

設(shè)用三個(gè)不同時(shí)刻的測量集Y_k、Y_k+1和Y_k+2形成試探性軌跡；k時(shí)刻、k+1時(shí)刻和k+2時(shí)刻的測量集分別表示為和其中，N_k、N_k+1和N_k+2分別表示測量集Y_k、Y_k+1和Y_k+2中測量的數(shù)目，t_k、t_k+1和t_k+2分別表示三個(gè)時(shí)刻的時(shí)間，表示k時(shí)刻測量集Y_k中的第e個(gè)測量，其中e＝1,2,…,N_k，和分別表示測量z_k,e的x分量和y分量，表示k+1時(shí)刻測量集Y_k+1中的第f個(gè)測量，其中f＝1,2,…,N_k+1，和分別表示測量z_k+1,f的x分量和y分量，表示k+2時(shí)刻測量集Y_k+2中的第g個(gè)測量，其中g(shù)＝1,2,…,N_k+2，和分別表示測量z_k+2,g的x分量和y分量；

分別從三個(gè)不同時(shí)刻的測量集Y_k、Y_k+1和Y_k+2中取出一個(gè)測量z_k,e、z_k+1,f和z_k+2,g形成一測量組，其中e＝1,2,…,N_k，f＝1,2,…,N_k+1，g＝1,2,…,N_k+2；

測試該測量組中的三個(gè)測量z_k,e、z_k+1,f和z_k+2,g是否同時(shí)滿足下列三個(gè)預(yù)設(shè)條件：

條件1：

條件2：

條件3：

其中，v_min、v_max、a_max和ω_max分別為最小速度、最大速度、最大加速度和最大轉(zhuǎn)彎率，||·||₂表示向量的2范數(shù)，和分別為x方向和y方向測量噪聲的方差，b和c為兩個(gè)控制參數(shù)，b的取值范圍為b＝1～3，而

若該測量組中的三個(gè)測量z_k,e、z_k+1,f和z_k+2,g同時(shí)滿足條件1、條件2和條件3，表明該測量組中的三個(gè)測量可形成一試探性軌跡，并轉(zhuǎn)入步驟2；

若該測量組中的三個(gè)測量z_k,e、z_k+1,f和z_k+2,g不能同時(shí)滿足條件1、條件2和條件3，表明該測量組中的三個(gè)測量不能形成一試探性軌跡；

在一個(gè)測量組的測試完成后，分別從測量集合Y_k、Y_k+1和Y_k+2中再取出一個(gè)測量形成下一測量組，重復(fù)執(zhí)行所述測試所述測量組中的三個(gè)測量是否同時(shí)滿足三個(gè)預(yù)設(shè)條件的操作，直至所有的測量組合都進(jìn)行了測試，測試結(jié)束；

在測試過程中，若某測量組中的三個(gè)測量同時(shí)滿足條件1、條件2和條件3，該測量組中的三個(gè)測量中的任一個(gè)測量不能再與其它兩個(gè)測量集合中的任何測量構(gòu)成新的測量組；

所述步驟2包括：

利用已形成試探性軌跡的所述測量組中的三個(gè)測量z_k,e、z_k+1,f和z_k+2,g得到目標(biāo)的轉(zhuǎn)彎率估計(jì)為

其中，sgn(a)為取變量a的符號(hào)，且有