机器人的运动规划方法和自动引导车与流程

1.本发明大致涉及机器人领域，尤其涉及机器人的运动规划方法以及自动引导车。


背景技术：

2.自20世纪50年代agv诞生以来，因其在货物运输、工业制造、产品装配等领域的广泛应用受到了越来越多的关注和研究。随着agv的发展，运动的柔性和平滑性显得越来越重要。曲线运动规划的主要目的是让机器人在复杂的运动场景中的平滑的衔接路径。作为移动agv运动控制的最基本环节之一，曲线运动规划结果的优劣直接决定着移动机器人的实用性及综合性能。
3.现有技术没有针对曲线起点和终点位置的速度进行平滑过度的方案，以及在曲线运动中的平稳避障和再运动过程。
4.背景技术部分的内容仅仅是发明人所知晓的技术，并不当然代表本领域的现有技术。


技术实现要素：

5.针对现有技术的至少一个问题，本发明实施例提一种机器人的运动规划方法，包括：
6.s101：获得第一路径和第二路径，其中所述第一路径和第二路径不共线；
7.s102：确定过渡路径的起点、终点、第一控制点和第二控制点，其中所述起点位于所述第一路径上，所述终点位于所述第二路径上，所述第一控制点位于所述第一路径和第二路径的交点，所述第二控制点位于所述起点与所述第一控制点之间；
8.s103：根据所述起点、终点、第一控制点和第二控制点，确定贝塞尔曲线；
9.s104：控制所述机器人按照所述贝塞尔曲线从所述第一路径运动到第二路径，并使得所述在所述起点和终点之间的速度和角速度连续。
10.根据本发明的一个方面，所述步骤s102还包括：确定所述过渡路径的第三控制点，所述第三控制点位于所述第一控制点与所述终点之间，
11.所述步骤s103包括：根据所述起点、终点、第一控制点、第二控制点和第三控制点，确定所述贝塞尔曲线。
12.根据本发明的一个方面，所述的运动规划方法还包括：根据所述机器人在所述起点和终点的速度，设置所述机器人在所述过渡路径上的加速和减速。
13.根据本发明的一个方面，所述的运动规划方法还包括：：
14.检测所述机器人前方的障碍物；
15.当所述机器人前方存在障碍物时，控制所述机器人速度减为零；当所述障碍物消除时，将所述机器人的速度加速到最大速度。
16.根据本发明的一个方面，所述的运动规划方法还包括：
17.获得所述机器人的当前位置和航向角；
18.根据所述贝塞尔曲线，获得所述机器人的参考位置和参考航向角；
19.获得所述机器人的位置偏差和航向角偏差；
20.通过pid控制，纠正所述机器人的位置偏差和航向角偏差。
21.根据本发明的一个方面，所述第二控制点和第三控制点选择成使得所述贝塞尔曲线的中间的曲率满足k(0.5)＜k。
22.本发明还提供一种自动引导车，包括：
23.车体；
24.驱动装置，所述驱动装置安装在所述车体上，并配置成可驱动自动引导车行进；
25.控制装置，所述控制装置配置成执行以下步骤：
26.s101：获得第一路径和第二路径，其中所述第一路径和第二路径不共线；
27.s102：确定过渡路径的起点、终点、第一控制点和第二控制点，其中所述起点位于所述第一路径上，所述终点位于所述第二路径上，所述第一控制点位于所述第一路径和第二路径的交点，所述第二控制点位于所述起点与所述第一控制点之间；
28.s103：根据所述起点、终点、第一控制点和第二控制点，确定贝塞尔曲线；
29.s104：控制所述自动引导车按照所述贝塞尔曲线从所述第一路径运动到第二路径，并使得所述在所述起点和终点之间的速度和角速度连续。
30.根据本发明的一个方面，所述步骤s102还包括：确定所述过渡路径的第三控制点，所述第三控制点位于所述第一控制点与所述终点之间，
31.所述步骤s103包括：根据所述起点、终点、第一控制点、第二控制点和第三控制点，确定所述贝塞尔曲线。
32.根据本发明的一个方面，所述控制装置还配置成根据所述机器人在所述起点和终点的速度，设置所述机器人在所述过渡路径上的加速和减速。
33.根据本发明的一个方面，所述的自动引导车还包括激光雷达，所述激光雷达配置成可检测所述机器人前方的障碍物；所述控制装置与所述激光雷达通讯，并配置成当所述机器人前方存在障碍物时，控制所述机器人速度减为零；当所述障碍物消除时，将所述机器人的速度加速到最大速度。
34.根据本发明的一个方面，所述的自动引导车还包括安装在所述车体上的惯性测量单元，所述惯性测量单元配置成可测量所述机器人的当前位置和航向角。另外，所述机器人的位置和航向角也可以通过全局导航方法来确定，如反光板、slam等。等所述控制装置与所述惯性测量单元通讯，并配置成；
35.根据所述贝塞尔曲线，获得所述机器人的参考位置和参考航向角；
36.获得所述机器人的位置偏差和航向角偏差；
37.通过pid控制，纠正所述机器人的位置偏差和航向角偏差。
38.根据本发明的一个方面，所述第二控制点和第三控制点选择成使得所述贝塞尔曲线的中间的曲率满足k(0.5)＜k。
39.本发明的实施例可以让机器人在复杂的运动场景中具有平滑的衔接路径，并且进行平稳避障。
40.本发明的实施例中，机器人或者自动引导车沿贝塞尔曲线运动，在轨迹的起点和终点以及中间点处处速度和角速度连续，加速度没有跳变，使得小车运动平滑；机器人可以
有效避障，遇到障碍物时，机器人能够平稳的减速并停下，障碍物消失后，机器人逐渐恢复形势速度；另外在曲线运动过程中引入pid反馈控制，对机器人的横向偏差和角度偏差进行收敛，确保机器人的运动路径的正确性。
附图说明
41.构成本公开的一部分的附图用来提供对本公开的进一步理解，本公开的示意性实施例及其说明用于解释本公开，并不构成对本公开的不当限定。在附图中：
42.图1示出了根据本发明一个实施例的可用于机器人的运动规划方法；
43.图2示出了根据图1所示方法进行运动规划的图示说明；
44.图3示出了根据本发明一个优选实施例的运动规划方法。
具体实施方式
45.在下文中，仅简单地描述了某些示例性实施例。正如本领域技术人员可认识到的那样，在不脱离本发明的精神或范围的情况下，可通过各种不同方式修改所描述的实施例。因此，附图和描述被认为本质上是示例性的而非限制性的。
46.在本发明的描述中，需要理解的是，术语"中心"、"纵向"、"横向"、"长度"、"宽度"、"厚度"、"上"、"下"、"前"、"后"、"左"、"右"、"竖直"、"水平"、"顶"、"底"、"内"、"外"、"顺时针"、"逆时针"等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语"第一"、"第二"仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有"第一"、"第二"的特征可以明示或者隐含地包括一个或者更多个所述特征。在本发明的描述中，"多个"的含义是两个或两个以上，除非另有明确具体的限定。
47.在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语"安装"、"相连"、"连接"应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接:可以是机械连接，也可以是电连接或可以相互通讯；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
48.在本发明中，除非另有明确的规定和限定，第一特征在第二特征之"上"或之"下"可以包括第一和第二特征直接接触，也可以包括第一和第二特征不是直接接触而是通过它们之间的另外的特征接触。而且，第一特征在第二特征"之上"、"上方"和"上面"包括第一特征在第二特征正上方和斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征"之下"、"下方"和"下面"包括第一特征在第二特征正上方和斜上方，或仅仅表示第一特征水平高度小于第二特征。
49.下文的公开提供了许多不同的实施方式或例子用来实现本发明的不同结构。为了简化本发明的公开，下文中对特定例子的部件和设置进行描述。当然，它们仅仅为示例，并且目的不在于限制本发明。此外，本发明可以在不同例子中重复参考数字和/或参考字母，这种重复是为了简化和清楚的目的，其本身不指示所讨论各种实施方式和/或设置之间的关系。此外，本发明提供了的各种特定的工艺和材料的例子，但是本领域普通技术人员可以
意识到其他工艺的应用和/或其他材料的使用。
50.以下结合附图对本发明的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。
51.图1示出了根据本发明一个实施例的可用于机器人的运动规划方法100，图2示出了运用所述运动规划方法100进行运动规划的图示说明。下面参考附图详细描述。
52.在步骤s101：获得第一路径和第二路径，其中所述第一路径和第二路径不共线。
53.如图2所示，第一路径r1和第二路径r2例如为机器人执行任务过程中必然经过的路径，可以由上位机来进行规划。例如机器人在执行搬运或者拣选任务过程中所需要经过的路径。第一路径r1和第二路径r2优选均为直线路径，如图2中所示的。本领域技术人员容易理解，本发明不限于此，第一路径r1和第二路径r2也可以为曲线路径，这些都在本发明的保护范围内。在第一路径r1和第二路径r2均为直线路径的情况下，如何在第一和第二路径之间进行平稳的过渡，对于机器人运动规划是至关重要的问题。另外，本发明所述的机器人优选为自动引导车agv。
54.在步骤s102：确定过渡路径的起点、终点、第一控制点和第二控制点，其中所述起点位于所述第一路径上，所述终点位于所述第二路径上，所述第一控制点位于所述第一路径和第二路径的交点，所述第二控制点位于所述起点与所述第一控制点之间。
55.优选的，所述步骤s102还包括：确定所述过渡路径的第三控制点，所述第三控制点位于所述第一控制点与所述终点之间。
56.图2中示出了过渡路径的起点s、终点e以及第一控制点p1、第二控制点p2和第三控制点p3。其中起点s位于所述第一路径r1上，所述终点e位于所述第二路径r2上，所述第一控制点p1位于所述第一路径r1和第二路径r2的交点，所述第二控制点p2位于所述起点s与所述第一控制点p1之间，第三控制点p3位于第一控制点p1与所述终点e之间。
57.在选择所述起点和终点时，要保证起点终点之间的曲线绕开墙角、柱子等固定障碍物。
58.另外，在选取控制点时，起点s和第二控制点p2在第一条路径所在的直线上，终点和第三控制点p3在第二条路径所在直线上，第一控制点p1在两条路径的交点，在满足曲线最大曲率不超过转向角约束情况下，两个控制点p1和p2越接近控制点p3越好。根据机器人的硬件底盘特性，通常需要限制其转弯的曲率，这个曲率就是曲线允许的最大曲率。
59.在步骤s103：根据所述起点、终点、第一控制点和第二控制点，确定贝塞尔曲线。
60.在步骤s102中确定了第三控制点的情况下，所述步骤s103包括：根据所述起点、终点、第一控制点、第二控制点和第三控制点，确定所述贝塞尔曲线。
61.在起点、终点、第一控制点、第二控制点和第三控制点被确定的情况下，就能够确定贝塞尔曲线的唯一形状，例如四阶贝塞尔曲线，作为所述过渡路径，下文将给出一个详细的计算实施例以用于说明。
62.在步骤s104：控制所述机器人按照所述贝塞尔曲线从所述第一路径运动到第二路径，并使得所述在所述起点和终点之间的速度和角速度连续。
63.在确定了贝塞尔曲线之后，控制机器人沿着该贝塞尔曲线完成从第一路径r1到第二路径r2的过渡，并且在运动过程中保持机器人的速度和角速度的连续，在过渡路径的起点和终点以及中间点处处速度和角速度连续，加速度没有跳变，使得机器人的运动平滑。
64.上面描述的实施例中包括三个控制点，最终产生了四阶贝塞尔曲线作为过渡路径，本发明不限于此，控制点的数目也可以是两个、四个或更多，从而贝塞尔曲线的阶数也可以相应变化，这些都在本发明的保护范围内。
65.根据本发明的一个实施例，所述运动规划方法还包括：根据所述机器人在所述起点和终点的速度，设置所述机器人在所述过渡路径上的加速和减速。
66.下面以四阶贝塞尔曲线为例给出一个具体的计算实施例。贝塞尔曲线是平面上具有几个控制点约束形状的曲线，四阶贝塞尔由起点终点以及三个控制点确定了唯一形状。四阶贝塞尔曲线上的点计算公式通过下面的参数方程来表示：
[0067][0068]
式中，a
x
、b
x
、c
x
、ay、by、cy是根据起点和控制点算出的系数，可以通过下式计算：
[0069][0070]
式中，xs、ys、xe、ye、x
p2
、y
p2
、x
p3
、y
p3
是起点、终点、第二控制点和第三控制点的横纵坐标，其中μ是时间变量，取值0～1，将μ的拆成微小单元δμs作为机器人插补运动的周期。为了使机器人进入过渡曲线时的速度和角速度连续，需要计算出起始时间微段δμs。已知曲线的起点为s，根据δμs求出下一时刻小车位置x、y，求出两点的距离和转向角，进而计算出小车进入曲线的初速度和角速度。同样的，根据曲线的末速度可以计算出δμe，根据小车在曲线中的最大速度可以求出δμ
max
，对δμ进行梯形曲线规划(例如匀加速匀减速)，可以得到δμ随时间变化的曲线。根据μ时刻和μ δμ可以求出曲线上两个时间点的位置，根据两个点的运动的距离和旋转的角度，求出小车的速度和角速度，也就规划出了小车的运动曲线。规划小车的运动曲线，就是对位置点和速度角速度进行规划，有了这三个量就能让机器人按照这个曲线按照设定的速度运动。本发明中，机器人的初速度、末速度和最大速度都可以是给定的。
[0071]
另外，在进行路径规划时，轨迹应当具有平滑度，也就是在路径衔接处不应该有拐角，而应该是平滑的过渡。假设第一路径和第二路径都为直线，贝塞尔曲线作为过渡路径，连接第一路径r1和第二路径r2。在选取贝塞尔曲线的控制点时，起点和终点必须是路径的衔接点，第二和第三控制点p2、p3分别在第一路径r1和第二路径r2的延长线上，第一控制点p1在两条路径的延长线的交点处，在满足曲线最大曲率不超过小车转向角约束k情况下，两个控制点p2、p3越接近控制点p1越好。曲线在时间μ处的曲率为
[0072][0073]
式中，x'(μ)、y'(μ)、x”(μ)、y”(μ)分别是x(μ)和y(μ)在的一阶导数和二阶导数。最
大曲率处是曲线中间点，此时
[0074][0075]
控制点p1和p2选取时满足k(0.5)＜k，其中k是机器人的硬件底盘所允许的最大曲率。
[0076]
在确定了贝塞尔曲线的起点终点以及控制点之后，就确定了贝塞尔曲线的形状，接下来就是对机器人的速度进行规划，以及在曲线中运动时的避障处理。由贝塞尔曲线的公式可知，曲线上的点是根据自变量μ计算出来的，将μ拆成很多个微段δμ进行，作为小车插补运动的周期，根据小车每个周期移动距离和角度计算出小车的速度和角速度，实现对小车的控制。
[0077]
根据机器人在运动过程中的平滑性要求，在路径衔接处的速度要连续，在求取曲线初速度时候优选明确曲线第一个插补周期的长度等于机器人在当前速度下单个周期的位移，所以贝塞尔曲线的第一个插补时间微段δμ1按照下列公式计算：
[0078]
(a
x
·
δμ3 b
x
·
δμ2 c
x
·
δμ)2 (ay·
δμ3 by·
δμ2 cy·
δμ)2＝v
2s
·
t
2period
[0079]
式中vs是机器人进入过渡曲线时的初速度，t
period
是机器人运动规划的周期。上述方程为9次方程，通过二分法可以求出δμs在0～1区间的唯一解。同样的，可以根据小车曲线段的末速度ve和v
max
求出δμe和δμ
max
，δμ的值通过下式计算：
[0080][0081]
式中，δμ
last
等于上一时刻的δμ，δμ
acc
表示对δμ进行梯形曲线规划的加速度，通过下式公式计算：
[0082][0083]
式中，a为机器人直线运动时加速度。
[0084]
有了δμ的实时规划，就可以求出当前μ的值，带入贝塞尔曲线的公式，结算出当前点的坐标值。于是，小车的速度和角速度通过下式计算：
[0085][0086]
图3示出了根据本发明一个优选实施例的运动规划方法(或运动控制方法)200，下面参考图3详细描述。
[0087]
在步骤s201，机器人获得第一直线路径r1和第二直线路径r2，二者不共线。机器人例如可以从上位机获得所述第一直线路经和第二直线路径。
[0088]
在步骤s202，确定过渡路径的起点、终点、以及控制点，如上文参考图1和图2描述的起点s、终点e以及控制点p1、p2和p3。起点、终点、以及控制点被确定以后，过渡路径遵循的贝塞尔曲线也将被确定。
[0089]
在步骤s203，根据机器人进入过渡路径和离开过渡路径时的速度确定机器人的初速度、末速度，对机器人的运动进行加减速规划。
[0090]
然后机器人开始按照确定好的贝塞尔曲线和加减速规划方案运动，并且在运动过程中持续监测在运动轨迹的前方是否遇到障碍物(s204)，例如可通过摄像头 图像处理的方式，或者通过激光雷达的方式，来确定前方是否有障碍物。
[0091]
当在步骤s204检测到障碍物时，进行到步骤s205，速度减小为0。否则，保持加速，直至速度增加至最大值并保持(步骤s206)。速度减小为0后，持续监测前方障碍物是否消失(s207)。当障碍物持续存在时，速度维持0(步骤s208)，持续监测，直至障碍物消失。当障碍物消失时，开始加速，直至增加至最大值(步骤s209)。如果机器人在运动过程中遇到障碍物，令δμ逐渐减小到零，此时有δμ＝δμ
last-δμ
acc
(δμ＞0)，当δμ逐渐减小到0时，小车按照加速a逐渐停下；当障碍物消失时，δμ逐渐增加到最大值δμ
max
，此时有δμ＝δμ
last
δμ
acc
(δμ＜δμ
max
)，小车按照加速a逐渐加速到最大速度。
[0092]
然后持续判断机器人与过渡路径的终点之间的距离是否达到开始减速的值(例如小于预设值)(步骤s210)，当已经达到时，机器人的速度逐渐减小至末速度(s211)；否则维持加速或者维持最大速度(s212)，直到机器人与过渡路径的终点之间的距离达到开始减速的值。
[0093]
上述方法200中，机器人在曲线运动过程中可以根据雷达的障碍物信息进行避障，出现障碍物时，δμ按照梯形曲线减为0，相应的速度也会减为0，障碍物消失，δμ再逐渐加大到δμ
max
，这样就可以实现机器人在曲线段运动过程中的避障以及继续行走功能。本发明中，速度和这个u变量是正相关的，当u为0时速度也为0。
[0094]
根据本发明的一个实施例，所述的运动规划方法200还包括：
[0095]
获得所述机器人的当前位置和航向角；
[0096]
根据所述贝塞尔曲线，获得所述机器人的参考位置和参考航向角；
[0097]
获得所述机器人的位置偏差和航向角偏差；
[0098]
通过pid控制，纠正所述机器人的位置偏差和航向角偏差。
[0099]
下面详细描述。在运动过程中，为了保证运动的准确性，可以引入pid控制。反馈控制策略针对机器人运动的横向误差和朝向角度误差。将μ带入四阶贝塞尔曲线公式计算出在全局坐标系下小车参考点位置(x
μ
,y
μ
)，机器人当前的实际位置为(x0,y0)，通过坐标变换得到参考点在小车坐标系下的坐标值以及角度为：
[0100][0101]
将横向误差offsetx＝-x、纵向误差offsety＝-y以及角度误差artctan(y,x)引入pid控制纠偏之后计算出角速度和速度补偿值：
[0102][0103]
上述方法不需引入微分控制环节，式中，ka、kb分别是是角度偏差和横向偏差的比例系数，ka kb＝1，通过设置两者的占比可以调节小车对于角度偏差和横向偏差的调节速
度，p、i分别是比例控制系数和积分控制系数。
[0104]
本发明的实施例中通过引入pid控制器实时的对机器人运动轨迹进行纠偏。机器人运行过程中的误差主要有三方面：纵向误差、横向误差和角度误差。将μ带入贝塞尔曲线的公式可以计算出小车的参考点坐标，根据小车导航信息获取小车当前位置，通过坐标变换计算出参考点在小车坐标系下的坐标值。以小车前进方向为x正方向，横向误差，纵向误差以及角度误差就分别为y、x、arctan(y,x)，经过pid控制纠偏后最终输出角速度和前进方向线速度。
[0105]
本发明还涉及一种自动引导车，包括：
[0106]
车体；
[0107]
驱动装置，所述驱动装置安装在所述车体上，并配置成可驱动自动引导车行进；
[0108]
控制装置，所述控制装置配置成执行以下步骤：
[0109]
s101：获得第一路径和第二路径，其中所述第一路径和第二路径不共线；
[0110]
s102：确定过渡路径的起点、终点、第一控制点和第二控制点，其中所述起点位于所述第一路径上，所述终点位于所述第二路径上，所述第一控制点位于所述第一路径和第二路径的交点，所述第二控制点位于所述起点与所述第一控制点之间；
[0111]
s103：根据所述起点、终点、第一控制点和第二控制点，确定贝塞尔曲线；
[0112]
s104：控制所述自动引导车按照所述贝塞尔曲线从所述第一路径运动到第二路径，并使得所述在所述起点和终点之间的速度和角速度连续。
[0113]
所述控制装置与所述驱动装置可以相互通信，从而控制所述驱动装置来控制自动引导车的运动。
[0114]
根据本发明的一个实施例，所述步骤s102还包括：确定所述过渡路径的第三控制点，所述第三控制点位于所述第一控制点与所述终点之间，
[0115]
所述步骤s103包括：根据所述起点、终点、第一控制点、第二控制点和第三控制点，确定所述贝塞尔曲线。
[0116]
根据本发明的一个实施例，所述控制装置还配置成根据所述机器人在所述起点和终点的速度，设置所述机器人在所述过渡路径上的加速和减速。
[0117]
根据本发明的一个实施例，所述的自动引导车还包括激光雷达，所述激光雷达配置成可检测所述机器人前方的障碍物；所述控制装置与所述激光雷达通讯，并配置成当所述机器人前方存在障碍物时，控制所述机器人速度减为零；当所述障碍物消除时，将所述机器人的速度加速到最大速度。
[0118]
根据本发明的一个实施例，所述的自动引导车还包括安装在所述车体上的惯性测量单元，所述惯性测量单元配置成可测量所述机器人的当前位置和航向角。另外，所述机器人的位置和航向角也可以通过全局导航方法来确定，如反光板、slam等。所述控制装置与所述惯性测量单元通讯，并配置成；
[0119]
根据所述贝塞尔曲线，获得所述机器人的参考位置和参考航向角；
[0120]
获得所述机器人的位置偏差和航向角偏差；
[0121]
通过pid控制，纠正所述机器人的位置偏差和航向角偏差。
[0122]
根据本发明的一个实施例，所述第二控制点和第三控制点选择成使得所述贝塞尔曲线的中间的曲率满足k(0.5)＜k。
[0123]
最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。