一种车辆线性二自由度动力学模型循迹控制方法及系统与流程

1.本发明涉及车辆线性二自由度动力学模型循迹控制技术领域，具体涉及一种车辆线性二自由度动力学模型循迹控制方法及系统。


背景技术：

2.现有的车辆线性二自由度动力学模型循迹控制方法，lqr（linear quadratic regulator，线性二次型调节器）运用较为广泛，被广泛运用于车辆循迹的横向控制中，对线性轨迹的跟踪有着良好的精度，但由于实际运用中存在对非线性轨迹区域的线性近似处理，lqr在非线性区域的循迹控制精度无法达到理想的控制精度。现有的改进方法多采用通过最优算法对lqr参数实时调节，但这种方式加大了对车载控制器的算力要求，对于一些算力较低的mcu（microcontroller unit，微控制单元），此种方式是不能满足工程需求的。
3.目前，现有l4级自动驾驶车辆采用lqr进行车辆的横向控制，首先基于自行车模型建立车辆的运动学模型，求解状态方程；再计算此时车辆的横向偏差与航向偏差；最后对偏差与控制量赋予不同的权重，建立代价函数，求解最优控制量，既方向盘转角。但是却具有如下缺点：（1）在计算横向偏差时无法避免将非线性跟踪区域近似为线性区域处理，这导致在非线性区域的控制精度无法达到目标要求；（2）未选择预瞄点，高速时车辆控制输出频率增大，稳定性降低。
4.针对上述技术问题，本发明提出了一种车辆线性二自由度动力学模型循迹控制方法，既可以改善车辆线性二自由度动力学模型在非线性区的循迹控制效果，又能做到对控制器算力几乎无要求。


技术实现要素：

5.针对现有技术中存在的缺陷，本发明的目的在于提供一种车辆线性二自由度动力学模型循迹控制方法及系统，能够提高在非线性区域的控制精度和高速时的车辆稳定性。
6.为实现上述目的，本发明采用的技术方案如下：一种车辆线性二自由度动力学模型循迹控制方法，包括：s100、基于车辆线性二自由度动力学模型，建立车辆偏差状态方程；s200、基于所述车辆线性二自由度动力学模型和参考轨迹，建立跟踪偏差模型；s300、基于所述车辆偏差状态方程和lqr横向控制算法，求解得到控制系数k值；s400、基于所述跟踪偏差模型和所述控制系数k值，计算得到前轮转角前馈控制量；s500、基于pid反馈控制算法，以及车辆当前状态与所述参考轨迹之间的实际误差量，计算得到前轮转角反馈控制量；s600、基于所述前轮转角前馈控制量与所述前轮转角反馈控制量，计算得到前轮转角控制量。
7.在上述技术方案的基础上，本发明还可以做如下改进：进一步，如上所述的方法，在lqr横向控制算法中加入车辆位置预测模型，所述车辆位置预测模型中的预瞄距离l基于固定长度l1和可变长度l2确定，其中，l1=rlength，l2=v*t，rlength为质心到后轴的距离，t为预瞄时间，v为车辆速度；当v小于第一预设速度时，预瞄距离l=l1，当v大于所述第一预设速度时，预瞄距离l=l1 l2。
8.进一步，如上所述的方法，当v在所述第一预设速度和第二预设速度之间时，lqr横向控制算法中的控制量权重r近似为线性变化，r=k*v b，当v为所述第一预设速度时确定低速控制量权重r1，当v为所述第二预设速度时确定高速控制量权重r2；其中，k*v b 表示从r1到r2的直线，b为此直线与y轴的交点，k为此直线的斜率，k>0。
9.进一步，如上所述的方法，其特征在于，s400包括：基于所述跟踪偏差模型计算得到偏差状态量，将所述偏差状态量与所述控制系数k值相乘得到前轮转角反馈控制量。
10.进一步，如上所述的方法，其特征在于，s600包括：将所述前轮转角前馈控制量与所述前轮转角反馈控制量相加得到前轮转角控制量。
11.本发明还提供了一种车辆线性二自由度动力学模型循迹控制系统，包括：第一建立模块，用于基于车辆线性二自由度动力学模型，建立车辆偏差状态方程；第二建立模块，用于基于所述车辆线性二自由度动力学模型和参考轨迹，建立跟踪偏差模型；求解模块，用于基于所述车辆偏差状态方程和lqr横向控制算法，求解得到控制系数k值；第一计算模块，用于基于所述跟踪偏差模型和所述控制系数k值，计算得到前轮转角前馈控制量；第二计算模块，用于基于pid反馈控制算法，以及车辆当前状态与所述参考轨迹之间的实际误差量，计算得到前轮转角反馈控制量；第三计算模块，用于基于所述前轮转角前馈控制量与所述前轮转角反馈控制量，计算得到前轮转角控制量。
12.在上述技术方案的基础上，本发明还可以做如下改进：进一步，如上所述的系统，在lqr横向控制算法中加入车辆位置预测模型，所述车辆位置预测模型中的预瞄距离l基于固定长度l1和可变长度l2确定，其中，l1=rlength，l2=v*t，rlength为质心到后轴的距离，t为预瞄时间，v为车辆速度；当v小于第一预设速度时，预瞄距离l=l1，当v大于所述第一预设速度时，预瞄距离l=l1 l2。
13.进一步，如上所述的系统，其特征在于，当v在所述第一预设速度和第二预设速度之间时，lqr横向控制算法中的控制量权重r近似为线性变化，r=k*v b，当v为所述第一预设速度时确定低速控制量权重r1，当v为所述第二预设速度时确定高速控制量权重r2；其中，k*v b 表示从r1到r2的直线，b为此直线与y轴的交点，k为此直线的斜率，k>
0。
14.进一步，如上所述的系统，所述第一计算模块具体用于基于所述跟踪偏差模型计算得到偏差状态量，将所述偏差状态量与所述控制系数k值相乘得到前轮转角反馈控制量。
15.进一步，如上所述的系统，所述第三计算模块具体用于将所述前轮转角前馈控制量与所述前轮转角反馈控制量相加得到前轮转角控制量。
16.本发明的有益效果在于：本发明利用pid反馈控制提高了lqr前馈控制在非线性区域的控制精度，并且pid与lqr对mcu的算力需求低，满足车辆对横向控制的超高实时性的需求。
附图说明
17.图1为本发明实施例中提供的一种车辆线性二自由度动力学模型循迹控制方法的流程示意图；图2为本发明实施例中提供的一种车辆线性二自由度动力学模型循迹控制方法的流程框架图；图3为本发明实施例中提供的一种车辆线性二自由度动力学模型循迹控制系统的结构示意图。
具体实施方式
18.为使本发明解决的技术问题、采用的技术方案和达到的技术效果更加清楚，下面将结合附图对本发明实施例的技术方案作进一步的详细描述。
19.本发明实施例提供了一种车辆线性二自由度动力学模型循迹控制方法，如图1所示，该方法包括：s100、基于车辆线性二自由度动力学模型，建立车辆偏差状态方程；s200、基于车辆线性二自由度动力学模型和参考轨迹，建立跟踪偏差模型；s300、基于车辆偏差状态方程和lqr横向控制算法，求解得到控制系数k值；s400、基于跟踪偏差模型和控制系数k值，计算得到前轮转角前馈控制量；s500、基于pid反馈控制算法，以及车辆当前状态与参考轨迹之间的实际误差量，计算得到前轮转角反馈控制量；s600、基于前轮转角前馈控制量与前轮转角反馈控制量，计算得到前轮转角控制量。
20.本发明实施例中，如图2所示，二自由度单轨车辆动力学模型即车辆线性二自由度动力学模型可以通过理论推导建立车辆偏差状态方程，再引入参考轨迹，可获得跟踪偏差模型；车辆偏差状态方程通过lqr算法可以求解出控制系数k的值，通过跟踪偏差模型可以计算出偏差状态量，将偏差状态量与k值相乘得到前轮转角前馈控制量；对参考轨迹得到的误差量进行pid（proportional、integral、differential，比例、积分、微分）计算，得到前轮转角的反馈控制量；最后，将前轮转角前馈与前轮转角反馈相加得到前轮转角控制量。
21.本发明实施例中，在lqr横向控制算法中加入车辆位置预测模型，车辆位置预测模型中的预瞄距离l基于固定长度l1和可变长度l2确定，其中，l1=rlength，l2=v*t，rlength为质心到后轴的距离，t为预瞄时间，v为车辆速度；当v小于第一预设速度时，预瞄距离l=
l1，当v大于所述第一预设速度时，预瞄距离l=l1 l2。当v在第一预设速度和第二预设速度之间时，lqr横向控制算法中的控制量权重r近似为线性变化，r=k*v b，当v为第一预设速度时确定低速控制量权重r1，当v为第二预设速度时确定高速控制量权重r2；其中，k*v b 表示从r1到r2的直线，b为此直线与y轴的交点，k为此直线的斜率，k>0。
22.举例来说，本发明以车辆当前状态和参考轨迹作为输入，eps方向盘控制指令的数值作为输出。使用基于二自由度车辆模型的lqr前馈横向控制算法作为基本输出依据，在此基础上根据车辆当前状态与参考轨迹间的实际误差，使用基于pid的反馈控制算法输出弥补跟踪误差所需的附加方向盘转角，二者相加，从而获得最终保持高精度轨迹跟踪所需的实际方向盘转角。同时，在lqr控制中加入了车辆位置预测模型，具体地，预瞄距离l的选取需要考虑固定长度l1=rlength，以及可变长度l2=v*0.4s，其中，rlength为质心到后轴的距离， v为车辆此时速度，在v<2m/s时，预瞄距离l=l1，在v>2m/s时，预瞄距离l=l1 l2。为了进一步增加车辆在高速下的稳定性，在v>2m/s时，控制量权重r=k*v b，其中k>0。为了提高在非线性区域的控制精度，根据当前预瞄点到参考轨迹的横向偏差，基于pid反馈控制算法输出方向盘转角误差弥补量。lqr前馈控制与pid反馈控制输出的和即为最终方向盘转角。本发明实施例中，车辆在2m/s以下控制较为稳定，可以不加随速度变化的预瞄距离，其它车辆应该视自身控制的稳定性适当增减此速度。本发明实施例中，使用的预瞄时间为0.4s，配合实验车辆的控制输出频率（50hz），以及底盘响应速度有较好的效果。针对不同的车辆，预瞄时间可以适当变化。本发明实施例中，控制量权重r在2m/s到5m/s之间近似为线性变化，2m/s时确定低速r1，5m/s时确定高速r2，其中k*v b表示的就是从低速r1到高速r2这条直线，k为斜率，b为此直线与y轴交点。
23.采用本发明实施例的方法，利用pid反馈控制提高了lqr前馈控制在非线性区域的控制精度，并且pid与lqr对mcu的算力需求低，满足车辆对横向控制的超高实时性的需求。而且，本发明在lqr前馈控制中增加了预测模型，同时将控制量权重r与速度v建立关系，提高了车辆高速下的稳定性。
24.需要说明的是，对于前述的各方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定是本发明所必须的。
25.根据本发明实施例的另一个方面，还提供了一种车辆线性二自由度动力学模型循迹控制系统，如图3所示，包括：第一建立模块100，用于基于车辆线性二自由度动力学模型，建立车辆偏差状态方程；第二建立模块200，用于基于车辆线性二自由度动力学模型和参考轨迹，建立跟踪偏差模型；求解模块300，用于基于车辆偏差状态方程和lqr横向控制算法，求解得到控制系数k值；第一计算模块400，用于基于跟踪偏差模型和控制系数k值，计算得到前轮转角前馈控制量；
第二计算模块500，用于基于pid反馈控制算法，以及车辆当前状态与参考轨迹之间的实际误差量，计算得到前轮转角反馈控制量；第三计算模块600，用于基于前轮转角前馈控制量与前轮转角反馈控制量，计算得到前轮转角控制量。
26.本发明实施例中，在lqr横向控制算法中加入车辆位置预测模型，车辆位置预测模型中的预瞄距离l基于固定长度l1和可变长度l2确定，其中，l1=rlength，l2=v*t，rlength为质心到后轴的距离，t为预瞄时间，v为车辆速度；当v小于第一预设速度时，预瞄距离l=l1，当v大于第一预设速度时，预瞄距离l=l1 l2。当v在第一预设速度和第二预设速度之间时，lqr横向控制算法中的控制量权重r近似为线性变化，r=k*v b，当v为第一预设速度时确定低速控制量权重r1，当v为第二预设速度时确定高速控制量权重r2；其中，k*v b 表示从r1到r2的直线，b为此直线与y轴的交点，k为此直线的斜率，k>0。
27.需要说明的是，本发明的车辆线性二自由度动力学模型循迹控制系统与前述的车辆线性二自由度动力学模型循迹控制方法属于同一发明构思，具体实施方式不再赘述。
28.采用本发明实施例的系统，利用pid反馈控制提高了lqr前馈控制在非线性区域的控制精度，并且pid与lqr对mcu的算力需求低，满足车辆对横向控制的超高实时性的需求。而且，本发明在lqr前馈控制中增加了预测模型，同时将控制量权重r与速度v建立关系，提高了车辆高速下的稳定性。
29.显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其同等技术的范围之内，则本发明也意图包含这些改动和变型在内。