[發明專利]一種移動機器人導航跟蹤方法

在審

• 申請號: 202110938021.8
• 申請日: 2021-08-16
• 公開（公告）號: CN113534816A
• 公開（公告）日: 2021-10-22
• 發明（設計）人: 夏科睿;馬姓;彭超
• 申請（專利權）人: 安徽元古紀智能科技有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 合肥市浩智運專利代理事務所(普通合伙) 34124
• 代理人: 丁瑞瑞

1.一種移動機器人導航跟蹤方法，其特征在于，所述方法包括：

步驟一：將移動機器人當前位置、當前方向以及移動機器人初始控制指令角速度和初始線速度代入移動機器人運動模型，獲取特定時間下移動機器人預估的目標位置A和預估的目標方向；

步驟二：獲取移動機器人預估的目標位置和預估的目標方向相對實際位置以及實際方向的角度偏差以及距離偏差，根據角度偏差獲取第一角速度控制量，根據距離偏差獲取第二角速度控制量；

步驟三：根據移動機器人預估的目標位置和預估的目標方向獲取移動機器人的初始線速度下的第三角速度控制量；

步驟四：利用第一角速度控制量、第二角速度控制量以及第三角速度控制量計算擬合角速度控制量，根據擬合角速度控制量對移動機器人的初始控制指令角速度以及初始線速度進行更新，修正預估的目標位置A和預估的目標方向與實際位置以及實際方向之間的偏差；

步驟五：利用修正偏差后得到的移動機器人運動模型對特定時間下的目標位置以及目標方向進行預估并按照預估的結果進行導航跟蹤控制。

2.根據權利要求1所述的一種移動機器人導航跟蹤方法，其特征在于，所述步驟二包括：根據移動機器人預估的目標位置A和預估的目標方向，在預估的目標方向上搜尋距離預估的目標位置A最近的路徑點B以及路徑點B前向最近路徑點C，獲取移動機器人預估的目標方向和BC朝向的角度偏差Ang_err，獲取預估的目標位置A到BC所在直線的距離偏差Dis_err。

3.根據權利要求2所述的一種移動機器人導航跟蹤方法，其特征在于，所述步驟二中距離偏差Dis_err的計算公式為

Dis_err＝((a_x-b_x)*(a_y-c_y)-(a_x-c_x)*(a_y-b_y))/sqrt((c_x-b_x)²+(c_y-b_y)²)，其中，A點坐標為(a_x,a_y)，B點坐標為(b_x,b_y)，C點坐標為(c_x,c_y)，sqrt為平方根函數；

角度偏差Ang_err的計算公式為Ang_err＝arctan(c_y-b_y,c_x-b_x)-Ang_car，其中，arctan為反正切函數。

4.根據權利要求3所述的一種移動機器人導航跟蹤方法，其特征在于，根據角度偏差通過PID控制方法對角度偏差乘以比例系數獲取第一角速度控制量w₁，根據距離偏差通過PID控制方法對距離偏差乘以比例系數獲取第二角速度控制量w₂。

5.根據權利要求4所述的一種移動機器人導航跟蹤方法，其特征在于，所述步驟三包括：根據在預估的目標方向上搜尋得到的距離預估的位置最近的路徑點B，前向搜尋路徑點B的遠程跟蹤點D，通過路徑點B和其前后兩個路徑點計算路徑點B的方向角α₁，通過路徑點D和其前后兩個路徑點計算路徑點D的方向角α₂；

通過公式R＝L/2*sin((α₂-α₁)/2)獲取移動機器人整體運動軌跡的圓弧半徑，其中，L為路徑點B與其遠程跟蹤點D連成的線段BD的長度；

通過公式w₃＝V/R獲取移動機器人的初始線速度下的第三角速度控制量，其中V表示移動機器人的初始線速度。

6.根據權利要求5所述的一種移動機器人導航跟蹤方法，其特征在于，所述步驟三中路徑點B的遠程跟蹤點D的選取方法為：在路徑點B的前向搜尋滿足搜索點到路徑點B的長度小于3倍的移動機器人長度的所有的點中距離路徑點B最遠的點作為遠程跟蹤點D。