CN116252581A - 直线行驶工况车身垂向及俯仰运动信息估算***及方法 - Google Patents

直线行驶工况车身垂向及俯仰运动信息估算***及方法 Download PDF

Info

Publication number
CN116252581A
CN116252581A CN202310249330.3A CN202310249330A CN116252581A CN 116252581 A CN116252581 A CN 116252581A CN 202310249330 A CN202310249330 A CN 202310249330A CN 116252581 A CN116252581 A CN 116252581A
Authority
CN
China
Prior art keywords
vehicle
vehicle body
vertical
module
speed
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202310249330.3A
Other languages
English (en)
Other versions
CN116252581B (zh
Inventor
史文库
吴骁
陈志勇
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jilin University
Original Assignee
Jilin University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jilin University filed Critical Jilin University
Priority to CN202310249330.3A priority Critical patent/CN116252581B/zh
Publication of CN116252581A publication Critical patent/CN116252581A/zh
Application granted granted Critical
Publication of CN116252581B publication Critical patent/CN116252581B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60G—VEHICLE SUSPENSION ARRANGEMENTS
    • B60G17/00—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load
    • B60G17/015—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load the regulating means comprising electric or electronic elements
    • B60G17/018—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load the regulating means comprising electric or electronic elements characterised by the use of a specific signal treatment or control method
    • B60G17/0182—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load the regulating means comprising electric or electronic elements characterised by the use of a specific signal treatment or control method involving parameter estimation, e.g. observer, Kalman filter
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60G—VEHICLE SUSPENSION ARRANGEMENTS
    • B60G17/00—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load
    • B60G17/015—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load the regulating means comprising electric or electronic elements
    • B60G17/016—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load the regulating means comprising electric or electronic elements characterised by their responsiveness, when the vehicle is travelling, to specific motion, a specific condition, or driver input
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60G—VEHICLE SUSPENSION ARRANGEMENTS
    • B60G17/00—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load
    • B60G17/015—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load the regulating means comprising electric or electronic elements
    • B60G17/019—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load the regulating means comprising electric or electronic elements characterised by the type of sensor or the arrangement thereof
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60G—VEHICLE SUSPENSION ARRANGEMENTS
    • B60G17/00—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load
    • B60G17/015—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load the regulating means comprising electric or electronic elements
    • B60G17/019—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load the regulating means comprising electric or electronic elements characterised by the type of sensor or the arrangement thereof
    • B60G17/01933—Velocity, e.g. relative velocity-displacement sensors
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60G—VEHICLE SUSPENSION ARRANGEMENTS
    • B60G2400/00—Indexing codes relating to detected, measured or calculated conditions or factors
    • B60G2400/05—Attitude
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60G—VEHICLE SUSPENSION ARRANGEMENTS
    • B60G2400/00—Indexing codes relating to detected, measured or calculated conditions or factors
    • B60G2400/20—Speed
    • B60G2400/204—Vehicle speed
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60G—VEHICLE SUSPENSION ARRANGEMENTS
    • B60G2400/00—Indexing codes relating to detected, measured or calculated conditions or factors
    • B60G2400/80—Exterior conditions
    • B60G2400/82—Ground surface

Landscapes

  • Engineering & Computer Science (AREA)
  • Mechanical Engineering (AREA)
  • Image Analysis (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明涉及一种直线行驶工况车身垂向及俯仰运动信息估算***及方法，***包括用于捕捉行车前方道路信息的图像信息捕捉模块、用于获取行车速度的车速估计模块、用于估算车身垂向位置及俯仰角度的车身垂向位置及俯仰角度估算模块以及车身垂向信息及俯仰角度信息估算与显示模块；车身垂向信息及俯仰角度信息估算与显示模块用于计算车辆垂向速度、垂向加速度、俯仰角速度、俯仰角加速度，并将其显示在中控显示器上。本发明利用视觉传感器来获取车辆的姿态，无需增加汽车垂向加速度传感器与转角传感器即可实时准确地测算车身垂向高度、垂向速度、垂向加速度、俯仰角度、俯仰角速度、俯仰角加速度，为汽车主动悬架、半主动悬架控制提供信息参考。

Description

直线行驶工况车身垂向及俯仰运动信息估算***及方法
技术领域
本发明属于汽车智能感知与控制技术领域，具体涉及一种基于单目视觉的直线行驶工况车身垂向及俯仰运动信息估算***及估算方法。
背景技术
随着计算机技术、仪器技术与电子控制技术的发展，智能汽车逐渐走入人们的视线。越来越多先进的传感器被使用到智能汽车中，为车辆提供更多的信息。然而，采用的传感器越多，智能车辆可获取的有效信息就越多，车辆成本也越高。如何在不增加传感器的情况下获取尽可能多的有效信息成为当前智能汽车研究的重点之一。
除了感知环境信息外，车用传感器的另一个重要用途是用来估计汽车行驶状态与动力学参数。汽车的垂向高度与俯仰角度是汽车主动悬架、半主动悬架控制的重要参数，影响着汽车的安全性、平顺性与操纵稳定性。然而，目前量产车上配备的惯性传感器一般只能测汽车x、y轴的加速度与z轴的横摆角速度，对汽车的垂向高度与俯仰角度的测量还需要加入其他传感器，这样会增加车辆成本。摄像机是当前智能汽车经常采用的传感器元件，如能建立一个通过车载摄像机估算的车辆垂向高度信息与俯仰角度信息的估算***及估算方法，对汽车主动悬架的控制以及汽车的平顺性、操纵性、舒适性和安全性至关重要。
发明内容
本发明的目的就在于提供一种基于单目视觉的直线行驶工况车身垂向及俯仰运动信息估算***，还提供一种基于单目视觉的直线行驶工况车身垂向及俯仰运动信息估算方法，以解决现有技术中无法通过单目视觉***计算车身垂向、俯仰运动姿态，导致主动悬架控制需要增加其他传感器进而提高成本的问题。
本发明的目的是通过以下技术方案实现的：
一种直线行驶工况车身垂向及俯仰运动信息估算***，包括图像信息捕捉模块1、车速估计模块2、车身垂向位置及俯仰角度估算模块3以及车身垂向信息及俯仰角度信息估算与显示模块4；
所述图像信息捕捉模块1，用于捕捉行车前方道路信息，包括：单目摄像机11、图像采集模块12以及特征提取与跟踪模块13；所述单目摄像机11将原始图像信息发送至图像采集模块12经畸变矫正后发送至特征提取与跟踪模块13；
所述车速估计模块2，用于获取行车速度；
所述车身垂向位置及俯仰角度估算模块3，用于估算车身垂向位置及俯仰角度，包括行车位移计算模块31、车辆姿态计算模块32和车辆姿态滤波模块33；
所述车速估计模块2将行车速度信息发送至行车位移计算模块31；
所述车辆姿态计算模块32，用于计算当前帧下车辆车身的垂向位移与俯仰角度，包括前一帧像素坐标记录模块321、前一帧车身姿态记录模块322、当前帧像素坐标记录模块323和当前帧车身姿态计算模块324；
所述特征提取与跟踪模块13将特征点相对于此相机的像素坐标和特征点在新的相邻帧上的像素坐标信息发送至当前帧像素坐标记录模块323；
所述前一帧像素坐标记录模块321、前一帧车身姿态记录模块322、当前帧像素坐标记录模块323分别将前一帧的特征点的像素坐标、前一帧车身的垂向位移与俯仰角度和当前帧的特征点的像素坐标信息发送至当前帧车身姿态计算模块324；所述当前帧车身姿态计算模块324将解算出的当前帧车身的垂向位移与俯仰角度信息发送至车辆姿态滤波模块33；
所述车辆姿态滤波模块33将当前的车辆垂向位置与俯仰角度发送至所述车身垂向信息及俯仰角度信息估算与显示模块4；
所述述车身垂向信息及俯仰角度信息估算与显示模块4，用于根据车辆垂向位置与俯仰角度，计算车辆垂向速度、垂向加速度、俯仰角速度、俯仰角加速度，并将其显示在中控显示器41上。
进一步地，所述单目摄像机11，用于通过摄像机的摄像头获取车辆前方道路原始图像信息；所述图像采集模块12，用于获取原始图像信息，并对获取的图像进行畸变矫正；所述特征提取与跟踪模块13，采用ORB算法提取畸变矫正后图像的特征点，获取特征点相对于此相机的像素坐标，并在相邻帧上对三维特征点进行跟踪，获取特征点在新的相邻帧上的像素坐标。
进一步地，所述行车位移计算模块31，通过行车速度，计算连续两帧图像对应时间间隔内的车辆行驶距离。
进一步地，所述前一帧像素坐标记录模块321，用于记录前一帧的特征点的像素坐标；所述前一帧车身姿态记录模块322，用于记录前一帧车身的垂向位移与俯仰角度；所述当前帧像素坐标记录模块323，用于记录当前帧的特征点的像素坐标；所述当前帧车身姿态计算模块324，用于结合前一帧的特征点的像素坐标、前一帧车身的垂向位移与俯仰角度、当前帧的特征点的像素坐标、连续两帧图像对应时间间隔内的车辆行驶距离，进而解算当前帧车身的垂向位移与俯仰角度。
进一步地，所述车辆姿态滤波模块33，采用卡尔曼滤波算法对车辆姿态计算模块得到的车身的垂向位移与俯仰角度进行滤波处理，用于获取当前的车辆垂向位置与俯仰角度。
进一步地，所述车身垂向信息及俯仰角度信息估算与显示模块4包括车身垂向速度计算模块、车身垂向加速度计算模块、车身俯仰角速度计算模块、车身俯仰角加速度计算模块以及车身垂向信息及俯仰信息显示模块。
更进一步地，所述车身垂向速度计算模块，通过结合相邻两帧的车身垂向位置与相邻帧的间隔时间，估算车身垂向速度；所述车身俯仰角速度计算模块，通过结合相邻两帧的车身俯仰角与相邻帧的间隔时间，估算车身俯仰角速度；所述车身垂向加速度计算模块，通过结合相邻两帧的车身垂向速度与相邻帧的间隔时间，估算车身垂向加速度；所述车身俯仰角加速度计算模块，通过结合相邻两帧的车身俯仰角速度与相邻帧的间隔时间，估算车身俯仰角加速度；所述车身垂向信息及俯仰信息显示模块，用于将车身垂向速度、车身垂向加速度、车身俯仰角速度、车身俯仰角加速度显示在中控显示器上。
一种基于单目视觉的直线行驶工况车身垂向及俯仰运动信息估算方法，包括以下步骤：
步骤S1、利用单目摄相机11获取车辆前方路面的原始图像，通过图像采集模块12获取摄像头的图像信息，并对获取的图像进行畸变矫正；
步骤S2、利用ORB算法提取畸变矫正后图像的特征点，获取特征点相对于此相机的像素坐标，并在相邻帧上对三维特征点进行跟踪，获取特征点在新的相邻帧上的像素坐标；
步骤S3、利用车速估计模块2获取行车速度，计算连续两帧图像对应时间间隔内的车辆行驶距离；
步骤S4、通过车辆姿态计算模块32，结合前一帧的特征点的像素坐标、前一帧车身的垂向位移与俯仰角度、当前帧的特征点的像素坐标，计算当前帧车身的垂向位移与俯仰角度；
步骤S5、通过车辆姿态滤波模块33，采用卡尔曼滤波算法对车辆姿态计算模块得到的车身的垂向位移与俯仰角度进行滤波处理，获取当前帧对应的车辆垂向位置与俯仰角度；
步骤S6、通过结合相邻两帧的车身垂向位移、俯仰角度与相邻帧的间隔时间，分别估算车身垂向速度、俯仰角速度；
步骤S7、通过结合相邻两帧的车身垂向速度、俯仰角速度与相邻帧的间隔时间，分别估算车身垂向加速度、俯仰角加速度。
与现有技术相比，本发明的有益效果是：
本发明利用视觉传感器来获取车辆的姿态，无需增加汽车垂向加速度传感器与转角传感器即可实时准确地测算车身垂向高度、垂向速度、垂向加速度、俯仰角度、俯仰角速度、俯仰角加速度，为汽车主动悬架、半主动悬架控制提供信息参考。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1是本发明车身垂向高度、俯仰角度估算***的结构框图；
图2是本发明车身垂向高度、俯仰角度估算方法的流程图；
图3是本发明的行车示意图。
图中，1.图像信息捕捉模块2.车速估计模块3.车身垂向位置及俯仰角度估算模块4.车身垂向信息及俯仰角度信息估算与显示模块11.单目摄像机12.图像采集模块13.特征提取与跟踪模块31.行车位移计算模块32.车辆姿态计算模块33.车辆姿态滤波模块321.前一帧像素坐标记录模块322.前一帧车身姿态记录模块323.当前帧像素坐标记录模块324.当前帧车身姿态计算模块41.中控显示器。
具体实施方式
下面结合实施例对本发明作进一步说明：
下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本发明的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
本发明运用单目摄像机与车速传感器计算车辆移动信息与车身高度、俯仰角度，然后，运用卡尔曼滤波算法测算车身高度与俯仰角度，再依次计算车身垂向速度、垂向加速度、俯仰角速度、俯仰角加速度，将所述车身垂向速度、垂向加速度、俯仰角速度、俯仰角加速度动态显示到中控显示器上。
实施例1
如图1所示，本发明直线行驶工况车身垂向及俯仰运动信息估算***，包括图像信息捕捉模块1、车速估计模块2、车身垂向位置及俯仰角度估算模块3以及车身垂向信息及俯仰角度信息估算与显示模块4。
所述图像信息捕捉模块1，用于捕捉行车前方道路信息，包括：单目摄像机11、图像采集模块12以及特征提取与跟踪模块13。
其中，所述单目摄像机11，用于通过摄像机的摄像头获取车辆前方道路原始图像信息；所述图像采集模块12，用于获取原始图像信息，并对获取的图像进行畸变矫正；所述特征提取与跟踪模块13，采用ORB算法提取畸变矫正后图像的特征点，获取特征点相对于此相机的像素坐标，并在相邻帧上对三维特征点进行跟踪，获取特征点在新的相邻帧上的像素坐标。
所述单目摄像机11将原始图像信息发送至图像采集模块12经畸变矫正后发送至特征提取与跟踪模块13。
所述车速估计模块2，用于获取行车速度。所述车身垂向位置及俯仰角度估算模块3，用于估算车身垂向位置及俯仰角度，包括行车位移计算模块31、车辆姿态计算模块32和车辆姿态滤波模块33。
所述车速估计模块2将行车速度信息发送至行车位移计算模块31。
其中，所述行车位移计算模块31，通过行车速度，计算连续两帧图像对应时间间隔内的车辆行驶距离；所述车辆姿态计算模块32，用于计算当前帧下车辆车身的垂向位移与俯仰角度，包括前一帧像素坐标记录模块321、前一帧车身姿态记录模块322、当前帧像素坐标记录模块323和当前帧车身姿态计算模块324。
所述前一帧像素坐标记录模块321，用于记录前一帧的特征点的像素坐标；
所述前一帧车身姿态记录模块322，用于记录前一帧车身的垂向位移与俯仰角度；
所述当前帧像素坐标记录模块323，用于记录当前帧的特征点的像素坐标；
所述当前帧车身姿态计算模块324，用于结合前一帧的特征点的像素坐标、前一帧车身的垂向位移与俯仰角度、当前帧的特征点的像素坐标、连续两帧图像对应时间间隔内的车辆行驶距离，进而解算当前帧车身的垂向位移与俯仰角度。
所述特征提取与跟踪模块13将特征点相对于此相机的像素坐标和特征点在新的相邻帧上的像素坐标信息发送至当前帧像素坐标记录模块323。
所述前一帧像素坐标记录模块321、前一帧车身姿态记录模块322、当前帧像素坐标记录模块323分别将前一帧的特征点的像素坐标、前一帧车身的垂向位移与俯仰角度和当前帧的特征点的像素坐标信息发送至当前帧车身姿态计算模块324。
所述当前帧车身姿态计算模块324将解算出的当前帧车身的垂向位移与俯仰角度信息发送至车辆姿态滤波模块33。
上述车身的垂向位移与俯仰角度涉及单目相机的坐标转换，所述坐标转换为特征点的世界坐标系与像素坐标系之间的相互转换。
如图3所示，Xw、Yw、Zw是世界坐标系的坐标轴，Xc、Yc、Zc是相机坐标系的坐标轴(像素坐标为二维平面，认为畸变矫正后的像素坐标与相机坐标平行，用μ与ν表示)。特别的，相机坐标系位于车辆前方，世界坐标系的原点位于车辆前方路面，且相机坐标系的原点位于世界坐标系Zw轴上，相机仅绕自身Xc轴旋转。
物体的世界坐标转换至像素坐标的过程满足如下关系式：
Figure BDA0004127216590000091
其中，s为比例系数，K为相机的内参矩阵，由相机特性决定，不随相机的位置改变，可由标定测出。R为相机坐标系的旋转矩阵，T为相机坐标系的平移矩阵。
Figure BDA0004127216590000092
其中，f为相机焦距，(μ0,ν0)是图像坐标系中心点到像素坐标系中心点的偏移量，dx是x向一个像素的长度，dy是y向一个像素的长度。
物体的世界坐标转换至像素坐标的过程还可由下式表示：
Figure BDA0004127216590000093
其中R为旋转重合矩阵，是不考虑相机绕自身轴线旋转的情况下，由世界坐标系转换到相机坐标系的转换矩阵，Tc是世界坐标系原点在相机坐标系下的位置，Rc是存在相机坐标系绕自身轴线旋转情况下的转换矩阵。K是相机内参矩阵。
Figure BDA0004127216590000101
/>
Figure BDA0004127216590000102
Figure BDA0004127216590000103
相机坐标系的原点位于世界坐标系Zw轴上，距离地面高度为h时，
Figure BDA0004127216590000104
该高度h是相机相对于地面的高度，h的变化反映了车身垂向高度的变化。
相机仅绕自身Xc轴旋转角度θ时，
Figure BDA0004127216590000105
当相机固定在汽车上时，由理论力学可知，汽车的俯仰角度即为相机绕自身Xc轴旋转的角度θ。
由上述可得：
Figure BDA0004127216590000106
因此，物体的世界坐标与物体成像对应的像素坐标之间的函数关系包含了h与θ两个变量。
由像素坐标系反推世界坐标，可得：
Figure BDA0004127216590000111
其中，K、R均为已知矩阵。
Figure BDA0004127216590000112
Figure BDA0004127216590000113
设
Figure BDA0004127216590000114
为抵消比例系数s的影响，设
Figure BDA0004127216590000115
则有
Figure BDA0004127216590000116
Figure BDA0004127216590000117
由于本方法捕捉的特征点在路面上，因此特征点的世界坐标Zw＝0。
设
Figure BDA0004127216590000121
Figure BDA0004127216590000122
Figure BDA0004127216590000123
Figure BDA0004127216590000124
对于确定参数的单目相机而言，确定特征点即可确定其像素坐标(μ,ν)，则G、R、g均为确定的值。
因此有
Figure BDA0004127216590000125
Xw＝tan(g+θ)h
设连续两帧图像中，前一帧特征点坐标为(μ1,ν1)，当前帧特征点坐标为(μ2,ν2)，两时刻特征点坐标分别为(Xw1,Yw1,Zw1)与(Xw2,Yw2,Zw2)。
由于捕捉的特征点位于路面，因此认为Zw1＝Zw2＝0。
由于汽车直行，认为Yw1＝Yw2,ΔYw＝Yw1-Yw2＝0。
ΔXw＝Xw1-Xw2,ΔXw是连续两帧时间间隔内汽车行驶的距离，由行车位移计算模块31计算得到。
因此有
Figure BDA0004127216590000126
/>
ΔXw＝Xw1-Xw2＝tan(g1+θ1)h1-tan(g2+θ2)h2
此公式为一马尔可夫过程，可由上一时刻的车辆姿态(h1,θ1)计算当前时刻的姿态(h2,θ2)。
所述车辆姿态滤波模块33，采用卡尔曼滤波算法对车辆姿态计算模块得到的车身的垂向位移与俯仰角度进行滤波处理，用于获取当前的车辆垂向位置与俯仰角度。
车辆的初始状态规定为(h0,θ0)，由汽车标定测量得到。
认为h2是当前帧汽车车身高度。
认为(θ2-θ0)是当前帧汽车车身俯仰角度。
由卡尔曼滤波可知，单目标***的卡尔曼滤波值可由以下公式计算：
Figure BDA0004127216590000131
其中，
Figure BDA0004127216590000132
是n时刻的卡尔曼滤波值，/>
Figure BDA0004127216590000133
是n时刻的***预测值，/>
Figure BDA0004127216590000134
是n时刻的观测值，bm是***状态噪声，bs是***的测量噪声。计算垂向高度h时，X＝h，计算俯仰角度θ时，X＝θ。
由于车辆运动时姿态围绕(h0,θ0)波动，故令
Figure BDA0004127216590000135
hn与θn通过迭代计算得到。
所述车辆姿态滤波模块33将当前的车辆垂向位置与俯仰角度发送至所述车身垂向信息及俯仰角度信息估算与显示模块4。
所述述车身垂向信息及俯仰角度信息估算与显示模块4，用于根据车辆垂向位置与俯仰角度，计算车辆垂向速度、垂向加速度、俯仰角速度、俯仰角加速度，并将其显示在中控显示器41上。所述车身垂向信息及俯仰角度信息估算与显示模块4包括车身垂向速度计算模块、车身垂向加速度计算模块、车身俯仰角速度计算模块、车身俯仰角加速度计算模块以及车身垂向信息及俯仰信息显示模块。所述车身垂向速度计算模块，通过结合相邻两帧的车身垂向位置与相邻帧的间隔时间，估算车身垂向速度。
所述车身俯仰角速度计算模块，通过结合相邻两帧的车身俯仰角与相邻帧的间隔时间，估算车身俯仰角速度。
所述车身垂向加速度计算模块，通过结合相邻两帧的车身垂向速度与相邻帧的间隔时间，估算车身垂向加速度。
所述车身俯仰角加速度计算模块，通过结合相邻两帧的车身俯仰角速度与相邻帧的间隔时间，估算车身俯仰角加速度。
所述车身垂向信息及俯仰信息显示模块，用于将车身垂向速度、车身垂向加速度、车身俯仰角速度、车身俯仰角加速度显示在中控显示器上。
实施例2
如图2所示，本发明基于单目视觉的直线行驶工况车身垂向及俯仰运动信息估算方法，具体包括以下步骤：
步骤S1、利用单目摄相机11获取车辆前方路面的原始图像，通过图像采集模块1获取摄像头的图像信息，并对获取的图像进行畸变矫正；
步骤S2、利用ORB算法提取畸变矫正后图像的特征点，获取特征点相对于此相机的像素坐标，并在相邻帧上对三维特征点进行跟踪，获取特征点在新的相邻帧上的像素坐标。
步骤S3、利用车速估计模块2获取行车速度，计算连续两帧图像对应时间间隔内的车辆行驶距离。
步骤S4、通过车辆姿态计算模块32，结合前一帧的特征点的像素坐标、前一帧车身的垂向位移与俯仰角度、当前帧的特征点的像素坐标，计算当前帧车身的垂向位移与俯仰角度。
步骤S5、通过车辆姿态滤波模块33，采用卡尔曼滤波算法对车辆姿态计算模块得到的车身的垂向位移与俯仰角度进行滤波处理，获取当前帧对应的车辆垂向位置与俯仰角度。
步骤S6、通过结合相邻两帧的车身垂向位移、俯仰角度与相邻帧的间隔时间，分别估算车身垂向速度、俯仰角速度。
步骤S7、通过结合相邻两帧的车身垂向速度、俯仰角速度与相邻帧的间隔时间，分别估算车身垂向加速度、俯仰角加速度。
注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。

Claims (8)

1.一种直线行驶工况车身垂向及俯仰运动信息估算***，其特征在于：包括图像信息捕捉模块(1)、车速估计模块(2)、车身垂向位置及俯仰角度估算模块(3)以及车身垂向信息及俯仰角度信息估算与显示模块(4)；
所述图像信息捕捉模块(1)，用于捕捉行车前方道路信息，包括：单目摄像机(11)、图像采集模块(12)以及特征提取与跟踪模块(13)；所述单目摄像机(11)将原始图像信息发送至图像采集模块(12)经畸变矫正后发送至特征提取与跟踪模块(13)；
所述车速估计模块(2)，用于获取行车速度；
所述车身垂向位置及俯仰角度估算模块(3)，用于估算车身垂向位置及俯仰角度，包括行车位移计算模块(31)、车辆姿态计算模块(32)和车辆姿态滤波模块(33)；
所述车速估计模块(2)将行车速度信息发送至行车位移计算模块(31)；
所述车辆姿态计算模块(32)，用于计算当前帧下车辆车身的垂向位移与俯仰角度，包括前一帧像素坐标记录模块(321)、前一帧车身姿态记录模块(322)、当前帧像素坐标记录模块(323)和当前帧车身姿态计算模块(324)；
所述特征提取与跟踪模块(13)将特征点相对于此相机的像素坐标和特征点在新的相邻帧上的像素坐标信息发送至当前帧像素坐标记录模块(323)；
所述前一帧像素坐标记录模块(321)、前一帧车身姿态记录模块(322)、当前帧像素坐标记录模块(323)分别将前一帧的特征点的像素坐标、前一帧车身的垂向位移与俯仰角度和当前帧的特征点的像素坐标信息发送至当前帧车身姿态计算模块(324)；所述当前帧车身姿态计算模块(324)将解算出的当前帧车身的垂向位移与俯仰角度信息发送至车辆姿态滤波模块(33)；
所述车辆姿态滤波模块(33)将当前的车辆垂向位置与俯仰角度发送至所述车身垂向信息及俯仰角度信息估算与显示模块(4)；
所述述车身垂向信息及俯仰角度信息估算与显示模块(4)，用于根据车辆垂向位置与俯仰角度，计算车辆垂向速度、垂向加速度、俯仰角速度、俯仰角加速度，并将其显示在中控显示器(41)上。
2.根据权利要求1所述的一种直线行驶工况车身垂向及俯仰运动信息估算***，其特征在于：所述单目摄像机(11)，用于通过摄像机的摄像头获取车辆前方道路原始图像信息；所述图像采集模块(12)，用于获取原始图像信息，并对获取的图像进行畸变矫正；所述特征提取与跟踪模块(13)，采用ORB算法提取畸变矫正后图像的特征点，获取特征点相对于此相机的像素坐标，并在相邻帧上对三维特征点进行跟踪，获取特征点在新的相邻帧上的像素坐标。
3.根据权利要求1所述的一种直线行驶工况车身垂向及俯仰运动信息估算***，其特征在于：所述行车位移计算模块(31)，通过行车速度计算连续两帧图像对应时间间隔内的车辆行驶距离。
4.根据权利要求1所述的一种直线行驶工况车身垂向及俯仰运动信息估算***，其特征在于：所述前一帧像素坐标记录模块(321)，用于记录前一帧的特征点的像素坐标；所述前一帧车身姿态记录模块(322)，用于记录前一帧车身的垂向位移与俯仰角度；所述当前帧像素坐标记录模块(323)，用于记录当前帧的特征点的像素坐标；所述当前帧车身姿态计算模块(324)，用于结合前一帧的特征点的像素坐标、前一帧车身的垂向位移与俯仰角度、当前帧的特征点的像素坐标、连续两帧图像对应时间间隔内的车辆行驶距离，进而解算当前帧车身的垂向位移与俯仰角度。
5.根据权利要求1所述的一种直线行驶工况车身垂向及俯仰运动信息估算***，其特征在于：所述车辆姿态滤波模块(33)，采用卡尔曼滤波算法对车辆姿态计算模块得到的车身的垂向位移与俯仰角度进行滤波处理，用于获取当前的车辆垂向位置与俯仰角度。
6.根据权利要求1所述的一种直线行驶工况车身垂向及俯仰运动信息估算***，其特征在于：所述车身垂向信息及俯仰角度信息估算与显示模块(4)包括车身垂向速度计算模块、车身垂向加速度计算模块、车身俯仰角速度计算模块、车身俯仰角加速度计算模块以及车身垂向信息及俯仰信息显示模块。
7.根据权利要求6所述的一种直线行驶工况车身垂向及俯仰运动信息估算***，其特征在于：所述车身垂向速度计算模块，通过结合相邻两帧的车身垂向位置与相邻帧的间隔时间，估算车身垂向速度；所述车身俯仰角速度计算模块，通过结合相邻两帧的车身俯仰角与相邻帧的间隔时间，估算车身俯仰角速度；所述车身垂向加速度计算模块，通过结合相邻两帧的车身垂向速度与相邻帧的间隔时间，估算车身垂向加速度；所述车身俯仰角加速度计算模块，通过结合相邻两帧的车身俯仰角速度与相邻帧的间隔时间，估算车身俯仰角加速度；所述车身垂向信息及俯仰信息显示模块，用于将车身垂向速度、车身垂向加速度、车身俯仰角速度、车身俯仰角加速度显示在中控显示器上。
8.一种基于单目视觉的直线行驶工况车身垂向及俯仰运动信息估算方法，其特征在于，包括以下步骤：
步骤S1、利用单目摄相机(11)获取车辆前方路面的原始图像，通过图像采集模块(12)获取摄像头的图像信息，并对获取的图像进行畸变矫正；
步骤S2、利用ORB算法提取畸变矫正后图像的特征点，获取特征点相对于此相机的像素坐标，并在相邻帧上对三维特征点进行跟踪，获取特征点在新的相邻帧上的像素坐标；
步骤S3、利用车速估计模块(2)获取行车速度，计算连续两帧图像对应时间间隔内的车辆行驶距离；
步骤S4、通过车辆姿态计算模块(32)，结合前一帧的特征点的像素坐标、前一帧车身的垂向位移与俯仰角度、当前帧的特征点的像素坐标，计算当前帧车身的垂向位移与俯仰角度；
步骤S5、通过车辆姿态滤波模块(33)，采用卡尔曼滤波算法对车辆姿态计算模块得到的车身的垂向位移与俯仰角度进行滤波处理，获取当前帧对应的车辆垂向位置与俯仰角度；
步骤S6、通过结合相邻两帧的车身垂向位移、俯仰角度与相邻帧的间隔时间，分别估算车身垂向速度、俯仰角速度；
步骤S7、通过结合相邻两帧的车身垂向速度、俯仰角速度与相邻帧的间隔时间，分别估算车身垂向加速度、俯仰角加速度。
CN202310249330.3A 2023-03-15 2023-03-15 直线行驶工况车身垂向及俯仰运动信息估算***及方法 Active CN116252581B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310249330.3A CN116252581B (zh) 2023-03-15 2023-03-15 直线行驶工况车身垂向及俯仰运动信息估算***及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310249330.3A CN116252581B (zh) 2023-03-15 2023-03-15 直线行驶工况车身垂向及俯仰运动信息估算***及方法

Publications (2)

Publication Number Publication Date
CN116252581A true CN116252581A (zh) 2023-06-13
CN116252581B CN116252581B (zh) 2024-01-16

Family

ID=86679198

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310249330.3A Active CN116252581B (zh) 2023-03-15 2023-03-15 直线行驶工况车身垂向及俯仰运动信息估算***及方法

Country Status (1)

Country Link
CN (1) CN116252581B (zh)

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20070154068A1 (en) * 2006-01-04 2007-07-05 Mobileye Technologies, Ltd. Estimating Distance To An Object Using A Sequence Of Images Recorded By A Monocular Camera
JP2012046081A (ja) * 2010-08-27 2012-03-08 Daihatsu Motor Co Ltd 走行状態判定装置
CN102538781A (zh) * 2011-12-14 2012-07-04 浙江大学 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
JP2015184956A (ja) * 2014-03-25 2015-10-22 日産自動車株式会社 自己位置算出装置及び自己位置算出方法
US20170024617A1 (en) * 2014-02-24 2017-01-26 Nissan Motor Co., Ltd. Self-Location Calculating Device and Self-Location Calculating Method
CN106525049A (zh) * 2016-11-08 2017-03-22 山东大学 一种基于计算机视觉的四足机器人本***姿跟踪方法
CN107833249A (zh) * 2017-09-29 2018-03-23 南京航空航天大学 一种基于视觉引导的舰载机着陆过程姿态预估方法
CN110386142A (zh) * 2018-04-17 2019-10-29 百度（美国）有限责任公司 用于自动驾驶车辆的俯仰角校准方法
WO2021035669A1 (zh) * 2019-08-30 2021-03-04 深圳市大疆创新科技有限公司 位姿预测方法、地图构建方法、可移动平台及存储介质
CN112639502A (zh) * 2018-09-07 2021-04-09 华为技术有限公司 机器人位姿估计
CN114910927A (zh) * 2021-02-10 2022-08-16 福特全球技术公司 使用单色成像的基于事件的车辆姿态估计

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20070154068A1 (en) * 2006-01-04 2007-07-05 Mobileye Technologies, Ltd. Estimating Distance To An Object Using A Sequence Of Images Recorded By A Monocular Camera
JP2012046081A (ja) * 2010-08-27 2012-03-08 Daihatsu Motor Co Ltd 走行状態判定装置
CN102538781A (zh) * 2011-12-14 2012-07-04 浙江大学 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
US20170024617A1 (en) * 2014-02-24 2017-01-26 Nissan Motor Co., Ltd. Self-Location Calculating Device and Self-Location Calculating Method
JP2015184956A (ja) * 2014-03-25 2015-10-22 日産自動車株式会社 自己位置算出装置及び自己位置算出方法
CN106525049A (zh) * 2016-11-08 2017-03-22 山东大学 一种基于计算机视觉的四足机器人本***姿跟踪方法
CN107833249A (zh) * 2017-09-29 2018-03-23 南京航空航天大学 一种基于视觉引导的舰载机着陆过程姿态预估方法
CN110386142A (zh) * 2018-04-17 2019-10-29 百度（美国）有限责任公司 用于自动驾驶车辆的俯仰角校准方法
CN112639502A (zh) * 2018-09-07 2021-04-09 华为技术有限公司 机器人位姿估计
WO2021035669A1 (zh) * 2019-08-30 2021-03-04 深圳市大疆创新科技有限公司 位姿预测方法、地图构建方法、可移动平台及存储介质
CN114910927A (zh) * 2021-02-10 2022-08-16 福特全球技术公司 使用单色成像的基于事件的车辆姿态估计

Also Published As

Publication number Publication date
CN116252581B (zh) 2024-01-16

Similar Documents

Publication Publication Date Title
WO2020119140A1 (zh) 同步定位与地图构建中关键帧提取方法、装置和智能设备
US6535114B1 (en) Method and apparatus for environment recognition
CN110745140B (zh) 一种基于连续图像约束位姿估计的车辆换道预警方法
CN111595333A (zh) 视觉惯性激光数据融合的模块化无人车定位方法及***
CN104180818B (zh) 一种单目视觉里程计算装置
CN103411621B (zh) 一种面向室内移动机器人的光流场视觉/ins组合导航方法
CN107121132B (zh) 求取车辆环境图像的方法和设备及识别环境中对象的方法
JP5804185B2 (ja) 移動物***置姿勢推定装置及び移動物***置姿勢推定方法
Jeong et al. Adaptive inverse perspective mapping for lane map generation with SLAM
US20180075614A1 (en) Method of Depth Estimation Using a Camera and Inertial Sensor
JP6708730B2 (ja) 移動体
CN111279354A (zh) 图像处理方法、设备及计算机可读存储介质
CN115371665B (zh) 一种基于深度相机和惯性融合的移动机器人定位方法
CN113503873B (zh) 一种多传感器融合的视觉定位方法
CN106845552A (zh) 在光强分布不均匀环境下的融合光流和sift特征点匹配的低动态载体速度计算方法
CN109345591B (zh) 一种车辆自身姿态检测方法和装置
WO2019156072A1 (ja) 姿勢推定装置
CN116442707B (zh) 基于双目视觉的车身垂向及俯仰运动信息估算***及方法
JP6768554B2 (ja) キャリブレーション装置
CN110720113A (zh) 一种参数处理方法、装置及摄像设备、飞行器
CN110197104B (zh) 基于车辆的测距方法及装置
CN113701750A (zh) 一种井下多传感器的融合定位***
CN109878528A (zh) 面向车载立体视觉***的头部运动姿态检测***
CN116252581B (zh) 直线行驶工况车身垂向及俯仰运动信息估算***及方法
Park et al. A novel line of sight control system for a robot vision tracking system, using vision feedback and motion-disturbance feedforward compensation

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant