[發明專利]一種旋翼無人機避障控制系統及其控制方法有效

申請號：	202010697379.1	申請日：	2020-07-20
公開（公告）號：	CN112000123B	公開（公告）日：	2023-03-14
發明（設計）人：	劉云平;徐澤;唐夢奇;蔣長勝	申請（專利權）人：	南京信息工程大學
主分類號：	G05D1/10	分類號：	G05D1/10
代理公司：	南京經緯專利商標代理有限公司 32200	代理人：	朱小兵
地址：	210044 江蘇***	國省代碼：	江蘇;32
權利要求書：	查看更多	說明書：	查看更多
摘要：
搜索關鍵詞：	一種無人機控制系統及其控制方法
鉆瓜網技術展會專利詞庫專利權人專利榜在售專利公布日期熱門專利

【權利要求書】：

1.一種基于ToF和視覺傳感器的旋翼無人機避障控制系統的控制方法，其特征在于：所述旋翼無人機避障控制系統包含5個避障子模塊和1個控制模塊，其中，所述避障子模塊包含第一ToF深度傳感器(2)、第一視覺傳感器(1)、第二視覺傳感器(3)、數據接口(5)、第一固定接口(6)和第二固定接口(7)和避障子模塊外殼(4)組成；

其中，第一視覺傳感器(1)和第二視覺傳感器(3)均用于采集圖像信息，對障礙物進行標記并計算障礙物相對避障子模塊的XY位置信息；

第一ToF深度傳感器(2)，用于對障礙物進行檢測，測得障礙物相對避障子模塊的深度距離信息；

數據接口(5)，用于將傳感器采集的數據發送至控制模塊；

5個避障子模塊包含安裝在無人機機頭反向位置的避障子模塊N(22)、無人機機頭左側的避障子模塊E(23)、無人機機頭位置的避障子模塊S(24)、無人機機頭右側位置的避障子模塊W(25)和無人機頂部位置的避障子模塊A(26)；

所述控制模塊包含第二ToF深度傳感器(8)、第三視覺傳感器(9)和第四視覺傳感器(10)、光流模塊(11)、姿態傳感器(12)、氣壓傳感器(13)、控制器(14)、差分定位模塊(15)、數據接口N(17)、數據接口E(18)、數據接口S(19)、數據接口W(20)、數據接口A(21)、控制模塊外殼(16)；

其中，氣壓傳感器(13)和第二ToF深度傳感器(8)融合進行高精度定高，其中第二ToF深度傳感器(8)測得的深度信息主要用在低空中，而氣壓傳感器(13)測得的氣壓數據主要用于高空中；

光流模塊(11)和差分定位模塊(15)結合進行定位，其中，光流模塊(11)用在差分定位模塊(15)定位精度較差或者無定位信息時輔助定位；所述控制方法具體包含如下步驟；

步驟1，初始化無人機；

步驟2，獲取本機的位置和速度以及利用傳感器感知周圍障礙物的位置；

步驟3，利用障礙物的位置信息估算障礙物的速度矢量；

步驟4，利用本機的位置和障礙物的位置估計位置差；

步驟5，判斷無人機是否處于障礙物的勢場內，如果處于勢場內，則執行步驟6，否則執行步驟2；

步驟6，計算勢場力和輔助牽引力；

步驟7，利用勢場力和輔助牽引力計算無人機期望的避障姿態信息；

在步驟2中：

定義v(t)以及p_u(t)＝[x_u,y_u,z_u]^T表示旋翼無人機在t時刻的速度以及位置，定義表示障礙物i在t時刻的位置，其中，旋翼無人機的位置信息中的平面位置[x_u,y_u]^T由光流模塊(11)和差分定位模塊(15)采集得，而高度位置[z_u]^T由氣壓傳感器(13)和第二ToF深度傳感器(8)采集得到；

在步驟3中：

定義障礙物i在t時刻的速度為表示上一時刻，t_i表示當前時刻，表示系統第一次檢測到障礙物的時刻，且當系統第一次檢測到障礙物i時，計且則障礙物速度的值和角度分別估算為：

在步驟4中：

定義無人機與障礙物i的位置差為d_xy(i,t)和d_z(i,t),位置差估算為：

其中，sgn為符號函數；

在步驟5中：

定義和為障礙物勢場范圍，和為障礙物基本勢場范圍，和為勢場調節參數；定義θ₁為障礙物i速度方向與旋翼無人機和障礙物i連線之間的角度，θ₂表示障礙物i和旋翼無人機速度方向之間的角度，表示輔助牽引勢場角度；

障礙物勢場范圍函數和為：

其中，為x-y平面上的勢場范圍函數，為y-z平面上的勢場范圍函數；該勢場范圍函數與障礙物速度有關，隨著障礙物速度的變化而變化，且其與障礙物的速度正相關；

障礙物勢場U_xy(i)和U_z(i)為：

其中，a₁、a₂、b₁和b₂為勢場調節參數，U_xy(i)為旋翼無人機與障礙物在x-y平面上的勢場，而U_z(i)為旋翼無人機與障礙物在y-z平面上的勢場；

為了快速離開障礙物，設計輔助牽引勢場U_a(i)，輔助牽引勢場為：