一种自动驾驶汽车低速自动泊车横向控制方法技术

本发明专利技术公开了一种自动驾驶汽车低速自动泊车横向控制方法，包括：规划层规划出泊车轨迹并发送到控制层；定位层对车辆的当前位置实时定位获得定位信息并发送到控制层；底层接收控制层发出的控制指令并实时反馈车辆的当前状态信息；控制层根据车辆运动学模型得出车辆运动的状态空间方程，并建立LQR控制模型，进行求解得出泊车过程中的反馈车辆前轮转角；利用LQR控制模型计算得到实际的反馈前轮转角deltaf_feedback，并结合求得的前馈输出转角deltaf_feedforward得到最终的总输出转角，根据总输出转角控制车辆泊车过程中前向行驶和后向行驶的过程。本发明专利技术的提高了控制的稳定性，同时降低了系统的稳态误差，优化了车辆控制结构，实现全自动泊车控制。

【技术实现步骤摘要】
一种自动驾驶汽车低速自动泊车横向控制方法
本专利技术涉及一种自动驾驶汽车低速自动泊车横向控制方法，属于自动驾驶横向控制的

技术介绍
自动驾驶在近年迎来了高速发展的契机，其中自动驾驶车的实际控制关乎车辆的安全性，稳定性以及舒适性，是自动驾驶技术落地的关键性技术。现有的自动驾驶控制技术分为横向控制技术以及纵向控制技术。横向控制关乎车辆的稳定性，现有的横向控制技术包括纯跟踪算法，Stanley算法，LQR算法，MPC算法等等。动力学LQR算法对车辆进行动力学建模，从而在中高速运动中更能符合车辆的运动特性，在横向控制效果中表现较好。而在低速泊车场景中，车辆转弯时往往面对转角较大的场景，此时动力学模型由于对转角的线性化处理，难以实现精准的横向控制，所以现有的低速横向控制以运动学模型为主。而常用的横向控制算法以PID和纯跟踪算法为主。但PID算法难以适应泊车下多场景的变化以及存在控制参数调节难度大的问题，而纯跟踪算法设置合适的预瞄距离是个很大的挑战，往往设置不合理会导致车辆转弯过程内切或跟踪过程较慢的情况。
技术实现思路
针对现有自动驾驶车辆在低速运动下的控制不精确以及参数调节难度大的问题，本专利技术提供一种自动驾驶汽车低速自动泊车横向控制方法，结合LQR算法的优点和运动学模型在低速的良好表现，提出运动学LQR算法用于车辆泊车过程中的前进，后退等过程。本专利技术具体采用以下技术方案解决上述技术问题：一种自动驾驶汽车低速自动泊车横向控制方法，包括以下步骤：步骤1：自动驾驶车辆的规划层根据当前环境规划出泊车轨迹并发送到控制层；自动驾驶车辆的定位层对车辆的当前位置实时定位获得定位信息并发送到控制层；自动驾驶车辆的底层接收控制层发出的控制指令并实时反馈车辆的当前状态信息；步骤2：自动驾驶车辆的控制层根据车辆运动学模型得出车辆运动的状态空间方程，并建立LQR控制模型；步骤3：自动驾驶车辆的控制层根据建立的LQR控制模型进行求解，推导出泊车过程中的反馈车辆前轮转角deltaf_feedback；步骤4：自动驾驶车辆的控制层结合规划层发送的泊车轨迹、定位层发送的定位信息、底层反馈的车辆的当前状态信息，利用LQR控制模型计算得到实际的反馈前轮转角deltaf_feedback，并结合求得的前馈输出转角deltaf_feedforward得到最终的总输出转角，根据总输出转角控制车辆泊车过程中前向行驶和后向行驶的过程。进一步地，作为本专利技术的一种优选技术方案，所述步骤1中自动驾驶车辆的规划层根据当前环境规划出泊车轨迹，具体为：自动驾驶车辆的规划层规划出到达泊车位的轨迹点集，由轨迹点集形成泊车轨迹，其中轨迹点集包括规划的所有轨迹点，每个轨迹点的信息包括：车辆规划的全局纵坐标x_n，横坐标y_n，航向信息fai_n，以及规划点的曲率信息Kappa_n。进一步地，作为本专利技术的一种优选技术方案，所述步骤1中自动驾驶车辆的定位层获得的定位信息包括车辆当前的全局纵坐标x_a，全局横坐标y_a，航向信息fai_a。进一步地，作为本专利技术的一种优选技术方案，所述步骤1中自动驾驶车辆的底层反馈车辆的当前状态信息包括车速和档位，以及车辆当前的前轮转角。进一步地，作为本专利技术的一种优选技术方案，所述步骤2中建立LQR控制模型，包括：假设在点(xr,yr,fai_r)处满足车辆运动学模型，在该点处对运动学模型进行线性化并据此构造得到状态空间矩阵：X_dot＝A*X+B*U；其中，X_dot＝[x_dot-xr_dot；y_dot-yr_dot；fai_dot-fai_r_dot]；此处x,y,fai为目标轨迹点的坐标和横摆角，xr,yr,fai_r分别为车辆的后轴轴心位置的横和纵坐标、后轴航向角，x_dot,y_dot,fai_dot为x,y,fai的导数，xr_dot,yr_dot,fai_r_dot为xr,yr,fai_r的导数，X_dot是自车与目标轨迹参数的导数的误差矩阵；并且，A＝[0,0,-vr*sin(fai_r)；0,0,vr*cos(fai_r)；0,0,0；]；X＝[x-xr；y-yr；fai-fai_r]；B＝[0；0；vr/L/cos(deltaf)/cos(deltaf)]；U＝delta；公式中，L表示车辆轴距；vr表示后轴速度；X为跟踪误差矩阵；U为控制输入矩阵，代表最优前轮转角；A与B为线性矩阵；将状态空间矩阵离散化获得LQR控制模型：X(k+1)＝At*X(k)+Bt*U(k)，其公式中At＝I+A*T；Bt＝B*T；I为单位矩阵，T为单位时间；X(k+1)为k+1时刻的跟踪误差矩阵，X(k)为k时刻的跟踪误差矩阵；U(K)为k时刻控制输入矩阵；然后，定义如下的评价函数：J(n)＝X(0)_T*Q*X(0)+...+X(n-1)_T*Q+X(n-1)+U(0)_T*R*U(0)+...+U(n-1)_T*R*U(n-1)其中，X(i)为车辆在i处的状态空间矩阵，U(i)为车辆在i处的控制输入矩阵；X(i)_T和U(i)_T分别为X(i)和U(i)的矩阵转置；Q和R分别为状态量和控制量的权重矩阵。进一步地，作为本专利技术的一种优选技术方案，所述步骤4中自动驾驶车辆的控制层利用LQR控制模型计算得到实际的反馈前轮转角deltaf_feedback，并结合求得的前馈输出转角deltaf_feedforward得到最终的总输出转角，具体为：根据定位层所发送车辆当前位置的定位信息预测下一时刻车辆的定位信息；根据预测的下一时刻车辆的定位信息和规划层发送的泊车轨迹，计算出当前自车预测位置点到泊车轨迹中距离最近的轨迹点，获得该距离最近的轨迹点的信息；根据距离最近的轨迹点的信息更新控制层中的参数，并由更新参数后的LQR控制模型求得实际的反馈前轮转角deltaf_feedback；根据车辆的轴距和跟踪目标点的曲率求得前馈输出转角deltaf_feedforward；将求得的反馈前轮转角deltaf_feedback和前馈输出转角deltaf_feedforward结合，得到最终的总输出转角deltaf_output。本专利技术采用上述技术方案，能产生如下技术效果：本专利技术的方法设计在低速泊车工况下，采用运动学模型，适用于转弯较大以及考虑平滑切换前进与后退的场景，由于结合了LQR算法，将最优控制代入了横向控制的过程中，提高了控制的稳定性，同时降低了系统的稳态误差。本专利技术提出运动学LQR控制模型用于车辆泊车过程中的前进，结合运动学模型在低速的良好表现，优化了车辆控制结构，无需驾驶员参与，实现全自动泊车控制。因此，本专利技术方法在实际泊车过程中，可以快速准确地达到自由泊车的目的，稳定性较高。附图说明图1为本专利技术方法采用的车辆运动学模型原理示意图。图2为本专利技术方法构建的LQR模型原理示意图。图本文档来自技高网...

【技术保护点】
1.一种自动驾驶汽车低速自动泊车横向控制方法，其特征在于，包括以下步骤：/n步骤1：自动驾驶车辆的规划层根据当前环境规划出泊车轨迹并发送到控制层；自动驾驶车辆的定位层对车辆的当前位置实时定位获得定位信息并发送到控制层；自动驾驶车辆的底层接收控制层发出的控制指令并实时反馈车辆的当前状态信息；/n步骤2：自动驾驶车辆的控制层根据车辆运动学模型得出车辆运动的状态空间方程，并建立LQR控制模型；/n步骤3：自动驾驶车辆的控制层根据建立的LQR控制模型进行求解，推导出泊车过程中的反馈车辆前轮转角deltaf_feedback；/n步骤4：自动驾驶车辆的控制层结合规划层发送的泊车轨迹、定位层发送的定位信息、底层反馈的车辆的当前状态信息，利用LQR控制模型计算得到实际的反馈前轮转角deltaf_feedback，并结合求得的前馈输出转角deltaf_feedforward得到最终的总输出转角，根据总输出转角控制车辆泊车过程中前向行驶和后向行驶的过程。/n

【技术特征摘要】
1.一种自动驾驶汽车低速自动泊车横向控制方法，其特征在于，包括以下步骤：
步骤1：自动驾驶车辆的规划层根据当前环境规划出泊车轨迹并发送到控制层；自动驾驶车辆的定位层对车辆的当前位置实时定位获得定位信息并发送到控制层；自动驾驶车辆的底层接收控制层发出的控制指令并实时反馈车辆的当前状态信息；
步骤2：自动驾驶车辆的控制层根据车辆运动学模型得出车辆运动的状态空间方程，并建立LQR控制模型；
步骤3：自动驾驶车辆的控制层根据建立的LQR控制模型进行求解，推导出泊车过程中的反馈车辆前轮转角deltaf_feedback；
步骤4：自动驾驶车辆的控制层结合规划层发送的泊车轨迹、定位层发送的定位信息、底层反馈的车辆的当前状态信息，利用LQR控制模型计算得到实际的反馈前轮转角deltaf_feedback，并结合求得的前馈输出转角deltaf_feedforward得到最终的总输出转角，根据总输出转角控制车辆泊车过程中前向行驶和后向行驶的过程。


2.根据权利要求1所述自动驾驶汽车低速自动泊车横向控制方法，其特征在于，所述步骤1中自动驾驶车辆的规划层根据当前环境规划出泊车轨迹，具体为：自动驾驶车辆的规划层规划出到达泊车位的轨迹点集，由轨迹点集形成泊车轨迹，其中轨迹点集包括规划的所有轨迹点，每个轨迹点的信息包括：车辆规划的全局纵坐标x_n，横坐标y_n，航向信息fai_n，以及规划点的曲率信息Kappa_n。


3.根据权利要求1所述自动驾驶汽车低速自动泊车横向控制方法，其特征在于，所述步骤1中自动驾驶车辆的定位层获得的定位信息包括车辆当前的全局纵坐标x_a，全局横坐标y_a，航向信息fai_a。


4.根据权利要求1所述自动驾驶汽车低速自动泊车横向控制方法，其特征在于，所述步骤1中自动驾驶车辆的底层反馈车辆的当前状态信息包括车速和档位，以及车辆当前的前轮转角。


5.根据权利要求1所述自动驾驶汽车低速自动泊车横向控制方法，其特征在于，所述步骤2中建立LQR控制模型，包括：
假设在点(xr,yr,fai_r)处满足车辆运动学模型，在该点处对运动学模型进行线性化并据此构造得到状态空间矩阵：X_dot＝A*X+B*U；
其中，X_dot＝[x_dot-xr_dot；y_dot-yr_dot；fai_dot-fai_r_dot]；
此处x,y,fai为目标轨迹点的坐标和横摆角，xr,yr,fai_r分别为车辆的后轴轴心位置的横和纵...

【专利技术属性】
技术研发人员：吴昊，
申请(专利权)人：的卢技术有限公司，
类型：发明
国别省市：江苏;32