[發明專利]光電吊艙的控制方法和裝置

在審

• 申請號: 202210351473.0
• 申請日: 2022-04-02
• 公開（公告）號: CN114660927A
• 公開（公告）日: 2022-06-24
• 發明（設計）人: 劉志;何博俠
• 申請（專利權）人: 南京博藍奇智能科技有限公司
• 主分類號: G05B11/42
• 分類號: G05B11/42
• 代理公司: 北京超凡宏宇專利代理事務所(特殊普通合伙) 11463
• 代理人: 安衛靜

1.一種光電吊艙的控制方法，其特征在于，包括：

獲取移動終端的方位數據、移動終端坐標系與光電吊艙坐標系之間的轉換矩陣；

分析用戶手持所述移動終端的姿態特征，以確定所述移動終端與所述光電吊艙的姿態對應關系，進而根據所述姿態對應關系建立姿態映射函數模型；

根據所述方位數據和所述轉換矩陣，計算所述移動終端的初始姿態角數據；

將所述初始姿態角數據輸入至所述姿態映射函數模型，計算得到所述光電吊艙的目標姿態數據，并根據所述目標姿態數據控制所述光電吊艙的姿態。

2.根據權利要求1所述的控制方法，其特征在于，獲取移動終端的方位數據，包括：

通過所述移動終端上設置的磁力傳感器獲取所述移動終端的地磁強度；

通過所述移動終端上設置的加速度傳感器獲取所述移動終端的加速度；

將所述地磁強度和所述加速度作為所述方位數據。

3.根據權利要求1所述的控制方法，其特征在于，獲取移動終端坐標系與光電吊艙坐標系之間的轉換矩陣，包括：

以所述移動終端的中心為原點，建立所述移動終端坐標系，其中，所述移動終端坐標系中，X軸為與所述移動終端的橫屏平行且水平向右的方向，Y軸為與所述移動終端的豎屏平行且豎直向上的方向，Z軸為與所述移動終端的屏幕垂直且指向外的方向；

以所述光電吊艙開始運動的起點為原點，建立所述光電吊艙坐標系，其中，所述光電吊艙坐標系為地理坐標系；

通過所述移動終端坐標系圍繞所述光電吊艙坐標系的每個坐標軸依次旋轉，得到所述移動終端坐標系轉換到所述光電吊艙坐標系的轉換矩陣。

4.根據權利要求1所述的控制方法，其特征在于，所述姿態映射函數模型，包括：方位角映射函數模型和俯仰角映射函數模型；

所述方位角映射函數模型包括：方位角角度映射函數和方位角速度映射函數，其中，所述方位角角度映射函數包括：表示方位角，Φ₀表示所述方位角的起始角度；所述方位角速度映射函數包括：

所述俯仰角映射函數模型包括：俯仰角角度映射函數和俯仰角速度映射函數，其中，所述俯仰角角度映射函數包括：θ表示俯仰角，Θ₀表示所述俯仰角的起始角度，k₁、k₂表示所述俯仰角的控制比例系數；所述俯仰角速度映射函數包括：

5.根據權利要求2所述的控制方法，其特征在于，根據所述方位數據和所述轉換矩陣，計算所述移動終端的初始姿態角數據，包括：

根據所述加速度和所述轉換矩陣，計算所述移動終端的俯仰角和橫滾角；

根據所述地磁強度、所述轉換矩陣、所述俯仰角和所述橫滾角，計算所述移動終端的方位角；

將所述俯仰角和所述方位角作為所述移動終端的初始姿態角數據。

6.根據權利要求5所述的控制方法，其特征在于，在根據所述方位數據和所述轉換矩陣，計算所述移動終端的初始姿態角數據之前，還包括：

采用滑動窗口均值濾波算法分別對所述地磁強度和所述加速度進行均值濾波處理，得到處理后的地磁強度和處理后的加速度；

根據所述加速度和所述轉換矩陣，計算所述移動終端的俯仰角和橫滾角包括：根據所述處理后的加速度和所述轉換矩陣，計算所述移動終端的俯仰角和橫滾角；

根據所述地磁強度、所述轉換矩陣、所述俯仰角和所述橫滾角，計算所述移動終端的方位角包括：根據所述處理后的地磁強度、所述轉換矩陣、所述俯仰角和所述橫滾角，計算所述移動終端的方位角。

7.根據權利要求6所述的控制方法，其特征在于，所述滑動窗口均值濾波算法的滑動窗口長度為8。