自动驾驶汽车的位姿估算方法

摘要

本发明公开了自动驾驶汽车的位姿估算方法，包括：根据预设的采样周期，实时地采用轮速传感器获取自动驾驶汽车的前轮轮速，同时采用方向盘转角传感器获取自动驾驶汽车的方向盘转角；根据汽车的方向盘转角计算汽车前轮轴的中心转向角；根据当前时刻汽车的前轮轮速、中心转向角，对上一时刻汽车的位置和航向角进行迭代计算，进而获得当前时刻汽车的位置和航向角。本发明只需要轮速传感器及方向盘转角传感器，配合简单的积分算法，即可完成对汽车位姿的估算，在使用成本上相比较依靠高精度的传感器而言大大降低，同时算法简单，易于实现，可广泛应用于自动驾驶行业中。

权利要求

1.自动驾驶汽车的位姿估算方法，其特征在于，包括以下步骤：S1、根据预设的采样周期，实时地采用轮速传感器获取自动驾驶汽车的前轮轮速，同时采用方向盘转角传感器获取自动驾驶汽车的方向盘转角；

S2、根据汽车的方向盘转角计算汽车前轮轴的中心转向角；

S3、根据当前时刻汽车的前轮轮速、中心转向角，对上一时刻汽车的位置和航向角进行迭代计算，进而获得当前时刻汽车的位置和航向角；

所述步骤S2，包括步骤S21～S23：

S21、根据汽车的方向盘转角计算汽车的齿条位移；

S22、对齿条位移进行多项式计算后，分别获得汽车的左前轮转角和右前轮转角；

S23、对汽车的左前轮转角和右前轮转角进行拟合后，获得汽车的中心转向角；

所述步骤S23，其具体为：

根据下式对汽车的左前轮转角和右前轮转角进行拟合后，获得汽车的中心转向角：上式中，α表示汽车的中心转向角，α1表示汽车的左前轮转角，α2表示汽车的右前轮转角。

2.根据权利要求1所述的自动驾驶汽车的位姿估算方法，其特征在于，所述步骤S1之前还包括以下步骤：S0、初始化自动驾驶汽车的位置和航向角。

3.根据权利要求1所述的自动驾驶汽车的位姿估算方法，其特征在于，所述步骤S21，其具体为：采用下式，根据方向盘转角计算汽车的齿条位移：s＝ηβ

其中，s表示汽车的齿条位移，β表示汽车的方向盘转角，η表示比例系数。

4.根据权利要求1所述的自动驾驶汽车的位姿估算方法，其特征在于，所述步骤S22，其具体为：根据下式，对齿条位移进行多项式计算后，分别获得汽车的左前轮转角和右前轮转角：上式中，α1(s)表示汽车的左前轮转角，α2(s)表示汽车的右前轮转角，s表示汽车的齿条位移，ki和pi分别表示n次多项式系数，其中i＝0,1，…，n。

5.根据权利要求1所述的自动驾驶汽车的位姿估算方法，其特征在于，所述步骤S3，其具体为：采用下式，根据当前时刻汽车的前轮轮速、中心转向角，对上一时刻汽车的位置和航向角进行迭代计算，进而获得当前时刻汽车的位置和航向角：上式中，Gxt表示当前时刻汽车的x轴位置，Gyt表示当前时刻汽车的y轴位置，θt表示当前时刻汽车的航向角，Gxt-1表示上一时刻汽车的x轴位置，Gyt-1表示上一时刻汽车的y轴位置，θt-1表示上一时刻汽车的航向角，vf表示汽车的前轮轮速，△T表示采样周期，α表示汽车的中心转向角，l表示汽车的车身轴距。

说明书

自动驾驶汽车的位姿估算方法

技术领域

[0001] 本发明涉及自动驾驶领域，特别是涉及一种自动驾驶汽车的位姿估算方法。

背景技术

[0002] 自动驾驶技术的实现，需要实时获取车辆的位姿信息(位置和航向角)，目前大多数科研机构及整车厂都是依靠高精度的GPS进行定位，结合陀螺仪、加速度计等进行自动驾驶汽车航向角的计算，谷歌研发的一些自动驾驶无人车则是依靠车顶搭载的多线激光雷达进行地图构建，利用SLAM技术实时估算车辆的位姿，这些方法都需要外加昂贵的传感器，同时测量精度受到传感器运行状态的影响很大，例如加速度计容易受到外部干扰而陀螺仪长时间使用容易产生漂移，GPS精度受到对流层折射、通讯延时等的影响。因为现有的自动驾驶汽车位姿估算方法都是基于高精度传感器来实现的，其高成本使得自动驾驶技术不易实现量产，同时基于单一的传感器数据不能保证估计的汽车位姿的精度和鲁棒性，通常的做法是利用多传感器进行数据的融合，弥补各自的缺陷，而多传感器数据融合算法复杂，算法的优劣直接影响到估算的汽车位姿的精度，成为自动驾驶技术的一个技术壁垒。总的来说，目前的自动驾驶汽车的位姿估算方法不仅需要采用高精度的传感器导致成本较高，而且识别算法较为复杂，这两个问题大大制约了自动驾驶汽车的位姿估算，也限制了自动驾驶技术的发展。

发明内容

[0003] 为了解决上述的技术问题，本发明的目的是提供一种自动驾驶汽车的位姿估算方法。

[0004] 本发明解决其技术问题所采用的技术方案是：

[0005] 自动驾驶汽车的位姿估算方法，包括以下步骤：

[0006] S1、根据预设的采样周期，实时地采用轮速传感器获取自动驾驶汽车的前轮轮速，同时采用方向盘转角传感器获取自动驾驶汽车的方向盘转角；

[0007] S2、根据汽车的方向盘转角计算汽车前轮轴的中心转向角；

[0008] S3、根据当前时刻汽车的前轮轮速、中心转向角，对上一时刻汽车的位置和航向角进行迭代计算，进而获得当前时刻汽车的位置和航向角。

[0009] 进一步，所述步骤S1之前还包括以下步骤：

[0010] S0、初始化自动驾驶汽车的位置和航向角。

[0011] 进一步，所述步骤S2，包括：

[0012] S21、根据汽车的方向盘转角计算汽车的齿条位移；

[0013] S22、对齿条位移进行多项式计算后，分别获得汽车的左前轮转角和右前轮转角；

[0014] S23、对汽车的左前轮转角和右前轮转角进行拟合后，获得汽车的中心转向角。

[0015] 进一步，所述步骤S21，其具体为：

[0016] 采用下式，根据方向盘转角计算汽车的齿条位移：

[0017] s＝ηβ

[0018] 其中，s表示汽车的齿条位移，β表示汽车的方向盘转角，η表示比例系数。

[0019] 进一步，所述步骤S22，其具体为：

[0020] 根据下式，对齿条位移进行多项式计算后，分别获得汽车的左前轮转角和右前轮转角：

[0021]

[0022] 上式中，α1(s)表示汽车的左前轮转角，α2(s)表示汽车的右前轮转角，s表示汽车的齿条位移，ki和pi分别表示n次多项式系数，其中i＝0,1，…，n。

[0023] 进一步，所述步骤S23，其具体为：

[0024] 根据下式对汽车的左前轮转角和右前轮转角进行拟合后，获得汽车的中心转向角：

[0025]

[0026] 上式中，α表示汽车的中心转向角，α1表示汽车的左前轮转角，α2表示汽车的右前轮转角。

[0027] 进一步，所述步骤S3，其具体为：

[0028] 采用下式，根据当前时刻汽车的前轮轮速、中心转向角，对上一时刻汽车的位置和航向角进行迭代计算，进而获得当前时刻汽车的位置和航向角：

[0029]

[0030] 上式中， 表示当前时刻汽车的x轴位置， 表示当前时刻汽车的y轴位置，θt表示当前时刻汽车的航向角， 表示上一时刻汽车的x轴位置， 表示上一时刻汽车的y轴位置，θt-1表示上一时刻汽车的航向角，vf表示汽车的前轮轮速，△T表示采样周期，α表示汽车的中心转向角，l表示汽车的车身轴距。

[0031] 本发明的有益效果是：本发明提供了一种自动驾驶汽车的位姿估算方法，包括：根据预设的采样周期，实时地采用轮速传感器获取自动驾驶汽车的前轮轮速，同时采用方向盘转角传感器获取自动驾驶汽车的方向盘转角；根据汽车的方向盘转角计算汽车前轮轴的中心转向角；根据当前时刻汽车的前轮轮速、中心转向角，对上一时刻汽车的位置和航向角进行迭代计算，进而获得当前时刻汽车的位置和航向角。本方法通过轮速传感器获取汽车运动时的前轮轮速，同时将方向盘转角传感器采集的方向盘转角换算为前轮轴的中心转向角，利用迭代计算可估算得到任意时刻汽车的位置和航向角信息。只需要轮速传感器及方向盘转角传感器，配合简单的积分算法，即可完成对汽车位姿的估算，在使用成本上相比较依靠高精度的传感器而言大大降低，同时算法简单，易于实现。

附图说明

[0032] 下面结合附图和实施例对本发明作进一步说明。

[0033] 图1是本发明的自动驾驶汽车的位姿估算方法的流程图；

[0034] 图2是本发明的自动驾驶汽车的位姿估算方法的具体实施例中建立的自动驾驶汽车的运动模型的示意图。

具体实施方式

[0035] 参照图1，本发明提供了自动驾驶汽车的位姿估算方法，包括以下步骤：

[0036] S1、根据预设的采样周期，实时地采用轮速传感器获取自动驾驶汽车的前轮轮速，同时采用方向盘转角传感器获取自动驾驶汽车的方向盘转角；

[0037] S2、根据汽车的方向盘转角计算汽车前轮轴的中心转向角；

[0038] S3、根据当前时刻汽车的前轮轮速、中心转向角，对上一时刻汽车的位置和航向角进行迭代计算，进而获得当前时刻汽车的位置和航向角。

[0039] 进一步作为优选的实施方式，所述步骤S1之前还包括以下步骤：

[0040] S0、初始化自动驾驶汽车的位置和航向角。

[0041] 进一步作为优选的实施方式，所述步骤S2，包括：

[0042] S21、根据汽车的方向盘转角计算汽车的齿条位移；

[0043] S22、对齿条位移进行多项式计算后，分别获得汽车的左前轮转角和右前轮转角；

[0044] S23、对汽车的左前轮转角和右前轮转角进行拟合后，获得汽车的中心转向角。

[0045] 进一步作为优选的实施方式，所述步骤S21，其具体为：

[0046] 采用下式，根据方向盘转角计算汽车的齿条位移：

[0047] s＝ηβ

[0048] 其中，s表示汽车的齿条位移，β表示汽车的方向盘转角，η表示比例系数。

[0049] 进一步作为优选的实施方式，所述步骤S22，其具体为：

[0050] 根据下式，对齿条位移进行多项式计算后，分别获得汽车的左前轮转角和右前轮转角：

[0051]

[0052] 上式中，α1(s)表示汽车的左前轮转角，α2(s)表示汽车的右前轮转角，s表示汽车的齿条位移，ki和pi分别表示n次多项式系数，其中i＝0,1，…，n。

[0053] 进一步作为优选的实施方式，所述步骤S23，其具体为：

[0054] 根据下式对汽车的左前轮转角和右前轮转角进行拟合后，获得汽车的中心转向角：

[0055]

[0056] 上式中，α表示汽车的中心转向角，α1表示汽车的左前轮转角，α2表示汽车的右前轮转角。

[0057] 进一步作为优选的实施方式，所述步骤S3，其具体为：

[0058] 采用下式，根据当前时刻汽车的前轮轮速、中心转向角，对上一时刻汽车的位置和航向角进行迭代计算，进而获得当前时刻汽车的位置和航向角：

[0059]

[0060] 上式中， 表示当前时刻汽车的x轴位置， 表示当前时刻汽车的y轴位置，θt表示当前时刻汽车的航向角， 表示上一时刻汽车的x轴位置， 表示上一时刻汽车的y轴位置，θt-1表示上一时刻汽车的航向角，vf表示汽车的前轮轮速，△T表示采样周期，α表示汽车的中心转向角，l表示汽车的车身轴距。

[0061] 以下结合具体实施例对本发明做详细说明。

[0062] 参照图1，自动驾驶汽车的位姿估算方法，包括以下步骤：

[0063] S0、初始化自动驾驶汽车的位置和航向角；将自动驾驶汽车的航向角初始化为0，位置初始化为Gx＝0,Gy＝0。

[0064] S1、根据预设的采样周期，实时地采用轮速传感器获取自动驾驶汽车的前轮轮速，同时采用方向盘转角传感器获取自动驾驶汽车的方向盘转角；

[0065] S2、根据汽车的方向盘转角计算汽车前轮轴的中心转向角，具体包括S21～S23：

[0066] S21、采用下式，根据方向盘转角计算汽车的齿条位移：

[0067] s＝ηβ

[0068] 其中，s表示汽车的齿条位移，β表示汽车的方向盘转角，η表示比例系数，η为固定值，表示齿条位移总量程与方向盘转角范围(左右打满的角度差，即方向盘转角总量程)的比值，假设齿条位移总量程为s0,汽车方向盘转角总量程为β0，那么可以计算获得比例系数：η＝s0/β0。

[0069] 因为方向盘通过长轴与齿轮固连，依靠齿轮啮合控制齿条的左右移动，齿条的平移通过空间连杆机构转为前轮的转向，由齿轮、齿条啮合关系可知，方向盘转角与齿条位移的关系可以近似处理为线性关系，当从方向盘传感器获取到方向盘转角β后可以通过固定的比例关系η获得齿条位移。

[0070] S22、根据下式，对齿条位移进行多项式计算后，分别获得汽车的左前轮转角和右前轮转角：

[0071]

[0072] 上式中，α1(s)表示汽车的左前轮转角，α2(s)表示汽车的右前轮转角，s表示汽车的齿条位移，ki和pi分别表示n次多项式系数，其中i＝0,1，…，n；

[0073] 本步骤的两个多项式是通过实验测量得到齿条位移、左前轮转角以及右前轮转角的大量数据后，根据得到的数据进行曲线拟合而获得的，准确度高，可以根据齿条位移准确地计算出左前轮转角以及右前轮转角的值。ki(i＝0,1，…，n)和pi(i＝0,1，…，n)分别表示拟合曲线的对应的n次多项式系数。

[0074] S23、根据下式对汽车的左前轮转角和右前轮转角进行拟合后，获得汽车的中心转向角：

[0075]

[0076] 上式中，α表示汽车的中心转向角，α1表示汽车的左前轮转角，α2表示汽车的右前轮转角。

[0077] S3、采用下式，根据当前时刻汽车的前轮轮速、中心转向角，对上一时刻汽车的位置和航向角进行迭代计算，进而获得当前时刻汽车的位置和航向角：

[0078]

[0079] 上式中， 表示当前时刻汽车的x轴位置， 表示当前时刻汽车的y轴位置，θt表示当前时刻汽车的航向角， 表示上一时刻汽车的x轴位置， 表示上一时刻汽车的y轴位置，θt-1表示上一时刻汽车的航向角，vf表示汽车的前轮轮速，△T表示采样周期，l表示汽车的车身轴距。

[0080] 以下结合图2说明步骤S3中的计算公式的原理。

[0081] 图2展示了自动驾驶汽车的运动学模型，图2中，ICC表示瞬时转向中心，(XG,OG,YG)为全局坐标系(里程计坐标系)，(xR,oR,yR)为固联于车身的局部坐标系，定义全局坐标系与初始状态时车身的局部坐标系重合。t时刻，自动驾驶汽车在全局坐标系下的位姿为[0082] 结合图2可获得全局坐标系与局部坐标系的齐次变换矩阵如下：

[0083]

[0084] 本发明中，简化方向控制器的设计，不考虑车辆质量转移，对建立的自动驾驶汽车的运动模型釆用以下几点限制条件：

[0085] 1、整个汽车视为刚体，车轮视为刚性轮，不考虑车轮与地面接触时的弹性变化对整个模型带来的影响；

[0086] 2、汽车处于二维平面上运动，不考虑俯仰和侧倾带来的影响；

[0087] 3、汽车运行的过程中，车轮在道路上作纯滚动运动，忽略滑动带来的影响；

[0088] 4、汽车运行的过程中，路面始终能够给车轮提供足够的静摩擦力，车辆不产生相对滑移或者滑动。

[0089] 因为以上4种情况属于较为特殊的情况，自动驾驶汽车一般不会碰到这些情况，因此结合以上的限定简化所建立的运动模型，从而简化计算过程。

[0090] 基于上述限制条件，由图2中的几何关系可获得如下计算公式：

[0091] R＝l/tanα

[0092] 以及

[0093]

[0094] 其中，R表示汽车的实时转弯半径，ω表示汽车的角速度，vr_x、vr_y分别表示汽车的后轮轴中心点的速度vr在全局坐标系下沿X、Y轴的速度分量，α1表示汽车的左前轮转角，α2表示汽车的右前轮转角，α为前轮轴中心点的转向角即中心转向角，由阿克曼转向原理可知内侧轮转角比外侧轮转角大4度左右(图中为α1>α2)，。在本算法开始启动时，初始化自动驾驶汽车的航向角为0，后续航向角则根据迭代公式计算，设θt为t时刻汽车的航向角，即局部坐标系x轴与全局坐标系X轴的夹角。定义逆时针方向时，转向角和航向角为正。

[0095] 汽车后轮轴中心点的速度vr与前轮轮速vf的关系如下：

[0096] vr＝vf cosα

[0097] 因此可获得：

[0098]

[0099] 在采样周期△T内，可以假设中心点的速度vr保持不变，车沿直线行驶，则可由速度分量分别计算出一个采样周期△T内在全局部坐标系下的位移增量 和 如下：

[0100]

[0101] 因此，根据全局坐标系计算出的一个采样周期内的位移增量，可计算出在任意t时刻，自动驾驶汽车在全局坐标系下的位姿：

[0102]

[0103] 将对应的表达式代入上式，即可获得：

[0104]

[0105] 因此可获知，自动驾驶汽车的位姿由4个参数(vf,α,△T,l)确定，其中车身轴距l和采样周期△T对于特定的车型来说是固定值；vf由轮速传感器提采集获得，前轮轴中心转向角α可由方向盘转角传感器所采集的方向盘转角计算获得。

[0106] 本方法基于阿克曼汽车转向原理，通过轮速传感器获取汽车运动时的前轮轮速，同时将方向盘转角传感器采集的方向盘转角换算为前轮轴的中心转向角，在微小的时间内认为汽车的行进速度及前轮轴的中心转向角不变，利用积分可估算得到任意时刻汽车的位置和航向角信息。只需要轮速传感器及方向盘转角传感器，配合简单的积分算法，即可完成对汽车位姿的估计，在使用成本上相比较依靠高精度的传感器而言大大降低，同时算法简单，易于实现。而且通过在连续的运动中将自动驾驶汽车的位姿进行反复的迭代积分，可实现对智能车位姿连续、稳定的估计与跟踪。

[0107] 以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做出种种的等同变形或替换，这些等同的变型或替换均包含在本申请权利要求所限定的范围内。