[發明專利]一種可自主規劃軌跡的挖掘機臂智能控制系統及方法

有效

• 申請號: 201910159140.6
• 申請日: 2019-03-04
• 公開（公告）號: CN109778939B
• 公開（公告）日: 2021-07-09
• 發明（設計）人: 李金銘;王一樂;艾云峰;馬厚雪;王凱;王飛躍
• 申請（專利權）人: 江蘇徐工工程機械研究院有限公司
• 主分類號: E02F9/20
• 分類號: E02F9/20;E02F9/22
• 代理公司: 南京縱橫知識產權代理有限公司 32224
• 代理人: 薛海霞;董建林

1.一種可自主規劃軌跡的挖掘機臂智能控制系統，其特征在于，所述的控制系統由環境感知模塊、軌跡規劃模塊以及執行模塊組成；所述的環境感知模塊，用于在施工過程中采集挖掘機臂的施工環境數據；所述的軌跡規劃模塊，利用環境感知模塊采集的施工環境數據完成挖掘機臂的運動規劃；所述的執行模塊，依據軌跡規劃模塊的輸出結果完成挖掘機臂的運動控制；

軌跡規劃模塊包含狀態決策子模塊、模型預測子模塊以及動作選擇子模塊；

所述狀態決策子模塊，其利用施工環境數據針對當前的施工環境狀態做出決策，即得到環境狀態s和反饋值r；

所述模型預測子模塊，依據當前計算的反饋值r，將當前的環境狀態s轉化為一個Q值，Q值用于量度挖掘機臂智能控制系統工作過程中機械臂在各個自由度上不同動作的發生概率；

所述的動作選擇子模塊，其依據來自狀態決策子模塊的反饋值r與來自模型預測子模塊的Q值完成運動規劃。

2.根據權利要求1所述的一種可自主規劃軌跡的挖掘機臂智能控制系統，其特征在于，狀態決策子模塊包含數據融合單元與反饋值計算單元；所述數據融合單元，用于將來自環境感知模塊的數據組成有效的數據形式，即環境狀態s；所述反饋值計算單元，依據環境狀態s給出一個反饋值r；環境狀態s包含利用施工環境數據推算得到的障礙物中心點信息、在不同方向上的障礙物尺度信息以及挖掘機臂到障礙物的距離信息；為障礙物中心點，為三維隨機向量，cloud(obj)為障礙物obj的點云信息；

在方向上障礙物的尺度信息如下：為以為端點的任意單位向量，sc為在方向上障礙物尺度信息，是cloud(obj)中的元素；

假設t₀時刻挖掘機臂上的點構成一個集合S，那么應以如下方式求取挖掘機臂到障礙物的距離信息L：

首先，計算方向

其次，計算與向上的障礙物尺度信息，如下所示：

最后，計算

其中，為挖掘機臂運動構件表面的一個點，obj_set為挖掘機臂工作空間中的障礙物列表。

3.根據權利要求2所述的一種可自主規劃軌跡的挖掘機臂智能控制系統，其特征在于，反饋值r的取值應滿足以下三個原則：可自主規劃軌跡的挖掘機臂智能控制系統工作過程中，挖掘機臂到障礙物obj_k的距離L_k應該不斷增大，即挖掘機鏟斗到目標點位置的距離L₀應該不斷減小，即dL₀＜0；挖掘機鏟斗到目標點姿態的差值Δθ應該不斷減小，即d(Δθ)＜0；在以上三個原則的基礎上，按照如下步驟求取反饋值r：

首先，求取挖掘機臂關節運動的期望速度挖掘機臂的正運動學可以表示為為挖掘機臂操作空間中的位姿向量，為挖掘機臂關節空間中的位姿向量，q_i為挖掘機臂關節空間中自由度i的位姿；挖掘機臂的一個微分運動可以表示為為挖掘機臂操作空間中的速度向量，為挖掘機臂關節空間中的速度向量，為挖掘機臂關節空間中自由度為i的運動速度；構造目標函數fun＝∑_k(α_kdL_k)+βdL₀+γd(Δθ)，其中，α_k＞0，β，γ＜0，α_k、β、γ分別表示目標函數中各個分量dL_k、dL₀與Δθ的權重系數；期望速度的求取過程是一個對目標函數ftn的最優化過程，具體可表示為：

其中，vf_max_i為挖掘機臂在自由度i上正向運動的最大速度，而vb_max_i為挖掘機臂在自由度i上反向運動的最大速度，為當前時刻挖掘機臂在自由度i上的運動速度，af_max_i為挖掘機臂在自由度i上做加速運動時的最大加速度，而ab_max_i為挖掘機臂在自由度i上做減速運動時的最大加速度，Δt為控制周期，而i＝1，2，…，n；

其次，對求得的期望速度進行歸一化：

其中，pre_r_i為將求得的自由度為i時的期望速度按照最大速度進行歸一化后得到的值，i＝1，2，…，n；

最后，計算反饋值r：

r＝(r_·i)

其中，k_i為增益系數，r_·i為挖掘機臂在自由度i上的各個反饋值所構成的列向量，而i＝1，2，…，n。