[发明专利]牵引车路径示教及基于路径示教的牵引车导航方法

权利要求书

1.一种牵引车路径示教方法，其特征在于，所述方法包括以下步骤：

按预设频率记录人工驾驶牵引车通过目标路段的行驶路径数据，所述行驶路径数据包括定位数据和电机工作数据；

对所述行驶路径数据进行滤波，剔除噪声点；

对滤波处理后的行驶路径数据进行压缩、离散获得关键点；

将所述关键点进行拟合得到样条曲线；

对所述样条曲线按预设频率离散得到目标路径的参考点集并存储在示教路径数据库中。

2.根据权利要求1所述的一种牵引车路径示教方法，其特征在于，所述对所述行驶路径数据进行滤波，剔除噪声点，包括：

对于每类行驶路径数据分别设置对应的误差值阈值，对于每类行驶路径数据，剔除大于对应类别的误差阈值的数据。

3.根据权利要求1所述的一种牵引车路径示教方法，其特征在于，所述对滤波处理后的行驶路径数据进行压缩、离散获得关键点，包括：

行驶路径数据中的定位数据包括多条轨迹曲线数据，对于每条轨迹曲线数据，采用Douglas-Peucker算法遍历其轨迹曲线上的数据点，按照预设比例分段所述轨迹曲线数据，找到每段轨迹曲线上相对于该轨迹曲线连成弦长的最大弓高值作为要保留的关键点。

4.根据权利要求3所述的一种牵引车路径示教方法，其特征在于，对于每条轨迹曲线数据的具体处理方式，包括：

(1)对于每一条轨迹曲线，在轨迹曲线首尾两点A，B之间连接一条直线AB，该直线为所述轨迹曲线的弦；

(2)遍历所述轨迹曲线上其他所有点，求每个点到直线AB的距离，找到最大距离的点C，最大距离记为dmax；

(3)比较该距离dmax与预先定义的阈值Dmax大小，如果dmaxDmax，则将该直线AB作为曲线段的近似，曲线段处理完毕；

(4)若dmax＝Dmax，则使C点将轨迹曲线AB分为AC和CB两段，并分别对这两段进行(1)～(3)步处理；

(5)当所有轨迹曲线都处理完毕时，依次连接各个分割点形成的折线，即为原始曲线的路径，其中所述分割点即为所述关键点。

5.根据权利要求1所述的一种牵引车路径示教方法，其特征在于，所述将所述关键点进行拟合得到样条曲线，包括：采用三次样条拟合方法对所述关键点进行拟合，将拟合得到的样条曲线作为所述目标路段的自动行驶路线。

6.根据权利要求1所述的一种牵引车路径示教方法，其特征在于，在接收到开始人工示教信号后，执行所述按预设频率记录人工驾驶牵引车通过目标路段的行驶路径数据的步骤；在接收到结束人工示教信号后，停止记录人工驾驶牵引车通过目标路段的行驶数据。

7.一种基于路径示教的牵引车导航方法，其特征在于，所述方法包括：

根据权利要求1-6中任一项所述的牵引车路径示教方法构建示教路径数据库；

根据牵引车搬运起点和终点确定示教路径数据库中对应的目标路径，并获取对应的目标路径的参考点集；

根据所述目标路径的参考点集对牵引车进行导航。

8.根据权利要求7所述的一种基于路径示教的牵引车导航方法，其特征在于，所述根据所述目标路径的参考点集对牵引车进行导航，包括：

基于牵引车单舵轮运动学模型，采用standy算法，将确定的目标路径的参考点集作为输入量，实时规划控制车辆速度与角度，使得车辆按照目标路径行驶。

9.根据权利要求8所述的一种基于路径示教的牵引车导航方法，其特征在于，所述采用standy算法，将确定的目标路径的参考点集作为输入量，实时规划控制车辆速度与角度，包括：

以当前定位点为初始位姿状态，以目标路径的参考点集中距离当前定位点最接近的点作为目标位姿状态；

通过standy算法作为控制率迭代使初始位姿状态和目标位姿状态之间的误差收敛至零。

10.根据权利要求8或9所述的一种基于路径示教的牵引车导航方法，其特征在于，

牵引车运动学模型为单舵轮后驱模型，两后轮中心连线的中点B作为参考点，则其运动学模型可描述为：

其中，(x，y)为牵引车的参考点的坐标；β为牵引车的舵角，并规定从参考点向前轮看逆时针为正，顺时针为负；L为牵引车车身长度；v_A为牵引车前轮驱动速度；θ为牵引车的航向角，即车身轴线与X轴正方向的夹角；ω为其航向角速度，定义逆时针为正；在全局坐标系下牵引车当前的位姿状态用[x，y，θ]^T来表示。