CN116184417A - 一种挂车夹角的测量方法、装置及车辆 - Google Patents
一种挂车夹角的测量方法、装置及车辆 Download PDFInfo
- Publication number
- CN116184417A CN116184417A CN202310175228.3A CN202310175228A CN116184417A CN 116184417 A CN116184417 A CN 116184417A CN 202310175228 A CN202310175228 A CN 202310175228A CN 116184417 A CN116184417 A CN 116184417A
- Authority
- CN
- China
- Prior art keywords
- trailer
- initial
- line laser
- laser radar
- angle
- 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.)
- Pending
Links
Images
Classifications
-
- 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
-
- 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/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- 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/87—Combinations of systems using electromagnetic waves other than radio waves
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
- B62D—MOTOR VEHICLES; TRAILERS
- B62D53/00—Tractor-trailer combinations; Road trains
- B62D53/04—Tractor-trailer combinations; Road trains comprising a vehicle carrying an essential part of the other vehicle's load by having supporting means for the front or rear part of the other vehicle
- B62D53/06—Semi-trailers
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
- G01B11/00—Measuring arrangements characterised by the use of optical techniques
- G01B11/26—Measuring arrangements characterised by the use of optical techniques for measuring angles or tapers; for testing the alignment of axes
-
- 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/06—Systems determining position data of a target
- G01S17/42—Simultaneous measurement of distance and other co-ordinates
-
- 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/88—Lidar systems specially adapted for specific applications
-
- 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/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
-
- 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
- G01—MEASURING; TESTING
- G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
- G01B11/00—Measuring arrangements characterised by the use of optical techniques
- G01B11/24—Measuring arrangements characterised by the use of optical techniques for measuring contours or curvatures
Abstract
本发明提供了一种挂车夹角的测量方法、装置及车辆,涉及车辆技术领域。其应用于一种包括牵引车和挂车的半挂车;在牵引车的两侧分别设置有至少一个多线激光雷达;方法包括:在预先设置的车辆坐标系下,获得挂车夹角为初始值时的挂车初始模型,得到挂车初始模型上的初始点云数据;控制牵引车的两侧分别设置的多线激光雷达发射激光,使得挂车的表面反射多线激光雷达发射的激光;控制各多线激光雷达分别接收挂车的表面反射的各自对应的激光点云;根据各多线激光雷达接收到的各自对应的激光点云和初始点云数据,采用点云匹配算法计算得到挂车夹角。本发明可以实现一种结构简单、快速准确测量挂车夹角的方法。
Description
技术领域
本发明涉及车辆技术领域,尤其涉及一种挂车夹角的测量方法、装置及车辆。
背景技术
目前,随着物流运输行业的发展,集装箱卡车等带有牵引车和挂车的车辆(以下简称半挂车)的应用越来越广泛。半挂车作为一种重型的交通运输工具,相比单体式卡车,半挂车更能够提高公路运输的综合经济效益,而随着自动驾驶技术的发展,挂车夹角(如图1所示的半挂车的上视图,挂车夹角是指牵引车11中轴线和挂车12中轴线之间的夹角α)作为自动驾驶规划和控制点基础成为了研究的重点。
当前的挂车夹角的测量方法仅仅能在挂车夹角比较小的情况下进行测量,对于挂车夹角较大的场景(例如,挂车夹角大于±40°)时,则难以得到准确的挂车夹角。因此,如何实现一种结构简单、快速准确测量挂车夹角的方法成为了一个亟待解决的问题。
发明内容
本发明的实施例提供一种挂车夹角的测量方法、装置及车辆,以实现一种结构简单、快速准确测量挂车夹角的方法。
为达到上述目的,本发明采用如下技术方案:
一方面,本发明实施例提供一种挂车夹角的测量方法,应用于一种半挂车,所述半挂车包括牵引车和挂车;在所述牵引车的两侧分别设置有至少一个多线激光雷达;
所述挂车夹角的测量方法,包括:
在预先设置的车辆坐标系下,获得挂车夹角为初始值时的挂车初始模型,得到所述挂车初始模型上的初始点云数据;
控制所述牵引车的两侧分别设置的多线激光雷达发射激光,使得所述挂车的表面反射多线激光雷达发射的激光;
控制各多线激光雷达分别接收挂车的表面反射的各自对应的激光点云;
根据各多线激光雷达接收到的各自对应的激光点云和所述初始点云数据,采用点云匹配算法计算得到挂车夹角。
又一方面,本发明实施例还提供一种挂车夹角的测量装置,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,所述处理器执行所述计算机程序时实现上述的挂车夹角的测量方法。
又一方面,本发明实施例还提供一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现上述的挂车夹角的测量方法。
又一方面,本发明实施例还提供一种车辆,包括上述的挂车夹角的测量装置,以及牵引车和挂车;在所述牵引车的两侧分别设置有至少一个多线激光雷达;所述挂车夹角的测量装置与所述多线激光雷达通信连接。
本发明实施例提供的一种挂车夹角的测量方法、装置及车辆,采用在牵引车的两侧分别设置至少一个多线激光雷达,两侧设置的多线激光雷达可以照射到挂车的表面,这样根据各多线激光雷达接收到的各自对应的激光点云和初始点云数据,采用点云匹配算法即可计算得到挂车夹角。另外,本发明采用根据各多线激光雷达接收到的各自对应的激光点云和挂车夹角为初始值时的初始点云数据即可进行计算得到挂车夹角,而不是采用单一的激光雷达的激光点云进行计算,其结果准确度大大提高。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图。
图1为挂车夹角的示意图;
图2为本发明实施例提供的一种挂车夹角的测量方法的流程图一;
图3为本发明实施例中的半挂车结构的下视图;
图4为本发明实施例中牵引车的尾部仅设置一个激光雷达时的工作场景示意图;
图5为本发明实施例中牵引车的两侧分别设置一个多线激光雷达时的工作场景示意图;
图6为本发明实施例中的牵引车的两侧设置多线激光雷达的分布示意图一;
图7为本发明实施例中的牵引车的两侧设置多线激光雷达的分布示意图二;
图8为本发明实施例提供的一种挂车夹角的测量方法的流程图二;
图9为本发明实施例中的外部多线激光雷达的采集环境示意图;
图10为本发明实施例中的预设区域范围的示意图;
图11为本发明实施例中所建立的一种激光雷达坐标系示意图;
图12为本发明实施例中的待处理角度数据的曲线示意图;
图13为本发明实施例中的车辆的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
为了使本领域的技术人员更加了解本发明,下面对本发明实施例中所涉及的部分技术术语进行解释说明:
点云:在逆向工程中通过测量仪器得到的物体外观表面的点数据集合称为点云。
ICP:Iterative Closest Point,迭代最近点算法,主要用于计算机视觉中深度图像的精确拼合,通过不断迭代最小化源数据与目标数据对应点来实现精确地拼合。目前已经有很多变种,主要热点是怎样高效、鲁棒的获得较好地拼合效果。
SVD:Singular Value Decomposition,奇异值分解算法,是一种可靠的求解平移向量与旋转矩阵的方法。
如图2所示,本发明实施例提供一种挂车夹角的测量方法,应用于如图3(图3为半挂车20的下视图)所示的一种半挂车20,半挂车20包括牵引车201和挂车202,牵引车201和挂车202通过转轴205连接,使得挂车202相对于牵引车201可以转动;在牵引车201的两侧(如牵引车201的前端左右两侧,即车辆前脸的左右两侧)分别设置有至少一个多线激光雷达203(例如可以是左右两侧分别设置一个多线激光雷达,两个多线激光雷达或者更多的多线激光雷达,此处由于图3的限制,仅展示了左右两侧分别设置一个多线激光雷达的情况)。
该挂车夹角的测量方法,包括:
步骤301、在预先设置的车辆坐标系下,获得挂车夹角为初始值时的挂车初始模型,得到挂车初始模型上的初始点云数据。
步骤302、控制牵引车的两侧分别设置的多线激光雷达发射激光,使得挂车的表面反射多线激光雷达发射的激光。
步骤303、控制各多线激光雷达分别接收挂车的表面反射的各自对应的激光点云。
步骤304、根据各多线激光雷达接收到的各自对应的激光点云和初始点云数据,采用点云匹配算法计算得到挂车夹角。
此处,如图4所示,在挂车202的前部还可以固定设置有具有反射面的反射板204,在牵引车201的尾部可以设置一个激光雷达206(一般设置在牵引车的尾部的中间位置,激光雷达一般采用单线激光雷达)该反射面朝向激光雷达206。考虑到在牵引车201的尾部仅设置一个激光雷达206也可以向反射板204的反射面发射激光,通过该一个激光雷达206一般也能够完成挂车夹角的测量。但是,对于挂车夹角较大的场景,如图4所示,反射板204可能已经移动到了牵引车201的侧面,进入到了激光雷达206的盲区,单一的激光雷达206发射的激光已经无法射到反射板204的反射面上,造成挂车夹角的测量失败。因此,本发明实施例中也可以采用如图4所示的方式来进行挂车夹角较小(例如挂车夹角小于40°)的场景的挂车夹角的测量,而采用上述步骤301至步骤304来进行挂车夹角较大(例如挂车夹角大于等于40°)的场景的挂车夹角的测量,但不仅局限于此。本发明实施例中的上述步骤301至步骤304也可以进行挂车夹角较小的场景的挂车夹角的测量。
而如图5所示,本发明采用在牵引车201的两侧(如牵引车201的前端左右两侧,即前脸的左右两侧)分别设置至少一个多线激光雷达203(例如可以是左右两侧分别设置一个多线激光雷达,两个多线激光雷达或者更多的多线激光雷达,此处由于图5的限制,仅展示了左右两侧分别设置一个多线激光雷达的情况),这样,在反射板204移动到了牵引车201的侧面时,至少其中一侧的多线激光雷达发射的激光可以射到挂车的表面,从而能够被用于挂车夹角的测量。另外,本发明采用根据各多线激光雷达接收到的各自对应的激光点云和挂车夹角为初始值时的初始点云数据进行计算得到挂车夹角,而不是采用单一的激光雷达的激光点云进行计算,其结果准确度大大提高。
此处,为了表明牵引车201的两侧分别设置至少一个多线激光雷达203,如图6和图7所示,多线激光雷达203在牵引车201的前端的分布方式可以为左右两侧各有一个多线激光雷达203(图6),或者各有两个多线激光雷达203(图7),但不仅局限于此,在不考虑成本的情况下,甚至还可以在左右两侧设置更多的多线激光雷达。
为了使本领域的技术人员更加了解本发明,下面列举一个更为详细的实施例,如图8所示,本发明实施例提供一种挂车夹角的测量方法,应用于上述如图3所示的半挂车20,该半挂车20的结构在上述已有描述,此处不再赘述。该方法包括:
步骤401、在预先设置的车辆坐标系下,采用预先设置的外部多线激光雷达向挂车夹角为0°时的挂车两侧发射激光,使得挂车两侧的表面分别反射外部多线激光雷达发射的激光。
步骤402、控制外部多线激光雷达接收挂车两侧的表面反射的激光点云。
步骤403、将挂车两侧的表面反射的激光点云采用迭代最近点算法得到挂车夹角为0°时的挂车初始模型,得到挂车初始模型上的初始点云数据。
一般情况下,上述步骤401至步骤403可以采用如下方式:如图9所示,可以在车辆行驶的车道一侧设置支架501,将外部多线激光雷达502架设在该支架501上(如架设高度在2m以上),这样,车辆可以保持挂车夹角为0°,先从一侧进入到外部多线激光雷达502的激光发射范围,再从另一侧进入到外部多线激光雷达502的激光发射范围,从而挂车两侧均可以被激光照射,外部多线激光雷达502也可以采集到挂车两侧的激光点云。之后经过迭代最近点算法可以得到挂车夹角为0°时的挂车初始模型,进而可以得到挂车初始模型上的初始点云数据,此处的初始点云数据记为{Pt}。
步骤404、控制牵引车的两侧分别设置的多线激光雷达发射激光,使得挂车的表面反射多线激光雷达发射的激光。
值得说明的是,本发明实施例中所采用多线激光雷达和外部多线激光雷达可以采用如16线、32线、64线激光雷达等,但不仅局限于此。
步骤405、控制各多线激光雷达分别接收挂车的表面反射的各自对应的激光点云。
一般情况下,各多线激光雷达的采样频率可以为10Hz,但不仅局限于此。另外,多线激光雷达在向外发射激光时带有自身对应的标识,从而可以保证多线激光雷达在接收挂车的表面反射的激光点云时,仅接收自身对应的激光点云,而不会接收到其他多线激光雷达对应的激光点云。另外,为了使得后续步骤408中各多线激光雷达各自对应的初始挂车夹角以当前周期中的采集时刻进行排列,各多线激光雷达的采集时刻优选不相同,以避免在同一采集时刻两个或更多的初始挂车夹角在一起,难以分辨。另外,由于半挂车20的结构特点,在存在一定挂车夹角时,一般仅有一侧的多线激光雷达可以接收挂车的表面反射的激光点云。
步骤406、对各多线激光雷达分别接收到的各自对应的激光点云进行预处理,并根据初始点云数据和预处理后的激光点云,采用迭代最近点算法得到各多线激光雷达各自对应的初始挂车夹角。
此处的步骤406可以采用迭代最近点算法来实现:
步骤1、对各多线激光雷达分别接收到的各自对应的激光点云进行感兴趣区域滤波,得到预设区域范围内的激光点云。
此处的预设区域范围可以采用如下方式确定,即:根据上一周期的挂车夹角及预先获知的挂车尺寸,将在上一周期的挂车外部的一预设距离区域范围确定为预设区域范围。
例如可以采用如下方式:
如图10所示,上一周期的挂车夹角及预先获知的挂车尺寸已经获知的情况下,可以得到挂车当前的姿态,从而可以将上一周期的挂车外部的一预设距离区域范围(图中阴影部分仅描述了在平面范围内的区域范围,实际上,在挂车的上部和下部也同样具有类似的区域范围),这样可以将该立体的区域范围作为预设区域范围。如此设置的原因是相邻周期的时间较短(一般为0.1s),挂车夹角的变化较为微小,因此挂车在当前周期与上一周期的变化较小,应处于该预设区域范围内。
步骤2、对预设区域范围内的激光点云进行噪点滤波,得到各多线激光雷达对应的噪点滤波后的激光点云,构成各多线激光雷达对应的当前次的挂车模型。
此处,采用噪点滤波可以将离群的点滤除,从而得到更为准确的激光点云。此处,该当前次的挂车模型的各个点构成的点集记为{Pn},其中n为正整数,例如{P1}表示第一次的挂车模型的各个点构成的点集。
步骤3、对于各当前次的挂车模型中的每个点,在初始点云数据集中确定与该每个点直线距离最近的点作为目标点。
步骤4、将每个点采用奇异值分解算法移动到每个点各自对应的目标点处,得到下一次的挂车模型,并生成当前次迭代的模型矩阵。
此处,当前次迭代的模型矩阵包括当前次迭代的旋转矩阵和当前次迭代的平移矩阵;当前次迭代的旋转矩阵包括车辆坐标系下的三个坐标轴当前次的偏转角度的三角函数关系。
在步骤4之后返回执行步骤3,直至当前次的挂车模型中的每个点与目标点的距离小于预先设置的距离阈值,继续执行步骤5。
本发明实施例中,在安装好多线激光雷达之后,建立激光雷达坐标系,多线激光雷达的激光点云的位置信息均基于该激光雷达坐标系中,如可建立如图11所示的激光雷达坐标系,但不仅局限于此,在激光雷达的坐标系建立中,还可以选择其他方向作为x轴,在水平面的一与x轴垂直的方向为y轴,与x轴和y轴均垂直的方向为z轴(图中未示出,一般为竖直向上方向),此处不再一一列举。
这样,经过每次迭代,得到的模型矩阵An可以表示为:其中,Rn表示第n次迭代的旋转矩阵,为一3行3列的矩阵;Tn表示当前次迭代的平移矩阵,为一3行1列的矩阵。该第n次迭代的旋转矩阵Rn一般经过3次旋转组成,即Rn=Rxn·Ryn·Rzn;Rxn为第n次迭代在车辆坐标系下的x轴的旋转矩阵,Ryn为第n次迭代在车辆坐标系下的y轴的旋转矩阵,Rzn为第n次迭代在车辆坐标系下的z轴的旋转矩阵;例如,在一实施例中,
其中,θxn为第n次迭代在车辆坐标系下的x轴的偏转角度;θyn为第n次迭代在车辆坐标系下的y轴的偏转角度;θzn为第n次迭代在车辆坐标系下的z轴的偏转角度。
步骤5、将每次迭代的模型矩阵相乘,得到相乘后的结果矩阵。
此处,例如共进行了n次迭代,则将每次迭代的模型矩阵相乘,得到相乘后的结果矩阵为A=An·A(n-1)·......·A2·A1,该结果矩阵中包括结果旋转矩阵和结果平移矩阵;结果旋转矩阵包括车辆坐标系下的三个坐标轴的偏转角度的三角函数关系。例如,可以将结果矩阵A中的结果旋转矩阵R表示为m00至m22表示结果旋转矩阵R中相应位置的偏转角度的三角函数关系,由于此处的三角函数关系较为复杂,此处不再列举具体的三角函数关系。
步骤6、根据结果旋转矩阵,确定各多线激光雷达各自对应的初始挂车夹角。
θx=atan2(m12,m22)
θy=atan2(-m02,cosθy)
θz=atan2(sinθx·m20-cosθx·m10,cosθx·m11-sinθx·m21)
其中,θx、θy、θz分别为当前周期绕x轴、y轴和z轴的偏转角度。而θz即相当于初始挂车夹角。
步骤407、根据预先设置的判断条件对各多线激光雷达各自对应的初始挂车夹角进行筛选。
此处,该步骤407可以采用如下两种方式实现,当然也可以通过该两种方式结合来实现:
方式一:
判断各多线激光雷达对应的噪点滤波后的激光点云的点数是否小于预先设置的点数阈值。
将多线激光雷达对应的噪点滤波后的激光点云的点数小于预先设置的点数阈值的多线激光雷达对应的初始挂车夹角剔除,保留多线激光雷达对应的噪点滤波后的激光点云的点数大于等于预先设置的点数阈值的多线激光雷达对应的初始挂车夹角。
此处,若多线激光雷达对应的噪点滤波后的激光点云的点数小于预先设置的点数阈值,则说明多线激光雷达对应的当前次的挂车模型是由较少的激光点云拟合而成的,若仅通过少数的激光点云拟合得到当前次的挂车模型,则得到的初始挂车夹角十分不准确,应被剔除。
方式二:
在当前周期非第一周期时,判断当前周期的各多线激光雷达各自对应的初始挂车夹角与进行卡尔曼滤波处理得到的上一周期的挂车夹角的偏差角度值是否大于预先设置的偏差角度阈值。
若当前周期的多线激光雷达对应的初始挂车夹角与进行卡尔曼滤波处理得到的上一周期的挂车夹角的偏差角度值大于预先设置的偏差角度阈值,则将当前周期的该多线激光雷达对应的初始挂车夹角剔除。
若当前周期的多线激光雷达对应的初始挂车夹角与进行卡尔曼滤波处理得到的上一周期的挂车夹角的偏差角度值小于等于预先设置的偏差角度阈值,则将当前周期的该多线激光雷达对应的初始挂车夹角保留。
此处,由于两个相邻周期(一般仅相差0.1秒)之间相差的时长较短,因此挂车夹角的变化不会过大,因此若当前周期的多线激光雷达对应的初始挂车夹角与进行卡尔曼滤波处理得到的上一周期的挂车夹角的偏差角度值大于预先设置的偏差角度阈值,则确定当前周期的多线激光雷达对应的初始挂车夹角为无效数据,应被剔除。
步骤408、将进行筛选后的各多线激光雷达各自对应的初始挂车夹角以当前周期中的采集时刻进行排列,形成待处理角度数据。
例如,以在牵引车的前脸的左右两侧各有两个多线激光雷达为例(例如此时仅左侧的两个多线激光雷达对应测得了初始挂车夹角),多线激光雷达的测量周期为0.1s,两个多线激光雷达的测量时刻相差0.05s,则如图12所示,横坐标为初始挂车夹角对应的时刻,纵坐标为初始挂车夹角的角度,整个纵坐标数据构成了待处理角度数据。
步骤409、根据待处理角度数据进行卡尔曼滤波处理,得到当前周期的挂车夹角。
此处进行卡尔曼滤波处理的原因是由于挂车表面本身并不平整,而且多线激光雷达自身也有观测误差,使得计算得到的初始挂车夹角存在一定误差,表现出来的现象为在车辆静止时,夹角也会有正负1°至2°的跳跃。为了解决这个问题,卡尔曼滤波处理能够对待处理角度数据中随着采集时刻排列的初始挂车夹角进行降噪处理,将初始挂车夹角和夹角变化的简易运动学模型融合起来得到一个平顺的输出结果,不仅能够确保在静止状态下测量挂车夹角测量数据的误差在正负0.5°以内,而且还能够确保在挂车夹角急速变化时测量数据可以相应的实时变化,避免明显的延迟。
在上述步骤409之后,可以返回步骤404进行下一周期的挂车夹角测量。
可见,通过上述步骤401至步骤409即可实现一种结构简单、快速准确测量挂车夹角的方法。
另外,本发明实施例还提供一种挂车夹角的测量装置,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,所述处理器执行所述计算机程序时实现上述图2或图8所对应的方法。
另外,本发明实施例还提供一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现上述图2或图8所对应的方法。
此外,如图13所示,本发明实施例还提供一种车辆50,包括上述的挂车夹角的测量装置601,以及牵引车201和挂车202(本发明所涉及的挂车202上可以搭载有挂箱);在所述牵引车201的两侧(如可以在牵引车201前端两侧,即车辆前脸的两侧)分别设置有至少一个多线激光雷达203;该挂车夹角的测量装置601与多线激光雷达203通信连接。
本发明实施例提供的一种挂车夹角的测量方法、装置及车辆,采用在牵引车的两侧分别设置至少一个多线激光雷达,两侧设置的多线激光雷达可以照射到挂车的表面,这样根据各多线激光雷达接收到的各自对应的激光点云和初始点云数据,采用点云匹配算法即可计算得到挂车夹角。另外,本发明采用根据各多线激光雷达接收到的各自对应的激光点云和挂车夹角为初始值时的初始点云数据即可进行计算得到挂车夹角,而不是采用单一的激光雷达的激光点云进行计算,其结果准确度大大提高。
本领域内的技术人员应明白,本发明的实施例可提供为方法、系统、或计算机程序产品。因此,本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上,使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
本发明中应用了具体实施例对本发明的原理及实施方式进行了阐述,以上实施例的说明只是用于帮助理解本发明的方法及其核心思想;同时,对于本领域的一般技术人员,依据本发明的思想,在具体实施方式及应用范围上均会有改变之处,综上所述,本说明书内容不应理解为对本发明的限制。
Claims (11)
1.一种挂车夹角的测量方法,其特征在于,应用于一种半挂车,所述半挂车包括牵引车和挂车;在所述牵引车的两侧分别设置有至少一个多线激光雷达;
所述挂车夹角的测量方法,包括:
在预先设置的车辆坐标系下,获得挂车夹角为初始值时的挂车初始模型,得到所述挂车初始模型上的初始点云数据;
控制所述牵引车的两侧分别设置的多线激光雷达发射激光,使得所述挂车的表面反射多线激光雷达发射的激光;
控制各多线激光雷达分别接收挂车的表面反射的各自对应的激光点云;
根据各多线激光雷达接收到的各自对应的激光点云和所述初始点云数据,采用点云匹配算法计算得到挂车夹角;
其中,所述根据各多线激光雷达接收到的各自对应的激光点云和所述初始点云数据,采用点云匹配算法计算得到挂车夹角,包括:
根据预先设置的判断条件对各多线激光雷达各自对应的初始挂车夹角进行筛选;
所述根据预先设置的判断条件对各多线激光雷达各自对应的初始挂车夹角进行筛选,包括:
在当前周期非第一周期时,判断当前周期的各多线激光雷达各自对应的初始挂车夹角与进行卡尔曼滤波处理得到的上一周期的挂车夹角的偏差角度值是否大于预先设置的偏差角度阈值。
2.根据权利要求1所述的方法,其特征在于,所述在预先设置的车辆坐标系下,获得挂车夹角为初始值时的挂车初始模型,得到所述挂车初始模型上的初始点云数据,包括:
在预先设置的车辆坐标系下,采用预先设置的外部多线激光雷达向挂车夹角为0°时的挂车两侧发射激光,使得所述挂车两侧的表面分别反射外部多线激光雷达发射的激光;
控制所述外部多线激光雷达接收挂车两侧的表面反射的激光点云;
将挂车两侧的表面反射的激光点云采用迭代最近点算法得到挂车夹角为0°时的挂车初始模型,得到所述挂车初始模型上的初始点云数据。
3.根据权利要求1所述的方法,其特征在于,所述根据各多线激光雷达接收到的各自对应的激光点云和所述初始点云数据,采用点云匹配算法计算得到挂车夹角,包括:
对各多线激光雷达分别接收到的各自对应的激光点云进行预处理,并根据初始点云数据和预处理后的激光点云,采用迭代最近点算法得到各多线激光雷达各自对应的初始挂车夹角;
将进行筛选后的各多线激光雷达各自对应的初始挂车夹角以当前周期中的采集时刻进行排列,形成待处理角度数据;
根据所述待处理角度数据进行卡尔曼滤波处理,得到当前周期的挂车夹角。
4.根据权利要求3所述的方法,其特征在于,所述对各多线激光雷达分别接收到的各自对应的激光点云进行预处理,并根据初始点云数据和预处理后的激光点云,采用迭代最近点算法得到各多线激光雷达各自对应的初始挂车夹角,包括:
步骤1、对各多线激光雷达分别接收到的各自对应的激光点云进行感兴趣区域滤波,得到预设区域范围内的激光点云;
步骤2、对所述预设区域范围内的激光点云进行噪点滤波,得到各多线激光雷达对应的噪点滤波后的激光点云,构成各多线激光雷达对应的当前次的挂车模型;
步骤3、对于各当前次的挂车模型中的每个点,在初始点云数据集中确定与该每个点直线距离最近的点作为目标点;
步骤4、将所述每个点采用奇异值分解算法移动到每个点各自对应的目标点处,得到下一次的挂车模型,并生成当前次迭代的模型矩阵;所述当前次迭代的模型矩阵包括当前次迭代的旋转矩阵和当前次迭代的平移矩阵;所述当前次迭代的旋转矩阵包括车辆坐标系下的三个坐标轴当前次的偏转角度的三角函数关系;
在步骤4之后返回执行步骤3,直至当前次的挂车模型中的每个点与所述目标点的距离小于预先设置的距离阈值,继续执行步骤5;
步骤5、将每次迭代的模型矩阵相乘,得到相乘后的结果矩阵;所述结果矩阵中包括结果旋转矩阵和结果平移矩阵;所述结果旋转矩阵包括车辆坐标系下的三个坐标轴的偏转角度的三角函数关系;
步骤6、根据所述结果旋转矩阵,确定各多线激光雷达各自对应的初始挂车夹角。
5.根据权利要求4所述的方法,其特征在于,还包括:
根据上一周期的挂车夹角及预先获知的挂车尺寸,将在上一周期的挂车外部的一预设距离区域范围确定为所述预设区域范围。
6.根据权利要求1所述的方法,其特征在于,所述根据预先设置的判断条件对各多线激光雷达各自对应的初始挂车夹角进行筛选,还包括:
判断各多线激光雷达对应的噪点滤波后的激光点云的点数是否小于预先设置的点数阈值;
将多线激光雷达对应的噪点滤波后的激光点云的点数小于所述预先设置的点数阈值的多线激光雷达对应的初始挂车夹角剔除,保留多线激光雷达对应的噪点滤波后的激光点云的点数大于等于所述预先设置的点数阈值的多线激光雷达对应的初始挂车夹角。
7.根据权利要求1所述的方法,其特征在于,所述根据预先设置的判断条件对各多线激光雷达各自对应的初始挂车夹角进行筛选,还包括:
若当前周期的多线激光雷达对应的初始挂车夹角与进行卡尔曼滤波处理得到的上一周期的挂车夹角的偏差角度值大于预先设置的偏差角度阈值,则将当前周期的该多线激光雷达对应的初始挂车夹角剔除;
若当前周期的多线激光雷达对应的初始挂车夹角与进行卡尔曼滤波处理得到的上一周期的挂车夹角的偏差角度值小于等于预先设置的偏差角度阈值,则将当前周期的该多线激光雷达对应的初始挂车夹角保留。
8.一种挂车夹角的测量装置,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,其特征在于,所述处理器执行所述计算机程序时实现权利要求1至7任一项所述的方法。
9.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,该程序被处理器执行时实现权利要求1至7任一项所述的方法。
10.一种车辆,其特征在于,包括权利要求8所述的挂车夹角的测量装置,以及牵引车和挂车;在所述牵引车的两侧分别设置有至少一个多线激光雷达;所述挂车夹角的测量装置与所述多线激光雷达通信连接。
11.根据权利要求10所述的车辆,其特征在于,所述牵引车的前端两侧分别设置有至少一个多线激光雷达。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310175228.3A CN116184417A (zh) | 2018-12-10 | 2018-12-10 | 一种挂车夹角的测量方法、装置及车辆 |
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310175228.3A CN116184417A (zh) | 2018-12-10 | 2018-12-10 | 一种挂车夹角的测量方法、装置及车辆 |
CN201811505593.1A CN111366938B (zh) | 2018-12-10 | 2018-12-10 | 一种挂车夹角的测量方法、装置及车辆 |
Related Parent Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811505593.1A Division CN111366938B (zh) | 2018-12-10 | 2018-12-10 | 一种挂车夹角的测量方法、装置及车辆 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN116184417A true CN116184417A (zh) | 2023-05-30 |
Family
ID=71077096
Family Applications (2)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310175228.3A Pending CN116184417A (zh) | 2018-12-10 | 2018-12-10 | 一种挂车夹角的测量方法、装置及车辆 |
CN201811505593.1A Active CN111366938B (zh) | 2018-12-10 | 2018-12-10 | 一种挂车夹角的测量方法、装置及车辆 |
Family Applications After (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811505593.1A Active CN111366938B (zh) | 2018-12-10 | 2018-12-10 | 一种挂车夹角的测量方法、装置及车辆 |
Country Status (6)
Country | Link |
---|---|
US (1) | US20210349217A1 (zh) |
EP (1) | EP3896491A4 (zh) |
JP (2) | JP7330276B2 (zh) |
CN (2) | CN116184417A (zh) |
AU (1) | AU2019399430A1 (zh) |
WO (1) | WO2020118917A1 (zh) |
Families Citing this family (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112082484A (zh) * | 2020-09-11 | 2020-12-15 | 武汉理工大学 | 一种基于单线激光雷达检测工程车车身偏移的装置及方法 |
CN111830526B (zh) * | 2020-09-17 | 2020-12-29 | 上海驭矩信息科技有限公司 | 一种基于多线激光数据融合的集装箱定位方法及装置 |
CN112362055B (zh) * | 2020-12-01 | 2022-12-30 | 苏州挚途科技有限公司 | 姿态估计方法、装置和电子设备 |
CN113063396B (zh) * | 2021-03-19 | 2022-09-27 | 北京斯年智驾科技有限公司 | 一种半挂车夹角的检测方法、装置、设备及存储介质 |
CN113158864A (zh) * | 2021-04-13 | 2021-07-23 | 北京小马慧行科技有限公司 | 卡车车头与挂车之间的夹角的确定方法与确定装置 |
CN113945185B (zh) * | 2021-09-30 | 2022-07-05 | 清智汽车科技(苏州)有限公司 | 铰接角测量装置及车辆及车辆铰接角测量方法 |
DE102021126814A1 (de) * | 2021-10-15 | 2023-04-20 | Zf Cv Systems Global Gmbh | Verfahren zum Lokalisieren eines Anhängers, Verarbeitungseinheit und Fahrzeug |
Family Cites Families (26)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP3721911B2 (ja) * | 2000-01-07 | 2005-11-30 | いすゞ自動車株式会社 | トレーラ連結角検出装置 |
JP2001334966A (ja) * | 2000-03-24 | 2001-12-04 | Isuzu Motors Ltd | トレーラ連結角検出装置 |
JP4383230B2 (ja) * | 2004-04-01 | 2009-12-16 | 三菱電機株式会社 | 3次元物体照合装置 |
US20120114181A1 (en) * | 2010-11-01 | 2012-05-10 | Borthwick James R | Vehicle pose estimation and load profiling |
JP2012225806A (ja) * | 2011-04-20 | 2012-11-15 | Toyota Central R&D Labs Inc | 道路勾配推定装置及びプログラム |
CN104166989B (zh) * | 2014-07-04 | 2017-02-15 | 电子科技大学中山学院 | 一种用于二维激光雷达点云匹配的快速icp方法 |
CN104778688B (zh) * | 2015-03-27 | 2018-03-13 | 华为技术有限公司 | 点云数据的配准方法及装置 |
US20160368336A1 (en) * | 2015-06-19 | 2016-12-22 | Paccar Inc | Use of laser scanner for autonomous truck operation |
CN105547288A (zh) * | 2015-12-08 | 2016-05-04 | 华中科技大学 | 一种煤矿井下移动设备自主定位的方法及系统 |
CN107980102B (zh) * | 2016-12-23 | 2021-07-09 | 深圳配天智能技术研究院有限公司 | 一种激光雷达扫描方法及激光雷达 |
DE102017111351A1 (de) * | 2017-05-24 | 2018-11-29 | Jena-Optronik Gmbh | Verfahren zur Erfassung und autonomer Verfolgung eines Zielobjekts mittels eines LIDAR-Sensors |
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 |
CN107728156B (zh) * | 2017-09-29 | 2019-11-22 | 西安知微传感技术有限公司 | 一种增强激光雷达抗干扰性的方法及系统 |
CN108717182B (zh) * | 2018-05-02 | 2023-10-31 | 深圳市速腾聚创科技有限公司 | 激光雷达抗干扰的方法及抗干扰激光雷达 |
CN108132471B (zh) * | 2017-12-08 | 2021-03-30 | 上海禾赛科技股份有限公司 | 发射及接收激光脉冲的方法、介质及激光雷达系统 |
CN108278981A (zh) * | 2018-02-11 | 2018-07-13 | 北京主线科技有限公司 | 检测无人驾驶挂车轴偏角的装置及其检测方法 |
CN108519604B (zh) * | 2018-03-08 | 2021-08-10 | 北京理工大学 | 一种基于伪随机码调制解调的固态面阵激光雷达测距方法 |
CN108749923A (zh) * | 2018-04-04 | 2018-11-06 | 北京图森未来科技有限公司 | 一种车辆 |
CN108761481A (zh) * | 2018-04-09 | 2018-11-06 | 北京图森未来科技有限公司 | 一种挂箱夹角测量系统及测量方法、半挂车 |
CN208059845U (zh) * | 2018-05-07 | 2018-11-06 | 机科(山东)重工科技股份有限公司 | 一种牵引车的拖头和拖板相对角度检测装置 |
CN108761479B (zh) * | 2018-06-07 | 2021-07-02 | 驭势科技(北京)有限公司 | 拖车拖斗检测方法、系统和无人驾驶拖车 |
CN108959173B (zh) * | 2018-07-06 | 2022-02-11 | 北京数字绿土科技有限公司 | 一种激光雷达点云数据的解算方法及装置 |
CN111656136B (zh) * | 2018-11-09 | 2023-09-29 | 北京嘀嘀无限科技发展有限公司 | 使用激光雷达的车辆定位系统 |
CN111288930A (zh) * | 2018-11-20 | 2020-06-16 | 北京图森智途科技有限公司 | 一种挂车夹角的测量方法、装置及车辆 |
CN116559903A (zh) * | 2020-03-23 | 2023-08-08 | 北京图森智途科技有限公司 | 一种挂车夹角的测量方法、装置及车辆 |
US20230114328A1 (en) * | 2021-09-27 | 2023-04-13 | Aeva, Inc. | Techniques for dynamic trailer monitoring using frequency modulated continuous wave based lidar |
-
2018
- 2018-12-10 CN CN202310175228.3A patent/CN116184417A/zh active Pending
- 2018-12-10 CN CN201811505593.1A patent/CN111366938B/zh active Active
-
2019
- 2019-03-06 WO PCT/CN2019/077075 patent/WO2020118917A1/zh unknown
- 2019-03-06 JP JP2021533593A patent/JP7330276B2/ja active Active
- 2019-03-06 AU AU2019399430A patent/AU2019399430A1/en active Pending
- 2019-03-06 EP EP19896541.0A patent/EP3896491A4/en active Pending
-
2021
- 2021-06-09 US US17/343,489 patent/US20210349217A1/en active Pending
-
2023
- 2023-08-08 JP JP2023129340A patent/JP2023159198A/ja active Pending
Also Published As
Publication number | Publication date |
---|---|
AU2019399430A1 (en) | 2021-07-22 |
EP3896491A1 (en) | 2021-10-20 |
WO2020118917A1 (zh) | 2020-06-18 |
JP7330276B2 (ja) | 2023-08-21 |
JP2022515355A (ja) | 2022-02-18 |
EP3896491A4 (en) | 2022-08-10 |
JP2023159198A (ja) | 2023-10-31 |
US20210349217A1 (en) | 2021-11-11 |
CN111366938A (zh) | 2020-07-03 |
CN111366938B (zh) | 2023-03-14 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111366938B (zh) | 一种挂车夹角的测量方法、装置及车辆 | |
US20210356261A1 (en) | Method and device for measuring trailer included angle, and vehicle | |
CN110658531B (zh) | 一种用于港口自动驾驶车辆的动态目标追踪方法 | |
US9251587B2 (en) | Motion estimation utilizing range detection-enhanced visual odometry | |
CN113432553B (zh) | 一种挂车夹角的测量方法、装置及车辆 | |
US9678210B2 (en) | Error estimation in real-time visual odometry system | |
JP5821275B2 (ja) | 移動体位置検出装置 | |
JP2018194562A (ja) | 道路境界検出方法及び装置 | |
KR102547274B1 (ko) | 이동 로봇 및 이의 위치 인식 방법 | |
CN114236564B (zh) | 动态环境下机器人定位的方法、机器人、装置及存储介质 | |
US20190257949A1 (en) | Self-calibrating sensor system for a wheeled vehicle | |
US11745740B2 (en) | Dynamically modifiable map | |
US20230300319A1 (en) | Automated real-time calibration | |
US11840234B1 (en) | Merge handling based on merge intentions over time | |
Choi et al. | Object vehicle tracking by convex interpolation with radar accuracy | |
CN114228411B (zh) | 连接控制方法、装置、设备及存储介质 | |
Iwata et al. | Forward obstacle detection in a lane by stereo vision | |
US20230303119A1 (en) | Lane change determination for vehicle on shoulder | |
CN117762137A (zh) | 一种基于激光雷达的车辆控制方法、装置、设备及介质 | |
CN115511975A (zh) | 一种单目相机的测距方法及计算机程序产品 | |
CN112166069A (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 |