基于双目视觉的车身垂向及俯仰运动信息估算系统及方法

1.本发明属于汽车智能感知与控制技术领域，具体涉及一种基于双目视觉的车身垂向及俯仰运动信息估算系统及估算方法。


背景技术：

2.预瞄悬架是将计算机视觉与悬架控制技术相结合的新型智能车辆控制技术，该技术的策略是利用车载视觉传感器检测车辆前方路面的高程值，再将路面高程值作为输入变量控制汽车悬架的相关参数，以提升汽车行驶时的舒适性与操纵特性。然而，汽车运行时自身的垂向振动以及俯仰运动会对路面高程值的识别精度产生较大影响。
3.现有解决方案多通过增加车身状态传感器来抵消车身自身的运动，这样一方面会提高汽车的制造成本，另一方面由于外加传感器自身的噪声与信息融合时的时延会使控制效果大打折扣。另外，在预瞄悬架控制时，还需要参考车身的垂向速度、加速度信息以及俯仰角速度、角加速度信息，此时还需额外增加其他传感器，增加造车成本。基于上述问题，如何在不额外增加传感器情况下，仅利用现有的视觉传感器测量车辆的垂向及俯仰运动信息成为当前汽车视觉及预瞄悬架控制研究的重点之一。
4.双目摄像机采样精度较高、成本低廉，是当前智能汽车经常采用的传感器元件，因此，如能考虑建立一个通过车载双目摄像机估算的车辆垂向高度信息与俯仰角度信息的估算系统及估算方法，对汽车主动悬架的控制以及汽车的平顺性、操纵性、舒适性和安全性至关重要。


技术实现要素：

5.本发明的目的就在于提供一种基于双目视觉的车身垂向及俯仰运动信息估算系统，还提供一种基于双目视觉的车身垂向及俯仰运动信息估算方法，以解决现有技术中无法通过车载视觉系统计算车身垂向、俯仰运动姿态，导致预瞄悬架控制需要增加其他传感器进而提高成本的问题。
6.本发明的目的是通过以下技术方案实现的：
7.一种基于双目视觉的车身垂向及俯仰运动信息估算系统，包括图像信息捕捉模块1、双目信息处理模块2、单目信息处理模块3、车身垂向及俯仰运动信息估算模块4和信息显示模块5；
8.所述图像信息捕捉模块1，用于捕捉行车前方道路信息，包括双目摄像机11、图像采集模块12和畸变矫正模块13；所述双目摄像机11将原始图像信息发送至图像采集模块12进行记录后再发送至畸变矫正模块13；
9.所述双目信息处理模块2，用于处理左、右摄像头的原始图像信息，提取特征点，完成特征点的匹配，计算特征点相对于双目相机的位置并记录特征点的像素坐标；所述双目信息处理模块2，包括特征点提取模块21、特征点匹配模块22和特征点位置计算模块23；
10.所述畸变矫正模块13将畸变矫正后的图像发送至特征点提取模块21提取特征点，
经特征点匹配模块22获取图像中一致的特征点后分别经特征点位置计算模块23和单目信息处理模块3处理后再发送至车身垂向及俯仰运动信息估算模块4；
11.所述单目信息处理模块3，用于记录获取的特征点在左摄像机的像素坐标；
12.所述车身垂向及俯仰运动信息估算模块4，用于计算车身的垂向运动信息与俯仰运动信息，包括车身垂向位置及俯仰角度估算模块41、车身垂向速度及俯仰角速度估算模块42以及车身垂向加速度及俯仰角加速度估算模块43；
13.所述车身垂向及俯仰运动信息估算模块4将计算的车辆当前行驶时的垂向位置及俯仰角度、车辆当前行驶时的垂向速度及俯仰角速度以及车辆当前行驶时的垂向加速度及俯仰角加速度信息发送至信息显示模块5；
14.所述信息显示模块5，用于将计算得到的垂向高度、垂向速度、垂向加速度、俯仰角度、俯仰角速度、俯仰角加速度值显示在中控显示器51上。
15.进一步地，所述双目摄像机11，用于通过摄像机的左、右摄像头获取车辆前方原始图像信息；所述图像采集模块12，用于采集、记录左、右摄像头的原始图像信息；所述畸变矫正模块13，用于将获取的图像进行畸变矫正。
16.进一步地，所述特征点提取模块21，用于在原始图像信息中提取特征点；所述特征点匹配模块22，用于匹配左右图像的特征点，得到图像中一致的特征点；所述特征点位置计算模块23，用于计算获取的特征点相对于双目相机的坐标。
17.进一步地，所述车身垂向位置及俯仰角度估算模块41，通过结合双目信息处理模块和单目信息处理模块的数据，来计算车辆当前行驶时的垂向位置及俯仰角度。
18.进一步地，所述车身垂向速度及俯仰角速度估算模块42，通过结合车身的垂向位置、俯仰角度，计算车辆当前行驶时的垂向速度及俯仰角速度；
19.进一步地，所述车身垂向加速度及俯仰角加速度估算模块43，通过结合车身的垂向速度、俯仰角速度，计算车辆当前行驶时的垂向加速度及俯仰角加速度。
20.一种基于双目视觉的车身垂向及俯仰运动信息估算方法，包括以下步骤：
21.步骤s1、利用双目摄相机11的左、右摄像机获取车辆前方的原始图像，通过图像采集模块12采集、记录原始图像信息，并通过畸变矫正模块13对获取的图像进行畸变矫正；
22.步骤s2、利用双目信息处理模块2，用于处理左、右摄像头的原始图像信息，提取特征点，完成特征点的匹配，计算特征点相对于双目摄像机11的位置坐标，得到双目相机车辆姿态方程组；
23.步骤s3、利用单目信息处理模块3，记录特征点在左摄像头像素坐标系内的坐标，得到单目相机车辆姿态方程组；
24.步骤s4、利用车身垂向及俯仰运动信息估算模块4，通过解算车辆姿态方程组计算车身的垂向运动信息与俯仰运动信息；
25.步骤s5、利用信息显示模块5将计算得到的垂向高度、垂向速度、垂向加速度、俯仰角度、俯仰角速度、俯仰角加速度值显示在中控显示器51上。
26.与现有技术相比，本发明的有益效果是：
27.本发明利用双目视觉传感器来获取车辆的姿态，无需额外增加汽车垂向加速度传感器与转角传感器即可实时准确地测算车身垂向高度、垂向速度、垂向加速度、俯仰角度、俯仰角速度、俯仰角加速度，为汽车主动悬架、半主动悬架控制提供信息参考。
附图说明
28.为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
29.图1是本发明车身垂向高度、俯仰角度估算系统的结构框图；
30.图2是本发明车身垂向高度、俯仰角度估算方法的流程图；
31.图3是本发明的行车示意图；
32.图4是双目相机xoz平面成像示意图；
33.图5是双目相机y轴投影比例示意图。
34.图中，1.图像信息捕捉模块2.双目信息处理模块3.单目信息处理模块4.车身垂向及俯仰运动信息估算模块5.信息显示模块11.双目摄像机12.图像采集模块13.畸变矫正模块21.特征点提取模块22.特征点匹配模块23.特征点位置计算模块41.车身垂向位置及俯仰角度估算模块42.车身垂向速度及俯仰角速度估算模块43.车身垂向加速度及俯仰角加速度估算模块51.中控显示器。
具体实施方式
35.下面结合实施例对本发明作进一步说明：
36.下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。
37.应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本发明的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
38.本发明运用双目摄像机来获取车辆前方图像信息，通过提取、匹配左右摄像头采集的图像的特征点，进而计算获取的特征点相对于双目相机的位置。通过记录特征点在左摄像机的像素坐标，结合单目相机像素坐标转换世界坐标，计算出当前车辆的垂向及俯仰运动信息。
39.实施例1
40.具体地，如图1所示，本发明基于双目视觉的车身垂向及俯仰运动信息估算系统，包括图像信息捕捉模块1、双目信息处理模块2、单目信息处理模块3、车身垂向及俯仰运动信息估算模块4以及信息显示模块5。所述图像信息捕捉模块1，用于捕捉行车前方道路信息，包括双目摄像机11、图像采集模块12和畸变矫正模块13。
41.其中，所述双目摄像机11，用于通过摄像机的左、右摄像头获取车辆前方原始图像信息；所述图像采集模块12，用于采集、记录左、右摄像头的原始图像信息；所述畸变矫正模块13，用于将获取的图像进行畸变矫正。
42.所述双目摄像机11将原始图像信息发送至图像采集模块12进行记录后再发送至畸变矫正模块13。
43.所述双目信息处理模块2，用于处理左、右摄像头的原始图像信息，提取特征点，完
成特征点的匹配，计算特征点相对于双目相机的位置并记录特征点的像素坐标，具体包括特征点提取模块21、特征点匹配模块22和特征点位置计算模块23。
44.其中，所述特征点提取模块21，用于在原始图像信息中提取特征点；所述特征点匹配模块22，用于匹配左右图像的特征点，得到图像中一致的特征点；所述特征点位置计算模块23，用于计算获取的特征点相对于双目相机的坐标。
45.所述畸变矫正模块13将畸变矫正后的图像发送至特征点提取模块21提取特征点，经特征点匹配模块22获取图像中一致的特征点后分别经特征点位置计算模块23和单目信息处理模块3处理后再发送至车身垂向及俯仰运动信息估算模块4。
46.所述单目信息处理模块3，用于记录获取的特征点在左摄像机的像素坐标。
47.所述车身垂向及俯仰运动信息估算模块4，用于计算车身的垂向运动信息与俯仰运动信息，具体包括：车身垂向位置及俯仰角度估算模块41、车身垂向速度及俯仰角速度估算模块42以及车身垂向加速度及俯仰角加速度估算模块43。
48.其中，所述车身垂向位置及俯仰角度估算模块41，通过结合双目信息处理模块和单目信息处理模块的数据，来计算车辆当前行驶时的垂向位置及俯仰角度；
49.所述车身垂向速度及俯仰角速度估算模块42，通过结合车身的垂向位置、俯仰角度，计算车辆当前行驶时的垂向速度及俯仰角速度；
50.所述车身垂向加速度及俯仰角加速度估算模块43，通过结合车身的垂向速度、俯仰角速度，计算车辆当前行驶时的垂向加速度及俯仰角加速度。
51.下面结合附图与公式对上述方法进行进一步说明：
52.双目相机部分：
53.如图4、图5所示，双目相机测距时满足如下公式：
[0054][0055][0056][0057]
其中，x、y、z是特征点的相机坐标，f是相机焦距，xl与xr分别是特征点在左、右图像坐标系上的像点与光轴的距离的x轴的投影(单位mm)，yl与yr分别是特征点在左、右图像坐标系上的像点与光轴的距离的y轴的投影(单位mm)，xl、xr、yl、yr可通过像素坐标与图像坐标转化得到，b是双目相机基线距离，
[0058]
设则由双目测距原理可得：
[0059][0060][0061][0062]
由坐标转换原理可知，世界坐标系转换至相机坐标系满足以下关系：
[0063][0064]
其中，(x
cl
,y
cl
,z
cl
)是特征点在双目相机左摄像机的相机坐标系的坐标，可由双目测距原理计算得到；(xw,yw,zw)是特征点在世界坐标系的坐标；(t
cx
,t
cy
,t
cz
)是相机位移向量；r为旋转重合矩阵，是不考虑相机绕自身轴线旋转的情况下，由世界坐标系转换到相机坐标系的转换矩阵；rc是存在相机坐标系绕自身轴线旋转情况下的转换矩阵。
[0065]
特别地，左相机坐标系位于车辆前方，世界坐标系的原点位于车辆前方路面，且左相机坐标系的原点位于世界坐标系zw轴上，相机高度为h，相机仅绕自身xc轴旋转，旋转角度为θ。由理论力学知识可知，当相机固定在车辆上时，相机的旋转角度θ即为车身俯仰角度，相机高度h反映了车辆的垂向位置，则
[0066]
t
cx
＝t
cz
＝0
[0067]
t
cy
＝h
[0068][0069][0070]
由矩阵变换可得：
[0071][0072]
进而得到双目相机车辆姿态方程组：
[0073][0074]
单目相机部分：
[0075]
单目相机(双目相机的左摄像机)需要进行标定，确定其初始高度h0与绕自身x轴的初始角度θ0。
[0076]
由单目信息处理模块3记录获取的特征点在左摄像机的像素坐标反推特征点在世界坐标系位置。
[0077]
如图3所示，xw、yw、zw是世界坐标系的坐标轴，xc、yc、zc是相机坐标系的坐标轴(像素坐标为二维平面，认为畸变矫正后的像素坐标与相机坐标平行，用μ与ν表示)。
[0078]
物体的世界坐标转换至像素坐标的过程满足如下关系式：
[0079]
[0080]
其中，s为比例系数，k为相机的内参矩阵，由相机特性决定，不随相机的位置改变，可由标定测出。r为相机坐标系的旋转矩阵，t为相机坐标系的平移矩阵。
[0081][0082]
其中，f为相机焦距，(μ0,ν0)是图像坐标系中心点到像素坐标系中心点的偏移量，dx是x向一个像素的长度，dy是y向一个像素的长度。
[0083]
物体的世界坐标转换至像素坐标的过程还可由下式表示：
[0084][0085]
其中，r为旋转重合矩阵，是不考虑相机绕自身轴线旋转的情况下，由世界坐标系转换到相机坐标系的转换矩阵，tc是世界坐标系原点在相机坐标系下的位置，rc是存在相机坐标系绕自身轴线旋转情况下的转换矩阵。k是相机内参矩阵。
[0086][0087]
由上述可得：
[0088][0089]
因此，物体的世界坐标与物体成像对应的像素坐标之间的函数关系包含了h与θ两个变量。
[0090]
由像素坐标系反推世界坐标，可得：
[0091][0092]
其中k、r均为已知矩阵。
[0093]
[0094][0095]
设
[0096][0097]
为抵消比例系数s的影响，设
[0098][0099]
则有
[0100][0101][0102]
设
[0103][0104][0105][0106][0107]
对于确定参数的单目相机而言，确定特征点即可确定其像素坐标(μ,ν)，则g、r、g均为确定的值。
[0108]
因此，得到单目相机车辆姿态方程组：
[0109][0110]
在车身垂向及俯仰运动信息估算模块4中联立双目、单目车辆姿态方程组，有：
[0111][0112]
通过该方程组解出θ、h、zw即可得到当前帧的相机姿态θ与h。
[0113]
对于路面上的特征点，其zw＝0。
[0114]
h为当前车辆垂向高度。
[0115]
θ-θ0是车辆俯仰角度。
[0116]
通过对比连续两帧的姿态θ与h，结合时间间隔即可计算出车辆的俯仰角速度、垂向速度；
[0117]
通过对比连续两帧的俯仰角速度与垂向速度，结合时间间隔即可计算出车辆的俯仰角加速度、垂向加速度。
[0118]
所述车身垂向及俯仰运动信息估算模块4将计算的车辆当前行驶时的垂向位置及俯仰角度、车辆当前行驶时的垂向速度及俯仰角速度以及车辆当前行驶时的垂向加速度及俯仰角加速度信息发送至信息显示模块5。
[0119]
所述信息显示模块5，用于将计算得到的垂向高度、垂向速度、垂向加速度、俯仰角度、俯仰角速度、俯仰角加速度值显示在中控显示器51上。
[0120]
实施例2
[0121]
如图2所示，本发明基于双目视觉的车身垂向及俯仰运动信息估算方法，具体包括以下步骤：
[0122]
步骤s1、利用双目摄相机11的左、右摄像机获取车辆前方的原始图像，通过图像采集模块12采集、记录原始图像信息，并通过畸变矫正模块13对获取的图像进行畸变矫正。
[0123]
步骤s2、利用双目信息处理模块2，用于处理左、右摄像头的原始图像信息，提取特征点，完成特征点的匹配，计算特征点相对于双目摄像机11的位置坐标，得到双目相机车辆姿态方程组。
[0124]
步骤s3、利用单目信息处理模块3，记录特征点在左摄像头像素坐标系内的坐标，得到单目相机车辆姿态方程组。
[0125]
步骤s4、利用车身垂向及俯仰运动信息估算模块4，通过解算车辆姿态方程组计算车身的垂向运动信息与俯仰运动信息。
[0126]
步骤s5、利用信息显示模块5将计算得到的垂向高度、垂向速度、垂向加速度、俯仰角度、俯仰角速度、俯仰角加速度值显示在中控显示器51上。
[0127]
注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。

技术特征：
1.一种基于双目视觉的车身垂向及俯仰运动信息估算系统，其特征在于：包括图像信息捕捉模块(1)、双目信息处理模块(2)、单目信息处理模块(3)、车身垂向及俯仰运动信息估算模块(4)和信息显示模块(5)；所述图像信息捕捉模块(1)，用于捕捉行车前方道路信息，包括双目摄像机(11)、图像采集模块(12)和畸变矫正模块(13)；所述双目摄像机(11)将原始图像信息发送至图像采集模块(12)进行记录后再发送至畸变矫正模块(13)；所述双目信息处理模块(2)，用于处理左、右摄像头的原始图像信息，提取特征点，完成特征点的匹配，计算特征点相对于双目相机的位置并记录特征点的像素坐标；所述双目信息处理模块(2)，包括特征点提取模块(21)、特征点匹配模块(22)和特征点位置计算模块(23)；所述畸变矫正模块(13)将畸变矫正后的图像发送至特征点提取模块(21)提取特征点，经特征点匹配模块(22)获取图像中一致的特征点后分别经特征点位置计算模块(23)和单目信息处理模块(3)处理后再发送至车身垂向及俯仰运动信息估算模块(4)；所述单目信息处理模块(3)，用于记录获取的特征点在左摄像机的像素坐标；所述车身垂向及俯仰运动信息估算模块(4)，用于计算车身的垂向运动信息与俯仰运动信息，包括车身垂向位置及俯仰角度估算模块(41)、车身垂向速度及俯仰角速度估算模块(42)以及车身垂向加速度及俯仰角加速度估算模块(43)；所述车身垂向及俯仰运动信息估算模块(4)将计算的车辆当前行驶时的垂向位置及俯仰角度、车辆当前行驶时的垂向速度及俯仰角速度以及车辆当前行驶时的垂向加速度及俯仰角加速度信息发送至信息显示模块(5)；所述信息显示模块(5)，用于将计算得到的垂向高度、垂向速度、垂向加速度、俯仰角度、俯仰角速度、俯仰角加速度值显示在中控显示器(51)上。2.根据权利要求1所述的一种基于双目视觉的车身垂向及俯仰运动信息估算系统，其特征在于：所述双目摄像机(11)，用于通过摄像机的左、右摄像头获取车辆前方原始图像信息；所述图像采集模块(12)，用于采集、记录左、右摄像头的原始图像信息；所述畸变矫正模块(13)，用于将获取的图像进行畸变矫正。3.根据权利要求1所述的一种基于双目视觉的车身垂向及俯仰运动信息估算系统，其特征在于：所述特征点提取模块(21)，用于在原始图像信息中提取特征点；所述特征点匹配模块(22)，用于匹配左右图像的特征点，得到图像中一致的特征点；所述特征点位置计算模块(23)，用于计算获取的特征点相对于双目相机的坐标。4.根据权利要求1所述的一种基于双目视觉的车身垂向及俯仰运动信息估算系统，其特征在于：所述车身垂向位置及俯仰角度估算模块(41)，通过结合双目信息处理模块和单目信息处理模块的数据，来计算车辆当前行驶时的垂向位置及俯仰角度。5.根据权利要求1所述的一种基于双目视觉的车身垂向及俯仰运动信息估算系统，其特征在于：所述车身垂向速度及俯仰角速度估算模块(42)，通过结合车身的垂向位置、俯仰角度，计算车辆当前行驶时的垂向速度及俯仰角速度。6.根据权利要求1所述的一种基于双目视觉的车身垂向及俯仰运动信息估算系统，其特征在于：所述车身垂向加速度及俯仰角加速度估算模块(43)，通过结合车身的垂向速度、俯仰角速度，计算车辆当前行驶时的垂向加速度及俯仰角加速度。
7.一种基于双目视觉的车身垂向及俯仰运动信息估算方法，其特征在于，包括以下步骤：步骤s1、利用双目摄相机(11)的左、右摄像机获取车辆前方的原始图像，通过图像采集模块(12)采集、记录原始图像信息，并通过畸变矫正模块13对获取的图像进行畸变矫正；步骤s2、利用双目信息处理模块(2)，用于处理左、右摄像头的原始图像信息，提取特征点，完成特征点的匹配，计算特征点相对于双目摄像机(11)的位置坐标，得到双目相机车辆姿态方程组；步骤s3、利用单目信息处理模块(3)，记录特征点在左摄像头像素坐标系内的坐标，得到单目相机车辆姿态方程组；步骤s4、利用车身垂向及俯仰运动信息估算模块(4)，通过解算车辆姿态方程组计算车身的垂向运动信息与俯仰运动信息；步骤s5、利用信息显示模块(5)将计算得到的垂向高度、垂向速度、垂向加速度、俯仰角度、俯仰角速度、俯仰角加速度值显示在中控显示器(51)上。

技术总结
本发明涉及一种基于双目视觉的车身垂向及俯仰运动信息估算系统，系统包括用于捕捉行车前方道路信息的图像信息捕捉模块、用于计算特征点相对于双目相机的位置并记录特征点的像素坐标的双目信息处理模块、用于记录获取的特征点在左摄像机像素坐标的单目信息处理模块、用于计算车身的垂向运动与俯仰运动信息的信息估算模块和用于将计算得到的垂向高度、垂向速度、垂向加速度、俯仰角度、俯仰角速度、俯仰角加速度值显示在中控显示器上的信息显示模块；本发明利用双目视觉传感器来获取车辆的姿态即可实时准确地测算车身垂向高度、垂向速度、垂向加速度、俯仰角度、俯仰角速度、俯仰角加速度，为汽车主动悬架、半主动悬架控制提供信息参考。信息参考。信息参考。


技术研发人员：史文库 吴骁 陈志勇
受保护的技术使用者：吉林大学
技术研发日：2023.03.15
技术公布日：2023/7/20