[發(fā)明專利]一種針對(duì)多無(wú)人機(jī)停泊的定位方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202110373363.X
• 申請(qǐng)日: 2021-04-07
• 公開(kāi)（公告）號(hào): CN112954600B
• 公開(kāi)（公告）日: 2022-10-25
• 發(fā)明（設(shè)計(jì)）人: 鄧曉衡;胡意祥
• 申請(qǐng)（專利權(quán)）人: 中南大學(xué)
• 主分類號(hào): H04W4/02
• 分類號(hào): H04W4/02;H04W4/021;H04W4/024;H04W4/44;H04W64/00;G06K17/00
• 代理公司: 長(zhǎng)沙永星專利商標(biāo)事務(wù)所(普通合伙) 43001
• 代理人: 周詠;米中業(yè)

1.一種針對(duì)多無(wú)人機(jī)停泊的定位方法，包括如下步驟：

S1.無(wú)人機(jī)采用定位技術(shù)導(dǎo)航至停泊平臺(tái)的設(shè)定范圍內(nèi)；

S2.無(wú)人機(jī)在停泊平臺(tái)的設(shè)定范圍內(nèi)后，無(wú)人機(jī)采用UWB定位技術(shù)完成粗定位；具體為采用如下步驟進(jìn)行粗定位：

a.針對(duì)停泊平臺(tái)的n個(gè)UWB定位基站，測(cè)量UWB定位基站兩兩之間的傳輸編碼信號(hào)的時(shí)間，并記為T_xy，x和y為兩個(gè)基站的下標(biāo)序號(hào)；然后建立數(shù)據(jù)表記錄每一個(gè)定位基站從自己傳輸編碼信號(hào)到其他所有定位基站的傳輸時(shí)間；同時(shí)每個(gè)定位基站建立編碼信號(hào)發(fā)送隊(duì)列，該隊(duì)列包括發(fā)送時(shí)間和發(fā)送的目標(biāo)標(biāo)簽；

b.無(wú)人機(jī)搭載的UWB定位標(biāo)簽t發(fā)送定位編碼信號(hào)給停泊平臺(tái)的所有UWB定位基站；

c.最先收到UWB定位標(biāo)簽t發(fā)送的定位編碼信號(hào)的基站記為S₀，并立刻返回編碼信號(hào)給定位標(biāo)簽t，定位標(biāo)簽t接收到編碼信號(hào)后通過(guò)無(wú)人機(jī)本地時(shí)鐘記錄接收時(shí)刻T₀；在接收到UWB定位標(biāo)簽t發(fā)送的定位編碼信號(hào)后，基站S₀同時(shí)把編碼信號(hào)發(fā)送給其他UWB定位基站，剩余的UWB定位基站按照原始下標(biāo)大小，從1開(kāi)始進(jìn)行編號(hào)，從而將剩余的UWB定位基站編號(hào)為{S₁,S₂,...,S_n}；

d.剩余的UWB定位基站，根據(jù)各自的編號(hào)，在基站S₀發(fā)出信號(hào)后的n*Δt時(shí)刻，依次發(fā)送編碼信號(hào)給無(wú)人機(jī)搭載的UWB定位標(biāo)簽t；剩余基站接收到基站S₀發(fā)送的編碼信號(hào)后，采用算式計(jì)算第n個(gè)UWB定位基站的發(fā)送編碼信號(hào)等待時(shí)間并按照從小到大插入發(fā)送隊(duì)列，并采用算式計(jì)算第n個(gè)UWB定位基站發(fā)送編碼信號(hào)給無(wú)人機(jī)UWB定位標(biāo)簽t的時(shí)刻；Δt為基站單位等待時(shí)間間隔；為第n個(gè)UWB定位基站接收到基站S₀發(fā)送的編碼信息的時(shí)刻；

e.無(wú)人機(jī)分別記錄接收到每一個(gè)UWB定位基站的時(shí)間T_n；n為接收編碼信息的先后順序；T_n＝T+ΔT_n+n*Δt，T為UWB定位基站S接收到無(wú)人機(jī)UWB定位標(biāo)簽t的編碼信號(hào)時(shí)，無(wú)人機(jī)的本地時(shí)鐘時(shí)刻；ΔT_n為各個(gè)UWB定位基站發(fā)送編碼信號(hào)給無(wú)人機(jī)定位標(biāo)簽t的時(shí)間；最后采用如下算式計(jì)算得到UWB定位標(biāo)簽t接收到任意兩個(gè)UWB定位基站發(fā)送編碼信號(hào)的時(shí)間差ΔT(n,m)為ΔT(n,m)＝|T_n-T_m-(m-n)Δt|，n和m表示UWB定位基站的下標(biāo)；

f.無(wú)人機(jī)將計(jì)算出來(lái)的所有ΔT(n,m)代入TDOA算法，從而計(jì)算得到無(wú)人機(jī)UWB定位標(biāo)簽t的位置，從而得到無(wú)人機(jī)當(dāng)前的三維位置坐標(biāo)；

g.根據(jù)步驟f得到的無(wú)人機(jī)當(dāng)前的三維位置坐標(biāo)，以及無(wú)人機(jī)的目標(biāo)位置，采用PID算法控制無(wú)人機(jī)飛行到目標(biāo)位置；采用PID算法進(jìn)行控制時(shí)，重復(fù)步驟b～步驟f進(jìn)行無(wú)人機(jī)當(dāng)前的三維位置坐標(biāo)的更新；

S3.無(wú)人機(jī)定位至停泊平臺(tái)上方；

S4.無(wú)人機(jī)通過(guò)精確定位停泊至停泊平臺(tái)；具體為采用如下步驟進(jìn)行精定位：

A.將攝像頭設(shè)置在無(wú)人機(jī)底部最中心，攝像頭的位置為(X_camera,Y_camera)；停泊平臺(tái)上設(shè)置二維碼，二維碼的中心位置設(shè)置為原點(diǎn)，原點(diǎn)坐標(biāo)為(0,0)；無(wú)人機(jī)的位置為(X_u,Y_u,Z_u)；

B.無(wú)人機(jī)對(duì)停泊平臺(tái)的二維碼進(jìn)行拍照，識(shí)別圖像中二維碼的四個(gè)端點(diǎn)坐標(biāo)為P1(x₁,y₁)、P2(x₂,y₂)、P3(x₃,y₃)和P4(x₄,y₄)；從而得到二維碼中心坐標(biāo)為

C.無(wú)人機(jī)調(diào)整自身的位置，直至位于圖像中的(X_a,Y_a)點(diǎn)；(X_a,Y_a)為坐標(biāo)原點(diǎn)(0,0)在圖像中的映射點(diǎn)；

D.采用如下算式計(jì)算無(wú)人機(jī)在當(dāng)前位置距離目標(biāo)位置在X方向的像素點(diǎn)偏移量：

式中x為X方向的控制量；X_u為當(dāng)前無(wú)人機(jī)在X方向的位置；e為移動(dòng)控制量；Δx為中間變量，且E為設(shè)定的閾值；

E.重復(fù)調(diào)整無(wú)人機(jī)的自身位置，直至無(wú)人機(jī)在X方向的中間變量|Δx|小于設(shè)定的閾值E；

F.采用如下算式計(jì)算無(wú)人機(jī)在當(dāng)前位置距離目標(biāo)位置在Y方向的像素點(diǎn)偏移量：

式中y為Y方向的控制量；Y_u為當(dāng)前無(wú)人機(jī)在Y方向的位置；e為移動(dòng)控制量；Δy為中間變量，且E1為設(shè)定的閾值；

G.重復(fù)調(diào)整無(wú)人機(jī)的自身位置，直至無(wú)人機(jī)在Y方向的中間變量|Δy|小于設(shè)定的閾值E1；

H.無(wú)人機(jī)控制自身降落至停泊平臺(tái)，并在降落過(guò)程中重復(fù)步驟A～步驟G調(diào)整無(wú)人機(jī)自身的位置。