CN112362055B - 姿态估计方法、装置和电子设备 - Google Patents

姿态估计方法、装置和电子设备 Download PDF

Info

Publication number
CN112362055B
CN112362055B CN202011391580.3A CN202011391580A CN112362055B CN 112362055 B CN112362055 B CN 112362055B CN 202011391580 A CN202011391580 A CN 202011391580A CN 112362055 B CN112362055 B CN 112362055B
Authority
CN
China
Prior art keywords
trailer
attitude angle
laser radar
attitude
point cloud
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.)
Active
Application number
CN202011391580.3A
Other languages
English (en)
Other versions
CN112362055A (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.)
Suzhou Zhitu Technology Co Ltd
Original Assignee
Suzhou Zhitu Technology Co Ltd
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 Suzhou Zhitu Technology Co Ltd filed Critical Suzhou Zhitu Technology Co Ltd
Priority to CN202011391580.3A priority Critical patent/CN112362055B/zh
Publication of CN112362055A publication Critical patent/CN112362055A/zh
Application granted granted Critical
Publication of CN112362055B publication Critical patent/CN112362055B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/50—Systems of measurement based on relative movement of target
    • G01S17/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4808—Evaluating distance, position or velocity data
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar image

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Multimedia (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明提供了一种姿态估计方法、装置和电子设备。其中，激光雷达设置在拖车车顶，惯性测量仪设置在拖车和挂车的连接点；该方法包括：接收激光雷达发送的点云数据，基于点云数据确定挂车的第一姿态角；基于速度分解对拖车和挂车建立运动学模型，基于运动学模型确定挂车的第二姿态角；接收惯性测量仪发送的测量数据，基于测量数据确定挂车的第三姿态角；基于卡尔曼滤波算法对第一姿态角、第二姿态角和第三姿态角进行融合滤波，得到挂车的最终姿态角。该方式只需要设置激光雷达和惯性测量仪，无需设置其他传感器就可以准确计算姿态角，成本较低；并且具备精确性和良好的鲁棒性，能很好地适应自动驾驶的各种工况，并且可用于商用车的姿态估计任务。

Description

姿态估计方法、装置和电子设备
技术领域
本发明涉及自动驾驶控制技术领域，尤其是涉及一种姿态估计方法、装置和电子设备。
背景技术
自动驾驶的浪潮正席卷着蓬勃发展的汽车行业，人们也普遍认为自动驾驶技术或先在商用车领域得到展现。但在自动驾驶控制过程中，要想准确地进行转向和稳定性控制，需要将挂车姿态角如横摆角和翻滚角作为控制器的输入参数。此外，如果挂车姿态估计不准确，会导致拖车－挂车形成夹死或失稳的情况，因此准确地测量挂车的姿态是必要的。
一般来说，为了准确地测量挂车的姿态是，需要在拖车和挂车中加装多个用于测量姿态角的控制器，这种方式需要较高的成本，并且测量结果也不准确。
发明内容
有鉴于此，本发明的目的在于提供及一种姿态估计方法、装置和电子设备，以节约成本，提高测量结果的准确性。
第一方面，本发明实施例提供了一种姿态估计方法，应用于控制器，激光雷达设置在拖车车顶，惯性测量仪设置在拖车和挂车的连接点；控制器与激光雷达通信连接，控制器与惯性测量仪通信连接；方法包括：接收激光雷达发送的点云数据，基于点云数据确定挂车的第一姿态角；基于速度分解对拖车和挂车建立运动学模型，基于运动学模型确定挂车的第二姿态角；接收惯性测量仪发送的测量数据，基于测量数据确定挂车的第三姿态角；基于卡尔曼滤波算法对第一姿态角、第二姿态角和第三姿态角进行融合滤波，得到挂车的最终姿态角。
在本发明较佳的实施例中，上述接收激光雷达发送的点云数据，基于点云数据确定挂车的第一姿态角的步骤，包括：对激光雷达的安装角进行离线标定，确定激光雷达的坐标系相对于拖车的坐标系的姿态偏差；接收激光雷达发送的点云数据；分割点云数据的感兴趣区域，得到激光雷达的坐标系下的姿态角；基于激光雷达的坐标系相对于拖车的姿态偏差和激光雷达的坐标系下的姿态角，确定拖车的坐标系下的姿态角；将拖车的坐标系下的姿态角作为第一姿态角。
在本发明较佳的实施例中，上述接收激光雷达发送的点云数据的步骤，包括：基于预先设定的数据采集周期，周期性接收激光雷达发送的点云数据。
在本发明较佳的实施例中，上述分割点云数据的感兴趣区域，得到激光雷达的坐标系下的姿态角的步骤，包括：基于预先标定的拖车转向、速度与挂车几何特征的关系，根据点云数据确定挂车的感兴趣区域，并且从点云数据中提取特征点；其中，点云数据为三维平面；将特征点从三维平面中降维到二维平面；基于最小二乘法对二维平面的特征点进行线性拟合；根据线性拟合得到的直线斜率，确定激光雷达的坐标系下的姿态角。
在本发明较佳的实施例中，上述运动学模型包括挂车和挂车行驶的距离、挂车的横摆角、拖车的横摆角、挂车的轴距和拖车的轴距。
在本发明较佳的实施例中，上述基于测量数据确定挂车的第三姿态角的步骤，包括：对测量数据进行积分处理，得到挂车的第三姿态角。
在本发明较佳的实施例中，上述基于卡尔曼滤波算法对第一姿态角、第二姿态角和第三姿态角进行融合滤波，得到挂车的最终姿态角的步骤之后，方法包括：将挂车的最终姿态角发送至终端设备中，以在终端设备显示挂车的最终姿态角。
第二方面，本发明实施例还提供一种姿态估计装置，应用于控制器，激光雷达设置在拖车车顶，惯性测量仪设置在拖车和挂车的连接点；控制器与激光雷达通信连接，控制器与惯性测量仪通信连接；装置包括：第一姿态角确定模块，用于接收激光雷达发送的点云数据，基于点云数据确定挂车的第一姿态角；第二姿态角确定模块，用于基于速度分解对拖车和挂车建立运动学模型，基于运动学模型确定挂车的第二姿态角；第三姿态角确定模块，用于接收惯性测量仪发送的测量数据，基于测量数据确定挂车的第三姿态角；最终姿态角确定模块，用于基于卡尔曼滤波算法对第一姿态角、第二姿态角和第三姿态角进行融合滤波，得到挂车的最终姿态角。
第三方面，本发明实施例还提供一种电子设备，包括处理器和存储器，存储器存储有能够被处理器执行的计算机可执行指令，处理器执行计算机可执行指令以实现上述的姿态估计方法的步骤。
第四方面，本发明实施例还提供一种计算机可读存储介质，计算机可读存储介质存储有计算机可执行指令，计算机可执行指令在被处理器调用和执行时，计算机可执行指令促使处理器实现上述的姿态估计方法的步骤。
本发明实施例带来了以下有益效果：
本发明实施例提供的一种姿态估计方法、装置和电子设备，控制器根据接收的激光雷达发送的点云数据确定挂车的第一姿态角；根据对拖车和挂车速度分解建立的运动学模型确定挂车的第二姿态角；根据惯性测量仪发送的测量数据确定挂车的第三姿态角；最终基于卡尔曼滤波算法对第一姿态角、第二姿态角和第三姿态角进行融合滤波，得到挂车的最终姿态角。该方式中只需要设置激光雷达和惯性测量仪，无需设置其他传感器就可以准确计算姿态角，成本较低；并且具备精确性和良好的鲁棒性，能很好地适应自动驾驶的各种工况，并且可用于商用车的姿态估计任务。
本公开的其他特征和优点将在随后的说明书中阐述，或者，部分特征和优点可以从说明书推知或毫无疑义地确定，或者通过实施本公开的上述技术即可得知。
为使本公开的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的一种拖车和挂车的传感器布置方式示意图；
图2为本发明实施例提供的一种姿态估计方法的流程图；
图3为本发明实施例提供的另一种姿态估计方法的流程图；
图4为本发明实施例提供的一种运动学模型的示意图；
图5为本发明实施例提供的一种姿态估计装置的结构示意图；
图6为本发明实施例提供的一种电子设备的结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
目前，为了准确地测量挂车的姿态是，需要在拖车和挂车中加装多个用于测量姿态角的控制器，这种方式需要较高的成本，并且测量结果也不准确。基于此，本发明实施例提供的一种姿态估计方法、装置和电子设备，具体涉及一种基于激光雷达和IMU(InertialMeasurement Unit，惯性测量仪)的挂车姿态估计方法及***。
为便于对本实施例进行理解，首先对本发明实施例所公开的一种姿态估计方法进行详细介绍。
实施例1
本发明实施提供一种姿态估计方法，该方法应用于控制器，激光雷达设置在拖车车顶，惯性测量仪设置在拖车和挂车的连接点；控制器与激光雷达通信连接，控制器与惯性测量仪通信连接。
参见图1所示的一种拖车和挂车的传感器布置方式示意图，十字标志的激光雷达设置在拖车车顶，图1中的设置在激光雷达设置在拖车车顶的中央。其中，拖车可以为用来拖运汽车的一种车，由底盘、起重装置、托举牵引装置、液压***、电控***、车体与工具箱等组成。挂车可以指由汽车(一般为拖车)牵引而本身无动力驱动装置的车辆。即拖车牵引挂车移动，动力驱动装置设置在拖车上，挂车上没有动力驱动装置。
拖车和挂车可以采用铰连接，惯性测量仪设置在拖车和挂车的连接点，即可设置在拖车和挂车的铰接点处。控制器可以设置在拖车和挂车的内部，其中，控制器既与激光雷达通信连接，又与惯性测量仪通信连接；控制器可以分别接收激光雷达和惯性测量仪发送的数据。
基于上述描述，参见图2所示的一种姿态估计方法的流程图，该姿态估计方法包括如下步骤：
步骤S202，接收激光雷达发送的点云数据，基于点云数据确定挂车的第一姿态角。
激光雷达(Lidar)是以发射激光束探测目标的位置、速度等特征量的雷达***。其工作原理是向目标发射探测信号(激光束)，然后将接收到的从目标反射回来的信号(目标回波)与发射信号进行比较，作适当处理后，就可获得目标的有关信息，如目标距离、方位、高度、速度、姿态、甚至形状等参数。
点云数据可以指在一个三维坐标***中的一组向量的集合，这些向量通常以X，Y，Z三维坐标的形式表示，而且一般主要用来代表一个物体的外表面形状。不经如此，除(X，Y，Z)代表的几何位置信息之外，点云数据还可以表示一个点的RGB(Red Green Blue，红绿蓝)颜色、灰度值、深度、分割结果等。
本实施例中的点云数据可以由激光雷达扫描获取，激光雷达获取点云数据后可以将点云数据发送至控制器。控制器接收点云数据后，可以对点云数据进行处理，得到挂车的第一姿态角。
另外需要说明的是，本实施例的第一姿态角、第二姿态角、第三姿态角和最终姿态角，均可以包括横摆角、俯仰角和翻滚角。
步骤S204，基于速度分解对拖车和挂车建立运动学模型，基于运动学模型确定挂车的第二姿态角。
本实施例可以基于速度分解对拖车和挂车进行运动学建模，得到运动学模型，根据建立的运动学模型可以确定挂车的第二姿态角。速度是一个矢量，速度分解可以指将速度分解为水平的x轴标量和垂直的y轴标量。通过分解的速度建立运动学模型，从而确定挂车的第二姿态角。
步骤S206，接收惯性测量仪发送的测量数据，基于测量数据确定挂车的第三姿态角。
本实施例中的IMU也会向控制器发送数据，即上述测量数据。控制器就可以基于上述测量数据估算挂车的姿态角，即第三姿态角。
步骤S208，基于卡尔曼滤波算法对第一姿态角、第二姿态角和第三姿态角进行融合滤波，得到挂车的最终姿态角。
在获得第一姿态角、第二姿态角和第三姿态角之后，控制器可以通过卡尔曼滤波算法对第一姿态角、第二姿态角和第三姿态角进行融合滤波，得到挂车的最终姿态角。最终姿态角是基于第一姿态角、第二姿态角和第三姿态角进行演算得到的，具有较高的精确性和良好的鲁棒性。
本发明实施例提供的一种姿态估计方法，控制器根据接收的激光雷达发送的点云数据确定挂车的第一姿态角；根据对拖车和挂车速度分解建立的运动学模型确定挂车的第二姿态角；根据惯性测量仪发送的测量数据确定挂车的第三姿态角；最终基于卡尔曼滤波算法对第一姿态角、第二姿态角和第三姿态角进行融合滤波，得到挂车的最终姿态角。该方式中只需要设置激光雷达和惯性测量仪，无需设置其他传感器就可以准确计算姿态角，成本较低；并且具备精确性和良好的鲁棒性，能很好地适应自动驾驶的各种工况，并且可用于商用车的姿态估计任务。
实施例2
本发明实施例还提供另一种姿态估计方法；该方法在上述实施例方法的基础上实现；该方法重点描述基于点云数据确定挂车的第一姿态角的具体实现方式。
如图3所示的另一种姿态估计方法的流程图，该姿态估计方法包括如下步骤：
步骤S302，对激光雷达的安装角进行离线标定，确定激光雷达的坐标系相对于拖车的坐标系的姿态偏差。
本实施例中的激光雷达设置在拖车车顶，使激光雷达可以扫描到挂车的边缘。在安装后可以对激光雷达进行离线标定，从而确定激光雷达的坐标系相对于拖车的坐标系的姿态偏差。
其中，标定主要是指使用标准的计量仪器对所使用仪器的准确度(精度)进行检测是否符合标准，一般大多用于精密度较高的仪器，标定也可以认为是校准。本实施例中的离线标定即对激光雷达进行离线校准，从而保证激光雷达的精密度符合要求。
步骤S304，接收激光雷达发送的点云数据。
完成离线标定之后，控制器可以接收激光雷达发送的点云数据。本实施例中的控制器可以周期性地接收激光雷达发送的点云数据，例如：基于预先设定的数据采集周期，周期性接收激光雷达发送的点云数据。
数据采集周期可以由用户预先是设定，例如：每0.1秒接收一次，或者每1秒接收一次等。控制器可以根据预先设定的数据采集周期，周期性接收激光雷达发送的点云数据。
步骤S306，分割点云数据的感兴趣区域，得到激光雷达的坐标系下的姿态角。
本实施例可以采用安装在车顶的三维激光雷达对挂车集装箱进行扫描，将扫描到的集装箱的点云数据进行ROI(Region Of Interest感兴趣区域)分割、筛选、拟合，从而计算出挂车在激光雷达坐标系下的姿态角。其中，机器视觉、图像处理中，从被处理的图像以方框、圆、椭圆、不规则多边形等方式勾勒出需要处理的区域，称为感兴趣区域。
具体来说，可以根据拖车运动状态信息及挂车的几何特征，确定激光雷达感兴趣区域，例如：基于预先标定的拖车转向、速度与挂车几何特征的关系，根据点云数据确定挂车的感兴趣区域，并且从点云数据中提取特征点；其中，点云数据为三维平面；将特征点从三维平面中降维到二维平面；基于最小二乘法对二维平面的特征点进行线性拟合；根据线性拟合得到的直线斜率，确定激光雷达的坐标系下的姿态角。
利用标定好的拖车转向、速度与挂车几何特征的关系，根据实时的拖车状态信息确定挂车所在的感兴趣区域，从而选出该工况下对应的特征点；特征点提取模块，用于根据拖车运动状态信息以及转向角度，选定感兴趣区域并剔除一些奇异点(如跳变较大的点)，从而提取点云数据中的特征点；其中，感兴趣区域的具体范围为：以挂车横摆角为例，如果拖车直行，方向的范围为(-1，1)，如果车辆左转且速度不为0，方向的范围为(-1，0)，如果车辆右转且速度不为0，方向的范围为(0，1)，X方向上的范围均为(3，4)，对于俯仰角和翻滚角估计的感兴趣区域选择方法同上。
将特征点从三维点云降维到二维平面，其中降维具体为：以横摆角为例，是将点云数据从XYZ空间通过去除Z坐标降维到XY平面上；并通过最小二乘法把二维平面上的特征点拟合成一条直线，利用最小二乘法对二维平面上的特征点进行线性拟合，从而根据直线斜率确定挂车的姿态角。其中，最小二乘法的具体公式可以为：
线性回归方程为：y＝b0+b1x；其中，线性回归方程的斜率和截距：
Figure BDA0002811709150000091
步骤S308，基于激光雷达的坐标系相对于拖车的姿态偏差和激光雷达的坐标系下的姿态角，确定拖车的坐标系下的姿态角。
在确定激光雷达的坐标系相对于拖车的姿态偏差和激光雷达的坐标系下的姿态角之后，可以进行坐标系转换，得到拖车的坐标系下的姿态角。
步骤S310，将拖车的坐标系下的姿态角作为第一姿态角。
拖车的坐标系下的姿态角即为本实施例中的第一姿态角。
步骤S312，基于速度分解对拖车和挂车建立运动学模型，基于运动学模型确定挂车的第二姿态角。
基于速度分解可以对拖车和挂车(truck-tratiler)进行运动学建模，其中，运动学模型包括挂车和挂车行驶的距离、挂车的横摆角、拖车的横摆角、挂车的轴距和拖车的轴距。
参见图4所示的一种运动学模型的示意图，对truck-trailer进行速度分解，则可以推导出运动学模型如下：
Figure BDA0002811709150000101
Figure BDA0002811709150000102
其中，s是车辆行驶的距离，θ0是拖车的横摆角，θ1是挂车的横摆角，L是拖车的轴距，d1是t挂车的轴距。
步骤S314，接收惯性测量仪发送的测量数据，基于测量数据确定挂车的第三姿态角。
控制器在接收惯性测量仪发送的测量数据之后，控制器可以通过对测量数据进行积分，得到挂车的第三姿态角。例如：对测量数据进行积分处理，得到挂车的第三姿态角。
在挂车上设置的惯性测量单元(IMU)自***启动后对挂车的姿态角进行状态预测。***假设陀螺过程噪声符合高斯过程有ωm(t)＝ω(t)+bg(t)+n(t)，其中ωm为IMU角速度测量值，bg为陀螺仪的零偏，n(t)为过程噪声。
步骤S316，基于卡尔曼滤波算法对第一姿态角、第二姿态角和第三姿态角进行融合滤波，得到挂车的最终姿态角。
通过上述步骤计算得到的第一姿态角和第二姿态角可以作为观测量代入卡尔曼滤波。
具体为，对状态量[θ,bg]，其中θ为挂车相对拖车车头姿态角，bg为陀螺仪的零偏,则有***状态方程如下：
Figure BDA0002811709150000103
其中
Figure BDA0002811709150000104
为先验状态量，F为状态转移矩阵，B为控制输入模型矩阵，uk为控制量，则有如下：
Figure BDA0002811709150000111
Pk|k-1＝FPk-1|k-1FT+Qk；
更新过程如下：Kk＝Pk|k-1HT(HPk|k-1HT+R)；
Figure BDA0002811709150000112
Figure BDA0002811709150000113
Pk|k＝(I-KkH)Pk|k-1。
其中zk为激光雷达对挂车姿态估计时为
Figure BDA0002811709150000114
zk为运动学模型估算出的姿态角时为
Figure BDA0002811709150000115
在获得最终姿态角之后，控制器还可以将其发送到其他的终端设备中，例如：将挂车的最终姿态角发送至终端设备中，以在终端设备显示挂车的最终姿态角。
终端设备可以为用户的手机、电脑、平板电脑或者其他终端等可以显示数据的设备，终端设备在接收到最终姿态角后，可以通过文字、表格、图画等形式在终端设备中备显示挂车的最终姿态角。
综上，本发明实施例公开一种基于激光雷达和IMU的挂车姿态估计方法及***，方法主要包括以下步骤：IMU安装在拖车挂车铰接点处，激光雷达安装在拖车车顶中央；对激光雷达的安装角进行离线标定；采用安装在车顶的三维激光雷达对挂车集装箱进行扫描，将扫描到的集装箱的点云数据进行ROI分割、筛选、拟合，从而计算出挂车在激光雷达坐标系下的姿态角：翻滚角、俯仰角、横摆角；基于速度分解，对拖车-挂车进行运动学建模；利用卡尔曼滤波算法对由运动学模型和IMU分别给出的预测值与由激光雷达点云数据估算出的观测值进行滤波，从而得到准确的挂车姿态角。该挂车姿态估计方法具备精确性和良好的鲁棒性，能很好地适应各种工况，可用于商用车的姿态估计任务。
本发明实施例提供的上述方法，利用激光雷达点云数据对挂车姿态角进行估算的方法，其通过安装在拖车车顶的激光雷达，实时采集并存储激光雷达扫描返回的点云数据，根据拖车运动状态信息以及挂车的几何特征，选定感兴趣区域，从而筛选出特征点，然后将特征点从三维点云分别投影到XY，XZ，YZ二维平面得到点集，并利用最小二乘法对特征点进行线性拟合，从而得到挂车姿态角，最后与通过拖车-挂车运动学模型估算出的姿态角分别作为观测量，与IMU通过积分得到的姿态角再进行融合滤波，从而保证得到鲁棒可靠的挂车姿态角。
基于上述一种基于激光雷达和IMU的挂车姿态估计方法，本发明还提供一种基于激光雷达和IMU的挂车姿态估计的***，如图3所示的一种基于激光雷达和IMU的挂车姿态估计的***包括以下功能模块：
数据采集模块，用于在拖车上方安装激光雷达，实时采集并存储激光雷达返回的点云数据；用于在挂车上设置惯性测量单元，采集并返回数据；
特征点提取模块，利用标定好的拖车转向、速度与挂车几何特征的关系，根据实时的拖车状态信息确定挂车所在的感兴趣区域，从而选出该工况下对应的特征点；特征点提取模块，用于根据拖车运动状态信息以及转向角度，选定感兴趣区域并剔除一些奇异点(如跳变较大的点)，从而提取点云数据中的特征点；
投影拟合模块，用于将特征点从三维点云降维到二维平面，并通过最小二乘法把二维平面上的特征点拟合成一条直线，计算其斜率，从而根据直线斜率确定挂车的姿态角；
姿态融合模块，用于根据直线的斜率计算出挂车在激光雷达坐标系下的姿态，与通过truck-trailer运动学模型估算出的姿态角分别作为观测量，与IMU通过积分得到的姿态角再进行融合滤波，从而保证得到鲁棒可靠的挂车姿态角。
实施例3
对应于上述方法实施例，本发明实施例提供了一种姿态估计装置，该装置应用于控制器，激光雷达设置在拖车车顶，惯性测量仪设置在拖车和挂车的连接点；控制器与激光雷达通信连接，控制器与惯性测量仪通信连接；如图5示的一种姿态估计装置的结构示意图，该姿态估计装置包括：
第一姿态角确定模块51，用于接收激光雷达发送的点云数据，基于点云数据确定挂车的第一姿态角；
第二姿态角确定模块52，用于基于速度分解对拖车和挂车建立运动学模型，基于运动学模型确定挂车的第二姿态角；
第三姿态角确定模块53，用于接收惯性测量仪发送的测量数据，基于测量数据确定挂车的第三姿态角；
最终姿态角确定模块54，用于基于卡尔曼滤波算法对第一姿态角、第二姿态角和第三姿态角进行融合滤波，得到挂车的最终姿态角。
本发明实施例提供的一种姿态估计装置，控制器根据接收的激光雷达发送的点云数据确定挂车的第一姿态角；根据对拖车和挂车速度分解建立的运动学模型确定挂车的第二姿态角；根据惯性测量仪发送的测量数据确定挂车的第三姿态角；最终基于卡尔曼滤波算法对第一姿态角、第二姿态角和第三姿态角进行融合滤波，得到挂车的最终姿态角。该方式中只需要设置激光雷达和惯性测量仪，无需设置其他传感器就可以准确计算姿态角，成本较低；并且具备精确性和良好的鲁棒性，能很好地适应自动驾驶的各种工况，并且可用于商用车的姿态估计任务。
上述第一姿态角确定模块，用于对激光雷达的安装角进行离线标定，确定激光雷达的坐标系相对于拖车的坐标系的姿态偏差；接收激光雷达发送的点云数据；分割点云数据的感兴趣区域，得到激光雷达的坐标系下的姿态角；基于激光雷达的坐标系相对于拖车的姿态偏差和激光雷达的坐标系下的姿态角，确定拖车的坐标系下的姿态角；将拖车的坐标系下的姿态角作为第一姿态角。
上述第一姿态角确定模块，用于基于预先设定的数据采集周期，周期性接收激光雷达发送的点云数据。
上述第一姿态角确定模块，用于基于预先标定的拖车转向、速度与挂车几何特征的关系，根据点云数据确定挂车的感兴趣区域，并且从点云数据中提取特征点；其中，点云数据为三维平面；将特征点从三维平面中降维到二维平面；基于最小二乘法对二维平面的特征点进行线性拟合；根据线性拟合得到的直线斜率，确定激光雷达的坐标系下的姿态角。
上述运动学模型包括挂车和挂车行驶的距离、挂车的横摆角、拖车的横摆角、挂车的轴距和拖车的轴距。
上述第三姿态角确定模块，用于对测量数据进行积分处理，得到挂车的第三姿态角。
上述装置还包括：最终姿态角发送模块，用于将挂车的最终姿态角发送至终端设备中，以在终端设备显示挂车的最终姿态角。
本发明实施例提供的姿态估计装置，与上述实施例提供的姿态估计方法具有相同的技术特征，所以也能解决相同的技术问题，达到相同的技术效果。
实施例4
本发明实施例还提供了一种电子设备，用于运行上述姿态估计方法；参见图6所示的一种电子设备的结构示意图，该电子设备包括存储器100和处理器101，其中，存储器100用于存储一条或多条计算机指令，一条或多条计算机指令被处理器101执行，以实现上述姿态估计方法。
进一步地，图6所示的电子设备还包括总线102和通信接口103，处理器101、通信接口103和存储器100通过总线102连接。
其中，存储器100可能包含高速随机存取存储器(RAM，Random Access Memory)，也可能还包括非不稳定的存储器(non-volatile memory)，例如至少一个磁盘存储器。通过至少一个通信接口103(可以是有线或者无线)实现该***网元与至少一个其他网元之间的通信连接，可以使用互联网，广域网，本地网，城域网等。总线102可以是ISA总线、PCI总线或EISA总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示，图6中仅用一个双向箭头表示，但并不表示仅有一根总线或一种类型的总线。
处理器101可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器101中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器101可以是通用处理器，包括中央处理器(Central Processing Unit，简称CPU)、网络处理器(Network Processor，简称NP)等；还可以是数字信号处理器(DigitalSignal Processor，简称DSP)、专用集成电路(Application Specific IntegratedCircuit，简称ASIC)、现场可编程门阵列(Field-Programmable Gate Array，简称FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本发明实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器100，处理器101读取存储器100中的信息，结合其硬件完成前述实施例的方法的步骤。
本发明实施例还提供了一种计算机可读存储介质，该计算机可读存储介质存储有计算机可执行指令，该计算机可执行指令在被处理器调用和执行时，计算机可执行指令促使处理器实现上述姿态估计方法，具体实现可参见方法实施例，在此不再赘述。
本发明实施例所提供的姿态估计方法、装置和电子设备的计算机程序产品，包括存储了程序代码的计算机可读存储介质，程序代码包括的指令可用于执行前面方法实施例中的方法，具体实现可参见方法实施例，在此不再赘述。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的装置和/或电子设备的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
最后应说明的是：以上所述实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。

Claims (8)

1.一种姿态估计方法，其特征在于，应用于控制器，激光雷达设置在拖车车顶，惯性测量仪设置在所述拖车和挂车的连接点；所述控制器与所述激光雷达通信连接，所述控制器与所述惯性测量仪通信连接；所述方法包括：
接收所述激光雷达发送的点云数据，基于所述点云数据确定所述挂车的第一姿态角；
基于速度分解对所述拖车和所述挂车建立运动学模型，基于所述运动学模型确定所述挂车的第二姿态角；
接收所述惯性测量仪发送的测量数据，基于所述测量数据确定所述挂车的第三姿态角；
基于卡尔曼滤波算法对所述第一姿态角、所述第二姿态角和所述第三姿态角进行融合滤波，得到所述挂车的最终姿态角；
其中，接收所述激光雷达发送的点云数据，基于所述点云数据确定所述挂车的第一姿态角的步骤，包括：
对所述激光雷达的安装角进行离线标定，确定所述激光雷达的坐标系相对于所述拖车的坐标系的姿态偏差；
接收所述激光雷达发送的点云数据；
分割所述点云数据的感兴趣区域，得到所述激光雷达的坐标系下的姿态角；
基于所述激光雷达的坐标系相对于所述拖车的姿态偏差和所述激光雷达的坐标系下的姿态角，确定所述拖车的坐标系下的姿态角；
将所述拖车的坐标系下的姿态角作为第一姿态角；
其中，分割所述点云数据的感兴趣区域，得到所述激光雷达的坐标系下的姿态角的步骤，包括：
基于预先标定的拖车转向、速度与挂车几何特征的关系，根据所述点云数据确定所述挂车的感兴趣区域，并且从所述点云数据中提取特征点；其中，所述点云数据为三维平面；
将所述特征点从所述三维平面中降维到二维平面；
基于最小二乘法对所述二维平面的特征点进行线性拟合；
根据所述线性拟合得到的直线斜率，确定所述激光雷达的坐标系下的姿态角。
2.根据权利要求1所述的方法，其特征在于，接收所述激光雷达发送的点云数据的步骤，包括：
基于预先设定的数据采集周期，周期性接收所述激光雷达发送的点云数据。
3.根据权利要求1所述的方法，其特征在于，所述运动学模型包括所述挂车和所述挂车行驶的距离、所述挂车的横摆角、所述拖车的横摆角、所述挂车的轴距和所述拖车的轴距。
4.根据权利要求1所述的方法，其特征在于，基于所述测量数据确定所述挂车的第三姿态角的步骤，包括：
对所述测量数据进行积分处理，得到所述挂车的第三姿态角。
5.根据权利要求1所述的方法，其特征在于，基于卡尔曼滤波算法对所述第一姿态角、所述第二姿态角和所述第三姿态角进行融合滤波，得到所述挂车的最终姿态角的步骤之后，所述方法包括：
将所述挂车的最终姿态角发送至终端设备中，以在所述终端设备显示所述挂车的最终姿态角。
6.一种姿态估计装置，其特征在于，应用于控制器，激光雷达设置在拖车车顶，惯性测量仪设置在所述拖车和挂车的连接点；所述控制器与所述激光雷达通信连接，所述控制器与所述惯性测量仪通信连接；所述装置包括：
第一姿态角确定模块，用于接收所述激光雷达发送的点云数据，基于所述点云数据确定所述挂车的第一姿态角；
第二姿态角确定模块，用于基于速度分解对所述拖车和所述挂车建立运动学模型，基于所述运动学模型确定所述挂车的第二姿态角；
第三姿态角确定模块，用于接收所述惯性测量仪发送的测量数据，基于所述测量数据确定所述挂车的第三姿态角；
最终姿态角确定模块，用于基于卡尔曼滤波算法对所述第一姿态角、所述第二姿态角和所述第三姿态角进行融合滤波，得到所述挂车的最终姿态角；
所述第一姿态角确定模块，用于对所述激光雷达的安装角进行离线标定，确定所述激光雷达的坐标系相对于所述拖车的坐标系的姿态偏差；接收所述激光雷达发送的点云数据；分割所述点云数据的感兴趣区域，得到所述激光雷达的坐标系下的姿态角；基于所述激光雷达的坐标系相对于所述拖车的姿态偏差和所述激光雷达的坐标系下的姿态角，确定所述拖车的坐标系下的姿态角；将所述拖车的坐标系下的姿态角作为第一姿态角；
所述第一姿态角确定模块，用于基于预先标定的拖车转向、速度与挂车几何特征的关系，根据所述点云数据确定所述挂车的感兴趣区域，并且从所述点云数据中提取特征点；其中，所述点云数据为三维平面；将所述特征点从所述三维平面中降维到二维平面；基于最小二乘法对所述二维平面的特征点进行线性拟合；根据所述线性拟合得到的直线斜率，确定所述激光雷达的坐标系下的姿态角。
7.一种电子设备，其特征在于，包括处理器和存储器，所述存储器存储有能够被所述处理器执行的计算机可执行指令，所述处理器执行所述计算机可执行指令以实现权利要求1-5任一项所述的姿态估计方法的步骤。
8.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令在被处理器调用和执行时，所述计算机可执行指令促使处理器实现权利要求1-5任一项所述的姿态估计方法的步骤。
CN202011391580.3A 2020-12-01 2020-12-01 姿态估计方法、装置和电子设备 Active CN112362055B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011391580.3A CN112362055B (zh) 2020-12-01 2020-12-01 姿态估计方法、装置和电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011391580.3A CN112362055B (zh) 2020-12-01 2020-12-01 姿态估计方法、装置和电子设备

Publications (2)

Publication Number Publication Date
CN112362055A CN112362055A (zh) 2021-02-12
CN112362055B true CN112362055B (zh) 2022-12-30

Family

ID=74536598

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011391580.3A Active CN112362055B (zh) 2020-12-01 2020-12-01 姿态估计方法、装置和电子设备

Country Status (1)

Country Link
CN (1) CN112362055B (zh)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113063396B (zh) * 2021-03-19 2022-09-27 北京斯年智驾科技有限公司 一种半挂车夹角的检测方法、装置、设备及存储介质
CN113124906A (zh) * 2021-05-06 2021-07-16 苏州挚途科技有限公司 基于在线标定的测距方法、装置及电子设备
CN113296107A (zh) * 2021-06-23 2021-08-24 上海西井信息科技有限公司 传感器协同检测拖挂角度的方法、***、设备及存储介质
CN113470088B (zh) * 2021-06-24 2022-09-20 上海于万科技有限公司 一种车挂箱状态的判断方法和***
CN113819880A (zh) * 2021-09-27 2021-12-21 江苏星图智能科技有限公司 牵引挂车夹角实时获取方法
CN113865586B (zh) * 2021-09-29 2024-02-06 苏州挚途科技有限公司 安装角度的估计方法、装置和自动驾驶***
CN114001729B (zh) * 2021-11-16 2024-04-26 苏州挚途科技有限公司 定位方法、装置及电子设备
CN117657144A (zh) * 2022-08-26 2024-03-08 北汽福田汽车股份有限公司 车辆控制的方法、装置、电子设备和车辆

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2001334966A (ja) * 2000-03-24 2001-12-04 Isuzu Motors Ltd トレーラ連結角検出装置
CN105270408A (zh) * 2014-05-27 2016-01-27 曼卡车和巴士股份公司 用于测定商用车的行驶动态状态的方法和驾驶员辅助***
CN107561570A (zh) * 2017-10-16 2018-01-09 北京耘华科技有限公司 一种拖挂车辆的位置和姿态测量方法及***
CN109959352A (zh) * 2019-03-01 2019-07-02 武汉光庭科技有限公司 利用激光点云计算卡车车头和挂车之间夹角的方法及***
CN111288930A (zh) * 2018-11-20 2020-06-16 北京图森智途科技有限公司 一种挂车夹角的测量方法、装置及车辆
CN111366938A (zh) * 2018-12-10 2020-07-03 北京图森智途科技有限公司 一种挂车夹角的测量方法、装置及车辆
CN111580066A (zh) * 2019-02-18 2020-08-25 上海汽车集团股份有限公司 转向角度检测方法、装置及***
CN111806464A (zh) * 2019-04-03 2020-10-23 伟摩有限责任公司 异常拖车行为的检测

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102017112786A1 (de) * 2017-06-09 2018-12-13 Valeo Schalter Und Sensoren Gmbh Verfahren zum Charakterisieren eines an ein Zugfahrzeug angehängten Anhängers, Fahrerassistenzsystem sowie Gespann
DE102017112785A1 (de) * 2017-06-09 2018-12-13 Valeo Schalter Und Sensoren Gmbh Verfahren zum Unterstützen eines Fahrers eines Gespanns beim Manövrieren mit dem Gespann, Totwinkelsystem sowie Gespann
US11125881B2 (en) * 2018-12-10 2021-09-21 Waymo Llc Lidar-based trailer tracking

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2001334966A (ja) * 2000-03-24 2001-12-04 Isuzu Motors Ltd トレーラ連結角検出装置
CN105270408A (zh) * 2014-05-27 2016-01-27 曼卡车和巴士股份公司 用于测定商用车的行驶动态状态的方法和驾驶员辅助***
CN107561570A (zh) * 2017-10-16 2018-01-09 北京耘华科技有限公司 一种拖挂车辆的位置和姿态测量方法及***
CN111288930A (zh) * 2018-11-20 2020-06-16 北京图森智途科技有限公司 一种挂车夹角的测量方法、装置及车辆
CN111366938A (zh) * 2018-12-10 2020-07-03 北京图森智途科技有限公司 一种挂车夹角的测量方法、装置及车辆
CN111580066A (zh) * 2019-02-18 2020-08-25 上海汽车集团股份有限公司 转向角度检测方法、装置及***
CN109959352A (zh) * 2019-03-01 2019-07-02 武汉光庭科技有限公司 利用激光点云计算卡车车头和挂车之间夹角的方法及***
CN111806464A (zh) * 2019-04-03 2020-10-23 伟摩有限责任公司 异常拖车行为的检测

Also Published As

Publication number Publication date
CN112362055A (zh) 2021-02-12

Similar Documents

Publication Publication Date Title
CN112362055B (zh) 姿态估计方法、装置和电子设备
US11320833B2 (en) Data processing method, apparatus and terminal
US10424081B2 (en) Method and apparatus for calibrating a camera system of a motor vehicle
CN111696160B (zh) 车载摄像头自动标定方法、设备及可读存储介质
US5978504A (en) Fast planar segmentation of range data for mobile robots
US20150329049A1 (en) Object detection device, driving assistance device, object detection method, and object detection program
JP2014146182A (ja) 車載画像処理装置
JP2002197469A (ja) 車線検出装置
CN110816522B (zh) 车辆姿态的控制方法、设备及计算机可读存储介质
CN111223150A (zh) 一种基于双消失点的车载摄像头外参数标定方法
CN112927309A (zh) 一种车载相机标定方法、装置、车载相机及存储介质
CN108550134B (zh) 建图效果指标的确定方法和建图效果指标的确定装置
CN110515376B (zh) 一种航迹推演校正的评价方法、终端和存储介质
CN114972427A (zh) 一种基于单目视觉的目标跟踪方法、终端设备及存储介质
US20200039434A1 (en) Method and system for determining and displaying a wading situation
CN114219770A (zh) 地面检测方法、装置、电子设备及存储介质
CN111332306A (zh) 一种基于机器视觉的交通道路感知辅助驾驶预警装置
US10970592B2 (en) Adhering substance detection apparatus and adhering substance detection method
CN112215214A (zh) 调整智能车载终端的摄像头偏移的方法及***
CN113643355A (zh) 一种目标车辆位置和朝向的检测方法、***及存储介质
CN112477868B (zh) 碰撞时间计算方法、装置、可读存储介质和计算机设备
CN114494200A (zh) 挂车旋转角度的测量方法和装置
JP2013183311A (ja) 車載カメラの姿勢角推定装置及び検出システム
JPH08329397A (ja) 車両認識装置及びそれを用いた車両接近報知装置
CN111332305A (zh) 一种主动预警型交通道路感知辅助驾驶预警***

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