1.本发明涉及车位定位技术领域，尤其涉及一种基于车位识别的定位方法及装置。
背景技术：
2.在地下停车场中，gps信号弱，无法进行正常的基站定位。因此局部定位精度尤为重要，尤其是对于自动泊车功能场景来说。局部定位中由于传感器的累积误差及外部扰动等，会导致基于轮速计及imu等传感器进行dr推算误差不断积累无法收敛。
3.因此需要外部输入进行误差纠正，现有技术cn202210627066.8公开了用于hpa的定位地图构建方法及基于hpa的车辆泊车方法，该方案是地图定位方法，需要先进行建图才能再基于地图车辆进行自定位修正，对于算力要求较高，且地图的精度要求也较高。整体传感器和域控制的性能要求都较高。
4.现有技术cn202110609565.x公开了一种用于电动车自动停泊和充换电的自动驾驶方法，预先建立的停车场高精地图，车辆进入所选停车场后，根据车辆gps信号加载停车场高精地图，该方案是典型的高精度地图方案，需要提前准备高精度地图，且在地下停车场等地方gps信号无法正常工作，无法通过高精度定位进行正常工作。
5.现有技术cn202011177574.8公开了一种自动泊车的方法和装置，该方案为纯视觉方案，通过相对位置进行泊车，不断重新计算车辆到车位的相对位置来进行泊车。此方案严重依赖视觉感知算法的精度，且在泊车过程中一旦出现严重干扰会直接影响到定位结果。抗干扰能力极差，且需要场景光照较好，感知算法的摄像头传感器及计算单元域控制的算力要求极高。
技术实现要素：
6.有鉴于此，本发明的目的在于提出一种基于车位识别的定位方法及装置，以解决现有技术依赖gps信号或地图的问题。
7.基于上述目的，本发明提供了一种基于车位识别的定位方法，其特征在于，包括
8.s1，开始；
9.s2，轮速直接定位；
10.s3，判断是否接收到目标车位；
11.若未接收到目标车位，直接输出轮速直接定位结果并返回s2再次进行轮速直接定位；
12.若接收到目标车位，融合目标车位数据进行融合定位，输出融合定位结果并返回s2再次进行轮速直接定位。
13.作为本技术的进一步改进，轮速直接定位包括
14.建立车轮定位坐标系，以车辆的后轴中心为原点，指向车头方向为x轴正方向，垂直x轴指向左后轮方向为y轴正方向，垂直x轴和y轴朝上的方向为z轴正方向，车头方向与x轴夹角为姿态角θ；
15.采用两轮自行车模型构建车辆运动学模型；
16.获取方向盘转角和两轮轮速，输入到车辆运动学模型中；
17.获得所用系数k，t时刻车辆质心滑移角β
t
，t时刻的车辆转向角速度t时刻车辆质心轨迹速度t时刻车辆质心的定位数据(x
′
t
,y
′
t
,θ
′
t
)和t时刻车辆后轴中心处的定位数据(x
t
,y
t
,θ
t
)。
18.所述系数k为：
[0019][0020]
所述t时刻车辆质心滑移角β
t
：
[0021][0022]
所述t时刻的车辆转向角速度
[0023][0024]
kf、kr分别为前后轮胎侧偏刚度系数，α为转向角，αf和αr分别为前后轮偏角，v是车辆质心处的速度，m为车辆质量，v
x
和vy分别为v在车辆坐标系x与y方向下的分解；lf和lr分别为车辆质心到前后轴的距离；β为滑移角，ωr为yaw方向转速，为t时刻的车辆转向角速度，为t时刻的车身速度输出，α
t
是前轮转向角，β
t
为t时刻车辆质心滑移角，k为计算所用系数。
[0025]
获得t时刻车辆质心轨迹速度和转向角速度
[0026][0027]
获得t时刻车辆质心的定位数据(x
′
t
,y
′
t
,θ
′
t
)计算如下：
[0028][0029]
获得t时刻车辆后轴中心处的定位数据(x
t
,y
t
,θ
t
)：
[0030]
[0031]
kf、kr分别为前后轮胎侧偏刚度系数，iz为车辆绕z轴的转动惯量，α为转向角，αf和αr分别为前后轮偏角；v是车辆质心处的速度，v
x
和vy分别为v在车辆坐标系x与y方向下的分解；lf和lr分别为车辆质心到前后轴的距离；β为滑移角，ωr为yaw方向转速，为t时刻的车辆转向角速度，为t时刻的车身速度输出，α
t
是前轮转向角，δt为t-1时刻与t时刻时间间隔；t＝0时刻，(x
′
t
,y
′
t
,θ
′
t
)＝(lr,0,0)。
[0032]
作为本技术的进一步改进，所述轮速直接定位通过轮速定位装置获得。
[0033]
作为本技术的进一步改进，所述融合定位包括
[0034]
获取目标车位的1点和2点分别在车辆坐标系下的坐标为(x1,y1)和(x2,y2)，1点和2点分别为目标车位靠近车辆的两个车位内角点，利用1点和2点坐标变化，进行融合定位。
[0035]
作为本技术的进一步改进，所述融合定位包括
[0036]
t＝t
p
时刻，第一次收到目标车位数据，1点和2点分别在车辆坐标系下的坐标为(x1,y1)和(x2,y2)，车辆定位数据为(x
p
,y
p
,θ
p
)；t＞t
p
时刻，1点和2点的坐标为(x
1t
,y
1t
)和(x
2t
,y
2t
)；在这两个时刻之间，车辆运动，车位静止，车位与车辆之间相对位置关系发生变化，等效为车辆静止，车位在车辆坐标系下运动，车位坐标变换等效为依次进行平移变换和旋转变换，设平移向量为t
pt
，设旋转角度为δφ
pt
。
[0037]
作为本技术的进一步改进，所述融合定位包括
[0038]
t＝t
p
时刻，1点与2点中点坐标为
[0039]
t＞t
p
时刻，1点与2点中点坐标为则
[0040][0041]
t＝t
p
时刻，2点到1点向量与x轴夹角φ
p
[0042][0043]
t＞t
p
时刻，2点到1点向量与x轴夹角φ
pt
[0044][0045][0046]
t＞t
p
时刻车辆在定位坐标系下的位置与姿态为
[0047][0048]
θ
t
＝θ
p-δφ
pt
；
[0049]
将预报值投影至像素平面得到得到车位角点投影残差为：
[0050][0051]
为1点反投影的像素坐标位置，与定位数据相关，
[0052]
为2点反投影的像素坐标位置，与定位数据相关，
[0053]
为接受到目标车位1点的像素坐标位置，
[0054]
为接收到目标车位2点的像素坐标位置；
[0055]
通过最小化损失函数来解决最小化重投影误差：
[0056][0057]
其中x为重投影残差和位置残差之和，rc为上诉反投影残差，rv为轮速递推残差，具体如下：
[0058]
四轮转向的轮速递推残差rv：
[0059][0060]
其中(x
t
,y
t
,θ
t
)为t时刻纯轮速递推结果，为t时刻轮速递推融合目标车位优化结果。
[0061]
一种基于车位识别装置，包括
[0062]
开启模块，用于开始；
[0063]
直接定位模块，用于轮速直接定位；
[0064]
判断融合模块，用于判断是否接收到目标车位；
[0065]
若未接收到目标车位，直接输出轮速直接定位结果并返回s2再次进行轮速直接定位；
[0066]
若接收到目标车位，融合目标车位数据进行融合定位，输出融合定位结果并返回s2再次进行轮速直接定位。
[0067]
作为本技术的进一步改进，所述判断融合模块包括计算单元，用于在t＝t
p
时刻，第一次收到目标车位数据，1点和2点分别在车辆坐标系下的坐标为(x1,y1)和(x2,y2)，车辆定位数据为(x
p
,y
p
,θ
p
)；t＞t
p
时刻，1点和2点的坐标为(x
1t
,y
1t
)和(x
2t
,y
2t
)；在这两个时刻之间，车辆运动，车位静止，车位与车辆之间相对位置关系发生变化，等效为车辆静止，车位在车辆坐标系下运动，车位坐标变换等效为依次进行平移变换和旋转变换，设平移向量为t
pt
，设旋转角度为δφ
pt
。
[0068]
本发明的有益效果：本技术提供了一个对于感知算法要求不高，且不依赖高精度地图，通过对于视觉感知结果和轮速计及imu等传感器的融合，来达到低成本的高精度定位
方案。在无gps信号的地下停车场也能进行自动泊车，且通过融合定位的方式提高了定位的鲁棒性和精度。
附图说明
[0069]
为了更清楚地说明本发明或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0070]
图1为本发明实施例一种基于车位识别的定位方法的处理流程图；
[0071]
图2为本发明实施例一种基于车位识别的定位方法的两轮自行车模型示意图；
[0072]
图3为本发明实施例一种基于车位识别的定位方法的车位相对位置变化示意图；
[0073]
图4为本发明实施例一种基于车位识别的定位方法的融合定位流程示意图。
具体实施方式
[0074]
为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，对本发明进一步详细说明。
[0075]
需要说明的是，除非另外定义，本发明使用的技术术语或者科学术语应当为本发明所属领域内具有一般技能的人士所理解的通常意义。本发明中使用的“第一”、“第二”以及类似的词语并不表示任何顺序、数量或者重要性，而只是用来区分不同的组成部分。“包括”或者“包含”等类似的词语意指出现该词前面的元件或者物件涵盖出现在该词后面列举的元件或者物件及其等同，而不排除其他元件或者物件。“连接”或者“相连”等类似的词语并非限定于物理的或者机械的连接，而是可以包括电性的连接，不管是直接的还是间接的。“上”、“下”、“左”、“右”等仅用于表示相对位置关系，当被描述对象的绝对位置改变后，则该相对位置关系也可能相应地改变。
[0076]
如图1-3所示，一种基于车位识别的定位方法，包括
[0077]
s1，开始；
[0078]
s2，轮速直接定位，所述轮速直接定位通过轮速定位装置获得，包括
[0079]
建立车轮定位坐标系，以车辆的后轴中心为原点，指向车头方向为x轴正方向，垂直x轴指向左后轮方向为y轴正方向，垂直x轴和y轴朝上的方向为z轴正方向，车头方向与x轴夹角为姿态角θ；
[0080]
采用两轮自行车模型构建车辆运动学模型；
[0081]
获取方向盘转角和两轮轮速，输入到车辆运动学模型中；
[0082]
获得所用系数k，t时刻车辆质心滑移角β
t
，t时刻的车辆转向角速度t时刻车辆质心轨迹速度t时刻车辆质心的定位数据(x
′
t
,y
′
t
,θ
′
t
)和t时刻车辆后轴中心处的定位数据(x
t
,y
t
,θ
t
)。
[0083]
所述系数k为：
[0084][0085]
所述t时刻车辆质心滑移角β
t
：
[0086][0087]
所述t时刻的车辆转向角速度
[0088][0089]
kf、kr分别为前后轮胎侧偏刚度系数，α为转向角，αf和αr分别为前后轮偏角，v是车辆质心处的速度，m为车辆质量，v
x
和vy分别为v在车辆坐标系x与y方向下的分解；lf和lr分别为车辆质心到前后轴的距离；β为滑移角，ωr为yaw方向转速，为t时刻的车辆转向角速度，为t时刻的车身速度输出，α
t
是前轮转向角，β
t
为t时刻车辆质心滑移角，k为计算所用系数。
[0090]
获得t时刻车辆质心轨迹速度和转向角速度
[0091][0092]
获得t时刻车辆质心的定位数据(x
′
t
,y
′
t
,θ
′
t
)计算如下：
[0093][0094]
获得t时刻车辆后轴中心处的定位数据(x
t
,y
t
,θ
t
)：
[0095][0096]
kf、kr分别为前后轮胎侧偏刚度系数，iz为车辆绕z轴的转动惯量，α为转向角，αf和αr分别为前后轮偏角；v是车辆质心处的速度，v
x
和vy分别为v在车辆坐标系x与y方向下的分解；lf和lr分别为车辆质心到前后轴的距离；β为滑移角，ωr为yaw方向转速，为t时刻的车辆转向角速度，为t时刻的车身速度输出，α
t
是前轮转向角，δt为t-1时刻与t时刻时间间隔；t＝0时刻，(x
′
t
,y
′
t
,θ
′
t
)＝(lr,0,0)。
[0097]
s3，判断是否接收到目标车位；
[0098]
若未接收到目标车位，直接输出轮速直接定位结果并返回s2再次进行轮速直接定
位；
[0099]
若接收到目标车位，融合目标车位数据进行融合定位，输出融合定位结果并返回s2再次进行轮速直接定位。
[0100]
所述融合定位包括
[0101]
获取目标车位的1点和2点分别在车辆坐标系下的坐标为(x1,y1)和(x2,y2)，1点和2点分别为目标车位靠近车辆的两个车位内角点，利用1点和2点坐标变化，进行融合定位。
[0102]
所述融合定位包括
[0103]
t＝t
p
时刻，第一次收到目标车位数据，1点和2点分别在车辆坐标系下的坐标为(x1,y1)和(x2,y2)，车辆定位数据为(x
p
,y
p
,θ
p
)；t＞t
p
时刻，1点和2点的坐标为(x
1t
,y
1t
)和(x
2t
,y
2t
)；在这两个时刻之间，车辆运动，车位静止，车位与车辆之间相对位置关系发生变化，等效为车辆静止，车位在车辆坐标系下运动，车位坐标变换等效为依次进行平移变换和旋转变换，设平移向量为t
pt
，设旋转角度为δφ
pt
。
[0104]
所述融合定位包括
[0105]
t＝t
p
时刻，1点与2点中点坐标为
[0106]
t＞t
p
时刻，1点与2点中点坐标为则
[0107][0108]
t＝t
p
时刻，2点到1点向量与x轴夹角φ
p
[0109][0110]
t＞t
p
时刻，2点到1点向量与x轴夹角φ
pt
[0111][0112][0113]
t＞t
p
时刻车辆在定位坐标系下的位置与姿态为
[0114][0115]
θ
t
＝θ
p-δφ
pt
；
[0116]
将预报值投影至像素平面得到得到车位角点投影残差为：
[0117][0118]
为1点反投影的像素坐标位置，与定位数据相关，
[0119]
为2点反投影的像素坐标位置，与定位数据相关，
[0120]
为接受到目标车位1点的像素坐标位置，
[0121]
为接收到目标车位2点的像素坐标位置；
[0122]
通过最小化损失函数来解决最小化重投影误差：
[0123][0124]
其中x为重投影残差和位置残差之和，rc为上诉反投影残差，rv为轮速递推残差，具体如下：
[0125]
四轮转向的轮速递推残差rv：
[0126][0127]
其中(x
t
,y
t
,θ
t
)为t时刻纯轮速递推结果，为t时刻轮速递推融合目标车位优化结果。
[0128]
如图4所示，1点和2点的获取与识别是通过相机抓取的，感知得到的车位点得到视觉计算位置，车轮直接定位通过轮速计与imu递推的得到位置，然后最小化两者残差的方法，联合优化得到优化后结果。
[0129]
一种基于车位识别装置，包括
[0130]
开启模块，用于开始；
[0131]
直接定位模块，用于轮速直接定位；
[0132]
判断融合模块，用于判断是否接收到目标车位；
[0133]
若未接收到目标车位，直接输出轮速直接定位结果并返回s2再次进行轮速直接定位；
[0134]
若接收到目标车位，融合目标车位数据进行融合定位，输出融合定位结果并返回s2再次进行轮速直接定位。
[0135]
所述判断融合模块包括计算单元，用于在t＝t
p
时刻，第一次收到目标车位数据，1点和2点分别在车辆坐标系下的坐标为(x1,y1)和(x2,y2)，车辆定位数据为(x
p
,y
p
,θ
p
)；t＞t
p
时刻，1点和2点的坐标为(x
1t
,y
1t
)和(x
2t
,y
2t
)；在这两个时刻之间，车辆运动，车位静止，车位与车辆之间相对位置关系发生变化，等效为车辆静止，车位在车辆坐标系下运动，车位坐标变换等效为依次进行平移变换和旋转变换，设平移向量为t
pt
，设旋转角度为δφ
pt
。
[0136]
所属领域的普通技术人员应当理解：以上任何实施例的讨论仅为示例性的，并非旨在暗示本发明的范围(包括权利要求)被限于这些例子；在本发明的思路下，以上实施例或者不同实施例中的技术特征之间也可以进行组合，步骤可以以任意顺序实现，并存在如上所述的本发明的不同方面的许多其它变化，为了简明它们没有在细节中提供。
[0137]
本发明旨在涵盖落入所附权利要求的宽泛范围之内的所有这样的替换、修改和变型。因此，凡在本发明的精神和原则之内，所做的任何省略、修改、等同替换、改进等，均应包含在本发明的保护范围之内。