CN103270543B - 驾驶辅助装置 - Google Patents

Abstract

驾驶辅助装置中，在无法接收GPS的区间利用立体摄像机或LRF等测量距离的距离传感器修正加速度传感器的零位误差时，为了以良好的精度推定零位误差，设置有：利用激光测距仪或立体摄像机等距离传感器测量与存在于车辆周边的物体的表面的多个点之间的距离，确定距离传感器的测量分辨率在规定精度之上的测量范围的最大测量范围确定单元；根据最大测量范围内测量的点求取特征点的特征点检测单元；从根据距离的变化逆推车辆行为的车辆平移·旋转角计算单元获得车辆行为，并根据该车辆行为和加速度传感器的测量值推定加速度传感器的参数的传感器参数推定单元。

Description

驾驶辅助装置

技术领域

本发明涉及对车辆进行控制的系统，尤其涉及用于辅助车辆驾驶的装置。

背景技术

用于车辆的位置推定、被称为推测导航法（dead reckoningnavigation，惯性导航法）的方法中，根据陀螺仪或加速度传感器等惯性传感器的测量值计算出车辆的前进方向和速度，通过对该车辆的前进方向和速度依次累加而推定车辆的位置。在该推测导航法中，由于位置的推定误差会逐渐累积，因此陀螺仪或加速度传感器的测量精度变得非常重要，需要进行该惯性传感器的校准和推定位置的修正。

现有技术中，存在利用GPS或直接测量车辆移动量的传感器的测量值，来对加速度传感器的零位误差（bias，零偏）、陀螺仪的零位误差和标度因数（scale factor）进行校准的方法（专利文献1、专利文献2、专利文献6）。然而，在室内或常发生多路径（multi-path）效应的地区等无法获取GPS信号或GPS的定位精度较差的情况下，无法使用基于GPS进行传感器校准的方法。在这种状况下，作为GPS的替代，有利用激光测距仪（Laser Range Finder，LRF）或立体摄像机等用于测量与周边物体间距离的距离传感器，来进行陀螺仪的修正的方法。专利文献3中记载了使用LRF或立体摄像机来推定车辆行为（即车辆运动状态）的方法，专利文献4中记载了利用摄像机提取白线的特征点来修正陀螺仪的方法。此外，专利文献5中记载了利用立体摄像机根据当前位置周边的物体来直接修正本车位置的技术。

通过使用这些技术，利用LRF或立体摄像机等距离传感器，能够进行位置的修正或陀螺仪等传感器的零位误差、标度因数的修正。

现有技术文献

专利文献

专利文献1：日本特开2003-75172号公报

专利文献2：日本特开2008-175721号公报

专利文献3：日本特开2006-160116号公报

专利文献4：日本特开2008-82932号公报

专利文献5：日本特开2000-97714号公报

专利文献6：日本特开2009-281799号公报

发明内容

发明要解决的技术问题

然而，如果利用摄像机或LRF等距离传感器通过与陀螺仪的修正相同的方法来修正加速度传感器，由于车辆加速度或车辆姿态的倾斜等会影响加速度传感器的值，并且车体的俯仰角（pitch angle）或侧倾角（roll angle，滚转角）等姿态变化会较大地影响摄像机或LRF的测量精度，因此无法以良好精度修正加速度传感器的零位误差。

解决问题的技术手段

本发明利用距离传感器、车辆平移·旋转角计算单元和传感器参数推定单元，来推定加速度传感器的零位误差，其中，上述距离传感器用于测量与车辆周边存在的物体之间的距离，上述车辆平移·旋转角计算单元根据由距离传感器测量到的与物体间的距离的变化而求取车辆行为，上述传感器参数推定单元根据从车辆平移·旋转角计算单元获得的本车行为和加速度传感器的测量值来推定加速度传感器的参数。

发明效果

通过本发明，能够根据距离传感器的值来高精度地推定加速度传感器的零位误差（bias）。

本发明的其它目的、特征和优点可通过下述关于附图的本发明实施例的记述得以明了。

附图说明

图1是表示具有本发明的驾驶辅助装置的车辆控制系统的结构的图。

图2是表示时序距离信息的数据结构的图。

图3是用于确定最大测量范围的处理流程图。

图4是表示距离传感器参数的数据结构的图。

图5是说明距离传感器参数的图。

图6是说明距离传感器参数的图。

图7是说明最大测量范围的缩小（限定）的图。

图8是说明最大测量范围的缩小（限定）的图。

图9是说明基于道路形状缩小（限定）最大测量范围的图。

图10是说明基于距离传感器进行的测量的图。

图11是计算平移距离/旋转角的处理流程图。

图12是进行特征点序列的对应的处理流程图。

图13是说明平移距离/旋转角的计算的图。

具体实施方式

以下参考附图对用于实施本发明的方式进行说明。

图1表示车辆控制系统的概要，该车辆控制系统包括采用了本发明的车辆用驾驶辅助装置。从用于测量自身位置到对象物体间的距离的LRF或立体摄像机等距离传感器102，对驾驶辅助装置101输入时序距离信息。时序距离信息的数据结构如图2所示。来自距离传感器102的时序距离信息包括测量距离的点（测量对象点）的个数即测量点数202，和测量点数份的测量对象点的位置203。当其被输入时，驾驶辅助装置101内的内部时钟的时刻，被作为表示测量距离的时刻的测量时刻201记录。测量对象点的位置203由距离传感器102内确定的坐标系表示，在以下的说明中以三维坐标系（x轴位置、y轴位置、z轴位置）表示。该测量对象点的位置的表示也能够通过（与测量对象点间的距离，测量对象点的方向）这样的极坐标系表示，本质上表示相同的内容。

以下说明驾驶辅助装置101内的处理。

驾驶辅助装置101中，通过最大测量范围确定单元110，设定在距离传感器102所测量的范围内作为特征点检测对象的最大测量范围，在该范围中从距离传感器102所测量的测量点中检测出特征点。利用图3对最大测量范围确定单元110确定最大测量范围的确定处理进行说明。该最大测量范围确定单元110在每次时序距离信息的测量时刻201被更新时启动。

首先，在步骤S301中，驾驶辅助装置101从距离传感器参数存储单元103获取距离传感器参数的数据。存储在该距离传感器参数存储单元103中的距离传感器参数的数据结构如图4所示。此外，主要参数的概念图如图5、图6所示。在距离传感器参数存储单元103中，存储了表示距离传感器参数是否有变更的、用于表示有无变更的参数变更标志401，以及大致划分为安装角、安装位置、测量精度分辨率（resolution）、测量范围的各个参数。作为距离传感器102的安装角，保存以车体水平的情况为基准的侧倾角402和俯仰角403，以及相对于车轴的偏航角（yaw angle）404。此外，作为距离传感器的安装位置，保存相对于车体轴的横向偏移量405和距离地面的高度406。进一步地，关于所使用的距离传感器的测量精度分辨率，保存了前后方向407、横向方向408、高度方向409的值。同样地，关于所使用的距离传感器的测量范围，保存了前后方向410、横向方向411、高度方向412的值。距离传感器参数的初始值全部为事先保存的根据距离传感器102的各规格值而求出的值，以及测量到的安装位置、安装角的值。

接着，在步骤S302中，根据从距离传感器参数存储单元103获得的距离传感器参数的参数变更标志401的值，判断参数值是否发生了变更。在参数值发生变更的情况下，由于根据距离传感器102相对于车辆的安装角和距离传感器的规格值而得到的初始最大测量范围也会发生变化，因此为了重新计算初始最大测量范围，前进到步骤S303。在参数值未变更的情况下，由于无需重新计算初始最大测量范围，前进到步骤S305。

在步骤S303中，根据步骤S301中获得的距离传感器参数中的安装角、安装位置和测量范围值来计算作为对象的距离传感器102的可测量距离的范围。可测量范围可由以下的式1给出。

[式1]

$\left\{\begin{array}{c}d=\frac{\mathrm{Hv}}{\tan \mathrm{\φ}}\·\·\·(\mathrm{\φ}>0)\\ d=D\·\·\·(\mathrm{\φ}\≤0)\end{array}\right.$

…(式1)

$w=\frac{W\·d}{D}$

$h=\frac{H\·d}{D}$

在此，d为可测量范围（前后方向），w为可测量范围（横向方向），h为可测量范围（高度方向），Hv为安装位置（距离地面的高度），φ为安装角（俯仰角），D为测量范围（前后方向），W为测量范围（横向方向），H为测量范围（高度方向）。

接着，在步骤S304中，根据步骤S301中获得的距离传感器参数的测量精度分辨率，计算出可测量范围中能够保持测量精度的范围。该可保持测量精度的范围如图7、图8所示，是位于距离传感器的测量范围的内侧，且前后方向、横向方向、高度方向的测量精度分辨率为测量精度的阈值之内的区域，将该区域作为初始最大测量范围存储。测量精度分辨率表示与车辆离开一定距离的地点上的测量点的间隔，在前后、横向、高度方向上分别具有值。为了保证测量精度，必须满足（测量点的间隔）≤（作为测量对象的物体的大小）的条件，因此，最大测量范围可如下表示。

[式2]

$\left\{\begin{array}{c}{d}_{\max }=\frac{L\·O}{\mathrm{rd}}\·\·\·(d>\frac{L\·O}{\mathrm{rd}})\\ d_{\max }=d\·\·\·(d\≤\frac{L\·O}{\mathrm{rd}})\end{array}\right.$

…(式2)

$\left\{\begin{array}{c}{w}_{\max }=\frac{L\·O}{\mathrm{rw}}\·\·\·(w>\frac{L\·O}{\mathrm{rw}})\\ w_{\max }=w\·\·\·(w\≤\frac{L\·O}{\mathrm{rw}})\end{array}\right.$

$\left\{\begin{array}{c}{h}_{\max }=\frac{L\·O}{\mathrm{rh}}\·\·\·(h>\frac{L\·O}{\mathrm{rh}})\\ h_{\max }=h\·\·\·(h\≤\frac{L\·O}{\mathrm{rh}})\end{array}\right.$

在此，d_max、w_max、h_max分别表示最大测量范围（前后方向）、最大测量范围（横向方向）、最大测量范围（高度方向），rw、rh、rd分别表示离开距离L时的分辨率（横向方向）、分辨率（高度方向）、分辨率（前后方向），O表示测量对象物体的最小尺寸。

这样求出的最大测量范围作为初始最大测量范围存储。

接着，在步骤S305中，从地图数据库104获得本车行驶的周边地图数据，在步骤S306中，判断包含本车行驶的道路的地图数据中是否存在道路形状数据。在此，道路形状数据为曲率数据、坡度数据等。在存在道路形状数据的情况下，前进到步骤S307，在没有的情况下，将初始最大测量范围作为最大测量范围存储并结束处理。

接着，在步骤S307中，判断步骤S304中计算的初始最大测量范围中是否存在道路形状数据的变化值为阈值以上的点。道路形状数据的变化值为本车周边某点的道路形状数据的值与其前方（即远方）的点的道路形状数据的值之差。在初始最大测量范围内的道路形状数据的变化值为阈值以下的情况下，将初始最大测量范围作为最大测量范围存储并结束处理。此外，在道路形状数据的变化值为阈值以上的情况下，将阈值以上的点作为道路形状的拐点，存储到达该拐点所需的距离（横向、高度、前后），进入步骤S308。

步骤S308中，将测量范围缩小（refine，限定）到道路形状的拐点之前，作为最大测量范围存储。当距离传感器测量的对象物体位于道路形状发生变化的场所时，在道路形状的拐点前后其外观很可能存在变化，经常导致误检测或误差。因此，对从本车来看位于道路形状的拐点前方（即远方）的物体不进行观测。如图9所示，将通过道路形状的拐点而缩小（限定）的测量范围作为最大测量范围存储并结束处理。

在特征点检测单元111中，检测所设定的最大测量范围内的特征点。利用从距离传感器102输入的时序距离信息，根据距离传感器102在时刻t测量到的与车辆周边的物体表面的各测量点的相对位置关系，检测作为物体特征点的测量点序列（特征点序列）。图10表示检测特征点序列的概念图。在某时刻t，将测量所得的多个测量点的位置分别投影到距离传感器内的坐标系的xy平面、yz平面、zx平面。在所投影的各平面上，通过以多个点作为一个单位的最小二乘法来计算出该点序列构成的矢量。在此，作为矢量计算对象的多个点可为所投影的各平面上规定数目的邻近点，或者为规定距离内的点的集合。接着，在相邻矢量所成的角度为一定值以上的情况下，识别为特征点部位，将构成两矢量的点序列全部作为特征点序列，与测量点的测量时刻一起存储到特征点初始位置存储单元112中。在每次距离传感器102测量与对象之间的距离时，对最大测量范围内的所有测量点进行该处理。

接着，在车辆平移·旋转角计算单元113中，计算车辆的平移变化与旋转变化。车辆平移·旋转角计算单元113通过如图11所示的流程的处理来计算车辆的平移和旋转角。首先，在步骤S1101中，判断特征点初始位置存储单元112中是否存在一定时间之前的特征点序列。在不存在的情况下，不计算平移·旋转角，结束处理。

在存在一定时间之前的特征点序列的情况下，前进到步骤S1102。将该一定时间之前的特征点序列与当前的特征点序列相比较，使彼此的特征点序列相对应。特征点序列的对应方式是，在一定时间之前的特征点序列与当前的特征点序列之间计算各特征点序列间的相关系数ρ，相关度最高的特征点序列互为对应特征点序列。相关系数ρ由以下的式3给出。

[式3]

$\mathrm{\ρ}=\frac{E(x-E\left(x\right))(y-E\left(y\right))}{\sqrt{E{(x-E\left(x\right))}^2\·E{(y-E\left(y\right))}^2}}$ …(式3)

在此，x为当前的特征点序列（矢量），y为一定时间之前的特征点序列（矢量），E（）表示期望值（expectation value）。

进行该特征点序列的对应的步骤S1102的处理流程如图12所示。首先在步骤S1201中，求出一定时间之前的特征点序列与当前特征点序列之间特征点序列彼此的相关系数ρ。然后在步骤S1202中，判断是否已按所有的特征点序列的组合计算了相关系数。如果仍有未计算相关系数的特征点序列的组合存在的情况下，反复进行返回步骤S1201对未计算相关系数的特征点序列的组合求取相关系数的处理。

接着在步骤S1203中，从尚未进行对应的特征点序列的组合中选择相关系数最高的特征点序列的组合。然后在步骤S1204中，判断进行了对应后的相关系数最高的特征点序列的相关系数的值是否比一定阈值大。如果在阈值以下，则认为再没有对应的特征点序列的组合，结束处理。如果在阈值以上，则使步骤S1205中选择的特征点序列的组合相对应（彼此对应）。然后，在步骤S1206中，确认当前特征点序列和一定时间之前的特征点序列的至少其中一方的全部特征点序列已经完成对应。如果当前特征点序列和一定时间之前的特征点序列的至少其中一方的全部特征点序列已经完成对应，则认为全部的特征点序列已经完成对应，结束处理。如果仍有未完成的特征点序列，则回到步骤S1203，选择相关系数接下来最大的特征点序列的组合，重复处理。

回到图11的流程，经过上述特征点序列的对应处理，对所确定的特征点序列的组合分别求取平移距离和旋转角（S1103）。

利用图13对求取平移距离和旋转角的处理进行说明。首先，如图13（a）所示，对于已对应的当前特征点序列和一段时间之前的特征点序列，求取使构成当前特征点序列的测量点分别投影到平面上后的投影点的重心点，并同样求取一段时间之前的特征点序列的重心点，存储使这两个重心点一致时重心点的移动量。接着，如图13（b）所示，以该重心为中心使当前的特征点序列旋转，通过最小二乘法计算与一定时间之前的特征点序列形状最接近时的旋转角，存储该旋转角。接着，如图13（c）所示，通过最小二乘法，求取使旋转了上述旋转角后的当前特征点序列与一定时间之前的特征点序列的各测量点的投影点之间的差为最小的平行移动量，将该平行移动量与上述重心点的移动量相加，求得平移距离。

然后，在步骤S1104中，判断是否对所有的特征点序列的组合进行了上述求取平移距离和旋转角的处理，在所有的组合都完成了处理的情况下，前进到步骤S1105。在操作未完成的情况下，反复进行返回步骤S1103对未处理的组合求取平移距离和旋转角的处理。

接着，在步骤S1105中，对于按各特征点序列的组合求出的平移距离、旋转角，求取各自的平均值，在步骤S1106中将该平均值设定为车辆的平移距离、旋转角。

驾驶辅助装置101定期地接收来自陀螺仪105和加速度传感器106的输出，由传感器累加单元114将陀螺仪105的输出值累加（积分）而转换成旋转角数据，并将加速度传感器106的输出值累加（积分）而转换成平移距离数据。此外，陀螺仪105、加速度传感器106的输出周期并不一定需要与距离传感器102的输出周期一致。

由于加速度传感器106的输出值也会受到重力加速度的影响，因此也可以例如利用与专利文献1相同的技术等计算车辆的姿态，除去车辆姿态带来的重力加速度的影响，转换成速度数据，通过将速度数据累加（积分）而转换成平移距离数据。

根据传感器输出值计算出旋转角数据、平移距离数据后，接着由传感器参数推定单元115推定陀螺仪105和加速度传感器106的参数。对利用距离传感器102的输出数据由车辆平移·旋转角计算单元113计算出的车辆平移距离和旋转角，与由传感器累加单元114计算出的平移距离数据和旋转角数据进行比较，能够计算出速度误差、角度误差。将计算出的速度误差和角度误差除以当前时刻与进行平移距离数据和旋转角数据的比较的时刻的差，即求出加速度和角速度的误差。该误差为单位时间的加速度误差和角速度误差。由于加速度传感器的噪声和加速度传感器的零位误差会影响加速度误差，因此可假定加速度传感器的噪声为白噪声，利用卡尔曼滤波器推定加速度传感器106的零位误差。

这样，利用由传感器参数推定单元115所推定的零位误差值，修正由传感器累加单元114求得的距离位移、角度位移，由位置推定单元116推定位置。位置推定使用卡尔曼滤波器，同时求取位置的误差协方差。推定出的位置和误差协方差从驾驶辅助装置101输出到车辆控制装置107。

当车辆控制装置107检测到从驾驶辅助装置101输出的误差协方差变大、本车位置的分布范围（存在范围）变大时，判断为本车位置的似然性（likelihood，确定度/信任度）降低，进行控制以将车辆速度减速到合适的速度。

上述发明针对实施例来实施，但本发明并不限定于此，本领域技术人员可知在本发明的主旨及其技术方案的范围内可进行各种变更和修正。

附图标记说明：

101：驾驶辅助装置

102：距离传感器

106：加速度传感器

107：车辆控制装置

110：最大测量范围确定单元

115：传感器参数推定单元

116：位置推定单元

Claims (3)

1.一种驾驶辅助装置，包括测量车辆的姿态或移动速度的变化的传感器，和测量与车辆周边物体的距离的距离测量单元，将确定的车辆位置输出，其特征在于，包括：

存储所述距离测量单元的特性信息的存储单元；

存储地形信息的地图数据库；

基于所述存储单元的所述特性信息和已测得的当前位置周边的地形信息，确定所述距离测量单元的测量数据处理对象范围的范围确定单元；

根据与所述范围确定单元所确定的测量数据处理对象范围内的物体之间的距离结果而检测特征点的特征点检测单元；

使已获得的特征点与当前获得的特征点对应，根据特征点的位置与方向的变化求取车辆行为的车辆平移·旋转角计算单元；

根据所述传感器的测量值和所述车辆行为来推定所述传感器的参数的传感器参数推定单元；和

利用所推定的传感器参数，计算修正后的位置的位置推定单元，

所述距离测量单元的特性信息至少包括该距离测量单元安装到车体上的安装角度，

所述范围确定单元，基于所述特性信息将所述距离测量单元的测量精度比规定精度更佳的范围作为测量数据处理对象范围，

所述范围确定单元，基于从所述地图数据库获取的当前位置周边的地形信息，将包括坡度变化在内的路面形状变化小于规定值的范围作为测量数据处理对象范围。

2.如权利要求1所述的驾驶辅助装置，其特征在于：

基于所述位置推定单元推定的车辆位置的似然性，进行减速控制。

3.一种驾驶辅助方法，设置测量车辆的姿态或移动速度的变化的传感器，和测量与车辆周边物体的距离的距离测量单元，将确定的车辆位置输出，其特征在于，包括：

范围确定处理，基于存储单元中存储的所述距离测量单元的特性信息和地图数据库中存储的已测得的当前位置周边的地形信息，确定所述距离测量单元的测量数据处理对象范围；

特征点检测处理，根据与该范围确定处理所确定的测量数据处理对象范围内的物体之间的距离结果而检测特征点；

车辆平移·旋转角计算处理，使已获得的特征点与当前获得的特征点对应，根据特征点的位置与方向的变化求取车辆行为；

传感器参数推定处理，根据所述传感器的测量值和所述车辆行为来推定所述传感器的参数；和

位置推定处理，利用所推定的传感器参数，计算修正后的位置，

所述距离测量单元的特性信息至少包括该距离测量单元安装到车体上的安装角度，

所述范围确定处理，基于所述特性信息将所述距离测量单元的测量精度比规定精度更佳的范围作为测量数据处理对象范围，

所述范围确定方法中，基于从所述地图数据库获取的当前位置周边的地形信息，将包括坡度变化在内的路面形状变化小于规定值的范围作为测量数据处理对象范围。