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

权利要求书

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所述的基于目标距离的铰接式扫地车速度规划方法的系统，其特征在于：包括：

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

控制执行系统，包括：

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

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