前后双差速轮之LQR控制

在之前的代码中，我们实现了前后两对双差速轮AGV的运动学正解和逆解。但为了实现对AGV的精确路径跟踪和姿态控制，我们需要引入控制算法。线性二次型调节器（LQR）是一种常用的最优控制方法，可以有效地将系统的状态误差最小化。本文将详细说明如何在之前的C++代码中加入LQR控制，实现对四轮AGV的路径跟踪控制

1. 建立AGV的状态空间模型

对于四轮AGV，我们需要建立适用于LQR控制的状态空间模型。由于AGV的运动学模型是非线性的，我们需要在工作点附近对其进行线性化。

1.1 非线性运动学模型：

AGV的非线性连续时间模型如下:

其中：

• x,y：AGV的位置坐标
• θ：AGV的朝向角
• v：线速度
• ω：角速度

控制输入：

对于AGV，控制输入为线速度 v 和角速度 ω。实际控制中，我们通过计算各个车轮的角速度来实现

1.2 线性化：