基于转弯曲率估计的无人车横向控制方法技术

本发明专利技术公开了一种基于转弯曲率估计的无人车横向控制方法。它包括预定轨迹的设定和GPS-INS组合定位系统数据的采集，以及向自动驾驶仪发出方向盘转角指令信号，特别是，基于先由道路的曲率设定预瞄距离，再由预瞄距离重新设定2个预瞄点A和B，之后，先将预瞄点A和B的经纬度变换到车辆坐标系，以计算出最优转弯曲率，再由最优转弯曲率计算出方向盘转角控制量，并输出到自动驾驶仪的方法实现了对无人车的横向控制。它经实测，当车速为20km/h时，最大跟踪误差仅为0.4m，当掉头车速5km/h时，最大跟踪误差仅0.6m，既实现了对各种复杂轨迹的精确跟随，又避免了单点预瞄反馈控制造成的方向盘抖动，使无人车驾驶的控制量更加平滑；可广泛地用于各种车辆的无人驾驶控制。

【技术实现步骤摘要】
基于转弯曲率估计的无人车横向控制方法
本专利技术涉及一种无人车横向控制方法，尤其是一种基于转弯曲率估计的无人车横向控制方法。
技术介绍
无人驾驶车辆是未来智能车辆的发展趋势之一。无人车的研究也将推动智能交通和智慧城市的发展，具有重大的战略意义。为此，国内外众多机构都展开了对无人车的研发工作，如中国专利技术专利CN102495631B于2013年8月21日公告的一种无人驾驶车辆跟踪预定轨迹的智能控制方法。该方法基于加装的车载计算机、GPS-INS组合定位系统和受车载计算机控制的车辆转向与速度伺服控制系统，采用路点坐标序列描述预定轨迹、等采样周期的计算机控制方式，先建立车身坐标系，确定车辆自身位置信息，再依据弧度值计算道路曲率特性的隶属度函数，定义模糊规则计算输出决策速度，之后按行驶1.5～2.5秒的距离在预定轨迹的坐标序列中搜索大于该距离并距车辆最近的轨迹点，使用自适应比例-微分-积分控制算法公式计算车辆运动的曲率，最后判断当前控制周期目标点是否为路点序列中被选中的最后一点：若是，则向刹车伺服控制系统输出信号启动刹车使车辆减速，若否，则使用控制器局域网模块输出电压信号保持控制油门开度使车辆继续前进。这种控制方法虽也能够使无人驾驶车辆实现对预定轨迹的跟踪功能，且对道路曲率特性变化具有一定的自适应能力，然其仅可近似的描述车辆附近的道路形状，而不能精确地描述道路的曲率，其在转向控制时，采用单点预瞄的横向误差PID(比例微分-积分)控制使得控制量受预瞄点横向误差的影响较大，当预瞄点存在跳动时，很容易引起方向盘转角的突变，导致控制量不够平滑。
技术实现思路
本专利技术要解决的技术问题为克服现有技术中的不足之处，提供一种在保证跟踪精度的同时，对无人驾驶车辆的控制更加平滑的基于转弯曲率估计的无人车横向控制方法。为解决本专利技术的技术问题，所采用的技术方案为：基于转弯曲率估计的无人车横向控制方法包括预定轨迹的设定和GPS-INS组合定位系统数据的采集，以及向自动驾驶仪发出方向盘转角指令信号，特别是主要步骤如下：步骤1，先将上一控制周期的预瞄距离d作为初始预瞄距离d0，若无预瞄距离d，则初始预瞄距离d0取值为15～30m，再采集GPS-INS组合定位系统的车辆位置P、速度v和航向a的信息；步骤2，先选择预定轨迹中的第一点A0，并判断第一点A0是否为预定轨迹中的最后一点，若是，则结束控制，否则由车辆位置P、第一点A0和航向a得出向量以及向量与航向a的夹角，再判断向量与航向a的夹角是否>90度，若是，则选择下一点作为新的第一点A0并进行新的判断直至≤90度；否则，计算出第一点A0到车辆位置P的距离s；步骤3，判断第一点A0到车辆位置P的距离s是否≥初始预瞄距离d0，若是，则将第一点A0作为预瞄点，否则，选择下一点作为新的第一点A0并进行新的计算第一点A0到车辆位置P的距离s，以及再次判断s＜d0，直至s≥d0；步骤4，先从预定轨迹中找出距离车辆位置P的最近点D，并计算出最近点D到第一点A0的轨迹的近处曲率平均值ρnear，以及第一点A0至其之后的2～10个点的轨迹的远处曲率平均值ρfar，再由近处曲率平均值ρnear和远处曲率平均值ρfar按下式得到预瞄距离d，式中的d0为初始预瞄距离、v为速度；步骤5，先以预瞄距离d的值作为预瞄距离，由预定轨迹中选择第一预瞄点A，并判断第一预瞄点A到车辆位置P的距离s是否≥预瞄距离d，若是，则将第一预瞄点A作为预瞄点，否则，选择下一点作为新的第一预瞄点A并进行新的计算第一预瞄点A到车辆位置P的距离s，以及再次判断s<d，直至s≥d，再以同样的方法，以预瞄距离d的值加长度≥1m作为预瞄距离，由预定轨迹中选择出第二预瞄点B；步骤6，以GPS-INS组合定位系统的坐标原点为原点、x轴为沿车身中线指向车辆前方、y轴为指向车身左侧的车辆坐标系下，设第一预瞄点A的纬度、经度坐标为(XA,YA)、以长度米为单位的车辆坐标为(xA,yA)、车辆位置P点的纬度、经度坐标为(XP,YP)，按以下方法变换：xA＝ycosa+xsinayA＝ysina-xcosa，式中的a为航向，得到第一预瞄点A在车辆坐标系下的坐标(xA，yA)，再以同样的变换方法得到第二预瞄点B在车辆坐标系下的坐标(xB，yB)；步骤7，由第一预瞄点A和第二预瞄点B在车辆坐标系下的坐标(xA,yA),(xB,yB)，经下式计算得到最优转弯曲率步骤8，先由下式计算得到方向盘转角控制量式中的I为车辆转向系传动比、L为车辆轴距、K为车辆不足转向系数、δ0为方向盘零位偏移、kp为用于调节动态性能的增益系数、v为速度，再将方向盘转角控制量δ输出到自动驾驶仪后，进入下一控制周期。作为基于转弯曲率估计的无人车横向控制方法的进一步改进：优选地，预定轨迹为由m个目标点组成的序列，其中，每个目标点序列为一m×3维的数组描述，数组中每一行的三个元素分别代表轨迹中一点的纬度、经度和该点处的道路曲率。优选地，GPS-INS组合定位系统的定位精度≤0.1m；利于确保控制的精度。优选地，步骤8中的I＝14，L＝2.7m，K＝0.0003,δ0＝5°,kp＝1.25。优选地，方向盘转角控制量δ经CAN总线输出到自动驾驶仪；使用现有的成熟技术，不仅稳定可靠，也利于降低成本。优选地，进入下一控制周期前，先删除本控制周期位于车辆后方预定轨迹中的所有点；利于减小数据的冗余度，提高计算机的运行效率。相对于现有技术的有益效果是：其一，本专利技术基于先由道路的曲率设定预瞄距离，再由预瞄距离重新设定2个预瞄点A和B，之后，先将预瞄点A和B的经纬度变换到车辆坐标系，以计算出最优转弯曲率，再由最优转弯曲率计算出方向盘转角控制量，并输出到自动驾驶仪的方法对无人车的横向控制，既实现了对各种复杂轨迹的精确跟随，又避免了单点预瞄反馈控制造成的方向盘抖动，使无人车驾驶的控制量更加平滑。其二，经对使用本专利技术的车辆轨迹的实际测量，当车速为20km/h时，其跟踪轨迹与预定轨迹几乎重合，最大跟踪误差仅为0.4m；且方向盘转动平稳，没有突变和毛刺。当掉头车速5km/h时，完成了非常复杂的轨迹跟随任务，且跟踪精度较高，最大跟踪误差仅0.6m。将其与掉头时单点预瞄PID控制量相比较，可看出本专利技术控制的方向盘转角明显地比PID控制时要平滑得多。附图说明图1是本专利技术的一种基本控制流程示意图。图2是使用本专利技术的车辆轨迹跟随效果图，测试时的车速为20km/h。图中的虚线为预定轨迹、实线为实测的跟随轨迹；由图可看出，两者几乎重合。图3是图2中方框的局部放大图。图中的虚线为预定轨迹、实线为实测的跟随轨迹；由图可看出，两者的误差很小，以20km/h的速度行驶，误差最大处仅0.4m。图4是图2所示曲线对应的方向盘转角曲线。由图可看出，方向盘转动平滑，没有跳动和毛刺。图5是使用本专利技术控制车辆掉头时的车辆轨迹跟随效果图，掉头时的车速为5km/h。图中的虚线为预定轨迹、实线为实测的跟随轨迹；可见在复杂的情况下，本方法依然有较高的跟踪精度。图6是将掉头时的PID控制与本专利技术的方向盘转角进行比较的结果图。其中，图上部的曲线图为PID控制的方向盘转角、下部的曲线图为本专利技术的方向盘转角，可看出本专利技术的控制量明显地比PID的平滑。具体实施方式下面本文档来自技高网...

【技术保护点】
一种基于转弯曲率估计的无人车横向控制方法，包括预定轨迹的设定和GPS‑INS组合定位系统数据的采集，以及向自动驾驶仪发出方向盘转角指令信号，其特征在于主要步骤如下：步骤1，先将上一控制周期的预瞄距离d作为初始预瞄距离d0，若无预瞄距离d，则初始预瞄距离d0取值为15～30m，再采集GPS‑INS组合定位系统的车辆位置P、速度v和航向a的信息；步骤2，先选择预定轨迹中的第一点A0，并判断第一点A0是否为预定轨迹中的最后一点，若是，则结束控制，否则由车辆位置P、第一点A0和航向a得出向量以及向量与航向a的夹角，再判断向量与航向a的夹角是否>90度，若是，则选择下一点作为新的第一点A0并进行新的判断直至≤90度；否则，计算出第一点A0到车辆位置P的距离s；步骤3，判断第一点A0到车辆位置P的距离s是否≥初始预瞄距离d0，若是，则将第一点A0作为预瞄点，否则，选择下一点作为新的第一点A0并进行新的计算第一点A0到车辆位置P的距离s，以及再次判断s＜d0，直至s≥d0；步骤4，先从预定轨迹中找出距离车辆位置P的最近点D，并计算出最近点D到第一点A0的轨迹的近处曲率平均值ρnear，以及第一点A0至其之后的2～10个点的轨迹的远处曲率平均值ρfar，再由近处曲率平均值ρnear和远处曲率平均值ρfar按下式得到预瞄距离d，d1=d0,ρnear>ρfard0-0.1v,ρnear≤ρfar,]]>dmin=20,0≤ρfar<0.0115,0.01≤ρfar<0.038,0.03≤ρfar<0.056,ρfar≥0.05,]]>d=d1,d1>dmindmin,d1≤dmin,]]>式中的d0为初始预瞄距离、v为速度；步骤5，先以预瞄距离d的值作为预瞄距离，由预定轨迹中选择第一预瞄点A，并判断第一预瞄点A到车辆位置P的距离s是否≥预瞄距离d，若是，则将第一预瞄点A作为预瞄点，否则，选择下一点作为新的第一预瞄点A并进行新的计算第一预瞄点A到车辆位置P的距离s，以及再次判断s<d，直至s≥d，再以同样的方法，以预瞄距离d的值加长度≥1m作为预瞄距离，由预定轨迹中选择出第二预瞄点B；步骤6，以GPS‑INS组合定位系统的坐标原点为原点、x轴为沿车身中线指向车辆前方、y轴为指向车身左侧的车辆坐标系下，设第一预瞄点A的纬度、经度坐标为(XA,YA)、以长度米为单位的车辆坐标为(xA,yA)、车辆位置P点的纬度、经度坐标为(XP,YP)，按以下方法变换：x=6371004π180(YA-YP)cosπxA180]]>y=6371004π180(XA-XP)]]>xA＝ycosa+xsinayA＝ysina‑xcosa，式中的a为航向，得到第一预瞄点A在车辆坐标系下的坐标(xA，yA)，再以同样的变换方法得到第二预瞄点B在车辆坐标系下的坐标(xB，yB)；步骤7，由第一预瞄点A和第二预瞄点B在车辆坐标系下的坐标(xA,yA),(xB,yB)，经下式计算得到最优转弯曲率ρ=2(yA2+yB2)xA2yA+xB2yB+yA3+yB3;]]>步骤8，先由下式计算得到方向盘转角控制量δ=kp180πρI(L+Kv2)+δ0,]]>式中的I为车辆转向系传动比、L为车辆轴距、K为车辆不足转向系数、δ0为方向盘零位偏移、kp为用于调节动态性能的增益系数、v为速度，再将方向盘转角控制量δ输出到自动驾驶仪后，进入下一控制周期。...

【技术特征摘要】
1.一种基于转弯曲率估计的无人车横向控制方法，包括预定轨迹的设定和GPS-INS组合定位系统数据的采集，以及向自动驾驶仪发出方向盘转角指令信号，其特征在于主要步骤如下：步骤1，先将上一控制周期的预瞄距离d作为初始预瞄距离d0，若无预瞄距离d，则初始预瞄距离d0取值为15～30m，再采集GPS-INS组合定位系统的车辆位置P、速度v和航向a的信息；步骤2，先选择预定轨迹中的第一点A0，并判断第一点A0是否为预定轨迹中的最后一点，若是，则结束控制，否则由车辆位置P、第一点A0和航向a得出向量以及向量与航向a的夹角，再判断向量与航向a的夹角是否大于90度，若是，则选择下一点作为新的第一点A0并进行新的判断直至小于等于90度；否则，计算出第一点A0到车辆位置P的距离s；步骤3，判断第一点A0到车辆位置P的距离s是否大于等于初始预瞄距离d0，若是，则将第一点A0作为预瞄点，否则，选择下一点作为新的第一点A0并进行新的计算第一点A0到车辆位置P的距离s，以及再次判断s与d0的大小关系，直至s≥d0；步骤4，先从预定轨迹中找出距离车辆位置P的最近点D，并计算出最近点D到第一点A0的轨迹的近处曲率平均值ρnear，以及第一点A0至其之后的2～10个点的轨迹的远处曲率平均值ρfar，再由近处曲率平均值ρnear和远处曲率平均值ρfar按下式得到预瞄距离d，式中的d0为初始预瞄距离、v为速度；步骤5，先以预瞄距离d的值作为预瞄距离，由预定轨迹中选择第一预瞄点A，并判断第一预瞄点A到车辆位置P的距离s是否大于等于预瞄距离d，若是，则将第一预瞄点A作为预瞄点，否则，选择下一点作为新的第一预瞄点A并进行新的计算第一预瞄点A到车辆位置P的距离s，以及再次判断s与d的大小关系，直至s≥d，再以同样的方法，在预瞄距离d上增加长...

【专利技术属性】
技术研发人员：刘跃，宋彦，梁华为，梅涛，
申请(专利权)人：中国科学院合肥物质科学研究院，
类型：发明
国别省市：安徽;34