[發明專利]用于軌跡跟蹤的掃地機前視位姿控制方法和系統

在審

• 申請號: 201911173670.2
• 申請日: 2019-11-26
• 公開（公告）號: CN110716561A
• 公開（公告）日: 2020-01-21
• 發明（設計）人: 高乙超
• 申請（專利權）人: 輕客智能科技(江蘇)有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 32376 無錫睿升知識產權代理事務所(普通合伙)
• 代理人: 張悅

1.一種用于軌跡跟蹤的掃地機前視位姿控制方法，其特征在于，包括：

從目標軌跡中選擇前視目標點，計算所述前視目標點與掃地機位姿的向量差，得到位姿誤差；

將所述位姿誤差傳遞給PID控制器，生成控制量；所述控制量包括X軸控制量、Y軸控制量和方向角控制量；

將所述X軸控制量、所述Y軸控制量合成線速度指令；根據所述方向角控制量生成角速度指令。

2.根據權利要求1所述的用于軌跡跟蹤的掃地機前視位姿控制方法，其特征在于，從目標軌跡中選擇前視目標點的方法是：

記錄上一個控制周期所選擇的前視目標點；

在當前控制周期中，從上一個控制周期所選擇的前視目標點開始依次計算所述目標軌跡中的各個路徑點到掃地機當前位置的歐式距離；

第一個到掃地機當前位置的歐式距離大于前視距離的點作為當前控制周期的前視目標點。

3.根據權利要求2所述的用于軌跡跟蹤的掃地機前視位姿控制方法，其特征在于，當目標軌跡更新后，以新的目標軌跡的第一個路徑點作為上一個控制周期的前視目標點。

4.根據權利要求1所述的用于軌跡跟蹤的掃地機前視位姿控制方法，其特征在于，位姿誤差包括X軸坐標誤差、Y軸坐標誤差和方向角度誤差；X軸坐標誤差、Y軸坐標誤差和方向角度誤差分別傳遞給三個獨立的PID控制器，三個獨立的PID控制器分別生成X軸控制量、Y軸控制量和方向角控制量。

5.根據權利要求1所述的用于軌跡跟蹤的掃地機前視位姿控制方法，其特征在于，對所述線速度進行限幅，實際線速度控制量v_cmd為：

其中，β為線速度衰減因子，v_max為最大線速度，v為所合成的線速度，d_θ是位姿誤差中的角度誤差，是根據角度誤差d_θ限制之后的掃地機線速度。

6.根據權利要求1所述的用于軌跡跟蹤的掃地機前視位姿控制方法，其特征在于，對所述角速度進行限幅，實際角速度控制量ω_cmd為：

ω_cmd＝min(ω_max,ω)

其中，ω_max為最大角速度，ω為所生成的角速度。

7.一種用于軌跡跟蹤的掃地機前視位姿控制系統，其特征在于，包括

前視目標點選擇器，用于從目標軌跡中選擇前視目標點，計算所述前視目標點與掃地機位姿的向量差，得到位姿誤差；

PID控制器，接收所述位姿誤差，并生成控制量；所述控制量包括X軸控制量、Y軸控制量和方向角控制量；

指令生成器，將所述X軸控制量、所述Y軸控制量合成線速度指令；根據所述方向角控制量生成角速度指令。

8.根據權利要求7所述的用于軌跡跟蹤的掃地機前視位姿控制系統，其特征在于，所述PID控制器為三個獨立的PID控制器；位姿誤差包括X軸坐標誤差、Y軸坐標誤差和方向角度誤差；X軸坐標誤差、Y軸坐標誤差和方向角度誤差分別傳遞給三個獨立的PID控制器，三個獨立的PID控制器分別生成X軸控制量、Y軸控制量和方向角控制量。

9.根據權利要求7所述的用于軌跡跟蹤的掃地機前視位姿控制系統，其特征在于，還包括線速度限幅模塊，對指令生成器所合成的線速度進行限幅，生成實際線速度；實際線速度控制量v_cmd為：

其中，β為線速度衰減因子，v_max為最大線速度，v為所合成的線速度，d_θ是位姿誤差中的角度誤差，是根據角度誤差d_θ限制之后的掃地機線速度。

10.根據權利要求7所述的用于軌跡跟蹤的掃地機前視位姿控制系統，其特征在于，還包括角速度限幅模塊，對指令生成器所生成的角速度進行限幅，實際角速度控制量ω_cmd為：

ω_cmd＝min(ω_max,ω)

其中，ω_max為最大角速度，ω為所生成的角速度。