[發明專利]一種抓臂式清污機器人軌跡在線修正控制方法

有效

• 申請號: 202011246779.7
• 申請日: 2020-11-10
• 公開（公告）號: CN112405535B
• 公開（公告）日: 2022-01-18
• 發明（設計）人: 陳榮娜;黃振東;張斌;李偉;權良紅;杜邦余
• 申請（專利權）人: 安徽省六安恒源機械有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 合肥正則元起專利代理事務所(普通合伙) 34160
• 代理人: 劉培越

本發明公開了一種抓臂式清污機器人軌跡在線修正控制方法，用軌跡修正的末端信號反饋的方法與實際末端位置構成閉環，通過在線修正算法，實際運行驗證表明能夠較好的修正末端軌跡誤差，滿足抓斗沿攔污柵運動的精度要求；在抓斗的上方安裝動態傾角儀，在抓斗后爪齒橫梁上攔污柵法向方向，安裝兩組接近傳感器，動態傾角儀時刻保證各個位置點抓斗姿態，使得后爪齒與攔污柵柵條維持一個特定的夾角。接近傳感器保證抓斗后爪齒上部橫梁與柵條的距離，兩者配合用于在線修正末端軌跡，保證后爪齒齒尖插入柵條一定深度且不碰撞攔污柵。

技術領域

本發明涉及一種軌跡在線修正控制方法，具體為一種抓臂式清污機器人軌跡在線修正控制方法，屬于機器人軌跡在線修正技術領域。

背景技術

通常一個泵站攔污閘會安裝多孔攔污柵，安裝時只是將攔污柵插入攔污柵閘槽內即可。攔污柵閘槽是土建結構，尺寸精度要求不是很高，再加上攔污柵自身制造和安裝時的誤差，每個攔污柵的實際擺放位置各不相同。進行軌跡規劃時，清污路徑是示教其中一孔攔污柵的柵條前沿得來，因此其他孔攔污柵的柵條與規劃的軌跡差別很大，清污機器人在清污過程中，實際清污軌跡并不能按照設定軌跡的沿著攔污柵向下運動，離太遠容易脫離攔污柵影響清污效果，太靠近攔污柵產生碰撞，因此需要在抓斗上安裝傳感器，對抓斗的實際運行軌跡進行在線修正。

為了解決上述技術問題，本發明提供如下技術方案。

發明內容

本發明的目的在于提供一種抓臂式清污機器人軌跡在線修正控制方法。

本發明的目的可以通過以下技術方案實現：

一種抓臂式清污機器人軌跡在線修正控制方法，包括以下步驟：

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

步驟二：根據抓臂空間軌跡規劃技術將P1到P3的運動路徑分為5個階段，P1-A加速階段，A-B第一段直線勻速階段，B-C直線段間軌跡過渡階段，C-D第二段直線勻速階段，D-P3減速階段；

步驟三：驅動大臂連桿相對于軌道移動車做旋轉運動，小臂液壓缸安裝在大臂連桿上，驅動小臂連桿相對于大臂連桿做旋轉運動，抓斗液壓缸安裝在小臂連桿上，驅動抓斗相對于小臂連桿做旋轉運動，使得抓斗沿攔污柵向下運行；

步驟四：分別獲取一號傳感器、二號傳感器、三號傳感器和四號傳感器的傳感器信號，并判斷出抓斗的移動方向；

若一號傳感器和三號傳感器傳感無信號，二號傳感器和四號傳感器有信號，則判斷末端抓斗處于誤差范圍內，速度v為0，末端軌跡不做調整；

若一號傳感器和三號傳感器有信號，二號傳感器和四號傳感器有信號，末端抓斗靠近攔污柵，速度v加速并保持正向勻速，將末端軌跡向外調整；

若一號傳感器、三號傳感器和二號傳感器、四號傳感器均無信號，末端抓斗遠離攔污柵，速度v減速并保持反向勻速，將末端軌跡向內調整；

若二號傳感器檢測到信號，且一號傳感器、三號傳感器、四號傳感器未檢測到信號，說明爪齒偏向左側，軌道移動車向右運行，直到四號傳感器檢測到信號為止，調整爪齒到達中間位置；

若一號傳感器、三號傳感器和二號傳感器未檢測到信號，四號傳感器檢測到信號，說明爪齒偏向右側，軌道移動車向左運行，直到二號傳感器檢測到信號為止，調整爪齒到達中間位置。

優選的，修正過程從P2點開始，到P3點結束，完成末端抓斗沿攔污柵清污過程的軌跡在線修正。

優選的，所述抓臂空間軌跡規劃技術具體為根據清污機器人實際清污路徑，構建軌跡規劃模型，將全程路徑分為5個階段，分別采用正弦加減速、五次多項式等插補方式進行路徑插補，經軌跡規劃后各個液壓缸軌跡平滑、連續。