[發明專利]路徑跟蹤方法及終端設備

有效

• 申請號: 201811289515.2
• 申請日: 2018-10-31
• 公開（公告）號: CN111123904B
• 公開（公告）日: 2023-07-18
• 發明（設計）人: 熊友軍;劉志超;張健
• 申請（專利權）人: 深圳市優必選科技有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 深圳中一聯合知識產權代理有限公司 44414
• 代理人: 張全文

1.一種路徑跟蹤方法，其特征在于，包括：

獲取預設路徑和運動目標的當前位置；

在所述預設路徑上確定所述當前位置對應的前瞻路徑點；

獲取所述前瞻路徑點對應的路徑曲率；

根據所述前瞻路徑點對應的路徑曲率確定所述運動目標在當前位置的調整速度，包括：選取所述前瞻路徑點所在路段內的至少一個路徑點；計算各個路徑點對應的路徑曲率及各個路徑點對應的第一距離；所述任一路徑點對應的第一距離為該任一路徑點與前一個路徑點之間的距離；計算各個路徑點對應的路徑曲率的絕對值與對應第一距離的乘積值，并將各個路徑點對應的乘積值求和，得到乘積值之和；根據所述乘積值之和確定調整系數；根據所述調整系數和所述運動目標的當前速度計算所述運動目標在當前位置的調整速度。

2.如權利要求1所述的路徑跟蹤方法，其特征在于，所述在所述預設路徑上確定所述當前位置對應的前瞻路徑點包括：

獲取所述當前位置對應的前瞻距離；

在所述預設路徑上選取第一路徑點作為所述前瞻路徑點；所述第一路徑點與所述當前位置之間的距離為所述當前位置對應的前瞻距離。

3.如權利要求2所述的路徑跟蹤方法，其特征在于，所述獲取所述當前位置對應的前瞻距離包括：

獲取預設前瞻距離，將所述預設前瞻距離作為所述當前位置對應的前瞻距離；或者，

獲取所述運動目標的上一位置對應的調整前瞻距離，將所述上一位置對應的調整前瞻距離作為所述當前位置對應的前瞻距離。

4.如權利要求1所述的路徑跟蹤方法，其特征在于，所述選取所述前瞻路徑點所在路段內的至少一個路徑點包括：

查找所述當前位置對應的第二路徑點；所述第二路徑點為所述預設路徑上所述當前位置對應的垂點；

在所述預設路徑上選取所述第二路徑點與所述前瞻路徑點之間的路段內的至少一個路徑點。

5.如權利要求1所述的路徑跟蹤方法，其特征在于，所述計算各個路徑點對應的路徑曲率包括：

計算任一路徑點的路徑曲率與前一路徑點的路徑曲率之間的差值作為該任一路徑點對應的路徑曲率。

6.如權利要求1所述的路徑跟蹤方法，其特征在于，所述根據所述乘積值之和確定調整系數包括：

計算所述乘積值之和與前瞻距離的第一比值；所述前瞻距離為所述運動目標的當前位置與所述前瞻路徑點之間的距離；

根據所述第一比值確定調整系數。

7.如權利要求6所述的路徑跟蹤方法，其特征在于，所述根據所述第一比值確定調整系數包括：

根據懲罰函數公式及所述第一比值計算所述調整系數；所述懲罰函數公式為：

其中，P為所述調整系數，χ為所述第一比值。

8.如權利要求1、5至7任一項所述的路徑跟蹤方法，其特征在于，所述當前速度包括當前線速度，所述調整速度包括調整線速度和調整角速度；

所述根據所述調整系數和所述運動目標的當前速度計算所述運動目標在當前位置的調整速度包括：

計算所述運動目標的當前線速度與所述調整系數的乘積，得到所述調整線速度；

計算前瞻距離與所述調整系數的乘積，得到調整前瞻距離；所述前瞻距離為所述運動目標的當前位置與所述前瞻路徑點之間的距離；

根據所述調整線速度、所述調整前瞻距離和偏差角度計算調整角速度；所述偏差角度為所述運動目標的當前位置到所述前瞻路徑點的向量方向與所述運動目標的當前線速度的方向之間的夾角角度。

9.如權利要求8所述的路徑跟蹤方法，其特征在于，所述根據所述調整線速度、所述調整前瞻距離和偏差角度計算調整角速度包括：

計算所述調整線速度與所述調整前瞻距離之間的第二比值；

計算所述偏差角度的正弦值與所述第二比值的乘積，得到所述調整角速度。