一种带有PID转角补偿的参数自整定LQR路径跟踪方法与流程

技术特征：
1.一种带有pid转角补偿的参数自整定lqr路径跟踪方法，其特征在于：所述方法包括以下步骤：步骤s1：实时采集参考路径信息、车辆当前的位姿信息与运动状态信息；步骤s2：根据采集的参考路径信息、车辆当前的位姿信息与运动状态信息确定目标参考点，计算车辆与目标参考点的偏差；步骤s3：根据车速信息，采用基于车速的lqr控制权重调整方法确定控制权重；步骤s4：根据步骤s2中计算的车辆与目标参考点的偏差值及步骤s3中确定的控制权重计算车辆的前轮转角；步骤s5：采用预瞄pid方法计算补偿前轮转角；步骤s6：计算车辆方向盘转角控制量，传递给转向执行机构。2.根据权利要求1所述的一种带有pid转角补偿的参数自整定lqr路径跟踪方法，其特征在于：所述步骤s1中，所述参考路径信息为离散的参考路径点集，其中参考路径点包含位置、航向角与曲率信息；所述车辆当前的位姿信息包括车辆质心位置、车辆航向角，所述运动状态信息包括车辆横摆角速度、车辆纵向速度、车辆横向速度。3.根据权利要求2所述的一种带有pid转角补偿的参数自整定lqr路径跟踪方法，其特征在于：所述步骤s2的具体步骤包括：s201：将参考路径信息中的参考路径点位置与车辆位置转换为utm坐标；s202：确定目标参考点，所述目标参考点包含质心参考点与预瞄参考点，分别为车辆质心参考点、车辆预瞄点参考点；s203：遍历参考路径点集，查找距离车辆质心位置最近的两个相邻路径点，利用线性插值法确定车辆实际的质心参考点；s204：根据车辆实际的质心参考点信息与车辆质心位置，计算车辆与参考点的横向位置偏差e
y
，航向偏差横向位置偏差变化率航向偏差变化率s205：遍历参考路径点集，查找距离车辆预瞄点位置最近的两个相邻路径点，利用线性插值法确定车辆实际的预瞄参考点；s206：根据车辆实际的预瞄参考点位置与车辆预瞄点位置，计算预瞄点与参考点的横向位置偏差e
pre
。4.根据权利要求1所述的一种带有pid转角补偿的参数自整定lqr路径跟踪方法，其特征在于：所述步骤s3的具体步骤包括：s301：建立路径跟踪误差模型，设计lqr路径跟踪控制器针对离散系统进行控制，通过求解合适的控制量u，使目标函数的值最小，目标函数中，权重矩阵q＝diag[q1,q2,q3,q4]，q1,q2,q3,q4分别为横向位置偏差e
y
，横向位置偏差变化率航向偏差航向偏差变化率控制量权重矩阵r＝[r]，r为前轮转角δ
f
的权重系数：lqr路径跟踪控制器目标函数的设定方法为：将路径跟踪误差模型表示为状态空间方程形式：其中：
u＝[δ
f
]，式中，m为整车质量；v
x
分别为车辆的纵向车速；c
f
，c
r
分别为车辆前、后轴车轮的侧偏刚度；i
z
为车辆绕垂线方向的转动惯量；l
f
、l
r
分别为车辆前、后轴与质心的距离；δ
f
为前轮转角；lqr路径跟踪控制器针对离散系统进行控制，在每一个控制时域输出一个最优控制量。所以，需要将连续系统转化为离散系统。设定控制步长为t，选择双线性离散化的方法，将状态空间方程离散化，可以得到：x(k 1)＝a
d
x(k) b
d
u(k)式中：a
d
＝(i
‑
ta/2)
‑1(i ta/2),b
d
＝tb路径跟踪控制器的控制目的不仅需要减小跟踪误差，同时，为了保证智能汽车的行驶稳定性，还需要使控制量较小。由此，定义如下目标函数：式中，q为状态权重矩阵；r为控制量权重矩阵。lqr控制方法通过求解合适的控制量u，使目标函数的值最小。目标函数中，权重矩阵q＝diag[q1,q2,q3,q4],分别为e
y
、和的权重系数；r＝[r],为δ
f
的权重系数，一共5个权重参数；s302：根据测试实验，在保证车辆路径跟踪的精准性和行驶的舒适性、无左右摇摆现象的条件下确定速度阈值v
th
的最优控制权重；s303：根据车辆纵向速度v
x
，调整权重参数q1,r的值。5.根据权利要求1所述的一种带有pid转角补偿的参数自整定lqr路径跟踪方法，其特征在于：所述步骤s5中根据步骤s2计算得到的预瞄偏差e
pre
，采用pid方法计算补偿前轮转角，计算公式为：式中，k
p
，k
i
，k
δ
分别表示比例、积分、微分常数，e
pre
(k)表示k时刻的预瞄横向偏差。
6.根据权利要求1所述的一种带有pid转角补偿的参数自整定lqr路径跟踪方法，其特征在于：所述步骤s6中，计算车辆方向盘转角控制量，传递给转向执行结构，车辆方向盘转角控制量δ的计算公式为：δ＝i
·
(δ1 δ2)式中，i表示车辆转向系统的传动比，δ1为车辆跟踪路径所需的前轮转角，δ2为前轮转角补偿前轮转角。7.根据权利要求3所述的一种带有pid转角补偿的参数自整定lqr路径跟踪方法，其特征在于：所述步骤s202中车辆预瞄点的计算公式如下：式中，x
pre
，y
pre
为车辆预瞄点位置，x，y为车辆质心位置，d
pre
为预瞄距离，为车辆航向角。8.根据权利要求4所述的一种带有pid转角补偿的参数自整定lqr路径跟踪方法，其特征在于：所述步骤s301中lqr路径跟踪控制器目标函数的设定方法为：将路径跟踪误差模型表示为状态空间方程形式：其中：其中：u＝[δ
f
]，式中，m为整车质量；v
x
分别为车辆的纵向车速；c
f
，c
r
分别为车辆前、后轴车轮的侧偏刚度；i
z
为车辆绕垂线方向的转动惯量；l
f
、l
r
分别为车辆前、后轴与质心的距离；δ
f
为前轮转角；lqr路径跟踪控制器针对离散系统进行控制，将连续系统转化为离散系统，设定控制步长为t，选择双线性离散化的方法，将状态空间方程离散化，得到：x(k 1)＝a
d
x(k) b
d
u(k)式中：a
d
＝(i
‑
ta/2)
‑1(i ta/2),b
d
＝tb；定义目标函数：
式中，q为状态权重矩阵；r为控制量权重矩阵；lqr控制方法通过求解控制量u，使目标函数的值最小。9.根据权利要求4所述的一种带有pid转角补偿的参数自整定lqr路径跟踪方法，其特征在于：所述步骤s302中最优控制权重为速度阈值v
th
预设为2m/s时的控制权重。10.根据权利要求4所述的一种带有pid转角补偿的参数自整定lqr路径跟踪方法，其特征在于：所述步骤s303中权重参数q1,r的计算公式为：,r的计算公式为：式中，q
1th
，r
th
为速度阈值内的最优控制权重，k1，k2为速度增益系数。

技术总结
本发明公开了一种带有PID转角补偿的参数自整定LQR路径跟踪方法，包括以下步骤：接收参考路径信息、车辆当前的位姿信息与运动状态信息；根据接收的信息，确定目标参考点，计算车辆与目标参考点的偏差；根据车速信息，采用基于车速的LQR控制权重调整方法确定控制权重；采用LQR控制方法计算车辆的前轮转角；采用预瞄PID方法计算补偿前轮转角；计算车辆方向盘转角控制量，传递给转向执行机构。本发明对一般LQR控制方法进行了改进，采用预瞄PID控制方法对LQR控制方法进行转角补偿，能有效的提高跟踪精度；并加入基于车速的LQR控制权重调整方法，提高控制器对于车速的适应性。提高控制器对于车速的适应性。提高控制器对于车速的适应性。


技术研发人员：胡杰 钟鑫凯 陈瑞楠 朱令磊 张敏超 徐文才
受保护的技术使用者：武汉理工大学
技术研发日：2021.07.22
技术公布日：2021/11/4