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

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN113608530A(43)申请公布日2021.11.05(21)申请号202110833093.6(22)申请日2021.07.22(71)申请人武汉理工大学地址430070湖北省武汉市洪山区珞狮路122号(72)发明人胡杰钟鑫凯陈瑞楠朱令磊张敏超徐文才(74)专利代理机构武汉开元知识产权代理有限公司42104代理人刘琳(51)Int.Cl.G05D1/02(2020.01)权利要求书4页说明书5页附图2页(54)发明名称一种带有PID转角补偿的参数自整定LQR路径跟踪方法(57)摘要本发明公开了一种带有PID转角补偿的参数自整定LQR路径跟踪方法，包括以下步骤：接收参考路径信息、车辆当前的位姿信息与运动状态信息；根据接收的信息，确定目标参考点，计算车辆与目标参考点的偏差；根据车速信息，采用基于车速的LQR控制权重调整方法确定控制权重；采用LQR控制方法计算车辆的前轮转角；采用预瞄PID方法计算补偿前轮转角；计算车辆方向盘转角控制量，传递给转向执行机构。本发明对一般LQR控制方法进行了改进，采用预瞄PID控制方法对LQR控制方法进行转角补偿，能有效的提高跟踪精度；并加入基于车速的LQR控制权重调整方法，提高控制器对于车速的适应性。CN113608530ACN113608530A权利要求书1/4页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：根据车辆实际的质心参考点信息与车辆质心位置，计算车辆与参考点的横向位置偏差ey，航向偏差横向位置偏差变化率航向偏差变化率S205：遍历参考路径点集，查找距离车辆预瞄点位置最近的两个相邻路径点，利用线性插值法确定车辆实际的预瞄参考点；S206：根据车辆实际的预瞄参考点位置与车辆预瞄点位置，计算预瞄点与参考点的横向位置偏差epre。4.根据权利要求1所述的一种带有PID转角补偿的参数自整定LQR路径跟踪方法，其特征在于：所述步骤S3的具体步骤包括：S301：建立路径跟踪误差模型，设计LQR路径跟踪控制器针对离散系统进行控制，通过求解合适的控制量U，使目标函数的值最小，目标函数中，权重矩阵Q＝diag[q1,q2,q3,q4]，q1,q2,q3,q4分别为横向位置偏差ey，横向位置偏差变化率航向偏差航向偏差变化率控制量权重矩阵R＝[r]，r为前轮转角δf的权重系数：LQR路径跟踪控制器目标函数的设定方法为：将路径跟踪误差模型表示为状态空间方程形式：其中：2CN113608530A权利要求书2/4页U＝[δf]，式中，m为整车质量；Vx分别为车辆的纵向车速；cf，cr分别为车辆前、后轴车轮的侧偏刚度；Iz为车辆绕垂线方向的转动惯量；lf、lr分别为车辆前、后轴与质心的距离；δf为前轮转角；LQR路径跟踪控制器针对离散系统进行控制，在每一个控制时域输出一个最优控制量。所以，需要将连续系统转化为离散系统。设定控制步长为T，选择双线性离散化的方法，将状态空间方程离散化，可以得到：X(k+1)＝AdX(k)+BdU(k)式中：‑1Ad＝(I‑TA/2)(I+TA/2),Bd＝TB路径跟踪控制器的控制目的不仅需要减小跟踪误差，同时，为了保证智能汽车的行驶稳定性，还需要使控制量较小。由此，定义如下目标函数：式中，Q为状态权重矩阵；R为控制量