基于B样条曲线的路径规划方法、装置、计算机可读存储介质与流程

本发明涉及路径规划领域，尤其涉及一种基于b样条曲线的路径规划方法、装置、计算机可读存储介质。

背景技术：

自动导引装置工作的过程中一般需要对其从起始点到目标点的路径进行规划，常用的路径规划方式有很多，比如规划路径为曲线，可使用插值的方式生成多项式插值，为了使曲线更容易计算，避免龙格现象，低阶可实现，可使用样条曲线插值，常见的样条曲线有贝塞尔曲线，b样条曲线等。为了使多项式次数独立于控制点数目，局部控制曲线路径，采用了b样条曲线进行路径规划而不用贝塞尔曲线。如果使用b样条曲线进行路径规划，主要输出内容为控制点和样条次数，据此可算出路径。自动导引装置根据路径行走，无限接近路径，

但现有技术中，因为自动导引装置本身的机械参数，控制和定位算法，以及整个系统的影响，自动导引装置与路径之间是有误差的，但会尽量将误差控制在合理范围内，使得自动导引装置到达目标点时与目标点的位置之间的误差在允许范围内。

在自动导引装置的运用过程中，不止需要考虑位置距离关系，还需要考虑角度关系，比如自动导引小车搬运货物，自动导引叉车插取栈板，搬运机器人自动扫码等场景，都需要自动导引装置在目的点的时候的角度也满足要求，否则无法完成货物对接搬运、栈板对接插取、扫码识别等工作。

而在需要同时考虑距离和角度的情况下，对于从起始点到目标点的路径规划有诸多考验，难度较大；现有技术中，部分自动导引装置采用了这样的一种路径和动作：自动导引装置直线行走一定距离，停止，原地旋转，再直线行走一定距离，如果角度偏差仍然过大，那么还需要停止，原地旋转，直到到达目标点；或者先原地旋转再直线行走，这样的路径和动作有若干缺点：一是花费时间较长；二是动作复杂；三是所需空间较大；四是整体效率低，尤其是自动导引装置本身体积较大的情况下。

技术实现要素：

本发明的目的在于提供一种基于b样条曲线的路径规划方法、装置、计算机可读存储介质，以解决现有技术花费时间较长、动作复杂、所需空间较大、整体效率低的问题。

如上构思，本发明所采用的技术方案是：基于b样条曲线的路径规划方法，包括：

获取起始控制点、目标控制点、第一控制点、第二控制点及原始路径，所述第二控制点相较于第一控制点距所述目标控制点更近；

确定特定控制点，所述特定控制点位于第一控制点和第二控制点之间；

根据起始控制点、第一控制点、特定控制点、第二控制点、目标控制点规划优化路径。

进一步地，所述特定控制点位于过所述第二控制点和所述目标控制点的直线上。

进一步地，所述确定特定控制点还包括：

计算第一比率，所述第一比率为所述起始控制点与所述第一控制点之间的距离和所述起始控制点与所述目标控制点之间的距离之间的比值，

计算第二比率，所述第二比率为所述特定控制点与所述目标控制点之间的距离和所述起始控制点与所述目标控制点之间的距离之间的比值，

所述第一比率和所述第二比率之和小于1时的若干点即为特定控制点。

进一步地，所述第一比率大于10%且小于40%，和/或，所述第二比率大于10%且小于40%。

进一步地，所述确定特定控制点还包括：

计算第三比率，所述第三比率为所述第二控制点与目标但之间的距离和所述起始控制点与所述目标控制点之间的距离之间的比值。

进一步地，所述第三比率小于或等于所述第二比率。

进一步地，所述目标点的坐标计算方法包括坐标系的平移旋转。

本发明还提供一种基于b样条曲线的路径规划装置，包括：

获取模块，所述获取模块获取起始点、目标点、第一控制点、第二控制点及原始路径；

确定模块，所述确定模块确定特定控制点；

路径规划模块，所述路径规划模块根据起始点、目标点、第一控制点、第二控制点规划原始路径；且，所述路径规划模块根据起始点、目标点、第一控制点、第二控制点、特定控制点规划优化路径。

本发明还提供一种计算机可读存储介质，其上存储有计算机程序，当所述计算机程序被处理器执行时实现如以上任一项所述的基于b样条曲线的路径规划方法。

本发明的有益效果：

本发明所述方法在原b样条曲线规划路径的基础上，通过确定特定控制点进行路径规划，使得移动设备的控制算法控制动作更加容易；并且规划的路径为连续的运动路径，不需要移动设备在某个位置点上进行停车，能够节约时间，速度快。

附图说明

图1为本发明所述基于b样条曲线的路径规划方法的步骤图；

图2为第一实施例所述基于b样条曲线的路径规划方法中若干控制点和路径示意图；

图3为第二实施例所述基于b样条曲线的路径规划方法中的部分参数示意图；

图4为第二实施例所述基于b样条曲线的路径规划方法中的部分参数示意图；

图5为第四实施例所述基于b样条曲线的路径规划装置的结构示意图。

具体实施方式

为使本发明解决的技术问题、采用的技术方案和达到的技术效果更加清楚，下面结合附图并通过具体实施方式来进一步说明本发明的技术方案。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部。

第一实施例

本实施例提供了一种基于b样条曲线的路径规划方法，参考图1，所述b样条曲线的路径规划方法包括：

获取起始控制点、目标控制点、第一控制点、第二控制点及原始路径，所述第二控制点相较于第一控制点距所述目标控制点更近；

确定特定控制点，所述特定控制点位于第一控制点和第二控制点之间；

根据起始控制点、第一控制点、特定控制点、第二控制点、目标控制点规划优化路径。

本实施例中，，可知的是，从起始控制点到目标控制点中的路径是多样化的，选择曲线路径作为从起始控制点到目标控制点的路径可以有效避免采用直线路径所需要的设备原地旋转等过程，这些过程耗时较长，会导致从起始控制点到目标控制点的效率较低；而曲线路径也可以采用多种方式胜场，比如使用插值的方式生成多项式插值，样条曲线插值等，但是为了使曲线路径更容易计算，避免龙格现象，低阶即可实现，因此优选采用样条曲线，为了使多项式次数独立于控制点数目，局部控制曲线路径，采用了b样条曲线进行路径规划。

b样条曲线规划生成的路径点一般包括四个控制点，起始控制点即当前位置，目标控制点即目标位置，在当前位置和目标位置中间设置两个控制点，分别为第一控制点和第二控制点，则可以在设备行进过程中给出控制点的坐标从而实现设备从起始控制点沿生成路径行走最终能够到达目标控制点，即目标位置；在此过程中，存在一条靠近起始控制点、第一控制点、第二控制点和目标控制点的曲线，这条曲线即原始路径。

采用本实施例所述方法进行路径规划的设备为agv叉车，或无人驾驶叉车，可理解的是，其他可行走设备如需进行路径规划也可以采用本实施例所述方法。

agv叉车的方向可以根据实际情况进行定义，一般情况下，可进行如下定义：与agv叉车的货叉平行且从货叉指向车头的方向为agv叉车的朝向，agv叉车的朝向与agv叉车所在场地的参照坐标系的x轴的夹角即为agv叉车的方向；而对于agv叉车即将前往的目标位置的方向也可以根据实际情况进行定义，一般情况下，可进行如下定义：目标位置的朝向与agv叉车所在场地的参照坐标系的x轴的夹角即为目标位置的方向；目标位置的朝向可根据实际情况进行定义，如果目标位置放置有栈板或者托盘，那么目标位置的朝向就是栈板或者托盘的朝向，可进行如下定义：垂直于栈板前端面指向agv叉车的朝向即为栈板的朝向；前端面即agv叉车插取栈板时距离agv叉车车头最近的端面。

根据b样条曲线的性质，距起始控制点更近的第一控制点应位于agv叉车的方向上，而距目标控制点更近的第二控制点应位于目标位置的方向上。

在一些特定情况下，b样条曲线规划的四个控制点的原始路径可能无法使得agv叉车能够准确到达目标位置，这里的准确有两层含义，一个是距离上的准确，一个是角度上的准确，对于agv叉车插取栈板这样的场景下，角度上的准确也很关键，因此，为了减小控制难度，提高控制准确性，规划一个新的控制点，即特定控制点，通过增加特定控制点，减小原始路径的某一段路径的曲率，使得控制难度降低，控制准确性提高。

考虑到一般第一控制点和第二控制点之间距离较远，曲率可能较大，可能还存在同一路径弯曲方向不同的情况，因此特定控制点增加在第一控制点和第二控制点之间，这里的“之间”范围较广，不限于特定控制点需要位于第一控制点和第二控制点作为端点的线段上；然后再根据起始控制点、第一控制点、特定控制点、第二控制点和目标控制点规划优化路径，基于b样条曲线进行规划即可。

本实施例中，所述特定控制点位于过所述第二控制点和所述目标控制点的直线上，新增特定控制点后，规划出优化路径，优化路径位于第二控制点和目标点之间的路径的曲率减小，便于终点附近误差控制。

而由于控制点的位置不同生成的b样条曲线的曲率不同，曲率越大时且要求误差小于某值时，转弯控制越困难，往往速度需要降低，使得agv叉车从起始位置到达目标位置得完成时间增加，并且可能到达目标位置时的精度有限，无论是距离上还是角度上，因此，可对特定控制点的位置进一步优化。

所述确定特定控制点还包括：

计算第一比率，所述第一比率为所述起始控制点与所述第一控制点之间的距离和所述起始控制点与所述目标控制点之间的距离之间的比值，

计算第二比率，所述第二比率为所述特定控制点与所述目标控制点之间的距离和所述起始控制点与所述目标控制点之间的距离之间的比值，

所述第一比率和所述第二比率之和小于1的若干点即为特定控制点。

通过计算第一比率、第二比率，且要求第一比率和第二比率之和小于1，则可以进一步缩小和优化特定控制点的位置。并且，所述第一比率大于10%且小于40%，和/或，所述第二比率大于10%且小于40%。

同时还可以计算第三比率，

且要求第一比率和第二比率之和小于1，第三比率小于第二比率，所述第三比率为所述第二控制点与目标但之间的距离和所述起始控制点与所述目标控制点之间的距离之间的比值，所述第三比率小于或等于所述第二比率的二分之一。。

确定了特定控制点，则可以根据起始控制点、第一控制点、特定控制点、第二控制点和目标控制点重新规划优化路径。

可理解的是，特定控制点的数量可以进一步增加，并非限于1个，新增加的特定控制点的确定方式可参照本实施例，本领域技术人员根据本实施例类推即可。

第二实施例

本实施例提供了一种基于b样条曲线的路径规划方法，本实施例应用场景为agv叉车插取栈板的场景，所述b样条曲线的路径规划方法包括：

获取起始控制点、目标控制点、第一控制点、第二控制点及原始路径，所述第二控制点相较于第一控制点距所述目标控制点更近；

确定特定控制点，所述特定控制点位于第一控制点和第二控制点之间；

根据起始控制点、第一控制点、特定控制点、第二控制点、目标控制点规划优化路径。

参考图2，起始控制点为a点，第一控制点为b点，第二控制点为d点，目标控制点为e点，特定控制点为c点；两条曲线，上侧一条为原始路径，下侧一条曲线为优化路径，优化路径相较于原始路径更平缓，尤其是靠近目标控制点的部分路径更加平缓，曲率小。

起始控制点即agv叉车当前位置，agv叉车的当前位置以坐标表示，具体的位置点可以根据实际需求进行设置，本实施例中设置为agv叉车的货叉下的两个后轮的圆心的连线的中点为其坐标参考点，起始时该参考点位置为其实控制点；目标控制点为agv叉车插取栈板时的参考点位置，即agv叉车插取栈板时的坐标；路径规划基于b样条曲线进行规划，在获取了agv叉车的当前位置和目标位置后，得到了起始控制点和目标控制点，进而确定第一控制点和第二控制点，以及b样条曲线规划得到的路径，路径形式为样条曲线。

所述确定特定控制点，所述特定控制点位于第一控制点和第二控制点之间，第一控制点和第二控制点之间的原始路径的曲率越小，控制难度越低，控制精度越高。

对于特定控制点的确定可先确定一个范围，通过逐步优化最终得到一个最优的唯一的特定控制点。

进一步确定特定控制点，所述特定控制点位于过所述第二控制点和所述目标控制点的直线上，这样使得agv叉车即将到达目标控制点前的曲率很小，能够平缓的接近目标控制点，控制难度低，。

但直线上可以有无数个点，为了进一步缩小范围确定特定控制点，根据计算和试验，引入了第一比率、第二比率和第三比率，根据第一比率、第二比率和第三比率进一步确定特定控制点。

即所述确定特定控制点还包括：

计算第一比率，所述第一比率为所述起始控制点与所述第一控制点之间的距离和所述起始控制点与所述目标控制点之间的距离之间的比值，

计算第二比率，所述第二比率为所述特定控制点与所述目标控制点之间的距离和所述起始控制点与所述目标控制点之间的距离之间的比值，

所述第一比率和所述第二比率之和小于1时的若干点即为特定控制点。

另外，还可以对第三比率进行计算，所述第三比率为所述第二控制点与目标但之间的距离和所述起始控制点与所述目标控制点之间的距离之间的比值。

可理解的是，特定控制点位于过所述第二控制点和所述目标控制点的直线上，且位于第一控制点和第二控制点之间，因此，第二比率应大于第三比率；另外，由于需要位于第二控制点和第三控制点之间的路径曲线曲率尽量小，因此，第一比率与第二比率的和小于1，否则，位于第二控制点和第三控制点之间的路径曲率就会变大。

然后计算不同的第一比率、第二比率和第三比率组合在一起时的位于起始控制点和目标控制点之间的路径曲率，选取若干个曲率最大值较小时的第一比率、第二比率和第三比率的组合；并且，换一种起始控制点和目标控制点的位姿，做同样的记录，取各种起始控制点和目标控制点位姿下曲率最大值都小的一组第一比率、第二比率和第三比率的值作为最终的参数值，采用参数值确定特定控制点的位置，同时，重新确定第一控制点、第二控制点的位置。

根据多次试验计算，所述第一比率大于10%且小于40%，和/或，所述第二比率大于10%且小于40%；所述第三比率小于或等于所述第二比率的二分之一。

确定了特定控制点之后，根据起始控制点、第一控制点、特定控制点、第二控制点和目标控制点规划优化路径。

可理解的是，在一些实施例中，所述特定控制点位于所述第一控制点和所述第二控制点之间的路径的一侧，所述第一控制点和所述第二控制点之间的线段位于所述路径的另一侧；而在某些实施例中，所述特定控制点、位于所述第一控制点和第二控制点之间的线段、位于第一控制点和第二控制点之间的路径之间的关系是，所述线段和所述路径交叉；在某些实施例中，还有其他关系。

第三实施例

本实施例提供了一种基于b样条曲线的路径规划方法，本实施例应用场景为agv叉车插取栈板的场景，所述b样条曲线的路径规划方法包括：

获取起始控制点、目标控制点、第一控制点、第二控制点及原始路径，所述第二控制点相较于第一控制点距所述目标控制点更近；

确定特定控制点，所述特定控制点位于第一控制点和第二控制点之间；

根据起始控制点、第一控制点、特定控制点、第二控制点、目标控制点规划优化路径。

本实施例中，对于目标控制点的获取采用坐标平移旋转计算的方式，场景与第二实施例相同，agv叉车插取栈板，agv叉车上设置有相机，通过视觉进行栈板识别。

参考图3-4，agv叉车本身设置了七个参数，包括第一参数ethup，第一参数ethup为相机安装带来的向上倾斜角度，简而言之，理论上相机安装后希望相机平面与水平地面垂直，但实际安装时相机的底面与地面不平行；第二参数ethright，第二参数ethright为相机安装带来的向右倾斜角度，简而言之，理论上相机安装后希望平行于与agv叉车的货叉长度方向垂直的平面，但实际安装时，相机平面与agv叉车的货叉长度方向垂直的平面不平行；第三参数eykinect，agv叉车货叉下的后轮中心连线的中垂线与相机平面有交点的距离；第四参数exkinect，第四参数exkinect是第三参数eykinect与相机表面交点到相机摄像头中心的距离；第五参数l_fork，为叉牙长度；第六参数l_forkend，为后轮中心到叉牙尖端连线的距离；第七参数l_init，为面向agv叉车的栈板中间脚的中点p距离栈板前端面的水平距离，可理解的是，这里的栈板下方设置有三排脚，相邻两排脚之间有空间用于容纳agv叉车的叉牙，对于只有两个脚或者多个脚的栈板而言，第七参数可以进行其他设置。

在agv叉车运行过程中，相机测得的参数包括：第八参数d_z，为点p到相机平面的距离；第九参数d_y，为从点p向相机平面做垂线的垂足r到相机镜头所在的平行于相机视野左右方向的直线的距离；第十参数d_x，从r向相机镜头所在的平行于相机视野左右方向的直线作垂线，垂足s到镜头中心的距离；第十一参数d_th，栈板面向agv叉车一侧的前端面与相机平面的夹角，从相机方向看向栈板，栈板左倾斜即左近右远时，定义第十一参数d_th大于0，该夹角的大小需要让相机在工作范围内看到栈板前端面，并计算出与栈板的夹角。

agv叉车为三轮车模型，其位置为后轮中心连线的中点，记当前位置为（xa,ya,thaa），xa为x轴坐标，ya为y轴坐标，tha为agv叉车的方向，坐标系为agv叉车所在场景的参考坐标系。

起始位置是a(xa,ya,tha)，目标位置是叉货终点是叉车居中插入，栈板紧贴叉牙根部时应该到达的位置，设该位置为q(xtar,ytar,thtar)。相机获得的数据是镜头到栈板面向叉车侧中间底座中点的的xyz三个方向的距离和栈板倾斜角度。因此需要根据第一参数、第二参数、第三参数、第四参数、第五参数、第六参数、第七参数、第八参数计算、第九参数、第十参数、第十一参数、第十二参数、第十三参数、第十四参数计算目标点位置，确定了起始控制点和目标控制点再进行路径规划得到原始路径。

坐标系平移旋转计算过程包括：

角度计算：thtar=thagv+d_th–ethright。

位置计算：利用坐标系旋转降相机获取的距离第八参数d_z+、第九参数d_y，第十参数d_x数据转化相机沿中心旋转到ethup=0的情况下，面向agv叉车的栈板中间脚的中点到镜头距离d_x1,d_y1,d_z1，关系为：

d_x1=d_x；

d_y1=d_y*cos(ethup)-d_z*sin(ethup)；

d_z1=d_y*sin(ethup)+d_z*cos(ethup)。

d_x1、d_y1、d_z1数据转化相机沿中心旋转到ethright=0的情况下面向agv叉车的栈板中间脚的中点到镜头的距离d_x2、d_y2,、d_z2。

d_x2=d_z1*sin(ethright)+d_x1*cos(ethright)；

d_y2=d_y1；

d_z2=d_z1*cos(ethright)-d_x1*sin(ethright)；

d_x2=d_x2-(exkinect-exkinect*cos(ethright))；

d_z2=d_z2-exkinect*sin(ethright)。

d_x2、d_y2、d_z2数据转化为面向agv叉车的栈板中间脚的中点到后轮中心点连线的中点的距离d_x3、d_y3、d_z3。

d_x3=eykinect-d_z；

d_y3=exkinect+d_x。

计算面向agv叉车的栈板中间脚的中点的坐标，xp为x轴坐标，yp为y轴坐标：

xp=xa-d_y3*sin(thagv)+d_x3*cos(thagv)；

yp=ya+d_y3*cos(thagv)+d_x3*sin(thagv)。

计算目标位置坐标(xtar,ytar,thtar)，即目标控制点坐标：

xtar=xp+(l_forkend+l_init)*sin(thp)；

ytar=yp–(l_forkeend+l_init)*cos(thp)。

本实施例采用b样条曲线进行路径规划，一段样条曲线作为路径，但是可以理解的是，也可以采用多段样条曲线，为了使栈板叉入的更准确，可以要求叉牙在进入栈板之前，（即叉牙根部距离栈板间距大于等于0的位置时），叉牙后轮中心的连线的中点（即目标控制点），就已经走到可以微调叉入的误差范围内，示例性地，真实场景举例，角度误差为0，从叉牙方向看栈板的左右误差为26mm。从叉牙方向看栈板的左右误差为0时，角度误差为1.585°。假设标定参数准确的情况下，叉货误差=控制误差+定位误差+位置计算误差，这些误差会以0为基准上下平均分布。因此目标控制点的最佳设置应该使无人叉车与栈板角度相同，栈板在叉车正中央。叉牙根部距离栈板间距最佳的取值为0，因为这样能让曲线起始控制点至目标控制点的距离尽可能的长，留给控制调整。

然后根据第一实施例或者第二实施例上述施例的方案规划优化路径。

第四实施例

本实施例提供一种基于b样条曲线的路径规划装置，参考图5，包括：

获取模块，所述获取模块获取起始控制点、目标控制点、第一控制点、第二控制点及原始路径；

确定模块，所述确定模块确定特定控制点；

路径规划模块，所述路径规划模块根据起始控制点、目标控制点、第一控制点、第二控制点规划原始路径；且，所述路径规划模块根据起始控制点、目标控制点、第一控制点、第二控制点、特定控制点规划优化路径。

第五实施例

本实施例提供一种计算机可读存储介质，其上存储有计算机程序，其当所述计算机程序被处理器执行时实现如第一实施例任一项所述的基于b样条曲线的路径规划方法。

本发明所述方法在原b样条曲线规划路径的基础上，通过确定特定控制点进行路径规划，使得移动设备的控制算法和控制动作更加容易；并且规划的路径为连续的运动路径，不需要移动设备在某个位置点上进行停车，能够节约时间，速度快。

以上实施方式只是阐述了本发明的基本原理和特性，本发明不受上述实施方式限制，在不脱离本发明精神和范围的前提下，本发明还有各种变化和改变，这些变化和改变都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。