CN112363505B - 一种基于目标距离的铰接式扫地车速度规划方法及系统 - Google Patents

Abstract

本发明涉及一种基于目标距离的铰接式扫地车速度规划方法，包括：扫地车运动规划模块将规划出的全局参考轨迹实时发给控制执行系统；对包含m个轨迹点参考轨迹进行重新插值成每两个点之间距离相等的n个轨迹点；确定加减速度距离，计算加减速度，得到基于目标距离的T形速度规划，对得到的梯形速度曲线进行一次数字卷积,用卷积函数对速度曲线进行平滑处理，得到S形加减速速度曲线。本发明还公开了一种基于目标距离的铰接式扫地车速度规划系统。本发明降低了扫地车因为铰接式转向延迟而走S形，超调以及惯性冲击的问题的概率；实时调整加速度，使得扫地车能够跟踪期望速度，能够很好的处理速度平滑的问题。

Description

一种基于目标距离的铰接式扫地车速度规划方法及系统

技术领域

本发明涉及智能车技术领域，尤其是一种基于目标距离的铰接式扫地车速度规划方法及系统。

背景技术

无人驾驶车辆的速度规划主要是根据期望路径的长度和前方障碍物的距离以及车身的各类约束生成一条能够和期望路径匹配的期望速度，其目的是在起步和停车时能够平滑进行，保证和障碍物在安全距离。传统梯形速度规划主要考虑速度和时间的线性关系，梯形速度规划普适性、实时性较好，但是在处理动态障碍物的时候，速度平滑的问题突出。

扫地车工作速度较低，加减速距离较短，扫地车的液压式转向机构易受环境影响，如温度、车身电量等，转向机构会出现转向不灵敏的现象，导致扫地车在作业时出现横向偏差过大，走S型的情况，即超调现象。同时，无人驾驶扫地车后车厢装有大量垃圾与车载电池，前车装有大量的水，扫地车在启停过程中，如不考虑加速度，则会导致扫地车带有很大惯性，对扫地车造成一定的损伤，因此在无人驾驶启停过程中必须对其速度进行规划，以减轻启停过程中对扫地车造成的损伤。

发明内容

本发明的首要目的在于提供一种能够使扫地车在启停以及遇到动态障碍物过程中根据实际车速和距离进行加减速，防止扫地车出现超调和惯性冲击现象的基于目标距离的铰接式扫地车速度规划方法。

为实现上述目的，本发明采用了以下技术方案：一种基于目标距离的铰接式扫地车速度规划方法，该方法包括下列顺序的步骤：

(1)扫地车运动规划模块将规划出的全局参考轨迹实时发给控制执行系统，控制执行系统对规划出的轨迹进行跟踪，同时对纵向的速度进行控制，实时发送的每一段轨迹包含m个轨迹点；

(2)对包含m个轨迹点的参考轨迹进行重新插值成每两个点之间距离相等的n个轨迹点，且距离均为d，n个轨迹点中的每个轨迹点包含的状态有位置、速度、航向信息，针对每一段轨迹的n个轨迹点进行速度规划；

(3)扫地车的最高车速由作业速度和稳态转向限制速度实时共同决定，初始速度为v₀,期望速度为v_exp，末速度为v_t，加速距离为S₁，匀速距离为S₂，减速距离为S₃；

(4)在初始加速阶段，根据当前位置状态获取初始车速v₀，局部目标点的期望速度v_exp，目标点的状态(x_exp,y_exp,θ_exp,v_exp)以及通过目标点的选取所决定的加速距离S₁，扫地车在无人驾驶的过程中不断靠近目标点，距离目标点的加速距离S₁是不断变小，扫地车需要在加速距离S₁内加速到作业速度，即从当前轨迹点加速到期望速度，当前位置到目标点之间每个点都规划出一个速度；在目标点的状态(x_exp,y_exp,θ_exp,v_exp)中，前两个参数代表位置，第三个参数代表航向；

(5)在匀速阶段，运动规划生成的轨迹中所有点的速度都将赋值期望速度，若稳态转向限制速度大于轨迹点期望速度，则轨迹点期望速度为最终期望速度，否则，轨迹点稳态转向限制速度为最终期望速度；

(6)在减速阶段，扫地车距离障碍物或者停车点的距离是S_obs,设初始速度为v₀，减速距离为S₃；

(7)得到基于目标距离的T形速度规划，对得到的梯形速度曲线进行一次数字卷积,用卷积函数对速度曲线进行平滑处理，得到S形加减速速度曲线。

所述步骤(4)具体是指：

加速距离S₁中所有轨迹点的数量为：

n＝S₁/d

需要的加速度为：

其中，v_exp是局部目标点的期望速度，v_cur是扫地车当前速度，当S₁的值为0时，则表示扫地车已经加速到期望速度v_exp；

计算出每个点的速度如下：

v_n+1＝v_n+a_n

其中n的值为0,1,...n-1，v_n+1表示下一轨迹点的速度，v_n表示当前轨迹点的速度，如此得到每一个点的速度，并且轨迹点的速度是逐渐递增的，车速逐渐的平滑的接近期望速度，当扫地车的车速达到期望速度，则将后面的所有轨迹点的速度全部赋值为v_exp；

在这个过程中每一个轨迹点的速度皆为每个点的期望速度，在速度规划的同时，实时计算每一个轨迹点的铰接转角γ以及其变化率

计算稳态转向限制速度v_sta，并将其与每一个轨迹点期望速度实时对比，若稳态转向限制速度大于轨迹点期望速度，则轨迹点期望速度为最终期望速度，否则，则轨迹点稳态转向限制速度为最终期望速度。

所述步骤(6)具体是指：

在当前位置到障碍物的位置的轨迹点的数量为n个：

n＝S_obs/d

若需要在障碍物或者停车点前L米处停车，则在这L米长的轨迹内的轨迹点的数为：

n_t＝L/d

停车点或者障碍物位置的期望速度为v_exp＝0，其状态为(x_exp,y_exp,θ_exp,v_exp)，减速距离S₃是通过停车点或者障碍物位置所决定的，扫地车在无人驾驶的过程中不断的靠近停车点或者障碍物位置，距离停车点或者障碍物位置的距离S₃不断变小，扫地车需要在减速距离S₃内减速到v_exp＝0；减速时的减速度计算方法如下：

其中，v_t是末速度，n是当前位置到障碍物位置的轨迹点总数，n_t是停车点距离障碍物位置轨迹点数；

在得到减速度后即可计算所有轨迹点的速度：

v_n+1＝v_n+a_n

其中，n的值为0,1,...i-1，得到每一个点的速度，并且轨迹点的速度是逐渐递减的，车速逐渐的平滑的接近期望速度，直到车速降为0；

对扫地车的稳态转向限制速度与每一个点的期望速度作对比，若稳态转向限制速度大于轨迹点期望速度，则轨迹点期望速度为最终期望速度，否则，则轨迹点稳态转向限制速度为最终期望速度，得到最终的期望速度。

所述步骤(7)中的卷积函数如下：

其中，N₁表示数字卷积序列长度，v₁为基于目标距离的T型速度规划函数，即：

其中，a_acc为设置的加速度，a_dec为设置的减速度；

利用卷积和原理得其相应离散形式,进行基于目标距离的S型速度规划：

其中，k为区间数目。

本发明的另一目的在于提供一种基于目标距离的铰接式扫地车速度规划系统，包括：

扫地车运动规划模块，接收行为目标，综合运用感知地图障碍物的分布，生成从当前位置到这个目标点的轨迹，并发送至控制执行系统；

控制执行系统，包括：

控制系统，跟踪局部期望轨迹，将其转化方向转角、油门、刹车、档位、灯光、喇叭的控制量，并发送给执行系统执行；

执行系统，接收来自控制系统的指令，实现执行机构行为的控制。

由上述技术方案可知，本发明的有益效果为：第一，运算量小，耗费资源低，能够很好的保证实时性；第二，降低了扫地车因为铰接式转向延迟而走S形，超调以及惯性冲击的问题的概率；第三，改进后的加减速度引入了扫地车的当前车速后，加速或者减速过程中可以是不断变化的，实时调整加速度，使得扫地车能够跟踪期望速度，能够很好的处理速度平滑的问题。

附图说明

图1为本发明的方法流程图；

图2为扫地车基于目标距离速度规划图；

图3为扫地车基于目标距离速度规划效果图；

图4为遇障时扫地车基于目标距离速度规划图；

图5为遇障时扫地车基于目标距离速度规划效果图。

具体实施方式

如图1、2、3所示，一种基于目标距离的铰接式扫地车速度规划方法，该方法包括下列顺序的步骤：

(1)扫地车运动规划模块将规划出的全局参考轨迹实时发给控制执行系统，控制执行系统对规划出的轨迹进行跟踪，同时对纵向的速度进行控制，实时发送的每一段轨迹包含m个轨迹点；

(2)对包含m个轨迹点的参考轨迹进行重新插值成每两个点之间距离相等的n个轨迹点，且距离均为d，n个轨迹点中的每个轨迹点包含的状态有位置、速度、航向信息，针对每一段轨迹的n个轨迹点进行速度规划；

(3)扫地车的最高车速由作业速度和稳态转向限制速度实时共同决定，初始速度为v₀,期望速度为v_exp，末速度为v_t，加速距离为S₁，匀速距离为S₂，减速距离为S₃；

(4)在初始加速阶段，根据当前位置状态获取初始车速v₀，局部目标点的期望速度v_exp，目标点的状态(x_exp,y_exp,θ_exp,v_exp)以及通过目标点的选取所决定的加速距离S₁，扫地车在无人驾驶的过程中不断靠近目标点，距离目标点的加速距离S₁是不断变小，扫地车需要在加速距离S₁内加速到作业速度，即从当前轨迹点加速到期望速度，当前位置到目标点之间每个点都规划出一个速度；在目标点的状态(x_exp,y_exp,θ_exp,v_exp)中，前两个参数代表位置，第三个参数代表航向；

(5)在匀速阶段，运动规划模块生成的轨迹中所有点的速度都将赋值期望速度，若稳态转向限制速度大于轨迹点期望速度，则轨迹点期望速度为最终期望速度，否则，轨迹点稳态转向限制速度为最终期望速度；

(6)在减速阶段，扫地车距离障碍物或者停车点的距离是S_obs,设初始速度为v₀，减速距离为S₃；

(7)得到基于目标距离的T形速度规划，对得到的梯形速度曲线进行一次数字卷积,用卷积函数对速度曲线进行平滑处理，得到S形加减速速度曲线。

所述步骤(4)具体是指：

加速距离S₁中所有轨迹点的数量为：

n＝S₁/d

需要的加速度为：

其中，v_exp是局部目标点的期望速度，v_cur是扫地车当前速度，当S₁的值为0时，则表示扫地车已经加速到期望速度v_exp；

计算出每个点的速度如下：

v_n+1＝v_n+a_n

其中n的值为0,1,...n-1，v_n+1表示下一轨迹点的速度，v_n表示当前轨迹点的速度，如此得到每一个点的速度，并且轨迹点的速度是逐渐递增的，车速逐渐的平滑的接近期望速度，当扫地车的车速达到期望速度，则将后面的所有轨迹点的速度全部赋值为v_exp；

在这个过程中每一个轨迹点的速度皆为每个点的期望速度，在速度规划的同时，实时计算每一个轨迹点的铰接转角γ以及其变化率

计算稳态转向限制速度v_sta，并将其与每一个轨迹点期望速度实时对比，若稳态转向限制速度大于轨迹点期望速度，则轨迹点期望速度为最终期望速度，否则，则轨迹点稳态转向限制速度为最终期望速度。

所述步骤(6)具体是指：

在当前位置到障碍物的位置的轨迹点的数量为n个：

n＝S_obs/d

若需要在障碍物或者停车点前L米处停车，则在这L米长的轨迹内的轨迹点的数为：

n_t＝L/d

停车点或者障碍物位置的期望速度为v_exp＝0，其状态为(x_exp,y_exp,θ_exp,v_exp)，减速距离S₃是通过停车点或者障碍物位置所决定的，扫地车在无人驾驶的过程中不断的靠近停车点或者障碍物位置，距离停车点或者障碍物位置的距离S₃不断变小，扫地车需要在减速距离S₃内减速到v_exp＝0；减速时的减速度计算方法如下：

其中，v_t是末速度，n是当前位置到障碍物位置的轨迹点总数，n_t是停车点距离障碍物位置轨迹点数；

在得到减速度后即可计算所有轨迹点的速度：

v_n+1＝v_n+a_n

其中，n的值为0,1,...i-1，得到每一个点的速度，并且轨迹点的速度是逐渐递减的，车速逐渐的平滑的接近期望速度，直到车速降为0；

对扫地车的稳态转向限制速度与每一个点的期望速度作对比，若稳态转向限制速度大于轨迹点期望速度，则轨迹点期望速度为最终期望速度，否则，则轨迹点稳态转向限制速度为最终期望速度。

所述步骤(7)中的卷积函数如下：

其中，N₁表示数字卷积序列长度，v₁为基于目标距离的T型速度规划函数，即：

其中，a_acc为设置的加速度，a_dec为设置的减速度；

利用卷积和原理得其相应离散形式,进行基于目标距离的S型速度规划：

其中，k为区间数目。

本系统包括：

扫地车运动规划模块，接收行为目标，综合运用感知地图障碍物的分布，生成从当前位置到这个目标点的轨迹，并发送至控制执行系统；

控制执行系统，包括：

控制系统，跟踪局部期望轨迹，将其转化方向转角、油门、刹车、档位、灯光、喇叭的控制量，并发送给执行系统执行；

执行系统，接收来自控制系统的指令，实现执行机构行为的控制。

以下结合图1至5对本发明作进一步的说明。

对于障碍物的处理，主要是处理扫地车在加速过程中或者在匀速行驶以及减速的时候突然遇到障碍物，导致扫地车减速甚至停车，待障碍物离开后又开始加速的问题。

本发明涉及到的扫地车本身车速限制在5km/h，相对较低，在加速或者减速过程中时间相对较短，不考虑增加过多的中间速度，因此在处理动态障碍物的问题时候，主要考虑安全、平稳的行驶，做好速度过度平滑处理。对于扫地车遇到的动态环境的处理主要是，在作业过程中，车前突然走过一人或者自行车，扫地车感知到障碍物时，行为决策模块立即发指令减速，减速还未减为0时或者已经减为0时，障碍物离开，扫地车立即开始加速，直至达到期望速度。

针对处理扫地车在加速过程中突然遇到障碍物，导致扫地车减速甚至停车，待障碍物离开后又开始加速的问题。如图4、5所示，初始速度为v₀，加速到v_x1时候突然有障碍物进入紧急停车的区域外，扫地车开始减速，减速到v_x2时候，障碍物离开，扫地车又开始加速,直至加速到v_exp。

首先第一段，扫地车在加速时突然闯进来一个障碍物，此时我们不讨论紧急停车和绕障的情况，因此障碍物距离本车有一定的范围，即L<S_obs<S₁，假设a点为加速的起始点，即汽车初始位置，b点是加速到期望速度的目标点，p点是扫地车遇到动态障碍物时候的点，从a点开始加速到b点的期望速度为v_exp，加速距离为S₁，轨迹点的数量为n，扫地车在p点时候加速度会发生变化，我们需要分析扫地车在p时候的状态：

可以得到，若障碍物在p点一直是静止的，假设p点是加速阶段的第i个点，且扫地车每个点的实际车速均可达到期望速度，那么未出现障碍物时候p点的加速度和速度可以通过上述公式得到为：

在p点出现障碍物后，汽车开始减速，此时p点的扫地车减速度即为

那么p点的下一个点的速度即为：

由此可以看出，此时p点作为一个速度变化点，我们可以得出，影响速度平滑过渡的因素主要是v_p和i。

加速度的变化量为：

因此可以的得到，其中加速时行驶的距离越大，在p点加速度变化量越大，全程加速或者减速距离越小；在p点的加速度变化量越大，期望作业速度越大，在p点加速度变化量越大，初始速度越大；在p点加速度变化量越大。加速度变化量越大，在p点代表速度过度的越不平滑。

扫地车生成基于障碍物目标距离的T型速度规划，再由卷积平滑处理生成S型速度规划。

由上面分析可以得出的结论是：增大全程加速或者减速距离，适量降低扫地车的期望作业速度有助于提高速度平滑度。

对于第二段，扫地车遇到障碍物后进行在减速，在减速阶段，障碍物离开，扫地车停止减速，进行加速。和第一段类似，假设扫地车在p点遇到障碍物开始减速，到达第i个点即p点后，障碍物离开，扫地车开始加速，设在a点时车速为v₀，v_exp是期望作业速度，在p点的速度为v_p，因此可以得到：

由上面的公式可以得到，在减速度变化方面，初始速度越小，在p点的减速度变化量就越大；全程加速或者减速的距离越大，在p点的减速度变化量就越大；期望作业速度越大，在p点的减速度变化量就越大，减速距离越大，在p点的减速度变化量就越大。

扫地车生成基于障碍物目标距离的T型速度规划，再由卷积平滑处理生成S型速度规划。

因此由上面分析可以得出的结论：适量增大全程加速或者减速距离，降低扫地车的期望作业速度有助于提高速度平滑度，得出的结论和前面第一段分析的结果一致。

至此，本改进的速度规划算法已经将扫地车可能遇到的情况进行了分析，该算法不仅能够很好的处理静态障碍物，对于动态障碍物问题的处理也很符合实际工程应用，将扫地车的实时车速设计到加速度的规划中，使得加速度的变化和当前车速实时相关，更好的处理速度平滑的问题；对于动态障碍物的随机出现，扫地车能够根据障碍物的距离以及当前车速，实时计算出加速度，安全、平滑的完成加速和减速过程，提高了通行安全性和效率。

Claims (2)

1.一种基于目标距离的铰接式扫地车速度规划方法，其特征在于：该方法包括下列顺序的步骤：

(1)扫地车运动规划模块将规划出的全局参考轨迹实时发给控制执行系统，控制执行系统对规划出的轨迹进行跟踪，同时对纵向的速度进行控制，实时发送的每一段轨迹包含m个轨迹点；

(2)对包含m个轨迹点的参考轨迹进行重新插值成每两个点之间距离相等的n个轨迹点，且距离均为d，n个轨迹点中的每个轨迹点包含的状态有位置、速度、航向信息，针对每一段轨迹的n个轨迹点进行速度规划；

(3)扫地车的最高车速由作业速度和稳态转向限制速度实时共同决定，初始速度为v₀,期望速度为v_exp，末速度为v_t，加速距离为S₁，匀速距离为S₂，减速距离为S₃；

(4)在初始加速阶段，根据当前位置状态获取初始车速v₀，局部目标点的期望速度v_exp，目标点的状态(x_exp,y_exp,θ_exp,v_exp)以及通过目标点的选取所决定的加速距离S₁，扫地车在无人驾驶的过程中不断靠近目标点，距离目标点的加速距离S₁是不断变小，扫地车需要在加速距离S₁内加速到作业速度，即从当前轨迹点加速到期望速度，当前位置到目标点之间每个点都规划出一个速度；在目标点的状态(x_exp,y_exp,θ_exp,v_exp)中，前两个参数代表位置，第三个参数代表航向；

(5)在匀速阶段，运动规划模块生成的轨迹中所有点的速度都将赋值期望速度，若稳态转向限制速度大于轨迹点期望速度，则轨迹点期望速度为最终期望速度，否则，轨迹点稳态转向限制速度为最终期望速度；

(6)在减速阶段，扫地车距离障碍物或者停车点的距离是S_obs,设初始速度为v₀，减速距离为S₃；

(7)得到基于目标距离的T形速度规划，对得到的梯形速度曲线进行一次数字卷积,用卷积函数对速度曲线进行平滑处理，得到S形加减速速度曲线；

所述步骤(4)具体是指：

加速距离S₁中所有轨迹点的数量为：

n＝S₁/d

需要的加速度为：

其中，v_exp是局部目标点的期望速度，v_cur是扫地车当前速度，当S₁的值为0时，则表示扫地车已经加速到期望速度v_exp；

计算出每个点的速度如下：

v_n+1＝v_n+a_n

其中n的值为0,1,...n-1，v_n+1表示下一轨迹点的速度，v_n表示当前轨迹点的速度，如此得到每一个点的速度，并且轨迹点的速度是逐渐递增的，车速逐渐的平滑的接近期望速度，当扫地车的车速达到期望速度，则将后面的所有轨迹点的速度全部赋值为v_exp；

在这个过程中每一个轨迹点的速度皆为每个点的期望速度，在速度规划的同时，实时计算每一个轨迹点的铰接转角γ以及其变化率

计算稳态转向限制速度v_sta，并将其与每一个轨迹点期望速度实时对比，若稳态转向限制速度大于轨迹点期望速度，则轨迹点期望速度为最终期望速度，否则，则轨迹点稳态转向限制速度为最终期望速度；

所述步骤(6)具体是指：

在当前位置到障碍物的位置的轨迹点的数量为n个：

n＝S_obs/d

若需要在障碍物或者停车点前L米处停车，则在这L米长的轨迹内的轨迹点的数为：

n_t＝L/d

停车点或者障碍物位置的期望速度为v_exp＝0，其状态为(x_exp,y_exp,θ_exp,v_exp)，减速距离S₃是通过停车点或者障碍物位置所决定的，扫地车在无人驾驶的过程中不断的靠近停车点或者障碍物位置，距离停车点或者障碍物位置的距离S₃不断变小，扫地车需要在减速距离S₃内减速到v_exp＝0；减速时的减速度计算方法如下：

其中，v_t是末速度，n是当前位置到障碍物位置的轨迹点总数，n_t是停车点距离障碍物位置轨迹点数；

在得到减速度后即可计算所有轨迹点的速度：

v_n+1＝v_n+a_n

其中，n的值为0,1,...i-1，得到每一个点的速度，并且轨迹点的速度是逐渐递减的，车速逐渐的平滑的接近期望速度，直到车速降为0；

对扫地车的稳态转向限制速度与每一个点的期望速度作对比，若稳态转向限制速度大于轨迹点期望速度，则轨迹点期望速度为最终期望速度，否则，则轨迹点稳态转向限制速度为最终期望速度；

所述步骤(7)中的卷积函数如下：

其中，N₁表示数字卷积序列长度，v₁为基于目标距离的T型速度规划函数，即：

其中，a_acc为设置的加速度，a_dec为设置的减速度；

利用卷积和原理得其相应离散形式,进行基于目标距离的S型速度规划：

其中，k为区间数目。

2.实施权利要求1所述的基于目标距离的铰接式扫地车速度规划方法的系统，其特征在于：包括：

扫地车运动规划模块，接收行为目标，综合运用感知地图障碍物的分布，生成从当前位置到这个目标点的轨迹，并发送至控制执行系统；

控制执行系统，包括：

控制系统，跟踪局部期望轨迹，将其转化方向转角、油门、刹车、档位、灯光、喇叭的控制量，并发送给执行系统执行；

执行系统，接收来自控制系统的指令，实现执行机构行为的控制。

Images