基于B-样条曲线的AGV路径规划与控制

基于B-样条曲线的AGV路径规划与控制

Automated Guided Vehicle,简称AGV,通常也称为AGV小车,指装备有电磁或光学等自动导航装置,能够沿规定的导航路径行驶,具有安全保护以及各种移载功能的运输车[1]。为了满足工厂对AGV小车行走路线以及多AGV小车调度系统的路径规划需求,简单的直线与斜线已经很难满足工业化AGV的高效率需求。文献[2-3]分别提出了Field-D和Theta的路径规划算法,但其需要对运动空间进行离散化,从而导致其轨迹曲率不连续,难以使用控制算法优化;文献[4]设计了一种基于Bezier曲线的具有连续曲率的路径规划算法,但曲线的灵活性较差,不能修改曲线的局部控制点;而文献[5]提出了B-样条方法,其具有连续的曲率,并且在有限阶内都十分平滑,而内在的量却是分段的,相较于Bezier曲线可以更好地控制曲线形状。

本文基于 B-样条曲线[6],设计了一套完善的曲线路径规划以及改进的LQR控制算法,可以使小车能够实现足够平滑的曲线移动轨迹的同时,确保 LQR 算法提供控制量的平稳性,提高工业重载AGV的安全性。

1 B-样条曲线

B-样条曲线是由 B-样条基曲线的线性组合,而 B-样条基函数是给定区间上的所有样条函数组成一个线性空间的基函数。一般情况下,一条 k 次的 B-样条曲线由 n+1 个控制点 Pi(i=0,1,… ,n)以及一个节点向量 U={u0,u1,… ,um}所定义,依次连接这些控制点可以构成一个控制曲线的特征多边形,k+1 阶(k次)B-样条曲线的表达式如式(1)所示(2≤k≤n+1):

$$ p(u)= \sum_{i=0}^n P_iN_{i,k}(u) \tag{1}$$

其中$N_{i,k}(u)$是 k 次 B-样条基函数,式(2)为其基函数递归公式:

$$\begin{cases}
N_{i,o}(u)={\begin{cases}
1&\ \quad {若ui\leqslant u\leqslant u_{i+1}}\\
0&\ \quad{其它}
\end{cases}}\\
N_{i,k}(u)=\dfrac {(u-u_i)N_{i,k=1}(u)}{u_{i+k}-u_i}+\dfrac{(u_{i+k1}-u)N_{i+1,k=1}(u)}{u_{i+k+1}-u_{i+1}},u_k\leqslant u \leqslant u_{n+1}
\end{cases}
\tag{2}$$

由式(1)、式(2),可以得出设计一条 B-样条曲线,我们需要的 n+1 个控制点、m+1 个节点向量,以及曲线的阶次$\rho$之间,需要满足条件 m=n+$\rho$+1。 所以可以根据修改 m、n、$\rho$ 这三个参数来控制曲线的形状。

而对于一条 B-样条曲线,我们对公式(1)进行求导可得其导数如式(3)所示:

$$\frac{d}{du}C(u)=C^‘(u)=\sum_{i=0}^{n-1}N_{i+1,p-1}(u)Q_i \tag{3}$$

考虑到工业 AGV 一般都是多车共同作业以及导航命令基本由调度系统下发, 为了让曲线能够同时衔接直线与斜线的路径需求, 我们需要将曲线第一个节点向量与最后一个节点向量相切[7],如图 1 所示。 而由曲线的导数性质可知,需要将第一个节点和最后一个节点的重复度设置为$\rho$+1。

图片[1]-基于B-样条曲线的AGV路径规划与控制
图 1 clamped 与非 clamped 曲线

2 LQR 线性二次型调节器及其改进

LQR(Linear Quadratic Regulator)即线性二次型调节器,其对象是现代控制理论中以状态空间形式给出的线性系统,而目标函数为对象状态和控制输入的二次型函数。 LQR 最优设计是指设计出的状态反馈控制器 K 要使二次型目标函数 J取最小值[8-9]。 小车运动学模型如图 2 所示。

图片[2]-基于B-样条曲线的AGV路径规划与控制
图 2 小车运动学模型

针对 AGV 小车的 LQR 控制器一般为如下公式:

$$
\chi=\begin{bmatrix}
\dot x-\dot x_r \\
\dot y-\dot y_r \\
\dot {\phi}-\dot {\phi}r \\ \end{bmatrix} ,u=\begin{bmatrix} v-v_r \\ \delta -\delta_r \\ \end{bmatrix}\tag{4}$$

$$x(k+1)=\begin{bmatrix} 1&0&-Tv_r\sin{\phi_1}\\ 0&1&Tv_r\cos{\phi_1}\\ 0&0&1\\ \end{bmatrix}x(k)+\begin{bmatrix} T\cos\phi_{\phi}&0\\
T\sin\phi_r&0\\
T\dfrac{\tan\phi_r}{l}&T\dfrac{v_r}{l\cos^2{\delta_r}}\\
\end{bmatrix}u(k)=AX(K)+Bu(k)
$$

式(3)表示的是 AGV 小车的离散状态空间方程,其中 X 向量代表小车去规划路径的 X 坐标偏差、Y 坐标偏差以及航向角偏差。工业 AGV 小车系统在行驶的过程中,不能给重载 AGV 小车太大的角速度。 所以这个系统可以建模为多目标优化最优控制问题, 目标函数可以表示为跟踪累计的跟踪偏差与累计的控制输入的加权,如式(5)所示:

$$
J=\sum_{k=1}^N(X^TQX+u^TRu)
\tag{5}$$

而解出的输入控制量为式(6):

$$
u=[(R+B^TPB)^{-1}B^TPA]X=KX
\tag{6}$$

其中 P 是式(6)黎卡提方程的解。

$$
P=A^TPA-A^TPB(R+B^TPB)^{-1}B^TPA+Q
\tag{7}$$

可以看到控制输入 u 的大小与累计误差公式中的权重矩阵R 相关。

3 控制系统设计

工业 AGV 一般都承载 1 t 左右的货物, 基于安全性考虑,我们可以将控制输入的权重矩阵 R 的值与 AGV 的实时速度进行绑定,以对 LQR 控制器进行优化。 当小车沿曲线行驶具有较高的速度时,需要控制小车的角速度变化不能太大,转角控制输入应相对较小,所以我们将权重矩阵 R 关于转角输入的变量以一定权重系数乘以当前速度; 当速度大时, 权重系数也相对较大,得到的相对转角变化量也相对小。

本文在 50×50 m 的地图上实现小车的路径规划, 基于cartographer 算法建立的栅格地图如图 3 所示。对于 B-样条曲线的设计, 根据前文对于 B-样条曲线性质的描述和工业 AGV的实际需求,采用 5 次二阶 clamped 的 B-样条曲线。 为了使得曲线能够更好地满足需求,我们采用 6 个控制点。

图片[3]-基于B-样条曲线的AGV路径规划与控制
图 3 栅格地图

所以本次实验的曲线参数有节点向量:[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[[0,0.2],[0,0.4],[0,0.6],[0,0.8],[1,0],[1,0],[1,0],[1,0],[1,0],[1,0], 控制点:[0,0],[1,0],[2,0.5],[3,1],[4,1.5],[5,5]。

将 LQR 的 R 矩阵两个变量设置为 1、0.5×Vyaw,Vyaw 即为小车行驶的角速度。

小车具体行驶过程控制与规划算法流程如下:
1)基于给定的曲线参数确定规划曲线。
2)获得当前小车的位置矢量 x1,寻找到规划曲线距离小车的最近点 x2,并计算相应的曲线曲率 w1
3)由矢量差 x1-x2、小车的航向角以及曲线曲率确定 LQR算法的三个系统参数$\dot x,\dot y,\dot yaw$。
4)由速度规划算法得出在位置 x2 小车应具有的速度,以及上一时刻小车的航向角。
5)由控制算法得到控制输入矢量 u1
6)将控制矢量 u1 转换为差速模型的线速度与角速度,并更新小车状态。
7)回到步骤 2),进行迭代更新直到轨迹完成。

图4是小车行驶过程中各个变量随时间变化的曲线,图 4a是规划的曲线图,图 4b、图 4c、图 4d 是规划曲线的 x、y、yaw 值随时间变化的曲线,图 4e 是规划曲线行驶的线速度与小车实际轨迹线速度对比图,图 4f 是规划曲线角速度与小车实际行驶角速度对比图。

图片[4]-基于B-样条曲线的AGV路径规划与控制
图 4 实验数据

4 结束语

本文所述的曲线路径以及控制算法为 AGV 小车的实际应用提供了一定的理论基础以及实验数据,可实际应用于 AGV 小车领域,并可以此为基础设计更多的算法。

基于B-样条曲线的AGV路径规划与控制-AGV吧
基于B-样条曲线的AGV路径规划与控制
此内容为免费资源,请登录后查看
0积分
免费资源
© 版权声明
THE END
喜欢就支持一下吧
点赞12 分享
评论 抢沙发

请登录后发表评论

    暂无评论内容