[發(fā)明專利]一種高精度多旋翼機(jī)協(xié)同定位方法及系統(tǒng)

有效

• 申請(qǐng)?zhí)枺?/td>: 202010725789.2
• 申請(qǐng)日: 2020-07-24
• 公開（公告）號(hào): CN112197761B
• 公開（公告）日: 2022-07-19
• 發(fā)明（設(shè)計(jì)）人: 林德福;楊希雯;宋韜;侯淼;王江;鄭多;張福彪
• 申請(qǐng)（專利權(quán)）人: 北京理工大學(xué)
• 主分類號(hào): G01C21/00
• 分類號(hào): G01C21/00;G01C21/16;G01S19/45;G01S19/47
• 代理公司: 北京康思博達(dá)知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 11426
• 代理人: 劉冬梅;范國(guó)鋒

1.一種高精度多旋翼機(jī)協(xié)同定位方法，其特征在于，所述方法包括：

步驟1、選定需要進(jìn)行定位的目標(biāo)；

步驟2、n個(gè)無(wú)人機(jī)進(jìn)行信息數(shù)據(jù)的采集，所述信息數(shù)據(jù)包括無(wú)人機(jī)自身參數(shù)信息、目標(biāo)視線角信息以及無(wú)人機(jī)與目標(biāo)的距離信息；

步驟3、對(duì)所述信息數(shù)據(jù)進(jìn)行處理，獲得目標(biāo)位置的估計(jì)值；

所述n個(gè)無(wú)人機(jī)將采集的信息數(shù)據(jù)發(fā)送至地面站后，地面站對(duì)所述信息數(shù)據(jù)進(jìn)行處理，獲得目標(biāo)坐標(biāo)估計(jì)值，根據(jù)最小二乘模型對(duì)所述信息數(shù)據(jù)進(jìn)行處理，

對(duì)所述信息數(shù)據(jù)進(jìn)行處理包括：

步驟3.1、確定目標(biāo)坐標(biāo)與無(wú)人機(jī)自身坐標(biāo)之間的坐標(biāo)轉(zhuǎn)換關(guān)系，

步驟3.2、確定目標(biāo)坐標(biāo)的估計(jì)值；

步驟3.1中，設(shè)定目標(biāo)坐標(biāo)為[x_T，y_T，z_T]^T，無(wú)人機(jī)自身坐標(biāo)為[x_U，y_U，z_U]^T，目標(biāo)坐標(biāo)與無(wú)人機(jī)自身坐標(biāo)之間的關(guān)系如下式所示：

其中，為從無(wú)人機(jī)機(jī)體坐標(biāo)系到導(dǎo)航坐標(biāo)系的轉(zhuǎn)換矩陣，為從光電吊艙基座坐標(biāo)系到無(wú)人機(jī)機(jī)體坐標(biāo)系的轉(zhuǎn)換矩陣，為從光電吊艙視線坐標(biāo)系到光電吊艙基座坐標(biāo)系的轉(zhuǎn)換矩陣；L表示無(wú)人機(jī)與目標(biāo)之間的距離；

步驟3.2包括：

步驟3.2.1、確定目標(biāo)坐標(biāo)與無(wú)人機(jī)自身坐標(biāo)的非線性方程；

步驟3.2.2、根據(jù)非線性方程建立最小二乘模型，得到目標(biāo)坐標(biāo)的估計(jì)值；

步驟3.2.1中：

設(shè)定目標(biāo)坐標(biāo)為Θ₁＝[x_T，y_T，z_T]^T，

設(shè)由測(cè)量量構(gòu)成的向量為y，y＝[x_U，y_U，z_U，α，β，φ，θ，Ψ，L]^T，

x_U，y_U，z_U表示無(wú)人機(jī)在大地直角坐標(biāo)系下的三維坐標(biāo)，α表示光電吊艙測(cè)量獲得的目標(biāo)方位角；β表示光電吊艙測(cè)量獲得的目標(biāo)高低角；φ表示無(wú)人機(jī)的滾轉(zhuǎn)角；θ表示無(wú)人機(jī)的俯仰角；Ψ表示無(wú)人機(jī)的偏航角；

設(shè)無(wú)人機(jī)發(fā)送至地面站的真實(shí)測(cè)量量的向量為z，

z＝y(tǒng)+v，v～N(0，R₀)

其中，v為測(cè)量噪聲，服從高斯分布，協(xié)方差矩陣為9×9的矩陣R₀，

將目標(biāo)坐標(biāo)與無(wú)人機(jī)坐標(biāo)之間的關(guān)系表示為下式：

Θ₁＝f(y)＝f(z-v) 式(2)，

其中，偽上述坐標(biāo)轉(zhuǎn)換關(guān)系式表示的非線性函數(shù)，

對(duì)式(2)進(jìn)行泰勒展開，得到：

即：

步驟3.2.2中，設(shè)n個(gè)無(wú)人機(jī)共在N個(gè)點(diǎn)進(jìn)行信息數(shù)據(jù)采集，可得到N組測(cè)量數(shù)據(jù)，得到N個(gè)式(3)，整合成矩陣形式，得到：

其中V為誤差項(xiàng)，其協(xié)方差矩陣為：

由加權(quán)最小二乘法求解上述線性估計(jì)問題，得到目標(biāo)坐標(biāo)估計(jì)量的表達(dá)式如式(5)所示：

其中，z_k為第k個(gè)點(diǎn)真實(shí)數(shù)據(jù)信息構(gòu)成的向量，k＝1、2、......N，A_k為f對(duì)z_k求偏導(dǎo)得到的雅可比矩陣，即

所得目標(biāo)坐標(biāo)估計(jì)值的估計(jì)誤差協(xié)方差矩陣為：

2.根據(jù)權(quán)利要求1所述的高精度多旋翼機(jī)協(xié)同定位方法，其特征在于，步驟1，所述選定需要進(jìn)行定位的目標(biāo)包括：將n個(gè)無(wú)人機(jī)懸停在探測(cè)區(qū)域內(nèi)，在無(wú)人機(jī)視野內(nèi)搜尋目標(biāo)，選定目標(biāo)，并設(shè)置無(wú)人機(jī)在目標(biāo)上方飛行。

3.根據(jù)權(quán)利要求1所述的高精度多旋翼機(jī)協(xié)同定位方法，其特征在于，步驟2中，所述無(wú)人機(jī)自身參數(shù)信息由組合導(dǎo)航模塊獲得，所述無(wú)人機(jī)自身參數(shù)信息包括無(wú)人機(jī)位置信息和無(wú)人機(jī)姿態(tài)信息，

所述無(wú)人機(jī)位置信息為無(wú)人機(jī)坐標(biāo)，所述無(wú)人機(jī)姿態(tài)信息包括無(wú)人機(jī)偏航角、俯仰角和滾轉(zhuǎn)角，

所述目標(biāo)視線角信息由光電吊艙獲得，所述目標(biāo)視線角信息包括目標(biāo)方位角和高低角。