CN112339748B - 自动泊车中通过环境扫描修正车辆位姿信息的方法及装置 - Google Patents

Abstract

本发明公开了一种自动泊车中通过环境扫描修正车辆位姿信息的方法及装置，在搜索车位前，以车辆当前位置作为原点建立坐标系；在搜索车位时，通过轮速脉冲信息、档位信息以及方向盘转角信息确定本车定位信息，进而确定本车行进的道路中线信息，结合本车与周围障碍物的距离信息确定局部环境信息；将局部环境信息录入至坐标系中，并在自动泊车时，通过实时采集的本车定位信息和距离信息确定实时环境信息，并与局部环境信息坐标体系进行比对，对局部环境坐标体系进行修正；在车辆运行过程中，基于局部环境信息坐标体系确定本车与障碍物的距离信息，将距离信息与实际距离信息进行比对，对车辆的位姿信息进行修正，以获得更精准的车辆位姿信息。

Description

自动泊车中通过环境扫描修正车辆位姿信息的方法及装置

技术领域

本发明属于自动泊车领域，更具体地，涉及一种自动泊车过程中通过环境扫描修正车辆位姿信息的方法及装置。

背景技术

实现自动泊车的一个关键因素就是解决定位问题。自动泊车的定位问题既包括自车的定位问题，还包括周围环境的定位问题。只有解决自车的定位问题和周围环境的定位问题，才能准确的将车辆泊入停车位中。

参考专利申请CN110979313A，公开了一种基于空间地图的自动泊车定位方法及系统，具体公开了以车辆启动时所在的位置为世界坐标原点，通过实时的轮速数据和轮胎的转角数据计算车辆相对于世界坐标原点的实时位置；以车辆的实时位置为基础，构建二维空间地图，将相机和超声波雷达采集的感知数据映射到二维空间地图中。

上述技术方案仅通过实时的轮速数据和轮胎的转角数据确定出车辆相对于原点的位置，并基于该位置建立二维空间地图，将相机和超声波雷达采集的环境数据映射至二维空间地图中，其构建的二维空间地图的准确性及环境信息的准确性不够高。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提出了一种自动泊车过程中通过环境扫描修正车辆位姿信息的方法及装置，可以在自动泊车过程中获得精准的环境信息和车辆定位信息。

为实现上述目的，按照本发明的一个方面，提供了一种自动泊车过程中通过环境扫描修正车辆位姿信息的方法，包括：

(1)开始搜索车位前，以当前车辆位姿为原点，构建坐标系，在搜索车位过程中，通过方向盘转角信息、档位信息以及轮速脉冲信息，确定本车定位信息；

例如，坐标系可以本车车头方向为X轴方向，本车左方为Y轴方向，当前位姿信息x＝0，y＝0，θ＝0，(x，y)表示本车横纵坐标值，θ表示航向角，并且航向角变化以逆时针方向为正；

(2)根据实时确定的本车定位信息形成由一条曲线表达的道路中线信息；

(3)获取本车与周围障碍物的距离信息，将距离信息与道路中线信息相结合形成环境信息，将环境信息录入坐标系完成对局部环境信息坐标体系的初次构建，该局部环境信息坐标体系由可通行区域、非通行区域、车位及特征点组成；

(4)在泊车过程中，在车辆反向通过相同路段时，根据本车定位信息、本车与周围障碍物的距离信息形成的实时环境信息，根据特征点对比从而对局部环境信息坐标体系进行修正；

(5)在车辆运行过程中，根据修正后的局部环境信息坐标体系，对车辆位姿信息进行修正。

在一些可选的实施方案中，步骤(2)包括：

在搜索车位过程中，根据本车的具体车型以及方向盘转角信息，确定对应的车辆航向角转角，基于轮速脉冲信息中包含的脉冲距离参数以及脉冲数确定车轮移动距离，根据档位信息、车辆航向角转角及车轮移动距离确定本车定位信息，然后根据实时更新的本车定位信息形成由一条曲线表达的道路中线信息。

在一些可选的实施方案中，步骤(4)包括：

在泊车过程中，在车辆反向通过相同路段时，根据本车定位信息、本车与周围障碍物的距离信息形成的实时环境信息中包括的特征点与搜索车位阶段形成的局部环境信息坐标体系内的特征点做对比，当出现连续的特征点位置同方向偏移，则根据实时环境信息对局部环境信息坐标体系中对应的环境信息进行该方向的修正；若只有个别的特征点对比时表现出偏差，则将该偏差作为可忽略偏差，不对局部环境坐标体系进行修正。

在一些可选的实施方案中，本车定位信息通过以下方式实现：

实时获取车辆左右后轮轮速传感器每个车辆底盘报文周期输出的脉冲差值ΔSum，并实时获取方向盘转角信息，从而确定每个车辆底盘报文周期的车辆航向角变化量Δθ；基于脉冲差值ΔSum、车辆航向角变化量Δθ以及原有车辆航向角θold，计算车辆位置变化量ΔX和ΔY，其中，ΔX＝ls*ΔSum平均*cos(θold+Δθ/2)，ΔY＝ls*ΔSum平均*sin(θold+Δθ/2)，ΔSum平均＝(ΔSum左后+Sum右后)/2，ls表示轮速脉冲信息中包含的脉冲距离参数；

将车辆位置变化量ΔX和ΔY、航向角变化量Δθ与上个周期车辆位姿信息Xold、Yold以及θold累加，根据档位不同进行正向或反向累加，得到本周期的本车定位信息Xnew、Ynew以及θnew，其中，Xnew＝Xold+ΔX，Ynew＝Yold+ΔY，θnew＝θold+Δθ。

在一些可选的实施方案中，步骤(5)包括：

通过本车定位信息与修正后的局部环境信息坐标体系中的特征点坐标信息，确定本车与障碍物的距离信息，将该距离信息与超声波雷达扫描获得的实际距离信息进行比对，若本车定位信息连续在同一方向发生偏移，则获取多个位置的偏移值，对偏移值进行中值滤波，得到平均偏移值，基于平均偏移值对本车定位信息进行修正，并输出修正后的结果作为最终的车辆位姿信息；若本车定位信息偶尔发生不规律的偏移并在预设时间内恢复到正常水平，则不修正本车定位信息，此时本车定位信息将作为最终的车辆位姿信息。

按照本发明的另一方面，提供了一种自动泊车过程中通过环境扫描修正车辆位姿信息的装置，包括：局部坐标系计算模块、车辆运动计算模块、融合处理模块及位姿修正模块；

所述局部坐标系计算模块，用于在开始搜索车位前，以当前车辆位姿为原点，构建坐标系，在搜索车位过程中，通过方向盘转角信息、档位信息以及轮速脉冲信息，确定本车定位信息；

所述车辆运动计算模块，用于根据实时确定的本车定位信息形成由一条曲线表达的道路中线信息；

所述融合模块，用于获取本车与周围障碍物的距离信息，将距离信息与道路中线信息相结合形成环境信息，将环境信息录入坐标系完成对局部环境信息坐标体系的初次构建，该局部环境信息坐标体系由可通行区域、非通行区域、车位及特征点组成；

所述局部坐标系计算模块，还用于在泊车过程中，在车辆反向通过相同路段时，根据本车定位信息、本车与周围障碍物的距离信息形成的实时环境信息，根据特征点对比从而对局部环境信息坐标体系进行修正；

所述位姿修正模块，用于在车辆运行过程中，根据修正后的局部环境信息坐标体系，对车辆位姿信息进行修正。

在一些可选的实施方案中，所述车辆运动计算模块，用于在搜索车位过程中，根据本车的具体车型以及方向盘转角信息，确定对应的车辆航向角转角，基于轮速脉冲信息中包含的脉冲距离参数以及脉冲数确定车轮移动距离，根据档位信息、车辆航向角转角及车轮移动距离确定本车定位信息，然后根据实时更新的本车定位信息形成由一条曲线表达的道路中线信息。

在一些可选的实施方案中，所述局部坐标系计算模块，用于在泊车过程中，在车辆反向通过相同路段时，根据本车定位信息、本车与周围障碍物的距离信息形成的实时环境信息中包括的特征点与搜索车位阶段形成的局部环境信息坐标体系内的特征点做对比，当出现连续的特征点位置同方向偏移，则根据实时环境信息对局部环境信息坐标体系中对应的环境信息进行该方向的修正；若只有个别的特征点对比时表现出偏差，则将该偏差作为可忽略偏差，不对局部环境坐标体系进行修正。

在一些可选的实施方案中，本车定位信息通过以下方式实现：

实时获取车辆左右后轮轮速传感器每个车辆底盘报文周期输出的脉冲差值ΔSum，并实时获取方向盘转角信息，从而确定每个车辆底盘报文周期的车辆航向角变化量Δθ；基于脉冲差值ΔSum、车辆航向角变化量Δθ以及原有车辆航向角θold，计算车辆位置变化量ΔX和ΔY，其中，ΔX＝ls*ΔSum平均*cos(θold+Δθ/2)，ΔY＝ls*ΔSum平均*sin(θold+Δθ/2)，ΔSum平均＝(ΔSum左后+Sum右后)/2，ls表示轮速脉冲信息中包含的脉冲距离参数；

将车辆位置变化量ΔX和ΔY、航向角变化量Δθ与上个周期车辆位姿信息Xold、Yold以及θold累加，根据档位不同进行正向或反向累加，得到本周期的本车定位信息Xnew、Ynew以及θnew，其中，Xnew＝Xold+ΔX，Ynew＝Yold+ΔY，θnew＝θold+Δθ。。

在一些可选的实施方案中，所述位姿修正模块，用于通过本车定位信息与修正后的局部环境信息坐标体系中的特征点坐标信息，确定本车与障碍物的距离信息，将该距离信息与超声波雷达扫描获得的实际距离信息进行比对，若本车定位信息连续在同一方向发生偏移，则获取多个位置的偏移值，对偏移值进行中值滤波，得到平均偏移值，基于平均偏移值对本车定位信息进行修正，并输出修正后的结果作为最终的车辆位姿信息；若本车定位信息偶尔发生不规律的偏移并在预设时间内恢复到正常水平，则不修正本车定位信息，此时本车定位信息将作为最终的车辆位姿信息。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

1.相比于传统的利用超声波雷达提取的距离信息确定环境信息的方法，本发明通过超声波雷达提取的距离信息、轮速传感器提取的轮速脉冲以及EPS提取的方向盘转角，确定局部环境信息，能够更高效、系统的使用了超声波雷达数据。

2.本发明通过在搜索车位过程中，基于距离信息、轮速脉冲信息、方向盘转角信息构建局部环境信息坐标体系，并在自动泊车过程中，通过超声波雷达重复周围环境信息，基于扫描结果修正局部环境信息坐标体系，以获得更精准的局部环境信息。

3.由方向盘转角信息和轮速脉冲信息确定车辆的位姿信息，进而根据局部环境坐标体系修正车辆的位姿信息，以获得更精准的车辆定位信息。

4.本发明通过利用车辆已有的传感器提取相应信息，在提高车辆定位信息和局部环境信息的精确度的同时，节约成本。

附图说明

图1是本发明实施例提供的一种自动泊车过程中通过环境扫描修正车辆位姿信息的方法流程示意图；

图2是本发明实施例提供的一种实际环境示意图；

图3是本发明实施例提供的一种局部环境信息坐标体系示意图；

图4是本发明实施例提供的一种本车定位示意图；

图5是本发明实施例提供的一种硬件连接示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

图1是本发明实施例提供的一种自动泊车过程中通过环境扫描修正车辆位姿信息的方法流程示意图，包括以下步骤：

S1：开始搜索车位前，以当前车辆位姿为原点，构建坐标系，在搜索车位过程中，通过方向盘转角信息、档位信息以及轮速脉冲信息，确定本车定位信息；

S2：根据实时确定的本车定位信息形成由一条曲线表达的道路中线信息；

S3：获取本车与周围障碍物的距离信息，将距离信息与道路中线信息相结合形成环境信息，将环境信息录入坐标系完成对局部环境信息坐标体系的初次构建，该局部环境信息坐标体系由道路(即可通行区域)、非通行区域、车位及特征点组成；

其中，可以通过超声波雷达提取车辆与周围障碍物的距离信息。

S4：在泊车过程中，在车辆反向通过相同路段时，根据本车定位信息、本车与周围障碍物的距离信息形成的实时环境信息，根据特征点对比从而对局部环境信息坐标体系进行修正；

S5：在车辆运行过程中，根据修正后的局部环境信息坐标体系，对车辆位姿信息进行修正。

其中，本发明是基于局部环境静止的前提下，确定环境位置信息和车辆位姿信息。

具体过程如下：

1.初始化阶段；

在开始搜索车位前，以本车位姿(例如，以车辆后轮的中点)为原点，本车当前位置的航向角为X轴方向，将与X轴垂直方向确定为Y轴方向，构建坐标系。这里，由于本车上超声波雷达的放置位置通常与地面存在一定高度；因此，在超声波雷达设定的检测高度位置以下的障碍物，默认是可以通过的，在此高度位置以上，能被雷达扫描到的障碍物均属于非通行区域，可以直接设置二维坐标系，实现对本车位置以及周围局部环境的定位。

2.搜索车位阶段---局部环境信息坐标体系的构建；

在搜索车位过程中，通过电动助力转向系统(Electric Power Steering，EPS)提取方向盘转角信息，根据本车具体车型以及方向盘转角信息，确定本车的航向角转角θ，通过电子选挡模块ESM提取档位信息，通过轮速传感器提取轮速脉冲信息，并基于轮速脉冲信息中包含的脉冲距离参数ls以及脉冲数，确定车轮移动距离，基于上述信息确定本车定位信息。根据实时更新的本车定位信息形成由一条曲线表达的道路中线信息。通过超声波雷达提取本车与周围障碍物的距离信息，获取本车与周围障碍物的距离信息，与道路中线信息相结合形成环境信息，将环境信息录入坐标系完成对局部环境信息坐标体系的初次构建。

如图2及图3所示，图2为实际环境，本车按照本车1到本车3的顺序通过道路；沿途实时获取本车定位信息形成道路中线信息，并基于道路中线和超声波雷达反馈的本车与道路周边环境的距离信息形成局部环境信息坐标体系；图3所示为道路(可通行区域)、非通行区域、车位及特征点组成的局部环境信息坐标体系。

3.泊车阶段---局部环境信息坐标体系的修正；

在自动泊车过程中，本车会反向经过同一路段，控制器会将新录入局部环境信息坐标体系的数据与原有环境信息数据做对比，具体做法是将两者中特征相同或相似的特征点做对比，从而修正局部环境信息坐标体系。

以图2中的平行车位1为例，车辆从本车3的位置移动到本车2的位置，再泊入平行车位1的位置，在这个泊车过程中，车辆运动距离较长；控制器在车辆从本车3位置移动到本车2位置的运行过程中，通过EPS提取的方向盘转角信息，ESM提取的档位信息，轮速传感器提取的轮速脉冲信息，确定本车实时定位信息；并通过本车实时定位信息与超声波雷达扫描的距离信息确定实时环境信息。控制器会将新生成的实时环境信息中包括的特征点(图3中矩形框框出来的特征点)与搜索车位阶段形成的局部环境信息坐标体系内的特征点做对比。当出现连续的特征点位置同方向偏移，控制器会根据实时环境信息对局部环境信息坐标体系中对应的环境信息进行该方向的修正。例如，在本车连续经过3个及以上的已检测特征点时，实时环境信息反映本车位置均向X轴方向偏移，并且偏移值处在同一水平如10cm、12cm、13cm，则控制器将筛除其中不合理值并进行加权后得到修正值10.5cm((10+12+13)/3*0.9该公式仅做举例，均值及加权不作为本发明实施例的唯一性限定)，此时控制器将局部环境坐标体系内的环境信息向X轴方向增加10.5cm；如果只有个别的特征点对比时表现出偏差，控制器将该偏差作为可忽略偏差，不对局部环境坐标体系进行修正。

4.本车定位信息的获取；

在本车的运行过程中(包括搜索车位阶段和泊车阶段)，控制器实时获取车辆左右后轮轮速传感器每个车辆底盘报文周期输出的脉冲差值ΔSum，并实时获取方向盘转角信息，从而确定每个车辆底盘报文周期的车辆航向角变化量Δθ；基于脉冲差值ΔSum、车辆航向角变化量Δθ以及原有车辆航向角θold，计算车辆位置变化量。计算原理如图4所示，具体计算过程如下：

1)获取ΔSum左后、ΔSum右后；

2)根据原有航向角θold以及该周期航向角变化值Δθ计算车辆位置变化值ΔX和ΔY，公式如下：

ΔSum平均＝(ΔSum左后+Sum右后)/2

ΔX＝ls*ΔSum平均*cos(θold+Δθ/2)

ΔY＝ls*ΔSum平均*sin(θold+Δθ/2)

3)将车辆位置变化值ΔX和ΔY、航向角变化值Δθ与上个周期车辆位姿信息Xold、Yold以及θold累加，根据档位不同进行正向或反向累加，比如倒挡(R档)时反向累加，前进挡(D档或M档等)时正向累加，其他档位不累加(N档或P档)得到本周期的本车定位信息Xnew、Ynew以及θnew；

Xnew＝Xold+ΔX

Ynew＝Yold+ΔY

θnew＝θold+Δθ

5.车辆的位姿信息修正；

由于本车定位信息与局部环境信息坐标体系使用的都是初始化阶段构建的坐标系，通过本车定位信息与局部环境信息坐标体系中的特征点坐标信息，确定本车与障碍物的距离信息，将该距离信息与超声波雷达扫描获得的实际距离信息进行比对；如果控制器通过对比发现本车定位信息连续在同一方向发生偏移，控制器通过获取多个位置的偏移值，对偏移值进行中值滤波，得到平均偏移值，基于平均偏移值对本车定位信息进行修正，例如，修正可以是直接加上偏移值，该偏移值应视为实际值与本车定位信息之间的差异值(有单位与方向)，即偏移值＝实际值-定位信息，故理论上实际值＝定位信息+偏移值。并输出修正后的结果作为最终的车辆位姿信息。如果控制器通过对比环境信息发现本车定位信息偶尔发生不规律的偏移并在预设时间内恢复到正常水平，控制器不会修正本车定位信息，此时本车定位信息将作为最终的车辆位姿信息输出，从而得到更精准的车辆位姿信息，其中，预设时间大小可以根据实际需要确定，一般时间较短。

本发明还提供了一种自动泊车过程中通过环境扫描修正车辆位姿信息的装置，包括：局部坐标系计算模块、车辆运动计算模块、融合处理模块及位姿修正模块；

局部坐标系计算模块，用于在开始搜索车位前，以当前车辆位姿为原点，构建坐标系，在搜索车位过程中，通过方向盘转角信息、档位信息以及轮速脉冲信息，确定本车定位信息；

车辆运动计算模块，用于根据实时确定的本车定位信息形成由一条曲线表达的道路中线信息；

融合模块，用于获取本车与周围障碍物的距离信息，将距离信息与道路中线信息相结合形成环境信息，将环境信息录入坐标系完成对局部环境信息坐标体系的初次构建，该局部环境信息坐标体系由可通行区域、非通行区域、车位及特征点组成；

局部坐标系计算模块，还用于在泊车过程中，在车辆反向通过相同路段时，根据本车定位信息、本车与周围障碍物的距离信息形成的实时环境信息，根据特征点对比从而对局部环境信息坐标体系进行修正；

位姿修正模块，用于在车辆运行过程中，根据修正后的局部环境信息坐标体系，对车辆位姿信息进行修正。

其中，各模块的具体实施方式可以参考上述方法实施例的描述。

如图5所示，该装置可以放置于车辆控制器中，同时车辆控制器连接超声波雷达、EPS、ESM和轮速传感器。

通过本发明，在搜索车位前，以本车当前位置作为原点建立坐标系；在搜索车位时，通过轮速传感器提取的轮速脉冲信息、ESM提取的档位信息以及EPS提取的方向盘转角信息，确定本车定位信息，进而确定搜索车位时本车行进的道路中线信息；根据超声波雷达提取的本车与周围障碍物的距离信息和道路中线信息，确定局部环境信息；将局部环境信息录入至坐标系中，并在自动泊车时，通过实时采集的本车定位信息和距离信息，确定实时环境信息；根据实时环境信息与局部环境信息坐标体系进行比对，基于比对结果，对局部环境坐标体系进行修正，以获得更精准的环境信息。在本车运行过程中，通过方向盘转角信息和轮速脉冲信息确定本车的定位信息，并基于局部环境信息坐标体系，确定本车与障碍物的距离信息，将距离信息与超声波雷达获取的实际距离信息进行比对，从而根据比对结果，对本车的位姿信息进行修正，以获得更精准的车辆位姿信息。

需要指出，根据实施的需要，可将本申请中描述的各个步骤/部件拆分为更多步骤/部件，也可将两个或多个步骤/部件或者步骤/部件的部分操作组合成新的步骤/部件，以实现本发明的目的。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种自动泊车过程中通过环境扫描修正车辆位姿信息的方法，其特征在于，包括：

(1)开始搜索车位前，以当前车辆位姿为原点，构建坐标系，在搜索车位过程中，通过方向盘转角信息、档位信息以及轮速脉冲信息，确定本车定位信息；

(2)根据实时确定的本车定位信息形成由一条曲线表达的道路中线信息；

(3)获取本车与周围障碍物的距离信息，将距离信息与道路中线信息相结合形成环境信息，将环境信息录入坐标系完成对局部环境信息坐标体系的初次构建，该局部环境信息坐标体系由可通行区域、非通行区域、车位及特征点组成；

(4)在泊车过程中，在车辆反向通过相同路段时，根据本车定位信息、本车与周围障碍物的距离信息形成的实时环境信息，根据特征点对比从而对局部环境信息坐标体系进行修正；

其中，步骤(4)包括：

在泊车过程中，在车辆反向通过相同路段时，根据本车定位信息、本车与周围障碍物的距离信息形成的实时环境信息中包括的特征点与搜索车位阶段形成的局部环境信息坐标体系内的特征点做对比，当出现连续的特征点位置同方向偏移，则根据实时环境信息对局部环境信息坐标体系中对应的环境信息进行该方向的修正；若只有个别的特征点对比时表现出偏差，则将该偏差作为可忽略偏差，不对局部环境坐标体系进行修正；

(5)在车辆运行过程中，根据修正后的局部环境信息坐标体系，对车辆位姿信息进行修正。

2.根据权利要求1所述的方法，其特征在于，步骤(2)包括：

在搜索车位过程中，根据本车的具体车型以及方向盘转角信息，确定对应的车辆航向角转角，基于轮速脉冲信息中包含的脉冲距离参数以及脉冲数确定车轮移动距离，根据档位信息、车辆航向角转角及车轮移动距离确定本车定位信息，然后根据实时更新的本车定位信息形成由一条曲线表达的道路中线信息。

3.根据权利要求1或2所述的方法，其特征在于，本车定位信息通过以下方式实现：

实时获取车辆左右后轮轮速传感器每个车辆底盘报文周期输出的脉冲差值ΔSum，并实时获取方向盘转角信息，从而确定每个车辆底盘报文周期的车辆航向角变化量Δθ；基于脉冲差值ΔSum、车辆航向角变化量Δθ以及原有车辆航向角θold，计算车辆位置变化量ΔX和ΔY，其中，ΔX＝ls*ΔSum平均*cos(θold+Δθ/2)，ΔY＝ls*ΔSum平均*sin(θold+Δθ/2)，ΔSum平均＝(ΔSum左后+ΔSum右后)/2，ls表示轮速脉冲信息中包含的脉冲距离参数；

将车辆位置变化量ΔX和ΔY、航向角变化量Δθ与上个周期车辆位姿信息Xold、Yold以及θold累加，根据档位不同进行正向或反向累加，得到本周期的本车定位信息Xnew、Ynew以及θnew，其中，Xnew＝Xold+ΔX，Ynew＝Yold+ΔY，θnew＝θold+Δθ。

4.根据权利要求3所述的方法，其特征在于，步骤(5)包括：

通过本车定位信息与修正后的局部环境信息坐标体系中的特征点坐标信息，确定本车与障碍物的距离信息，将该距离信息与超声波雷达扫描获得的实际距离信息进行比对，若本车定位信息连续在同一方向发生偏移，则获取多个位置的偏移值，对偏移值进行中值滤波，得到平均偏移值，基于平均偏移值对本车定位信息进行修正，并输出修正后的结果作为最终的车辆位姿信息；若本车定位信息偶尔发生不规律的偏移并在预设时间内恢复到正常水平，则不修正本车定位信息，此时本车定位信息将作为最终的车辆位姿信息。

5.一种自动泊车过程中通过环境扫描修正车辆位姿信息的装置，其特征在于，包括：局部坐标系计算模块、车辆运动计算模块、融合处理模块及位姿修正模块；

所述局部坐标系计算模块，用于在开始搜索车位前，以当前车辆位姿为原点，构建坐标系，在搜索车位过程中，通过方向盘转角信息、档位信息以及轮速脉冲信息，确定本车定位信息；

所述车辆运动计算模块，用于根据实时确定的本车定位信息形成由一条曲线表达的道路中线信息；

所述融合处理模块，用于获取本车与周围障碍物的距离信息，将距离信息与道路中线信息相结合形成环境信息，将环境信息录入坐标系完成对局部环境信息坐标体系的初次构建，该局部环境信息坐标体系由可通行区域、非通行区域、车位及特征点组成；

所述局部坐标系计算模块，还用于在泊车过程中，在车辆反向通过相同路段时，根据本车定位信息、本车与周围障碍物的距离信息形成的实时环境信息中包括的特征点与搜索车位阶段形成的局部环境信息坐标体系内的特征点做对比，当出现连续的特征点位置同方向偏移，则根据实时环境信息对局部环境信息坐标体系中对应的环境信息进行该方向的修正；若只有个别的特征点对比时表现出偏差，则将该偏差作为可忽略偏差，不对局部环境坐标体系进行修正；

所述位姿修正模块，用于在车辆运行过程中，根据修正后的局部环境信息坐标体系，对车辆位姿信息进行修正。

6.根据权利要求5所述的装置，其特征在于，所述车辆运动计算模块，用于在搜索车位过程中，根据本车的具体车型以及方向盘转角信息，确定对应的车辆航向角转角，基于轮速脉冲信息中包含的脉冲距离参数以及脉冲数确定车轮移动距离，根据档位信息、车辆航向角转角及车轮移动距离确定本车定位信息，然后根据实时更新的本车定位信息形成由一条曲线表达的道路中线信息。

7.根据权利要求5或6所述的装置，其特征在于，本车定位信息通过以下方式实现：

实时获取车辆左右后轮轮速传感器每个车辆底盘报文周期输出的脉冲差值ΔSum，并实时获取方向盘转角信息，从而确定每个车辆底盘报文周期的车辆航向角变化量Δθ；基于脉冲差值ΔSum、车辆航向角变化量Δθ以及原有车辆航向角θold，计算车辆位置变化量ΔX和ΔY，其中，ΔX＝ls*ΔSum平均*cos(θold+Δθ/2)，ΔY＝ls*ΔSum平均*sin(θold+Δθ/2)，ΔSum平均＝(ΔSum左后+ΔSum右后)/2，ls表示轮速脉冲信息中包含的脉冲距离参数；

将车辆位置变化量ΔX和ΔY、航向角变化量Δθ与上个周期车辆位姿信息Xold、Yold以及θold累加，根据档位不同进行正向或反向累加，得到本周期的本车定位信息Xnew、Ynew以及θnew，其中，Xnew＝Xold+ΔX，Ynew＝Yold+ΔY，θnew＝θold+Δθ。

8.根据权利要求7所述的装置，其特征在于，所述位姿修正模块，用于通过本车定位信息与修正后的局部环境信息坐标体系中的特征点坐标信息，确定本车与障碍物的距离信息，将该距离信息与超声波雷达扫描获得的实际距离信息进行比对，若本车定位信息连续在同一方向发生偏移，则获取多个位置的偏移值，对偏移值进行中值滤波，得到平均偏移值，基于平均偏移值对本车定位信息进行修正，并输出修正后的结果作为最终的车辆位姿信息；若本车定位信息偶尔发生不规律的偏移并在预设时间内恢复到正常水平，则不修正本车定位信息，此时本车定位信息将作为最终的车辆位姿信息。

Images