一种叉车的LQR控制方法、装置、存储介质和控制器与流程

本发明涉及车辆控制技术领域，尤其涉及一种叉车的lqr控制方法、装置、存储介质和控制器。

背景技术：

lqr(linearquadraticregulator)即线性二次型调节器控制算法，作为一种主流的最优控制算法，凭借其控制精度高的优势受到开发人员的青睐，广泛应用于无人车的控制领域。

然而，对于叉车等非常规车辆来说，由于行驶速度较慢，目前通常采用的是一些较为简单的非lqr的控制方法，可是随着对叉车的控制精度要求的提高，如何将lqr算法应用于叉车的控制成为技术人员急需考虑的问题。

技术实现要素：

有鉴于此，本发明实施例提供了一种叉车的lqr控制方法、装置、存储介质和控制器，能够将lqr算法应用于叉车的控制，从而提高叉车的控制精度。

本发明实施例的第一方面，提供了一种叉车的lqr控制方法，包括：

构建原点处于叉车运动起始位置的坐标系，所述叉车具有一个用于转向和前进的驱动轮；

根据所述叉车的前轴和后轴之间的轴距、所述驱动轮的转向角度、所述叉车的航向角以及所述叉车的速度，求解所述叉车于所述坐标系下的速度分量以及角速度；

将所述叉车于所述坐标系下的速度分量以及角速度表示为矩阵形式，构建所述叉车的运动学模型；

将lqr算法应用于所述运动学模型，实现对所述叉车的lqr控制。

本发明实施例的第二方面，提供了一种叉车的lqr控制装置，包括：

坐标系构建模块，用于构建原点处于叉车运动起始位置的坐标系，所述叉车具有一个用于转向和前进的驱动轮；

速度求解模块，用于根据所述叉车的前轴和后轴之间的轴距、所述驱动轮的转向角度、所述叉车的航向角以及所述叉车的速度，求解所述叉车于所述坐标系下的速度分量以及角速度；

运动学模型构建模块，用于将所述叉车于所述坐标系下的速度分量以及角速度表示为矩阵形式，构建所述叉车的运动学模型；

lqr控制模块，用于将lqr算法应用于所述运动学模型，实现对所述叉车的lqr控制。

本发明实施例的第三方面，提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机可读指令，所述计算机可读指令被处理器执行时实现如本发明实施例的第一方面提出的叉车的lqr控制方法的步骤。

本发明实施例的第四方面，提供了一种控制器，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机可读指令，所述处理器执行所述计算机可读指令时实现如本发明实施例的第一方面提出的叉车的lqr控制方法的步骤。

本发明实施例提出的叉车的lqr控制方法包括：构建原点处于叉车运动起始位置的坐标系；根据所述叉车的前轴和后轴之间的轴距、驱动轮的转向角度、所述叉车的航向角以及所述叉车的速度，求解所述叉车于所述坐标系下的速度分量以及角速度；将所述叉车于所述坐标系下的速度分量以及角速度表示为矩阵形式，构建所述叉车的运动学模型；将lqr算法应用于所述运动学模型，实现对所述叉车的lqr控制。lqr是一种基于模型的控制方法，本发明实施例通过对叉车的运动状态进行分析，推导出叉车的运动学模型，然后将lqr算法应用于该运动学模型中，从而实现对叉车的lqr控制，提高了叉车的控制精度。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的一种叉车的lqr控制方法的一个实施例的流程图；

图2是本发明实施例提供的一种叉车的lqr控制装置的一个实施例的结构图；

图3是本发明实施例提供的一种控制器的示意图。

具体实施方式

本发明实施例提供了一种叉车的lqr控制方法、装置、存储介质和控制器，能够将lqr算法应用于叉车的控制，从而提高叉车的控制精度。

为使得本发明的发明目的、特征、优点能够更加的明显和易懂，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，下面所描述的实施例仅仅是本发明一部分实施例，而非全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

请参阅图1，本发明实施例中一种叉车的lqr控制方法包括：

101、构建原点处于叉车运动起始位置的坐标系，所述叉车具有一个用于转向和前进的驱动轮；

由于lqr是一种基于模型的控制方法，为了实现对叉车的lqr控制，首先需要推导叉车的运动学模型。为描述叉车的运动学模型，构建原点处于叉车运动起始位置的坐标系，比如构建一个xoy坐标系，(x，y)表示叉车的运动位置。在结构上，该叉车具有一个用于转向和前进的驱动轮，以及若干数量的支撑轮。

102、根据所述叉车的前轴和后轴之间的轴距、所述驱动轮的转向角度、所述叉车的航向角以及所述叉车的速度，求解所述叉车于所述坐标系下的速度分量以及角速度；

然后，根据所述叉车的前轴和后轴之间的轴距、所述驱动轮的转向角度、所述叉车的航向角以及所述叉车的速度，求解所述叉车于所述坐标系下的速度分量以及角速度。由于叉车的各个支撑轮的转动方向固定，故可以假设支撑轮中心速度方向垂直于叉车的后轴，再根据驱动轮的转向角度和轴距可以确定叉车的转弯半径为：

在公式(1.1)中，r表示叉车的转弯半径，l表示轴距，δ表示驱动轮的转向角度。

接着，可以在叉车的车体上构建一个车身坐标系，在该车身坐标系下，可以获得以下公式：

其中，表示叉车于x方向的速度分量，表示叉车于y方向的速度分量，v表示叉车的速度(即线速度)，w表示叉车的角速度，l表示轴距，δ表示驱动轮的转向角度。

对车身坐标系下的公式(1.2)、(1.3)和(1.4)做坐标变换，在步骤101所构建的坐标系下表示，得到以下公式：

其中，表示叉车于x方向的速度分量，表示叉车于y方向的速度分量，v表示叉车的速度(即线速度)，ω表示叉车的角速度，l表示轴距，δ表示驱动轮的转向角度，ψ表示叉车的航向角。至此，即得到了所述叉车于所述坐标系下的速度分量以及角速度的表达式，即上述公式(1.5)、(1.6)和(1.7)。

103、将所述叉车于所述坐标系下的速度分量以及角速度表示为矩阵形式，构建所述叉车的运动学模型；

在得到所述叉车于所述坐标系下的速度分量以及角速度的表达式之后，将其表示为矩阵的形式，从而构建该叉车的运动学模型为：

通过对叉车运动状态的分析，构建出叉车的运动学模型，接下来即可将lqr算法应用于该运动学模型中。

104、将lqr算法应用于所述运动学模型，实现对所述叉车的lqr控制。

在获得叉车的运动学模型之后，即可将lqr算法应用于所述运动学模型，实现对所述叉车的lqr控制。

具体的，步骤104可以包括：

(1)取所述叉车于所述坐标系的位置坐标和所述航向角作为lqr控制的状态量，取所述叉车的速度和所述驱动轮的转向角度作为lqr控制的控制量；

(2)将所述运动学模型表示为所述状态量和所述控制量的函数；

(3)将所述函数在所述驱动轮的预设参考轨迹点处执行泰勒展开处理，得到第一矩阵表达式；

(4)根据预设的周期对所述第一矩阵表达式执行离散化处理，得到第二矩阵表达式；

(5)结合所述第二矩阵表达式求解预先构建的lqr评价函数的最优解，获得最优控制量；

(6)根据所述最优控制量对所述叉车执行lqr控制。

对于上述步骤(1)，取所述叉车于所述坐标系的位置坐标和所述航向角作为lqr控制的状态量，可表示为m＝[x，y，ψ]^t；取所述叉车的速度和所述驱动轮的转向角度作为lqr控制的控制量，可表示为u＝[v，δ]。

对于上述步骤(2)，将所述运动学模型表示为所述状态量和所述控制量的函数。由于速度可视作位移的导数，角速度可视作航向角的导数，故该运动学模型可表示为函数：

对于上述步骤(3)，将所述函数在所述驱动轮的预设参考轨迹点处执行泰勒展开处理，得到第一矩阵表达式。该参考轨迹点是任意选取的处于叉车运动轨迹上的点，满足该运动学模型。具体的，可以给定叉车运动的参考轨迹点(mr，ur)，其中mr＝[xr，yr，ψr]^t，ur＝[r，δr]，该参考轨迹点满足该运动学模型，然后将函数于该参考轨迹点处泰勒展开线性化处理后，得到以下矩阵表达式：

令得到所述第一矩阵表达式为：

其中，表示状态量偏差，表示控制量偏差(也叫控制量的反馈)，表示状态量偏差的导数。

对于上述步骤(4)，根据预设的周期对所述第一矩阵表达式执行离散化处理，得到第二矩阵表达式。具体的，以周期t(t为控制器下发控制命令的周期)进行离散化处理后，得到第二矩阵表达式为：

其中，ak和bk均为矩阵，k，k+1…代表不同的时刻点，即k＝(0,1,2,3…n)相邻的时刻点之间相差一个所述周期t，即k和k+1之间相差一个t，k+1和k+2之间相差一个t，以此类推。表示k时刻的表示k+1时刻的表示k时刻的矩阵ak和bk的具体表达式如下：

对于上述步骤(5)，结合所述第二矩阵表达式求解预先构建的lqr评价函数的最优解，获得最优控制量。最优控制量是使得该评价函数最小的控制量，将最优控制量所对应的控制指令下发至要控制的叉车运动学模型，从而实现叉车的最优控制。求解lqr评价函数的过程属于一个复杂的最优控制问题，基本的求解步骤为：对所述预先构建的lqr评价函数引入拉格朗日乘子，构造无约束最优化问题；构建基于所述lqr评价函数的哈密顿函数，并将所述哈密顿函数代入所述拉格朗日乘子，求解所述无约束最优化问题，得到目标方程组；结合所述目标方程组和所述第二矩阵表达式，采用迭代求解的方式计算获得所述最优控制量。

主流的lqr评价函数为：

上述主流的lqr评价函数缺乏对反馈控制量的调控，容易产生控制指令波动较大的问题，造成控制不平滑。针对主流lqr评价函数存在的问题，本申请对该主流函数进行了改进，得到以下lqr评价函数：

其中，q为状态量的权矩阵、r为控制量的权矩阵、w为反馈控制量(即控制量偏差)的权矩阵。权矩阵w为一个对角矩阵，获取的方法和权矩阵q、权矩阵r类似，都是预设的已知矩阵。通过引入权矩阵w，加入对反馈控制量的调控，从而避免产生控制不平滑的问题。

求解该改进的lqr评价函数的过程大致包括：

(a)对该改进的lqr评价函数引入拉格朗日乘子，得到公式(1.8)；

(b)构建基于该改进的lqr评价函数的哈密顿函数，如下公式(1.9)：

(c)将公式(1.9)代入公式(1.8)，得到：

为了使该改进的lqr评价函数最小，f应满足对的导数为0，对的导数为0，对的导数为0，可以得到如下方程组：

(d)假设结合所述第二矩阵表达式以及上述方程组包含的公式(1.11)、(1.12)和(1.13)，进行迭代求解，计算该改进的lqr评价函数的最优解，从而获得最优控制量。

具体的，迭代求解的初始条件为：cn＝q，dn＝0，由时刻k＝n开始一步一步向前迭代运算，直至k＝0，迭代求解相关的方程式如下：

k＝(bk^t*ck+1*bk+r+w)^-1*bk^t*ck+1*ak(1.14)

ku＝(bk^t*ck+1*bk+r+w)^-1*r(1.15)

kv＝(bk^t*ck+1*bk+r+w)^-1*bk^t(1.16)

ck＝ak^t*ck+1*(ak-bk*k)+q(1.17)

dk＝(ak-bk*k)^t*dk+1-k^t*r*(ur)k(1.18)

在所述初始条件下，通过不断迭代求解公式(1.14)至(1.18)，直至当前时刻k＝0，可以求出k(0)，ku(0)、kv(0)和d(1)，最终可以得到最优控制量为：

需要说明的是，上述方程式中的各个未说明的符号，比如cn、dn、ck、dk、ck+1、dk+1、k、ku、kv等，均是为了求解lqr评价函数的最优解，在执行迭代运算时引入的中间计算参数，这些中间计算参数本身不含任何物理意义，具体可参照最优控制理论中的相关内容。

另外，在传统lqr控制算法中，在执行迭代运算时是不会更新矩阵a和矩阵b的，因此动态跟踪信号的能力较差。本申请将矩阵a改进为ak，矩阵b改进为bk，在上述迭代运算中，矩阵ak和bk均是随着时刻k的变化而不断迭代更新的，从而可以达到更好的动态信号跟踪效果。

对于上述步骤(6)，在求解得到最优控制量之后，即可将该最优控制量下发至叉车的运动学模型中，从而实现根据所述最优控制量对所述叉车执行lqr控制。

应当理解，本发明实施例的各个步骤的实施主体为控制叉车动作的相关控制器。

本发明实施例提出的叉车的lqr控制方法包括：构建原点处于叉车运动起始位置的坐标系；根据所述叉车的前轴和后轴之间的轴距、驱动轮的转向角度、所述叉车的航向角以及所述叉车的速度，求解所述叉车于所述坐标系下的速度分量以及角速度；将所述叉车于所述坐标系下的速度分量以及角速度表示为矩阵形式，构建所述叉车的运动学模型；将lqr算法应用于所述运动学模型，实现对所述叉车的lqr控制。lqr是一种基于模型的控制方法，本发明实施例通过对叉车的运动状态进行分析，推导出叉车的运动学模型，然后将lqr算法应用于该运动学模型中，从而实现对叉车的lqr控制，提高了叉车的控制精度。

应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。

上面主要描述了一种叉车的lqr控制方法，下面将对一种叉车的lqr控制装置进行描述。

请参阅图2，本发明实施例中一种叉车的lqr控制装置的一个实施例包括：

坐标系构建模块201，用于构建原点处于叉车运动起始位置的坐标系，所述叉车具有一个用于转向和前进的驱动轮；

速度求解模块202，用于根据所述叉车的前轴和后轴之间的轴距、所述驱动轮的转向角度、所述叉车的航向角以及所述叉车的速度，求解所述叉车于所述坐标系下的速度分量以及角速度；

运动学模型构建模块203，用于将所述叉车于所述坐标系下的速度分量以及角速度表示为矩阵形式，构建所述叉车的运动学模型；

lqr控制模块204，用于将lqr算法应用于所述运动学模型，实现对所述叉车的lqr控制。

进一步的，所述叉车于所述坐标系下的速度分量以及角速度的表达式为：

所述叉车的运动学模型为：

其中，表示所述叉车于所述坐标系的第一方向的速度分量，表示所述叉车于所述坐标系的第二方向的速度分量，v表示所述叉车的速度，δ表示所述驱动轮的转向角度，ψ表示所述叉车的航向角，ω表示所述叉车的角速度，l表示所述轴距。

进一步的，所述lqr控制模块可以包括：

状态量与控制量选取单元，用于取所述叉车于所述坐标系的位置坐标和所述航向角作为lqr控制的状态量，取所述叉车的速度和所述驱动轮的转向角度作为lqr控制的控制量；

模型转换单元，用于将所述运动学模型表示为所述状态量和所述控制量的函数；

泰勒展开线性化单元，用于将所述函数在所述驱动轮的预设参考轨迹点处执行泰勒展开处理，得到第一矩阵表达式；

离散化单元，用于根据预设的周期对所述第一矩阵表达式执行离散化处理，得到第二矩阵表达式；

最优解求解单元，用于结合所述第二矩阵表达式求解预先构建的lqr评价函数的最优解，获得最优控制量；

lqr控制单元，用于根据所述最优控制量对所述叉车执行lqr控制。

具体的，所述lqr控制的状态量表示为m＝[x，y，ψ]^t，其中x、y为所述叉车于所述坐标系中的位置坐标，所述lqr控制的控制量表示为u＝[v，δ]；

所述函数表示为：

所述预设参考轨迹点表示为(mr，ur)，其中mr＝[xr，yr，ψr]^t，ur＝[vr，δr]；

所述将所述函数在所述驱动轮的预设参考轨迹点处执行泰勒展开处理后，得到以下矩阵表达式：

令得到所述第一矩阵表达式为：

根据预设的周期t对所述第一矩阵表达式执行离散化处理，得到所述第二矩阵表达式为：

其中，表示状态量偏差，表示控制量偏差，表示状态量偏差的导数，k，k+1…代表不同的时刻点，相邻的时刻点之间相差一个所述周期t，

更进一步的，所述最优解求解单元可以包括：

拉格朗日乘子引入子单元，用于对所述预先构建的lqr评价函数引入拉格朗日乘子，构造无约束最优化问题；

哈密顿函数构建子单元，用于构建基于所述lqr评价函数的哈密顿函数，并将所述哈密顿函数代入所述拉格朗日乘子，求解所述无约束最优化问题，得到目标方程组；

迭代求解子单元，用于结合所述目标方程组和所述第二矩阵表达式，采用迭代求解的方式计算获得所述最优控制量。

进一步的，所述lqr评价函数的表达式为：

其中，q为状态量的权矩阵、r为控制量的权矩阵、w为反馈控制量的权矩阵。

本发明实施例还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机可读指令，所述计算机可读指令被处理器执行时实现如图1表示的任意一种叉车的lqr控制方法的步骤。

本发明实施例还提供一种控制器，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机可读指令，所述处理器执行所述计算机可读指令时实现如图1表示的任意一种叉车的lqr控制方法的步骤。

图3是本发明一实施例提供的控制器的示意图。如图3所示，该实施例的控制器3包括：处理器30、存储器31以及存储在所述存储器31中并可在所述处理器30上运行的计算机可读指令32。所述处理器30执行所述计算机可读指令32时实现上述叉车的lqr控制方法的步骤，例如图1所示的步骤101至104。或者，所述处理器30执行所述计算机可读指令32时实现上述各装置实施例中各模块/单元的功能，例如图2所示模块201至204的功能。

示例性的，所述计算机可读指令32可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器31中，并由所述处理器30执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机可读指令段，该指令段用于描述所述计算机可读指令32在所述控制器3中的执行过程。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，控制器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read-onlymemory)、随机存取存储器(ram，randomaccessmemory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。