航迹推算定位方法及自动泊车系统与流程

本发明涉及室内移动机器人、辅助泊车系统领域，特别地，涉及一种基于超声波传感器和轮速传感器的航迹推算定位方法及自动泊车系统。

背景技术：

随着工业飞速发展，汽车作为一种运输或代步工具逐渐成为人们工作与生活的重要组成部分。汽车技术已成为集成自动控制理论、人工智能理论、优化理论、传感器技术、通信技术、信号处理技术、图像处理技术、信息融合技术等多理论、多技术于一体的综合性技术。随着人民生活条件的同益改善，人们对汽车性能的要求也在不断提高，要求汽车不但具备较高的操纵稳定性、良好的乘坐舒适性、可观的燃油经济性，还要具有较高的驾驶安全性。人口数量的增加以及城市建筑设施的不断扩增，使有限的城市空间变得拥挤不堪，汽车数量的逐年增加，不仅导致交通同趋拥堵，每辆汽车平均分配到的停放空间也在逐渐缩小。

狭小的泊车空间及驾驶员有限的视野范围给汽车行驶，尤其是泊车入位操作增加了困难。泊车过程中驾驶员视野范围有限，仅能依靠后视镜获取车辆周围环境信息，不仅要时刻注意车辆与周围物体相对距离，同时还要通过离合器和制动器控制车辆平稳缓慢运动，并且要配合适当的方向盘转角，以保证车辆安全地进入目标车位。当目标车位过小或驾驶员泊车经验不足，方向盘转向操作和车速未能合理配合时，驾驶员需多次进出车位，调整车辆位姿，才能完成泊车。

自动泊车系统成为近年来国内外研究热点，随着传感技术的快速发展，研究人员将多种传感器技术应用于泊车辅助系统。其中车辆在泊车过程中，其自身的航迹和位姿定位技术是关键。

定位方法在机器人控制领域研究成果较多，所使用传感器不同，定位方法也不同，车辆定位或机器人定位主要分为：绝对定位、相对定位；根据定位原理及使用的传感器，相对定位方法主要为：基于视觉技术的相对定位方法和航迹推算法。

航迹推算法主要利用里程计、陀螺仪等车辆内部传感器测量车辆行驶状态，得到采样周期内车辆位置和方位角增量，分别对位姿增量累计，得到相对于初始状态的车辆位姿。根据传感器类型，航迹推算法又可分为：基于惯性导航仪的航迹推算法、基于里程计的航迹推算法。

由于视觉技术的定位方法，获取环境信息量大，但对信息处理器的数据处理能力要求高，且视觉传感器受环境光线强度影响大。光照过强或过暗都将影响基于视觉传感器定位方法的定位精度。采用惯性导航仪器测量车辆行驶状态较困难。

现有关于机器人定位技术主要有两种：gps定位、imu加视觉。这两种方法各有缺点：gps定位的方法对环境要求较高，在室内或者桥洞下gps信号不稳定，很难达到稳定的定位效果；imu加视觉的方式，成本较高，且在恶劣环境下，图像质量交叉，视觉的用处将受到严重的制约。

技术实现要素：

本发明目的在于提供一种基于超声波传感器和轮速传感器的航迹推算定位方法，以解决室内移动机器人、辅助泊车系统在车辆低速运动状态下，采用惯性导航设备测量车辆行驶状态比较困难，利用超声波传感器确定泊车空间及泊车空间坐标系，使用轮速传感器的数据，采用航迹推算算法，定位车辆在泊车空间坐标系中的初始位姿和泊车过程中的车辆位姿；在移动机器人领域当中，可用于移动机器人自主对充电桩位置的探索。

为实现上述目的，本发明的一方面提供了一种基于超声波传感器和轮速传感器的航迹推算定位方法，包括步骤：

s1.启动泊车系统，获取超声波传感器数据和轮速传感器数据，建立泊车坐标系；

s2.进行泊车位空间尺寸计算，并判断是否满足平行泊车位或垂直泊车位空间尺寸标准，倘若是，进入步骤s3，否则重新进行泊车位空间尺寸计算；

s3.进行泊车车辆初始位置计算，获取左右后轮轮速传感器数据，或者获取左右前轮轮速传感器及方向盘转角信息，并计算后轮轴中心点运动轨迹。

作为优选的技术方案之一，步骤s2中平行泊车位空间尺寸标准的确定方法如下：

根据自动泊车系统的路径规划分析与推算，采用双圆弧叠加进行轨迹规划的平行泊车最小泊车位空间尺寸为：a×b(依据路径规划约束条件推算)，采用b样条进行轨迹规划的平行泊车最小泊车位空间尺寸为：c×d(依据路径规划约束条件推算)；为了提高道路两侧车位的利用率，当车位满足a×(b-0.2)或c×(d-0.2)时，认为是有效车位；设定目标车位长度为：lco，宽度为：lko，实测车位长度为：ic；根据车位长度确定路径规划方法，如果实测车位长度lc≥a时，泊车系统采用双圆弧叠加进行轨迹规划，即lco＝a，lko＝b；如果实测车位长度c≤lc≤a时，泊车系统采用b样条进行轨迹规划，即lco＝c，lko＝d；以上两种泊车方式都采用以前车为车辆停放基准。

作为优选的技术方案之一，步骤s2中垂直泊车位空间尺寸标准的确定方法如下：

设定目标车位长度为：lco，宽度为：lko；根据自动泊车系统的路径规划分析与推算，采用双圆弧叠加进行轨迹规划的垂直泊车最小泊车位空间尺寸为：e×f(依据路径规划约束条件推算)，采用b样条进行轨迹规划的平行泊车最小泊车位空间尺寸为：g×h(依据路径规划约束条件推算)；为了提高道路两侧车位的利用率，当车位满足(e-0.2)×f或(g-0.2)×h时，认为是有效车位。

作为优选的技术方案之一，步骤s3包括如下步骤：进行泊车车辆初始位置计算，根据数据异常检测算法检测前轮数据，若左右前轮数据正常，则用左右前轮数据计算后轮轴中心点运动轨迹；若左右前轮数据异常，则用左右后轮数据计算后轮轴中心点运动轨迹。

作为优选的技术方案之一，步骤s3中采用左右后轮数据计算后轮轴中心点运动轨迹的具体方法是：进行泊车车辆初始位置计算，获取左右后轮轮速传感器数据，推算后轮轴中心点速度vr，推算车辆航向角θ，计算后轮轴中心点运动轨迹(xr(t),yr(t))。

作为进一步优选的技术方案之一，由can总线得到后轮左右两轮的轮速传感器给出的车轮转动距离δsrl和δsrr，两者分别为左轮转动距离和右轮转动距离，对于后轮中心mr，其瞬时速度为vr，满足如下公式：

式中，xr′(t)、yr′(t)分别为后轮轴中心在x，y轴方向的瞬时速度；δxr、δyr为x、y轴方向的瞬时位移；vr(t)为后轮轴中心瞬时速度；θ为后轮航向角。

其中航向角θ的瞬时改变量θ′(t)为：

为了简化计算，将左右后轮的瞬时速度的平均值作为后轮中心点的瞬时速度，设后轮中心处mr的速度vr为左右后轮瞬时速度的平均值，即：

将(3)式代入(1)式，合并(2)式，整理得：

式中w为后轮轴轮距。

作为优选的技术方案之一，步骤s3中采用左右前轮数据计算后轮轴中心点运动轨迹的具体方法是：进行泊车车辆初始位置计算，获取左右前轮轮速传感器及方向盘转角信息，计算前轮中心转向角计算前轮轴中心点速度vf，推算后轮轴中心点速度vr，推算车辆航向角θ，计算后轮轴中心点运动轨迹(xr(t),yr(t))。

作为进一步优选的技术方案之一，由can总线得到前轮左右两轮的轮速传感器给出的车轮转动距离δsfl和δsfr，方向盘转向角为φ可知，由方向盘转向角为φ得前轮中心转向角

其中k为方向盘转角φ与前轮中心转向角之间的传动比。

在坐标系xoy中，前后轴中心点位置mr和mf坐标关系为：

变换为：

对上式进行微分：

一般情况下，泊车速度比较低，可以假定无滑轮现象，即后轮的轨迹在车辆垂直方向速度为0，其方程式表示为：

y′rcosθ-x′rsinθ＝0(9)

将(8)式代入(9)中得：

x′f·sinθ-y′f·cosθ+θ′·l＝0(10)

前轮轴中心点mf处的x,y方向速度为：

将(11)式代入(10)中可得到车辆回转角速度：

将(11)式和(12)代入(7)得到后轮轴中间点mr在x,y方向速度为：

其中vf为前轮中心处mf的速度，为了通过左右前轮瞬时位移δsfl、δsfr，求出vf，即可得到后轮轴中间点mr的运动轨迹。

设前轮中心转向角在瞬时不变，车辆瞬时沿作圆轨道运动，瞬时前进的距离分别为：δsfl、δsfr；

为了简化计算，设前轮中心处mf的速度vf为左右前轮瞬时速度的平均值，即：

在δt时间内，由：

将(14)式代入(15)(16)式，整理得到：

通过单位时间内左右前轮运动行程δsfl、δsfr和方向盘转角φ，计算车身方位角增量和后轴中点位置增量，分别对车身方位角增量和位置增量累加，可得出车辆相对起始状态的位姿。式中由公式(5)得到，l为车辆前后轴距。

作为优选的技术方案之一，步骤s3中泊车车辆初始位置的计算方法如下：

泊车系统根据实测车位长度，确定泊车方式和目标车位尺寸；根据实测车位宽度，采用以前车为车辆停放基准；坐标系的原点以开始启动自动泊车时为基准，建立坐标系并计算车辆初始位置，其车辆初始坐标为：

在(18)式中，δs＝(xs2-xs1-lco)/2，lrad为测位超声波安装点到后轴距离，yrd1为超声波测得最小侧向距离，lk为车辆宽度，x0、y0为车辆以泊车位外侧边界线为基准停车时，车辆泊车起点坐标。

作为进一步优选的技术方案之一，车位长度和车位宽度的检测方法如下：

泊车系统开启后，泊车系统通过超声波传感器检测右侧车位，根据超声波测距的变化，判断车位起始点与终止点，并记录对应车轮里程xs1、xs2；车辆里程位xs1～xs2时，记超声波测距最小值为yrd2，车轮里程大于xs2时，记超声波测距最小值为yrd1；

若车位满足xs2-xs1≥a，且yrd2-yrd1≥b，或者车位满足xs2-xs1≥c，且yrd2-yrd1≥d，泊车系统可以通过人机交互系统，确定泊车方位(即左侧或右侧泊车)，车辆进入泊车起始区域后，根据超声波传感器测距更新yrd1；车辆停止运动后，泊车系统记录停车时车辆里程xs3；泊车系统根据超声波测距及车辆里程，得到车位长度lc＝xs2-xs1，车位宽度lk＝yrd2-yrd1。

本发明的另一方面提供一种自动泊车系统，所述泊车系统采用上述所述的航迹推算定位方法。

本发明的有益效果在于：

1、本发明提供一种基于超声波传感器和轮速传感器的航迹推算定位方法，以解决室内移动机器人、辅助泊车系统在车辆低速运动状态下，采用惯性导航设备测量车辆行驶状态比较困难，利用超声波传感器确定泊车空间及泊车空间坐标系，使用轮速传感器的数据，采用航迹推算算法，定位车辆在泊车空间坐标系中的初始位姿和泊车过程中的车辆位姿；在移动机器人领域当中，可用于移动机器人自主对充电桩位置的探索。

2、本发明的主要特点是受环境影响较小，且成本低；传感器静态漂移测量误差小；在轮速传感器传输数据出现跳变时，通过计算动态阈值，可有效滤掉非正常数据，对车辆或者机器人进行有效定位。

3、图3-6为仿真结果图，从图中，可以得知：传统方法单次定位误差经常会发生跳变，定位误差变得很大，而本发明单次定位误差曲线较为稳定，且误差都比较小，说明传统方法误差发生跳变的原因是前轮传感器数据出错，而利用本发明方法可以有效避免前轮传感器数据出错给小车定位带来误差。同理，传统方法各组实验平均定位误差比本发明方法更大，且不稳定，这同样是前轮传感器数据出错导致的。

附图说明

构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的限定。在附图中：

图1是本发明实施例中车辆运动位姿航迹推算定位图；

图2是本发明实施例提供的一种基于超声波传感器和轮速传感器的航迹推算定位方法的流程图；

图3是传统方法定位单次误差曲线图；

图4是本发明实施例提供的一种基于超声波传感器和轮速传感器的航迹推算定位方法定位单次误差曲线图；

图5是传统方法各组实验平均定位误差曲线图；

图6是本发明实施例发明实施例提供的一种基于超声波传感器和轮速传感器的航迹推算定位方法各组实验平均定位误差曲线图。

具体实施方式

以下将结合实施例对本发明的构思、具体结构及产生的技术效果进行清楚、完整地描述，以充分地理解本发明的目的、特征和效果。显然，所描述的实施例只是本发明的一部分实施例，而不是全部实施例，基于本发明的实施例，本领域的技术人员在不付出创造性劳动的前提下所获得的其他实施例，均属于本发明保护的范围。

实施例1：

本发明实施例1提供一种基于超声波传感器和轮速传感器的航迹推算定位方法，，该方法使用左右前轮码盘脉冲计数和方向盘转角信息，测量车轮位移，根据采样周期内车辆的变化，计算车辆方位角增量和位置增量，得到相对初始状态的位姿，定位车辆在泊车空间坐标系中的位姿。该算法的具体步骤为：

1、泊车位标准的确定

(1)、平行泊车位空间尺寸标准

根据自动泊车系统的路径规划分析与推算，采用双圆弧叠加进行轨迹规划的平行泊车最小泊车位空间尺寸为：a×b(依据路径规划约束条件推算)，采用b样条进行轨迹规划的平行泊车最小泊车位空间尺寸为：c×d(依据路径规划约束条件推算)。为了提高道路两侧车位的利用率，当车位满足a×(b-0.2)或c×(d-0.2)时，认为是有效车位。记目标车位长度为：lco，宽度为：lko。

根据车位长度确定路径规划方法，如果实测车位长度lc≥a时，泊车系统采用双圆弧叠加进行轨迹规划，即lco＝a，lko＝b；如果实测车位长度c≤lc≤a时，泊车系统采用b样条进行轨迹规划，即lco＝c，lko＝d；以上两种泊车方式都采用以前车为车辆停放基准。

(2)垂直泊车位空间尺寸标准

根据自动泊车系统的路径规划分析与推算，采用双圆弧叠加进行轨迹规划的垂直泊车最小泊车位空间尺寸为：e×f(依据路径规划约束条件推算)，采用b样条进行轨迹规划的平行泊车最小泊车位空间尺寸为：g×h(依据路径规划约束条件推算)。为了提高道路两侧车位的利用率，当车位满足(e-0.2)×f或(g-0.2)×h时，认为是有效车位。记目标车位长度为：lco，宽度为：lko。

2、泊车初始位置计算

(1)、泊车位检测方法及确定

泊车系统开启后，泊车系统通过超声波传感器检测右侧车位，根据超声波测距的变化，判断车位起始点与终止点，并记录对应车轮里程xs1、xs2。车辆里程位xs1～xs2时，记超声波测距最小值为yrd2，车轮里程大于xs2时，记超声波测距最小值为yrd1。

若车位满足xs2-xs1≥a，且yrd2-yrd1≥b，或者车位满足xs2-xs1≥c，且yrd2-yrd1≥d，泊车系统可以通过人机交互系统，确定泊车方位(即左侧或右侧泊车)，车辆进入泊车起始区域后，根据超声波传感器测距更新yrd1。车辆停止运动后，泊车系统记录停车时车辆里程xs3。泊车系统根据超声波测距及车辆里程，得到车位长度lc＝xs2-xs1，车位宽度lk＝yrd2-yrd1。

(2)车辆初始位计算

泊车系统根据实测车位长度，确定泊车方式和目标车位尺寸。根据实测车位宽度，采用以前车为车辆停放基准。

坐标系的原点以开始启动自动泊车时为基准，建立坐标系并计算车辆初始位置，如1所示，其车辆初始坐标为：

在(1)式中，δs＝(xs2-xs1-lco)/2，lrad为测位超声波安装点到后轴距离，yrd1为超声波测得最小侧向距离，lk为车辆宽度，x0、y0为车辆以泊车位外侧边界线为基准停车时，车辆泊车起点坐标。

3、基于航迹推算方法的车辆运动位姿计算方法

航迹推算方法主要使用左右后轮码盘脉冲计数和方向盘转角信息，测量车轮位移，根据采样周期内车辆的变化，计算车辆方位角增量和位置增量，得到相对初始状态的位姿，定位车辆在泊车空间坐标系中的位姿，根据can总线获取的左右轮码盘脉冲计数和方向盘转角信息的种类，进行下列两种基于航迹推算方法的车辆运动位姿计算方法：

(1)已知左右前轮轮速传感器数据和方向盘信息推算车辆运动位姿

由can总线得到前轮左右两轮的轮速传感器给出的车轮转动距离δsfl和δsfr，方向盘转向角为φ可知，由方向盘转向角为φ得前轮中心转向角

其中k为方向盘转角φ与前轮中心转向角之间的传动比。

在坐标系xoy中，前后轴中心点位置mr和mf坐标关系为：

变换为：

对上式进行微分：

一般情况下，泊车速度比较低，可以假定无滑轮现象，即后轮的轨迹在车辆垂直方向速度为0，其方程式表示为：

y′rcosθ-x′rsinθ＝0(6)

将(5)式代入(6)中得：

x′f·sinθ-y′f·cosθ+θ′·l＝0(7)

由图2可知，前轮轴中心点mf处的x,y方向速度为：

将(8)式代入(7)中可得到车辆回转角速度：

将(8)式和(9)代入(5)得到后轮轴中间点mr在x,y方向速度为：

其中vf为前轮中心处mf的速度。为了通过左右前轮瞬时位移δsfl、δsfr，求出vf，即可得到后轮轴中间点mr的运动轨迹。

设前轮中心转向角在瞬时不变，车辆瞬时沿作圆轨道运动，瞬时前进的距离分别为：δsfl、δsfr。

为了简化计算，设前轮中心处mf的速度vf为左右前轮瞬时速度的平均值，即：

在δt时间内，由：

将(11)式代入(12)(13)式，整理得到：

通过单位时间内左右前轮运动行程δsfl、δsfr和方向盘转角φ，计算车身方位角增量和后轴中点位置增量，分别对车身方位角增量和位置增量累加，可得出车辆相对起始状态的位姿。式中由公式(2)得到，l为车辆前后轴距。

(2)已知左右后轮轮速传感器数据推算车辆运动位姿

由can总线得到后轮左右两轮的轮速传感器给出的车轮转动距离δsrl和δsrr。如图3所示，对于后轮中心mr，其瞬时速度为vr，满足如下公式：

其中航向角θ为：

为了简化计算，设后轮中心处mr的速度vr为左右后轮瞬时速度的平均值，即：

将(17)式代入(15)式，合并(16)式，整理得：

式中w为后轮轴轮距。

实施例2

本发明的实施例2提供一种自动泊车系统，所述泊车系统采用实施例1所述的航迹推算定位方法。

需要说明的是，在整个申请文件中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实例的说明只是用于帮助理解本发明的方法及其核心思想。以上仅是本发明的优选实施方式，应当指出，由于文字表达的有限性，而客观上存在无限的试试方式，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进、润饰或变化，也可以将上述技术特征以适当的方式进行组合；这些改进润饰、变化或组合，或未经改进将发明的构思和技术方案直接应用于其它场合的，均应视为本发明的保护范围。