[發明專利]一種抓臂式清污機器人運動控制系統

在審

• 申請號: 202011246801.8
• 申請日: 2020-11-10
• 公開（公告）號: CN112276911A
• 公開（公告）日: 2021-01-29
• 發明（設計）人: 黃振東;高云峰;陳榮娜;王亞;王之輝;袁家寶
• 申請（專利權）人: 安徽省六安恒源機械有限公司
• 主分類號: B25J9/00
• 分類號: B25J9/00;B25J9/16;B25J13/00
• 代理公司: 合肥正則元起專利代理事務所(普通合伙) 34160
• 代理人: 劉培越

1.一種抓臂式清污機器人運動控制系統，其特征在于，包括數據庫管理模塊、運動控制模塊、操作模塊、異常檢出模塊、數據采集模塊；所述運動控制模塊包括數據存儲單元、軌跡規劃單元、運動學單元、通訊單元、示教單元、安全管理單元以及人機交互單元；

所述軌跡規劃單元用于對抓臂式清污機器人進行抓臂空間軌跡規劃，具體的規劃過程包括以下步驟：

步驟一：根據清污機器人實際清污路徑，設定清污機器人的起始位置為P1，攔污柵上端點為P2，河面為P3；

步驟二：在清污過程中，清污機器人從起始位置P1向下運動，到達攔污柵上端點P2，然后沿著攔污柵向下運動，其末端抓斗爪齒與攔污柵重合，到達河面P3，抓取污物；

步驟三：采用5次多項式進行兩段直線軌跡的過渡，過渡起始點B位置P1，速度V1，加速度0，過渡終止點C位置P2，速度V2，加速度0；

步驟四：構建軌跡規劃模型，將全程路徑分為5個階段，分別采用正弦加減速、五次多項式等插補方式進行路徑插補，p1-A加速階段，A-B第一段直線勻速階段，B-C直線段間軌跡過渡階段，C-D第二段直線勻速階段，D-P3減速階段，方位角與末端位置y、z都采用相同的軌跡規劃方式；

步驟五：按照上述規劃方法對清污機器人實際運動軌跡規劃，P1、P2、P3點的位置和方位角的數據由實際示教所得，給定設置勻速速度v_c，最大加速度a_max，規劃末端運動軌跡，經逆運動學解算，得到各個液壓缸運動軌跡，分別規劃出抓斗的運動軌跡和速度，使得各關節位置、速度曲線連續無突變，路徑點間過渡平滑。

2.根據權利要求1所述的一種抓臂式清污機器人運動控制系統，其特征在于：所述數據庫管理模塊用于實現數據變量、程序文本的建立及保存；所述運動控制模塊用于實現點到點運動、關節插補、末端直線插補、軌跡點過渡插補，控制各軸位置、速度、加速度，末端姿態；所述操作模塊用于實現示教、再現、程序編譯操作，手動控制單軸點動，末端線性運動，自動運行程序；所述異常檢出模塊用于實現急停異常、伺服異常以及限位異常；所述數據采集模塊用于集成測距傳感器，實時檢測修正末端位置。

3.根據權利要求1所述的一種抓臂式清污機器人運動控制系統，其特征在于：所述數據存儲單元用于保存示教的位置點和軌跡程序和用戶建立的參數模板，提高程序重復調用性；所述軌跡規劃單元用于在關節空間和末端笛卡爾空間采用插補算法，進行加速度、速度、位置、姿態的插補，形成平滑的運動曲線；所述運動學單元用于建立各關節軸坐標系，測量DH參數，編寫機械臂正逆運動學；所述通訊單元用于通過工業以太網實現與示教器、液壓伺服控制器之間的數據傳遞；所述示教單元用于通過示教器手動控制機械臂單軸點動、末端線性運動，示教需要的運動點，并形成運動軌跡和程序；所述安全管理單元用于對運動過程中出現的異常情況進行實時反饋，并報錯處理，響應急停按鈕，并進行初始化；所述人機交互單元用于完成所有模塊的操作、顯示和交互功能。

4.根據權利要求1所述的一種抓臂式清污機器人運動控制系統，其特征在于：所述清污機器人用于清理附著纏繞在柵條上垃圾污物，清污機器人將抓斗爪齒伸入攔污柵柵條內，抓斗不與攔污柵刮碰，沿著柵條從上到下按設定的軌跡運行；清污機器人是一種平面三自由度串聯機器人，末端姿態是繞著垂直于平面的軸進行旋轉。