CN111208807A - 一种基于b样条曲线的agv运动控制方法 - Google Patents

Abstract

本发明公开了一种基于B样条曲线的AGV运动控制方法，包括如下步骤：（1）根据实际需求，规划AGV必须经过的行止站点；（2）通过行止站点反解控制点；（3）由控制点计算得到B样条参数方程,B样条为三阶B样条；（4）用B样条参数方程表示AGV的行驶路径；（5）采用自适应参数更新算法，进行参数更新，得到实时变化的参考点；（6）结合AGV的实际位姿与期望位姿的误差，得到位姿误差；（7）考虑到实际的AGV的转动惯量，根据AGV行进路径的曲率调节车速v，并根据小车的车速调节角速率ω；（8）采用PID控制方法，消除位姿误差，控制AGV按期望路径运动。本发明基于B样条曲线的AGV运动控制方法，使得AGV行驶路径更光滑；AGV舵轮摆角均匀，AGV运动更平稳。

Description

一种基于B样条曲线的AGV运动控制方法

技术领域

本发明属于AGV导航领域，具体涉及一种基于B样条曲线的AGV运动控制方法。

背景技术

AGV（自动导向小车）运行过程中，由于一些系统固有的误差，如两驱动点解同步性不好，两驱动轮直径差异、减速器传动效果的差异及一些非系统误差，如车轮与地面发生滑动、装载货物重心偏移产生的惯性负载等情况的存在，即使小车最初运行时没有偏移，这些误差也会随着运行时间的增加而累积，导致小车发生偏移，所以，如何发现并纠正偏移是小车控制系统的首要任务。目前现有技术中有尝试采用 NURBS非均匀有理B样条（Non-Uniform Rational B-Splines）建模的方法对AGV进行控制。NURBS是一种优秀的建模方式，具体解释是：Non-Uniform(非均匀性）：是指一个控制顶点的影响力的范围能够改变。Rational(有理）：是指每个NURBS物体都可以用有理多项式形式表达式来定义。B-Spline(B样条）：是指用路线来构建一条曲线，在一个或更多的点之间以内插值替换的。但是这只是一种理论仿真，将其直接应用于AGV的运动控制时，会出现一些AGV运动不够平稳、路径不够光滑等问题，所以需要作出改进。

发明内容

因此，针对上述的问题，本发明提出一种基于B样条曲线的AGV运动控制方法，其算法简单且曲线拟合效果好。

为了解决上述技术问题，本发明采用的技术方案为，本发明一种基于B样条曲线的AGV运动控制方法，包括如下步骤：

（1）根据实际需求，规划AGV必须经过的行止站点；

（2）通过所述行止站点反解控制点；

（3）由所述控制点计算得到B样条参数方程,所述B样条为三阶B样条；

（4）用所述B样条参数方程表示AGV的行驶路径；

（5）采用自适应参数更新算法，进行参数更新，得到实时变化的参考点；

（6）结合AGV的实际位姿与期望位姿的误差，得到位姿误差；

（7）考虑到实际的AGV的转动惯量，根据AGV行进路径的曲率调节车速v，并根据小车的车速调节角速率ω；

（8）采用PID控制方法，消除位姿误差，控制AGV按期望路径运动。

进一步地，本方法，控制点选择：首先选取AGV当前位置点作为第一个控制点，即前进或后退路径起始点；再于AGV当前前进或后退方向上选取一个靠近起始点的点作为第二个控制点；其次， 再结合AGV当前前进或后退方向与原目标点要求方向夹角的角平分线上，距离AGV一定距离处选取一点作为倒数第一个控制点，即终止点；再于角平分线上靠近终止点且靠近起始点一侧选取一点作为倒数第二个控制点；最后，选取第二个控制点与倒数第二个控制点间的中间点为新的一个控制点，或者根据需要选择此两点间的其他点作为新控制点。

进一步地，本方法，在步骤（8）之前考虑到实际过程中有响应时延，应用卡尔曼滤波以AGV当前位姿预测下几个周期的位姿。

进一步地，本方法，所述位姿误差包括位置误差和姿态误差，所述实际位姿通过如下方法获得：利用导航型激光扫描传感器建立笛卡尔全局坐标系，再根据车身尺寸参数推算出AGV当前位置点的全局坐标，并以AGV的运动中心的当前位置作为AGV的当前位置；并通过增量编码器和角度编码器实时监测AGV的舵轮的ѱ、vf作为AGV的当前姿态。

进一步地，本方法，通过采集多组AGV运动过程中的速度和加速度信息，通过卡尔曼滤波算法估计出AGV不同时刻的位置信息；建立卡尔曼滤波过程中新息方差理论值的和实际测量值间的匹配度变量，当匹配度偏离1时，通过生成自适应调节因子，校准匹配度变量，保证卡尔曼滤波算法对AGV位置和速度的精准估计。

进一步地，本方法，所述B样条参数方程通过如下方法获得：

步骤A:根据B样条的递推定义，将3阶B样条曲线的点表示如下：P ( t ) = P j - 1 ( k ), t ∈ [ u j , u j + 1 ] , 且P i ( 0 ) = P i P i ( l ) = ( 1 - a i ( l ) ) Pi - 1 ( l - 1 ) + a i ( l ) P i ( l - 1 ) a i ( l ) = t - u j - 1 u j + k - l- 2 - u j - 1 , l = 1,2 , . . . , k , i = j - k + l , . . . , j , j + 1 ; - -- ( 1 )其中t为参数，Pi为B样条曲线第i个控制点；uj为B样条曲线中第j个控 制点的矢量值，它决定了参数t的取值范围；Pi(l)为B样条曲线段第l次递推过程中的比例分位点，它由第l-1次递推过程中确定的Pil两个比例分位点通过 比例值和计算得来；令公式（1）中的k=3，可知i的取值为j-2，j-1，j，j+1，j+2，即确定一段三阶B样条曲线段需要4个控制点：Pj-2、Pj-1、Pj、Pj+1，以及各控制点的矢量值uj-2、uj-1、uj、uj+1（各控制点的矢量值确定了t的取值范围，为了使递 推计算时t的取值范围一致；

步骤B:确定各控制点的矢量值ui，i=0,1,2,3...n，也即参数t的取值范围；

步骤C：将AGV小车的位置信息采样点按照时间先后顺序进行排列，并作为 B样条拟合曲线的各个控制点Pi，i=0,1,2,3...n，从第1个控制点起按顺序取连续的4个采样点作为计算该一段3阶B样条曲线的4个控制点Pj-2、Pj-1、Pj、 Pj+1；

步骤D：按顺序取接下来的4个控制点来计算下一段B样条曲线，重复步骤 3的过程，直至采样点间的所有B样条曲线段都已计算完毕；

步骤E：针对采样点总个数不为4的倍数，即最后一段B样条曲线的控制点可能不够4个的情况，可顺移至前一段B样条曲线的控制点进行补充；

步骤F：联合所有B样条曲线段的表达式，即可得到一个连续光滑的B样条曲线，该曲线与实际的定位终端运行轨迹将具有良好的拟合程度。

进一步地，本方法，当前位置的所述控制点在无障碍区的第一位置时通过前进步骤进行位姿调整；当前位置的至少部分所述控制点在障碍区的第二位置时通过结合后退、前进步骤进行位姿调整，其中所述第一位置包括目标位置、临时障碍或拐角位置处。

本发明所采用的思路是，为了使采集的位置信息采样点之间的曲线能够更加吻合定位终端的移动轨迹，采用B样条曲线拟合其行驶轨迹。B样条曲线是一种多项式曲线的特殊表示形式，通过曲线进行AGV行驶轨迹的拟合具有更好的平滑程度，也就更加符合AGV在行驶状态下的移动轨迹。 并采用三阶B样条算法，三阶B样条算法在线段的节点处具有二阶导连续的特性与实际的车辆行驶轨迹较为吻合并且计算量较小适合在AGV的应用。而且通过考虑到实际的AGV的转动惯量根据AGV行进的曲率调节车速v，并根据小车的车速v调节角速率w；考虑到实际过程中有响应时延，应用卡尔曼滤波以AGV当前位姿预测下几个周期的位姿。从而达到与实际车辆行驶轨迹更好的拟合程度，拟合曲线具有良好的平滑效果。

有益效果：本发明基于B样条曲线的AGV运动控制方法，使得AGV行驶路径更光滑；AGV舵轮摆角均匀，AGV运动更平稳。主要体现在：3阶B样条曲线在曲线的吻合程度和平滑程度上都比较符合车辆轨迹的实际应用场景，可对直线、曲线可统一规划，用相同形式的参数方程表示；二阶导数连续；具有局部特性，便于控制，路径易修改，。而且本发明结合曲率和根据当前位姿预测下几个周期的位姿，因此与实际轨迹吻合程度较高，可以对生成的B样条曲线进行一定程度的调整，在工程中具有更好灵活度从而可以适应更多环境下的应用。除干涉位置外，其他位置调整在行进过程中通过前进步骤中进行，不需要停下或后退调整，在干涉位置，比如目标位置、临时障碍或拐角位置处，才需要通过后退步骤和前进步骤结合进行调整。

附图说明

图1为本发明基于B样条曲线的AGV运动控制方法的原理图。

具体实施方式

现结合附图和具体实施方式对本发明进一步说明。

如图1所示，本发明一种基于B样条曲线的AGV运动控制方法，包括如下步骤：

（1）根据实际需求，规划AGV必须经过的行止站点；

（2）通过所述行止站点反解控制点；

（3）由所述控制点计算得到B样条参数方程,所述B样条为三阶B样条；

（4）用所述B样条参数方程表示AGV的行驶路径；

（5）采用自适应参数更新算法，进行参数更新，得到实时变化的参考点；

（6）结合AGV的实际位姿与期望位姿的误差，得到位姿误差；

（7）考虑到实际的AGV的转动惯量，根据AGV行进路径的曲率调节车速v，并根据小车的车速调节角速率ω；

（8）采用PID控制方法，消除位姿误差，控制AGV按期望路径运动。

进一步地，本方法，控制点选择：首先选取AGV当前位置点作为第一个控制点，即前进或后退路径起始点；再于AGV当前前进或后退方向上选取一个靠近起始点的点作为第二个控制点；其次， 再结合AGV当前前进或后退方向与原目标点要求方向夹角的角平分线上，距离AGV一定距离处选取一点作为倒数第一个控制点，即终止点；再于角平分线上靠近终止点且靠近起始点一侧选取一点作为倒数第二个控制点；最后，选取第二个控制点与倒数第二个控制点间的中间点为新的一个控制点，或者根据需要选择此两点间的其他点作为新控制点。

进一步地，本方法，在步骤（8）之前考虑到实际过程中有响应时延，应用卡尔曼滤波以AGV当前位姿预测下几个周期的位姿。

进一步地，本方法，所述位姿误差包括位置误差和姿态误差，所述实际位姿通过如下方法获得：利用导航型激光扫描传感器建立笛卡尔全局坐标系，再根据车身尺寸参数推算出AGV当前位置点的全局坐标，并以AGV的运动中心的当前位置作为AGV的当前位置；并通过增量编码器和角度编码器实时监测AGV的舵轮的ѱ、vf作为AGV的当前姿态。

进一步地，本方法，通过采集多组AGV运动过程中的速度和加速度信息，通过卡尔曼滤波算法估计出AGV不同时刻的位置信息；建立卡尔曼滤波过程中新息方差理论值的和实际测量值间的匹配度变量，当匹配度偏离1时，通过生成自适应调节因子，校准匹配度变量，保证卡尔曼滤波算法对AGV位置和速度的精准估计。

进一步地，本方法，所述B样条参数方程通过如下方法获得：

步骤A:根据B样条的递推定义，将3阶B样条曲线的点表示如下：P ( t ) = P j - 1 ( k ), t ∈ [ u j , u j + 1 ] , 且P i ( 0 ) = P i P i ( l ) = ( 1 - a i ( l ) ) Pi - 1 ( l - 1 ) + a i ( l ) P i ( l - 1 ) a i ( l ) = t - u j - 1 u j + k - l- 2 - u j - 1 , l = 1,2 , . . . , k , i = j - k + l , . . . , j , j + 1 ; - -- ( 1 )其中t为参数，Pi为B样条曲线第i个控制点；uj为B样条曲线中第j个控 制点的矢量值，它决定了参数t的取值范围；Pi(l)为B样条曲线段第l次递推过程中的比例分位点，它由第l-1次递推过程中确定的Pil两个比例分位点通过 比例值和计算得来；令公式（1）中的k=3，可知i的取值为j-2，j-1，j，j+1，j+2，即确定一段三阶B样条曲线段需要4个控制点：Pj-2、Pj-1、Pj、Pj+1，以及各控制点的矢量值uj-2、uj-1、uj、uj+1（各控制点的矢量值确定了t的取值范围，为了使递 推计算时t的取值范围一致；

步骤B:确定各控制点的矢量值ui，i=0,1,2,3...n，也即参数t的取值范围；

步骤C：将AGV小车的位置信息采样点按照时间先后顺序进行排列，并作为 B样条拟合曲线的各个控制点Pi，i=0,1,2,3...n，从第1个控制点起按顺序取连续的4个采样点作为计算该一段3阶B样条曲线的4个控制点Pj-2、Pj-1、Pj、 Pj+1；

步骤D：按顺序取接下来的4个控制点来计算下一段B样条曲线，重复步骤 3的过程，直至采样点间的所有B样条曲线段都已计算完毕；

步骤E：针对采样点总个数不为4的倍数，即最后一段B样条曲线的控制点可能不够4个的情况，可顺移至前一段B样条曲线的控制点进行补充；

步骤F：联合所有B样条曲线段的表达式，即可得到一个连续光滑的B样条曲线，该曲线与实际的定位终端运行轨迹将具有良好的拟合程度。

进一步地，本方法，当前位置的所述控制点在无障碍区的第一位置时通过前进步骤进行位姿调整；当前位置的至少部分所述控制点在障碍区的第二位置时通过结合后退、前进步骤进行位姿调整，其中所述第一位置包括目标位置、临时障碍或拐角位置处。

为了使采集的位置信息采样点之间的曲线能够更加吻合定位终端的移动轨迹，采用B样条曲线拟合其行驶轨迹。B样条曲线是一种多项式曲线的特殊表示形式，通过曲线进行车辆行驶轨迹的拟合具有更好的平滑程度，也就更 加符合车辆在行驶状态下的移动轨迹。另外，由于B样条曲线具有良好的局部 性质，在拟合车辆运行轨迹的过程中，即使个别采样的误差较大，对拟合曲线 的影响也只是局部的并不会对整体产生影响，也就是说使用B样条曲线的拟合 过程具有良好的统计特性和滤波作用。

尽管结合优选实施方案具体展示和介绍了本发明，但所属领域的技术人员 应该明白，在不脱离所附权利要求书所限定的本发明的精神和范围内，在形式 上和细节上可以对本发明做出各种变化，均为本发明的保护范围。

Claims (7)

1.一种基于B样条曲线的AGV运动控制方法，其特征在于，包括如下步骤：

（1）根据实际需求，规划AGV必须经过的行止站点；

（2）通过所述行止站点反解控制点；

（3）由所述控制点计算得到B样条参数方程,所述B样条为三阶B样条；

（4）用所述B样条参数方程表示AGV的行驶路径；

（5）采用自适应参数更新算法，进行参数更新，得到实时变化的参考点；

（6）结合AGV的实际位姿与期望位姿的误差，得到位姿误差；

（7）考虑到实际的AGV的转动惯量，根据AGV行进路径的曲率调节车速v，并根据小车的车速调节角速率ω；

（8）采用PID控制方法，消除位姿误差，控制AGV按期望路径运动。

2.根据权利要求1所述的智能叉车的位姿调整路径规划方法，其特征在于，控制点选择：首先选取AGV当前位置点作为第一个控制点，即前进或后退路径起始点；再于AGV当前前进或后退方向上选取一个靠近起始点的点作为第二个控制点；其次， 再结合AGV当前前进或后退方向与原目标点要求方向夹角的角平分线上，距离AGV一定距离处选取一点作为倒数第一个控制点，即终止点；再于角平分线上靠近终止点且靠近起始点一侧选取一点作为倒数第二个控制点；最后，选取第二个控制点与倒数第二个控制点间的中间点为新的一个控制点，或者根据需要选择此两点间的其他点作为新控制点。

3.根据权利要求2所述的一种基于B样条曲线的AGV运动控制方法，其特征在于，在步骤（8）之前考虑到实际过程中有响应时延，应用卡尔曼滤波以AGV当前位姿预测下几个周期的位姿。

4.根据权利要求3所述的一种基于B样条曲线的AGV运动控制方法，其特征在于，所述位姿误差包括位置误差和姿态误差，所述实际位姿通过如下方法获得：利用导航型激光扫描传感器建立笛卡尔全局坐标系，再根据车身尺寸参数推算出AGV当前位置点的全局坐标，并以AGV的运动中心的当前位置作为AGV的当前位置；并通过增量编码器和角度编码器实时监测AGV的舵轮的ѱ、vf作为AGV的当前姿态。

5.根据权利要求4所述的一种基于B样条曲线的AGV运动控制方法，其特征在于，通过采集多组AGV运动过程中的速度和加速度信息，通过卡尔曼滤波算法估计出AGV不同时刻的位置信息；建立卡尔曼滤波过程中新息方差理论值的和实际测量值间的匹配度变量，当匹配度偏离1时，通过生成自适应调节因子，校准匹配度变量，保证卡尔曼滤波算法对AGV位置和速度的精准估计。

6.根据权利要求4所述的一种基于B样条曲线的AGV运动控制方法，其特征在于，所述B样条参数方程通过如下方法获得：

步骤A:根据B样条的递推定义，将3阶B样条曲线的点表示如下：P ( t ) = P j - 1 ( k ) ,t ∈ [ u j , u j + 1 ] , 且P i ( 0 ) = P i P i ( l ) = ( 1 - a i ( l ) ) P i- 1 ( l - 1 ) + a i ( l ) P i ( l - 1 ) a i ( l ) = t - u j - 1 u j + k - l -2 - u j - 1 , l = 1,2 , . . . , k , i = j - k + l , . . . , j , j + 1 ; - - -( 1 )其中t为参数，Pi为B样条曲线第i个控制点；uj为B样条曲线中第j个控 制点的矢量值，它决定了参数t的取值范围；Pi(l)为B样条曲线段第l次递推过程中的比例分位点，它由第l-1次递推过程中确定的Pil两个比例分位点通过 比例值和计算得来；令公式（1）中的k=3，可知i的取值为j-2，j-1，j，j+1，j+2，即确定一段三阶B样条曲线段需要4个控制点：Pj-2、Pj-1、Pj、Pj+1，以及各控制点的矢量值uj-2、uj-1、uj、uj+1（各控制点的矢量值确定了t的取值范围，为了使递 推计算时t的取值范围一致；

步骤B:确定各控制点的矢量值ui，i=0,1,2,3...n，也即参数t的取值范围；

步骤C：将AGV小车的位置信息采样点按照时间先后顺序进行排列，并作为 B样条拟合曲线的各个控制点Pi，i=0,1,2,3...n，从第1个控制点起按顺序取连续的4个采样点作为计算该一段3阶B样条曲线的4个控制点Pj-2、Pj-1、Pj、 Pj+1；

步骤D：按顺序取接下来的4个控制点来计算下一段B样条曲线，重复步骤 3的过程，直至采样点间的所有B样条曲线段都已计算完毕；

步骤E：针对采样点总个数不为4的倍数，即最后一段B样条曲线的控制点可能不够4个的情况，可顺移至前一段B样条曲线的控制点进行补充；

步骤F：联合所有B样条曲线段的表达式，即可得到一个连续光滑的B样条曲线，该曲线与实际的定位终端运行轨迹将具有良好的拟合程度。

7.根据权利要求5所述的一种基于B样条曲线的AGV运动控制方法，其特征在于，当前位置的所述控制点在无障碍区的第一位置时通过前进步骤进行位姿调整；当前位置的至少部分所述控制点在障碍区的第二位置时通过结合后退、前进步骤进行位姿调整，其中所述第一位置包括目标位置、临时障碍或拐角位置处。

Images