機器人控制方法、動作計算裝置及機器人系統

專利類型

發明

專利國別 (專利申請國家)

中華民國

專利申請案號

110104082

專利證號

I 741943

專利獲證名稱

機器人控制方法、動作計算裝置及機器人系統

專利所屬機關 (申請機關)

國立陽明交通大學

獲證日期

2021/10/01

技術說明

本發明針對協作型機械臂提出終端持有工具時之即時安全避碰設計。所提出的避碰設計分為三個部分，分別是動態物體追蹤、終端工具點雲追蹤與預測以及即時避碰運動規劃。本發明以KinectV2深度攝影機對環境中物體與人員進行追蹤，接著透過K-D tree演算法分割工具點雲與障礙物點雲。針對終端工具的點雲，我們提出一套演算法對終端工具建立模型並預測其下一時刻之姿態，解決攝影機視線遮蔽的問題。在即時安全避碰設計方面，本發明提出一個新的人工力場演算法(Artificial Potential Field, APF)使機械臂達成更靈活地閃避。本發明將終端工具整個體積納入考量，除了在障礙物與終端工具間產生排斥力(Repulsive force)及在目標點與終端產生吸引力(Attractive force) 以外，我們提出旋轉扭矩(Virtual torque)的設計並將其加入人工力場，使機械臂之終端工具得以旋轉來調整姿態，提前預防以降低發生碰撞的機率，也可以使為了閃避而產生的額外動作變得較為平順，縮短避碰路徑。本發明以六軸協作型機械臂TM5-700進行實際驗證，實驗結果顯示採用本發明之旋轉扭矩比沒有採用時，其額外之閃避路徑可減少80.43%，成功驗證本發明所提出的方法可以讓終端持有工具的機械臂更有效地避開環境中的障礙物。 This invention proposes a real-time obstacle avoidance control scheme for a 6-DOF manipulator with a tool in the end effector. The system consists of environment monitoring, robot-tool segmentation and collision-free motion planning of the manipulator. A Kinect V2 RGBD camera is used to track obstacles including human and objects in the working space. Then the K-D tree algorithm is adopted to cluster point clouds of the tool and the obstacles. For robot-tool segmentation, we propose a method to model the tool in the end effector and predict its pose in order to solve the camera occlusion problem. For collision-free motion planning, a novel potential field algorithm is proposed to take into consideration of the pose of the tool. A virtual torque is proposed and added to the potential field in order to generate a smoother and shorter avoidance motion. The experimental results on a TM5-700 Cobot show that the manipulator with a tool in the end effector effectively avoid obstacles in real time and completed the assigned task. It is shown that the path length with the proposed virtual torque is shortened by 80.43% compared with the case without using the virtual torque.