基于B样条曲线的路径规划方法、运动控制方法及系统技术方案

本发明专利技术公开了一种基于B样条曲线的路径规划方法，包括：根据获取的站点坐标选取型值点，选取的型值点个数至少为B样条阶数加1；根据选取的型值点坐标计算得到控制点；由控制点计算得到B样条参数方程，用B样条参数方程表示规划的路径。可以根据B样条路径动态规划动态选取型值点，型值点选取少，可以根据型值点计算得到控制点，计算过程简单。根据曲率调节车速，并根据小车的车速调节角速率，采用PID控制方法，消除位姿误差，控制机器人按期望路径运动。控制机器人按期望路径运动。控制机器人按期望路径运动。

【技术实现步骤摘要】
基于B样条曲线的路径规划方法、运动控制方法及系统


[0001]本专利技术涉及机器人的路径规划及控制
，具体地涉及一种适用于AGV的基于B样条曲线的路径规划方法、运动控制方法及系统。

技术介绍

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

技术实现思路

[0004]为了解决上述存在的技术问题，本专利技术提供了一种基于B样条曲线的路径规划方法、运动控制方法及系统，可以根据B样条路径动态规划动态选取型值点，型值点选取少，可以根据型值点计算得到控制点，计算过程简单。根据曲率调节车速，并根据小车的车速调节角速率，采用PID控制方法，消除位姿误差，控制机器人按期望路径运动。
[0005]本专利技术的技术方案是：
[0006]一种基于B样条曲线的路径规划方法，包括以下步骤：
[0007]S01：根据获取的站点坐标选取型值点，选取的型值点个数至少为B样条阶数加1；
[0008]S02：根据选取的型值点坐标计算得到控制点；
[0009]S03：由控制点计算得到B样条参数方程，用B样条参数方程表示规划的路径。
[0010]优选的技术方案中，所述步骤S02中计算控制点的方法包括：
[0011]得到n个型值点的点列Q1，Q2，
…
，Q
n
，将这n个型值点作为n-1段B样条曲线的端点，根据曲线的端点性质得到下列方程：
[0012]P
i-1
+4P
i
+P
i+1
＝6Q
i
(i＝1，2，
…
，n)；
[0013]添加两个边界条件：
[0014]5P0+P1＝6Q1；
[0015]P
n
+5P
n+1
＝6Q
n
[0016]其中P为控制点，Q为型值点；
[0017]用矩阵表示方程组：
[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样条参数本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于B样条曲线的路径规划方法，其特征在于，包括以下步骤：S01：根据获取的站点坐标选取型值点，选取的型值点个数至少为B样条阶数加1；S02：根据选取的型值点坐标计算得到控制点；S03：由控制点计算得到B样条参数方程，用B样条参数方程表示规划的路径。2.根据权利要求1所述的基于B样条曲线的路径规划方法，其特征在于，所述步骤S02中计算控制点的方法包括：得到n个型值点的点列Q1，Q2，
…
，Q
n
，将这n个型值点作为n-1段B样条曲线的端点，根据曲线的端点性质得到下列方程：P
i-1
+4P
i
+P
i+1
＝6Q
i
(i＝1，2，
…
，n)；添加两个边界条件：5P0+P1＝6Q1；P
n
+5P
n+1
＝6Q
n
其中P为控制点，Q为型值点；用矩阵表示方程组：将系数矩阵分解为一个对角线上元素为1的下二对角矩阵L和一个上二对角矩阵U的相乘，即AP＝LUP＝Q；令UP＝y，则Ly＝Q，求得y，进一步由UP＝y，求得控制点P。3.根据权利要求1所述的基于B样条曲线的路径规划方法，其特征在于，所述步骤S03中，B样条阶次为3，得到B样条基函数：第i段B样条曲线为：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]；其中，m为控制点个数，s为B样条曲线的变量。4.一种基于B样条曲线的路径的运动控制方法，其特征在于，通过权利要求1-3任一种基于B样条曲线的路径规划方法得到B样条路径；
根据实时变化的目标参考点进行运动控制；得到B样条曲线的更新机器人运动中心角度δ
f
和机器人运动中心的速度v，控制机器人以恒定速度v运动时，控制参数为：其中，(p(s),q(s))为目标参考点坐标，ψ
d
为当前运动中心位置与目标参考点的连线与x轴的夹角，ψ为机器人运行方向与x轴的夹角，k为经验值，θ
r
为B样条曲线的切线夹角，v＝const为常量，参数γ＝αvcos(ψ
d-ψ)，α＞0，d
ρ
＝1/α，d
ρ
表示ρ的上确界，ρ为当前运动中心位置与目标参考点的距离。5.根据权利要求4所述的基于B样条曲线的路径的运动控制方法，其特征在于，控制机器人根据曲率调节速度时，B样条曲线的更新由一阶导的变化得到：其中v0是期望速度，避免奇点的ψ
d
计算方式：其中，ε是一个常数，是ρ的边界值；计算运动中心角度：其中，计算运动中心速度：v＝γρcos(Δψ)其中，Δψ＝ψ
d-ψ，γ为参数。6.根据权利要求4所述的基于B样条曲线的路径的运动控制方法，其特征在于，还包括应用卡尔曼滤波以机器人当前位姿预测下几个周期的位姿，通过卡尔曼滤波算法对状态量和协方差的更新计算，具体包括以下步骤：对位姿观测系统进行建模，当前位姿的预测值μ
t-1
表示上一时刻的位姿，A
t
、B
t
为系...

【专利技术属性】
技术研发人员：刘胜明，甄武斌，司秀芬，
申请(专利权)人：苏州艾吉威机器人有限公司，
类型：发明
国别省市：