基于车身位姿的环境感知方法与流程

1.本发明涉及车辆控制技术领域，具体涉及基于车身位姿的环境感知方法。


背景技术：

2.在驾驶辅助功能中，需要进行车道线、车位线等地面标线的位置进行环境感知。车辆往往根据从车身四周的传感器获取的物体位置，采用固定的映射计算得到物体的感知位置，计算过程中的映射计算是基于车辆处于平稳无倾斜的地面条件进行的。
3.但是在实际使用过程中，在进行感知识别时，车辆并不一定处于平稳无倾斜的地面条件，地面的倾斜会导致感知位置的映射计算产生偏差，车辆环境感知识别的位置不准确，影响驾驶辅助功能的实现。


技术实现要素：

4.为了解决上述问题，本发明提出基于车身位姿的环境感知方法，能够基于车身位姿，确定环境感知的物体的真实位置，提高环境感知的准确度。
5.本发明实施例提供一种基于车身位姿的环境感知方法，所述方法包括：
6.通过获取车辆的车身位姿参数和车辆传感器标定的传感器外参，构建预设的理想水平坐标系、传感器坐标系和以车辆所在的真实平面建立的真实平面坐标系间的坐标转换模型；
7.根据所述传感器获取待定位点在所述传感器坐标系的感知坐标和所述坐标转换模型计算所述待定位点在所述真实平面坐标系的映射坐标。
8.优选地，所述真实平面坐标系的构建过程具体包括：
9.以车辆预设位置在水平面的垂足作为所述理想水平坐标系的坐标原点；
10.根据所述车身位姿参数相对所述理想水平坐标系建立车身所处平面的车身坐标系；
11.以所述车身坐标系的原点位置为中心，根据所述车身位姿参数中的车身所处真实平面相对水平的偏航角和俯仰角对所述理想水平坐标系进行对应的旋转，得到当前车辆所处真实平面的真实平面坐标系。
12.作为一种优选方案，所述坐标转换模型构建过程包括：
13.根据所述车身位姿参数确定由所述理想水平坐标系到车身平面所在的车身坐标系的理想-车身坐标变换矩阵，以及由所述车身坐标系到所述真实平面坐标系的车身-真实坐标变换矩阵；
14.根据所述传感器外参确定由所述传感器坐标系到所述车身坐标系的传感器-车身坐标变换矩阵；
15.将所述理想-车身坐标变换矩阵、所述车身-真实坐标变换矩阵和所述传感器-车身坐标变换矩阵作为所述坐标转换模型。
16.进一步地，所述理想-车身坐标变换矩阵为
17.所述车身-真实坐标变换矩阵为
18.所述传感器-车身坐标变换矩阵为t
bc
，t
bc
所述传感器外参；
19.其中，平移矩阵t＝[0 0 h]，h为所述车身坐标系和所述平面坐标系的高度差，r为所述车身坐标系相对所述真实平面坐标系的旋转矩阵，由所述车身位姿参数中的偏航角和俯仰角确定。
[0020]
优选地，所述根据所述传感器获取待定位点的在所述传感器坐标系的感知坐标和所述坐标转换模型计算所述待定位点在所述真实平面坐标系的映射坐标，具体包括：
[0021]
确定所述待定位点在所述理想水平坐标系下的两条理想定位线；两条理想定位线分别为所述理想水平坐标系的坐标原点和所述待定位点相连的第一理想定位线、所述理想水平坐标系的传感器光心和所述待定位点相连的第二理想定位线；
[0022]
根据所述坐标转换模型将所述第一理想定位线转换到所述真实平面坐标系中作为第一真实定位线；
[0023]
根据所述坐标转换模型将所述第二理想定位线转换到所述真实平面坐标系中作为第二真实定位线；
[0024]
将所述第一真实定位线和所述第二真实定位线的交点确定为所述映射坐标。
[0025]
作为上述方案的改进，所述第一真实定位线的转换过程具体包括：
[0026]
根据所述理想水平坐标系到所述真实平面坐标系的旋转角度将所述理想水平坐标系的坐标原点和所述感知坐标相连的第一理想定位线旋转到所述真实平面坐标系中过坐标原点的第一真实定位线。
[0027]
优选地，所述第二真实定位线的确定过程包括：
[0028]
根据所述坐标转换模型计算所述传感器坐标系到所述真实平面坐标系的传感器-真实坐标变换矩阵
[0029]
根据所述传感器-真实坐标变换矩阵将所述待定位点在传感器坐标系的感知坐标p
′c转换为在所述真实平面坐标系中第一转换坐标p
gr
＝t
grc
p
′c；
[0030]
根据所述坐标转换模型中的车身-真实坐标变换矩阵t
grb
将所述传感器光心在所述车身坐标系的坐标cb转换为在所述真实平面坐标系中第二转换坐标c
gr
＝t
grb
cb；
[0031]
将所述第一转换坐标p
gr
和所述第二转换坐标c
gr
的连线作为所述第二真实定位线；
[0032]
其中，所述理想水平坐标系到所述传感器坐标系的理想-传感器坐标变换矩阵所述理想水平坐标系到所述真实平面坐标系的理想-真实坐标变换矩阵t
grg
＝t
grb
t
bg
；t
bc
为所述传感器坐标系到所述车身坐标系的传感器-车身坐标变换矩阵，t
bg
为所述理想水平坐标系到车身平面所在的车身坐标系的理想-车身坐标变换矩阵，t
grb
为所述车身坐标系到所述真实平面坐标系的车身-真实坐标变换矩阵。
[0033]
作为上述方案的并列实施方案，所述第二真实定位线的确定过程包括：
[0034]
根据所述坐标转换模型计算所述传感器坐标系到所述真实平面坐标系的传感器-真实坐标变换矩阵
[0035]
根据所述传感器-真实坐标变换矩阵将所述待定位点在传感器坐标系的感知坐标p
′c转换为在所述真实平面坐标系中第一转换坐标p
gr
＝t
grc
p
′c；
[0036]
计算所述待定位点的感知坐标p
′c在所述传感器坐标系的归一化平面的归一化坐标pc；
[0037]
根据所述传感器-真实坐标变换矩阵t
grc
将所述归一化坐标pc转化到为在所述真实平面坐标系中归一转换坐标p
cr
＝t
grc
pc；
[0038]
将所述归一转换坐标p
cr
和所述第一转换坐标p
gr
的连线作为所述第二真实定位线；
[0039]
其中，p
′c＝[x y z]
t
，pc＝[x/z y/z 1]
t
，所述理想水平坐标系到所述传感器坐标系的理想-传感器坐标变换矩阵所述理想水平坐标系到所述真实平面坐标系的理想-真实坐标变换矩阵t
grg
＝t
grb
t
bg
；t
bc
为所述传感器坐标系到所述车身坐标系的传感器-车身坐标变换矩阵，t
bg
为所述理想水平坐标系到车身平面所在的车身坐标系的理想-车身坐标变换矩阵，t
grb
为所述车身坐标系到所述真实平面坐标系的车身-真实坐标变换矩阵。
[0040]
作为上述方案的并列实施方案，所述第二真实定位线的确定过程包括：
[0041]
根据所述坐标转换模型计算所述传感器坐标系到所述真实平面坐标系的传感器-真实坐标变换矩阵
[0042]
计算所述待定位点的感知坐标在所述传感器坐标系的归一化平面的归一化坐标pc；
[0043]
根据所述传感器-真实坐标变换矩阵t
grc
将所述归一化坐标pc转化到为在所述真实平面坐标系中归一转换坐标p
cr
＝t
grc
pc；
[0044]
根据所述坐标转换模型中的车身-真实坐标变换矩阵t
grb
将所述传感器光心在所述车身坐标系的坐标cb转换为在所述真实平面坐标系中第二转换坐标c
gr
＝t
grb
cb；
[0045]
将所述归一转换坐标p
cr
和所述第二转换坐标c
gr
的连线作为所述第二真实定位线；
[0046]
其中，p
′c＝[x y z]
t
，pc＝[x/z y/z 1]
t
，所述理想水平坐标系到所述传感器坐标系的理想-传感器坐标变换矩阵所述理想水平坐标系到所述真实平面坐标系的理想-真实坐标变换矩阵t
grg
＝t
grb
t
bg
；t
bc
为所述传感器坐标系到所述车身坐标系的传感器-车身坐标变换矩阵，t
bg
为所述理想水平坐标系到车身平面所在的车身坐标系的理想-车身坐标变换矩阵，t
grb
为所述车身坐标系到所述真实平面坐标系的车身-真实坐标变换矩阵。
[0047]
本发明另一实施例还提供一种基于车身位姿的环境感知方法，所述方法包括：
[0048]
根据所述坐标转换模型分别计算所述传感器坐标系中n个待定位点的感知坐标到在所述真实平面坐标系的对应n个映射坐标，得到n个感知坐标和对应的映射坐标的点对组
成的点对组，n为不小于4的整数；
[0049]
根据所述点对组计算所述传感器坐标系到所述真实平面坐标系的坐标映射方程；
[0050]
根据所述坐标映射方程将传感器获取的图像映射到真实平面中。
[0051]
优选地，根据所述点对组计算所述传感器坐标系到所述真实平面坐标系的坐标映射方程，具体包括：
[0052]
构建由所述传感器坐标系到所述真实平面坐标系归一化坐标转换的8自由度的转换矩阵；
[0053]
根据所述点对组求解所述转换矩阵中的自由度参数；
[0054]
根据计算出的自由度参数对所述转换矩阵进行计算，得到所述坐标映射方程；
[0055]
其中，所述坐标映射方程为其中，所述坐标映射方程为为所述传感器坐标系中的归一化坐标转化到所述真实平面坐标系中的归一化坐标；所述转换矩阵为所述转换矩阵的自由度参数的矩阵形式表示为自由度参数的矩阵形式中对应参数(u1，v1，1)(u2，v2，1)、(u3，v3，1)(u4，v4，1)、(u5，v5，1)(u6，v6，1)和(u7，v7，1)(u8，v8，1)分别为所述点对组中的四组归一化坐标，s为二维齐次坐标归一化的尺度因子，h9为归一化参数。
[0056]
优选地，所述方法还包括：
[0057]
计算车辆配置的不同传感器的坐标映射方程；
[0058]
根据不同传感器的坐标映射方程分别将对应传感器获取的图像映射为真实平面下的真实图像；
[0059]
将映射后的多幅真实图像进行拼接，得到真实平面下的360度全景影像。
[0060]
作为一种优选方案，所述方法还包括：
[0061]
根据所述坐标映射方程将车辆配置的传感器获取的地面语义信息的坐标位置映射为真实平面下的真实地面语义信息；
[0062]
所述地面语义信息包括车位线和/或车道线。
[0063]
与现有技术相比，本发明通过综合车辆的车身位姿参数，在进行坐标映射时，考虑车身所述平面和真实平面存在的偏差，根据车身位姿参数和传感器外参计算坐标系间的坐标转换模型，根据确定的坐标转换模型能够将传感器坐标系下的感知坐标映射到真实平面坐标系中，实现准确的环境感知。克服现有技术中忽略了车辆处于的真实平面和理想平面的偏差，会导致映射的位置出现偏差，提高环境感知的精度。
附图说明
[0064]
图1是本发明实施例提供的一种基于车身位姿的环境感知方法的流程示意图；
[0065]
图2是本发明实施例提供的坐标系的位置示意图；
[0066]
图3是本发明实施例提供的待定位点的理想定位线的位置示意图；
[0067]
图4是本发明实施提供的待定位点的真实定位线的位置示意图；
[0068]
图5本发明另一实施例中一种基于车身位姿的环境感知方法的流程示意图；
[0069]
图6是本发明实施例提供的一种车辆环境感知的感知图像示意图。
具体实施方式
[0070]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0071]
参见图1，是本发明实施例提供的一种基于车身位姿的环境感知方法的流程示意图，所述方法包括步骤s1～s2：
[0072]
s1，通过获取车辆的车身位姿参数和车辆传感器标定的传感器外参，构建预设的理想水平坐标系、传感器坐标系和以车辆所在的真实平面建立的真实平面坐标系间的坐标转换模型；
[0073]
s2，根据所述传感器获取待定位点在所述传感器坐标系的感知坐标和所述坐标转换模型计算所述待定位点在所述真实平面坐标系的映射坐标。
[0074]
在本实施例具体实施时，在车辆进行环境感知时需要先根据车辆配置的位姿感知模组识别车辆的车身位姿参数，位姿感知模组包括定位系统、惯性测量单元等，能够测量车辆当前所处位置和姿态，即车辆当前所在的真实平面，以及车辆相对真实平面的倾斜信息，根据车身位姿参数能够确定以车身平面的车身坐标系与以理想水平坐标系、及车辆所在的真实平面构建的真实平面坐标系之间的坐标转换；
[0075]
车辆还配置有感知外部待定位点的传感器，传感器安装在车身的位置以及对应的角度对应车辆的传感器外参，根据传感器外参能够构建传感器坐标系与车辆的车身坐标系之间的坐标转换；
[0076]
传感器包括各种用于环境探测的传感器，包括相机和雷达传感器等。
[0077]
根据车身位姿参数和传感器外参，构建预设的理想水平坐标系、传感器坐标系和以车辆所在的真实平面建立的真实平面坐标系间的坐标转换模型；坐标转换模型中用于各个坐标系的坐标转换计算。
[0078]
需要说明的是，在本实施例中，理想水平坐标系、真实平面坐标系、传感器坐标系和车身坐标系的具体构建的方式以坐标系定义位置，理想水平坐标系为以理想水平面为xy平面展开，真实平面坐标系为以真实平面为xy平面展开，车身坐标系以车辆底盘所在平面为xy平面展开，传感器坐标系以与传感器探测方向中心垂直的平面为xy平面；所述四个坐标系可以分别以车身的四个不同位置作为坐标原点建立；
[0079]
需要说明的是，坐标系具体构建位置不影响方案具体实施，坐标系构建后均可根据车身位姿参数和传感器外参确定坐标系的坐标转换模型；
[0080]
根据车辆配置的传感器获取待定位点在所述传感器坐标系的感知坐标，现有技术中，往往通过在所述传感器坐标系的感知坐标，采用固定的坐标映射模型将其转化到特定坐标系下的坐标，作为真实坐标，其中特定坐标系往往以理想水平坐标系确定，这种方法忽略了车辆处于的真实平面和理想平面的偏差，会导致映射的位置出现偏差；
[0081]
因此本方案根据预先确定的坐标转换模型，能够将在所述传感器坐标系的感知坐标进行映射，计算所述待定位点在所述真实平面坐标系的映射坐标。
[0082]
通过综合车辆的车身位姿参数，在进行坐标映射时，考虑车身所述平面和真实平面存在的偏差，根据车身位姿参数和传感器外参计算坐标系间的坐标转换模型，根据确定的坐标转换模型能够将传感器坐标系下的感知坐标映射到真实平面坐标系中，实现准确的环境感知。
[0083]
在本发明提供的又一实施例中，所述真实平面坐标系的构建构成包括：
[0084]
根据所述车身位姿参数相对所述理想水平坐标系建立车身所处平面的车身坐标系；
[0085]
以所述车身坐标系的原点位置为中心，根据所述车身位姿参数中的车身所处真实平面相对水平的偏航角和俯仰角对所述理想水平坐标系进行对应的旋转，得到当前车辆所处真实平面的真实平面坐标系。
[0086]
在本实施例具体实施时，参见图2，是本发明实施例提供的坐标系的位置示意图；
[0087]
以所述车辆的后轴中心b到水平面π的垂足g作为理想水平坐标系的坐标原点，建立理想水平坐标系；
[0088]
将车辆的后轴中心b作为车身坐标系的坐标原点，以车身地面平面作为xy平面建立车身坐标系；
[0089]
需要说明的是，在本实施例中，以车身相对水平面无倾斜去确定车身坐标系，根据所述车身位姿参数中车辆距离水平面的垂线长度h将所述理想水平坐标系向上平移，以确定车身平面所在的车身坐标系；
[0090]
根据车身位姿参数确定车身所处真实平面相对水平平面的倾斜角度，所述倾斜角度包括偏航角和俯仰角，以车辆的后轴中心b为旋转中心，根据倾斜角度对所述理想水平坐标系进行旋转，得到真实平面坐标系，其中，真实平面坐标系的坐标原点为gr，真实平面坐标系的xy平面即车辆所在的真实平面πr；
[0091]
所述传感器坐标系具体为以所述传感器光心为坐标原点c，传感器正向探测方向为z坐标轴确定的，传感器坐标系的归一化平面为πc，根据传感器的安装位置和探测方向，能够确定传感器坐标系和车身坐标系的坐标转换，即标定传感器外参。
[0092]
需要说明的是，在本实施例中提供的图2仅为坐标系位置的在垂直面上的截面图，
仅表示坐标系和车辆的二维分布，在实际执行过程中坐标系为三维坐标系。
[0093]
需要说明的是，本实施例中提供一种优选的坐标系构建的实施例，在其他实施例中，坐标系建立的位置可根据实际情况确定，坐标系确定位置并不影响本方案具体实施。
[0094]
通过将理想水平坐标系进行旋转得到实际平面坐标系，能够简化后续坐标转换的计算过程。
[0095]
在本发明提供的又一实施例中，所述坐标转换模型构建过程包括：
[0096]
根据所述车身位姿参数确定由所述理想水平坐标系到车身平面所在的车身坐标系的理想-车身坐标变换矩阵，以及由所述车身坐标系到所述真实平面坐标系的车身-真实坐标变换矩阵；
[0097]
根据所述传感器外参确定由所述传感器坐标系到所述车身坐标系的传感器-车身坐标变换矩阵；
[0098]
将所述理想-车身坐标变换矩阵、所述车身-真实坐标变换矩阵和所述传感器-车身坐标变换矩阵作为所述坐标转换模型。
[0099]
在本实施例具体实施时，坐标转换模型包括理想-车身坐标变换矩阵、车身-真实坐标变换矩阵和传感器-车身坐标变换矩阵；
[0100]
理想-车身坐标变换矩阵为理想水平坐标系到车身坐标系的坐标变换矩阵，车身位姿参数中包括车身平面相对理想水平面的倾斜参数，又根据在坐标系建立时，车身坐标系和理想水平坐标系的偏移量，计算出理想水平坐标系到车身坐标系的理想-车身坐标变换矩阵；
[0101]
车身-真实坐标变换矩阵为车身坐标系到真实平面坐标系的坐标变换矩阵，车身位姿参数中包括车身平面相对车身所在的真实平面的倾斜参数，又根据在坐标系建立时，车身坐标系和真实平面坐标系的偏移量，计算出车身坐标系到真实平面坐标系的车身-真实坐标变换矩阵；
[0102]
传感器-车身坐标变换矩阵为传感器坐标系到车身坐标系的坐标变换矩阵，传感器在安装时，根据传感器相对车身的安装位置和探测方向，能够确定传感器坐标系和车身坐标系的坐标转换，即标定传感器外参，即车身坐标系到真实平面坐标系的车身-真实坐标变换矩阵。
[0103]
将理想-车身坐标变换矩阵、车身-真实坐标变换矩阵和传感器-车身坐标变换矩阵作为坐标转换模型，根据变换矩阵可逆性，可以推倒出四种坐标系间的变换矩阵，根据变换矩阵进行计算，能够确定传感器获取的感知坐标在真实水平坐标系下的映射坐标。
[0104]
在本发明提供的又一实施例中，所述理想-车身坐标变换矩阵为车身坐标变换矩阵为
[0105]
所述车身-真实坐标变换矩阵为
[0106]
所述传感器-车身坐标变换矩阵为t
bc
，t
bc
所述传感器外参；
[0107]
其中，平移矩阵t＝[0 0 h]，h为所述车身坐标系和所述平面坐标系的高度差，r为
所述车身坐标系相对所述真实平面坐标系的旋转矩阵，由所述车身位姿参数中的偏航角和俯仰角确定。
[0108]
在本实施例具体实施时，参见图2，在图2建立的坐标系位置下，车身坐标系和理想水平坐标系之间仅存在z轴上的偏移量h，因此可确定理想-车身坐标变换矩阵为
[0109]
车身坐标系和真实平面坐标系之间围绕车身坐标系的坐标原点为中心，对车身坐标系向z轴偏移h后的坐标系(即理想平面坐标系)的纯旋转，因此可确定车身-真实坐标变换矩阵为r为所述车身坐标系相对所述真实平面坐标系的旋转矩阵，由所述车身位姿参数中的偏航角和俯仰角确定，用以表征真实平面坐标系相对车身坐标系的旋转过程；平移矩阵t＝[0 0 h]用来表征真实平面坐标系相对车身坐标系的平移过程；
[0110]
传感器-车身坐标变换矩阵即传感器相对车身的位置，可根据传感器的安装位置和探测角度确定，即预先标定的传感器外参t
bc
。
[0111]
通过构建的坐标系模型，能够推倒出坐标系间的坐标转换矩阵。
[0112]
在本发明提供的又一实施例中，所述步骤s2具体包括：
[0113]
确定所述待定位点在所述理想水平坐标系下的两条理想定位线；两条理想定位线分别为所述理想水平坐标系的坐标原点和所述待定位点相连的第一理想定位线、所述理想水平坐标系的传感器光心和所述待定位点相连的第二理想定位线。
[0114]
根据所述坐标转换模型将所述第一理想定位线转换到所述真实平面坐标系中作为第一真实定位线。
[0115]
根据所述坐标转换模型将所述第二理想定位线转换到所述真实平面坐标系中作为第二真实定位线。
[0116]
将所述第一真实定位线和所述第二真实定位线的交点确定为所述映射坐标。
[0117]
在本实施例具体实施时，参见图3，是本发明实施例提供的待定位点的理想定位线的位置示意图。
[0118]
以待定位点的在理想水平坐标系的理想坐标p处于理想水平面上为例说明具体坐标转换过程。
[0119]
确定所述待定位点在理想水平坐标系下的两条理想定位线，分别为第一理想定位线和第二理想定位线。
[0120]
在本实施例中，第一理想定位线为理想坐标p和所述理想水平坐标系的坐标原点g的连线，由于以待定位点的在理想水平坐标系的理想坐标p处于理想水平面，因此第一理想定位线处于水平面π内。
[0121]
第二理想定位线为理想坐标p与理想水平坐标系中的传感器光心c的连线cp。
[0122]
第一理想定位线和第二理想定位线的交点即为待定位点在理想水平坐标系中的位置。
[0123]
在坐标转换过程中，根据所述坐标转换模型将所述第一理想定位线转换到所述真
实平面坐标系中作为第一真实定位线；根据所述坐标转换模型将所述第二理想定位线转换到所述真实平面坐标系中作为第二真实定位线；根据第一真实定位线和第二真实定位线的交点确定待定位点在真实平面坐标系中的映射位置。
[0124]
需要说明的是，在本实施例中，采用两条定位线来实现坐标的映射，在其他实施例中，可采用其他坐标映射方法，例如列出坐标映射矩阵，结合坐标转换模型中的坐标转换矩阵，对坐标映射矩阵的求解，进而确定坐标映射方法。
[0125]
分别对两条定位线进行转换，根据在真实平面坐标系中的定位线的交点，准确实现坐标映射。
[0126]
在发明提供的又一实施例中，所述第一真实定位线的转换过程具体包括：
[0127]
根据所述理想水平坐标系到所述真实平面坐标系的旋转角度将所述理想水平坐标系的坐标原点和所述感知坐标相连的第一理想定位线旋转到所述真实平面坐标系中过坐标原点的第一真实定位线。
[0128]
在本实施例具体实施时，参见图4，是本发明实施提供的待定位点的真实定位线的位置示意图：
[0129]
第一理想定位线gp为经过理想水平坐标系的坐标原点的线段，因此在进行坐标转换后第一真实定位也会经过真实平面坐标系的坐标原点；
[0130]
又因理想水平坐标系和真实平面坐标系为以车身坐标系的坐标原点为旋转中心发生的纯旋转，因此可根据理想水平坐标系和真实平面坐标系的旋转角度将第一理想定位线进行旋转，确定所述真实平面坐标系中过坐标原点的第一真实定位线grp’。
[0131]
第一理想定位线经过坐标原点，因此在进行转换时，可确定第一真实定位线中的一点为坐标原点，可通过旋转确定第一真实定位线。
[0132]
需要说明的是，在本实施例中，第一理想定位线位于理想水平坐标系的水平面内，因此旋转后的第一真实定位线为真实平面坐标系真实平面内的线，在其他实施例中，第一理想定位线不位于理想水平坐标系的水平面内，具体计算方法相同。
[0133]
在本发明提供的又一实施例中，所述第二真实定位线的确定过程包括：
[0134]
根据所述坐标转换模型计算所述传感器坐标系到所述真实平面坐标系的传感器-真实坐标变换矩阵
[0135]
根据所述传感器-真实坐标变换矩阵将所述待定位点在传感器坐标系的感知坐标p
′c转换为在所述真实平面坐标系中第一转换坐标p
gr
＝t
grc
p
′c；
[0136]
根据所述坐标转换模型中的车身-真实坐标变换矩阵t
grb
将所述传感器光心在所述车身坐标系的坐标cb转换为在所述真实平面坐标系中第二转换坐标c
gr
＝t
grb
cb；
[0137]
将所述第一转换坐标p
gr
和所述第二转换坐标c
gr
的连线作为所述第二真实定位线；
[0138]
其中，所述理想水平坐标系到所述传感器坐标系的理想-传感器坐标变换矩阵所述理想水平坐标系到所述真实平面坐标系的理想-真实坐标变换矩阵t
grg
＝t
grb
t
bg
；t
bc
为所述传感器坐标系到所述车身坐标系的传感器-车身坐标变换矩阵，t
bg
为所述理想水平坐标系到车身平面所在的车身坐标系的理想-车身坐标变换矩阵，t
grb
为所述车身坐标系到所述真实平面坐标系的车身-真实坐标变换矩阵。
[0139]
在本实施例具体实施时，所述第二定位线的确定过程具体包括：
[0140]
根据理想水平坐标系到传感器坐标系的理想-传感器坐标变换矩阵以及理想水平坐标系到真实平面坐标系的理想-真实坐标变换矩阵t
grg
＝t
grb
t
bg
计算传感器坐标系到真实平面坐标系的传感器-真实坐标变换矩阵
[0141]
根据所述传感器-真实坐标变换矩阵t
grc
将所述待定位点在传感器坐标系的感知坐标p
′c转换为在所述真实平面坐标系中第一转换坐标p
gr
＝t
grc
p
′c；
[0142]
根据车身-真实坐标变换矩阵t
grb
将所述传感器光心在所述车身坐标系的坐标转换为在所述真实平面坐标系中第二转换坐标c
gr
＝t
grb
cb；
[0143]
将所述第一转换坐标p
gr
和所述第二转换坐标c
gr
的连线作为所述第二真实定位线；
[0144]
将第一真实定位线grp’和第二真实定位线p
grcgr
之间的交点pr的坐标确定为待定位点映射后的映射坐标；
[0145]
其中，c
gr
＝[x
1 y
1 z1]
t
，p
gr
＝[x
2 y
3 z4]
t
，t
bc
为所述传感器坐标系到所述车身坐标系的传感器-车身坐标变换矩阵，t
bg
为所述理想水平坐标系到车身平面所在的车身坐标系的理想-车身坐标变换矩阵，t
grb
为所述车身坐标系到所述真实平面坐标系的车身-真实坐标变换矩阵。
[0146]
通过将传感器光心和感知坐标转化为真实平面坐标系的坐标，确定第二真实定位线，根据第一真实定位线和第二真实定位线，确定待定位点映射后的映射坐标，实现真实的环境感知定位。
[0147]
在本发明提供的又一实施例中，所述第二真实定位线的确定过程包括：
[0148]
根据所述坐标转换模型计算所述传感器坐标系到所述真实平面坐标系的传感器-真实坐标变换矩阵
[0149]
根据所述传感器-真实坐标变换矩阵将所述待定位点在传感器坐标系的感知坐标p
′c转换为在所述真实平面坐标系中第一转换坐标p
gr
＝t
grc
p
′c；
[0150]
计算所述待定位点的感知坐标p
′c在所述传感器坐标系的归一化平面的归一化坐标pc；
[0151]
根据所述传感器-真实坐标变换矩阵t
grc
将所述归一化坐标pc转化到为在所述真实平面坐标系中归一转换坐标p
cr
＝t
grc
pc；
[0152]
将所述归一转换坐标p
cr
和所述第一转换坐标p
gr
的连线作为所述第二真实定位线；
[0153]
其中，p
′c＝[x y z]
t
，pc＝[x/z y/z 1]
t
，所述理想水平坐标系到所述传感器坐标系的理想-传感器坐标变换矩阵所述理想水平坐标系到所述真实平面坐标系的理想-真实坐标变换矩阵t
grg
＝t
grb
t
bg
；t
bc
为所述传感器坐标系到所述车身坐标系的传感器-车身坐标变换矩阵，t
bg
为所述理想水平坐标系到车身平面所在的车身坐标系的理想-车身坐标变换矩阵，t
grb
为所述车身坐标系到所述真实平面坐标系的车身-真实坐标变
换矩阵。
[0154]
在本实施例具体实施时，作为上述实施例的并列实施方式，参见图2，第二理想定位线上的点还包括，待定位点在传感器的归一化平面上的归一化坐标pc，因此在进行第二理想定位线的转换时，可采用归一化坐标pc进行第二真实定位线的定位；
[0155]
具体的，根据理想水平坐标系到传感器坐标系的理想-传感器坐标变换矩阵以及理想水平坐标系到真实平面坐标系的理想-真实坐标变换矩阵t
grg
＝t
grb
t
bg
计算传感器坐标系到真实平面坐标系的传感器-真实坐标变换矩阵
[0156]
根据所述传感器-真实坐标变换矩阵t
grc
将所述待定位点在传感器坐标系的感知坐标p
′c转换为在所述真实平面坐标系中第一转换坐标p
gr
＝t
grc
p
′c；
[0157]
计算所述待定位点的感知坐标p
′c在所述传感器坐标系的归一化平面的归一化坐标pc，所述待定位点在传感器坐标系下的坐标为p
′c＝t
cg
p＝[x y z]
t
，因此在归一化平面归一化后的归一化坐标pc＝[x/z y/z 1]
t
；
[0158]
根据所述传感器-真实坐标变换矩阵t
grc
将所述归一化坐标pc转化到为在所述真实平面坐标系中归一转换坐标p
cr
＝t
grc
pc；
[0159]
将所述归一转换坐标p
cr
和所述第一转换坐标p
gr
的连线作为所述第二真实定位线；
[0160]
根据第二真实定位线和第一真实定位线，实现待定位点在真实平面坐标系的映射。
[0161]
第二真实定位线的确定需要两个点，因此，在第二理想定位线上的点在真实平面坐标系中的坐标均可用于确定第二真实定位线。
[0162]
在本发明提供的又一实施例中，所述第二真实定位线的确定过程包括：
[0163]
根据所述坐标转换模型计算所述传感器坐标系到所述真实平面坐标系的传感器-真实坐标变换矩阵
[0164]
计算所述待定位点的感知坐标在所述传感器坐标系的归一化平面的归一化坐标pc；
[0165]
根据所述传感器-真实坐标变换矩阵t
grc
将所述归一化坐标pc转化到为在所述真实平面坐标系中归一转换坐标p
cr
＝t
grc
pc；
[0166]
根据所述坐标转换模型中的车身-真实坐标变换矩阵t
grb
将所述传感器光心在所述车身坐标系的坐标cb转换为在所述真实平面坐标系中第二转换坐标c
gr
＝t
grb
cb；
[0167]
将所述归一转换坐标p
cr
和所述第二转换坐标c
gr
的连线作为所述第二真实定位线；
[0168]
其中，p
′c＝[x y z]
t
，pc＝[x/z y/z 1]
t
，所述理想水平坐标系到所述传感器坐标系的理想-传感器坐标变换矩阵所述理想水平坐标系到所述真实平面坐标系的理想-真实坐标变换矩阵t
grg
＝t
grb
t
bg
；t
bc
为所述传感器坐标系到所述车身坐标系的传感器-车身坐标变换矩阵，t
bg
为所述理想水平坐标系到车身平面所在的车身坐标系的理想-车身坐标变换矩阵，t
grb
为所述车身坐标系到所述真实平面坐标系的车身-真实坐标变
换矩阵。
[0169]
在本发明实施例具体实施时，作为上述实施例的并列实施方案，参见图2，第二理想定位线上的点还包括，待定位点在传感器的归一化平面上的归一化坐标pc，因此在进行第二理想定位线的转换时，可采用归一化坐标pc进行第二真实定位线的定位；
[0170]
具体的，根据车身-真实坐标变换矩阵t
grb
将所述传感器光心在所述车身坐标系的坐标转换为在所述真实平面坐标系中第二转换坐标c
gr
＝t
grb
cb；
[0171]
计算所述待定位点的感知坐标p
′c在所述传感器坐标系的归一化平面的归一化坐标pc，所述待定位点在传感器坐标系下的坐标为p
′c＝t
cg
p＝[x y z]
t
，因此在归一化平面归一化后的归一化坐标pc＝[x/z y/z 1]
t
；
[0172]
根据所述传感器-真实坐标变换矩阵t
grc
将所述归一化坐标pc转化到为在所述真实平面坐标系中归一转换坐标p
cr
＝t
grc
pc；
[0173]
将所述归一转换坐标p
cr
和所述第二转换坐标c
gr
的连线作为所述第二真实定位线；
[0174]
根据第二真实定位线和第一真实定位线，实现待定位点在真实平面坐标系的映射。
[0175]
参见图4，用于确定第二真实定位线的点包括在真实平面坐标系中的归一转换坐标p
cr
、第二转换坐标c
gr
、第一转换坐标p
gr
；第二真实定位线的确定需要两个点，因此，在第二理想定位线上的点在真实平面坐标系中的坐标均可用于确定第二真实定位线。
[0176]
通过综合车辆的车身位姿参数，在进行坐标映射时，考虑车身所述平面和真实平面存在的偏差，根据车身位姿参数和传感器外参计算坐标系间的坐标转换模型，根据确定的坐标转换模型能够将传感器坐标系下的感知坐标映射到真实平面坐标系中，实现准确的环境感知。克服现有技术中忽略了车辆处于的真实平面和理想平面的偏差，会导致映射的位置出现偏差，提高环境感知的精度。
[0177]
参见图5，是本发明另一实施例中一种基于车身位姿的环境感知方法的流程示意图，所述方法包括步骤s501～s503：
[0178]
s501，通过获取车辆的车身位姿参数和车辆传感器标定的传感器外参，构建预设的理想水平坐标系、传感器坐标系和以车辆所在的真实平面建立的真实平面坐标系间的坐标转换模型；
[0179]
s502，根据所述坐标转换模型分别计算所述传感器坐标系中n个待定位点的感知坐标到在所述真实平面坐标系的对应n个映射坐标，得到n个感知坐标和对应的映射坐标的点对组成的点对组，n为不小于4的整数；
[0180]
s503，根据所述点对组计算所述传感器坐标系到所述真实平面坐标系的坐标映射方程，根据所述坐标映射方程将传感器获取的图像映射到真实平面中。
[0181]
在本实施例具体实施时，在车辆进行环境感知时需要先根据车辆配置的位姿感知模组识别车辆的车身位姿参数，位姿感知模组包括定位系统、惯性测量单元等，能够测量车辆当前所处位置和姿态，即车辆当前所在的真实平面，以及车辆相对真实平面的倾斜信息，根据车身位姿参数能够确定以车身平面的车身坐标系与以理想水平坐标系、及车辆所处真实平面构建的真实平面坐标系之间的坐标转换；
[0182]
车辆还配置有感知外部待定位点的传感器，传感器安装在车身的位置以及对应的
角度对应车辆的传感器外参，根据传感器外参能够构建传感器坐标系与车辆的车身坐标系之间的坐标转换；
[0183]
传感器包括各种用于环境探测的传感器，包括相机和雷达传感器等。
[0184]
根据车身位姿参数和传感器外参，构建预设的理想水平坐标系、传感器坐标系和以车辆所在的真实平面建立的真实平面坐标系间的坐标转换模型；坐标转换模型中用于各个坐标系的坐标转换计算。
[0185]
需要说明的是，在本实施例中，理想水平坐标系、真实平面坐标系、传感器坐标系和车身坐标系的具体构建的方式以坐标系定义位置，理想水平坐标系为以理想水平面为xy平面展开，真实平面坐标系为以真实平面为xy平面展开，车身坐标系以车辆底盘所在平面为xy平面展开，传感器坐标系以与传感器探测方向中心垂直的平面为xy平面，三个坐标系的原点均围绕车辆设置，以便于计算；需要说明的是，坐标系具体构建位置不影响方案具体实施，坐标系构建后均可根据车身位姿参数和传感器外参确定坐标系的坐标转换模型。
[0186]
由于所述传感器坐标系到所述真实平面坐标系之间的坐标映射计算过程需要变换矩阵为8自由度矩阵，因此需要预先计算所述传感器坐标系中至少4个待定位点的感知坐标到在所述真实平面坐标系的对应至少4个映射坐标，得到4个感知坐标和对应的映射坐标的点对组成的点对组；
[0187]
根据所述点对组计算所述传感器坐标系到所述真实平面坐标系的坐标映射方程，根据所述坐标映射方程将传感器获取的图像映射到真实平面中。
[0188]
通过映射坐标计算方法，计算坐标系的映射点对，根据至少四个点对，组成的点对组的坐标，求解坐标系的坐标映射方程，根据坐标映射方程能够直接进行坐标映射，用于对传感器获取的原始图像中的坐标进行变换，修正图像。
[0189]
本实施例中映射坐标点的计算方法采用上述实施例中任一实施例提供的基于车身位姿的环境感知方法计算映射坐标的方法，上述实施例中对映射坐标的实施过程进行了具体说明，在此不作赘述。
[0190]
在本发明提供的又一实施例中，所述步骤s503具体包括：
[0191]
构建由所述传感器坐标系到所述真实平面坐标系归一化坐标转换的8自由度的转换矩阵；
[0192]
根据所述点对组求解所述转换矩阵中的自由度参数；
[0193]
根据计算出的自由度参数对所述转换矩阵进行计算，得到所述坐标映射方程；
[0194]
其中，所述坐标映射方程为其中，所述坐标映射方程为为所述传感器坐标系中的归一化坐标转化到所述真实平面坐标系中的归一化坐标；所述转换矩阵为所述转换矩阵的自由度参数的矩阵形式表示为
自由度参数的矩阵形式中对应参数(u1，v1，1)(u2，v2，1)、(u3，v3，1)(u4，v4，1)、(u5，v5，1)(u6，v6，1)和(u7，v7，1)(u8，v8，1)分别为所述点对组中的四组归一化坐标，s为二维齐次坐标归一化的尺度因子，h9为归一化参数。
[0195]
在本实施例具体实施时，将传感器坐标系中的归一化坐标转化到所述真实平面坐标系中的归一化坐标的线性映射可用一个8自由度的矩阵来描述，具体为：
[0196][0197]
其中，h1～h9为转换矩阵参数，s为二维齐次坐标归一化的尺度因子，h9为归一化参数。
[0198]
对以上计算公式进行计算得到坐标映射方程
[0199]
其中h1～h8的自由度参数需要根据所述点对组中的四组归一化坐标进行求解确定，具体为：
[0200]
因此自由度参数
[0201]
根据四对点对(u1，v1，1)(u2，v2，1)、(u3，v3，1)(u4，v4，1)、(u5，v5，1)(u6，v6，1)和(u7，v7，1)(u8，v8，1)进行计算，能够确定坐标映射方程；
[0202]
根据坐标映射方程，能够对图像中的点直接进行映射计算，可用于直接进行图像的映射计算。
[0203]
在本发明提供的又一实施例中，所述方法还包括：
[0204]
计算车辆配置的不同传感器的坐标映射方程；
[0205]
根据不同传感器的坐标映射方程分别将对应传感器获取的图像映射为真实平面下的真实图像；
[0206]
将映射后的多幅真实图像进行拼接，得到真实平面下的360度全景影像。在本实施例具体实施时，可通过多个传感器分别获取车辆周围不同角度范围的传感器图像，每一传感器对应不同的传感器外参，分别计算各个传感器的坐标映射方程；
[0207]
根据不同的坐标映射方程对不同传感器获取的图像进行映射计算，将不同传感器获取的图像映射为真实平面下的多幅真实图像；
[0208]
将映射后的多幅真实图像进行拼接，得到真实平面下的360度全景影像。
[0209]
根据不同传感器的坐标映射方程，对不同传感器获取的图像进行映射到真实平面，进行拼接，获得车身的360度全景影像，提高车辆360度全景影像的精确度。
[0210]
在本发明提供的又一实施例中，根据所述坐标映射方程将车辆配置的传感器获取的地面语义信息的坐标位置映射为真实平面下的真实地面语义信息；
[0211]
所述地面语义信息包括车位线和/或车道线。
[0212]
在本实施例具体实施时，参见图6，是本发明实施例提供的一种车辆环境感知的感知图像示意图；
[0213]
通过车辆配置的传感器获取的地面语义信息，地面语义信息包括车位线和/或车道线，在本实施例中以车道线具体说明，传感器获取的车道线将其转换为理想水平坐标系中的得到第一车道线a，第二车道线为车辆处于理想水平状态下的车道线位置，但是受位姿影响，第二车道线并不准确；
[0214]
通过传感器的坐标映射方程将传感器获取的车道线映射到真实平面中，得到第二车道线b。
[0215]
通过坐标映射方程可以直接对获取的地面语义信息进行单独映射，能够高效提取，映射车道线和车位线，实现地面语义的准确定位。
[0216]
应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。