基于B样条曲线的路径规划方法、运动控制方法及系统与流程

基于b样条曲线的路径规划方法、运动控制方法及系统
技术领域
1.本发明涉及机器人的路径规划及控制技术领域，具体地涉及一种适用于agv的基于b样条曲线的路径规划方法、运动控制方法及系统。


背景技术：

2.自动导引装置工作的过程中一般需要对其从起始点到目标点的路径进行规划。常用的路径规划方式有很多，比如规划路径为曲线，可使用插值的方式生成多项式插值。为了使曲线更容易计算，避免龙格现象，低阶可实现，可使用样条曲线插值，常见的样条曲线有贝塞尔曲线，b样条曲线等。为了使多项式次数独立于控制点数目，局部控制曲线路径，采用了b样条曲线进行路径规划而不用贝塞尔曲线。如果使用b样条曲线进行路径规划，主要输出内容为控制点和样条次数，据此可算出路径。自动导引装置根据路径行走，无限接近路径，但现有技术中，因为自动导引装置本身的机械参数，控制和定位算法，以及整个系统的影响，自动导引装置与路径之间是有误差的，但会尽量将误差控制在合理范围内，使得自动导引装置到达目标点时与目标点的位置之间的误差在允许范围内。
3.样条曲线的绘制在于样条曲线的控制点，但人工控制样条曲线的控制点时，可能造成样条曲线的曲率过大从而导致agv在运行当中偏转过快，造成不必要的损失。


技术实现要素：

4.为了解决上述存在的技术问题，本发明提供了一种基于b样条曲线的路径规划方法、运动控制方法及系统，可以根据b样条路径动态规划动态选取型值点，型值点选取少，可以根据型值点计算得到控制点，计算过程简单。根据曲率调节车速，并根据小车的车速调节角速率，采用pid控制方法，消除位姿误差，控制机器人按期望路径运动。
5.本发明的技术方案是：
6.一种基于b样条曲线的路径规划方法，包括以下步骤：
7.s01：根据获取的站点坐标选取型值点，选取的型值点个数至少为b样条阶数加1；
8.s02：根据选取的型值点坐标计算得到控制点；
9.s03：由控制点计算得到b样条参数方程，用b样条参数方程表示规划的路径。
10.优选的技术方案中，所述步骤s02中计算控制点的方法包括：
11.得到n个型值点的点列q1，q2，
…
，q
n
，将这n个型值点作为n-1段b样条曲线的端点，根据曲线的端点性质得到下列方程：
12.p
i-1
+4p
i
+p
i+1
＝6q
i
(i＝1，2，
…
，n)；
13.添加两个边界条件：
14.5p0+p1＝6q1；
15.p
n
+5p
n+1
＝6q
n
16.其中p为控制点，q为型值点；
17.用矩阵表示方程组：
[0018][0019]
将系数矩阵分解为一个对角线上元素为1的下二对角矩阵l和一个上二对角矩阵u的相乘，即ap＝lup＝q；
[0020]
令up＝y，则ly＝q，求得y，进一步由up＝y，求得控制点p。
[0021]
优选的技术方案中，所述步骤s03中，b样条阶次为3，得到b样条基函数：
[0022][0023]
第i段b样条曲线为：
[0024]
bsplines
i
＝g
0,3
(s)p
i
+g
1,3
(s)p
i+1
+g
2,3
(s)p
i+2
+g
3,3
(s)p
i+3
i∈[0,m-2]；
[0025]
其中，m为控制点个数，s为b样条曲线的变量。
[0026]
本发明还公开了一种基于b样条曲线的路径的运动控制方法，通过上述基于b样条曲线的路径规划方法得到b样条路径；
[0027]
根据实时变化的目标参考点进行运动控制；
[0028]
得到b样条曲线的更新机器人运动中心角度δ
f
和机器人运动中心的速度v，控制机器人以恒定速度v运动时，控制参数为：
[0029][0030]
其中，(p(s),q(s))为目标参考点坐标，ψ
d
为当前运动中心位置与目标参考点的连线与x轴的夹角，ψ为机器人运行方向与x轴的夹角，k为经验值，θ
r
为b样条曲线的切线夹角，v＝const为常量，参数γ＝αvcos(ψ
d-ψ)，α＞0，d
ρ
＝1/α，d
ρ
表示ρ的上确界，ρ为当前运动中心位置与目标参考点的距离。
[0031]
优选的技术方案中，控制机器人根据曲率调节速度时，b样条曲线的更新由一阶
导的变化得到：其中v0是期望速度，是期望速度，
[0032]
避免奇点的ψ
d
计算方式：
[0033][0034]
其中，ε是一个常数，是ρ的边界值；
[0035]
计算运动中心角度：
[0036][0037]
其中，
[0038]
计算运动中心速度：
[0039]
v＝γρcos(δψ)
[0040]
其中，δψ＝ψ
d-ψ，γ为参数。
[0041]
优选的技术方案中，还包括应用卡尔曼滤波以机器人当前位姿预测下几个周期的位姿，通过卡尔曼滤波算法对状态量和协方差的更新计算，具体包括以下步骤：
[0042]
对位姿观测系统进行建模，当前位姿的预测值μ
t-1
表示上一时刻的位姿，a
t
、b
t
为系统模型参数，u
t
表示t时刻对系统模型的控制量；
[0043]
计算t时刻当前位姿的预测值对应的协方差σ
t-1
为上一时刻的位姿μ
t-1
对应的协方差，为a
t
的转置矩阵，r
t
表示系统模型的协方差；
[0044]
计算卡尔曼增益k
t
，c
t
为测量系统的参数，为c
t
的转置，q
t
表示测量误差的协方差；
[0045]
根据当前位姿的预测值其对应的协方差以及卡尔曼滤波增益k
t
得到时刻t的最优估算值μ
t
，其中z
t
为当前位姿的测量值；
[0046]
更新最优估算值μ
t
所对应的协方差σ
t
，i为单元矩阵。
[0047]
本发明又公开了一种基于b样条曲线的路径规划系统，包括：
[0048]
型值点选取模块：根据获取的站点坐标选取型值点，选取的型值点个数至少为b样条阶数加1；
[0049]
控制点计算模块：根据选取的型值点坐标计算得到控制点；
[0050]
路径规划模块：由控制点计算得到b样条参数方程，用b样条参数方程表示规划的路径。
[0051]
优选的技术方案中，所述计算控制点的方法包括：
[0052]
得到n个型值点的点列q1，q2，
…
，q
n
，将这n个型值点作为n-1段b样条曲线的端点，根据曲线的端点性质得到下列方程：
[0053]
p
i-1
+4p
i
+p
i+1
＝6q
i
(i＝1，2，
…
，n)；
[0054]
添加两个边界条件：
[0055]
5p0+p1＝6q1；
[0056]
p
n
+5p
n+1
＝6q
n
[0057]
其中p为控制点，q为型值点；
[0058]
用矩阵表示方程组：
[0059][0060]
将系数矩阵分解为一个对角线上元素为1的下二对角矩阵l和一个上二对角矩阵u的相乘，即ap＝lup＝q；
[0061]
令up＝y，则ly＝q，求得y，进一步由up＝y，求得控制点p。
[0062]
本发明又公开了一种基于b样条曲线的路径的运动控制系统，包括上述基于b样条曲线的路径规划系统和运动控制模块；
[0063]
通过基于b样条曲线的路径规划系统得到b样条路径；
[0064]
所述运动控制模块，根据实时变化的目标参考点进行运动控制；
[0065]
得到b样条曲线的更新机器人运动中心角度δ
f
和机器人运动中心的速度v，控制机器人以恒定速度v运动时，控制参数为：
[0066][0067]
其中，(p(s),q(s))为目标参考点坐标，ψ
d
为当前运动中心位置与目标参考点的连线与x轴的夹角，ψ为机器人运行方向与x轴的夹角，k为经验值，θ
r
为b样条曲线的切线夹角，v＝const为常量，参数γ＝αvcos(ψ
d-ψ)，α＞0，d
ρ
＝1/α，d
ρ
表示ρ的上确界，ρ为当前运动中心位置与目标参考点的距离。
[0068]
优选的技术方案中，所述运动控制模块控制机器人根据曲率调节速度时，b样条曲线的更新由一阶导的变化得到：其中v0是期望速度
[0069]
避免奇点的ψ
d
计算方式：
[0070][0071]
其中，ε是一个常数，是ρ的边界值；
[0072]
计算运动中心角度：
[0073][0074]
其中，
[0075]
计算运动中心速度：
[0076]
v＝γρcos(δψ)
[0077]
其中，δψ＝ψ
d-ψ，γ为参数。
[0078]
与现有技术相比，本发明的优点是：
[0079]
1、可以根据b样条路径动态规划动态选取型值点，型值点选取少，可以根据型值点计算得到控制点，计算过程简单，得到的路径更光滑。
[0080]
2、根据曲率调节车速，并根据小车的车速调节角速率，采用pid控制方法，消除位姿误差，控制agv按期望路径运动，使得agv运行更加平稳。
附图说明
[0081]
下面结合附图及实施例对本发明作进一步描述：
[0082]
图1为本发明基于b样条曲线的路径规划方法的流程图；
[0083]
图2为本发明一实施例的路径示意图；
[0084]
图3为本发明较佳的实施例的基于b样条曲线的路径的运动控制方法的流程图；
[0085]
图4为本发明控制原理图；
[0086]
图5为本发明运动控制的关系示意图；
[0087]
图6为本发明基于b样条曲线的路径规划系统的原理框图；
[0088]
图7为本发明基于b样条路径的运动控制系统的原理框图。
具体实施方式
[0089]
为使本发明的目的、技术方案和优点更加清楚明了，下面结合具体实施方式并参照附图，对本发明进一步详细说明。应该理解，这些描述只是示例性的，而并非要限制本发明的范围。此外，在以下说明中，省略了对公知结构和技术的描述，以避免不必要地混淆本发明的概念。
[0090]
实施例：
[0091]
下面结合附图，对本发明的较佳实施例作进一步说明。
[0092]
如图1所示，一种基于b样条曲线的路径规划方法，包括以下步骤：
[0093]
s01：根据获取的站点坐标选取型值点，选取的型值点个数至少为b样条阶数加1；
[0094]
s02：根据选取的型值点坐标计算得到控制点；
[0095]
s03：由控制点计算得到b样条参数方程，用b样条参数方程表示规划的路径。
[0096]
型值点的选取：
[0097]
(1)b样条路径不发生变更(b样条路径静态规划)：
[0098]
a、路径上选取的型值点个数应至少为b样条阶数加1；
[0099]
b、简单曲线路径(如90度弧线)应至少选取顶点作为型值点；
[0100]
c、复杂曲线可适当多选几个型值点，包括路径上的点和不在路径上的顶点；
[0101]
d、直线上的型值点选取路径上的点。
[0102]
(2)b样条路径动态规划：
[0103]
b样条路径规划中最重要的部分就是型值点的选择，由于非b样条期望路径的下发方式发生了变化，那么相应的b样条路径选择型值点的过程也应随之改变。b样条路径动态规划的型值点选点应当遵循以下原则：
[0104]
1、型值点应包含路径上所有的站点，且型值点数量总是多于站点数量；
[0105]
2、机器人(agv)在路径起点时，下发的该小段路径所包含的站点数至少为3个，以后每更新一次至少增加一个站点；
[0106]
3、每一小段直线路径的最后两个站点的中点应作为一个型值点，圆弧顶点应作为一个型值点；
[0107]
4、每一段新的路径的下发时机应不晚于车辆到达上段路径的倒数第二个站点。
[0108]
简单规则路径的控制点选取
[0109]
简单的规则路径如：直线、90度弧线、180度调头、平行直线迁移及这些路径类型的组合路径。这种简单路径选取控制点的方法较为简单，即将型值点的第一个点和最后一个点扩展为双重点，前两个控制点重合且等于第一个型值点，最后两个控制点重合且等于最后一个型值点。
[0110]
如图2所示的实际路径多由简单路径拼接而成，因此控制点的选点方法为将型值点的第一个点和最后一个点扩展为双重点，前两个控制点重合且等于第一个型值点，最后两个控制点重合且等于最后一个型值点。
[0111]
图中有四段路径，分别按照时间窗动态下发，其中q0到q15是路径的型值点，p0到p17是路径的控制点。
[0112]
[0113][0114]
复杂不规则路径的控制点计算—反解控制点
[0115]
对于复杂的不规则路径，直接由型值点得到控制点比较困难，可以用以下方法通过型值点反求控制点。
[0116]
得到n个型值点的点列q1，q2，
…
，q
n
，将这n个型值点作为n-1段b样条曲线的端点，由3阶b样条性质可以得到，一整条b样条曲线有n+2个控制点。根据曲线的端点性质可以得到：
[0117]
p
i-1
+4p
i
+p
i+1
＝6q
i
(i＝1，2，
…
，n)
[0118]
但是此时作为未知量的控制点的个数为n+2个，而上式只能写出n个方程，由此可以看出方程数量不够，从而无法解出唯一的一组控制点。因此，需要添加两个边界条件：
[0119]
5p0+p1＝6q1[0120]
p
n
+5p
n+1
＝6q
n
[0121]
由此我们可以将上述问题用线性方程组的形式来表示：
[0122][0123]
上式中，系数矩阵是三对角矩阵，其秩为n+2，控制点存在唯一的一组解。由于此系数矩阵比较特殊，且为了减小求解控制点的算法复杂度，我们不直接对系数矩阵求逆而是采用追赶法来进行求解。
[0124]
将系数矩阵a分解为：
[0125]
a＝lu
[0126]
其中，l为一个对角线上元素为1的下二对角矩阵，u为一个上二对角矩阵u，即：
[0127][0128]
[0129][0130]
令
[0131]
那么原线性方程组就可以写成：
[0132]
ap＝lup＝q
[0133]
令up＝y，则：
[0134]
ly＝q
[0135]
l是对角元为1的二对角矩阵，因此易由上式求得y。在得到y以后：
[0136]
up＝y
[0137]
由于u为二对角矩阵，即可通过线性回代求得控制点p。
[0138]
由控制点计算得到b样条参数方程方法：
[0139]
我们用的是三阶b样条，b样条阶次为3，得到b样条基函数：
[0140][0141]
第i段b样条曲线为：
[0142]
bsplines
i
＝g
0,3
(s)p
i
+g
1,3
(s)p
i+1
+g
2,3
(s)p
i+2
+g
3,3
(s)p
i+3
i∈[0,m-2]；
[0143]
其中，m为控制点个数，s为b样条曲线的变量。
[0144]
本发明还公开了一种基于b样条曲线的路径的运动控制方法，通过上述基于b样条曲线的路径规划方法得到b样条路径，在得到的b样条路径上进行运动控制。
[0145]
较佳的实施例中，基于b样条曲线的路径的运动控制方法如图3所示，完整的步骤包括以下步骤：
[0146]
1、根据实际需求，对agv必须经过的点(即站点)进行规划；
[0147]
2、通过上述站点(此时可称为型值点)反解控制点；
[0148]
3、由控制点计算得到b样条参数方程(我们用的是三阶b样条)；
[0149]
4、用上述b样条参数方程表示agv的行驶路径；
[0150]
5、采用自适应参数更新算法，进行参数更新，得到实时变化的参考点；
[0151]
6、结合agv的实际位姿，得到位姿误差；
[0152]
7、考虑到实际的agv小车有转动惯量，根据曲率调节车速，并根据小车的车速调节角速率；
[0153]
8、考虑到实际过程中有响应时延，应用卡尔曼滤波以agv当前位姿预测下几个周期的位姿；
[0154]
9、计及7、8所述因素，采用pid控制方法，消除位姿误差，控制agv按期望路径运动。
[0155]
控制原理框图如图4所示。
[0156]
运动控制包括以下步骤：
[0157]
根据实时变化的目标参考点进行运动控制；
[0158]
得到b样条曲线的更新机器人运动中心角度δ
f
和机器人运动中心的速度v。
[0159]
运动控制包括控制机器人以恒定速度v运动，和控制机器人根据曲率调节速度。
[0160]
具体的控制算法中的控制模型如图5所示：
[0161]
agv运动中心的当前位置表示agv的实时位姿，即(x,y)，(p(s),q(s))表示agv运动的目标参考点，在b样条路径上。
[0162]
图5中ψ
d
＝atan2(δy,δx)。通过b样条路径的生成中计算，我们得到了b样条路径，其表达形式是分段的参数方程，因此图中参考点可以表示为：
[0163]
x
d
＝p(s)
[0164]
y
d
＝q(s)
[0165]
其中s的更新由其一阶导的变化实现。
[0166]
控制机器人以恒定速度v运动时：
[0167]
小车运行速度是一个常数，设定一个参数γ，将速度和角度的控制简单化以及满足ρ有上确界，需满足：
[0168][0169]
d表示期望位姿，d
ρ
表示ρ的上确界，其中为ρ的一阶导数，为d
ρ
的一阶导数。
[0170]
由于则
[0171]
因此(1)式可表示为：
[0172][0173]
因为因此：
[0174][0175]
ψ表示车体运行方向与x轴的夹角；
[0176]
则根据得到：
[0177][0178]
给定γ＝αvcos(ψ
d-ψ)，α＞0，d
ρ
＝1/α，θ
r
为b样条曲线s的切线夹角，因此(3)式可表示为：
[0179][0180]
因此得到控制算法如下：
[0181][0182]
δ
f
为运动中心角度，k为经验值，v表示运动中心的速度，为常数。表示b样条曲线的更新。
[0183]
控制机器人根据曲率调节速度，并根据小车的车速调节角速率：
[0184]
图5中δx＝x
d-x,δy＝y
d-y，ψ
d
＝atan2(δy,δx)。通过计算得到了b样条路径，其表达形式是分段的参数方程，因此图中参考点可以表示为：
[0185]
x
d
＝p(s)
[0186]
y
d
＝q(s)
[0187]
其中s的更新由其一阶导的变化实现：
[0188][0189]
其中v0是期望速度，
[0190]
避免奇点的ψ
d
计算方式：
[0191][0192]
其中θ
r
是参考点切线倾角，ε是一个常数，是ρ的边界值。运动中心角度的计算：
[0193][0194]
其中
[0195]
运动中心速度计算：
[0196]
v＝γρcos(δψ)
[0197]
其中δψ＝ψ
d-ψ
[0198]
因此得到控制算法如下：
[0199][0200]
应用卡尔曼滤波以机器人当前位姿预测下几个周期的位姿，通过卡尔曼滤波算法对状态量和协方差的更新计算，具体包括以下步骤：
[0201]
对位姿观测系统进行建模，当前位姿的预测值μ
t-1
表示上一时刻的位姿，a
t
、b
t
为系统模型参数，u
t
表示t时刻对系统模型的控制量；
[0202]
计算t时刻当前位姿的预测值对应的协方差σ
t-1
为上一时刻的位姿μ
t-1
对应的协方差，为a
t
的转置矩阵，r
t
表示系统模型的协方差；
[0203]
计算卡尔曼增益k
t
，c
t
为测量系统的参数，为c
t
的转置，q
t
表示测量误差的协方差；
[0204]
根据当前位姿的预测值其对应的协方差以及卡尔曼滤波增益k
t
得到时刻t的最优估算值μ
t
，其中z
t
为当前位姿的测量值；
[0205]
更新最优估算值μ
t
所对应的协方差σ
t
，i为单元矩阵。
[0206]
如图6所示，本发明又公开了一种基于b样条曲线的路径规划系统，包括：
[0207]
型值点选取模块：根据获取的站点坐标选取型值点，选取的型值点个数至少为b样条阶数加1；
[0208]
控制点计算模块：根据选取的型值点坐标计算得到控制点；
[0209]
路径规划模块：由控制点计算得到b样条参数方程，用b样条参数方程表示规划的路径。
[0210]
如图7所示，本发明又公开了一种基于b样条曲线的路径的运动控制系统，包括上述基于b样条曲线的路径规划系统和运动控制模块；
[0211]
通过基于b样条曲线的路径规划系统得到b样条路径；
[0212]
所述运动控制模块，根据实时变化的目标参考点进行运动控制；
[0213]
得到b样条曲线的更新机器人运动中心角度δ
f
和机器人运动中心的速度v，控制机器人以恒定速度v运动时，控制参数为：
[0214][0215]
其中，(p(s),q(s))为目标参考点坐标，ψ
d
为当前运动中心位置与目标参考点的连线与x轴的夹角，ψ为机器人运行方向与x轴的夹角，k为经验值，θ
r
为b样条曲线的切线夹角，v＝const为常量，参数γ＝αvcos(ψ
d-ψ)，α＞0，d
ρ
＝1/α，d
ρ
表示ρ的上确界，ρ为当前运动中心位置与目标参考点的距离。
[0216]
运动控制模块控制机器人根据曲率调节速度时，控制参数为：
[0217][0218]
应当理解的是，本发明的上述具体实施方式仅仅用于示例性说明或解释本发明的原理，而不构成对本发明的限制。因此，在不偏离本发明的精神和范围的情况下所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。此外，本发明所附权利要求旨在涵盖落入所附权利要求范围和边界、或者这种范围和边界的等同形式内的全部变化和修改例。