[發明專利]無人機環繞物體外形輪廓飛行的控制方法

有效

• 申請號: 201710190045.3
• 申請日: 2017-03-28
• 公開（公告）號: CN106970641B
• 公開（公告）日: 2019-12-24
• 發明（設計）人: 莫宏偉;屈利偉;陳孝森;竇笑
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種無人機環繞物體外形輪廓飛行的控制方法，包括如下步驟：

步驟A，無人機開機自檢，檢查系統中各傳感器工作狀態，當有故障時通過提示裝置發出警報；

步驟B，遙控無人機起飛，使環繞目標進入目標可檢測范圍，開啟目標環繞模式，通過提示裝置發出環繞模式開啟成功或開啟失敗的提示信息；

步驟C，利用激光雷達探測信息解算無人機機頭與目標中心方向的夾角即轉差角及無人機與目標的距離即目標距離；

步驟D，將轉差角輸入轉向PID控制器，轉向PID控制器輸出量用于控制無人機鎖定目標；

步驟E，將目標距離輸入定距PID控制器，定距PID控制器輸出量用于控制無人機與目標維持在設定距離，環繞模式下通過遙控器俯仰通道改變定距距離；

步驟F，控制無人機環繞目標飛行；

其特征是步驟C具體包括：將激光雷達探測信息變換到水平面內，在目標可檢測范圍搜索連續體，合并可以合并的連續體，尋找探測點數最多連續體，確定連續體邊界，計算轉差角和目標距離；

步驟C1，利用陀螺儀和加速度計信息計算無人機的姿態，包括橫滾角、俯仰角和航向角，激光雷達與無人機具有相同的姿態角；

載體坐標系到水平坐標系的坐標變換矩陣為：

其中α為橫滾角，β為俯仰角，利用坐標變換矩陣將激光雷達探測信息變換到水平面內；

步驟C2，所述連續體是指根據判定規則確定的激光雷達探測點的集合；連續體均由一個激光雷達探測點逐步擴充得到，連續體擴充判定規則為：

其中r和γ分別為激光雷達探測點的探測距離和方位角，下標表示激光雷達探測點編號，min表示小括號中取二者中較小值，dDis為相鄰激光雷達探測點的探測距離差，arcDis為相鄰探測點以其中最小探測距離為半徑所夾圓弧長度，FACD為設定的判定因數，使用連續體擴充判定規則確定連續體；

步驟C3，使用連續體擴充判定規則對間隔探測點數小于a的連續體邊界探測點進行判定，合并可以合并的連續體，a為正整數；

步驟C4，根據連續體距離在[minDis,maxDis]范圍內尋找點數最多的連續體即最大連續體，其中minDis和maxDis為設定的搜索最大連續體的最小和最大的環繞目標與無人機的距離；

對于包含b個探測點的連續體其連續體距離oDis計算公式為：

oDis＝(r₁+r₂+···+r_b)/b

步驟C5，當最大連續體左側邊界小于等于右側邊界時，從右側邊界向內搜尋探測點作為新邊界，將新邊界與左側邊界比較，若新邊界距離小于左側邊界距離或新邊界與左側邊界夾角小于χ度，停止繼續向內搜尋，將新邊界作為最大連續體的新右側邊界，χ為可選設定夾角值；

當最大連續體右側邊界小于左側邊界時，從左側邊界向內搜尋探測點作為新邊界，將新邊界與右側邊界比較，若新邊界距離小于右側邊界距離或新邊界與右側邊界夾角小于χ度，停止繼續向內搜尋，將新邊界作為最大連續體的新左側邊界；

將左右兩側邊界所確定的連續體作為環繞目標；

步驟C6，目標中心方向為：

γ_o＝(γ_l+γ_r)/2

其中γ_o為目標中心方向，γ_l為左側邊界方位角，γ_r為右側邊界方位角；

目標方向與機頭方向的角度差簡稱轉差角為

其中為轉差角，γ_u為機頭在激光雷達探測區域對應方位角；

無人機與環繞目標距離簡稱目標距離為

r_o＝(r₁+···+r_m+r_m+1+···+r_2m)/(2*m)

其中r_o為目標距離，r₁,r₂···r_m為目標中心方向左側與目標中心方向鄰近的連續m個激光雷達探測點，r_m+1,r_m+2···r_2m為目標中心方向右側與目標中心方向鄰近的連續m個激光雷達探測點，m為可選正整數值。