[發(fā)明專(zhuān)利]一種抓臂式清污機(jī)器人運(yùn)動(dòng)控制系統(tǒng)

在審

• 申請(qǐng)?zhí)枺?/td>: 202011246801.8
• 申請(qǐng)日: 2020-11-10
• 公開(kāi)（公告）號(hào): CN112276911A
• 公開(kāi)（公告）日: 2021-01-29
• 發(fā)明（設(shè)計(jì)）人: 黃振東;高云峰;陳榮娜;王亞;王之輝;袁家寶
• 申請(qǐng)（專(zhuān)利權(quán)）人: 安徽省六安恒源機(jī)械有限公司
• 主分類(lèi)號(hào): B25J9/00
• 分類(lèi)號(hào): B25J9/00;B25J9/16;B25J13/00
• 代理公司: 合肥正則元起專(zhuān)利代理事務(wù)所(普通合伙) 34160
• 代理人: 劉培越

本發(fā)明公開(kāi)了一種抓臂式清污機(jī)器人運(yùn)動(dòng)控制系統(tǒng)，為了將附著纏繞在柵條上垃圾污物徹底清理干凈，需要清污機(jī)的抓斗爪齒伸入攔污柵柵條內(nèi)，同時(shí)也必須保證抓斗不能與攔污柵刮碰，沿著柵條從上到下按設(shè)定的軌跡運(yùn)行。要求清污機(jī)在工作工程中，抓斗運(yùn)行軌跡要平滑連續(xù)，控制好各個(gè)關(guān)節(jié)節(jié)點(diǎn)的速度、加速度及脈動(dòng)的連續(xù)變化以及其執(zhí)行效率。清污機(jī)器人是一種平面三自由度串聯(lián)機(jī)器人，末端姿態(tài)是繞著垂直于平面的軸進(jìn)行旋轉(zhuǎn)，其末端執(zhí)行器的軌跡規(guī)劃方法為根據(jù)清污機(jī)器人實(shí)際清污路徑，構(gòu)建軌跡規(guī)劃模型，將全程路徑分為5個(gè)階段，分別采用正弦加減速、五次多項(xiàng)式等插補(bǔ)方式進(jìn)行路徑插補(bǔ)，經(jīng)軌跡規(guī)劃后各個(gè)液壓缸軌跡平滑、連續(xù)。

技術(shù)領(lǐng)域

本發(fā)明涉及一種運(yùn)動(dòng)控制系統(tǒng)，具體為一種抓臂式清污機(jī)器人運(yùn)動(dòng)控制系統(tǒng)，屬于清污機(jī)器人運(yùn)動(dòng)控制技術(shù)領(lǐng)域。

背景技術(shù)

清污機(jī)器人是水利設(shè)施中對(duì)攔水閘門(mén)、攔水壩前及水力發(fā)電設(shè)備進(jìn)水口處的雜物進(jìn)行攔截和清理的設(shè)備。清污過(guò)程中依靠抓臂強(qiáng)行下壓，可同時(shí)實(shí)現(xiàn)攔污柵上污物的清理和攔污柵前污物的抓取，相較于其他清污設(shè)備：回轉(zhuǎn)式清污機(jī)、耙斗式清污機(jī)等，抓臂式清污機(jī)器人能有效的提高清污效果和清污效率，特別是對(duì)于集中擁堵在取排水口攔污柵前的高強(qiáng)度上游污物，也能及時(shí)有效的清理干凈。

為了將附著纏繞在柵條上垃圾污物徹底清理干凈，需要清污機(jī)的抓斗爪齒伸入攔污柵柵槽內(nèi)，同時(shí)也必須保證抓斗不能與攔污柵刮碰，沿著柵條從上到下按設(shè)定的軌跡運(yùn)行。要求清污機(jī)在工作工程中，抓斗運(yùn)行軌跡要平滑連續(xù)，控制好各個(gè)關(guān)節(jié)節(jié)點(diǎn)的速度、加速度及脈動(dòng)的連續(xù)變化以及其執(zhí)行效率。但是目前沒(méi)有對(duì)該類(lèi)清污機(jī)器人控制系統(tǒng)的研究，且市面上沒(méi)有能適用的成熟產(chǎn)品，因此，急需一種抓臂式清污機(jī)器人運(yùn)動(dòng)控制系統(tǒng)，分別規(guī)劃出各個(gè)液壓缸的運(yùn)動(dòng)軌跡和速度，使得各關(guān)節(jié)位置、速度曲線連續(xù)無(wú)突變，路徑點(diǎn)間過(guò)渡平滑。

為了解決上述技術(shù)問(wèn)題，本發(fā)明提供如下技術(shù)方案。

發(fā)明內(nèi)容

本發(fā)明的目的在于提供一種抓臂式清污機(jī)器人運(yùn)動(dòng)控制系統(tǒng)，本發(fā)明分別規(guī)劃出各個(gè)液壓缸的運(yùn)動(dòng)軌跡和速度，使得各關(guān)節(jié)位置、速度曲線連續(xù)無(wú)突變，路徑點(diǎn)間過(guò)渡平滑。根據(jù)清污機(jī)器人運(yùn)動(dòng)要求，清污過(guò)程中運(yùn)動(dòng)軌跡平滑、連續(xù)、無(wú)暫停，因此將P1到P3的運(yùn)動(dòng)路徑劃分為5個(gè)階段。p1-A加速階段，A-B 第一段直線勻速階段，B-C直線段間軌跡過(guò)渡階段，C-D第二段直線勻速階段， D-P3減速階段。由于故方位角φ與末端位置y、z都采用相同的軌跡規(guī)劃方式。這里為了加減速啟停平滑，加速段和減速段采用正弦加減速的軌跡規(guī)劃方法。為了運(yùn)動(dòng)軌跡連續(xù)、無(wú)暫停，采用5次多項(xiàng)式進(jìn)行兩段直線軌跡的過(guò)渡，過(guò)渡起始點(diǎn)B位置P1，速度V1，加速度0，過(guò)渡終止點(diǎn)C位置P2，速度V2，加速度0。

本發(fā)明的目的可以通過(guò)以下技術(shù)方案實(shí)現(xiàn)：

清污機(jī)器人運(yùn)動(dòng)控制系統(tǒng)硬件主要包括嵌入式控制器、伺服比例閥、液壓泵、接近傳感器和動(dòng)態(tài)傾角儀等。主控設(shè)備是用于工業(yè)控制任務(wù)的嵌入式控制器，可提供周期2ms的實(shí)時(shí)運(yùn)行環(huán)境，實(shí)現(xiàn)運(yùn)動(dòng)學(xué)、軌跡模型、在線修正模型的建立，并通過(guò)其I/O模塊，實(shí)現(xiàn)傳感器信息的采集和伺服比例閥的控制，從而控制清污機(jī)器人按照規(guī)劃的軌跡運(yùn)動(dòng)并對(duì)末端軌跡進(jìn)行實(shí)時(shí)在線修正。

一種抓臂式清污機(jī)器人運(yùn)動(dòng)控制系統(tǒng)，包括數(shù)據(jù)庫(kù)管理模塊、運(yùn)動(dòng)控制模塊、操作模塊、異常檢出模塊、數(shù)據(jù)采集模塊；所述運(yùn)動(dòng)控制模塊包括數(shù)據(jù)存儲(chǔ)單元、軌跡規(guī)劃單元、運(yùn)動(dòng)學(xué)單元、通訊單元、示教單元、安全管理單元以及人機(jī)交互單元；

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

步驟一：根據(jù)清污機(jī)器人實(shí)際清污路徑，設(shè)定清污機(jī)器人的起始位置為P1，攔污柵上端點(diǎn)為P2，河面為P3；

步驟二：在清污過(guò)程中，清污機(jī)器人從起始位置P1向下運(yùn)動(dòng)，到達(dá)攔污柵上端點(diǎn)P2，然后沿著攔污柵向下運(yùn)動(dòng)，其末端抓斗爪齒與攔污柵重合，到達(dá)河面P3，抓取污物；