[發明專利]挖掘機臂運動控制方法、系統及裝置

在審

• 申請號: 201811328944.6
• 申請日: 2018-11-08
• 公開（公告）號: CN109296019A
• 公開（公告）日: 2019-02-01
• 發明（設計）人: 盧興陽;隋少龍;張越
• 申請（專利權）人: 北京拓疆者智能科技有限公司
• 主分類號: E02F9/14
• 分類號: E02F9/14;E02F9/20
• 代理公司: 北京超凡志成知識產權代理事務所(普通合伙) 11371
• 代理人: 范彥揚

1.一種挖掘機臂運動控制方法，其特征在于，包括：

獲取當前目標挖掘點的位置信息、挖掘面與挖掘機基坐標系的夾角信息及挖斗切面與挖掘面的夾角信息；

通過預設的逆運動學模型解算得到挖掘機臂的期望位姿狀態；

確定挖掘機臂當前位姿狀態與期望位姿狀態的位姿差值，并生成位姿差值信號；

將所述位姿差值信號發送給控制器，以使所述控制器控制所述挖掘機臂運動到所述期望位姿狀態。

2.根據權利要求1所述的挖掘機臂運動控制方法，其特征在于，還包括：

通過傳感器模塊監測所述挖掘機臂是否運動至所述期望位姿狀態；

如果是，向所述控制器發送停止運動指令；

如果否，重新確定當前位姿狀態與所述期望位姿狀態的位姿差值，生成位姿差值信號，并將位姿差值信號作為控制器輸入信號發送給所述控制器。

3.根據權利要求1或2所述的挖掘機臂運動控制方法，其特征在于，所述通過預設的逆運動學模型解算得到挖掘機臂的期望位姿狀態的步驟，包括：

根據所述當前目標挖掘點的位置信息、挖掘面與挖掘機基坐標系的夾角信息及挖斗切面與挖掘面的夾角信息建立運動學模型；所述運動學模型為：

其中，c₁＝cosθ₁,c₁₂＝cos(θ₁+θ₂),c₁₂₃＝cos(θ₁+θ₂+θ₃)，s₁＝sinθ₁,s₁₂＝sin(θ₁+θ₂),s₁₂₃＝sin(θ₁+θ₂+θ₃)，θ₁表示動臂與基座標系橫軸方向的夾角，θ₂表示動臂與小臂的夾角，θ₃表示小臂與挖斗的夾角，α表示挖斗齒尖處的角度值，β表示挖斗切面與挖掘面的夾角，γ表示挖掘面與挖掘機基座標系的夾角，l₁表示挖掘機的動臂長度，l₂表示挖掘機的小臂長度，l₃表示挖掘機的挖斗長度，(x，y，z)為挖斗的齒尖在挖掘機基坐標系下的位置坐標；

根據所述運動學模型確定逆運動學模型；通過所述逆運動學模型解算得到挖掘機臂的期望位姿狀態；所述逆運動學模型為：

4.根據權利要求1或2所述的挖掘機臂運動控制方法，其特征在于，將所述位姿差值信號發送給控制器，以使所述控制器控制所述挖掘機臂運動到所述期望位姿狀態的步驟，包括：

將所述位姿差值信號發送給控制器，以使所述控制器生成所述挖掘機臂的控制輸入信號；

通過所述控制輸入信號控制比例電磁閥以使所述挖掘機臂運動到所述期望位姿狀態；所述比例電磁閥設置在所述挖掘機臂的各個關節。

5.根據權利要求1或2所述的挖掘機臂運動控制方法，其特征在于，所述獲取當前目標挖掘點的位置信息、挖掘面與挖掘機基坐標系的夾角信息及挖斗切面與挖掘面的夾角信息的步驟，包括：

建立基座標系，所述基坐標系的橫軸與所述挖掘機的履帶共面，所述基坐標系的縱軸垂直于所述挖掘機的履帶所在平面；

根據所述基座標系確定當前目標挖掘點的位置信息、挖掘面與挖掘機基坐標系的夾角信息及挖斗切面與挖掘面的夾角信息。