CN114279452B - 无人集卡挂车姿态检测方法及检测系统 - Google Patents
无人集卡挂车姿态检测方法及检测系统 Download PDFInfo
- Publication number
- CN114279452B CN114279452B CN202111642309.7A CN202111642309A CN114279452B CN 114279452 B CN114279452 B CN 114279452B CN 202111642309 A CN202111642309 A CN 202111642309A CN 114279452 B CN114279452 B CN 114279452B
- Authority
- CN
- China
- Prior art keywords
- trailer
- angle
- data
- point cloud
- model
- 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
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 41
- 230000033001 locomotion Effects 0.000 claims abstract description 25
- 238000000034 method Methods 0.000 claims abstract description 17
- 238000001914 filtration Methods 0.000 claims abstract description 14
- 238000005259 measurement Methods 0.000 claims description 13
- 238000013145 classification model Methods 0.000 claims description 12
- 238000012216 screening Methods 0.000 claims description 7
- 238000010801 machine learning Methods 0.000 claims description 5
- 230000003137 locomotive effect Effects 0.000 abstract description 14
- 230000008859 change Effects 0.000 abstract description 5
- 230000009286 beneficial effect Effects 0.000 abstract description 2
- 238000004364 calculation method Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 3
- 239000011159 matrix material Substances 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 239000003550 marker Substances 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000002452 interceptive effect Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000012706 support-vector machine Methods 0.000 description 1
- 239000000725 suspension Substances 0.000 description 1
- 238000012549 training Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Image Processing (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
本申请公开了一种无人集卡挂车姿态检测方法及检测系统,检测方法包括如下步骤:S1:构建车辆运动模型形成车头和挂车的运动轨迹估计数据,获取挂车姿态角度;S2:基于传感器获取挂车数据,以点云图像获取挂车角度的测量值;S3:构建扩展卡尔曼滤波模型,输出模型的结果作为无人集卡挂车的姿态数据。本发明的有益效果:1:在当前帧检测时利用挂车的对称性特点并引入距离均值的样本参考值降低挂车角度与实际角度的误差值;2:利用扩展卡尔曼滤波模型对当前帧检测和运动预测进行揉和以实现挂车姿态的精确预测且输出的姿态以平滑趋势变化;3:本申请的方案可以在不增加其他辅助硬件的情况下,获得满足决策规划需求的高精度的挂车夹角数据。
Description
技术领域
本申请涉及无人集卡自动驾驶技术领域,尤其涉及一种基于扩展卡尔曼滤波模型的无人集卡挂车姿态检测方法及检测系统。
背景技术
集装箱卡车(以下简称集卡),是一种由牵引车头和挂车共同组成的运载卡车。在集卡无人驾驶领域,需要依据自车以外的其他车辆的状态,来做路径规划以及避障处理等。为了防止自车挂车对他车挂车的检测造成干扰,需要时刻明确自车挂车的姿态。
挂车和牵引车头通过机械铰接点(位于挂车中轴面与车头中轴面的交线处)连接,默认集卡在地面行驶时二者均为刚体运动,且二者均不离开地面。车体坐标系通常建立在车头的某固定位置处,所以挂车的姿态便可以由俯视情况下,挂车绕铰接点的角度来表示,即挂车中轴面与牵引车头中轴面的相对角度。下文挂车姿态、挂车角度、挂车夹角,均指上述相对角度。
目前常见的挂车角度检测方法有以下几类:
1、在挂车上安装特殊的标志物,通过传感器对特殊标志物的检测进而获得挂车的角度。此种方法需要准确标定标志物在挂车载体的位置,且通常需要多个标志物才能唯一确定,并且检测的准确性受安装条件或天气的影响较大。
2、通过检测集装箱的姿态来获得挂车的姿态数据。但在实际应用中,集装箱有或者无或者变形等问题的存在,使得最终得到的夹角精度无法满足决策规划的需要。
3、利用激光雷达检测的方法。激光雷达是自动驾驶领域常用的采像设备,可以采集挂车的点云数据。根据是否载有箱体分类处理:在挂车空载时,通过其正确角度下具有最大点数量这一判据来获取挂车角度;在满载时通过拟合箱体前表面获得挂车角度。该方法在装载半箱体时,无法归类处理;另外,在自车大转角且附近有其他车辆时,上述判据很容易失效。
发明内容
为了解决上述技术问题,本申请实施例提供一种基于扩展卡尔曼滤波模型的无人集卡挂车姿态检测方法及系统,可以在不增加其他辅助硬件的情况下,获得满足决策规划需求的高精度的挂车夹角数据。
本申请实施例第一方面提供了一种无人集卡挂车姿态检测方法,包括如下步骤:
S1:获取无人集卡车头的定位导航系统数据,构建车辆运动模型形成车头和挂车的运动轨迹估计数据,基于该模型获取挂车姿态角度;
S2:基于内置或外置的传感器获取挂车数据,将点云从雷达坐标系下转换到车体坐标系后获取挂车的位置,形成挂车角度的测量值;
S3:基于步骤S1中的挂车姿态角度和步骤S2的挂车角度的测量值构建扩展卡尔曼滤波模型,输出模型的结果作为无人集卡挂车的姿态数据。
进一步地,步骤S1中车头内置的定位导航系统获取车头的实时绝对角度。
进一步地,车辆运动模型由车头内置的定位导航系统和车头的底盘信息构建,对每个时刻下的挂车相对车头的位置进行预测,并与车头的实时绝对角度参照,形成挂车角度的测量值。
进一步地,步骤S2中对点云转换到车体坐标系后形成的点云图像进行分析,将同等缩放挂车的长宽比例所形成的矩形框在图像中移动,寻找矩形框在图像中的最佳位置,该位置中矩形框内的点云以轴对称形式分布。
进一步地,最佳位置的寻找利用矩形框内的点应呈现对称性的特点,将矩形框分割成左右对称分布的若干子框,获取每个子框内的点云中的点数量以及子框内每个点至挂车尾垂直距离的均值,均记为每个子框的参比数据;在挂车的所有旋转范围内遍历每个角度的矩形框的样本数据,该样本数据包含当前角度下的每个子框的参比数据;对样本数据进行分析获取最佳位置,该位置下的矩形框的对称性最优。
进一步地,构建分类模型分析样本数据,模型输出的角度作为挂车角度的测量值,若模型输出的角度数据超过1个,将其中点数量最大的点云对应的样本数据作为输出值。
本申请实施例第二方面提供了一种无人集卡挂车姿态检测系统,包括:
挂车姿态模块,读取无人集卡车头的定位导航系统数据以及车头底盘信息,获取车头和挂车的运动轨迹估计数据,形成每个时刻下的挂车姿态;
挂车角度模块,获取场景中的传感器的挂车数据,将点云从雷达坐标系下转换到车体坐标系后分析挂车的位置,并寻找最佳挂车角度的测量值;
姿态输出模块,基于挂车姿态模块和挂车角度模块构建扩展卡尔曼滤波模型,该模型的唯一输出结果作为无人集卡挂车的姿态。
进一步地,挂车角度模块包括:
点云图像单元,基于传感器的挂车数据的点云数据转换至车体坐标系下形成点云图像,将同等缩放挂车的长宽比例所形成的矩形框置于点云图像中;
样本获取单元,将矩形框分割成左右对称分布的若干子框,在挂车的所有旋转范围内遍历每个角度下矩形框的样本数据;
分类模型单元,基于样本获取单元的历史数据以及机器学习二分类方法构建模型,基于矩形框在图像中的最佳位置应符合轴对称形式分布作为判定基准,输出样本获取单元中的最佳样本数据,该样本数据对应的角度作为挂车角度的测量值。
进一步地,样本数据包含每个子框内的点云中的点数量以及子框内每个点至挂车尾垂直距离的均值。
进一步地,,挂车角度模块还包括:
二次筛选单元,在分类模型单元输出的角度数据超过1个时对分类模型单元的输出数据进行二次筛选,将其中点数量最大的点云对应的样本数据作为最佳样本数据输出。
本发明的有益效果:
1:在当前帧检测时利用挂车的对称性特点并引入距离均值的样本参考值降低挂车角度与实际角度的误差值;
2:利用扩展卡尔曼滤波模型对当前帧检测和运动预测进行揉和以实现挂车姿态的精确预测且输出的姿态以平滑趋势变化;
3:本申请的方案可以在不增加其他辅助硬件的情况下,获得满足决策规划需求的高精度的挂车夹角数据。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本申请的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1是本申请实施例提供的检测方法的流程图;
图2是挂车姿态的瞬时力学分析图;
图3是点云集图像示意图;
图4是矩形框对称性的示意图;
图5是实际操作中矩形框对称性的缺陷示意图;
图6是不平滑的主轴姿态示意图;
图7是本申请实施例提供的检测系统的逻辑框图。
具体实施方式
为使得本申请的申请目的、特征、优点能够更加的明显和易懂,下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述,显然,下面所描述的实施例仅仅是本申请一部分实施例,而非全部的实施例。基于本申请中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例,都属于本申请保护的范围。
下面结合附图和具体实施例,进一步阐明本发明。
在本申请的描述中,需要理解的是,术语“上”、“下”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系,仅是为了便于描述本申请和简化描述,而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作,因此不能理解为对本申请的限制。
目前无人集卡方面比较常见的姿态监控多数以利用当前帧下的雷达点云数据去检测,基本判定时只是以挂车区域覆盖最大点数目作为优选输出值,在比较空旷的地方这个判据很稳定,判定简单运行也快。但车周围极近处有障碍物时,且真正的车挂只能被扫到一个面时(指车满载时,安装于车头顶上的顶雷达,只能扫描到车厢前面板,从俯视图来看,就是一条线,这条线的点数量远远小于附近障碍物车点数),误差较高。
因此考虑到上述的技术缺陷,本申请提出了一种基于扩展卡尔曼滤波模型的无人集卡挂车姿态检测,包括如下步骤:
S1:获取无人集卡车头的定位导航系统数据,构建车辆运动模型形成车头和挂车的运动轨迹估计数据,基于该模型获取挂车姿态角度。
作为一个具体实施例,车头内置的定位导航系统获取车头的实时绝对角度。
车辆运动模型由车头内置的定位导航系统和车头的底盘信息构建,将车头的底盘信息转变为挂车的运动状态,对每个时刻下的挂车相对车头的位置进行预测,并与车头的实时绝对角度参照,形成挂车角度的测量值。
具体地,在t-1时刻,如图2所示,Vhead是车头瞬时的牵引速度,该速度可以通过牵引车头的底盘信息实时获得。O表示牵引车头和挂车的衔接点,OO1表示挂车,长度用Lwheelbase表示。挂车此时和牵引车头的夹角为θt-1,则Vhead在挂车O点处分解为法向速度V1和切向速度V2。切向速度V2为挂车的转动提供动力,OO1是等效的转动半径。这里说明,O1点是挂车的等效支撑点(挂车两侧后端车轮的中间位置),即转动的瞬时圆心。Lwheelbase可以通过对机械结构的预先测量而获得。
由此可得挂车转动的角速度由速度分解可知V2=Vhead*sinθt-1。所以/>如上图2所表述的时刻,在瞬时的Vhead作用下,角度θ有变小的趋势,故符号修正φt-1→t=-φt-1→t。
综上,挂车在t时刻的预测函数可以表示为: 上述公式对θ的偏导数表示为/>
基于上述内容,本申请在车辆运动模型中,将挂车姿态的影响函数表示βt+θt=(βt-1+θt-1)+Φt-1→t,其中θt是t时刻下挂车相对于车头的角度,即本步骤所求的挂车姿态角度,βt是t时刻下牵引车头的绝对角度,Φt-1→t是t-1时刻到t时刻挂车的绝对角位移。无人集卡在车头处装有高精度的定位导航系统,所以任意时刻车头的绝对角度β可以实时获得,只有挂车的绝对角位移未知。
S2:基于内置或外置的传感器获取挂车数据,将点云从雷达坐标系下转换到车体坐标系形成点云图像并进行分析,将同等缩放挂车的长宽比例所形成的矩形框在图像中移动,寻找矩形框在图像中的最佳位置,该位置中矩形框内的点以轴对称形式分布。
这里的最佳位置的寻找利用矩形框内的点应呈现对称性的特点,将矩形框分割成左右对称分布的若干子框,获取每个子框内的点云中的点数量以及子框内每个点至挂车尾垂直距离的均值,均记为每个子框的参比数据;在挂车的所有旋转范围内遍历每个角度的矩形框的样本数据,该样本数据包含当前角度下的每个子框的参比数据;对样本数据进行分析获取最佳位置,该位置下的矩形框的对称性最优。
作为一个具体实施例,传感器可以安装CCD图像传感器,或者激光雷达等三维传感器。
本发明实施例采用安装在牵引车头顶部中心位置的多线激光雷达(10hz),获取挂车角度的测量值。
本步骤中,以牵引车头后轴中心为车体坐标系的原点,车头前进方向为x轴,依据右手法则定义y轴。通过事先标定好的雷达外参,可以获得从雷达坐标系向车体坐标系的转化矩阵R|T,如此将点云从雷达坐标系转化到车体坐标系。
考虑到挂车相对于铰接点的最大旋转角度是固定的,所以,这里可确定挂车点云的最大旋转范围。将该范围内的三维点云按一定的缩放比例,投影在xoy平面,形成点云集图像ImageP,如图3。
已知自车挂车的实际长宽尺寸,以该尺寸按同样的缩放比例在点云集图像ImageP上构建矩形框,矩形框绕旋转中心转动。
当矩形框恰好处在挂车的真实位置时(如图3的实线),矩形框内点的分布,沿挂车中轴面具有极佳的对称性。对称性的考虑,是因为在当前帧的检测中,对称性可以作为衡量当前检测是否准确的标准,且容易描述,避免上述某些场景下挂废到离谱的情况。
对称性具体体现为:将矩形框分成10个格子,分别命名为A1~A5、B1~B5,如图4所示。统计每个小格子里点的数量,记录为Num1~Num10;统计每个小格子里点到挂车尾面S的距离的均值,记录为AveDis1~AveDis10,最后形成20维的数组。对称性表现为A1、B1两格子的Num和AveDis相似,并以此类推。
一般情况下挂车的最大旋转角度是60度,所以在样本数据获取时,初始遍历角度为[-60,60],之后每一次遍历角度为上次测量角度+-15度。角度遍历范围表示为[-b,b],使矩形框按step=0.5从-b到b转动。当矩形框处在挂车的真正角度时(上图实线框),获得正样本;否则,标记为负样本(上图虚线框)。
样本采集要覆盖多种情况:自车附近有其他车辆、自车附近有围栏、自车近距离有锁站等障碍物;自车直行状态、自车大角度转弯状态;自车挂车载有集装箱状态、自车空载状态、自车半空载状态等。
最终采集约4000个样本,正负样本比例1:5,每个样本20维数据。对[4000x20]样本数据,采用传统的机器学习二分类方法——SVM支持向量机,进行训练。获得分类超平面模型,记为M。
在车辆实时运行中,对当前帧点云集图像ImageP,在角度遍历范围内遍历,采用训练好的分类模型M去判别,从而获得正类。正类对应的角度即为t时刻挂车角度的测量值Z(t)。
如果出现某一帧下正类数量>1,再次以“囊括点数量最大的矩形框对应的角度为真正的挂车角度”这一判据进行二次筛选,以确保所输出的测量值Z(t)唯一。
S3:基于步骤S1中的挂车姿态角度和步骤S2的挂车角度的测量值构建扩展卡尔曼滤波模型,输出模型的结果作为无人集卡挂车的姿态数据。
虽然在步骤S2中的对称性能够体现挂车的角度,但是在实际操作中,如真正的角度是1度,那检测成1.2度,0.8度,只从当前帧看起来所呈现的图像很小,如图5所示曲线和实线代表矩形框的对称轴,这两个情况下的对称性近似,此外车连续跑起来的时候,车挂就显得很晃动,这样的晃动会导致pnc路径规划时不能通过本应该通过的窄路。以上都是因为当前帧检测是不能达到更小的分辨率,同时不能沿时间轴将得到的多个检测结果去平滑,就是说,每两帧之间的变化有点突变。如图6所示,表示不同时刻车的主轴姿态,很明显的事挂车的状态变化并不平滑。
因此本步骤中加入扩展卡尔曼滤波,扩展卡尔曼的预测值由步骤S1的运动模型提供,测量值由步骤S2的当前帧检测提供,全程都一直由预测联动当前帧检测,二者共同作用,将最终结果推向更平滑更靠近标准的真值。
而且通过上述二者共同作用的扩展卡尔曼滤波模型,比单独利用步骤S2的当前帧检测以及步骤S1的运动模型去推算更稳定。具体地说,假定当前帧检测给运动学提供一个初始值,之后单纯只用步骤S1的运动模型推算,缺点是运动学推算需要定位信息、需要车底盘信息,定位信息错误时,整个结论一定是错误的。定位信息绝大部分时候没问题,但不能保证实时正确,在无人驾驶这个领域,一旦出现错误,车辆运行也有非常大的危险。所以可以利用定位,但不能绝对依赖它,在定位出错的时候必须有弥补措施。
所以本方案中,判断当前帧检测和预测差异,差异太大(这是偶尔),就认为预测因为定位或底盘的数据无效,会给扩展卡尔曼reset,重新赋初值。差异不大时(这是对大多数时候),用这两个共同结果,会得到一个更稳定平滑更贴近真值的总体结果。
作为一个具体的实施例,所构建的扩展卡尔曼滤波模型的核心公式为:θ(t|t-1)=Aθ(t-1|t-1),P(t|t-1)=t(t-1|t-1)AT+Q,θ(t|t)=θ(t|t-1)+K(t)(Z(t)-Hθ(t|t-1)),P(t|t)=(I-K(t)H)P(t|t-1),其中θ(t-1|t-1)、θ(t|t)分别是t-1和t时刻系统状态量的最优估计值,这里指本申请中所最终求的挂车角度。θ(t|t-1)是根据t-1时刻的最优估计预测的t时刻的挂车角度。
A是状态转移矩阵,是挂车姿态的影响函数对θ的偏导数在t-1时刻的取值。挂车的角度受与其链接的牵引车头的运动的影响。
K(t)是卡尔曼增益,是滤波迭代计算的中间结果。
Z(t)是测量值,本步骤S2中的挂车角度。
考虑到测量值和系统状态量一致,所以矩阵H为I。
Q和R分别表示滤波器噪声和测量噪声,二者的取值会反应预测值和测量值对最终估计结果的影响程度。
优选地,Q、R分别设定为10-5、10-3,能达到比较理想的挂车角度检测效果。在扩展卡尔曼滤波模型中,由于系统状态量为一维变量,所以AT=A、HT=H、I=1。代入Q、R、A、Z(t),设置θ的初始值=Z(0),便可以得到任意时刻的车挂夹角。
上述方法在不增加硬件成本的情况下,可以获得精确的车挂姿态,满足路径规划模块的需求。
本申请实施例第二方面提供了一种无人集卡挂车姿态检测系统,如图7所示,检测系统包括:
挂车姿态模块100,读取无人集卡车头的定位导航系统数据以及车头底盘信息,获取车头和挂车的运动轨迹估计数据,形成每个时刻下的挂车姿态。
本模块根据车头和挂车的位置关系以及移动关系,通过定位导航系统数据获取到车头的实时绝对角度,挂车在运动时以车头和挂车的衔接点作为牵引力的传递点,根据物理力学的构建,能够将底盘信息中的车头运动速度转角等等信息分析得到挂车的运动状态,形成基于导航等数据下的挂车姿态数据。
挂车角度模块200,获取场景中的传感器的挂车数据,将点云从雷达坐标系下转换到车体坐标系后分析挂车的位置,并寻找最佳挂车角度的测量值。
一般情况下,本模块中设置的传感器包含在场景内以及各个车头上,传感器可以安装CCD图像传感器,或者激光雷达等三维传感器。
其中,挂车角度模块包括:
点云图像单元210,牵引车头后轴中心为车体坐标系的原点,车头前进方向为x轴,依据右手法则定义y轴。通过事先标定好的雷达外参,基于传感器的挂车数据的点云数据转换至车体坐标系下形成点云图像,考虑到挂车相对于铰接点的最大旋转角度是固定的,可确定挂车点云的最大旋转范围。将该范围内的三维点云按一定的缩放比例,投影在xoy平面,将同等缩放挂车的长宽比例所形成的矩形框置于点云图像中。
样本获取单元220,将矩形框分割成左右对称分布的若干子框,在挂车的所有旋转范围内遍历每个角度下矩形框的样本数据;样本数据包含每个子框内的点云中的点数量以及子框内每个点至挂车尾垂直距离的均值。
可以理解的是,考虑到挂车的形状特点,当矩形框恰好处在挂车的真实位置时,矩形框内点云的分布,沿挂车中轴面具有极佳的对称性。
目前有的无人集卡的检测方案是基于点云中的点分布数量去进行判定,但是数量的判定方法过于简单,在实际操作中,角度上的微小变化,在点数量上的变化甚至可以不变,尤其是在车身抖动的情况下,比如驻车的时候抖动会相对多一些,挂车运行过程中的抖动也是正常情况,在这种情况下,如果只是依靠点数目的判定,基本上会使得每一时刻的判定结果出现不平滑的情况,比如上一时刻还是-0.8度,下一时刻就变成0.5度的变化,为了降低这种情况,本模块中的点云是还设置有第二个参照参数,其是以每个子框内每个点至挂车尾垂直距离的均值去作为参数,均值的加入直接撇除了数目上的影响,而更加看中对称性的真实性。在两个参数的条件下,本单元中所形成的结论数据更加的接近真实情况。
分类模型单元230,基于样本获取单元的历史数据以及机器学习二分类方法构建模型,基于矩形框在图像中的最佳位置应符合轴对称形式分布作为判定基准,输出样本获取单元中的最佳样本数据,该样本数据对应的角度作为挂车角度的测量值。
可以理解的是,由于样本数据非常多,且整个系统处于不断的计算过程中,因此选用机器学习二分类方法去构建分析模块对历史数据进行训练,进而可以对当前的数据进行实时快速分析,每一次的数据都可以作为历史数据不断地训练模型,从而使得最终的模型在分析时的准确性更高。
二次筛选单元240,在分类模型单元输出的角度数据超过1个时对分类模型单元的输出数据进行二次筛选,将其中点数量最大的点云对应的样本数据作为最佳样本数据输出。
姿态输出模块300,基于挂车姿态模块和挂车角度模块构建扩展卡尔曼滤波模型,该模型的唯一输出结果作为无人集卡挂车的姿态。
虽然样本获取单元220中的样本参数获取方式能够降低输出数据不平滑的问题,但是始终无法完全避免,而挂车姿态模块100所输出的数据需要保证定位导航系统数据以及车头底盘信息均能够正常获取或者数据正常,这两个单元模块如果其中之一发生异常,单靠另一个是很难对挂车的姿态进行精准估计的,因此本模块中引入了扩展卡尔曼滤波模型,利用扩展卡尔曼滤波的特点,判断当前帧检测和预测差异,差异太大(这是偶尔),就认为预测中的定位或底盘数据异常,会给扩展卡尔曼重置,重新赋初值。差异不大时(这是对大多数时候),用这两个共同结果,会得到一个更稳定平滑更贴近真值的总体结果。
以上详细描述了本发明的优选实施方式,但是本发明并不限于上述实施方式中的具体细节,在本发明的技术构思范围内,可以对本发明的技术方案进行多种等同变换(如数量、形状、位置等),这些等同变换均属于本发明的保护。
Claims (6)
1.一种无人集卡挂车姿态检测方法,其特征在于:所述检测方法包括如下步骤:
S1:获取无人集卡车头的定位导航系统数据,构建车辆运动模型形成车头和挂车的运动轨迹估计数据,基于该模型获取挂车姿态角度;
S2:基于内置或外置的传感器获取挂车数据,将点云从雷达坐标系下转换到车体坐标系后获取挂车的位置,形成挂车角度的测量值;
其中,步骤S2中对点云转换到车体坐标系后形成的点云图像进行分析,将同等缩放挂车的长宽比例所形成的矩形框在图像中移动,寻找矩形框在图像中的最佳位置,该位置中矩形框内的点云以轴对称形式分布;所述最佳位置的寻找利用矩形框内的点应呈现对称性的特点,将矩形框分割成左右对称分布的若干子框,获取每个子框内的点云中的点数量以及子框内每个点至挂车尾垂直距离的均值,均记为每个子框的参比数据;在挂车的所有旋转范围内遍历每个角度的矩形框的样本数据,该样本数据包含当前角度下的每个子框的参比数据;对所述样本数据进行分析获取最佳位置,该位置下的矩形框的对称性最优;
S3:基于步骤S1中的挂车姿态角度和步骤S2的挂车角度的测量值构建扩展卡尔曼滤波模型,输出模型的结果作为无人集卡挂车的姿态数据。
2.根据权利要求1所述的无人集卡挂车姿态检测方法,其特征在于:
所述步骤S1中车头内置的定位导航系统获取车头的实时绝对角度。
3.根据权利要求1所述的无人集卡挂车姿态检测方法,其特征在于:
所述车辆运动模型由车头内置的定位导航系统和车头的底盘信息构建,对每个时刻下的挂车相对车头的位置进行预测,并与车头的实时绝对角度参照,形成挂车角度的测量值。
4.根据权利要求3所述的无人集卡挂车姿态检测方法,其特征在于:
构建分类模型分析所述样本数据,模型输出的角度作为挂车角度的测量值,若模型输出的角度数据超过1个,将其中点数量最大的点云对应的样本数据作为输出值。
5.一种无人集卡挂车姿态检测系统,其特征在于:所述检测系统包括:
挂车姿态模块,读取无人集卡车头的定位导航系统数据以及车头底盘信息,获取车头和挂车的运动轨迹估计数据,形成每个时刻下的挂车姿态;
挂车角度模块,获取场景中的传感器的挂车数据,将点云从雷达坐标系下转换到车体坐标系后分析挂车的位置,并寻找最佳挂车角度的测量值;
姿态输出模块,基于所述挂车姿态模块和挂车角度模块构建扩展卡尔曼滤波模型,该模型的唯一输出结果作为无人集卡挂车的姿态;
其中,所述挂车角度模块包括:
点云图像单元,基于传感器的挂车数据的点云数据转换至车体坐标系下形成点云图像,将同等缩放挂车的长宽比例所形成的矩形框置于点云图像中;
样本获取单元,将矩形框分割成左右对称分布的若干子框,在挂车的所有旋转范围内遍历每个角度下矩形框的样本数据;
分类模型单元,基于样本获取单元的历史数据以及机器学习二分类方法构建模型,基于矩形框在图像中的最佳位置应符合轴对称形式分布作为判定基准,输出所述样本获取单元中的最佳样本数据,该样本数据对应的角度作为挂车角度的测量值;所述样本数据包含每个子框内的点云中的点数量以及子框内每个点至挂车尾垂直距离的均值。
6.根据权利要求5所述的无人集卡挂车姿态检测系统,其特征在于:所述挂车角度模块还包括:
二次筛选单元,在所述分类模型单元输出的角度数据超过1个时对分类模型单元的输出数据进行二次筛选,将其中点数量最大的点云对应的样本数据作为最佳样本数据输出。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111642309.7A CN114279452B (zh) | 2021-12-29 | 2021-12-29 | 无人集卡挂车姿态检测方法及检测系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111642309.7A CN114279452B (zh) | 2021-12-29 | 2021-12-29 | 无人集卡挂车姿态检测方法及检测系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114279452A CN114279452A (zh) | 2022-04-05 |
CN114279452B true CN114279452B (zh) | 2024-04-12 |
Family
ID=80878115
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111642309.7A Active CN114279452B (zh) | 2021-12-29 | 2021-12-29 | 无人集卡挂车姿态检测方法及检测系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114279452B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116101300B (zh) * | 2023-04-06 | 2023-06-23 | 巨野金牛车业有限公司 | 一种基于大数据的挂车运输安全检测系统 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109959352A (zh) * | 2019-03-01 | 2019-07-02 | 武汉光庭科技有限公司 | 利用激光点云计算卡车车头和挂车之间夹角的方法及系统 |
CN110781827A (zh) * | 2019-10-25 | 2020-02-11 | 中山大学 | 一种基于激光雷达与扇状空间分割的路沿检测系统及其方法 |
CN111797734A (zh) * | 2020-06-22 | 2020-10-20 | 广州视源电子科技股份有限公司 | 车辆点云数据处理方法、装置、设备和存储介质 |
CN112083726A (zh) * | 2020-09-04 | 2020-12-15 | 湖南大学 | 一种面向园区自动驾驶的双滤波器融合定位系统 |
CN113063396A (zh) * | 2021-03-19 | 2021-07-02 | 北京斯年智驾科技有限公司 | 一种半挂车夹角的检测方法、装置、设备及存储介质 |
CN113158864A (zh) * | 2021-04-13 | 2021-07-23 | 北京小马慧行科技有限公司 | 卡车车头与挂车之间的夹角的确定方法与确定装置 |
EP3885796A1 (en) * | 2018-11-20 | 2021-09-29 | Beijing Tusen Zhitu Technology Co., Ltd. | Method and device for measuring trailer included angle, and vehicle |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
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 |
-
2021
- 2021-12-29 CN CN202111642309.7A patent/CN114279452B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP3885796A1 (en) * | 2018-11-20 | 2021-09-29 | Beijing Tusen Zhitu Technology Co., Ltd. | Method and device for measuring trailer included angle, and vehicle |
CN109959352A (zh) * | 2019-03-01 | 2019-07-02 | 武汉光庭科技有限公司 | 利用激光点云计算卡车车头和挂车之间夹角的方法及系统 |
CN110781827A (zh) * | 2019-10-25 | 2020-02-11 | 中山大学 | 一种基于激光雷达与扇状空间分割的路沿检测系统及其方法 |
CN111797734A (zh) * | 2020-06-22 | 2020-10-20 | 广州视源电子科技股份有限公司 | 车辆点云数据处理方法、装置、设备和存储介质 |
CN112083726A (zh) * | 2020-09-04 | 2020-12-15 | 湖南大学 | 一种面向园区自动驾驶的双滤波器融合定位系统 |
CN113063396A (zh) * | 2021-03-19 | 2021-07-02 | 北京斯年智驾科技有限公司 | 一种半挂车夹角的检测方法、装置、设备及存储介质 |
CN113158864A (zh) * | 2021-04-13 | 2021-07-23 | 北京小马慧行科技有限公司 | 卡车车头与挂车之间的夹角的确定方法与确定装置 |
Non-Patent Citations (1)
Title |
---|
Simultaneous estimation of steering and articulation angle in a truck-semitrailer combination solely based on trailer signals;Zygimantas Ziaukas等;2019 American Control Conference;全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN114279452A (zh) | 2022-04-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110411462B (zh) | 一种gnss/惯导/车道线约束/里程计多源融合方法 | |
US11002859B1 (en) | Intelligent vehicle positioning method based on feature point calibration | |
CN105404844B (zh) | 一种基于多线激光雷达的道路边界检测方法 | |
CN110745140B (zh) | 一种基于连续图像约束位姿估计的车辆换道预警方法 | |
CN112083726B (zh) | 一种面向园区自动驾驶的双滤波器融合定位系统 | |
CN109359409A (zh) | 一种基于视觉与激光雷达传感器的车辆可通过性检测系统 | |
CN110794406B (zh) | 多源传感器数据融合系统和方法 | |
JP2013129289A (ja) | 車両状態量推定装置、車両用操舵制御装置 | |
CN114018248B (zh) | 一种融合码盘和激光雷达的里程计方法与建图方法 | |
CN111142091A (zh) | 一种融合车载信息的自动驾驶系统激光雷达在线标定方法 | |
EP4026049A1 (en) | Vehicular sensor system calibration | |
CN114279452B (zh) | 无人集卡挂车姿态检测方法及检测系统 | |
US20210373138A1 (en) | Dynamic lidar alignment | |
CN105867373A (zh) | 一种基于激光雷达数据的移动机器人位姿推算方法及系统 | |
CN113947639A (zh) | 基于多雷达点云线特征的自适应在线估计标定系统及方法 | |
CN114821526A (zh) | 基于4d毫米波雷达点云的障碍物三维边框检测方法 | |
CN115908539A (zh) | 一种目标体积自动测量方法和装置、存储介质 | |
Parra-Tsunekawa et al. | A kalman-filtering-based approach for improving terrain mapping in off-road autonomous vehicles | |
CN112278891B (zh) | 一种车厢内部姿态检测方法 | |
CN106885523B (zh) | 一种车辆路径跟随误差视觉测量优化方法 | |
CN111402328B (zh) | 一种基于激光里程计的位姿计算方法及装置 | |
CN114475581B (zh) | 基于轮速脉冲和imu卡尔曼滤波融合的自动泊车定位方法 | |
CN114162170B (zh) | 一种轨道测量系统及测量方法 | |
CN113091693B (zh) | 一种基于图像超分辨率技术的单目视觉长程测距方法 | |
CN114049542A (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 |