CN109631896A - 一种基于车辆视觉和运动信息的停车场自主泊车定位方法 - Google Patents

Abstract

本发明涉及一种基于车辆视觉和运动信息的停车场自主泊车定位方法，包括以下步骤：1)位于地下车库的智能车通过车载双目摄像头采集图像数据，并采用基于双目视觉的同步定位与建图方法获取智能车在局部点云地图中的位置信息，完成一次定位；2)智能车在行驶过程中不断对车身周围的库位角点进行检测，判断是否经过库位；3)在检测到库位后，判断当前库位书否为可泊入库位；4)根据库位角点坐标以及车速和方向盘转角信息，采用扩展卡尔曼滤波进行融合，得到智能车相对于库位角点的精确相对位置，完成二次定位。与现有技术相比，本发明具有适应无GNSS信号环境、实时性和鲁棒性高、定位精度高、成本低、实用性强等优点。

Description

一种基于车辆视觉和运动信息的停车场自主泊车定位方法

技术领域

本发明涉及车辆自主泊车技术领域，尤其是涉及一种基于车辆视觉和运动信 息的停车场自主泊车定位方法。

背景技术

近年来，自动驾驶相关技术发展迅速，已经成为投资和科研的热点。而自主 泊车技术由于可以消除繁琐的泊车操作、复杂的泊车环境对驾驶员所造成的困扰， 已经成为自动驾驶技术落地和产业化的最佳途径之一，吸引了许多企业和高校投入 大量资金进行研发。

然而，为了空间的充分应用，越来越多的停车场建在地下。由于目前的智能 汽车定位大都依赖GNSS，在GNSS信号缺失的地下停车场通常必须依靠自车传感 器进行定位。现有的室内停车场定位方案主要依靠无线设备例如WiFi或UWB等， 其一方面需要布设大量基站设备，成本高昂；另一方面定位精度(米级)难以满足无 人驾驶需求。而基于激光雷达的室内SLAM方案成本太高，难以在量产车型中装 备。此外，泊入库位过程中自车相对于库位的位置的确定一般仅依赖于航位推算， 累计误差大，导致车辆最终姿态与理想姿态偏差较大。上述缺陷均是自主泊车系统 普及过程中亟需解决的问题。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于车辆视 觉和运动信息的停车场自主泊车定位方法。

本发明的目的可以通过以下技术方案来实现：

一种基于车辆视觉和运动信息的停车场自主泊车定位方法，用以在GNSS信 号缺失的地下停车场通过前视、环视相机以及车辆底盘信息实现定位，包括以下步 骤：

1)位于地下车库的智能车通过车载双目摄像头采集图像数据，并采用基于双 目视觉的同步定位与建图方法获取智能车在局部点云地图中的位置信息，完成一次 定位；

2)智能车在行驶过程中不断对车身周围的库位角点进行检测，判断是否经过 库位；

3)在检测到库位后，将车辆坐标系下的库位坐标转化到局部点云地图坐标系 中，按照标准库位空间大小计算库位范围内地图点的数目，若该数目超过设定阈值， 则表示当前库位已被占据，不可泊入，若该数目小于设定阈值，则表示当前库位为 可泊入库位；

4)在检测到可泊入库位后，智能车根据库位角点坐标以及车速和方向盘转角 信息，采用扩展卡尔曼滤波进行融合，得到智能车相对于库位角点的精确相对位置， 完成二次定位。

所述的步骤1)具体包括以下步骤：

11)通过设置在后视镜后不锈钢横梁上的前视双目相机获取车辆前方的双目图像信息，并采用棋盘格内参标定法对前视双目相机进行标定，获取两个前视单目相 机的内参和双目相机的基线距离；

12)在双目图像中提取候选点，完成双目匹配，建立视差图；

13)在候选点中选取激活点，计算激活点的深度，从而获得深度的初步估计， 将此深度作为先验信息，通过帧间匹配获取后续帧中激活点的深度信息，并利用像 素坐标和深度值建立半稠密点云地图；

14)将选取的激活点投影至相邻帧，通过非线性最小二乘法最小化光度误差获 得对后续帧的位姿估计。

所述的步骤13)中，激活点的深度z的计算式为：

其中，f为相机焦距，b为双目基线距离，d为视差。

所述的步骤2)具体包括以下步骤：

21)通过鱼眼环视相机获取车辆周围的环视图像信息，并通过棋盘格内参标定 法对鱼眼相机的内参数进行标定，通过地面标志点对鱼眼相机的外参数进行标定， 所述的鱼眼环视相机设有4个，分别设置在车辆前后保险杠以及左右后视镜处；

22)从环视图像中提取库位入口的两个角点，并获取库位角点在车辆坐标系中 的位置坐标。

所述的步骤3)具体包括以下步骤：

31)利用车辆在局部点云地图中的当前位姿，将车辆坐标系下库位入口角点坐 标转化到局部点云地图坐标系下；

32)根据标准库位长宽高信息结合库位入口角点构建用以表示库位空间的长方体空间，统计局部点云地图中该库位空间中的点的数目，如果大于设定阈值则表示 该库位被占据，否则，则发出可泊入信息。

所述的步骤12)具体包括以下步骤：

121)选择候选点：将双目图像分成多个32×32像素的块，并在每个块中选择 像素梯度大于随区域变化的像素梯度阈值的所有像素点作为候选点；

122)双目匹配建立视差图并计算视差d，具体为：

计算左目图像I₁待匹配像素和右目图像I₂中匹配点的归一化互相关性值NCC， 在右目图像中的水平极线上进行搜索，当NCC值最大时对应的点即为匹配点，并 且获取视差d，所述的归一化互相关性值NCC的计算式为：

其中，I₁(x,y)为左目图像中(x,y)处的像素值，为左目图像待匹配像 素(p_x,p_y)所在匹配窗口内所有像素的均值，(p_x,p_y)为左目图像待匹配像素坐标， (p_x+d,p_y)为右目图像匹配点坐标，I₂(x+d,y)为右目图像中(x+d,y)处的像 素值，为右目图像匹配点(p_x+d,p_y)所在匹配窗口内所有像素的均 值，W_p为以待匹配像素坐标为中心的匹配窗口，通常为3×3匹配窗口。

所述的步骤14)具体为：

图像帧i遍历所有用于计算的帧集合F，像素点p遍历图像帧i中的点集合P_i， 图像帧j遍历所有帧中能够观测到点p的对应点p′，总光度误差E_photo为当前帧位 姿的函数，通过非线性最小二乘法的数值计算方法高斯-牛顿法最小化总光度误差 进行估计位姿，所述的总光度误差E_photo的计算式为：

E_pj＝w_pL_γ(I_j[p′]-I_i[p])

其中，E_pj为像素点p与其在第j帧中对应投影点的光度误差，I_i,I_j分别表示 第i帧图像和第j帧图像，w_p为与强度梯度有关的加权系数，L_γ(I₁,I₂)为Huber函 数，c为常数，为第i帧图像中像素点p处的像素梯度，I_j[p′]为第j帧图 像中像素点p′处的像素强度，I_i[p]为第i帧图像中像素点p处的像素强度。

所述的步骤31)中，库位角点在局部点云地图中的坐标(x′,y′)的转化式为：

其中，θ为车辆坐标系下的库位观测角，(x,y)为车辆在局部点云地图中的坐标，为航向角，(Lot.x,Lot.y)为车辆坐标系下库位角点坐标，r为相对车辆距离。

所述的步骤4)具体包括以下步骤：

41)车辆开始泊入库位时，通过读取CAN总线上的车辆底盘信息并进行解析 获取车辆的实时速度和方向盘转角信息；

42)将实时库位角点位置信息以及车速和方向盘转角输入至扩展卡尔曼滤波器进行融合，通过计算欧式距离判断是否为同一库位，对重复观测的库位位置和车辆 在局部坐标系下的位姿进行闭环优化，消除累积噪声，最终得到高精度的车辆位姿 和库位位置。

所述的步骤42)中，每一时刻获得的库位角点坐标和车辆底盘信息均含有测 量误差，车辆和库位所处位置为待估计量，采用扩展卡尔曼滤波对进行描述，具体 为：

421)定义状态空间转移方程和状态空间的观测方程为：

X_k＝f(X_k-1,U_k-1,W_k-1)

Z_k＝h(X_k,V_k)

其中，k为当前时刻，k-1为上一时刻，X为包含车辆位姿及库位角点坐标的 状态向量，U表示输入的车辆底盘数据，Z中包含了车辆对库位角点坐标的观测， W为系统噪声向量，f()为状态空间转移方程，h()为状态空间观测方程；

422)扩展卡尔曼滤波的具体过程为：

预测更新：

X_k＝f(X_k-1,U_k-1,W_k-1)

P(k)^-＝F(k)P(k-1)F(k)^T+Q

测量更新：

K(k)＝P(k)^-H(k)^T(H(k)P(k)^-H(k)^T+R)^-1

P(k)＝(I-K(k)H(k))P(k)^-

其中，P为误差协方差矩阵，K为卡尔曼增益矩阵，F为f()的雅可比矩阵，H 为h()的雅可比矩阵，Q和R分别为系统噪声协方差阵和观测噪声矩阵协方差阵；

423)通过预测更新和测量更新进行迭代，最终得到融合后的定位结果。

与现有技术相比，本发明具有以下优点：

一、适应无GNSS信号环境：一次定位实现了停车场等无GNSS信号环境中提供 实时定位结果。

二、实时性和鲁棒性高：相比其他基于特征点提取的纯视觉SLAM方法本一次 定位方法有更好的实时性和鲁棒性，并且可建立半稠密点云局部地图，用于导航、 避障、规划和库位占据情况的判断。

三、定位精度高：二次定位可以实现车辆位姿和库位位置的高精度厘米级定位，可以保证车辆良好的泊入姿态。

四、成本低、实用性强：整个定位系统仅使用低成本传感器，且不依赖对停车 场的改造，实用性强。

附图说明

图1为本发明的基于车辆视觉和运动信息的停车场自主泊车定位方法的流程 图。

图2为坐标转化的示意图。

图3为所设计扩展卡尔曼滤波器程序框图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

实施例

如图1所示，一种基于车辆视觉和运动信息的停车场自主泊车高精度定位方法，包括以下步骤：

步骤1、智能车驶入地下车库后，使用车载双目摄像头采集图像数据，利用基 于双目视觉的同步定位与建图(SLAM)方法得到智能车在局部点云地图中的位置 信息，即一次定位；

步骤1.1通过低成本的前视双目相机获取车辆前方的图像信息；

步骤1.1.1对视觉传感器进行布置

采用两个同样的前视单目相机组成双目相机，两个单目相机固定于后视镜后的不锈钢横梁上，间距为40cm左右。前视相机分辨率为1080p或以上，水平视场角 75-90度之间，帧率为25Hz，延迟小于5ms；

步骤1.1.2标定

通过棋盘格内参标定法(为现有技术)对前视双目相机进行标定，求得两个前视单目相机的内参和双目相机的基线距离。

步骤1.2在双目图像中提取候选点，完成双目匹配，建立视差图；

步骤1.2.1候选点的选择

图像将在计算时被分成32×32像素的块。对于每个块，计算其区域自适应像 素梯度阈值，并选择具有大于该阈值的像素梯度的所有点。最终可以保证择取的候 选点在图像中均匀分布，且位于图像中像素梯度较大的地方。

步骤1.2.2双目匹配建立视差图

假定左目图像中有一像素点p(x,y),须找到右目图像中的匹配点p’(x+d,y)， 双目匹配通过计算两个像素点的归一化互相关性计算(Normalization Cross-Correlation，NCC)实现。NCC的计算公式为：

其中，

左边为图像I₁，右边为图像I₂，左目图像待匹配像素坐标为(p_x,p_y)，右目图像 匹配点坐标为(p_x+d,p_y)，W_p表示以待匹配像素坐标为中心的匹配窗口，通常为3×3 匹配窗口，I₁(x,y)表示(x,y)处的像素值，表示匹配窗口所有像素的均值。

经过上式计算出的NCC值为越大则表示两个像素点的相关性越强。在右目图 像中的水平极线上进行搜索，NCC值最大的点即为匹配点，只需求出右目中匹配 点坐标即可得视差d；

步骤1.2.3对所有候选点用步骤1.2.2中方法求视差，即可得视差图。

步骤1.3在步骤1.2中的候选点中选取激活点，计算其深度，从而获得深度的 初步估计。利用此深度作为先验信息，后续帧中激活点的深度信息来源于帧间匹配。 利用像素坐标和深度值建立半稠密点云地图；

深度的计算过程：

在得到视差d后，根据利用双目相机模型推出的公式：

其中，z为三维坐标点的深度值，f为相机焦距，b为双目基线距离，d为视差。

经过上式可算得空间三维坐标点的深度值。

后续帧中激活点的深度来源于帧间匹配，仍然采用极线搜索的方法，由于有了 深度先验信息，只需要在极线上进行小范围搜索。

步骤1.4将步骤1.3中选取的激活点投影至相邻帧，通过非线性最小二乘的方 法最小化光度误差可以获得对后续帧的位姿估计。

步骤1.4.1光度误差的计算

光度误差可用下述公式描述：

E_pj＝w_pL_γ(I_j[p′]-I_i[p])

式中，I_i,I_j分别表示第i帧图像和第j帧图像，L_γ(I₁,I₂)是Huber函数，w_p是与 强度梯度有关的加权系数，其计算公式为：

式中，c是一个常数，该系数随着强度梯度值的增大而减小，高梯度像素块对 应着低权值。

总的光度误差由下式给出：

式中，i遍历所有用于计算的帧集合F，p遍历图像帧i中的点集合P_i，j遍历 所有帧中能够观测到点p的对应点。总的光度误差是当前帧位姿的函数，位姿的估 计通过最小化总的光度误差来进行。

步骤1.4.2总光度误差的最小化

采用非线性最小二乘法的数值计算方法高斯-牛顿法进行迭代求解，算法步骤为：

给定初始值x₀；

对于第k次迭代，求出当前的雅可比矩阵J(x_k)和误差f(x_k)；

求解增量方程：J(x_k)^TJ(x_k)Δx_k＝-J(x_k)^Tf(x_k)；

若Δx_k足够小，则停止，此时x_k值为求解值。否则，令x_k+1＝x_k+Δx_k，返回步骤 二继续求解。

最终，迭代收敛后，最终得到的x_k的值即为最优的位姿估计值。

步骤2、智能车在行驶过程中不断使用环视检测模块对车身周围的库位角点进 行检测，判断是否经过库位；

步骤2.1通过低成本的鱼眼环视相机获取车辆周围的图像信息；

步骤2.1.1环视相机的布置

本发明采用4个鱼眼环视相机，布置于车辆前后保险杠，以及左右后视镜，分 辨率为720p或以上，水平视场角180度，帧率为25Hz，延迟小于5ms；

步骤2.1.2环视相机的标定

通过棋盘格内参标定法对鱼眼相机的内参数进行标定，通过地面标志点对鱼眼相机的外参数进行标定，获得拼接后的俯视图如图2所示。

步骤2.2从环视图像中提取库位入口的两个角点，并计算库位角点在车辆坐标 系中的位置坐标，将一次定位所得的局部点云地图进行保存。

步骤2.2.1库位的布置

库位需为停车场标准库位，其库位线须平直清晰，库位角清晰且成90°；

步骤2.2.2检测并计算

在获得环视相机俯视图后，利用机器视觉方法，检测俯视图中的库位入口角点。由于俯视图标定在的车身坐标系(x轴方向垂直航向向右，y轴方向沿航向指向车辆 行驶方向)下，因此库位近端两个角点坐标可直接读出，远端两个角点利用库位规 则(所述库位规则，为每个车库固有的规则，非本发明技术方案需要披露内容)估算 得到。

步骤3、如果检测到库位，将车辆坐标系下的库位坐标转化到局部点云地图坐 标系中，按照标准库位空间大小计算库位范围内地图点的数目，如果超过某一阈值 则表示库位已被占据，否则为空库位，给出可泊入信息；

步骤3.1利用车辆在局部点云地图中的当前位姿，将步骤2.2所得的车辆坐标 系下库位入口角点坐标转化到局部点云地图坐标系下；

坐标转化的具体方式为：

假定车辆在局部点云地图中的坐标为(x,y)，航向角为车辆坐标系下库位 角点坐标为(Lot.x,Lot.y)，则相对车辆距离r为：

其中，θ为车辆坐标系下的库位观测角，(x′,y′)为经过坐标转化后，库位角点 在局部点云地图中的坐标。

步骤3.2利用标准的库位长宽高结合库位入口角点构建一长方体空间表示库位空间，计算所保存的局部点云地图中该库位空间中的点的数目，如果大于某一阈值 则表示该库位被占据，否则则发出可泊入信息。

根据《汽车库建筑设计规范》JGJ100-98相关规定，选取车库空间为 5.8m×3.0m×2.4m。库位中点数目的阈值需要依照实验结果确定。

步骤4、如果检测到可泊入库位，则智能车通过环视图像拼接与处理得到库位 角点坐标，通过采集自车的CAN总线信号得到车速和方向盘转角信息，利用扩展 卡尔曼滤波对两种信息源进行融合，得到智能车相对于库位角点的精确相对位置， 即二次定位。

步骤4.1车辆开始泊入库位时，通过读取CAN总线上的车辆底盘信息并进行 解析获取车辆的实时速度和方向盘转角信息；

步骤4.2将环视检测模块发来的实时库位角点信息，以及车速和方向盘转角输 入扩展卡尔曼滤波器进行融合，通过计算欧式距离判断是否为同一库位，对重复观 测的库位位置和车辆在局部坐标系下的位姿进行闭环优化，不断消除累积噪声，最 终得到高精度的车辆位姿和库位位置(厘米级)。

所述扩展卡尔曼滤波器为适用于非线性系统的卡尔曼滤波器，它基于联合概率分布和贝叶斯理论，可以根据一系列具有噪声的时序传感器数据，对未知量进行最 大后验概率的估计。在本方法中，每一时刻获得的库位角点坐标和车辆底盘信息均 含有测量误差，而车辆和库位所处位置则为待估计量，则可用扩展卡尔曼滤波对该 问题进行描述：

定义状态空间转移方程和状态空间的观测方程如下：

X_k＝f(X_k-1,U_k-1,W_k-1)

Z_k＝h(X_k,V_k)

其中，k为当前时刻,k-1为上一时刻，X为包含车辆位姿及库位角点坐标的状 态向量，U表示输入的车辆底盘数据，Z中包含了车辆对库位角点坐标的观测，W 代表系统噪声向量，f()表示状态空间转移方程的表达式，h()表示状态空间观测方 程的表达式；

扩展卡尔曼滤波的具体过程如下：

预测更新：

X_k＝f(X_k-1,U_k-1,W_k-1)

P(k)^-＝F(k)P(k-1)F(k)^T+Q

测量更新：

K(k)＝P(k)^-H(k)^T(H(k)P(k)^-H(k)^T+R)^-1

P(k)＝(I-K(k)H(k))P(k)^-

其中，P表示误差协方差矩阵，K表示卡尔曼增益矩阵，F表示函数f()的雅可 比矩阵，H表示函数h()的雅可比矩阵，Q和R分别表示系统噪声协方差阵和观测 噪声矩阵协方差阵。

根据上述流程和公式设计扩展卡尔曼滤波器，其程序流程图如图3所示，利用 预测更新和测量更新的不断迭代，最终即可得到融合后的定位结果。

Claims (10)

1.一种基于车辆视觉和运动信息的停车场自主泊车定位方法，用以在GNSS信号缺失的地下停车场通过前视、环视相机以及车辆底盘信息实现定位，其特征在于，包括以下步骤：

1)位于地下车库的智能车通过车载双目摄像头采集图像数据，并采用基于双目视觉的同步定位与建图方法获取智能车在局部点云地图中的位置信息，完成一次定位；

2)智能车在行驶过程中不断对车身周围的库位角点进行检测，判断是否经过库位；

3)在检测到库位后，将车辆坐标系下的库位坐标转化到局部点云地图坐标系中，按照标准库位空间大小计算库位范围内地图点的数目，若该数目超过设定阈值，则表示当前库位已被占据，不可泊入，若该数目小于设定阈值，则表示当前库位为可泊入库位；

4)在检测到可泊入库位后，智能车根据库位角点坐标以及车速和方向盘转角信息，采用扩展卡尔曼滤波进行融合，得到智能车相对于库位角点的精确相对位置，完成二次定位。

2.根据权利要求1所述的一种基于车辆视觉和运动信息的停车场自主泊车定位方法，其特征在于，所述的步骤1)具体包括以下步骤：

11)通过设置在后视镜后不锈钢横梁上的前视双目相机获取车辆前方的双目图像信息，并采用棋盘格内参标定法对前视双目相机进行标定，获取两个前视单目相机的内参和双目相机的基线距离；

12)在双目图像中提取候选点，完成双目匹配，建立视差图；

13)在候选点中选取激活点，计算激活点的深度，从而获得深度的初步估计，将此深度作为先验信息，通过帧间匹配获取后续帧中激活点的深度信息，并利用像素坐标和深度值建立半稠密点云地图；

14)将选取的激活点投影至相邻帧，通过非线性最小二乘法最小化光度误差获得对后续帧的位姿估计。

3.根据权利要求2所述的一种基于车辆视觉和运动信息的停车场自主泊车定位方法，其特征在于，所述的步骤13)中，激活点的深度z的计算式为：

其中，f为相机焦距，b为双目基线距离，d为视差。

4.根据权利要求1所述的一种基于车辆视觉和运动信息的停车场自主泊车定位方法，其特征在于，所述的步骤2)具体包括以下步骤：

21)通过鱼眼环视相机获取车辆周围的环视图像信息，并通过棋盘格内参标定法对鱼眼相机的内参数进行标定，通过地面标志点对鱼眼相机的外参数进行标定，所述的鱼眼环视相机设有4个，分别设置在车辆前后保险杠以及左右后视镜处；

22)从环视图像中提取库位入口的两个角点，并获取库位角点在车辆坐标系中的位置坐标。

5.根据权利要求1所述的一种基于车辆视觉和运动信息的停车场自主泊车定位方法，其特征在于，所述的步骤3)具体包括以下步骤：

31)利用车辆在局部点云地图中的当前位姿，将车辆坐标系下库位入口角点坐标转化到局部点云地图坐标系下；

32)根据标准库位长宽高信息结合库位入口角点构建用以表示库位空间的长方体空间，统计局部点云地图中该库位空间中的点的数目，如果大于设定阈值则表示该库位被占据，否则，则发出可泊入信息。

6.根据权利要求2所述的一种基于车辆视觉和运动信息的停车场自主泊车定位方法，其特征在于，所述的步骤12)具体包括以下步骤：

121)选择候选点：将双目图像分成多个32×32像素的块，并在每个块中选择像素梯度大于随区域变化的像素梯度阈值的所有像素点作为候选点；

122)双目匹配建立视差图并计算视差d，具体为：

计算左目图像I₁待匹配像素和右目图像I₂中匹配点的归一化互相关性值NCC，在右目图像中的水平极线上进行搜索，当NCC值最大时对应的点即为匹配点，并且获取视差d，所述的归一化互相关性值NCC的计算式为：

其中，I₁(x,y)为左目图像中(x,y)处的像素值，为左目图像待匹配像素(p_x,p_y)所在匹配窗口内所有像素的均值，(p_x,p_y)为左目图像待匹配像素坐标，(p_x+d,p_y)为右目图像匹配点坐标，I₂(x+d,y)为右目图像中(x+d,y)处的像素值，为右目图像匹配点(p_x+d,p_y)所在匹配窗口内所有像素的均值，W_p为以待匹配像素坐标为中心的匹配窗口。

7.根据权利要求2所述的一种基于车辆视觉和运动信息的停车场自主泊车定位方法，其特征在于，所述的步骤14)具体为：

图像帧i遍历所有用于计算的帧集合F，像素点p遍历图像帧i中的点集合P_i，图像帧j遍历所有帧中能够观测到点p的对应点p′，总光度误差E_photo为当前帧位姿的函数，通过非线性最小二乘法的数值计算方法高斯-牛顿法最小化总光度误差进行估计位姿，所述的总光度误差E_photo的计算式为：

E_pj＝w_pL_γ(I_j[p′]-I_i[p])

其中，E_pj为像素点p与其在第j帧中对应投影点的光度误差，I_i,I_j分别表示第i帧图像和第j帧图像，w_p为与强度梯度有关的加权系数，L_γ(I₁,I₂)为Huber函数，c为常数，||▽I_i(p)||₂为第i帧图像中像素点p处的像素梯度，I_j[p′]为第j帧图像中像素点p′处的像素强度，I_i[p]为第i帧图像中像素点p处的像素强度。

8.根据权利要求6所述的一种基于车辆视觉和运动信息的停车场自主泊车定位方法，其特征在于，所述的步骤31)中，库位角点在局部点云地图中的坐标(x′,y′)的转化式为：

其中，θ为车辆坐标系下的库位观测角，(x,y)为车辆在局部点云地图中的坐标，为航向角，(Lot.x,Lot.y)为车辆坐标系下库位角点坐标，r为相对车辆距离。

9.根据权利要求1所述的一种基于车辆视觉和运动信息的停车场自主泊车定位方法，其特征在于，所述的步骤4)具体包括以下步骤：

41)车辆开始泊入库位时，通过读取CAN总线上的车辆底盘信息并进行解析获取车辆的实时速度和方向盘转角信息；

42)将实时库位角点位置信息以及车速和方向盘转角输入至扩展卡尔曼滤波器进行融合，通过计算欧式距离判断是否为同一库位，对重复观测的库位位置和车辆在局部坐标系下的位姿进行闭环优化，消除累积噪声，最终得到高精度的车辆位姿和库位位置。

10.根据权利要求9所述的一种基于车辆视觉和运动信息的停车场自主泊车定位方法，其特征在于，所述的步骤42)中，每一时刻获得的库位角点坐标和车辆底盘信息均含有测量误差，车辆和库位所处位置为待估计量，采用扩展卡尔曼滤波对进行描述，具体为：

421)定义状态空间转移方程和状态空间的观测方程为：

X_k＝f(X_k-1,U_k-1,W_k-1)

Z_k＝h(X_k,V_k)

其中，k为当前时刻，k-1为上一时刻，X为包含车辆位姿及库位角点坐标的状态向量，U表示输入的车辆底盘数据，Z中包含了车辆对库位角点坐标的观测，W为系统噪声向量，f()为状态空间转移方程，h()为状态空间观测方程；

422)扩展卡尔曼滤波的具体过程为：

预测更新：

X_k＝f(X_k-1,U_k-1,W_k-1)

P(k)-＝F(k)P(k-1)F(k)^T+Q

测量更新：

K(k)＝P(k)^-H(k)^T(H(k)P(k)^-H(k)^T+R)^-1

P(k)＝(I-K(k)H(k))P(k)^-

其中，P为误差协方差矩阵，K为卡尔曼增益矩阵，F为f()的雅可比矩阵，H为h()的雅可比矩阵，Q和R分别为系统噪声协方差阵和观测噪声矩阵协方差阵；

423)通过预测更新和测量更新进行迭代，最终得到融合后的定位结果。