本发明涉及一种基于激光点云对称面提取的对称目标位姿获取方法，属于智能系统位姿获取领域。
背景技术：
1、目前，目标位姿获取相关方法众多，主要可分为三类，基于图像的目标检测与定位方法，基于激光雷达的目标定位方法以及基于其他传感器的定位方法。
2、基于图像的目标检测与定位方法主要使用神经网络解决定位问题，例如首先生成候选区域/框，再通过深度学习定位的spp-net、faster r-cnn等方法，以及直接基于深度学习回归的yolo方法等。基于图像的目标检测与定位方法主要目的是在图片中标记出目标的位置，具体是指使用边框将目标框出。这些方法成熟稳定，也可通过使用双目相机或深度相机等将目标位置解算到实际的物理空间中。
3、使用激光雷达的目标定位方法中3d目标检测及其在自动驾驶中的应用已有大量研究，常见的方法分为两步，第一步使用pointnet++网络等方法进行点云特征提取，第二步使用例如anchor-based网络方法完成核心目标检测。基于激光雷达的目标检测方法很多也逐渐成熟，这些方法可以在空间中以较高精度框出不同目标的具体位置和空间朝向。
4、基于其他传感器的定位方法，常见的有基于调频连续波(fmcw)多输入多输出(mimo)雷达的定位方法等。
5、目前，这三类方法在实际应用中主要具有以下问题：
6、首先，部分方法目标检测精度不足，主要存在于基于图像的目标检测与定位方法以及基于调频连续波(fmcw)多输入多输出(mimo)雷达的定位方法。基于图像的目标检测与定位方法的核心目的主要在目标检测而非目标定位，该方法只能得到目标的大致位置。基于调频连续波(fmcw)多输入多输出(mimo)雷达的定位方法主要针对的是较远距离的目标定位，并不完全聚焦于近距离目标，其定位精度也不够理想。
7、第二，部分方法无法完成对空间位姿的提取。主要存在于基于图像的目标检测与定位方法以及基于调频连续波(fmcw)多输入多输出(mimo)雷达的定位方法。这两种方法从机理上无法有效提取目标空间构型，不能解算目标的空间朝向。
8、最后，部分方法计算复杂，难以在算力非常有限的计算单元中部署，难以满足实时性需求。主要存在于基于图像的目标检测与定位方法和基于激光雷达的定位方法。这两种采用神经网络方法，方法计算复杂度较高，难以保证目标位姿获取的实时性。
技术实现思路
1、为了解决在已知对称目标粗略的参考位置，通过激光点云获取对称目标高精度位姿并充分满足实时性要求的问题，本发明的目的是提供一种基于激光点云对称面提取的对称目标位姿获取方法，该方法通过参考位姿构造对称面，将对称目标点云基于此面作对称，将对称后的点云与对称目标原点云作点云匹配，以此修正对称面位置，再通过修正后的对称面进一步解算对称目标位姿。
2、本发明的目的是通过下述技术方案实现的。
3、本发明公开的一种基于激光点云对称面提取的对称目标位姿获取方法，包括以下步骤：
4、步骤一、通过和激光雷达点云数据对对称目标点云进行提取并进行体素滤波；
5、采集激光雷达点云数据，在已知的对称目标参考位置附近进行范围滤波，借助目标姿态进一步缩小目标范围，以提取对称目标点云；所述附近是指，以目标为中心，半径不超过3m的范围；然后，对对称目标点云进行体素滤波；得到对称目标的真实点云，记为点云a；并记激光雷达点云数据所在的原始坐标系为xyz坐标系。
6、步骤二、构造参考位姿处的对称目标对称面，基于此对称面将真实点云作对称；
7、根据对称目标参考位姿，构造对称目标在该处的对称面，称为对称目标对称面；对称目标参考位置表示为x0，y0，航向角表示为θ0，则生成的对称面表示为：
8、y-y0＝(x-x0)tanθ0 (1)
9、其中x，y为xyz坐标系中的坐标变量。
10、再将真实点云基于此对称面作对称，即真实点云中的点(x1，y1，z1)关于此对称面对称为点(x′1，y′1，z′1)，其中
11、x′1＝(x1-x0)cos 2θ0+(y1-y0)sin2θ0+x0 (2a)
12、y′1＝(x1-x0)sin 2θ0-(y1-y0)cos 2θ0+y0 (2b)
13、z′1＝z1 (2c)
14、将步骤一得到的点云a中的每个点按照公式(1)所示的对称面作对称；得到对称目标对称点云，记为点云b；
15、步骤三、使用点云匹配方法将点云a与点云b匹配，得到点云a与点云b最佳匹配时，点云b到点云a的变换t；
16、所述t包含x和y方向的平移量，记为x2，y2，以及航向方向的旋转量，记为θ2；
17、步骤四、通过点云b到点云a的变换t，修正步骤二对称目标对称面，并进一步确定初步修正后的对称目标位姿；
18、将xyz坐标系的坐标原点平移到式(1)平面与y轴交点处，称为坐标系x′y′z′；则在坐标系x′y′z′中的等效变换t’记为x′2，y′2，θ′2；其中
19、x′2＝x2-(y0-x0 tanθ0)sinθ2 (3a)
20、y′2＝y2-(y0-x0 tanθ0)(1-cosθ2) (3b)
21、θ′2＝θ2 (3c)
22、此时，在坐标系x′y′z′中，对称面过原点，则对点云b旋转α角度后，点云b与点云a将关于式(1)平面旋转α/2角度后的平面面对称；同样将点云b移动(x，y)后，点云b与点云a将关于式(1)平面旋转移动(x/2，y/2)后的平面面对称；由此可以通过变换t’对点云b与点云a的对称面即修正，修正后的对称面在坐标系x′y′z′表示为：
23、
24、其中x′，y′为x′y′坐标系的坐标变量。
25、将式(4)变换回坐标系xyz，得到修正后的对称面在坐标系xyz中表示为
26、
27、经过第一次修正后的对称目标位姿，即对称目标位置表示为x′0，y′0，航向角表示为θ′
28、x′0＝xk+((x0-xk)cosθ′+(y0-yk)sinθ′)cosθ′ (6a)
29、y′0＝yk+((x0-xk)cosθ′+(y0-yk)sinθ′)sinθ′ (6b)
30、
31、其中中间变量
32、
33、步骤五、根据几何坐标关系估计步骤四修正后对称面平行方向的目标中心位置，得到最终修正后的对称目标位姿；
34、得到步骤四中的对称目标修正位姿x′0，y′0，θ′后，在θ′角度平行方向，即修正后对称面平行方向求目标点云的平均位置；即对所有(x1，y1，z1)∈a，a指点云a中所有点组成的集合，计算平均值：
35、m＝agv(x1 cosθ′+y1 sinθ′)， (7)
36、将位姿x′0，y′0，θ′平移到该平均位置，得到最终修正后对称目标位姿，最终修正后对称目标位置表示为x″0，y″0，航向角表示为θ″
37、x″0＝m cosθ′+(-xk sinθ′+yk cosθ′)(-sinθ′) (8a)
38、y″0＝m sinθ′+(-xk sinθ′+ykcosθ′)cosθ′ (8b)
39、θ″＝θ′ (8c)
40、有益效果：
41、1.本发明公开的一种基于激光点云对称面提取的对称目标位姿获取方法，聚焦于车辆、充电桩等目标的对称特性，将目标定位问题转化为点云匹配问题，通过广泛研究的点云匹配方法，实现对称目标的精确定位。
42、2.本发明公开的一种基于激光点云对称面提取的对称目标位姿获取方法，通过对称特性部分提取了目标的形状特征，能够对目标的位置和姿态同时进行提取。
43、3.本发明公开的一种基于激光点云对称面提取的对称目标位姿获取方法，通过参考位姿构造对称面，将对称目标点云基于此面作对称，将对称后的点云与对称目标原点云作点云匹配，以此修正对称面位置，再通过修正后的对称面进一步解算对称目标位姿，完成对称目标获取，具有对称目标获取方法简单、对计算单元算力要求低、实时性高等优点。
1.一种基于激光点云对称面提取的对称目标位姿获取方法，其特征在于：包括以下步骤：
2.如权利要求1所述的一种基于激光点云对称面提取的对称目标位姿获取方法，其特征在于：步骤一实现方法为，
3.如权利要求2所述的一种基于激光点云对称面提取的对称目标位姿获取方法，其特征在于：步骤二实现方法为，
4.如权利要求3所述的一种基于激光点云对称面提取的对称目标位姿获取方法，其特征在于：步骤三中，
5.如权利要求4所述的一种基于激光点云对称面提取的对称目标位姿获取方法，其特征在于：步骤四实现方法为，
6.如权利要求5所述的一种基于激光点云对称面提取的对称目标位姿获取方法，其特征在于：步骤五实现方法为，
