[發明專利]軌跡跟隨實時控制方法、裝置和控制系統以及存儲介質

在審

• 申請號: 202111154584.4
• 申請日: 2021-09-29
• 公開（公告）號: CN113791535A
• 公開（公告）日: 2021-12-14
• 發明（設計）人: 謝傳泉;張東泉;浦劍濤;孟唐宇
• 申請（專利權）人: 北京布科思科技有限公司;山東布科思機器人有限責任公司;深圳布科思科技有限公司
• 主分類號: G05B11/42
• 分類號: G05B11/42
• 代理公司: 北京市鼎立東審知識產權代理有限公司 11751
• 代理人: 朱慧娟;陳佳妹

1.一種軌跡跟隨實時控制方法，其特征在于，包括如下步驟：

S1、獲取跟隨物與標簽之間的初始數據；

S2、預設計算規則，將所述初始數據按照所述預設計算規則計算并轉化，獲取所述標簽與所述跟隨物的驅動中心點之間的轉換數據；

S3、根據所述轉換數據設定運動參數，并根據所述運動參數建立PI運動控制方程；

S4、根據所述PI運動控制方程控制所述跟隨物的行走軌跡。

2.根據權利要求1所述的一種軌跡跟隨實時控制方法，其特征在于，所述獲取所述標簽與所述跟隨物的驅動中心點之間的轉換數據，包括：

S210、獲取角度距離測量設備與所述標簽之間的投影距離Li：

Li＝sqrt(L*L-H*H)，其中，

L為所述角度距離測量設備和所述標簽之間的相對距離；

H為所述角度距離測量設備和所述標簽之間的相對高度；

S220、根據所述投影距離Li，轉換并獲得所述標簽與所述跟隨物的驅動中心點之間的距離Lii：

Lii＝sqrt(Li*Li+D*D-2*Li*D*cos(π-fabs(theta)))，其中，

D為所述角度距離測量設備和所述跟隨物的驅動中心點之間的相對距離；

theta為所述標簽和所述角度距離測量設備測量之間與所述角度距離測量設備和所述跟隨物前進方向之間的夾角；

S230、根據所述投影距離Li和所述標簽與所述跟隨物的驅動中心點之間的距離Lii，轉換并獲得所述標簽和所述跟隨物的驅動中心點之間與所述跟隨物的驅動中心點和所述跟隨物前進方向之間的夾角Theta_i：

Theta_i＝acos((D*D+Lii*Lii-Li*Li)/(2*D*Lii))。

3.根據權利要求2所述的一種軌跡跟隨實時控制方法，其特征在于，所述根據所述轉換數據設定運動參數，包括：

S310、設置角速度運動參數：包括：

設置最小角速度Vw_min；

根據Vw_min，設置角速度段曲線：

Vw_current_max＝K*Theta_i+Vw_min；

其中：

K為大于0的限速系數；

Vw_current_max為當前允許的最大角速度；

S320、設置線速度運動參數，包括：

設置最小線速度Vx_max

設置線速度段曲線

Vx_current_max＝K1*Vw_current_max+Vx_max；

其中：

K1為小于0的線速度系數；

Vx_current_max為當前允許的最大線速度；

S330、設置角度距離運動參數，包括：

設置最佳跟隨距離D1，D1為跟隨過程中所述跟隨物的驅動中心點距離所述標簽的跟隨距離；

設置急停距離D2，D2為跟隨過程中所述跟隨物的驅動中心點距離所述標簽的最短距離，D2D1；

設置只角度跟隨距離D3，D3為跟隨過程中所述跟隨物的驅動中心點距離所述標簽的距離且當D2D3D1時，車輛只執行角速度。

設置目標跟隨角度Theta_target，Theta_target為跟隨過程中所述標簽和所述跟隨物的驅動中心點之間與所述跟隨物的驅動中心點和所述跟隨物前進方向之間的期望角度；

設置停止線速度角度值Theta_3，Theta_3為跟隨過程中所述標簽和所述跟隨物的驅動中心點之間與所述跟隨物的驅動中心點和所述跟隨物前進方向之間的停止線速度夾角。