CN119472317A 一种基于非线性优化算法的agv轨迹跟踪控制方法 （山东科技大学）

一种基于非线性优化算法的AGV轨迹跟踪控本发明公开了一种基于非线性优化算法的运动模型，根据参考轨迹设计AGV的二次损失函将最优解的第一个控制输入转化为控制指令驱22.根据权利要求1所述基于非线性优化算其中，x1(k+1)、x2(k+1)、x3(k+1)分别表示第k+1个采样时刻AGV在平其中，x(k)=[x1(k),xz(k),x3(k)]'、x'(k)=[xf(k),x(k),x(k)]'分别表示第k个采样时刻AGV在平面坐标系下的位姿、参考位姿；x(k)、x(k)、分别为第个采样时刻AGV在平面坐标系下的参考横坐标、参考纵坐标、参考航向角；u(k)=[u(k),uz(k)]'、u'(k)=[uf(k),u⃞(k)]'分别表示第k个采样时刻4.根据权利要求3所述基于非线性优化算法的AGV步骤3.1、记所有采样时刻k=0,...,N的控制输入为x=[u'(0),.…,u'(N)]',其中，u'(0)、u'(N)分别为第0个采样时刻AGV在平面坐标系下的控制输入U(0)的转置、第N个采样时刻AGV在平面坐标系下的控制输入U(N)的转置；给定所有采样时刻3k=0,...,N的控制输入X的初始迭代值；表示第k+1个采样时刻x(k+1)对应的协态变量；l1(k+1)、A2(k+1)、3(k+1)分别为第k+1个采样时刻x1(k+1)、x2(k+1)、x3(k+1)对应的协态变量；x(k+1)=[x1(k+1),xz(k+1),x3(k+1)]'表示第k+1个采A(N+1)=03X1(5)；其中，A(k)表示第k个采样时刻x(k)对应的协态变量；A(N+1)表示第N+1个采样时刻x(N+1)对应的协态变量，此时的协态变量是给定的终端值；x(N+1)表和个和第M-1个索引位置的元素。4HO(-)为第二哈密尔顿函数；o(k+1)表示第k+1个采样时刻x(k+1)对应的倒向迭代伴随状态；n(k)表示第k个采样时刻A(k)对应的正向迭代伴随状态；o'(k+1)为o(k+1)的转置；n'(k)为n(k)的转置；取第0个索引位置的元素并记为p1(x(0),u(0),(1))；n(0)=o(N+1)=03x1(10)；其中，n(k+1)表示第k+1个采样时刻A(k+1)对应的正向迭代伴随状态；o(k)表示第k个采样时刻x(k)对应的倒向迭代伴随状态；n(0)表示第0个采样时刻1(1)对应的正向迭代伴随状态，此时的正向迭代伴随状态是给定的初始值；o(N+1)表示第N+1个采样时刻X(N+1)对应的倒向迭代伴随状态，此时的倒向迭代伴随状态是给定的终端值；k=0,...,N的正向迭其中，v2J(0)表示二次损失函数J关于所有采样时刻控制输入的海森矩阵第一行元5森矩阵第一行的第M-2个和第M-1个索引位置的元素；xuλ为pz(x(0),u(0),A(1))，改变第二哈密尔顿函数中的p1(x(0),u(0),A(1))为6.根据权利要求5所述基于非线性优化算法步骤5.1、给定第0次迭代时x的初始值xo和算法停止阈值t>0；xmr1=xm-hn(xm)(12)；n分别表示外循环、内循环迭代次数的序号；xm1、xm分别表示第失函数J在xm处关于所有采样时刻控制输入的梯度、海森矩阵；r表示给定的与维度相容的正定加权矩阵；xm1作为非线性优化算法的最优解。6[0001]本发明属于轨迹跟踪技术领域，具体涉及一种基于非线性优化算法的AGV轨迹跟[0004]为了解决上述问题，本发明提出了一种基于非线性优化算法的AGV轨迹跟踪控制其中，x1(k+1)、x2(k+1)、x3(k+1)分别表示第k+1个采样时刻AGV7x'(k)=[xf(k),x(k),x(k)]'分别表示第k个采样时刻AGV在平面坐标系下的位姿、参考位姿；xf(k)、x(k)、分别为第k个采样时刻AGV在平面坐标系下u(k)=[u(k),uz(k)]'、u'(k)=[uf(k),u⃞(k)]'分别表示第k个采样时刻AGV在平面坐标系下的控制输入、参考输入；u(k)、u(k)分别为第k个采样时刻AGV在平面坐标系下的参考线速度、参考步骤3.1、记所有采样时刻k=0,...,N的控制输入为转置；给定所有采样时刻K=0,...,N的控制输入X的初始迭代值；A(k+1)=[A1(k+1),l2(k+1),d3(k+1)'表示第k+1个采样时刻x(k+1)对应的协态变量；l1(k+1)、A2(k+1)、3(k+1)分别为第k+1个x(k+1)=[x1(k+1),xz(k+1),x3(k+1)]'表示第k+1个采样时刻AGV8A(N+1)=03X1(5)；其中，A(k)表示第k个采样时刻x(k)对应的协态变量；A(N+1)表示第N+1个采样时刻X(N+1)对应的协态变量，此时的协态变量是给定的终端值；x(N+1)表示第N+1个采样时刻AGV在平面坐标系下的位姿；03X1表示3维的零向和个和第M-1个索引位置的元素。其中，为第二哈密尔顿函数；o(k+1)表示第k+1个采样时刻x(k+1)对应的倒向迭代伴随状态；n(k)表示第k个采样时刻A(k)对应的正向迭代伴个采样时刻AGV在平面坐标系下的初始位姿、第0个采样时刻AGV在平面坐标系下的控制输9n(0)=o(N+1)=03x1(10)；其中，n(k+1)表示第k+1个采样时刻A(k+1)对应的正向迭代伴随状态；o(k)表示第k个采样时刻x(k)对应的倒向迭代伴随状态；n(0)表示第0个1(1)对应的正向迭代伴随状态，此时的正向迭代伴随状态是给定的初始值；o(N+1)表示第N+1个采样时刻X(N+1)对应的倒向迭代伴随状态，此时的倒向迭代伴随状态是给定的终端值；k=0,...,N的正其中，v2J(0)表示二次损失函数J关于所有采样时刻控制输入的海森矩阵第一森矩阵第一行的第M-2个和第M-1个索引位置的元素；x记为pz(x(0),u(0),A(1))，改变第二哈密尔顿函数中的p1(x(0),u(0),A(1))为步骤5.1、给定第0次迭代时x的初始值xo和算法停止阈值t>0；xm1；xmr1=xm-hn(xm)(12)；失函数J在xm处关于所有采样时刻控制输入的梯度、海森矩阵；r表示给定的与维度相容的正定加权矩阵；xm1作为非线性优化算法的最优解。[0011]本发明所带来的有益技术效果：本发明的AGV轨迹跟踪方法具有优异的稳定性其中，x1(k+1)、x2(k+1)、x3(k+1)分别表示第k+1个采样时刻AGVx'(k)=[xf(k),x(k),x(k)]'分别表示第k个采样时刻AGV在平面坐标系下的位xf(k)、x(k)、分别为第k个采样时刻AGV在平面坐标系下的参考u(k)=[u(k),uz(k)]'、u'(k)=[uf(k),u⃞(k)]'分别表示第k个采样时刻AGV在平面坐标系下的控制输入、参考输入；u(k)、u(k)分别为第k个采样时刻AGV在平面坐标系下的参考线速度、参考步骤3.1、记所有采样时刻k=0,...,N的控制输入为转置。给定所有采样时刻K=0,...,N的控制输入X的初始迭代值；A(k+1)=[A1(k+1),l2(k+1),d3(k+1)'表示第k+1个采样时刻x(k+1)对应的协态变量；l1(k+1)、A2(k+1)、3(k+1)分别为第k+1个x(k+1)=[x1(k+1),xz(k+1),x3(k+1)]'表示第k+1个采样时刻AGVA(k+1)满足如下的方程：A(N+1)=03X1(5)；其中，A(k)表示第k个采样时刻x(k)对应的协态变量；A(N+1)表示第N+1个采样时刻X(N+1)对应的协态变量，此时的协态变量是给定的终端值；x(N+1)表示第N+1个采样时刻AGV在平面坐标系下的位姿；03X1表示3维的零向M=2(N+1)；为梯度的第M-2个和第M-1个索引位置的元素。执行公式（6得到二次损失函数J其中，为第二哈密尔顿函数；o(k+1)表示第k+1个采样时刻x(k+1)对应的倒向迭代伴随状态；n(k)表示第k个采样时刻A(k)对应的正向迭代伴n(0)=o(N+1)=03x1(10)；其中，n(k+1)表示第k+1个采样时刻A(k+1)对应的正向迭代伴随状态；o(k)表示第k个采样时刻x(k)对应的倒向迭代伴随状态；n(0)表示第0个1(1)对应的正向迭代伴随状态，此时的正向迭代伴随状态是给定的初始值；o(N+1)表示第N+1个采样时刻X(N+1)对应的倒向迭代伴随状态，此时的倒向迭代伴随状态是给定的终端值。k=0,...,N的正其中，v2J(0)表示二次损失函数J关于所有采样时刻控制输入的海森矩阵第一森矩阵第一行的第M-2个和第M-1个索引位置的元素。执行公式（11得到海森矩x行元素的计算方式相同，计算时，取第1个索引位置的元素pz(x(0),u(0),A(1))，改变第二哈密尔顿步骤5.1、给定第0次迭代时x的初始值xo和算法停止阈值t>0；xm1；xmr1=xm-hn(xm)(12)；失函数J在xm处关于所有采样时刻控制输