基于转弯曲率估计的无人车横向控制方法
【技术领域】
[0001] 本发明涉及一种无人车横向控制方法，尤其是一种基于转弯曲率估计的无人车横 向控制方法。
【背景技术】
[0002] 无人驾驶车辆是未来智能车辆的发展趋势之一。无人车的研宄也将推动智能交通 和智慧城市的发展，具有重大的战略意义。为此，国内外众多机构都展开了对无人车的研发 工作，如中国发明专利CN102495631B于2013年8月21日公告的一种无人驾驶车辆跟踪 预定轨迹的智能控制方法。该方法基于加装的车载计算机、GPS-INS组合定位系统和受车 载计算机控制的车辆转向与速度伺服控制系统，采用路点坐标序列描述预定轨迹、等采样 周期的计算机控制方式，先建立车身坐标系，确定车辆自身位置信息，再依据弧度值计算道 路曲率特性的隶属度函数，定义模糊规则计算输出决策速度，之后按行驶1. 5?2. 5秒的距 离在预定轨迹的坐标序列中搜索大于该距离并距车辆最近的轨迹点，使用自适应比例-微 分-积分控制算法公式计算车辆运动的曲率，最后判断当前控制周期目标点是否为路点序 列中被选中的最后一点：若是，则向刹车伺服控制系统输出信号启动刹车使车辆减速，若 否，则使用控制器局域网模块输出电压信号保持控制油门开度使车辆继续前进。这种控制 方法虽也能够使无人驾驶车辆实现对预定轨迹的跟踪功能，且对道路曲率特性变化具有一 定的自适应能力，然其仅可近似的描述车辆附近的道路形状，而不能精确地描述道路的曲 率，其在转向控制时，采用单点预瞄的横向误差PID(比例微分-积分）控制使得控制量受 预瞄点横向误差的影响较大，当预瞄点存在跳动时，很容易引起方向盘转角的突变，导致控 制量不够平滑。
【发明内容】
[0003] 本发明要解决的技术问题为克服现有技术中的不足之处，提供一种在保证跟踪精 度的同时，对无人驾驶车辆的控制更加平滑的基于转弯曲率估计的无人车横向控制方法。
[0004] 为解决本发明的技术问题，所采用的技术方案为：基于转弯曲率估计的无人车横 向控制方法包括预定轨迹的设定和GPS-INS组合定位系统数据的采集，以及向自动驾驶仪 发出方向盘转角指令信号，特别是主要步骤如下：
[0005] 步骤1，先将上一控制周期的预瞄距离d作为初始预瞄距离屯，若无预瞄距离d，则 初始预瞄距离屯取值为15?30m，再采集GPS-INS组合定位系统的车辆位置P、速度v和 航向a的信息；
[0006] 步骤2,先选择预定轨迹中的第一点A0,并判断第一点A0是否为预定轨迹中的最 后一点，若是，则结束控制，否则由车辆位置P、第一点A0和航向a得出向量^^，以及向量 M与航向a的夹角，再判断向量7^与航向a的夹角是否>90度，若是，则选择下一点作 为新的第一点A0并进行新的判断直至< 90度；否则，计算出第一点A0到车辆位置P的距 离S;
[0007] 步骤3,判断第一点A0到车辆位置P的距离s是否多初始预瞄距离dQ，若是，则将 第一点A0作为预目苗点，否则，选择下一点作为新的第一点A0并进行新的计算第一点A0到 车辆位置P的距离s，以及再次判断s<屯，直至s彡dQ;
[0008] 步骤4,先从预定轨迹中找出距离车辆位置P的最近点D，并计算出最近点D到第 一点A0的轨迹的近处曲率平均值Pnea,，以及第一点A0至其之后的2?10个点的轨迹的 远处曲率平均值pfm，再由近处曲率平均值pn_和远处曲率平均值p^按下式得到预瞄 距离d，
【主权项】
1. 一种基于转弯曲率估计的无人车横向控制方法，包括预定轨迹的设定和GPS-INS组 合定位系统数据的采集，以及向自动驾驶仪发出方向盘转角指令信号，其特征在于主要步 骤如下： 步骤1，先将上一控制周期的预瞄距离d作为初始预瞄距离屯，若无预瞄距离d，则初始 预瞄距离dQ取值为15?30m，再采集GPS-INS组合定位系统的车辆位置P、速度v和航向a 的信息； 步骤2,先选择预定轨迹中的第一点A0,并判断第一点A0是否为预定轨迹中的最后一 点，若是，则结束控制，否则由车辆位置P、第一点A0和航向a得出向量以及向量7^ 与航向a的夹角，再判断向量％与航向a的夹角是否>90度，若是，贝1」选择下一点作为新 的第一点A0并进行新的判断直至< 90度；否则，计算出第一点A0到车辆位置P的距离s; 步骤3,判断第一点A0到车辆位置P的距离s是否多初始预瞄距离屯，若是，则将第一 点A0作为预目苗点，否则，选择下一点作为新的第一点A0并进行新的计算第一点A0到车辆 位置P的距离s，以及再次判断s<屯，直至s彡dQ; 步骤4,先从预定轨迹中找出距离车辆位置P的最近点D，并计算出最近点D到第一点A0的轨迹的近处曲率平均值Pnem，以及第一点A0至其之后的2?10个点的轨迹的远处曲 率平均值Pfm，再由近处曲率平均值Pn_和远处曲率平均值P按下式得到预瞄距离d，
式中的屯为初始预瞄距离、v为速度； 步骤5,先以预瞄距离d的值作为预瞄距离，由预定轨迹中选择第一预瞄点A，并判断第 一预瞄点A到车辆位置P的距离s是否多预瞄距离d，若是，则将第一预瞄点A作为预瞄点， 否则，选择下一点作为新的第一预瞄点A并进行新的计算第一预瞄点A到车辆位置P的距 离s，以及再次判断s〈d，直至s多d，再以同样的方法，以预瞄距离d的值加长度多lm作为 预瞄距离，由预定轨迹中选择出第二预瞄点B; 步骤6，以GPS-INS组合定位系统的坐标原点为原点、x轴为沿车身中线指向车辆前方、y轴为指向车身左侧的车辆坐标系下，设第一预瞄点A的炜度、经度坐标为（XA，YA)、以长度 米为单位的车辆坐标为（xA，yA)、车辆位置P点的炜度、经度坐标为（XP，YP)，按以下方法变 换：
xA= ycosa+xsina yA= ysina-xcosa， 式中的a为航向，得到第一预瞄点A在车辆坐标系下的坐标（xA，yA)， 再以同样的变换方法得到第二预瞄点B在车辆坐标系下的坐标（xB，yB); 步骤7,由第一预目苗点A和第二预目苗点B在车辆坐标系下的坐标（xA,yA)，（xB,yB)，经下 式计算得到最优转弯曲率
步骤8,先由下式计算得到方向盘转角控制量
式中的I为车辆转向系传动比、L为车辆轴距、K为车辆不足转向系数、为方向盘零 位偏移、kp为用于调节动态性能的增益系数、v为速度，再将方向盘转角控制量S输出到自 动驾驶仪后，进入下一控制周期。
2. 根据权利要求1所述的基于转弯曲率估计的无人车横向控制方法，其特征是预定轨 迹为由m个目标点组成的序列，其中，每个目标点序列为一mX3维的数组描述，数组中每一 行的三个元素分别代表轨迹中一点的炜度、经度和该点处的道路曲率。
3. 根据权利要求1所述的基于转弯曲率估计的无人车横向控制方法，其特征是 GPS-INS组合定位系统的定位精度< 0.lm。
4. 根据权利要求1所述的基于转弯曲率估计的无人车横向控制方法，其特征是步骤8 中的I= 14,L= 2. 7m，K= 0? 0003,S。= 5°，kp= 1. 25。
5. 根据权利要求1所述的基于转弯曲率估计的无人车横向控制方法，其特征是方向盘 转角控制量S经CAN总线输出到自动驾驶仪。
6. 根据权利要求1所述的基于转弯曲率估计的无人车横向控制方法，其特征是进入下 一控制周期前，先删除本控制周期位于车辆后方预定轨迹中的所有点。
【专利摘要】本发明公开了一种基于转弯曲率估计的无人车横向控制方法。它包括预定轨迹的设定和GPS-INS组合定位系统数据的采集，以及向自动驾驶仪发出方向盘转角指令信号，特别是，基于先由道路的曲率设定预瞄距离，再由预瞄距离重新设定2个预瞄点A和B，之后，先将预瞄点A和B的经纬度变换到车辆坐标系，以计算出最优转弯曲率，再由最优转弯曲率计算出方向盘转角控制量，并输出到自动驾驶仪的方法实现了对无人车的横向控制。它经实测，当车速为20km/h时，最大跟踪误差仅为0.4m，当掉头车速5km/h时，最大跟踪误差仅0.6m，既实现了对各种复杂轨迹的精确跟随，又避免了单点预瞄反馈控制造成的方向盘抖动，使无人车驾驶的控制量更加平滑；可广泛地用于各种车辆的无人驾驶控制。
【IPC分类】G05D1-02
【公开号】CN104571112
【申请号】CN201510018459
【发明人】刘跃, 宋彦, 梁华为, 梅涛
【申请人】中国科学院合肥物质科学研究院
【公开日】2015年4月29日
【申请日】2015年1月14日