CN115946121B - 一种用于机械臂基于动态规划的传送带跟踪方法 - Google Patents
一种用于机械臂基于动态规划的传送带跟踪方法 Download PDFInfo
- Publication number
- CN115946121B CN115946121B CN202310046750.1A CN202310046750A CN115946121B CN 115946121 B CN115946121 B CN 115946121B CN 202310046750 A CN202310046750 A CN 202310046750A CN 115946121 B CN115946121 B CN 115946121B
- Authority
- CN
- China
- Prior art keywords
- speed
- acceleration
- conveyor belt
- robot
- planning
- 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
Links
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Manipulator (AREA)
Abstract
本发明涉及机器人抓取技术领域，公开了一种用于机械臂基于动态规划的传送带跟踪方法，首先在每个控制周期内获取机器人和传送带上物体的当前状态，包括机器人的位置、速度和加速度以及传送带上物体的位置、运行速度及加速度，然后计算得到机器人与传送带上物体的相对位置、相对速度和相对加速度，得到新的正向轮廓速度和反向轮廓速度；对机器人给定的加速度和减速度进行补偿，得到新的正向轮廓加速度和反向轮廓减速度，最后对轨迹的平滑需求选择不同方式的规划方法通过得到的参数作为输入进行轨迹规划。本发明避免了由于传送带速度波动或者在变加速情况下造成的位置预测不准，导致的定位精度和跟踪速度误差较大的缺点，保证了跟踪和抓取精度。
Description
技术领域
本发明涉及机器人抓取技术领域，具体为一种用于机械臂基于动态规划的传送带跟踪方法。
背景技术
机械臂对于随传送带移动的物体进行动态抓取，涉及到机械手的抓取动作要和物体的移动速度相协调，即机械手要对随传送带移动的物料位置进行跟踪和预测，等位置和速度一致时再进行动态抓取动作。目前，常用的传送带跟踪功能通常采用基于PID的动态跟踪方法和基于位置预测的拦截方法。
传统的基于PID的跟踪方法在传送带速度相对高时，容易造成跟踪轨迹曲率大，恶化跟踪效果。传统PID形式跟踪算法本质是通过高频的目标位置刷新，实时调整机器人末端的位置和方向，使机器人末端的运动方向始终指向目标物体的当前位置，直至跟踪完成两者的运动方向和速度一致，之后随着目标物体移动。
拦截式抓取是根据当前物体的位置，速度和加速度来计算出器将来任意时刻的位置，然后规划机器人末端的动作，从而控制机器人直接运动至此位置，这样就可以缩短机器人末端的运动距离，从而缩短抓取时间，提高抓取效率。
目前拦截式抓取对于速度一定的匀速运动或者加速度一定的匀加速运动，在变加速运动情况下容易造成定位精度和跟踪速度误差较大，不满足精度要求高的场所，同时密化拦截式计算频率容易导致运动过程中机器人的速度和加速度变化频繁甚至存在换向，因此为保证机器人运动平滑无换向冲击，就需要一种基于动态规划的传送带跟踪方法。
发明内容
本发明的目的在于提供一种用于机械臂基于动态规划的传送带跟踪方法，本发明继承了拦截式抓取耗时短、抓取效率高的特点。同时通过步进式动态规划方式，避免了由于传送带速度波动或者在变加速情况下造成的位置预测不准，进而导致的定位精度和跟踪速度误差较大的缺点。也能避免拦截式方法频繁规划带来的速度和加速度频繁变化的问题，因而可以在无需附加传感器和提高硬件成本的前提下，减小传送带跟踪误差、保证了跟踪和抓取精度。
本发明是这样实现的：
本发明提供一种用于机械臂基于动态规划的传送带跟踪方法，S1：在机械臂的每个控制周期开始，首先获取机械臂机器人的当前状态，具体包括机器人机械臂的当前位置P_r(xr,yr,zr)、当前速度V_r和当前加速度Acc_r，同时需要获得当前传送带上物体的位置P_c(xc,yc,zc)、运行速度V_c及加速度Acc_c，
S2：根据S1获得的数据，计算得到机器人与传送带上物体的相对位置P、相对速度值V和相对加速度A，作为后续速度规划的输入参数，如式1-式3；
P=P_c-P_r 式1
V=V_c-V_r 式2
A=Acc_c-Acc_r 式3
S3：再根据物体速度V_c大小和方向，对于给定的轮廓速度VP进行补偿，得到新的正向轮廓速度VPP和反向轮廓速度VPN；对于机器人机械臂给定的加速度Acc和减速度Dec进行补偿，得到新的正向轮廓加速度AccP和反向轮廓减速度DecN，如式4-式5；
AccP=Acc-Acc_r 式4
DecN=Dec+Acc_r 式5
S4：最后根据对轨迹的平滑需求选择不同方式的规划方法并以计算得到的参数作为输入进行轨迹规划，得到轨迹各段的加速度值和持续时间；
以T型曲线速度规划算法为例进行说明，
通用T型曲线规划算法的输入接口包括：起始位置Ps、终止位置Pe、起始速度Vs、终止速度Ve、轮廓加速度Accle和轮廓减速度Decle；结合S1-S3可知，如式6-式11；
Ps=P_r 式6
Pe=P_c 式7
Vs=V 式8
Ve=0 式9
Accle=AccP 式10
Decle=DecN 式11
T型曲线规划算法规划得到的参数为轨迹各段的加速度值和持续时间。根据初始相对位置和相对速度的方向，所规划轨迹分为反向减速段、正向加速段、匀速段、正向减速段、反向加速段。
各段的加速度值和持续时间分别为：
反向减速段：加速度值AD1=AccP或DecN；持续时间Tad1。
正向加速段：加速度值AD2=AccP或DecN；持续时间Tad2。
匀速段：加速度值0=AccP或DecN；持续时间Tad3。
正向减速段：加速度值AD4=AccP或DecN；持续时间Tad4。
反向加速段：加速度值AD5=AccP或DecN；持续时间Tad5。
S5：将S1获得的机器人和传送带上物体的各项参数，以及规划得到的各段加速度值和持续时间带入到T型曲线规划算法的位置、速度求解中，得到下一个周期输出的机器人的加速度值、速度值和位置值；
S6：由于传送带在运动过程中可能为匀速、匀变速和变加速运动，仅一次规划得到的抓取位置和抓取速度与实际抓取位置和抓取位置之间可能存在位置偏差，因此需要不断规划修正，即每一个周期开始前都需要根据当前状态进行动态规划。因此在下一周期开始时，周期内部重复S1-S5步骤，进行动态规划。
进一步，在步骤S1中提到的位置和速度均在相同参考坐标系下表示，该参考坐标系可以选择但不限于世界坐标系或者机器人坐标系。根据自动化行业惯例或相关标准，传送带布置方向与机器人的坐标轴X轴方向平行，传送带速度方向沿机器人的X轴正向。
进一步，在步骤S6中，动态规划方法的速度规划方法包括采用T型速度规划、五段式S型速度规划或者七段式S型速度规划。
进一步，本发明提供一种计算机存储介质，在所述存储介质上存储有计算机程序，当计算机程序被执行时，实现如上述任一所述的方法。
与现有技术相比，本发明的有益效果是：
1.拦截式抓取的关键式抓取点的求取以及机器人整个运动过程中的速度规划。相较于以往将抓取点求取转化为追击问题，对追击时间和位置进行优化求解，本方法基于选定的速度规划方法进行动态规划，从而求得轨迹中各段时间，进而计算得到理论的追击位置。
2.本发明提供的思路适用于各类高阶平滑速度规划算法，可拓展应用到不同的应用场所。
3.本发明规划得到的加速度和速度曲线平滑，无频繁换向情况发生，从而提高了跟踪和动态抓取精度。
附图说明
为了更清楚地说明本发明实施方式的技术方案，下面将对实施方式中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1是本发明的实施方法流程图；
图2是本发明的传送带匀速测试用例效果图；
图3是本发明的传送带小加速度匀加速测试用例效果图；
图4是本发明的传送带大加速度匀加速测试用例效果图；
图5是本发明的传送带变加速测试用例效果图。
具体实施方式
为使本发明实施方式的目的、技术方案和优点更加清楚，下面将结合本发明实施方式中的附图，对本发明实施方式中的技术方案进行清楚、完整地描述，显然，所描述的实施方式是本发明一部分实施方式，而不是全部的实施方式。基于本发明中的实施方式，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施方式，都属于本发明保护的范围。因此，以下对在附图中提供的本发明的实施方式的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施方式。基于本发明中的实施方式，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施方式，都属于本发明保护的范围。
请参阅图1-5，一种用于机械臂基于动态规划的传送带跟踪方法，具体按以下步骤执行；
S1：在机械臂的每个控制周期开始，首先获取机械臂机器人的当前状态，具体包括机器人机械臂的当前位置P_r(xr,yr,zr)、当前速度V_r和当前加速度Acc_r，同时需要获得当前传送带上物体的位置P_c(xc,yc,zc)、运行速度V_c及加速度Acc_c；在所述的位置和速度均在相同参考坐标系下表示，该参考坐标系可以选择但不限于世界坐标系或者机器人坐标系。根据自动化行业惯例或相关标准，传送带布置方向与机器人的坐标轴X轴方向平行，传送带速度方向沿机器人的X轴正向。
S2：根据S1获得的数据，计算得到机器人与传送带上物体的相对位置P、相对速度值V和相对加速度A，作为后续速度规划的输入参数，如式1-式3；
P=P_c-P_r 式1
V=V_c-V_r 式2
A=Acc_c-Acc_r 式3
S3：再根据物体速度V_c大小和方向，对于给定的轮廓速度VP进行补偿，得到新的正向轮廓速度VPP和反向轮廓速度VPN；对于机器人机械臂给定的加速度Acc和减速度Dec进行补偿，得到新的正向轮廓加速度AccP和反向轮廓减速度DecN，如式4-式5；
AccP=Acc-Acc_r 式4
DecN=Dec+Acc_r 式5
S4：最后根据对轨迹的平滑需求选择不同方式的规划方法并以计算得到的参数作为输入进行轨迹规划，得到轨迹各段的加速度值和持续时间；
以T型曲线速度规划算法为例进行说明，
通用T型曲线规划算法的输入接口包括：起始位置Ps、终止位置Pe、起始速度Vs、终止速度Ve、轮廓加速度Accle和轮廓减速度Decle；结合S1-S3可知，如式6-式11；
Ps=P_r 式6
Pe=P_c 式7
Vs=V 式8
Ve=0 式9
Accle=AccP 式10
Decle=DecN 式11
T型曲线规划算法规划得到的参数为轨迹各段的加速度值和持续时间。根据初始相对位置和相对速度的方向，所规划轨迹分为反向减速段、正向加速段、匀速段、正向减速段、反向加速段。
各段的加速度值和持续时间分别为：
反向减速段：加速度值AD1=AccP或DecN；持续时间Tad1。
正向加速段：加速度值AD2=AccP或DecN；持续时间Tad2。
匀速段：加速度值0=AccP或DecN；持续时间Tad3。
正向减速段：加速度值AD4=AccP或DecN；持续时间Tad4。
反向加速段：加速度值AD5=AccP或DecN；持续时间Tad5。
S5：将S1获得的机器人和传送带上物体的各项参数，以及规划得到的各段加速度值和持续时间带入到T型曲线规划算法的位置、速度求解中，得到下一个周期输出的机器人的加速度值、速度值和位置值；
S6：由于传送带在运动过程中可能为匀速、匀变速和变加速运动，仅一次规划得到的抓取位置和抓取速度与实际抓取位置和抓取位置之间可能存在位置偏差，因此需要不断规划修正，即每一个周期开始前都需要根据当前状态进行动态规划。因此在下一周期开始时，周期内部重复S1-S5步骤，进行动态规划。其中动态规划方法的速度动态规划方法包括采用T型速度规划、五段式S型速度规划或者七段式S型速度规划。
本实施例中，本发明提供一种计算机存储介质，在所述存储介质上存储有计算机程序，当计算机程序被执行时，实现如上述任一所述的方法。
以上所述仅为本发明的优选实施方式而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (7)
1.一种用于机械臂基于动态规划的传送带跟踪方法，其特征在于：具体按以下步骤执行：
S1：在机械臂的每个控制周期开始，首先获取机械臂机器人的当前状态，具体包括机器人机械臂的当前位置P_r(xr,yr,zr)、当前速度V_r和当前加速度Acc_r，同时需要获得当前传送带上物体的位置P_c(xc,yc,zc)、运行速度V_c及加速度Acc_c；
S2：根据S1获得的数据，计算得到机器人与传送带上物体的相对位置P、相对速度值V和相对加速度A，作为后续速度规划的输入参数，如式1-式3；
P=P_c-P_r 式1
V=V_c-V_r 式2
A=Acc_c-Acc_r 式3
S3：再根据物体速度V_c大小和方向，对于给定的轮廓速度VP进行补偿，得到新的正向轮廓速度VPP和反向轮廓速度VPN；对于机器人机械臂给定的加速度Acc和减速度Dec进行补偿，得到新的正向轮廓加速度AccP和反向轮廓减速度DecN，如式4-式5；
AccP=Acc-Acc_r 式4
DecN=Dec+Acc_r 式5
S4：最后根据对轨迹的平滑需求选择不同方式的规划方法并以计算得到的参数作为输入进行轨迹规划，得到轨迹各段的加速度值和持续时间；
S5：将S1获得的机器人和传送带上物体的各项参数，以及规划得到的各段加速度值和持续时间带入到T型曲线规划算法的位置、速度求解中，得到下一个周期输出的机器人的加速度值、速度值和位置值；
S6：由于传送带在运动过程中可能为匀速、匀变速和变加速运动，仅一次规划得到的抓取位置和抓取速度与实际抓取位置和抓取位置之间可能存在位置偏差，因此需要不断规划修正，即每一个周期开始前都需要根据当前状态进行动态规划，因此在下一周期开始时，周期内部重复S1-S5步骤，进行动态规划。
2.根据权利要求1所述的一种用于机械臂基于动态规划的传送带跟踪方法，其特征在于，在步骤S1中提到的位置和速度均在相同参考坐标系下表示，该参考坐标系选择世界坐标系或者机器人坐标系，传送带布置方向与机器人的坐标轴X轴方向平行，传送带速度方向沿机器人的X轴正向。
3.根据权利要求1所述的一种用于机械臂基于动态规划的传送带跟踪方法，其特征在于，在步骤S4中，通用T型曲线规划算法的输入接口包括起始位置Ps、终止位置Pe、起始速度Vs、终止速度Ve、轮廓加速度Accle和轮廓减速度Decle；结合S1-S3，壳得到式6-式11；
Ps=P_r 式6
Pe=P_c 式7
Vs=V 式8
Ve=0 式9
Accle=AccP 式10
Decle=DecN 式11。
4.根据权利要求3所述的一种用于机械臂基于动态规划的传送带跟踪方法，其特征在于，根据T型曲线规划算法规划得到的参数为轨迹各段的加速度值和持续时间，根据初始相对位置和相对速度的方向，所规划轨迹分为反向减速段、正向加速段、匀速段、正向减速段、反向加速段。
5.根据权利要求4所述的一种用于机械臂基于动态规划的传送带跟踪方法，其特征在于，各段的加速度值和持续时间分别为：
反向减速段：加速度值AD1=AccP或DecN；持续时间Tad1；正向加速段：加速度值AD2=AccP或DecN；持续时间Tad2；
匀速段：加速度值0=AccP或DecN；持续时间Tad3；
正向减速段：加速度值AD4=AccP或DecN；持续时间Tad4；
反向加速段：加速度值AD5=AccP或DecN；持续时间Tad5。
6.根据权利要求1所述的一种用于机械臂基于动态规划的传送带跟踪方法，其特征在于，在步骤S6中，动态规划方法的速度规划方法包括采用T型速度规划、五段式S型速度规划或者七段式S型速度规划。
7.一种计算机存储介质，其特征在于，在所述存储介质上存储有计算机程序，当计算机程序被执行时，实现如上述权利要求1-6任一所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310046750.1A CN115946121B (zh) | 2023-01-31 | 2023-01-31 | 一种用于机械臂基于动态规划的传送带跟踪方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310046750.1A CN115946121B (zh) | 2023-01-31 | 2023-01-31 | 一种用于机械臂基于动态规划的传送带跟踪方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115946121A CN115946121A (zh) | 2023-04-11 |
| CN115946121B true CN115946121B (zh) | 2023-06-27 |
Family
ID=85902812
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310046750.1A Active CN115946121B (zh) | 2023-01-31 | 2023-01-31 | 一种用于机械臂基于动态规划的传送带跟踪方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115946121B (zh) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20070076161A (ko) * | 2006-01-18 | 2007-07-24 | 주식회사 로보스타 | 로봇의 작업선 추종방법 및 그 시스템 |
| CN105700530A (zh) * | 2016-04-11 | 2016-06-22 | 南京埃斯顿自动化股份有限公司 | 一种机器人关节空间传送带跟随运动的轨迹规划方法 |
| CN109483554A (zh) * | 2019-01-22 | 2019-03-19 | 清华大学 | 基于全局和局部视觉语义的机器人动态抓取方法及系统 |
| CN110653826A (zh) * | 2019-10-23 | 2020-01-07 | 苏州艾利特机器人有限公司 | 一种面向传送带目标跟随的机器人实时轨迹规划方法 |
| CN111168675A (zh) * | 2020-01-08 | 2020-05-19 | 北京航空航天大学 | 一种家用服务机器人的机械臂动态避障运动规划方法 |
| CN113703433A (zh) * | 2020-05-21 | 2021-11-26 | 北京配天技术有限公司 | 机器人运动轨迹的速度规划方法及装置 |
| CN113858213A (zh) * | 2021-11-09 | 2021-12-31 | 南京埃斯顿机器人工程有限公司 | 一种用于目标同步的机器人动态轨迹规划方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB2584677B (en) * | 2019-06-10 | 2023-02-15 | Glowbuzzer Ltd | Method and apparatus for trajectory-planning |
-
2023
- 2023-01-31 CN CN202310046750.1A patent/CN115946121B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20070076161A (ko) * | 2006-01-18 | 2007-07-24 | 주식회사 로보스타 | 로봇의 작업선 추종방법 및 그 시스템 |
| CN105700530A (zh) * | 2016-04-11 | 2016-06-22 | 南京埃斯顿自动化股份有限公司 | 一种机器人关节空间传送带跟随运动的轨迹规划方法 |
| CN109483554A (zh) * | 2019-01-22 | 2019-03-19 | 清华大学 | 基于全局和局部视觉语义的机器人动态抓取方法及系统 |
| CN110653826A (zh) * | 2019-10-23 | 2020-01-07 | 苏州艾利特机器人有限公司 | 一种面向传送带目标跟随的机器人实时轨迹规划方法 |
| CN111168675A (zh) * | 2020-01-08 | 2020-05-19 | 北京航空航天大学 | 一种家用服务机器人的机械臂动态避障运动规划方法 |
| CN113703433A (zh) * | 2020-05-21 | 2021-11-26 | 北京配天技术有限公司 | 机器人运动轨迹的速度规划方法及装置 |
| CN113858213A (zh) * | 2021-11-09 | 2021-12-31 | 南京埃斯顿机器人工程有限公司 | 一种用于目标同步的机器人动态轨迹规划方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115946121A (zh) | 2023-04-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP2022608B1 (en) | Improved blending algorithm for trajectory planning | |
| CN106647282B (zh) | 一种考虑末端运动误差的六自由度机器人轨迹规划方法 | |
| CN112757306B (zh) | 一种机械臂逆解多解选择和时间最优轨迹规划算法 | |
| CN107160394B (zh) | 一种直线运动模组精确控制方法 | |
| CN108568817B (zh) | 一种基于贝塞尔曲线的Delta机器人轨迹连接控制方法 | |
| CN111897216A (zh) | 一种多运动段速度规划和插补方法 | |
| CN108319228B (zh) | 一种数控系统轨迹规划中加减速控制方法 | |
| WO2022160894A1 (zh) | 基于迭代学习的数控机床快速误差补偿控制系统及方法 | |
| CN103941647A (zh) | 用于嵌入式数控设备的柔性加减速控制插补方法 | |
| CN111597702A (zh) | 一种火箭着陆轨迹规划方法及装置 | |
| CN107932505B (zh) | 基于关节臂机器人的最优打磨任务路径规划方法和装置 | |
| CN109032128B (zh) | 离散多agv非质点系统的三角编队控制方法 | |
| CN109313429B (zh) | S型速度规划方法、装置、系统、机器人以及数控机床 | |
| CN115946121B (zh) | 一种用于机械臂基于动态规划的传送带跟踪方法 | |
| CN112720492B (zh) | 多轴机器人的复杂轨迹光顺方法、装置、介质及电子设备 | |
| EP4265378A1 (en) | Path generation method of generating path of robot using command from external apparatus and robot control system | |
| CN106444382B (zh) | 一种保证物理可行性的串联机器人动力学参数辨识方法 | |
| CN112666986B (zh) | 一种天线伺服轨迹规划方法和系统 | |
| CN113763434B (zh) | 一种基于卡尔曼滤波多运动模型切换的目标轨迹预测方法 | |
| CN115741679B (zh) | 一种基于高阶平滑规划和速度叠加的动态抓取算法 | |
| CN113967917B (zh) | 机械臂多关节轨迹时间同步方法、系统及存储介质 | |
| CN116400584B (zh) | 大载荷电液位置伺服系统快速精确控制系统应用方法 | |
| Li et al. | Trajectory planning of high precision collaborative robots | |
| US20230321835A1 (en) | Method and apparatus for tuning robot system | |
| CN114253241B (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 |