CN114193449B - 一种锚杆支护机器人工作臂轨迹规划方法 - Google Patents

一种锚杆支护机器人工作臂轨迹规划方法 Download PDF

Info

Publication number
CN114193449B
CN114193449B CN202111494905.5A CN202111494905A CN114193449B CN 114193449 B CN114193449 B CN 114193449B CN 202111494905 A CN202111494905 A CN 202111494905A CN 114193449 B CN114193449 B CN 114193449B
Authority
CN
China
Prior art keywords
working arm
robot
anchor bolt
anchor
spline
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.)
Active
Application number
CN202111494905.5A
Other languages
English (en)
Other versions
CN114193449A (zh
Inventor
邓镓敏
高志强
郭治富
阎志伟
金江
刘伟立
赵永红
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Taiyuan Institute of China Coal Technology and Engineering Group
Shanxi Tiandi Coal Mining Machinery Co Ltd
Original Assignee
Taiyuan Institute of China Coal Technology and Engineering Group
Shanxi Tiandi Coal Mining Machinery Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Taiyuan Institute of China Coal Technology and Engineering Group, Shanxi Tiandi Coal Mining Machinery Co Ltd filed Critical Taiyuan Institute of China Coal Technology and Engineering Group
Priority to CN202111494905.5A priority Critical patent/CN114193449B/zh
Publication of CN114193449A publication Critical patent/CN114193449A/zh
Application granted granted Critical
Publication of CN114193449B publication Critical patent/CN114193449B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B25J9/1666—Avoiding collision or forbidden zones

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Numerical Control (AREA)

Abstract

本发明属于锚杆支护机器人自动锚杆支护作业的技术领域，具体是一种锚杆支护机器人工作臂轨迹规划方法。包括以下步骤，S100～确定巷道安装锚杆处相对锚护机器人的位置信息；S200～确定锚杆支护机器人工作臂的运动路径；S300～根据工作臂运动路径，确定工作臂各关节的B样条轨迹的插值时间和节点矢量；S400～确定关节i的B样条轨迹的控制点Pij；S500～根据S400确定的控制点，确定关节i的B样条轨迹，计算得到关节i的速度和加速度。本发明既可以保证锚杆机器人工作臂经过预设的路径点，又可以满足锚杆机器人工作臂的速度和加速度等约束条件，使得锚杆支护机器人在井下作业时可以避开外界环境障碍，又可以运动连续平稳。

Description

一种锚杆支护机器人工作臂轨迹规划方法
技术领域
本发明属于锚杆支护机器人自动锚杆支护作业的技术领域，具体是一种锚杆支护机器人工作臂轨迹规划方法。
背景技术
锚杆支护机器人是煤矿巷道支护的重要装备，对巷道高效安全掘进具有巨大的意义。锚杆支护机器人工作臂的轨迹规划是自动化锚杆支护的技术基础。锚杆支护机器人工作臂轨迹规划表示为位移、速度和加速度等位姿运动量关于时间的函数，一般在关节空间和笛卡尔空间中进行。煤矿井下复杂的工作环境，对锚杆支护机器人作业有很高的要求。锚杆支护机器人工作臂在运动过程中需要保证运动平稳且无冲击，同时还需要合理的规避外界环境中的障碍。
然而目前应用广泛的点对点轨迹规划方法（多项式轨迹规划、梯形轨迹规划等方法）无法同时满足上述要求，工作臂在相邻两锚孔运动时容易与机身和外界环境发生碰撞，为井下安全支护带来了严重的安全隐患。
发明内容
本发明针对点对点轨迹规划方法的问题，提出了一种基锚杆支护机器人工作臂轨迹规划方法。本发明方法能够保证锚杆支护机器人工作臂可以经过预设的路径点，满足锚杆支护机器人工作时的避障要求；同时能够保证锚杆支护机器人工作臂在运动过程中对速度和加速度的连续性要求，使得锚杆支护机器人工作臂运动平稳且无冲击。
本发明采取以下技术方案：一种锚杆支护机器人工作臂轨迹规划方法，包括以下步骤，S100～确定巷道安装锚杆处相对锚护机器人的位置信息；S200～确定锚杆支护机器人工作臂的运动路径；S300～根据工作臂运动路径，确定工作臂各关节的B样条轨迹的插值时间和节点矢量；S400～确定关节i的B样条轨迹的控制点Pij；S500～根据S400确定的控制点，确定关节i的B样条轨迹，计算得到关节i的速度/>和加速度/>。
步骤S100的具体过程为，通过锚杆支护机器人视觉模块识别出巷道内需要安装锚杆的位置，选取相邻两个锚固位置作为工作臂的起始位置点S0和终止位置点Sk，为了满足锚杆支护作业要求，锚杆支护机器人工作臂在起始位置点S0和终止位置点Sk的速度和加速度均要求为0。
步骤S200的具体过程为，根据工作臂的避障需求，在起始位置点S0和终止位置点Sk之间确定一条路径，即使用一条光滑曲线连接起始位置点S0和终止位置点Sk，并通过包围盒算法检测工作臂按照该路径运动是否会与巷道、锚杆支护机器人自身发生碰撞，然后离散为k-1个路径点[S 1, S 2, S 3, …, S k-1]，并根据工作臂的运动学模型，计算出所有路径点[S 0, S 1, S 2, S 3, …, S k-1, S k ]相对于锚杆支护机器人的关节坐标。
步骤S300的具体过程为，
B样条轨迹的模型为：
其中，为关节i的B样条轨迹；t为工作臂的运动时间；i表示机器人工作臂的第i个关节；p为B样条轨迹的阶数；/>为关节i的B样条轨迹的控制点；/>的表达式为：
式中，t i 为B样条轨迹的时间节点矢量；
对分别求一阶导和二阶导，则有：
式中，，
，
，
，
，
在锚杆支护机器人工作臂运动的时间范围中，确定B样条轨迹的时间节点矢量为：
。
步骤S400的具体过程为，
根据S100确定的工作臂在起始位置和终止位置速度和加速度均为零，即：
根据下式计算出B样条轨迹的控制点
式中：
，/>，/>。
与现有技术相比，本发明保留了B样条曲线的优良特性，根据各关节约束条件计算得到的B样条轨迹，既可以保证锚杆机器人工作臂经过预设的路径点，又可以满足锚杆机器人工作臂的速度和加速度等约束条件，使得锚杆支护机器人在井下作业时可以避开外界环境障碍，又可以运动连续平稳。
附图说明
图1是本发明的流程图；
图2是本发明用于锚杆支护机器人工作臂得到的关节1运动轨迹、运动速度和加速度图；
图3是本发明用于锚杆支护机器人工作臂得到的关节2运动轨迹、运动速度和加速度图；
图4是本发明用于锚杆支护机器人工作臂得到的关节3运动轨迹、运动速度和加速度图；
图5是本发明用于锚杆支护机器人工作臂得到的末端运动路径图。
具体实施方式
本发明提出的一种锚杆支护机器人工作臂轨迹规划方法，其步骤为：
S100，确定巷道安装锚杆处相对锚护机器人的位置信息。
通过锚杆支护机器人视觉模块识别出巷道内需要安装锚杆的位置，然后在巷道空间中计算出一条无障碍路径，离散成k+1个路径点，选取相邻两个锚固位置作为工作臂的起始路径点S0和终止路径点Sk。为了满足锚杆支护的作业要求，锚杆支护机器人工作臂在起始位置和终止位置的速度和加速度均要求为0。
S200，确定锚杆支护机器人工作臂的运动路径。
首先通过MD-H参数法得到锚杆支护机器人N自由度工作臂的运动学模型：
式中：为工作臂关节N相对N-1的位姿矩阵，/>为工作臂末端钻架的姿态矩阵，/>为工作臂末端钻架上的钻头的位置向量。
根据工作臂的运动学模型，可以得到工作臂的逆运动学模型：已知安装在工作臂末端钻架上的钻头位姿，即可得到对应的工作臂关节坐标。
然后根据工作臂运动的避障需求，由锚杆支护机器人的规划模块确定工作臂在自动锚杆支护时的无障碍运动路径，并根据工作臂的逆运动学模型计算出路径点[S 0, S 1,S 2, S 3, …, S k-1, S k ]的关节坐标。
S300，确定B样条曲线轨迹规划的插值时间和节点矢量。
根据B样条曲线的性质，得到关节i的B样条轨迹的模型为：
其中，i表示机器人工作臂的第i个关节；p为B样条轨迹的阶数；为关节i的B样条轨迹的控制点；/>的表达式为：
式中，时间t为B样条轨迹的时间节点矢量。
对分别求一阶导和二阶导，则有：
式中，，
，
，
，
。
在锚杆支护机器人工作臂运动的时间范围中，确定B样条轨迹的时间节点矢量为：
。
S400，确定B样条曲线的控制点。
根据S100确定的工作臂在起始位置和终止位置速度、加速度均为零，即：
计算出B样条曲线的控制点坐标
其中：
，/>，/>。
式中，为路径点[S 0, S 1, S 2, S 3, …, S k-1, S k ]对应的关节i的坐标向量（含起始点和终止点的速度和加速度），由步骤S200得到。
S500，根据S400确定的控制点，确定关节i的B样条轨迹/>，进而求一阶导和二阶导得到关节i的速度/>和加速度/>。得到的轨迹将作为锚杆支护机器人工作臂控制器的输入，进而实现工作臂在狭小的井下实现无碰撞自动锚杆支护作业。
实施例：
以锚杆支护机器人从相邻两锚孔之间的路径为例，采用本发明所述的B样条曲线插值轨迹规划方法，取k=6，即路径离散成7个路径点，同时采用4阶B样条曲线，即p=4。
根据锚杆支护机器人的视觉模块，识别锚孔，并计算锚孔相对锚杆支护机器人工作臂基坐标系的坐标，得到锚孔相对工作臂基的关节坐标为[-12.4457°，14.2120°，443.1127mm，-14.2120°，12.4457°，0]，得到锚孔相对工作臂基的关节坐标为[-3.9344°，14.5603°，381.6752mm，-14.5603°，3.9344°，0]；同时，工作臂在起始位置点和终止位置点处的速度v 0、v 6和加速度a 0、a 6均为零。
根据锚杆支护机器人工作臂的避障需求，计算路径点[S 1, S 2, S 3, S 4, S 5]的关节坐标。
根据工作臂各关节的速度、加速度和连续性约束条件，求得各分段多项式的插值时间均为1.5s，由此确定B样条轨迹的时间节点为[0, 0, 0, 0, 0, 0.75, 2.25, 3.75,5.25, 6.75, 8.25, 9, 9, 9, 9, 9]。
根据工作臂各关节的时间节点和路径点关节坐标，计算得到各关节的B样条轨迹，关节1、关节2和关节3的轨迹、速度和加速度分别如附图2、附图3和附图4所示，关节4和关节5的轨迹分别与关节2、1的轨迹互为相反数，进而得到工作臂末端的运动路径，如附图5所示。

Claims (3)

1.一种锚杆支护机器人工作臂轨迹规划方法，其特征在于：包括以下步骤，
S100～确定巷道安装锚杆处相对锚护机器人的位置信息；
S200～确定锚杆支护机器人工作臂的运动路径；
步骤S200的具体过程为，根据工作臂的避障需求，在起始位置点S0和终止位置点Sk之间确定一条路径，即使用一条光滑曲线连接起始位置点S0和终止位置点Sk，并通过包围盒算法检测工作臂按照该路径运动是否会与巷道、锚杆支护机器人自身发生碰撞，然后离散为k-1个路径点[S 1, S 2, S 3, …, S k-1]，并根据工作臂的运动学模型，计算出所有路径点[S 0, S 1,S 2, S 3, …, S k-1, S k ]相对于锚杆支护机器人的关节坐标；
S300～根据工作臂运动路径，确定工作臂各关节的B样条轨迹的插值时间和节点矢量；
所述的步骤S300的具体过程为，
B样条轨迹的模型为：
其中，为关节i的B样条轨迹；t为工作臂的运动时间；i表示机器人工作臂的第i个关节；p为B样条轨迹的阶数；/>为关节i的B样条轨迹的控制点；/>的表达式为：
式中，时间t i 为B样条轨迹的时间节点矢量；
对分别求一阶导和二阶导，则有：
式中，，
，
，
，
，
在锚杆支护机器人工作臂运动的时间范围中，确定B样条轨迹的时间节点矢量为：
；
S400～确定关节i的B样条轨迹的控制点；
S500～根据S400确定的控制点，确定关节i的B样条轨迹/>，计算得到关节i的速度/>和加速度/>。
2.根据权利要求1所述的锚杆支护机器人工作臂轨迹规划方法，其特征在于：步骤S100的具体过程为，通过锚杆支护机器人视觉模块识别出巷道内需要安装锚杆的位置，选取相邻两个锚固位置作为工作臂的起始位置点S0和终止位置点Sk，为了满足锚杆支护作业要求，锚杆支护机器人工作臂在起始位置点S0和终止位置点Sk的速度和加速度均要求为0。
3.根据权利要求2所述的锚杆支护机器人工作臂轨迹规划方法，其特征在于：所述的步骤S400的具体过程为，
根据S100确定的工作臂在起始位置和终止位置速度和加速度均为零，即：
根据下式计算出B样条轨迹的控制点
式中：
，/>，/>。
CN202111494905.5A 2021-12-09 2021-12-09 一种锚杆支护机器人工作臂轨迹规划方法 Active CN114193449B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111494905.5A CN114193449B (zh) 2021-12-09 2021-12-09 一种锚杆支护机器人工作臂轨迹规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111494905.5A CN114193449B (zh) 2021-12-09 2021-12-09 一种锚杆支护机器人工作臂轨迹规划方法

Publications (2)

Publication Number Publication Date
CN114193449A CN114193449A (zh) 2022-03-18
CN114193449B true CN114193449B (zh) 2023-08-11

Family

ID=80651529

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111494905.5A Active CN114193449B (zh) 2021-12-09 2021-12-09 一种锚杆支护机器人工作臂轨迹规划方法

Country Status (1)

Country Link
CN (1) CN114193449B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115781692A (zh) * 2023-01-09 2023-03-14 三一重型装备有限公司 一种机械臂路径规划方法、装置、智能锚护机及存储介质
CN115807685B (zh) * 2023-01-20 2023-04-28 太原理工大学 矿用锚护装备钻锚作业的自动控制系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105785921A (zh) * 2016-03-25 2016-07-20 华南理工大学 一种工业机器人nurbs曲线插补时的速度规划方法
CN107116549A (zh) * 2017-03-15 2017-09-01 广东工业大学 一种基于四阶三次b样条函数的机器人轨迹规划方法及仿人机器人平台
WO2018133439A1 (zh) * 2017-01-19 2018-07-26 北京工业大学 一种考虑末端运动误差的六自由度机器人轨迹规划方法
CN113119112A (zh) * 2021-03-18 2021-07-16 上海交通大学 适用于六自由度机器人视觉测量的运动规划方法及系统
CN113296407A (zh) * 2021-05-25 2021-08-24 南京航空航天大学 一种基于5次非均匀有理b样条的多机协同轨迹优化方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6857145B2 (ja) * 2018-03-09 2021-04-14 株式会社日立製作所 軌道計画装置、軌道計画方法、及び生産システム

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105785921A (zh) * 2016-03-25 2016-07-20 华南理工大学 一种工业机器人nurbs曲线插补时的速度规划方法
WO2018133439A1 (zh) * 2017-01-19 2018-07-26 北京工业大学 一种考虑末端运动误差的六自由度机器人轨迹规划方法
CN107116549A (zh) * 2017-03-15 2017-09-01 广东工业大学 一种基于四阶三次b样条函数的机器人轨迹规划方法及仿人机器人平台
CN113119112A (zh) * 2021-03-18 2021-07-16 上海交通大学 适用于六自由度机器人视觉测量的运动规划方法及系统
CN113296407A (zh) * 2021-05-25 2021-08-24 南京航空航天大学 一种基于5次非均匀有理b样条的多机协同轨迹优化方法

Also Published As

Publication number Publication date
CN114193449A (zh) 2022-03-18

Similar Documents

Publication Publication Date Title
CN114193449B (zh) 一种锚杆支护机器人工作臂轨迹规划方法
US20210291362A1 (en) Active damping system
CN101898358B (zh) 用于控制机械手的方法及装置
JP2019521872A (ja) 可撓性アーム上に取り付けられたロボットアームの動的補償
CN115605328A (zh) 一种用于工业机器人的碰撞避免运动规划方法
CN110561419B (zh) 臂型线约束柔性机器人轨迹规划方法及装置
CN110455290B (zh) 一种智能液压挖掘机的最优轨迹规划方法
CN113334385A (zh) 一种自驱动关节臂测量机直线轨迹间平滑过渡的规划方法
Wang et al. A control method for hydraulic manipulators in automatic emulsion filling
WO2020161910A1 (ja) 制御装置、制御方法、記録媒体
CN113420403A (zh) 一种液压支架与刮板输送机的推移机构运动规划方法
US10022868B2 (en) Inverse kinematic solution for multi-joint link mechanism, and teaching-data creating device using the inverse kinematic solution
CN115157251A (zh) 机器人的奇异防护方法和机器人系统
JPH09212225A (ja) ロボットの教示装置
CN115544768A (zh) 一种自主挖掘作业轨迹生成方法及系统
CN114227695B (zh) 一种锚杆支护机器人工作臂轨迹规划方法及系统
CN113954081A (zh) 一种锚杆支护机器人的工作臂轨迹规划方法及系统
CN115586770A (zh) 基于龙芯嵌入式系统部署ros的机器人路径规划方法
JPH09212229A (ja) ロボットの教示装置
CN111119919B (zh) 一种柔臂掘进机推进系统控制计算方法
CN116733441B (zh) 一种隧道打孔机器人及其智能目标识别与自动化打孔方法
CN117055361B (zh) 基于滑模模型预测控制的移动机器人控制方法
CN117408078A (zh) 一种多臂架防碰撞轨迹实时优化方法、系统、介质和设备
CN114003044B (zh) 一种地下线缆机器人小车维修编队控制装置及方法
CN116330286B (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