[發明專利]工業機器人平滑抓取移動物體的運動規劃系統及其方法

有效

• 申請號: 201710500906.3
• 申請日: 2017-06-27
• 公開（公告）號: CN107414825B
• 公開（公告）日: 2019-06-11
• 發明（設計）人: 尚偉偉;張馳;叢爽;宋方井
• 申請（專利權）人: 中國科學技術大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 安徽省合肥新安專利代理有限責任公司 34101
• 代理人: 陸麗莉;何梅生

1.一種工業機器人平滑抓取移動物體的運動規劃系統，是應用于由工業機器人、攝像頭、傳送帶、目標物體和障礙物所組成的工業環境中，在所述傳送帶的一側設置有工業機器人，在所述傳送帶的另一側設置有所述攝像頭；所述攝像頭的視場為所述工業機器人的工作空間；所述傳送帶上放置有所述目標物體；所述工業機器人包含由肩關節、肘關節和腕關節構成的手臂平面和末端執行器；分別選取所述工業機器人的肘關節、腕關節、肩關節和肘關節之間連桿的中點以及肘關節和腕關節之間連桿的中點為球心，并以為半徑的四個球體共同作為所述工業機器人的免碰撞區域；所述運動規劃系統包括：平滑抓取模型構建模塊，避障算法模塊，平滑抓取算法模塊；

所述平滑抓取模型構建模塊根據所述攝像頭視場內的目標物體和所述工業機器人的末端執行器之間的相對關系構建當前時刻的吸引向量；并通過在間隔時間Δt內吸引向量的變化量，來估算所述目標物體的速度，從而得到下一時刻的吸引向量；

所述平滑抓取模型構建模塊根據所述攝像頭視場內的障礙物和所述工業機器人的各個關節之間的距離向量以及所述免碰撞區域，構建所述工業機器人的手臂平面和所述障礙物之間的排斥向量；

所述避障算法模塊是以所述障礙物與所述工業機器人的各個關節之間的距離向量作為輸入來判斷所述障礙物是否出現在所述免碰撞區域內，若出現，則根據所述排斥向量，計算所述工業機器人的位置，從而避開所述障礙物；

所述平滑抓取算法模塊在所述障礙物離開所述免碰撞區域內的條件下，將所述目標物體與所述工業機器人的末端執行器之間的吸引向量以及所述目標物體的速度作為輸入，計算所述目標物體的運動軌跡；從而根據所述目標物體的運動軌跡生成所述工業機器人的跟蹤軌跡，以實現對所述目標物體的平滑抓取。

2.一種工業機器人平滑抓取移動物體的運動規劃方法，是應用于由工業機器人、攝像頭、傳送帶、目標物體和障礙物所組成的環境中，在所述傳送帶的一側設置有工業機器人，在所述傳送帶的另一側設置有所述攝像頭；所述攝像頭的視場為工業機器人的工作空間；所述傳送帶上放置有所述目標物體；所述工業機器人包含由肩關節、肘關節和腕關節構成的手臂平面和末端執行器；分別選取所述工業機器人的肘關節、腕關節、肩關節和肘關節之間連桿的中點以及肘關節和腕關節之間連桿的中點為球心，并以為半徑的四個球體共同作為所述工業機器人的免碰撞區域；所述運動規劃方法是按如下步驟進行：

步驟1、根據所述工業機器人的正運動學，得到所述工業機器人的末端執行器在所述工業機器人的基坐標系下的位姿

式(1)中，表示所述末端執行器在所述基坐標系下的空間位置坐標，分別表示的是所述末端執行器在基坐標系下的俯仰角、偏航角、滾轉角；

步驟2、利用所述攝像頭獲取所述目標物體在所述基坐標系下的位姿

式(2)中，表示所述目標物體在所述基坐標系下的空間位置坐標，分別表示的是所述目標物體在基坐標系下的俯仰角、偏航角、滾轉角；

步驟3、利用式(3)獲得當前時刻t₁下所述目標物和所述工業機器人的末端執行器之間的吸引向量V_att：

式(3)中，V_trans表示所述吸引向量V_att中的平移向量，T表示轉置；V_rot表示所述吸引向量V_att中的旋轉向量，并有：

步驟4、利用式(6)計算所述吸引向量V_att在間隔時間Δt的變化量，從而得到所述目標物體的運動速度

步驟5、利用式(7)計算下一時刻t₂的吸引向量V_att′：

步驟6、利用所述攝像頭檢測視場內是否存在所述障礙物，若存在，則執行步驟7，否則，執行步驟8；

步驟7、構造排斥向量：

步驟7.1、檢測所述障礙物在所述基坐標系下的初始時刻的位置和相鄰時刻的位置并計算運動向量表示所述障礙物在所述基坐標系下的空間位置坐標；

步驟7.2、選取所述工業機器人的肩關節S、肘關節E、腕關節W所張成的手臂平面S-E-W的單位法向量所述單位法向量的方向與運動向量沿所述手臂平面法向的分量的方向相反；按式(8)計算單位法向量和運動向量的夾角γ：

步驟7.3、利用式(9)獲得所述工業機器人的手臂平面S-E-W和所述障礙物之間的排斥向量V_R：

V_R＝v_R·u_j (9)

式(9)中，v_R表示所述排斥向量V_R的模值，u_j表示所述排斥向量V_R的方向，并有：

式(10)中，V_jmax表示所述工業機器人上與肩關節相連的腰部關節的最大角速度；α表示比例因子；

式(11)中，u表示所述腰部關節順時針旋轉，-u表示所述腰部關節逆時針旋轉；

步驟7.4、判斷所述夾角γ≠π/2是否成立，若成立，表示所述障礙物正在靠近所述手臂平面，則執行步驟7.5；否則，表示所述手臂平面法向量和所述障礙物的運動向量平行；則執行步驟7.7；

步驟7.5、以所述排斥向量作為所述工業機器人的腰部關節的速度，計算在所述相鄰時刻內所述腰部關節的位置，并控制所述工業機器人運動到相應的位置上；

步驟7.6、在所述工業機器人運動到相應位置后，按式(12)更新所述手臂平面S-E-W的單位法向量后，返回步驟7.4執行：

式(12)中，β表示步驟7.5中所述腰部關節在所述相鄰時刻內旋轉的角度；η表示初始時刻下所述手臂平面S-E-W在所述基坐標系的x-y平面上的投影與x軸的夾角；

步驟7.7、所述攝像頭檢測在免碰撞區域內是否存在障礙物，若存在，則返回步驟7.4執行；若不存在，則執行步驟8；

步驟8、依據所述下一時刻t₂的吸引向量V_att′，在向量V_att′-V_att上取N個路徑點，并利用式(13)得到第n個路徑點的位置x_n：

步驟9、利用逆運動學解出所述第n個路徑點對應工業機器人的各個關節的關節角，再利用式(14)得到到達所述第n個路徑點的速度

式(14)中，J⁺表示雅克比矩陣的偽逆，表示所述末端執行器到達所述第n個路徑點時在笛卡爾空間中的速度，即為目標物體的運動速度是關節角的限制約束，并有第i個關節角的限制約束

式(15)中，和分別是第i個關節角的上限和下限；θ^D表示任一關節角的緩沖區域的長度，表示任一關節角的補償速度，i∈[1,5]；

步驟10、根據所述第n個路徑點的位置x_n和速度得到第n個路徑點，從而控制所述工業機器人按照N個路徑點所構成的跟蹤軌跡運動，直到滿足式(16)時，閉合所述工業機器人的夾持器，并向上提起目標物體，從而實現對所述目標物體的平滑抓取：

||V_trans||<d，V_rot＝0 (16)

式(16)中，d表示抓取時所述末端執行器和目標物體之間的最大距離。