[go: up one dir, main page]

CN114879704A - 一种机器人绕障控制方法及系统 - Google Patents

一种机器人绕障控制方法及系统 Download PDF

Info

Publication number
CN114879704A
CN114879704A CN202210808876.3A CN202210808876A CN114879704A CN 114879704 A CN114879704 A CN 114879704A CN 202210808876 A CN202210808876 A CN 202210808876A CN 114879704 A CN114879704 A CN 114879704A
Authority
CN
China
Prior art keywords
obstacle
robot
path
pose
running
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
Application number
CN202210808876.3A
Other languages
English (en)
Other versions
CN114879704B (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.)
Shandong University
Original Assignee
Shandong University
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 Shandong University filed Critical Shandong University
Priority to CN202210808876.3A priority Critical patent/CN114879704B/zh
Publication of CN114879704A publication Critical patent/CN114879704A/zh
Application granted granted Critical
Publication of CN114879704B publication Critical patent/CN114879704B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明提供了一种机器人绕障控制方法及系统，属于机器人绕障控制技术领域；在原始路径方向上叠加偏移，修正机器人原行驶轨迹得到数条新的绕障路径；选择距离最短的绕障路径，并对路径参数进行前瞻预处理，得到目标轨迹的运行参数，驱动机器人沿目标轨迹运行过程中，根据目标轨迹在机器人坐标系下的相对位姿关系，对机器人的运行线速度按照速度方向与目标方位角，建立运动学约束关系；按照约束后的速度驱动机器人运行，直至各个预设障碍物检测区域均未触发，等待机器人的移动距离达到目标运行距离后取消对原始轨迹的修偏量，恢复原路径运行；本发明实现了更精准的机器人绕障控制，避免与障碍物相撞，提高了机器人的控制精度。

Description

一种机器人绕障控制方法及系统
技术领域
本发明涉及机器人绕障控制技术领域，特别涉及一种机器人绕障控制方法及系统。
背景技术
本部分的陈述仅仅是提供了与本发明相关的背景技术，并不必然构成现有技术。
在机器人的日常作业过程中，为了提升作业效率和安全性因素，通常会规划机器人的固定行走路线。在机器人的行走路线中出现无法预料的障碍的情况下，此时如果采用较为常用的自由路径规划方法的话，会规划一条到达目标点的自由路径。
但是，发明人发现，上述方案得到的路径可能会偏离原固定轨迹较大，从而导致额外事故的出现；此外，机器人作业精度是有较高要求的，通过自由路径规划得出的路径，因不能沿原路径行驶，其运行精度也会打折扣。
发明内容
为了解决现有技术的不足，本发明提供了一种机器人绕障控制方法及系统，在满足固定路径跟踪的同时，实现了更精准的机器人绕障控制，避免与障碍物相撞，提高了机器人的控制精度。
为了实现上述目的，本发明采用如下技术方案：
本发明第一方面提供了一种机器人绕障控制方法。
一种机器人绕障控制方法，包括以下过程：
获取机器人运动前方的图像数据；
根据获取的图像数据，提取机器人运动前方的障碍物特征点，根据障碍物与检测区域的关系进行机器人降速控制，同时根据障碍物特征点得到障碍物的位姿和尺寸，并对障碍物的尺寸进行膨胀处理；
根据障碍物位姿及膨胀处理后的尺寸，在预设路径参数范围内，在原始路径方向上叠加偏移，修正机器人原行驶轨迹得到数条新的绕障路径；
选择距离最短的绕障路径，并对路径参数进行前瞻预处理，得到目标轨迹的运行参数，驱动机器人沿目标轨迹运行过程中，根据目标轨迹在机器人坐标系下的相对位姿关系，对机器人的运行线速度按照速度方向与目标方位角，建立运动学约束关系；
按照约束后的速度驱动机器人运行，直至各个预设障碍物检测区域均未触发，等待机器人的移动距离达到目标运行距离后取消对原始轨迹的修偏量，恢复原路径运行。
作为可选的一种实现方式，根据障碍物特征点得到障碍物的位姿和尺寸，包括：
统计落在检测边框中的障碍物位姿数据，计算得出障碍物边界数据，从而得出障碍物尺寸；根据检测传感器相对于机器人坐标系的位姿数据以及障碍物边界数据，得到障碍物相对于机器人坐标系的位姿。
作为可选的进一步限定，障碍物相对于机器人坐标系的位姿，包括：
Figure 100002_DEST_PATH_IMAGE001
其中，（xr，yr，θr）是检测传感器相对于机器人坐标系的位姿分量，xmax、xmin、ymax和ymin为障碍物边界数据。
作为可选的进一步限定，对障碍物的尺寸进行膨胀处理，包括：
加入尺寸膨胀系数，将障碍物扩充为圆柱体，圆柱体的截面半径为Robs，β为膨胀系数，则：
Figure 789222DEST_PATH_IMAGE002
其中，lobs为障碍物在y轴方向的长度，wobs为障碍物在x轴方向的长度。
作为可选的进一步限定，在预设路径参数范围内，在原始路径方向上叠加偏移，修正机器人原行驶轨迹得到数条新的绕障路径，包括：
假定当前路径的起点为p 0 ，终点为p 1 ，机器人的当前点为p r ，当前路径的方向矢量为（xl，yl），pt为机器人到p0 p1直线的垂足点，p o 为障碍物中心坐标，轨迹偏移量叠加方向为（-yl，xl）；
Figure 100002_DEST_PATH_IMAGE003
为障碍物到路径的距离；
Figure 739860DEST_PATH_IMAGE004
；
则路径修正的可选偏移量为：
Figure 100002_DEST_PATH_IMAGE005
取距离最短的偏移量作为路径的目标偏移数据。
作为可选的进一步限定，对路径参数进行前瞻预处理，包括：距离前瞻和速度前瞻；
距离前瞻，包括：
Figure 684682DEST_PATH_IMAGE006
速度前瞻，包括：
Figure 100002_DEST_PATH_IMAGE007
其中，Rr为机器人本体的半径，vmax为机器人最大速度，lslow为预设缓速距离，acc为机器人行驶加速度。
作为可选的进一步限定，运动学约束，包括：对机器人的线速度和角速度进行修正，包括：
Figure 105343DEST_PATH_IMAGE008
其中，θ为目标轨迹点相对于机器人的夹角，
Figure 100002_DEST_PATH_IMAGE009
为修正目标线速度，
Figure 110208DEST_PATH_IMAGE010
为修正目标角速度，k1、k2和k3为预设调节参数，
Figure 100002_DEST_PATH_IMAGE011
为修正前的线速度，
Figure 548142DEST_PATH_IMAGE012
为修正前的角速度。
作为可选的一种实现方式，根据障碍物与检测区域的关系进行机器人降速控制，包括：
如果障碍物进入到机器人的第一预设检测区域时，启动无级降速过程，如果障碍物处于机器人的第二预设区域，则机器人速度降为零。
本发明第二方面提供了一种机器人绕障控制系统。
一种机器人绕障控制系统，包括：
数据获取模块，被配置为：获取机器人运动前方的图像数据；
障碍物识别模块，被配置为：根据获取的图像数据，提取机器人运动前方的障碍物特征点，根据障碍物与检测区域的关系进行机器人降速控制，同时根据障碍物特征点得到障碍物的位姿和尺寸，并对障碍物的尺寸进行膨胀处理；
路径修正模块，被配置为：根据障碍物位姿及膨胀处理后的尺寸，在预设路径参数范围内，在原始路径方向上叠加偏移，修正机器人原行驶轨迹得到数条新的绕障路径；
运动学约束构建模块，被配置为：选择距离最短的绕障路径，并对路径参数进行前瞻预处理，得到目标轨迹的运行参数，驱动机器人沿目标轨迹运行过程中，根据目标轨迹在机器人坐标系下的相对位姿关系，对机器人的运行线速度按照速度方向与目标方位角，建立运动学约束关系；
机器人驱动模块，被配置为：按照约束后的速度驱动机器人运行，直至各个预设障碍物检测区域均未触发，等待机器人的移动距离达到目标运行距离后取消对原始轨迹的修偏量，恢复原路径运行。
作为可选的一种实现方式，根据障碍物特征点得到障碍物的位姿和尺寸，包括：
统计落在检测边框中的障碍物位姿数据，计算得出障碍物边界数据，从而得出障碍物尺寸；根据检测传感器相对于机器人坐标系的位姿数据以及障碍物边界数据，得到障碍物相对于机器人坐标系的位姿；
障碍物相对于机器人坐标系的位姿，包括：
Figure DEST_PATH_IMAGE013
其中，（xr，yr，θr）是检测传感器相对于机器人坐标系的位姿分量，xmax、xmin、ymax和ymin为障碍物边界数据。
与现有技术相比，本发明的有益效果是：
1、本发明所述的机器人绕障控制方法及系统，在满足固定路径跟踪的同时，实现了更精准的绕障控制，避免与障碍物相撞，提高了机器人的控制精度。
2、本发明所述的机器人绕障控制方法及系统，通过设定第一预设区间为无级降速区间和第二预设区间为停止区间，使得机器人在遇到障碍物后会及时降速，紧急情况下直接停止，减少了机器人与障碍发生碰撞。
3、本发明所述的机器人绕障控制方法及系统，通过将绕障轨迹修正范围约束在路径宽度参数内，沿着垂直路径的方向叠加偏移量，计算绕过障碍的多条轨迹参数；使得机器人修正后的绕障路径不会偏离原轨迹太多，避免了机器人出现脱离轨迹运行的问题。
4、本发明所述的机器人绕障控制方法及系统，通过限定路径宽度范围，缩减了绕障路径的搜索空间，只有规划的绕障路径在路径限定范围内，才能执行绕障的动作，从而进一步减少了碰撞概率。
5、本发明所述的机器人绕障控制方法及系统，所述的在路径宽度范围内规划得出数条绕障路径，根据膨胀后的障碍物以及车身尺寸选取满足条件的路径；随着车体位姿不断更新，障碍物与车体的相对位姿关系也随之更新；相应的需要不断切换满足要求的绕障轨迹，从而实现最优轨迹的切换。
6、本发明所述的机器人绕障控制方法及系统，通过绕障路径切换，更新得到机器人新的规划轨迹。通过对新轨迹进行前瞻处理，得到机器人坐标系下的新目标位姿；根据目标点的相对位姿关系，建立速度正向运动学模型，根据输入的线速度和目标角度，得到修正的线速度和角速度，用于驱动机器人绕障；从而实现拟人化的绕障作业。
本发明附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
图1为本发明实施例1提供的障碍感知示意图。
图2为本发明实施例1提供的机器人绕障控制方法的流程示意图。
图3为本发明实施例1提供的路径修正示意图。
图4为本发明实施例1提供的运动学约束计算示意图。
图5为本发明实施例1提供的无级降速和避障区域示意图。
图6为本发明实施例1提供的绕障路径规划示意图。
具体实施方式
下面结合附图与实施例对本发明作进一步说明。
应该指出，以下详细说明都是示例性的，旨在对本发明提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
实施例1：
如图1、图2、图3和图4所示，本发明实施例1提供了一种的机器人绕障控制方法，包括以下过程：
S1：在机器人跟随设定路径行走的过程中，通过车身固有传感器感知周围的环境信息；
S2：当有障碍物进入到机器人的第一预设检测区域时，启动无级降速过程，逐步降低机器人的运行速度，如果障碍物处于机器人的第二预设区域，则机器人速度降为零；
S3：通过对获取到的感知传感器数据进行特征提取，分析障碍物的尺寸以及障碍物在机器人坐标系下的具体方位，确定机器人目标运行距离；
S4：为了避免机器人与障碍物碰撞，对障碍物的尺寸进行膨胀处理；
S5：根据障碍物方位及大小，在预设路径参数范围内，在原始路径方向上叠加偏移，修正机器人当前行驶轨迹得到数条新的绕障路径；
S6：根据距离优先原则，选择绕障路径，并对路径参数进行前瞻预处理，得到目标轨迹的运行参数；
S7：驱动机器人沿目标轨迹运行过程中，根据目标轨迹在自身坐标系下的相对位姿关系，对机器人的运行线速度按照速度方向与目标方位角，建立运动学约束关系；
S8：按照约束后的速度驱动运行，直至第一及第二预设检测区域均未触发，等待车体的移动距离达到目标运行距离后取消对原始轨迹的修偏量，重新调用速度运动学约束模型驱动运行，从而绕过障碍恢复实现原路径运行。
本实施例中，障碍识别为通过对激光点云数据进行空间位置斜率特征计算，通过阈值判断，得出描述障碍物空间信息的特征点，根据特征点的空间坐标，得出障碍物相对于车体的位姿关系；路径再规划算法，是指在源路径宽度方向内，规划绕过障碍物的新轨迹；速度再规划算法，是指通过对新路径的前瞻规划，得出新路径下的目标点，计算新目标路径点在车体坐标系下的位姿，在原速度参数上，叠加位姿约束权重，从而实现对新轨迹的跟踪。
更具体的，S3中，障碍物特征提取，包括：
通过ORB特征提取的方法，提取机器人前方的FAST特征点；将提取到的FAST特征点转换到机器人坐标系下，如果该特征点不在机器人的避障区域，则不处理。如果该特征点落在了机器人避障区域，则将该位姿转换到世界坐标系，并将障碍进行一次累计计数；如果连续3帧检测到的特征点均落在了相同位姿，则认为检测到了有效障碍。
分析障碍物的尺寸以及障碍物在机器人坐标系下的具体方位，包括：
尺寸分析及障碍物位姿判定：通过基于深度学习的YOLO目标检测方法，对识别数据集中障碍物进行检测，并确定障碍物检测边框；获取此时对应的激光雷达及ORB特征点数据，统计落在检测边框中的位姿数据，计算得出障碍物边界数据：
Figure 31076DEST_PATH_IMAGE014
（1-1）
从而得出障碍物尺寸信息：
Figure DEST_PATH_IMAGE015
（1-2）
其中，l obs 为障碍物在y轴方向的长度，w obs 为障碍物在x轴方向的长度，h obs 为障碍物在z轴方向的长度。
障碍物相对于机器人的位姿信息。由于机器人只是在平面内运动，只需要x，y及姿态角θ，
Figure 300384DEST_PATH_IMAGE016
是检测传感器相对于机器人坐标系的位姿分量，
Figure DEST_PATH_IMAGE017
是障碍物相对于机器人坐标系的位姿分量：
Figure 741729DEST_PATH_IMAGE018
（1-3）
S4中，对障碍物的尺寸进行膨胀处理，包括：
为了有效避免机器人本体与障碍物的碰撞，加入尺寸膨胀系数，将障碍物扩充为圆柱体，其半径为Robs，β为膨胀系数：
Figure DEST_PATH_IMAGE019
（1-4）
S5中，更具体的：
当有效障碍物被检测到后，为实现机器人的绕障动作，需要对原始路径进行再修正，修正的方式是将当前路径沿垂直路径的方向进行平移，平移至障碍物范围之外：
如图5所示，假定当前路径的起点为p0，终点为p1，机器人的当前点为pr，当前路径的方向矢量为（xl，yl），pt为机器人到直线的垂足，po为障碍物中心坐标，轨迹偏移量叠加方向为（-yl，xl）。
Figure 666960DEST_PATH_IMAGE020
为障碍物到路径的距离,则路径修正的可选偏移量,：
Figure DEST_PATH_IMAGE021
（1-5）
Figure 953585DEST_PATH_IMAGE022
（1-6）
通过对比计算得到障碍物边界数据，得出满足条件的偏移信息，取距离最短的偏移量作为路径的目标偏移数据。
S6中，路径参数的前瞻处理，包括：距离前瞻，速度前瞻。
距离前瞻
Figure DEST_PATH_IMAGE023
是根据障碍的位姿尺寸以及机器人本体的半径Rr，计算出目标轨迹的路程；速度前瞻是根据目标轨迹的路程，重新规划运行速度
Figure 77398DEST_PATH_IMAGE024
。如下式：
Figure 361749DEST_PATH_IMAGE025
为机器人最大速度，
Figure DEST_PATH_IMAGE026
预设缓速距离，
Figure 836593DEST_PATH_IMAGE027
机器人行驶加速度：
Figure DEST_PATH_IMAGE028
（1-7）
Figure 926909DEST_PATH_IMAGE029
（1-8）
S7中，运动学约束，包括：机器人的驱动过程中，根据目标路径点的相对位姿关系，为实现更好的轨迹跟踪，对机器人的线速度，角速度进行修正。如图6所示，θ为目标轨迹点相对于机器人的夹角，则修正目标线速度为
Figure 108491DEST_PATH_IMAGE009
，修正目标角速度为
Figure 298164DEST_PATH_IMAGE010
，
Figure DEST_PATH_IMAGE030
为预设调节参数：
Figure 260304DEST_PATH_IMAGE031
（1-9）
其中，
Figure 888731DEST_PATH_IMAGE011
为修正前的线速度，
Figure 190400DEST_PATH_IMAGE012
为修正前的角速度。
本发明所述的机器人绕障控制方法，包括基于视觉传感器的障碍判定，此处传感器并不仅限于激光雷达，也可以是超声波障碍识别等等；基于障碍信息加入无级降速判定区域，停止判定区域；在无级降速区域，车体速度由原速度，根据障碍相对距离，逐步修正车速为最小行驶速度；在机器人绕障功能触发后，以所述障碍信息的空间位姿，为实现绕障行驶，在垂直原轨迹方向上，在轨迹宽度参数约束范围内，创建新绕障轨迹，实现绕障轨迹重规划；为了实现对绕障轨迹的跟踪，加入对绕障轨迹的前瞻预处理，得到重新规划的轨迹运行参数；基于再规划的轨迹参数，建立目标轨迹与当车体前速度的运动学约束，对车体的运行速度进行修正，得到绕障速度；在绕障运行过程中，加入对第一预设及第二预设障碍检测，保障绕障过程的安全性；完成绕障后车体恢复对原始路径的跟踪控制。本发明适用于机器人的路径跟踪过程中的绕障，以及绕障过程中的速度规划步骤，可有效避免作业过程中发生碰撞。
实施例2：
本发明实施例2提供了一种机器人绕障控制系统，包括：
数据获取模块，被配置为：获取机器人运动前方的图像数据；
障碍物识别模块，被配置为：根据获取的图像数据，提取机器人运动前方的障碍物特征点，根据障碍物与检测区域的关系进行机器人降速控制，同时根据障碍物特征点得到障碍物的位姿和尺寸，并对障碍物的尺寸进行膨胀处理；
路径修正模块，被配置为：根据障碍物位姿及膨胀处理后的尺寸，在预设路径参数范围内，在原始路径方向上叠加偏移，修正机器人原行驶轨迹得到数条新的绕障路径；
运动学约束构建模块，被配置为：选择距离最短的绕障路径，并对路径参数进行前瞻预处理，得到目标轨迹的运行参数，驱动机器人沿目标轨迹运行过程中，根据目标轨迹在机器人坐标系下的相对位姿关系，对机器人的运行线速度按照速度方向与目标方位角，建立运动学约束关系；
机器人驱动模块，被配置为：按照约束后的速度驱动机器人运行，直至各个预设障碍物检测区域均未触发，等待机器人的移动距离达到目标运行距离后取消对原始轨迹的修偏量，恢复原路径运行。
更具体的，
更具体的，障碍物识别模块中，障碍物特征提取，包括：
通过ORB特征提取的方法，提取机器人前方的FAST特征点；将提取到的FAST特征点转换到机器人坐标系下，如果该特征点不在机器人的避障区域，则不处理。如果该特征点落在了机器人避障区域，则将该位姿转换到世界坐标系，并将障碍进行一次累计计数；如果连续3帧检测到的特征点均落在了相同位姿，则认为检测到了有效障碍。
分析障碍物的尺寸以及障碍物在机器人坐标系下的具体方位，包括：
尺寸分析及障碍物位姿判定：通过基于深度学习的YOLO目标检测方法，对识别数据集中障碍物进行检测，并确定障碍物检测边框；获取此时对应的激光雷达及ORB特征点数据，统计落在检测边框中的位姿数据，计算得出障碍物边界数据：
Figure DEST_PATH_IMAGE032
（2-1）
从而得出障碍物尺寸信息：
Figure 613291DEST_PATH_IMAGE033
（2-2）
其中，l obs 为障碍物在y轴方向的长度，w obs 为障碍物在x轴方向的长度，h obs 为障碍物在z轴方向的长度。
障碍物相对于机器人的位姿信息。由于机器人只是在平面内运动，只需要x，y及姿态角θ，
Figure DEST_PATH_IMAGE034
是检测传感器相对于机器人坐标系的位姿分量，
Figure 410DEST_PATH_IMAGE035
是障碍物相对于机器人坐标系的位姿分量：
Figure DEST_PATH_IMAGE036
（2-3）
对障碍物的尺寸进行膨胀处理，包括：
为了有效避免机器人本体与障碍物的碰撞，加入尺寸膨胀系数，将障碍物扩充为圆柱体，其半径为
Figure 432528DEST_PATH_IMAGE037
，β为膨胀系数：
Figure DEST_PATH_IMAGE038
（2-4）
根据障碍物与检测区域的关系进行机器人降速控制，包括：
如果障碍物进入到机器人的第一预设检测区域时，启动无级降速过程，如果障碍物处于机器人的第二预设区域，则机器人速度降为零。
路径修正模块，更具体的，包括：
当有效障碍物被检测到后，为实现机器人的绕障动作，需要对原始路径进行再修正，修正的方式是将当前路径沿垂直路径的方向进行平移，平移至障碍物范围之外：
如图5所示，假定当前路径的起点为p0，终点为p1，机器人的当前点为pr，当前路径的方向矢量为（xl，yl），pt为机器人到直线的垂足，po为障碍物中心坐标，轨迹偏移量叠加方向为（-yl，xl）。
Figure 385441DEST_PATH_IMAGE020
为障碍物到路径的距离,则路径修正的可选偏移量,：
Figure 182495DEST_PATH_IMAGE021
（2-5）
Figure 853648DEST_PATH_IMAGE022
（2-6）
通过对比计算得到障碍物边界数据，得出满足条件的偏移信息，取距离最短的偏移量作为路径的目标偏移数据。
运动学约束构建模块中，前瞻处理，包括：距离前瞻，速度前瞻。
距离前瞻
Figure 27141DEST_PATH_IMAGE039
是根据障碍的位姿尺寸以及机器人本体的半径Rr，计算出目标轨迹的路程；速度前瞻是根据目标轨迹的路程，重新规划运行速度
Figure DEST_PATH_IMAGE040
。如下式：
Figure 100139DEST_PATH_IMAGE041
为机器人最大速度，
Figure DEST_PATH_IMAGE042
为预设缓速距离，
Figure 68095DEST_PATH_IMAGE043
为机器人行驶加速度：
Figure DEST_PATH_IMAGE044
（2-7）
Figure 220684DEST_PATH_IMAGE045
（2-8）
运动学约束，包括：机器人的驱动过程中，根据目标路径点的相对位姿关系，为实现更好的轨迹跟踪，对机器人的线速度，角速度进行修正。如图6所示，θ为目标轨迹点相对于机器人的夹角，则修正目标线速度为
Figure 932288DEST_PATH_IMAGE009
，修正目标角速度为
Figure 859793DEST_PATH_IMAGE010
，
Figure 998650DEST_PATH_IMAGE030
为预设调节参数：
Figure 644395DEST_PATH_IMAGE031
（2-9）
其中，
Figure 894111DEST_PATH_IMAGE011
为修正前的线速度，
Figure 879385DEST_PATH_IMAGE012
为修正前的角速度。
机器人驱动模块，具体的，包括：
按照约束后的速度驱动运行，直至第一及第二预设检测区域均未触发，等待车体的移动距离达到目标运行距离后取消对原始轨迹的修偏量，重新调用速度运动学约束模型驱动运行，从而绕过障碍恢复实现原路径运行。
本发明所述的机器人绕障控制系统，包括基于视觉传感器的障碍判定，此处传感器并不仅限于激光雷达，也可以是超声波障碍识别等等；基于障碍信息加入无级降速判定区域，停止判定区域；在无级降速区域，车体速度由原速度，根据障碍相对距离，逐步修正车速为最小行驶速度；在机器人绕障功能触发后，以所述障碍信息的空间位姿，为实现绕障行驶，在垂直原轨迹方向上，在轨迹宽度参数约束范围内，创建新绕障轨迹，实现绕障轨迹重规划；为了实现对绕障轨迹的跟踪，加入对绕障轨迹的前瞻预处理，得到重新规划的轨迹运行参数；基于再规划的轨迹参数，建立目标轨迹与当车体前速度的运动学约束，对车体的运行速度进行修正，得到绕障速度；在绕障运行过程中，加入对第一预设及第二预设障碍检测，保障绕障过程的安全性；完成绕障后车体恢复对原始路径的跟踪控制。本发明适用于机器人的路径跟踪过程中的绕障，以及绕障过程中的速度规划步骤，可有效避免作业过程中发生碰撞。
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用硬件实施例、软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质（包括但不限于磁盘存储器和光学存储器等）上实施的计算机程序产品的形式。
本发明是参照根据本发明实施例的方法、设备（系统）、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体（Read-Only Memory，ROM）或随机存储记忆体（RandomAccessMemory，RAM）等。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种机器人绕障控制方法，其特征在于：
包括以下过程：
获取机器人运动前方的图像数据；
根据获取的图像数据，提取机器人运动前方的障碍物特征点，根据障碍物与检测区域的关系进行机器人降速控制，同时根据障碍物特征点得到障碍物的位姿和尺寸，并对障碍物的尺寸进行膨胀处理；
根据障碍物位姿及膨胀处理后的尺寸，在预设路径参数范围内，在原始路径方向上叠加偏移，修正机器人原行驶轨迹得到数条新的绕障路径；
选择距离最短的绕障路径，并对路径参数进行前瞻预处理，得到目标轨迹的运行参数，驱动机器人沿目标轨迹运行过程中，根据目标轨迹在机器人坐标系下的相对位姿关系，对机器人的运行线速度按照速度方向与目标方位角，建立运动学约束关系；
按照约束后的速度驱动机器人运行，直至各个预设障碍物检测区域均未触发，等待机器人的移动距离达到目标运行距离后取消对原始轨迹的修偏量，恢复原路径运行。
2.如权利要求1所述的机器人绕障控制方法，其特征在于：
根据障碍物特征点得到障碍物的位姿和尺寸，包括：
统计落在检测边框中的障碍物位姿数据，计算得出障碍物边界数据，从而得出障碍物尺寸；根据检测传感器相对于机器人坐标系的位姿数据以及障碍物边界数据，得到障碍物相对于机器人坐标系的位姿。
3.如权利要求2所述的机器人绕障控制方法，其特征在于：
障碍物相对于机器人坐标系的位姿，包括：
Figure DEST_PATH_IMAGE001
其中，（xr，yr，θr）是检测传感器相对于机器人坐标系的位姿分量，xmax、xmin、ymax和ymin为障碍物边界数据。
4.如权利要求3所述的机器人绕障控制方法，其特征在于：
对障碍物的尺寸进行膨胀处理，包括：
加入尺寸膨胀系数，将障碍物扩充为圆柱体，圆柱体的截面半径为Robs，β为膨胀系数，则：
Figure 363984DEST_PATH_IMAGE002
其中，l obs 为障碍物在y轴方向的长度，w obs 为障碍物在x轴方向的长度。
5.如权利要求4所述的机器人绕障控制方法，其特征在于：
在预设路径参数范围内，在原始路径方向上叠加偏移，修正机器人原行驶轨迹得到数条新的绕障路径，包括：
假定当前路径的起点为p 0 ，终点为p 1 ，机器人的当前点为p r ，当前路径的方向矢量为（xl，yl），pt为机器人到p0 p1直线的垂足点，p o 为障碍物中心坐标，轨迹偏移量叠加方向为（-yl，xl）；
Figure DEST_PATH_IMAGE003
为障碍物到路径的距离；
Figure 701425DEST_PATH_IMAGE004
；
则路径修正的可选偏移量为：
Figure DEST_PATH_IMAGE005
取距离最短的偏移量作为路径的目标偏移数据。
6.如权利要求4所述的机器人绕障控制方法，其特征在于：
对路径参数进行前瞻预处理，包括：距离前瞻和速度前瞻；
距离前瞻，包括：
Figure 344896DEST_PATH_IMAGE006
速度前瞻，包括：
Figure DEST_PATH_IMAGE007
其中，Rr为机器人本体的半径，vmax为机器人最大速度，lslow为预设缓速距离，acc为机器人行驶加速度。
7.如权利要求4所述的机器人绕障控制方法，其特征在于：
运动学约束，包括：对机器人的线速度和角速度进行修正，包括：
Figure 742379DEST_PATH_IMAGE008
其中，θ为目标轨迹点相对于机器人的夹角，
Figure DEST_PATH_IMAGE009
为修正目标线速度，
Figure 533617DEST_PATH_IMAGE010
为修正目标角速度，k1、k2和k3为预设调节参数，
Figure DEST_PATH_IMAGE011
为修正前的线速度，
Figure 674749DEST_PATH_IMAGE012
为修正前的角速度。
8.如权利要求1所述的机器人绕障控制方法，其特征在于：
根据障碍物与检测区域的关系进行机器人降速控制，包括：
如果障碍物进入到机器人的第一预设检测区域时，启动无级降速过程，如果障碍物处于机器人的第二预设区域，则机器人速度降为零。
9.一种机器人绕障控制系统，其特征在于：
包括：
数据获取模块，被配置为：获取机器人运动前方的图像数据；
障碍物识别模块，被配置为：根据获取的图像数据，提取机器人运动前方的障碍物特征点，根据障碍物与检测区域的关系进行机器人降速控制，同时根据障碍物特征点得到障碍物的位姿和尺寸，并对障碍物的尺寸进行膨胀处理；
路径修正模块，被配置为：根据障碍物位姿及膨胀处理后的尺寸，在预设路径参数范围内，在原始路径方向上叠加偏移，修正机器人原行驶轨迹得到数条新的绕障路径；
运动学约束构建模块，被配置为：选择距离最短的绕障路径，并对路径参数进行前瞻预处理，得到目标轨迹的运行参数，驱动机器人沿目标轨迹运行过程中，根据目标轨迹在机器人坐标系下的相对位姿关系，对机器人的运行线速度按照速度方向与目标方位角，建立运动学约束关系；
机器人驱动模块，被配置为：按照约束后的速度驱动机器人运行，直至各个预设障碍物检测区域均未触发，等待机器人的移动距离达到目标运行距离后取消对原始轨迹的修偏量，恢复原路径运。
10.如权利要求9所述的机器人绕障控制系统，其特征在于：
根据障碍物特征点得到障碍物的位姿和尺寸，包括：
统计落在检测边框中的障碍物位姿数据，计算得出障碍物边界数据，从而得出障碍物尺寸；根据检测传感器相对于机器人坐标系的位姿数据以及障碍物边界数据，得到障碍物相对于机器人坐标系的位姿；
障碍物相对于机器人坐标系的位姿，包括：
Figure 172726DEST_PATH_IMAGE001
其中，（xr，yr，θr）是检测传感器相对于机器人坐标系的位姿分量，xmax、xmin、ymax和ymin为障碍物边界数据。
CN202210808876.3A 2022-07-11 2022-07-11 一种机器人绕障控制方法及系统 Active CN114879704B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210808876.3A CN114879704B (zh) 2022-07-11 2022-07-11 一种机器人绕障控制方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210808876.3A CN114879704B (zh) 2022-07-11 2022-07-11 一种机器人绕障控制方法及系统

Publications (2)

Publication Number Publication Date
CN114879704A true CN114879704A (zh) 2022-08-09
CN114879704B CN114879704B (zh) 2022-11-25

Family

ID=82683644

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210808876.3A Active CN114879704B (zh) 2022-07-11 2022-07-11 一种机器人绕障控制方法及系统

Country Status (1)

Country Link
CN (1) CN114879704B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115509260A (zh) * 2022-10-25 2022-12-23 广东汇天航空航天科技有限公司 轨迹规划方法、装置、设备及存储介质
CN116149339A (zh) * 2023-04-21 2023-05-23 武汉奋进智能机器有限公司 轨道设备的行走纠偏方法、设备、介质及轨道设备系统
CN116552261A (zh) * 2023-06-02 2023-08-08 陕西长瑞安驰信息技术集团有限公司 一种锥桶移动机器人及其应用
WO2024113261A1 (zh) * 2022-11-30 2024-06-06 汤恩智能科技(上海)有限公司 机器人及其避障方法，机器人系统及存储介质

Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2001333823A (ja) * 2000-05-29 2001-12-04 Daifuku Co Ltd 移動棚設備の障害物検出装置
CN105549597A (zh) * 2016-02-04 2016-05-04 同济大学 一种基于环境不确定性的无人车动态路径规划方法
CN109886215A (zh) * 2019-02-26 2019-06-14 常熟理工学院 基于机器视觉的低速园区无人车巡航及紧急制动系统
CN110550029A (zh) * 2019-08-12 2019-12-10 华为技术有限公司 障碍物避让方法及装置
CN111123952A (zh) * 2019-12-31 2020-05-08 华为技术有限公司 一种轨迹规划方法及装置
CN111338361A (zh) * 2020-05-22 2020-06-26 浙江远传信息技术股份有限公司 低速无人车避障方法、装置、设备及介质
US20200394924A1 (en) * 2017-10-26 2020-12-17 Guangzhou Xaircraft Technology Co., Ltd Method for Configuring Navigation Chart, Method for Avoiding Obstacle and Device, Terminal and Unmanned Aerial Vehicle
CN112837554A (zh) * 2021-03-09 2021-05-25 济南大学 基于双目相机的agv定位导航方法及系统
CN112987763A (zh) * 2021-05-11 2021-06-18 南京理工大学紫金学院 一种基于ros的自主导航机器人控制系统的智能小车
CN113391642A (zh) * 2021-05-28 2021-09-14 西南交通大学 一种基于单目视觉的无人机自主避障方法及系统
CN113848933A (zh) * 2021-10-11 2021-12-28 徐州徐工环境技术有限公司 一种清扫机器人的全方位避障方法及装置
CN114200945A (zh) * 2021-12-13 2022-03-18 哈尔滨工业大学芜湖机器人产业技术研究院 一种移动机器人的安全控制方法
CN114355923A (zh) * 2021-12-28 2022-04-15 杭州电子科技大学 一种a*引导下基于mpc的轨迹规划及跟踪方法
CN114829221A (zh) * 2019-10-16 2022-07-29 洛科梅森公司 基于视觉的跟随引领者横向控制器
CN114859914A (zh) * 2022-05-09 2022-08-05 广东利元亨智能装备股份有限公司 障碍物检测方法、装置、设备及存储介质

Patent Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2001333823A (ja) * 2000-05-29 2001-12-04 Daifuku Co Ltd 移動棚設備の障害物検出装置
CN105549597A (zh) * 2016-02-04 2016-05-04 同济大学 一种基于环境不确定性的无人车动态路径规划方法
US20200394924A1 (en) * 2017-10-26 2020-12-17 Guangzhou Xaircraft Technology Co., Ltd Method for Configuring Navigation Chart, Method for Avoiding Obstacle and Device, Terminal and Unmanned Aerial Vehicle
CN109886215A (zh) * 2019-02-26 2019-06-14 常熟理工学院 基于机器视觉的低速园区无人车巡航及紧急制动系统
CN110550029A (zh) * 2019-08-12 2019-12-10 华为技术有限公司 障碍物避让方法及装置
CN114829221A (zh) * 2019-10-16 2022-07-29 洛科梅森公司 基于视觉的跟随引领者横向控制器
CN111123952A (zh) * 2019-12-31 2020-05-08 华为技术有限公司 一种轨迹规划方法及装置
CN111338361A (zh) * 2020-05-22 2020-06-26 浙江远传信息技术股份有限公司 低速无人车避障方法、装置、设备及介质
CN112837554A (zh) * 2021-03-09 2021-05-25 济南大学 基于双目相机的agv定位导航方法及系统
CN112987763A (zh) * 2021-05-11 2021-06-18 南京理工大学紫金学院 一种基于ros的自主导航机器人控制系统的智能小车
CN113391642A (zh) * 2021-05-28 2021-09-14 西南交通大学 一种基于单目视觉的无人机自主避障方法及系统
CN113848933A (zh) * 2021-10-11 2021-12-28 徐州徐工环境技术有限公司 一种清扫机器人的全方位避障方法及装置
CN114200945A (zh) * 2021-12-13 2022-03-18 哈尔滨工业大学芜湖机器人产业技术研究院 一种移动机器人的安全控制方法
CN114355923A (zh) * 2021-12-28 2022-04-15 杭州电子科技大学 一种a*引导下基于mpc的轨迹规划及跟踪方法
CN114859914A (zh) * 2022-05-09 2022-08-05 广东利元亨智能装备股份有限公司 障碍物检测方法、装置、设备及存储介质

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
JUN ZHOU 等: "《Data-Driven Driving StAgricultural Logistics Vehicleate Control for Unmanned 》", 《IEEE》 *
MUHAMMAD FUAD 等: "《Autonomous Indoor Vehicle Navigation Using Modified Steering Velocity Obstacles》", 《2020 INTERNATIONAL SEMINAR ON INTELLIGENT TECHNOLOGY AND ITS APPLICATIONS》 *
SAMWEL OPIYO 等: "《Medial axis-based machine-vision system for orchard robot navigation》", 《COMPUTERS ANDELECTRONICSINAGRICULTURE》 *
王刚 等: "《家庭服务机器人自主定位控制》", 《计算技术与自动化》 *
胡孟慧 等: "《基于改进式RRT的手术机器人在线避障运动规划》", 《电动工具》 *
莫冬炎 等: "《基于环境感知的果园机器人自主导航技术研究进展》", 《机电工程技术》 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115509260A (zh) * 2022-10-25 2022-12-23 广东汇天航空航天科技有限公司 轨迹规划方法、装置、设备及存储介质
WO2024113261A1 (zh) * 2022-11-30 2024-06-06 汤恩智能科技(上海)有限公司 机器人及其避障方法，机器人系统及存储介质
CN116149339A (zh) * 2023-04-21 2023-05-23 武汉奋进智能机器有限公司 轨道设备的行走纠偏方法、设备、介质及轨道设备系统
CN116149339B (zh) * 2023-04-21 2023-07-18 武汉奋进智能机器有限公司 轨道设备的行走纠偏方法、设备、介质及轨道设备系统
CN116552261A (zh) * 2023-06-02 2023-08-08 陕西长瑞安驰信息技术集团有限公司 一种锥桶移动机器人及其应用
CN116552261B (zh) * 2023-06-02 2023-10-10 陕西长瑞安驰信息技术集团有限公司 一种锥桶移动机器人及其应用

Also Published As

Publication number Publication date
CN114879704B (zh) 2022-11-25

Similar Documents

Publication Publication Date Title
CN114879704A (zh) 一种机器人绕障控制方法及系统
CN110023164B (zh) 车辆运动控制装置、车辆运动控制方法和车辆运动控制系统
CN111830979B (zh) 一种轨迹优化方法和装置
WO2021135728A1 (zh) 一种自动驾驶车辆碰撞预测判断方法及装置
CN107264531B (zh) 一种半结构化环境中智能车辆自主换道超车运动规划方法
WO2022053026A1 (zh) 一种自动驾驶会车场景处理方法及其装置、车辆、存储介质
US9235767B2 (en) Detection region modification for driving assistance apparatus and driving assistance method
JP6067623B2 (ja) 走行制御装置
JP6959056B2 (ja) 移動ロボットの制御装置と制御方法
US11814072B2 (en) Method and system for conditional operation of an autonomous agent
CN110361013A (zh) 一种用于车辆模型的路径规划系统及方法
US11914386B2 (en) Mobile object control device, mobile object control method, and storage medium
KR20160070467A (ko) 장애물 회피를 위한 멀티로봇 시스템 및 장애물 회피를 위한 스위칭 대형 전략을 이용하는 방법
US12037014B2 (en) Method for determining an avoidance path of a motor vehicle
KR102057428B1 (ko) 차량의 주행 제어 방법 및 주행 제어 장치
CN116513245B (zh) 一种基于行车环境的快速自动驾驶轨迹规划方法
US20220019228A1 (en) Mobile object control method, mobile object control device, mobile object and storage medium
CN114647245B (zh) 一种基于人工势场的无人车弯道避障路径规划方法
JP7584313B2 (ja) 移動体制御装置、移動体制御方法、およびプログラム
JP2022181105A (ja) 移動体、制御装置及びそのプログラム
US12128883B2 (en) Driving assistance device, monitoring device, driving assistance method, and storage medium
JP2017132422A (ja) 車両制御システム
US11697413B2 (en) Vehicle control device
JP2007331459A (ja) 車両用制動力制御装置および車両用制動力制御方法
RU2783780C1 (ru) Способ для адаптивного управления характеристиками движения высокоавтоматизированного транспортного средства в зависимости от уровня неопределенности в оценке окружающей ситуации

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