[發明專利]一種無人機三維空間運動控制方法及系統

有效

• 申請號: 202111580932.4
• 申請日: 2021-12-22
• 公開（公告）號: CN113961020B
• 公開（公告）日: 2022-04-08
• 發明（設計）人: 黃立;張欽彥;林家民;張原藝;王龍;薛源;劉華斌
• 申請（專利權）人: 普宙科技（深圳）有限公司
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 北京集智東方知識產權代理有限公司 11578
• 代理人: 吳倩;龔建蓉

1.一種無人機三維空間運動控制方法，其特征在于，所述的控制方法包括以下步驟：

在無人機運動過程中，將無人機的實時位置C點到運動目標點A點之間的距離d輸入PID位置環控制器，并基于第一算法計算得到所述無人機在三維空間運動時的三維目標合速度，其中，所述第一算法所采用公式如下：

；

其中，k_p、k_i、k_d分別為PID控制算法的比例、積分、微分系數；

基于第二算法將所述三維目標合速度分解為水平目標速度和豎直目標速度；

根據所述水平目標速度和豎直目標速度分別對應在水平方向、豎直方向做串級PID控制，以得到無人機驅動電機的控制量，并根據所述無人機驅動電機的控制量控制無人機向運動目標點A運動；

在所述無人機運動前，所述控制方法還包括以下步驟：

獲取無人機運動的起始位點O點以及運動目標點A點；

以O點作為坐標原點建立三維坐標系OXYZ，其中，XY為水平面，OZ為豎直方向；O點到A點的距離記為預定運動距離m，m≠0；

在控制無人機開始從O點向A點運動時，所述運動過程中，獲取無人機的實時位置C點，且C點到O點之間的距離記為實時距離n，且nm，所述無人機的實時位置C點到A點之間的實時距離d=m-n；

所述獲取無人機的實時位置C點的步驟具體包括：通過安裝在所述無人機上的GPS模塊傳感器測量得到所述無人機的實時位置C點；

所述第二算法所采用的公式如下：

；

其中，m₁為運動距離m在水平方向的投影長度，m₂為運動距離m在豎直方向的投影長度。

2.一種無人機三維空間運動控制系統，其特征在于，所述的控制系統包括：

數據輸入單元，在無人機運動過程中，將無人機的實時位置C點到運動目標點A點之間的距離d輸入PID位置環控制器，

第一計算單元，用于基于第一算法計算得到所述無人機在三維空間運動時的三維目標合速度，

第二計算單元，用于基于第二算法將所述三維目標合速度分解為水平目標速度和豎直目標速度，

飛行控制單元，用于根據所述水平目標速度和豎直目標速度分別對應在水平方向、豎直方向做串級PID控制，以得到無人機驅動電機的控制量；

飛行執行單元，用于根據所述無人機驅動電機的控制量控制無人機向運動目標點A運動；

所述的控制系統還包括：運動距離確定單元，用于獲取無人機運動的起始位點O點以及運動目標點A點；以O點作為坐標原點建立三維坐標系OXYZ，其中，XY為水平面，OZ為豎直方向；O點到A點的距離記為預定運動距離m，m≠0；

所述的控制系統還包括：

實時距離確定單元，用于在控制無人機開始從O點向A點運動時，獲取無人機的實時位置C點，且C點到O點之間的距離記為實時距離n，且nm，所述無人機的實時位置C點到A點之間的實時距離d=m-n。

3.根據權利要求2所述的無人機三維空間運動控制系統，其特征在于，所實時距離確定單元包括：

GPS模塊傳感器，用于測量獲取所述無人機的實時位置C點。