挂车偏转角度测量方法与流程

1.本发明涉及智能辅助驾驶技术领域，具体涉及一种挂车偏转角度测量方法。


背景技术：

2.目前，随着旅游业的强势崛起，以房车为代表的带有牵引车和挂车的车辆越来越受到人们的欢迎。这种自驾半挂房车，不仅能够丰富人们的出游交通方式，还减少了公共交通所带来的人员密集接触，而随着智能辅助驾驶技术的发展，挂车夹角的检测在行驶过程中可检测牵引车与挂车的夹角，避免因夹角过大造成的交通事故的发生。
3.中国发明专利cn111366938中提供一种基于多线激光雷达的测量夹角方法，在牵引车的两侧分别设置多线激光雷达，预先设定好车辆坐标系和挂车夹角的初始值，控制个激光雷达分别接受挂车表面反射的激光点云，采用匹配算法得到挂车夹角。这种基于激光雷达的夹角测量方法，首先要求的硬件成本高，激光雷达价格昂贵，会极大增加车辆成本；其次激光雷达的算法复杂、算法的开发难度大，容易受到其他障碍物干扰，不利于实际应用和批量投产，再次激光雷达未达到车规级要求标准，在复杂道路环境下易损坏。
4.基于此，研究一种结构简单、灵活安装，算法简单且测量结果更为准确的挂车偏转角度测量方法是极为必要的。


技术实现要素：

5.为解决上述技术问题，本发明提供一种挂车偏转角度测量方法，该方法测量过程不易受到干扰、测量结果更为准确。
6.本发明采用如下技术方案：
7.设计一种挂车偏转角度测量方法，包括下列步骤：
8.(1)在牵引车的车顶后部中间位置设置测量装置a，在挂车的车顶中后部设置与测量装置a配套的测量装置b，使测量装置a与测量装置b位于同一水平线上，设牵引车与挂车的铰接点为c，且测量装置a、测量装置b和铰接点c共同所在的竖直平面与牵引车的车头截面相垂直；
9.(2)测得测量装置a与铰接点的水平距离为d1，测量装置b与铰接点的水平距离为d2；
10.(3)当牵引车和挂车的夹角不为0时，通过测量装置a和测量装置b测得的数据来计算d3；
11.(4)根据得到的d1、d2、d3的值，计算d1与d3的夹角α：
[0012][0013]
(5)根据d1与d3的夹角α，计算测量装置b与d1所在竖直平面的水平距离h；
[0014]
(6)根据d2、d3和h计算出挂车的角度θ：
[0015][0016]
优选的，所述测量装置a为毫米波雷达，所述测量装置b为角反射器，具体包括下列步骤：
[0017]
(1)在牵引车的车顶后部中间位置设置毫米波雷达，在挂车的车顶中后部设置与毫米波雷达配套的角反射器，使毫米波雷达与角反射器的中心线位于同一水平线上，设牵引车与挂车的铰接点为c，且毫米波雷达、角反射器和铰接点 c共同所在的竖直平面与牵引车的车头截面相垂直；
[0018]
(2)测量毫米波雷达与铰接点的水平距离d1，测量角反射器与铰接点的水平距离d2；当牵引车和挂车的夹角为0时，毫米波雷达与角反射器之间的距离 d3应等于d1 d2；将通过毫米波雷达和角反射器测得的距离d3与d1 d2的值相比较，得到一个校验误差x，使得d1 d2＝d3 x；
[0019]
(3)当牵引车和挂车的夹角不为0时，通过毫米波雷达和角反射器测得的数据来计算d3；
[0020]
毫米波雷达发射chrip信号，经过时间t后，接收到反射回来的信号，其频率差为s，即可得到中频信号的频率f1为：
[0021]
f1＝s*t
[0022]
式中，s为chrip的斜率，t为延时时间，由于延时时间t与目标距离d有如下关系：
[0023][0024]
式中，d为毫米波雷达与目标之间的径向距离，c为光速；
[0025]
因此，由中频信号的频率即可得到目标与毫米波雷达之间的距离，如下式：
[0026][0027][0028]
则修正后的距离d3计算为：
[0029][0030]
(4)根据d1、d2和修正后的距离d3的值，计算：
[0031][0032]
(5)根据d1与d3的夹角α，计算角反射器与d1所在竖直平面的水平距离h：
[0033][0034]
(6)根据d2、d3和h计算出挂车的角度θ：
[0035][0036]
所述毫米波雷达可以采用ars408毫米波雷达、ars410毫米波雷达等合适型号的产品；所述角反射器由三块等腰三角形的金属板组合而成，形成空心、一侧开口的三棱锥形状，使用时，角反射器的开口朝向毫米波雷达所在方向。
[0037]
优选的，所述测量装置a为摄像头，所述测量装置b为参考标志物，所述参考标志物为直径rcm、长lcm的杆状物体；具体包括下列步骤：
[0038]
(1)在牵引车的车顶后部中间位置设置摄像头，在挂车的车顶中后部设置与摄像头配套的参考标志物，使摄像头和参考标志物的中心线位于同一水平线上，设牵引车与挂车的铰接点为c，且摄像头、参考标志物和铰接点c共同所在的竖直平面与牵引车的车头截面相垂直；
[0039]
(2)测得摄像头与铰接点的水平距离为d1，参考标志物与铰接点的水平距离为d2；
[0040]
(3)当牵引车和挂车的夹角不为0时，通过摄像头和参考标志物测得的数据来计算d3；
[0041]
使用标定工具，依次计算出摄像头的焦距f、图像中心点(u0,v0)，推算出旋转矩阵r和平移向量t，初始化参考标志物中心点的像素坐标为(u3,v3)；
[0042]
根据成像原理，计算参考标志物在成像中的长度，假设该摄像头的像素点长dx，该参考标志物的两端像素坐标分别为(u1,v1)和(u2,v2),则参考标志物在成像中的长度为：
[0043]
s1＝|u1-u2|*dx；
[0044]
计算摄像头与参考标志物之间的水平距离d3：
[0045][0046]
(4)根据得到的距离数据d1、d2、d3，以及参考标志物中心点的像素坐标，计算d1与d3的夹角α：
[0047]
①
首先根据距离数据计算d1与d3的夹角α1：
[0048][0049]
②
根据摄像头的成像原理，偏移后参考标志物中心点的像素坐标为(u4,v4), 计算出参考标志物在图像上的平移距离s2：
[0050]
s2＝|u3-u4|*dx；
[0051]
根据焦距f，算出偏移角度α2：
[0052][0053]
③
对计算距离得到的夹角α1和计算偏移角得到的α2，进行加权，得到偏移角度α：
[0054]
假设：α＝x1*α1 x2*α2，
[0055]
测试10组挂车夹角姿态，得到真实数据α＝[δ1,δ2,...,δ10],α2＝[γ1,γ2,...,γ10]，计算得到的10组x1与x2，求其平均值为(x1,x2)，然后带入上式得
到融合更新后的角度α；
[0056]
(5)根据步骤(4)得到的融合更新后的角度α，先对距离d3进行修正，再根据修正后的距离d3计算参考标志物与d1所在竖直平面的水平距离h；
[0057]
根据d1、d2和融合更新后的角度α，对距离d3进行修正：
[0058][0059]
然后计算得到参考标志物与d1所在竖直平面的水平距离h:
[0060][0061]
(6)根据d2、步骤(5)修正后的d3和h计算出挂车的角度θ：
[0062][0063]
所述摄像头可采用车载adas摄像头，所述标定工具为车载adas摄像头配套附带的标定上位机软件；所述参考标志物长度为5-15cm，直径为0.5-2cm，也可采用其他形状、大小的参考标志物，以适于摄像头捕捉和便于标定工具计算相关数据为准。
[0064]
本发明的有益效果在于：
[0065]
1.本发明挂车偏转角度测量方法采用的测量装置可以是配套的毫米波雷达和毫米波雷达反射器，也可以是摄像头和参考标志物，结构简单、便于安装；采用毫米波雷达和摄像头来测量距离数据，误差小且不易受到干扰；摄像头和毫米波雷达的算法相对简单、开发难度小。
[0066]
2.采用摄像头和参考标志物的方式来计算距离和偏移角度，测量过程不易受到干扰且计算简单；针对摄像头的多参数模型进行数据融合的方法对偏转角进行修正，能够更加精确的计算偏转角度，减小模型误差。
[0067]
3.本发明挂车偏转角度测量方法采用的摄像头或者毫米波雷达作为测量装置，其硬件成本低，结构简单，可在车辆生产过程中或已经运营的车辆上使用，安装操作简便，可满足批量应用和量产需求。
[0068]
4.本发明挂车偏转角度测量方法采用的摄像头或者毫米波雷达满足国家规定的aec-q系列标准，经过cnas认证的机构进行硬件车规级测试的，能够满足车辆高速路、城市道路、乡村道路等全工况情况下使用的，能够满足车辆的车规级标准，允许在车辆上使用，而激光雷达无法满足车规级需求。
附图说明
[0069]
图1是本发明挂车偏转角度测量方法中测量装置的安装位置示意图；
[0070]
图2是牵引车与挂车之间角度为0时，测量装置之间的距离和位置关系示意图；
[0071]
图3是挂车发生偏转时测量装置之间的距离和位置关系示意图；
[0072]
图4是第三方相机标定工具的界面示意图；
[0073]
图5是相机标定方法示意图；
[0074]
图6是摄像头的坐标系转换示意图；
[0075]
图7是摄像头的成像原理示意图。
[0076]
图中标号：1牵引车，2挂车，3测量装置a的安装位置，4为测量装置b 的安装位置，
具体实施方式
[0077]
下面结合实施例来说明本发明的具体实施方式，但以下实施例只是用来详细说明本发明，并不以任何方式限制本发明的范围。在以下实施例中所涉及的设备元件如无特别说明，均为常规设备元件。
[0078]
实施例1：一种挂车偏转角度测量方法，参见图1，包括下列步骤：
[0079]
(1)在牵引车1的车顶后部中间位置设置毫米波雷达，在挂车2的车顶中后部设置与毫米波雷达配套的角反射器，使毫米波雷达与角反射器的中心线位于同一水平线上，设牵引车1与挂车2的铰接点为c，且毫米波雷达、角反射器和铰接点c共同所在的竖直平面与牵引车1的车头截面相垂直。
[0080]
毫米波雷达可以采用ars408毫米波雷达、ars410毫米波雷达等合适型号的产品；角反射器由三块等腰三角形的金属板组合而成，形成空心、一侧开口的三棱锥形状，使用时，角反射器的开口朝向毫米波雷达所在方向。
[0081]
(2)测量毫米波雷达与铰接点的水平距离d1，测量角反射器与铰接点的水平距离d2，d1和d2均为定值，不随挂车角度的变化而改变；当牵引车和挂车的夹角为0时，参见图2，毫米波雷达与角反射器之间的距离d3应等于d1 d2；将通过毫米波雷达和角反射器测得的距离d3与d1 d2的值相比较，得到一个校验误差x，使得d1 d2＝d3 x。
[0082]
(3)当牵引车和挂车的夹角不为0时，参见图3，通过毫米波雷达和角反射器测得的数据来计算d3。
[0083]
毫米波雷达发射chrip信号，经过时间t后，接收到反射回来的信号，其频率差为s，即可得到中频信号的频率f1为：
[0084]
f1＝s*t
[0085]
式中，s为chrip的斜率，t为延时时间，由于延时时间t与目标距离d有如下关系：
[0086][0087]
式中，d为毫米波雷达与目标之间的径向距离，c为光速；
[0088]
因此，由中频信号的频率即可得到目标与毫米波雷达之间的距离，如下式：
[0089][0090][0091]
则修正后的距离d3计算为：
[0092][0093]
(4)根据d1、d2和修正后的距离d3的值，计算：
[0094][0095]
(5)根据d1与d3的夹角α，计算角反射器与d1所在竖直平面的水平距离h：
[0096][0097]
(6)根据d2、d3和h计算出挂车的角度θ：
[0098][0099]
在使用时，计算过程可以通过上位控制器来实现，毫米波雷达和上位控制器之间通过can信号通讯，实现数据传输，上位控制器可采用mcu控制器，内置计算芯片，用于实现数据的计算。上位控制器将计算出的挂车偏转角度结果传输到车辆的整车控制器，并显示输出结果，使驾驶员能够实时获知挂车偏转的角度，注意行车安全。
[0100]
实施例2：一种挂车偏转角度测量方法，包括下列步骤：
[0101]
(1)在牵引车1的车顶后部中间位置设置摄像头，参见图1、图2，在挂车2的车顶中后部设置与摄像头配套的参考标志物，参考标志物为直径rcm、长 lcm的杆状物体；使摄像头和参考标志物的中心线位于同一水平线上，设牵引车1与挂车2的铰接点为c，且摄像头、参考标志物和铰接点c共同所在的竖直平面与牵引车1的车头截面相垂直；
[0102]
(2)测得摄像头与铰接点的水平距离为d1，参考标志物与铰接点的水平距离为d2，d1和d2均为定值，不随挂车角度的变化而改变；
[0103]
(3)当牵引车和挂车的夹角不为0时，参见图3，通过摄像头和参考标志物测得的数据来计算d3。
[0104]
使用标定工具，依次计算出摄像头的焦距f、图像中心点(u0,v0)，推算出旋转矩阵r和平移向量t，初始化参考标志物中心点的像素坐标为(u3,v3)，参见图4-图6；
[0105]
根据成像原理，参见图7，计算参考标志物在成像中的长度，假设该摄像头的像素点长dx，该参考标志物的两端像素坐标分别为(u1,v1)和(u2,v2), 则参考标志物在成像中的长度为：
[0106]
s1＝|u1-u2|*dx；
[0107]
计算摄像头与参考标志物之间的水平距离d3：
[0108][0109]
(4)根据得到的距离数据d1、d2、d3，以及参考标志物中心点的像素坐标，计算d1与d3的夹角α：
[0110]
①
首先根据距离数据计算d1与d3的夹角α1：
[0111][0112]
②
根据摄像头的成像原理，偏移后参考标志物中心点的像素坐标为(u4,v4), 计算出参考标志物在图像上的平移距离s2：
[0113]
s2＝|u3-u4|*dx；
[0114]
根据焦距f，算出偏移角度α2：
[0115][0116]
③
对计算距离得到的夹角α1和计算偏移角得到的α2，进行加权，得到偏移角度α：
[0117]
假设：α＝x1*α1 x2*α2
[0118]
测试10组挂车夹角姿态，得到真实数据α＝[δ1,δ2,...,δ10],α2＝[γ1,γ2,...,γ10]，计算得到的10组x1与x2，求其平均值为(x1,x2)，然后带入上式得到融合更新后的角度α。
[0119]
(5)根据步骤(4)得到的融合更新后的角度α，先对距离d3进行修正，再根据修正后的距离d3计算参考标志物与d1所在竖直平面的水平距离h；
[0120]
根据d1、d2和融合更新后的角度α，对距离d3进行修正：
[0121][0122]
然后计算得到参考标志物与d1所在竖直平面的水平距离h:
[0123][0124]
(6)根据d2、步骤(5)修正后的d3和h计算出挂车的角度θ：
[0125][0126]
摄像头可采用车载adas摄像头，标定工具为车载adas摄像头配套附带的标定上位机软件；参考标志物长度为5-15cm，直径为0.5-2cm，也可采用其他形状、大小的参考标志物，以适于摄像头捕捉和便于标定工具计算相关数据为准。
[0127]
在使用时，标定上位机软件计算出的数据传输至上位控制器，上位控制器可采用mcu控制器，内置计算芯片，用于实现数据的计算。上位控制器将计算出的挂车偏转角度结果传输到车辆的整车控制器，并显示输出结果，使驾驶员能够实时获知挂车偏转的角度，注意行车安全。
[0128]
上面结合实施例对本发明作了详细的说明，但是所属技术领域的技术人员能够理解，在不脱离本发明宗旨的前提下，还可以对上述实施例中的各个具体参数进行变更，形成多个具体的实施例，均为本发明的常见变化范围，在此不再一一详述。