[發明專利]一種無人機自主安全控制方法

有效

• 申請號: 202011060931.2
• 申請日: 2020-09-30
• 公開（公告）號: CN112241178B
• 公開（公告）日: 2022-01-25
• 發明（設計）人: 李照宏;王毅;唐瑞卿;劉宇佳;陳根浦;紀海成;劉海亮;張頌
• 申請（專利權）人: 成都飛機工業（集團）有限責任公司
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 成都君合集專利代理事務所(普通合伙) 51228
• 代理人: 尹玉

1.一種無人機自主安全控制方法，其特征在于，當無人機規劃路徑經過保護區域時，判斷相交路徑為直線路徑或圓弧路徑，若直線路徑段與保護域相交，采用中間航路點法生成同向旋轉解與反向旋轉解，以無人機位置、中間航路點、待飛航點進行路徑重規劃；若圓弧路徑段與保護域相交，若圓弧路徑與保護域相交，采用調整運動圓弧曲率的方法計算與保護域相切的點規避保護域；若重規劃后的路徑與圓弧段或者直線段相交，通過迭代產生避開保護域的路徑；

采用射線掃描法判斷無人機位置與邊界地圖之間的關系，假設邊界地圖由不規則n邊形構成，規定逆時針為正，頂點按照逆時針方向排列，依次為P_i(x_i,y_i),i＝0,1,2,…,n-1，過P(x,y)向右作一條水平射線，計算與多邊形邊界的交點，若交點個數為奇數，則點在多邊形內部，若交點個數為偶數，則點在多邊形外；

射線法計算如下：

首先快速剔除矩形外的點，記A＝{0,1,2,…,n-1},n≥3,

若P(x,y)不滿足X_min＜x＜X_max,Y_min＜y＜Y_max,則無人機越界；若不滿足，對于矩陣內的點，通過點P(x,y)與各邊距離判斷P(x,y)是否為控制頂點集P_i(x_i,y_i),i＝0,1,2,…,n-1的邊點與頂點；對P(x,y)與各邊距離公式如下：

若d滿足d＜ε，則P(x,y)為邊點，無人機越界；

若對i＝0,1,2,…,n-1，滿足則P(x,y)為頂點，無人機越界；

若P(x,y)非頂點或邊點，則通過P(x,y)向右作一條水平射線，計算與多邊形邊界的交點，若交點個數為奇數，則點在多邊形內部，若交點個數為偶數，則點在多邊形外；

對ε任意小，j＝1,2,…,n-1,n，若滿足|y_j-1-y_j|≤ε，則交點個數為0，

若滿足|x_j-1-x_j|≤ε，則有一個交點，且交點坐標為x_p＝x_j-1,y_p＝y，

若滿足|y_j-1-y_j|＞ε,|x_j-1-x_j|＞ε，則有一個交點，且交點坐標為：

若P(x,y)滿足x＜x_p,min(y_j-1,y_j)≤y≤max(y_j-1,y_j)，且交點不為多邊形頂點，則交點個數加1，若滿足上式且P(x,y)交點為多邊形頂點，則同側頂點交點個數加2，異側頂點交點個數加1。

2.根據權利要求1所述的一種無人機自主安全控制方法，其特征在于，當無人機位置位于保護域的高度區間內時，若所規避對象的類型屬性為柱形，則以保護域最大半徑為保護圓半徑，否則，以保護域在無人機高度平面的水平投影圓半徑疊加安全距離為保護圓半徑；以保護圓半徑為路徑重規劃所用的安全圓半徑。

3.根據權利要求1或2所述的一種無人機自主安全控制方法，其特征在于，直線路徑與保護域相交判據：保護域中心點距離直線路徑段的距離小于保護域半徑，且交點至少有一個點在直線路徑段上。

4.根據權利要求3所述的一種無人機自主安全控制方法，其特征在于，采用中間航路點算法生成中間航路點，若中間航路點所在圓弧半徑小于最小轉彎半徑，則以最小轉彎半徑為重規劃路徑圓弧段半徑，否則，以中間航路點所在圓弧半徑作為重規劃路徑圓弧段半徑。

5.根據權利要求1或2所述的一種無人機自主安全控制方法，其特征在于，圓弧路徑與保護域相交判據：保護圓心距圓弧路徑段圓心的距離＜保護圓半徑+圓弧路徑半徑，保護圓心距圓弧路徑段圓心的距離＞|保護圓半徑-圓弧路徑半徑|，且至少一個交點在圓弧路徑段內。