CN112323755A - 一种抓臂式清污机器人控制方法 - Google Patents
一种抓臂式清污机器人控制方法 Download PDFInfo
- Publication number
- CN112323755A CN112323755A CN202011246802.2A CN202011246802A CN112323755A CN 112323755 A CN112323755 A CN 112323755A CN 202011246802 A CN202011246802 A CN 202011246802A CN 112323755 A CN112323755 A CN 112323755A
- Authority
- CN
- China
- Prior art keywords
- hydraulic cylinder
- arm
- grab bucket
- small
- connecting rod
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000004140 cleaning Methods 0.000 title claims abstract description 63
- 239000010813 municipal solid waste Substances 0.000 title claims abstract description 59
- 238000000034 method Methods 0.000 title claims abstract description 41
- 239000010865 sewage Substances 0.000 claims abstract description 49
- 230000008569 process Effects 0.000 claims abstract description 13
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims abstract description 11
- 230000007704 transition Effects 0.000 claims description 12
- 230000004888 barrier function Effects 0.000 claims description 10
- 230000001133 acceleration Effects 0.000 claims description 9
- 238000004458 analytical method Methods 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000005202 decontamination Methods 0.000 claims description 8
- 230000003588 decontaminative effect Effects 0.000 claims description 8
- 238000005516 engineering process Methods 0.000 claims description 5
- 238000004088 simulation Methods 0.000 claims description 5
- 239000010727 cylinder oil Substances 0.000 claims description 4
- 230000000903 blocking effect Effects 0.000 claims description 2
- 230000000694 effects Effects 0.000 abstract description 10
- 230000009471 action Effects 0.000 abstract description 8
- 230000001174 ascending effect Effects 0.000 abstract description 4
- 238000002474 experimental method Methods 0.000 abstract description 3
- 230000007246 mechanism Effects 0.000 abstract description 3
- 230000004044 response Effects 0.000 abstract description 2
- 210000000245 forearm Anatomy 0.000 description 25
- 230000036544 posture Effects 0.000 description 6
- 238000012360 testing method Methods 0.000 description 5
- 210000000078 claw Anatomy 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 238000010248 power generation Methods 0.000 description 2
- TVEXGJYMHHTVKP-UHFFFAOYSA-N 6-oxabicyclo[3.2.1]oct-3-en-7-one Chemical compound C1C2C(=O)OC1C=CC2 TVEXGJYMHHTVKP-UHFFFAOYSA-N 0.000 description 1
- 229910000831 Steel Inorganic materials 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000003111 delayed effect Effects 0.000 description 1
- 230000010534 mechanism of action Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000000630 rising effect Effects 0.000 description 1
- 239000013049 sediment Substances 0.000 description 1
- 239000002689 soil Substances 0.000 description 1
- 239000010959 steel Substances 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
Images
Classifications
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02B—HYDRAULIC ENGINEERING
- E02B15/00—Cleaning or keeping clear the surface of open water; Apparatus therefor
- E02B15/04—Devices for cleaning or keeping clear the surface of open water from oil or like floating materials by separating or removing these materials
- E02B15/10—Devices for removing the material from the surface
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F3/00—Dredgers; Soil-shifting machines
- E02F3/04—Dredgers; Soil-shifting machines mechanically-driven
- E02F3/28—Dredgers; Soil-shifting machines mechanically-driven with digging tools mounted on a dipper- or bucket-arm, i.e. there is either one arm or a pair of arms, e.g. dippers, buckets
- E02F3/36—Component parts
- E02F3/40—Dippers; Buckets ; Grab devices, e.g. manufacturing processes for buckets, form, geometry or material of buckets
- E02F3/413—Dippers; Buckets ; Grab devices, e.g. manufacturing processes for buckets, form, geometry or material of buckets with grabbing device
- E02F3/4133—Dippers; Buckets ; Grab devices, e.g. manufacturing processes for buckets, form, geometry or material of buckets with grabbing device grabs carried out as loaders or mounted on a tractor
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Engineering & Computer Science (AREA)
- Mechanical Engineering (AREA)
- Mathematical Physics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Analysis (AREA)
- Data Mining & Analysis (AREA)
- Computational Mathematics (AREA)
- Structural Engineering (AREA)
- Civil Engineering (AREA)
- Mathematical Optimization (AREA)
- Theoretical Computer Science (AREA)
- Pure & Applied Mathematics (AREA)
- Operations Research (AREA)
- Algebra (AREA)
- Environmental & Geological Engineering (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- Mining & Mineral Resources (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种抓臂式清污机器人控制方法，针对水利设施中清污机清污效果较差的现状，设计清污机器人可实现拦污栅和河道的污物清理，采用几何法分析清污机器人三自由度串联结构，求解各关节液压缸与末端位的非线性关系；根据其清污过程路径重复的特点，设计位置控制器，在位置反馈控制的基础上，加入速度前馈控制，实验验证各个液压缸响应速度快，位置精度较高；清污机器人主要由轨道移动车、抓臂、液压伺服泵站及智能控制***构成；其中抓臂作为清污动作的主要执行机构，固定安装在轨道移动车上，通过液压缸牵引做旋转运动，实现抓斗在水平、垂直方向的运动，从而完成清污的上升、下降动作。
Description
技术领域
本发明涉及一种清污机器人，具体为一种抓臂式清污机器人控制方法，属于方法研究技术领域。
背景技术
水利设施中为了保证拦水闸门的正常启闭或水力发电设备的安全运转，需要对拦水闸门、拦水坝前及水力发电设备进水口处的杂物进行拦截和清理。现有的清污设备多为回转式、耙斗式和液压抓斗式清污机，其中回转式和耙斗式清污机清污效果差、清理能力弱。液压抓斗式清污机采用采用液压抓斗清理拦污栅前的污物，但由于抓斗是由钢丝绳牵引的，清污时完全依靠抓斗的自重下压抓取污物，清污效果仍然不理想。
在专利CN108301456A中公开了水底清污机器人及其使用方法，该机器人包括：设于底部用于承载和运输的行驶机构，行驶机构连接设置于机器人内部的控制***，控制***连接有无线接收装置，机器人内部为过滤腔，过滤腔上游连接设于机器人前底部的吸污口，过滤腔内设置过滤片，覆盖于过滤腔顶部对应于过滤片上方位置设有透明的污渣人工清理窗口，于过滤片下游连接设有自吸式水泵，自吸式水泵的出口连接设于机器人后上部的负压式出水口，于机器人后尾部设有连接控制***的视频监控镜头。但存在的不足：清污效果差、清理能力弱。
为了解决上述技术问题，本发明提供如下技术方案。
发明内容
本发明的目的在于提供一种抓臂式清污机器人控制方法，针对水利设施中清污机清污效果较差的现状，设计清污机器人可实现拦污栅和河道的污物清理，采用几何法分析清污机器人三自由度串联结构，求解各关节液压缸与末端位的非线性关系；根据其清污过程路径重复的特点，设计位置控制器，在位置反馈控制的基础上，加入速度前馈控制，实验验证各个液压缸响应速度快，位置精度较高；清污机器人主要由轨道移动车、移动轮、大臂液压缸、小臂液压缸、大臂连杆、抓斗液压缸、小臂连杆、抓斗、梳齿以及泥斗车构成；其中抓斗、梳齿作为清污动作的主要执行机构，活动安装在小臂连杆上，大臂连杆、小臂连杆、抓斗相邻两关节间铰接，通过大臂液压缸、小臂液压缸及抓斗液压缸牵引做旋转运动，实现抓斗在水平、垂直方向的运动，从而完成清污的上升、下降动作；根据清污机器人清污过程路径重复的特点，采用速度前馈+位置反馈控制机械臂运动，其中速度前馈控制预测***所需速度，位置反馈控制减小***误差；按照实际清污路径进行验证，速度前馈控制可较好的跟踪规划的轨迹，且位置反馈控制能有效的减小***误差，从而达到清污机器人清污过程运行平稳、可靠，清污效果良好的效果。
本发明所要解决的技术问题为：
(1)如何实现抓斗梳齿能够平稳运动，无碰撞的***拦污栅中进行污物清理；
(2)如何克服清污机器人清污过程路径重复带来的***误差问题；
(3)如何模拟计算得出大臂液压缸、小臂液压缸及抓斗液压缸的前瞻运动趋势；
(4)如何分析得出运动路径规划，又如何参考运动路径规划得到大臂液压缸、小臂液压缸及抓斗液压缸的参考位置问题。
本发明的目的可以通过以下技术方案实现：
一种抓臂式清污机器人控制方法，具体控制步骤如下：
步骤一：设定清污机器人的起始位置为P1，拦污栅上端点为P2，河面为P3；
步骤二：根据抓臂空间轨迹规划技术将P1到P3的运动路径分为5个阶段，P1-A加速阶段，A-B第一段直线匀速阶段，B-C直线段间轨迹过渡阶段，C-D第二段直线匀速阶段，D-P3减速阶段；
步骤三：根据运动学分析得出运动路径规划，参考运动路径规划得到大臂液压缸、小臂液压缸及抓斗液压缸的参考位置；
步骤五：将大臂液压缸、小臂液压缸及抓斗液压缸的参考位置变化通过差分计算，可得到参考运动速度；
步骤六：将参考运动速度与速度增益值(Kv)相乘，计算出速度前馈控制的输入；
步骤七：速度前馈控制进行模拟计算得出大臂液压缸、小臂液压缸及抓斗液压缸的前瞻运动趋势，并控制伺服比例阀来控制液压缸油压的压力；
步骤八：安装于大臂液压缸、小臂液压缸及抓斗液压缸的编码器进行大臂液压缸、小臂液压缸及抓斗液压缸的实时位置获取；
步骤九：将大臂液压缸、小臂液压缸及抓斗液压缸与位置增益值(Kp)相乘，计算得出位置反馈控制的输出，将位置反馈控制的输出与大臂液压缸、小臂液压缸及抓斗液压缸的参考位置进行比对，进而对大臂液压缸、小臂液压缸及抓斗液压缸的位置进行实时修订。
优选的，步骤一中的运动学分析，具体分析方法如下：
S1：建立坐标，其中AE为大臂连杆，BD为大臂液压缸，EQ为小臂连杆，FG为小臂液压缸，NR为抓斗液压缸，RS为梳齿，QRS为抓斗，Q距RS的距离为定值h，在机器人清污过程中，保持Q位置与拦污栅距离为定值h，同时调整RS姿态与拦污栅平行；
S2：根据清污机器人的参数尺寸，求解RS在运动空间的位置和姿态；
S3：根据平面几何关系求解Q位置，大臂关节角度θ1、小臂关节角度θ2与Q位置(y，z)的函数关系式如下：
y＝lAEcosθ1+lEQcos(θ1+θ2)
z＝lAEsinθ1+lEQsin(θ1+θ2)
式中，lAE为大臂连杆长度，lEQ为小臂连杆长度；
S4：求解RS的姿态，平面内的角度是直接相加的，因此大臂关节角度θ1、小臂关节角度θ2、抓斗关节角度θ3、梳齿角度θ4之和即为RS的方位角
式中，θ4是梳齿相对抓斗的角度，且θ4为定值；
S5：得到大臂关节角度、小臂关节角度、抓斗关节角度与梳齿角度的函数关系，由于各编码器测量的是对应液压缸的行程，由于各旋转关节角度和对应驱动液压缸行程之间是非线性关系，因此，需要求解两者的关系式；
根据余弦定理将大臂液压缸行程lBD转换为角度∠DAB：
式中，lAD、lAB为大臂液压缸两端点距原点A的距离，与lBD构成三角形；
S6：大臂关节角度θ1与角度∠DAB的关系：
θ1＝∠DAB+∠CAB-π/2-∠DAE
式中，∠CAB、∠DAE为大臂液压缸两端点的关节角；
S7：同理可得，小臂关节角度θ2与小臂液压缸行程lFG的关系：
θ2＝π-∠GEF-∠GEA-∠FEQ
式中，lEG、lEF为小臂液压缸两端点距原点E的距离，与lFG构成三角形；∠GEA、∠FEQ为小臂液压缸两端点的关节角；
小臂关节角度θ3与小臂液压缸行程的关系：
θ3＝∠NQE+∠NQR-π
式中，lQN、lQR为抓斗液压缸两端点距原点Q的距离，与lNR构成三角形，∠NQE为抓斗液压缸一端点的关节角。
优选的，S2中清污机器人，包括轨道移动车、移动轮、大臂液压缸、小臂液压缸、大臂连杆、抓斗液压缸、小臂连杆、抓斗、梳齿以及泥斗车；拦水坝上放置有轨道移动车，所述轨道移动车内部安装有移动轮，所述轨道移动车内部安装有大臂连杆，所述大臂连杆一端活动连接有小臂连杆，所述小臂连杆一端连接有抓斗，所述抓斗内部安装有梳齿，所述轨道移动车内部安装有大臂液压缸，所述大臂液压缸连接着轨道移动车与大臂连杆，所述大臂连杆表面安装有小臂液压缸，所述小臂液压缸连接着大臂连杆与小臂连杆，所述小臂连杆表面安装有抓斗液压缸，所述抓斗液压缸连接着小臂连杆与抓斗，所述轨道移动车下侧放置有泥斗车。
优选的，所述大臂液压缸、小臂液压缸以及抓斗液压缸内部均安装有编码器与伺服比例控制阀。
优选的，所述大臂液压缸、小臂液压缸以及抓斗液压缸均由液压伺服比例阀控制，通过对液压伺服比例阀的开合来实现大臂液压缸、小臂液压缸以及抓斗液压流量和流向的控制，进而控制大臂液压缸、小臂液压缸以及抓斗液压的伸缩长度和方向。
优选的，该清污机器人是一种平面三自由度串联机器人，其大臂连杆、小臂连杆主要用于位置的移动，抓斗用于姿态的调整。
优选的，清污机器人的具体清污步骤如下：
步骤一：开动清污机器人，通过轨道移动车内部的移动轮将清污机器人运送至清污位置，机械臂运动平面与拦污栅平面垂直；
步骤二：驱动大臂连杆相对于轨道移动车做旋转运动，小臂液压缸安装在大臂连杆上，驱动小臂连杆相对于大臂连杆做旋转运动，抓斗液压缸安装在小臂连杆上，驱动抓斗相对于小臂连杆做旋转运动，使得抓斗沿拦污栅向下运行；
步骤三：抓斗运行到底后，闭合抓取污物，驱动大臂液压缸、小臂液压缸以及抓斗液压缸，使得抓斗沿拦污栅向上运行，回到初始位置；
步骤四：驱动抓斗，使得抓斗内部的污物落入泥斗车。
优选的，为了将附着缠绕在栅条上的垃圾污物彻底清理干净，需要清污机的抓斗后爪齿***拦污栅栅条一定深度，同时必须保证爪齿不能与拦污栅刮碰，沿着栅条从上到下按设定的轨迹运行。根据清污机器人实际清污路径，构建轨迹规划模型，在清污过程中，清污机器人从起始P1位置向下运动，到达拦污栅上端点P2，然后沿着拦污栅向下运动，其末端抓斗爪齿始终与拦污栅重合，最终到达河面P3，抓取污物。运动路径由P1-P2和P2-P3的两条直线段组成，因此需要在操作空间对直线段上的点进行插值，规划清污机器人的运动轨迹。根据清污机器人运动要求，清污过程中运动轨迹平滑、连续、无暂停，因此将P1到P3的运动路径按照图所示划分为5个阶段。p1-A加速阶段，A-B第一段直线匀速阶段，B-C直线段间轨迹过渡阶段，C-D第二段直线匀速阶段，D-P3减速阶段。为加减速启停平滑，加速段和减速段采用正弦加减速的轨迹规划方法。为运动轨迹连续、无暂停，采用5次多项式进行两段直线轨迹的过渡，过渡起始点B位置，速度v1，过渡终止点C位置，速度v2。
这里P1、P2、P3点的位置和方位角的数据由实际示教所得。在触摸屏上给定设置匀速速度V1、V2，最大加速度a，控制***规划末端运动轨迹，经逆运动学解算，得到各个液压缸运动轨迹，分别规划出抓斗的运动轨迹和速度，使得各关节位置、速度曲线连续无突变，路径点间过渡平滑。
与现有技术相比，本发明的有益效果是：
1、根据运动学分析得出运动路径规划，参考运动路径规划得到大臂液压缸、小臂液压缸及抓斗液压缸的参考位置；将大臂液压缸、小臂液压缸及抓斗液压缸的参考位置变化通过差分计算，可得到参考运动速度；将参考运动速度作为速度前馈控制的输入，通过与速度增益值(Kv)相乘，计算出速度前馈控制的输出；速度前馈控制进行模拟计算得出大臂液压缸、小臂液压缸及抓斗液压缸的前瞻运动趋势，使得***可快速响应；安装于大臂液压缸、小臂液压缸及抓斗液压缸的编码器将所测的实际位置与参考位置进行对比，计算得出误差值；将误差值作为位置反馈的输入，通过与位置增益值(Kp)相乘，计算得出位置反馈控制的输出；便可消除***误差。
2、清污机器人主要由轨道移动车、移动轮、大臂液压缸、小臂液压缸、大臂连杆、抓斗液压缸、小臂连杆、抓斗、梳齿以及泥斗车构成；其中抓斗、梳齿作为清污动作的主要执行机构，活动安装在小臂连杆上，大臂连杆、小臂连杆、抓斗相邻两关节间铰接，通过大臂液压缸、小臂液压缸及抓斗液压缸牵引做旋转运动，实现抓斗在水平、垂直方向的运动，从而完成清污的上升、下降动作，进而实现抓斗梳齿能够平稳运动，无碰撞的***拦污栅中进行污物清理。
3、清污机器人的运动结构分为轨道移动车和抓臂，抓臂由大臂液压缸、小臂液压缸、大臂连杆、抓斗液压缸、小臂连杆、抓斗以及梳齿组成，轨道移动车由伺服电机驱动，用于将抓臂运送至清污位置，抓臂由大臂液压缸、小臂液压缸以及抓斗液压缸驱动，大臂液压缸安装在轨道移动车上，驱动大臂连杆相对于轨道移动车做旋转运动；小臂液压缸安装在大臂连杆表面，驱动小臂连杆相对于大臂连杆做旋转运动；抓斗液压缸安装在小臂连杆表面，驱动抓斗相对于小臂连杆做旋转运动；采用液压伺服比例阀分别对三个运动关节的液压缸进行控制，通过对液压伺服比例阀的开合来实现液压缸流量和流向的控制，即控制液压缸的伸缩长度和方向；同时在大臂液压缸、小臂液压缸和抓斗液压缸内部安装编码器，测量大臂液压缸、小臂液压缸和抓斗液压缸的实际行程。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明中清污机器人的结构示意图；
图2为大臂液压缸参考位置和实际位置测试图；
图3为小臂液压缸参考位置和实际位置测试图；
图4为抓斗液压缸参考位置和实际位置测试图；
图5为本发明中清污机器人结构简图；
图6为本发明中速度前馈控制框图；
图7为本发明中清污机器人运动轨迹。
图中：1、轨道移动车；2、移动轮；3、大臂液压缸；4、小臂液压缸；5、大臂连杆；6、抓斗液压缸；7、小臂连杆；8、抓斗；9、梳齿；10、泥斗车。
具体实施方式
下面将结合实施例对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
请参阅图1-7所示，一种抓臂式清污机器人控制方法，具体控制步骤如下：
步骤一：设定清污机器人的起始位置为P1，拦污栅上端点为P2，河面为P3；
步骤二：根据抓臂空间轨迹规划技术将P1到P3的运动路径分为5个阶段，P1-A加速阶段，A-B第一段直线匀速阶段，B-C直线段间轨迹过渡阶段，C-D第二段直线匀速阶段，D-P3减速阶段；
步骤三：根据运动学分析得出运动路径规划，参考运动路径规划得到大臂液压缸3、小臂液压缸4及抓斗液压缸6的参考位置；
步骤五：将大臂液压缸3、小臂液压缸4及抓斗液压缸6的参考位置变化通过差分计算，可得到参考运动速度；
步骤六：将参考运动速度与速度增益值Kv相乘，计算出速度前馈控制的输入；
步骤七：速度前馈控制进行模拟计算得出大臂液压缸3、小臂液压缸4及抓斗液压缸6的前瞻运动趋势，并控制伺服比例阀来控制液压缸油压的压力；
步骤八：安装于大臂液压缸3、小臂液压缸4及抓斗液压缸6的编码器进行大臂液压缸3、小臂液压缸4及抓斗液压缸6的实时位置获取；
步骤九：将大臂液压缸3、小臂液压缸4及抓斗液压缸6与位置增益值Kp相乘，计算得出位置反馈控制的输出，将位置反馈控制的输出与大臂液压缸3、小臂液压缸4及抓斗液压缸6的参考位置进行比对，进而对大臂液压缸3、小臂液压缸4及抓斗液压缸6的位置进行实时修订。
其中，步骤一中的运动学分析，具体分析方法如下：
S1：建立坐标，其中AE为大臂连杆，BD为大臂液压缸，EQ为小臂连杆，FG为小臂液压缸，NR为抓斗液压缸，RS为梳齿，QRS为抓斗，Q距RS的距离为定值h，在机器人清污过程中，保持Q位置与拦污栅距离为定值h，同时调整RS姿态与拦污栅平行；
S2：根据清污机器人的参数尺寸，求解RS在运动空间的位置和姿态；
S3：根据平面几何关系求解Q位置，大臂关节角度θ1、小臂关节角度θ2与Q位置y，z的函数关系式如下：
y＝lAEcosθ1+lEQcos(θ1+θ2)
z＝lAEsinθ1+lEQsin(θ1+θ2)
式中，lAE为大臂连杆长度，lEQ为小臂连杆长度；
S4：求解RS的姿态，平面内的角度是直接相加的，因此大臂关节角度θ1、小臂关节角度θ2、抓斗关节角度θ3、梳齿角度θ4之和即为RS的方位角
式中，θ4是梳齿相对抓斗的角度，且θ4为定值；
S5：得到大臂关节角度、小臂关节角度、抓斗关节角度与梳齿角度的函数关系，由于各编码器测量的是对应液压缸的行程，由于各旋转关节角度和对应驱动液压缸行程之间是非线性关系，因此，需要求解两者的关系式；
根据余弦定理将大臂液压缸行程lBD转换为角度∠DAB：
式中，lAD、lAB为大臂液压缸两端点距原点A的距离，与lBD构成三角形；
S6：大臂关节角度θ1与角度∠DAB的关系：
θ1＝∠DAB+∠CAB-π/2-∠DAE
式中，∠CAB、∠DAE为大臂液压缸两端点的关节角；
S7：同理可得，小臂关节角度θ2与小臂液压缸行程lFG的关系：
θ2＝π-∠GEF-∠GEA-∠FEQ
式中，lEG、lEF为小臂液压缸两端点距原点E的距离，与lFG构成三角形；∠GEA、∠FEQ为小臂液压缸两端点的关节角；
小臂关节角度θ3与小臂液压缸行程的关系：
θ3＝∠NQE+∠NQR-π
式中，lQN、lQR为抓斗液压缸两端点距原点Q的距离，与lNR构成三角形，∠NQE为抓斗液压缸一端点的关节角。
其中，S2中清污机器人，包括轨道移动车1、移动轮2、大臂液压缸3、小臂液压缸4、大臂连杆5、抓斗液压缸6、小臂连杆7、抓斗8、梳齿9以及泥斗车10；拦水坝上放置有轨道移动车1，所述轨道移动车1内部安装有移动轮2，所述轨道移动车1内部安装有大臂连杆5，所述大臂连杆5一端活动连接有小臂连杆7，所述小臂连杆7一端连接有抓斗8，所述抓斗8内部安装有梳齿9，所述轨道移动车1内部安装有大臂液压缸3，所述大臂液压缸3连接着轨道移动车1与大臂连杆5，所述大臂连杆5表面安装有小臂液压缸4，所述小臂液压缸4连接着大臂连杆5与小臂连杆7，所述小臂连杆7表面安装有抓斗液压缸6，所述抓斗液压缸6连接着小臂连杆7与抓斗8，所述轨道移动车1下侧放置有泥斗车10。
其中，所述大臂液压缸3、小臂液压缸4以及抓斗液压缸6内部均安装有编码器与伺服比例控制阀。
其中，所述大臂液压缸3、小臂液压缸4以及抓斗液压缸6均由液压伺服比例阀控制，通过对液压伺服比例阀的开合来实现大臂液压缸3、小臂液压缸4以及抓斗8液压流量和流向的控制，进而控制大臂液压缸3、小臂液压缸4以及抓斗8液压的伸缩长度和方向。
其中，该清污机器人是一种平面三自由度串联机器人，其大臂连杆5、小臂连杆7主要用于位置的移动，抓斗8用于姿态的调整。
其中，清污机器人的具体清污步骤如下：
步骤一：开动清污机器人，通过轨道移动车1内部的移动轮2将清污机器人运送至清污位置，机械臂运动平面与拦污栅平面垂直；
步骤二：驱动大臂连杆5相对于轨道移动车1做旋转运动，小臂液压缸4安装在大臂连杆5上，驱动小臂连杆7相对于大臂连杆5做旋转运动，抓斗液压缸6安装在小臂连杆7上，驱动抓斗8相对于小臂连杆7做旋转运动，使得抓斗8沿拦污栅向下运行；
步骤三：抓斗8运行到底后，闭合抓取污物，驱动大臂液压缸3、小臂液压缸4以及抓斗液压缸6，使得抓斗8沿拦污栅向上运行，回到初始位置；
步骤四：驱动抓斗8，使得抓斗8内部的污物落入泥斗车10。
上述公式均是去量化取其数值计算，公式是由采集大量数据进行软件模拟得到最近真实情况的一个公式，公式中的预设参数由本领域的技术人员根据实际情况设定。
本发明的工作原理：设定清污机器人的起始位置为P1，拦污栅上端点为P2，河面为P3；根据抓臂空间轨迹规划技术将P1到P3的运动路径分为5个阶段，P1-A加速阶段，A-B第一段直线匀速阶段，B-C直线段间轨迹过渡阶段，C-D第二段直线匀速阶段，D-P3减速阶段；根据运动学分析得出运动路径规划，参考运动路径规划得到大臂液压缸3、小臂液压缸4及抓斗液压缸6的参考位置；将大臂液压缸3、小臂液压缸4及抓斗液压缸6的参考位置变化通过差分计算，可得到参考运动速度；将参考运动速度与速度增益值Kv相乘，计算出速度前馈控制的输入；速度前馈控制进行模拟计算得出大臂液压缸3、小臂液压缸4及抓斗液压缸6的前瞻运动趋势，并控制伺服比例阀来控制液压缸油压的压力；安装于大臂液压缸3、小臂液压缸4及抓斗液压缸6的编码器进行大臂液压缸3、小臂液压缸4及抓斗液压缸6的实时位置获取；将大臂液压缸3、小臂液压缸4及抓斗液压缸6与位置增益值Kp相乘，计算得出位置反馈控制的输出，将位置反馈控制的输出与大臂液压缸3、小臂液压缸4及抓斗液压缸6的参考位置进行比对，进而对大臂液压缸3、小臂液压缸4及抓斗液压缸6的位置进行实时修订。
清污机器人主要由轨道轨道移动车1、移动轮2、大臂液压缸3、小臂液压缸4、大臂连杆5、抓斗液压缸6、小臂连杆7、抓斗8、梳齿9以及泥斗车10构成；其中抓斗8、梳齿9作为清污动作的主要执行机构，活动安装在小臂连杆7上，大臂连杆5、小臂连杆7、抓斗8相邻两关节间铰接，通过大臂液压缸3、小臂液压缸4及抓斗液压缸6牵引做旋转运动，实现抓斗8在水平、垂直方向的运动，从而完成清污的上升、下降动作，进而实现抓斗8梳齿9能够平稳运动，无碰撞的***拦污栅中进行污物清理。
试验测试时，按照实际的清污路径，经运动学解算，给出各个液压缸的参考位置输入，对其进行测试；清污机器人按照工作流程，抓斗8分为上升和下降两种运动状态，分别大臂液压缸3、小臂液压缸4以及抓斗液压缸6的伸长和缩回动作，所以分别对大臂液压缸3、小臂液压缸4以及抓斗液压缸6测试其速度前馈控制的效果；所设置的位置反馈和速度前馈增益值如表1所示。
表1三个液压缸增益值
| KP-u | KP-d | KV-u | KV-d | |
| 大臂液压缸 | 0.0098 | 0.0122 | 0.0039 | 0.0044 |
| 小臂液压缸 | 0.0098 | 0.0098 | 0.0029 | 0.0044 |
| 抓斗液压缸 | 0.0024 | 0.0019 | 0.0008 | 0.0012 |
其中KP-u表示抓斗8上升运动时各液压缸的位置反馈增益，KP-d表示抓斗8下降运动时各液压缸的位置反馈增益，KV-u表示抓斗8上升运动时各液压缸的速度前馈增益，KV-d表示抓斗8下降运动时各液压缸的速度前馈增益。
实验结果
位置输入为清污过程规划的参考位置时，对清污机器人大臂液压缸3、小臂液压缸4以及抓斗8液压缸的速度前馈控制进行测试，结果分别如图2、3、4所示。
从图中可以看出，大臂液压缸3、小臂液压缸4以及抓斗8液压缸的实际位置曲线和参考位置曲线基本重合，大臂液压缸3、小臂液压缸4以及抓斗8液压缸的实际运动轨迹能跟踪上规划的运动轨迹，且起始状态无延迟，运动过程位置无较大误差，表明速度前馈控制效果较好。清污机器人清污过程中，梳齿能够***拦污栅中，并平稳运动，无碰撞，清污效果较好。
在本说明书的描述中，参考术语“一个实施例”、“示例”、“具体示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制。
Claims (6)
1.一种抓臂式清污机器人控制方法，其特征在于，具体控制步骤如下：
步骤一：设定清污机器人的起始位置为P1，拦污栅上端点为P2，河面为P3；
步骤二：根据抓臂空间轨迹规划技术将P1到P3的运动路径分为5个阶段，P1-A加速阶段，A-B第一段直线匀速阶段，B-C直线段间轨迹过渡阶段，C-D第二段直线匀速阶段，D-P3减速阶段；
步骤三：根据运动学分析得出运动路径规划，参考运动路径规划得到大臂液压缸(3)、小臂液压缸(4)及抓斗液压缸(6)的参考位置；
步骤五：将大臂液压缸(3)、小臂液压缸(4)及抓斗液压缸(6)的参考位置变化通过差分计算，可得到参考运动速度；
步骤六：将参考运动速度与速度增益值(Kv)相乘，计算出速度前馈控制的输入；
步骤七：速度前馈控制进行模拟计算得出大臂液压缸(3)、小臂液压缸(4)及抓斗液压缸(6)的前瞻运动趋势，并控制伺服比例阀来控制液压缸油压的压力；
步骤八：安装于大臂液压缸(3)、小臂液压缸(4)及抓斗液压缸(6)的编码器进行大臂液压缸(3)、小臂液压缸(4)及抓斗液压缸(6)的实时位置获取；
步骤九：将大臂液压缸(3)、小臂液压缸(4)及抓斗液压缸(6)实时获取的位置与位置增益值(Kp)相乘，计算得出位置反馈控制的输出，将位置反馈控制的输出与大臂液压缸(3)、小臂液压缸(4)及抓斗液压缸(6)的参考位置进行比对，进而对大臂液压缸(3)、小臂液压缸(4)及抓斗液压缸(6)的位置进行实时修订。
2.根据权利要求1所述的一种抓臂式清污机器人控制方法，其特征在于：所述抓臂空间轨迹规划技术用于根据清污机器人实际清污路径，构建轨迹规划模型，在清污过程中，清污机器人从起始P1位置向下运动，到达拦污栅上端点P2，然后沿着拦污栅向下运动，其末端抓斗(8)始终与拦污栅重合，最终到达河面P3，抓取污物；采用5次多项式进行两段直线轨迹的过渡，过渡起始点B位置，速度V1，过渡终止点C位置，速度V2。
3.根据权利要求1所述的一种抓臂式清污机器人控制方法，其特征在于：S2中清污机器人，包括轨道移动车(1)、移动轮(2)、大臂液压缸(3)、小臂液压缸(4)、大臂连杆(5)、抓斗液压缸(6)、小臂连杆(7)、抓斗(8)、梳齿(9)以及泥斗车(10)；拦水坝上放置有轨道移动车(1)，所述轨道移动车(1)内部安装有移动轮(2)，所述轨道移动车(1)内部安装有大臂连杆(5)，所述大臂连杆(5)一端活动连接有小臂连杆(7)，所述小臂连杆(7)一端连接有抓斗(8)，所述抓斗(8)内部安装有梳齿(9)，所述轨道移动车(1)内部安装有大臂液压缸(3)，所述大臂液压缸(3)连接着轨道移动车(1)与大臂连杆(5)，所述大臂连杆(5)表面安装有小臂液压缸(4)，所述小臂液压缸(4)连接着大臂连杆(5)与小臂连杆(7)，所述小臂连杆(7)表面安装有抓斗液压缸(6)，所述抓斗液压缸(6)连接着小臂连杆(7)与抓斗(8)，所述轨道移动车(1)下侧放置有泥斗车(10)。
4.根据权利要求3所述的一种抓臂式清污机器人控制方法，其特征在于：所述大臂液压缸(3)、小臂液压缸(4)以及抓斗液压缸(6)内部均安装有行程编码器。
5.根据权利要求3所述的一种抓臂式清污机器人控制方法，其特征在于：所述大臂液压缸(3)、小臂液压缸(4)以及抓斗液压缸(6)均由液压伺服比例阀控制，通过对液压伺服比例阀的开合来实现大臂液压缸(3)、小臂液压缸(4)以及抓斗(8)液压流量和流向的控制，进而控制大臂液压缸(3)、小臂液压缸(4)以及抓斗(8)液压的伸缩长度和方向。
6.根据权利要求3所述的一种抓臂式清污机器人控制方法，其特征在于：清污机器人的具体清污步骤如下：
步骤一：开动清污机器人，通过轨道移动车(1)内部的移动轮(2)将清污机器人运送至清污位置，机械臂运动平面与拦污栅平面垂直；
步骤二：驱动大臂连杆(5)相对于轨道移动车(1)做旋转运动，小臂液压缸(4)安装在大臂连杆(5)上，驱动小臂连杆(7)相对于大臂连杆(5)做旋转运动，抓斗液压缸(6)安装在小臂连杆(7)上，驱动抓斗(8)相对于小臂连杆(7)做旋转运动，使得抓斗(8)沿拦污栅向下运行；
步骤三：抓斗(8)运行到底后，闭合抓取污物，驱动大臂液压缸(3)、小臂液压缸(4)以及抓斗液压缸(6)，使得抓斗(8)沿拦污栅向上运行，回到初始位置；
步骤四：驱动抓斗(8)，使得抓斗(8)内部的污物落入泥斗车(10)。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011246802.2A CN112323755B (zh) | 2020-11-10 | 2020-11-10 | 一种抓臂式清污机器人控制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011246802.2A CN112323755B (zh) | 2020-11-10 | 2020-11-10 | 一种抓臂式清污机器人控制方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112323755A true CN112323755A (zh) | 2021-02-05 |
| CN112323755B CN112323755B (zh) | 2022-11-08 |
Family
ID=74318128
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011246802.2A Active CN112323755B (zh) | 2020-11-10 | 2020-11-10 | 一种抓臂式清污机器人控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112323755B (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113026700A (zh) * | 2021-03-24 | 2021-06-25 | 中国长江电力股份有限公司 | 清污机器人漂浮物清污方法 |
| CN114518731A (zh) * | 2022-01-27 | 2022-05-20 | 哈尔滨工程大学 | 可远程应用的大场景工业现场无标记增强现实辅助制造和运维*** |
| CN115331166A (zh) * | 2022-08-12 | 2022-11-11 | 广州市净水有限公司 | 一种格栅智能监控预警方法和*** |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105500386A (zh) * | 2016-01-12 | 2016-04-20 | 上海海事大学 | 一种遥控水下机器人的抓斗装置 |
| CN105922265A (zh) * | 2016-06-20 | 2016-09-07 | 广州视源电子科技股份有限公司 | 一种机械臂的运动轨迹规划方法、装置及机器人 |
| CN106592661A (zh) * | 2017-01-23 | 2017-04-26 | 安徽省六安恒源机械有限公司 | 清污机 |
| CN108919637A (zh) * | 2018-06-13 | 2018-11-30 | 武汉市政工程设计研究院有限责任公司 | 一种抓斗式清污机的自动控制方法及*** |
| CN109623820A (zh) * | 2018-12-25 | 2019-04-16 | 哈工大机器人（合肥）国际创新研究院 | 一种机器人空间轨迹过渡方法 |
-
2020
- 2020-11-10 CN CN202011246802.2A patent/CN112323755B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105500386A (zh) * | 2016-01-12 | 2016-04-20 | 上海海事大学 | 一种遥控水下机器人的抓斗装置 |
| CN105922265A (zh) * | 2016-06-20 | 2016-09-07 | 广州视源电子科技股份有限公司 | 一种机械臂的运动轨迹规划方法、装置及机器人 |
| CN106592661A (zh) * | 2017-01-23 | 2017-04-26 | 安徽省六安恒源机械有限公司 | 清污机 |
| CN108919637A (zh) * | 2018-06-13 | 2018-11-30 | 武汉市政工程设计研究院有限责任公司 | 一种抓斗式清污机的自动控制方法及*** |
| CN109623820A (zh) * | 2018-12-25 | 2019-04-16 | 哈工大机器人（合肥）国际创新研究院 | 一种机器人空间轨迹过渡方法 |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113026700A (zh) * | 2021-03-24 | 2021-06-25 | 中国长江电力股份有限公司 | 清污机器人漂浮物清污方法 |
| CN113026700B (zh) * | 2021-03-24 | 2022-03-15 | 中国长江电力股份有限公司 | 清污机器人漂浮物清污方法 |
| CN114518731A (zh) * | 2022-01-27 | 2022-05-20 | 哈尔滨工程大学 | 可远程应用的大场景工业现场无标记增强现实辅助制造和运维*** |
| CN114518731B (zh) * | 2022-01-27 | 2023-03-31 | 哈尔滨工程大学 | 可远程应用的大场景工业现场无标记增强现实辅助制造和运维*** |
| CN115331166A (zh) * | 2022-08-12 | 2022-11-11 | 广州市净水有限公司 | 一种格栅智能监控预警方法和*** |
| CN115331166B (zh) * | 2022-08-12 | 2023-10-27 | 广州市净水有限公司 | 一种格栅清污机智能监控预警方法和*** |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112323755B (zh) | 2022-11-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112323755B (zh) | 一种抓臂式清污机器人控制方法 | |
| CN101716568B (zh) | 机器人喷涂***喷涂室内的方法 | |
| CN106475374B (zh) | 一种自动管型材内腔冲洗装置及控制*** | |
| CN106419768A (zh) | 墙面清洗机 | |
| CN103176409B (zh) | 一种快速准确实现混凝土泵车臂架运动轨迹的方法 | |
| CN109396086B (zh) | 一种全自动多自由度路灯清洗装置及其控制方法 | |
| CN105013733B (zh) | 变电站带电水冲洗辅冲机器人及方法 | |
| CN209368926U (zh) | 河道清淤装置 | |
| CN106592661B (zh) | 清污机 | |
| CN109845491B (zh) | 利用双臂苹果采摘圆形分级收集机器人的采摘分级方法 | |
| CN109396088A (zh) | Pc构件模台清扫装置及其工作方法 | |
| CN110455290B (zh) | 一种智能液压挖掘机的最优轨迹规划方法 | |
| CN204171001U (zh) | 基于激光测距的变电站水冲洗机器人 | |
| CN106553196A (zh) | 能自动卸垃圾的环卫机器人及其控制方法 | |
| CN112317478A (zh) | 一种车载式集装箱自动清扫车 | |
| CN102183963B (zh) | 一种高空作业平台的工作轨迹控制方法 | |
| DE212021000094U1 (de) | Produktionslinie von Doppelblock-Schienenschwellen für Hochgeschwindigkeitsbahnen | |
| CN106193268A (zh) | 水陆两栖多单元轮式管道机器人检修*** | |
| CN112959976A (zh) | 列车煤渣清扫设备及其清扫方法 | |
| CN106926233A (zh) | 一种平面机械手运动路径的规划方法 | |
| CN109249903B (zh) | 一种高速列车的自动清洗装置 | |
| DE4415434C2 (de) | Automatische Reinigungsvorrichtung für Fenster- und/oder Fassadenflächen | |
| CN206229786U (zh) | 一种自动管型材内腔冲洗装置及控制*** | |
| CN204178179U (zh) | 变电站带电水冲洗机器人控制*** | |
| CN207344570U (zh) | 一种含直线驱动器的三自由度吸扫机器人机构 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |