[發(fā)明專利]基于環(huán)境感知的實時多任務(wù)多人人機交互系統(tǒng)

有效

• 申請?zhí)枺?/td>: 202111115194.6
• 申請日: 2021-09-23
• 公開（公告）號: CN113829343B
• 公開（公告）日: 2023-09-29
• 發(fā)明（設(shè)計）人: 歐林林;張旭環(huán);許成軍;張鑫;禹鑫燚
• 申請（專利權(quán)）人: 浙江工業(yè)大學(xué)
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 杭州天正專利事務(wù)所有限公司 33201
• 代理人: 王兵

1.基于環(huán)境感知的實時多任務(wù)多人人機交互系統(tǒng)，其特征在于：包括感知模塊、控制模塊、決策模塊，感知模塊通過相機去感知人機交互中的多人人體姿態(tài)信息、機器人位置信息和物體的位置信息，將獲取的人機姿態(tài)信息傳遞給決策模塊和控制模塊，決策模塊結(jié)合任務(wù)需求對收集到的人體姿態(tài)和機器人位置進(jìn)行監(jiān)聽和更新，并反饋到感知模塊，同時更新人機姿態(tài)信息，控制模塊通過獲取決策模塊提供的目標(biāo)位置信息和感知模塊提供的機器人當(dāng)前位置信息，對當(dāng)前機器人任務(wù)路徑重新規(guī)劃，將更新后的任務(wù)軌跡發(fā)送到真實機械臂，同時將機器人的更新后的位置信息反饋給決策模塊，GUI可視化處理相關(guān)的實驗結(jié)果；

所述感知模塊：首先對整體系統(tǒng)進(jìn)行標(biāo)定，包括相機和機器人位置，規(guī)定人機姿態(tài)表示方式，然后進(jìn)行多人人體姿態(tài)估計、識別和跟蹤，其中姿態(tài)估計得到人機交互中人的關(guān)節(jié)位置信息，而識別和跟蹤識別場景中的操作人員和非操作人員，包括不相關(guān)場外人員干擾；坐標(biāo)系標(biāo)定過程，是人機交互系統(tǒng)確定任意剛體變換的基礎(chǔ)，標(biāo)定過程確定機器人相對于相機的位置和方向；設(shè)w為世界坐標(biāo)，c為相機坐標(biāo)，r₀為機器人的基坐標(biāo)，h₀為人類的根關(guān)節(jié)；對于固定相機和機器人，通過機器人手眼標(biāo)定方法計算變換矩陣單目相機外參矩陣表示相機c相對于世界坐標(biāo)w的變換，機器人的基坐標(biāo)到世界坐標(biāo)的變換表示為公式(1)，變換是常數(shù)矩陣；

人體姿態(tài)估計與跟蹤過程，是使用基于深度學(xué)習(xí)的算法進(jìn)行單目多人人體姿態(tài)估計，設(shè)計多任務(wù)結(jié)構(gòu)網(wǎng)絡(luò)，有多個分支；網(wǎng)絡(luò)中Mobile?Net作為骨干網(wǎng)絡(luò)Backbone，2D姿態(tài)估計和3D姿態(tài)估計兩個階段都是基于骨干網(wǎng)絡(luò)完成各自的任務(wù)，2D姿態(tài)估計部分包括InitialStage和Refinement?Stage，首先輸入人體圖像，代表人體圖像信息的是表示人體2D姿態(tài)的15個關(guān)節(jié)點，每個關(guān)節(jié)點包含對應(yīng)的像素坐標(biāo)和對應(yīng)的置信度，表示神經(jīng)網(wǎng)絡(luò)對關(guān)節(jié)點的檢測和評估；3D階段的任務(wù)基于2D姿態(tài)估計的結(jié)果，通過Backbone、ResBlock、RefinementStage完成提取人體的三維姿態(tài)，人體3D姿態(tài)信息包括每個關(guān)節(jié)點相對于根關(guān)節(jié)的空間坐標(biāo)位置，最終轉(zhuǎn)換到相機坐標(biāo)下表示；應(yīng)用一種基于貪心算法的連續(xù)幀三維位姿跟蹤算法，解決操作者和非操作者在人機交互系統(tǒng)中的連續(xù)跟蹤和識別問題，使得在配對過程中因關(guān)聯(lián)錯誤或遮擋而在某些幀中不存在的情況下，也能有效地跟蹤骨架；因為當(dāng)前階段僅存在三維姿態(tài)，骨架之間代價函數(shù)被定義為：

其中，n＝(1，2，3，...，N)表示關(guān)節(jié)編號，以及N為骨架的總關(guān)節(jié)數(shù)量；表示當(dāng)前時刻的編號為i的姿態(tài)，表示該骨架的第n個關(guān)節(jié),是用來表示第n個關(guān)節(jié)在t時刻是否存在；對人機姿態(tài)分別進(jìn)行表示，機械臂任意連桿r_j在世界坐標(biāo)系下的變換表示為：

將人體的位置在世界坐標(biāo)中表達(dá)出來；假設(shè)表示關(guān)節(jié)h_j相對于根關(guān)節(jié)h₀的位置，關(guān)節(jié)在世界坐標(biāo)中的位置通過式(5)計算出來；

其中為相機坐標(biāo)與世界坐標(biāo)的轉(zhuǎn)換矩陣，為相機坐標(biāo)與人體根關(guān)節(jié)的之間的變換矩陣；

所述決策模塊：在決策模塊中，系統(tǒng)根據(jù)先驗信息決定交互的規(guī)則，先驗信息包括交互任務(wù)、操作人員數(shù)量；在交互過程中，通過實時接收感知模塊中的人機姿態(tài)，根據(jù)交互規(guī)則決定機器人的運動軌跡；此外，決策模塊還對人機交互場景進(jìn)行監(jiān)控、可視化，生成機器人任務(wù)目標(biāo)位置和記錄相關(guān)的度量參數(shù)；

在人機安全交互中，人體關(guān)節(jié)點被當(dāng)作移動或者靜止的障礙物；對于移動障礙物避障的處理方法是利用障礙物與機器人末端之間的引力和斥力相加使得機器人進(jìn)行主動避讓；在交互過程中，通過實時計算機器人的各個連桿與人體的各個連桿的距離，目標(biāo)點根據(jù)引力和斥力向量相加進(jìn)行修正；當(dāng)人體與機器人距離足夠大的情況下，機器人向任務(wù)目標(biāo)運動，目標(biāo)位置對機器人工具坐標(biāo)系r_tool產(chǎn)生引力向量F_o；

當(dāng)檢測到人體i連桿坐標(biāo)系與r_tool小于一定的安全閾值時，產(chǎn)生斥力向量

通過斥力向量與引力向量相加生成合成力F_add：

其中，I，J分別表示人的集合和每個人的關(guān)節(jié)集合；進(jìn)而利用下式生成目標(biāo)點

其中，δ∈(0，∞)為目標(biāo)點距離系數(shù)，用于調(diào)整合成力與生成的目標(biāo)點之間的線性關(guān)系；針對人機協(xié)作任務(wù)，物體的位置是通過檢測其表面粘貼的標(biāo)記生成；目標(biāo)點就是物體在世界坐標(biāo)系下的表示，基于下式生成目標(biāo)點；

其中，表示物體在相機c下的姿態(tài)信息表示；人機協(xié)作任務(wù)目標(biāo)點要根據(jù)任務(wù)的需求進(jìn)行單獨設(shè)計；

所述控制模塊：在交互過程中，操作人員與機器人在同一工作空間進(jìn)行交互、協(xié)作，并為機器人指定任務(wù)目標(biāo)；在人機交互過程中，一方面，機器人需要對頻繁變化的目標(biāo)做出快速響應(yīng)，以提高效率和交互舒適性；另一方面要考慮機器人安全，糾正不合理的目標(biāo)指令，及時反饋給操作者；控制模塊進(jìn)行基于邊界約束的目標(biāo)校正和設(shè)計基于機器人模型預(yù)測控制的底層控制器；根據(jù)障礙物邊界約束生成路徑，并輸入到機器人控制器，通過機器人控制器來實現(xiàn)對機械臂的實際操作；

設(shè)置奇異狀態(tài)邊界以限制機器人的工作空間范圍；假設(shè)機器人工作空間W{R′}，目標(biāo)姿態(tài)在超出W{R′}時將被重新限制到W{R′}的邊界，以此避免奇異姿態(tài)的出現(xiàn)；除工作空間的限制外，機器人的運動還受到工作空間中靜態(tài)障礙物的限制，對于體積較大的靜態(tài)障礙物，機器人難以通過運動規(guī)劃對其進(jìn)行避讓，根據(jù)此類障礙物的形狀形成邊界，在奇異狀態(tài)邊界限制的基礎(chǔ)上對工作空間進(jìn)行限制；障礙物邊界的生成根據(jù)障礙物的接觸閾值，在障礙物周圍形成外包絡(luò)邊界，將目標(biāo)姿態(tài)限制在外包絡(luò)邊界外，以免和環(huán)境發(fā)生碰撞；機器人在運動時會與自身發(fā)生碰撞，在機械臂連桿周圍根據(jù)接觸閾值設(shè)置外包絡(luò)邊界約束，將目標(biāo)姿態(tài)修正在邊界外，防止自碰撞的發(fā)生；

在人機交互系統(tǒng)中，在給定目標(biāo)位置的情況下，通過逆運動學(xué)求解各關(guān)節(jié)的狀態(tài)；將每個關(guān)節(jié)的當(dāng)前狀態(tài)和目標(biāo)狀態(tài)作為非線性優(yōu)化器的輸入，輸出控制動作u_i(i＝1，...，6)；將u_i輸入MPC控制器，通過更新關(guān)節(jié)狀態(tài)實時控制機械臂的運動；

控制器計算機器人從起始位姿q₀到期望位姿T_goal的軌跡；軌跡規(guī)劃的非線性模型預(yù)測控制問題表示為：

X_k和U_k分別對應(yīng)關(guān)節(jié)位置和速度極限和關(guān)節(jié)加速度極限約束；l_k(x_k，u_k)為第k個瞬間的階段成本，以參考跟蹤誤差為基準(zhǔn)，跟蹤誤差包括控制誤差、系統(tǒng)狀態(tài)誤差，則表示為：

l_k(x_k，u_k)＝(x_k-x_ref)^TQ_k(x_k-x_ref)+(u_k-u_ref)^TR_k(u_k-u_ref)????????(13)

l_N(x_k)＝(x_N-x_ref)^TQ_N(x_N-x_ref)???????????????????(14)

其中R_k,Q_k分別表示為加速度和系統(tǒng)狀態(tài)的懲罰系數(shù)；

通過調(diào)整懲罰系數(shù)的大小，針對不同的交互情況采用不同的機器人控制策略，約束機器人的運動，實現(xiàn)機器人交互控制。