CN109405837B - 物体定位方法、应用和车辆 - Google Patents

Abstract

本发明提供了一种物体定位方法、应用和车辆。所述物体定位方法包括下列步骤：在所述物体之外任取一固定点作为原点建立所述物体的直角坐标系XoY，其中，所述直角坐标系的X轴和Y轴所在的平面为水平面，在所述物体上任取一点作为参考点、并任取一方向作为参考方向，将所述参考点在所述直角坐标系上的x坐标、y坐标、所述参考方向与在所述平面中任取的一固定直线所围成的航向角θ、所述物体在单位时间内所走过的距离s、所述物体的转弯曲率c作为状态向量，其中，所述参考点在一时刻k上的x坐标x_k、y坐标y_k通过在上一时刻k‑1的x坐标x_k‑1、y坐标y_k‑1、航向角θ_k‑1、距离s_k‑1、转弯曲率c_k‑1来得到。

Description

物体定位方法、应用和车辆

技术领域

本发明涉及定位领域，特别是基于扩展卡尔曼滤波的车辆低速局部定位算法，具体而言，涉及一种物体定位方法、其在泊车过程中的应用以及相应的车辆。

背景技术

在定位领域中，例如CN106610294A公开了一种定位方法及装置。其中，该定位方法用于车辆室内定位，该定位方法包括：获取某时刻车辆的地理位置、行驶速度和角速度；基于该时刻车辆的地理位置、行驶速度和角速度进行航位推算，得到车辆在当前时刻的推算地理位置。可以使用卡尔曼滤波器来实现对推算地理位置的修正，以提高车辆的地理位置的准确性。具体地，基于卡尔曼滤波器的误差补偿对推算地理位置进行修正：建立状态方程，并建立量测方程。在量测方程修正推算地理位置时，将地图匹配定位得到的地图地理位置的经纬度和航向信息与航迹推算得到的推算地理位置的经纬度和航向信息作差，得到量测方程的观测值，基于该观测值对推算地理位置进行修正。该定位方法没有涉及到航向角或者说方向盘转角，而主要采用了位置和速度作为参量；另一方面没有涉及扩展卡尔曼滤波，即没有涉及将非线性系统转化为线性系统所需的雅克比矩阵。

又如在辅助驾驶系统中，自动泊车技术是重要的组成部分。如CN103453913A公开了一种室内停车场精确的组合定位方法，由DR(Dead Reckoning，航位推算)模块和室内停车场电子地图匹配模块构成，DR模块使用车载的传感器，包括加速计、里程计、陀螺仪，坡度检测仪等，进行车辆位置和状态的推断。其采用了两个卡尔曼滤波器，具体而言，将航位推算得到的估计位置和实时车辆速度、位置信息输入卡尔曼滤波器，再次得到车辆的估计位置输入到地图匹配器中进行匹配，在地图匹配器和卡尔曼滤波器之间的增益回路中循环定位过程，从而卡尔曼滤波器和地图匹配器构成了一个反馈回路。由此其卡尔曼滤波器的输入变量包括了地图匹配位置、车辆速度等变量，这是由于其定位导航的目的所决定的。此外，其没有涉及扩展卡尔曼滤波，即没有涉及将非线性系统转化为线性系统所需的雅克比矩阵。

在自动泊车系统中，基于相对低成本的传感器(轮速传感器和方向盘转角传感器)的定位算法对于泊车功能的实现有着至关重要的作用，基于轮速传感器的定位方法受地面条件影响较大，在凹凸不平的地面定位效果会受到挑战。

发明内容

本发明的目的在于优化物体定位效果，特别是利用扩展卡尔曼滤波算法对泊车过程中本车相对车位坐标系的局部位姿进行优化的状态估计方法来达到很好的定位效果。

此外，本发明还旨在解决或者缓解现有技术中存在的其它技术问题。

本发明通过提供一种物体定位方法、其在泊车过程中的应用以及相应的车辆来解决上述问题，具体而言，根据本发明的一方面，提供了：

一种物体定位方法，其中，所述物体定位方法包括下列步骤：

在所述物体之外任取一固定点作为原点建立所述物体的直角坐标系XoY，其中，所述直角坐标系的X轴和Y轴所在的平面为水平面，

在所述物体上任取一点作为参考点、并任取一方向作为参考方向，

将所述参考点在所述直角坐标系上的x坐标、y坐标、所述参考方向与在所述平面中任取的一固定直线所围成的航向角θ、所述物体在单位时间内所走过的距离s、所述物体的转弯曲率c作为状态向量，

其中，所述参考点在一时刻k上的x坐标x_k、y坐标y_k通过在上一时刻k-1的x坐标x_k-1、y坐标y_k-1、航向角θ_k-1、距离s_k-1、转弯曲率c_k-1来得到。

可选地，根据本发明的一种实施方式，所述状态向量通过扩展卡尔曼滤波来进行更新。

可选地，根据本发明的一种实施方式，所述物体为车辆。

可选地，根据本发明的一种实施方式，所述参考点为所述车辆的后轴中心。

可选地，根据本发明的一种实施方式，所述航向角θ为所述车辆的前进方向与所述直角坐标系的X轴所包夹的角。

可选地，根据本发明的一种实施方式，所述距离s通过所述车辆的轮速传感器算出。

可选地，根据本发明的一种实施方式，所述转弯曲率c通过所述车辆的方向盘转角传感器算出。

根据本发明的另一方面，本发明提供了一种根据本发明的物体定位方法在泊车过程中的应用。

可选地，根据本发明的一种实施方式，所述原点为泊车位边界的一点。

根据本发明的再一方面，本发明提供了一种车辆，其中，所述车辆具有用于执行根据本发明的物体定位方法的装置，并且/或者所述车辆通过根据本发明的应用进行泊车。

所提供的物体定位方法、其在泊车过程中的应用以及相应的车辆的有益之处包括：优化了定位效果，特别是利用扩展卡尔曼滤波对全自动泊车场景下的车轮轮速里程计定位优化，提高了定位的精度和对不同地面环境的适应性。

附图说明

参考附图，本发明的上述以及其它的特征将变得显而易见，其中，

图1和图2分别示出了根据本发明的物体定位方法在泊车应用中的一种实施方式的示意图。

具体实施方式

容易理解，根据本发明的技术方案，在不变更本发明实质精神下，本领域的一般技术人员可以提出可相互替换的多种结构方式以及实现方式。因此，以下具体实施方式以及附图仅是对本发明的技术方案的示例性说明，而不应当视为本发明的全部或者视为对本发明技术方案的限定或限制。

在本说明书中提到或者可能提到的上、下、左、右、前、后、正面、背面、顶部、底部等方位用语是相对于各附图中所示的构造进行定义的，它们是相对的概念，因此有可能会根据其所处不同位置、不同使用状态而进行相应地变化。所以，也不应当将这些或者其他的方位用语解释为限制性用语。此外，术语“第一”、“第二”、“第三”等或类似表述仅用于描述与区分目的，而不能理解为指示或暗示相应的构件的相对重要性。

根据本发明的物体定位方法包括下列步骤：

在所述物体之外任取一固定点作为原点建立所述物体的直角坐标系XoY，其中，所述直角坐标系的X轴和Y轴所在的平面为水平面，

在所述物体上任取一点作为参考点、并任取一方向作为参考方向，

将所述参考点在所述直角坐标系上的x坐标、y坐标、所述参考方向与在所述平面中任取的一固定直线所围成的航向角θ、所述物体在单位时间内所走过的距离s、所述物体的转弯曲率c作为状态向量，

其中，所述参考点在一时刻k上的x坐标x_k、y坐标y_k通过在上一时刻k-1的x坐标x_k-1、y坐标y_k-1、航向角θ_k-1、距离s_k-1、转弯曲率c_k-1来得到。

应当理解，所述固定点表示固定不动的点，以便作为原点进行参照，由此，所述直角坐标系也是固定的坐标系。所述直角坐标系的X轴和Y轴所在的平面为水平面，由此例如在本方法应用于车辆(即所述物体为车辆)的情况下，所述平面与车辆移动时所述车辆任一点所经过的路段构成的平面相重合或平行。在所述物体上任取一点作为参考点，由此所述参考点随着所述物体的运动而相应运动，类似地，任取一方向作为参考方向，由此所述参考方向随着所述物体方向的变化(即姿态的变化)而变化。同样，所述固定直线为在位置方面固定不动的直线，由此所述航向角θ的变化只取决于所述物体方向的变化(即姿态的变化)而变化。所述单位时间是指一定的时间，其表示以某一个时间段作为一个单位，在本例中具体例如为k-1时刻到k时刻，等。所述转弯曲率是转弯半径的倒数，其单位是m^(-1)，其中，所述转弯半径是指汽车行驶过程中，由转向中心到前外转向轮与地面接触点的距离。其中，曲率表明曲线偏离直线的程度。数学上表明曲线在某一点的弯曲程度的数值。将状态变量在某一时刻的值，称为系统在该时刻的状态。将状态变量在0时刻的值称为系统的初始状态或起始状态。而状态向量指不仅具有大小而且具有方向的状态变量。

由此，根据上述技术方案，所述参考点在某时刻上的位置能够得以计算出、例如能够先给定所述参考点和航向角的初始状态。示例性地，参考图1，其示出了根据本发明的物体定位方法在泊车应用中的一种实施方式的示意图。应当理解，泊车即停车，指将车辆存放入泊车位/停车位的动作。图1中的阴影区域示例性地表示泊车位，点P为参考点。由于车辆能够被近似看作为立方体，由此，泊车位相应地也能够看作为立方体，其在所述直角坐标系的平面下呈现为矩形。在一些特定的实施方式中，所述泊车位也能够看作为其它形状。

为了便于计算，将所述原点定义为泊车位边界的一点，在此为矩形的泊车位的左上角的点，该点在所述车辆的泊车过程中相比于其它矩形顶点最靠近所述车辆。而所述直角坐标系的X和Y轴相应地分别与所述矩形的短边和长边重合并且背离所述泊车位指向。同样，这种坐标系的建立方式也是示例性的，并且其它建立方式在本质上不影响本方法的应用。其中，所述泊车位的探测和数据能够例如通过超声波或摄像头来检测。

在车辆方面，所述参考点被示例性地被设置为所述车辆的后轴中心，以便简化计算。所述车辆中的任意一点、例如前轴中心也是可行的。而所述航向角θ示例性地被设置为所述车辆的前进方向或者说后退方向与所述直角坐标系的X轴所包夹的角。由此，在所述直角坐标系确定的情况下，所述参考点关于所述直角坐标系的位姿(位置和姿态、即x、y坐标和航向角θ)得以确定。例如在泊车场景中，本车会先用超声波传感器去探测车位，然后确定以车位右上角为原点的坐标系，从而得到起始点即图中点P的位姿。对于所述距离s和所述转弯曲率c而言，一方面，所述距离s通过所述车辆的轮速传感器(或称为里程计)算出。应当理解，里程计指如装在汽车上的测量行程的装置，在车辆中例如被理解为脉冲计数的轮速传感器或者说轮速脉冲计数传感器。轮速传感器则是用来测量汽车车轮转速的传感器，其脉冲的频率，即每秒钟产生的脉冲个数，反映了车轮旋转的快慢。另一方面，所述转弯曲率c通过所述车辆的方向盘转角传感器算出。

具体地，在图中所示出的泊车场景中，本方法所对应的公式为

参考图2(其示出了根据本发明的物体定位方法在泊车应用中的一种实施方式的示意图)来说明如何得到本公式。图2表明了车辆在泊车场景中的过程示例。其中点P和Q分别为同一参考点在k-1和k时刻下的点。由此可以看出，在当前的示例中车辆前进行驶。M、N分别为点P、Q的航向，X为平行于直角坐标系x轴的辅助线。位于x轴方向上方的点为P、Q两点所形成的圆弧的圆心。由此：

P(x_k-1,y_k-1,θ_k-1)Q(x_k,y_k,θ_k)

x_k＝x_k-1+PQ*cos(∠XPQ)

PQ＝2*R*sin(a)

R＝1/c_k-1 a＝0.5*s_k-1/R＝0.5*s_k-1*c_k-1(角度＝弧长/半径)

∠XPQ＝∠MPQ+b

∠MPQ＝a，b＝θ_k-1

∠XPQ＝a+θ_k-1

则x_k＝x_k-1+2*sin(0.5*s_k-1*c_k-1)*cos(θ_k-1+0.5*s_k-1*c_k-1)/c_k-1

同理y_k＝y_k-1+2*sin(0.5*s_k-1*c_k-1)*sin(θ_k-1+0.5*s_k-1*c_k-1)/c_k-1θ_k＝θ_k-1+s_k-1*c_k-1

s_k-1*c_k-1＝s_k-1/R，为变化的姿态角

其中，V_k为k时刻需要估计的状态向量。X_k为k时刻本车后轴中心在车位坐标系xoy下的x坐标，Y_k为k时刻本车后轴中心在车位坐标系xoy下的y坐标，θ为k时刻本车航向与x轴的夹角。s_k为k时刻由轮速传感器的脉冲计数算出的k-1时刻到k时刻这一周期内本车走过的距离，c_k为k时刻的方向盘转角对应的本车转弯曲率，这两个变量为传感器检测值经过简单运算后得到。应当理解，本公式连同下面要提到的公式是根据具体的应用场景和车辆模型得到的，其变量之间的计算关系以及系数根据实际场景和模型发生变化。

至此而言，本发明通过使用车辆通常配置的相对低成本的轮速脉冲计数传感器和方向盘转角传感器来达到定位目的。

为了进一步优化定位效果，所述状态向量通过扩展卡尔曼滤波来进行更新。应当理解，卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法，简单来说是一种最优化自回归数据处理算法。卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻的估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。也就是说，卡尔曼滤波的实质是由量测值重构系统的状态向量。它以“预测—实测—修正”的顺序递推，根据系统的量测值来消除随机干扰，再现系统的状态，或根据系统的量测值从被污染的系统中恢复系统的本来面目。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。卡尔曼滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。它便于计算机编程实现，并能够对现场采集的数据进行实时的更新和处理。

而扩展卡尔曼滤波是由卡尔曼滤波考虑时间非线性的动态系统，常应用于目标跟踪系统，在状态转移方程确定的情况下，扩展卡尔曼滤波已经成为了非线性系统状态估计的事实标准。扩展卡尔曼滤波与卡尔曼滤波最大的不同之处在于，扩展卡尔曼滤波的状态转移矩阵(上一时刻的状态信息)和观测矩阵(一步预测)都是状态信息的雅克比矩阵。也就是说，与线性系统的卡尔曼滤波相比，非线性滤波过程中新增的一些矩阵是将系统线性化表示的时候所计算得到的矩阵，就是对多元函数求偏导构成的雅克比矩阵。所谓的状态转移是指事物由一种状态转移到另一种状态。

具体到本例中，根据上述公式计算扩展卡尔曼滤波中的状态转移雅克比矩阵如下：

令

φ_k-1＝θ_k-1+s_k-1*c_k-1，

则有

F₁₃＝[cos(φ_k-1)-cos(θ_k-1)]/c_k-1

F₁₄＝cos(φ_k-1)

F₁₅＝s_k-1*cos(φ_k-1)/c_k-1+[sin(θ_k-1)-sin(φ_k-1)]/c_k-1 ²

F₂₃＝[sin(φ_k-1)-sin(θ_k-1)]/c_k-1

F₂₄＝sin(φ_k-1)

F₁₅＝s_k-1*sin(φ_k-1)/c_k-1+[cos(φ_k-1)-cos(θ_k-1)]/c_k-1 ²

系统的观测方程(是指在观测值与待估参数之间建立的函数关系式)为

其中，H_k为观测方程转移矩阵，R_s与R_c分别为距离s和曲率c的测量不确定度(是与测量结果相关联的一个参数，用于表征合理赋予被测量的值的分散性)，或者说分别是指轮速传感器和方向盘转角传感器的测量不确定度，与传感器本身特性相关。而D_k是指观测量(传感器检测值)的噪声向量。也就是说，所述测量不确定度和所述噪声向量取决于所使用的传感器(在此为轮速传感器和方向盘转角传感器)，在相应的传感器确定的情况下，所述测量不确定度和所述噪声向量也随之得以确定。

综上可以计算卡尔曼增益为

K＝F_k*H_k ^T*(H_k*F_k*H_k ^T+R_k)^-1

所以，状态向量的最终输出为

其中，update表示更新之意。其中的x_{k_update}、y_{k_update}、θ_{k_update}即为当前状态量的输出。

应用该算法，在泊车场景中，本车在车位坐标系下的定位精度可以达到x方向±0.04m每米的汽车运动路程，y方向可以达到±0.04m每米的汽车运动路程，角度预测上可以做到0.01rad每弧度。且在比较不平的砖面上有着相比于不使用该方法的算法明显更好的效果。

由此，本发明还旨在保护根据本发明的物体定位方法在泊车过程中的应用。应当理解的是，本发明的物体定位方法或应用可被执行在各种车辆上，包括汽油车、柴油车、轿车、货车、客车、电动汽车等等。因此，本发明的主题还旨在保护具有用于执行根据本发明的物体定位方法的装置并且/或者通过根据本发明的应用进行泊车的各种车辆。

综上所述，本发明的方法提供了优化的物体定位效果，特别是在车辆领域中，其创新性地将位置、航向角和当前测量值整体地看成状态向量，应用扩展卡尔曼滤波来消除误差，由此可以取得较好的定位效果。

应当理解的是，所有以上的优选实施例都是示例性而非限制性的，本领域技术人员在本发明的构思下对以上描述的具体实施例做出的各种改型或变形都应在本发明的法律保护范围内。

Claims (6)

1.一种物体定位方法，其特征在于，所述物体定位方法包括下列步骤：

在所述物体之外任取一固定点作为原点建立所述物体的直角坐标系XoY，其中，所述直角坐标系的X轴和Y轴所在的平面为水平面，

在所述物体上任取一点作为参考点、并任取一方向作为参考方向，

将所述参考点在所述直角坐标系上的x坐标、y坐标、所述参考方向与在所述平面中任取的一固定直线所围成的航向角θ、所述物体在单位时间内所走过的距离s、所述物体的转弯曲率c作为状态向量，

其中，所述参考点在一时刻k上的x坐标x_k、y坐标y_k通过在上一时刻k-1的x坐标x_k-1、y坐标y_k-1、航向角θ_k-1、距离s_k-1、转弯曲率c_k-1来得到，

其中，所述物体为车辆，所述物体定位方法应用于泊车过程，所述状态向量通过扩展卡尔曼滤波来进行更新，所述状态向量的计算方式为：

其中，V_k为k时刻需要估计的状态向量，并且所述扩展卡尔曼滤波中的状态转移雅克比矩阵为：

其中，令φ_k-1＝θ_k-1+s_k-1*c_k-1，

则有

所述转弯曲率c通过所述车辆的方向盘转角传感器算出。

2.根据权利要求1所述的物体定位方法，其特征在于，所述参考点为所述车辆的后轴中心。

3.根据权利要求1所述的物体定位方法，其特征在于，所述航向角θ为所述车辆的前进方向与所述直角坐标系的X轴所包夹的角。

4.根据权利要求1所述的物体定位方法，其特征在于，所述距离s通过所述车辆的轮速传感器算出。

5.根据权利要求1所述的物体定位方法，其特征在于，所述原点为泊车位边界的一点。

6.一种车辆，其特征在于，所述车辆具有用于执行根据权利要求1-5中任一项所述的物体定位方法的装置。

Images