[發(fā)明專利]一種用于雙機(jī)器人協(xié)作的運動規(guī)劃方法及系統(tǒng)

有效

• 申請?zhí)枺?/td>: 201911048824.5
• 申請日: 2019-10-31
• 公開（公告）號: CN110666804B
• 公開（公告）日: 2021-07-13
• 發(fā)明（設(shè)計）人: 吳海彬;游曉君;黃藝?yán)?/a>
• 申請（專利權(quán)）人: 福州大學(xué)
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 福州元創(chuàng)專利商標(biāo)代理有限公司 35100
• 代理人: 錢莉;蔡學(xué)俊

1.一種用于雙機(jī)器人協(xié)作的運動規(guī)劃方法，其特征在于，包括以下步驟：

在ROS系統(tǒng)中，建立雙機(jī)器人及工作環(huán)境的模型描述文件以及雙機(jī)器人的MoveIt配置文件；

進(jìn)行初始化設(shè)置，并創(chuàng)建操作對象，將操作對象作為參數(shù)創(chuàng)建雙臂指令，編排雙機(jī)器人執(zhí)行任務(wù)的動作；

逐條解析雙臂指令，并調(diào)用MoveIt函數(shù)對該指令進(jìn)行運動規(guī)劃，并將規(guī)劃結(jié)果進(jìn)行保存與發(fā)布；若存在規(guī)劃有誤的指令則停止解析與規(guī)劃；

其中，一項任務(wù)由多條雙臂指令組成，每條雙臂指令由兩條單臂指令組成，每臺機(jī)器人各執(zhí)行其中的一條單臂指令；

其中，具體包括以下步驟：

步驟1：在SolidWorks中建立雙機(jī)器人及其工作環(huán)境的裝配圖，在ROS中生成該雙機(jī)器人的MoveIt配置文件；

步驟2：在ROS中加載urdf文件和配置文件，然后依次啟動：規(guī)劃結(jié)果處理單元、MoveIt的主要節(jié)點move_group、rviz節(jié)點、人機(jī)交互單元；在人機(jī)交互單元中設(shè)置雙機(jī)器人的初始點；

步驟3：用戶在人機(jī)交互單元中創(chuàng)建若干個操作對象，所述操作對象包括待操作物體和標(biāo)記點；

步驟4：創(chuàng)建若干條雙臂指令形成雙機(jī)器人執(zhí)行任務(wù)的動作過程，在每條雙臂指令中，將調(diào)用一個操作對象；

步驟5：點擊人機(jī)交互界面上的“執(zhí)行”按鈕，人機(jī)交互單元將雙臂操作指令逐條解析，再逐條調(diào)用MoveIt的函數(shù)接口；MoveIt通過move_group節(jié)點進(jìn)行運動規(guī)劃并將運動規(guī)劃結(jié)果封裝為FollowJointTrajectory消息發(fā)往規(guī)劃結(jié)果處理單元中；若某一條指令的運動規(guī)劃有誤，則停止解析與規(guī)劃并在人機(jī)交互界面上顯示錯誤，否則進(jìn)入下一步；

步驟6：規(guī)劃結(jié)果處理單元接收FollowJointTrajectory消息，將其拆分為左右臂的運動軌跡，并寫入文本文件中保存，同時使用關(guān)節(jié)消息發(fā)布器將運動規(guī)劃結(jié)果轉(zhuǎn)化為JointState類型的消息發(fā)布在ROS上；rviz訂閱該消息后，將顯示雙機(jī)器人的運動過程；人機(jī)交互單元訂閱該消息后，將在界面中更新雙機(jī)器人的位姿信息；

步驟7：若雙臂指令全部解析完畢，則本次任務(wù)執(zhí)行完成，否則回到步驟5解析下一條指令。

2.根據(jù)權(quán)利要求1所述的一種用于雙機(jī)器人協(xié)作的運動規(guī)劃方法，其特征在于，所述初始化設(shè)置具體為：在ROS系統(tǒng)中，將雙機(jī)器人及其工作環(huán)境的模型進(jìn)行顯示，并設(shè)置雙機(jī)器人的初始點。

3.根據(jù)權(quán)利要求1所述的一種用于雙機(jī)器人協(xié)作的運動規(guī)劃方法，其特征在于，所述逐條解析雙臂指令，并調(diào)用MoveIt函數(shù)對該指令進(jìn)行運動規(guī)劃，并將規(guī)劃結(jié)果進(jìn)行保存與發(fā)布，若存在規(guī)劃有誤的指令則停止解析與規(guī)劃具體包括以下步驟：

步驟S1：判斷當(dāng)前雙臂指令的規(guī)劃結(jié)果是否超出預(yù)設(shè)的運動范圍或者是否產(chǎn)生碰撞，若是，則判斷該條指令的規(guī)劃有誤，結(jié)束解析與規(guī)劃，否則進(jìn)入步驟S2；

步驟S2：保存并發(fā)布當(dāng)前雙臂指令的規(guī)劃結(jié)果，并判斷是否已經(jīng)解析完所有的雙臂指令，若是，則結(jié)束解析與規(guī)劃，否則返回步驟S1。

4.根據(jù)權(quán)利要求3所述的一種用于雙機(jī)器人協(xié)作的運動規(guī)劃方法，其特征在于，所述保存并發(fā)布當(dāng)前雙臂指令的規(guī)劃結(jié)果具體為：實時接收運動規(guī)劃結(jié)果和發(fā)布雙機(jī)器人的位姿消息，同時將運動規(guī)劃結(jié)果寫入文件中保存；雙機(jī)器人每次執(zhí)行任務(wù)，都生成一份保存運動規(guī)劃結(jié)果的文件。

5.根據(jù)權(quán)利要求1所述的一種用于雙機(jī)器人協(xié)作的運動規(guī)劃方法，其特征在于，所述操作對象的類型包括待操作物體及標(biāo)記點，所述待操作物體是雙機(jī)器人在執(zhí)行任務(wù)的過程中所取用的包括零件、輔件、工具在內(nèi)的物體進(jìn)行簡化后的簡單幾何體；所述標(biāo)記點是雙機(jī)器人運動過程中必須經(jīng)過或到達(dá)的特定點；創(chuàng)建待操作物體時，輸入的參數(shù)為幾何體尺寸以及物體位姿；創(chuàng)建標(biāo)記點時，輸入的參數(shù)為點的位姿；對生成的待操作物體和標(biāo)記點分別進(jìn)行編號。