CN111366086A - 面向自动化装车的车厢位姿测量***及方法 - Google Patents

面向自动化装车的车厢位姿测量***及方法 Download PDF

Info

Publication number
CN111366086A
CN111366086A CN202010361420.8A CN202010361420A CN111366086A CN 111366086 A CN111366086 A CN 111366086A CN 202010361420 A CN202010361420 A CN 202010361420A CN 111366086 A CN111366086 A CN 111366086A
Authority
CN
China
Prior art keywords
coordinate
carriage
value
point
values
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
CN202010361420.8A
Other languages
English (en)
Other versions
CN111366086B (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.)
Northeastern University China
Original Assignee
Northeastern University China
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 Northeastern University China filed Critical Northeastern University China
Priority to CN202010361420.8A priority Critical patent/CN111366086B/zh
Publication of CN111366086A publication Critical patent/CN111366086A/zh
Application granted granted Critical
Publication of CN111366086B publication Critical patent/CN111366086B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/002—Measuring arrangements characterised by the use of optical techniques for measuring two or more coordinates
    • 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/02—Measuring arrangements characterised by the use of optical techniques for measuring length, width or thickness
    • G01B11/03—Measuring arrangements characterised by the use of optical techniques for measuring length, width or thickness by measuring coordinates of points
    • 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

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本发明提出一种面向自动化装车的车厢位姿测量***及方法，包括机控制***和测量机构，所述测量机构包括伸缩杆机构、360°激光扫描测距雷达、激光测距仪，测量待测车厢的位姿时，通过控制***控制伸缩杆机构带动360°激光扫描测距雷达和激光测距仪下移，控制***根据360°激光扫描测距雷达采集的数据识别车厢的存在，当识别到车厢存在时，触发激光测距仪采集深度数据，通过360°激光扫描测距雷达采集车厢内部的数据实现车厢位姿的估算。通过实时数据来计算车厢顶点坐标，确保了检测的实时性，通过特定算法筛选出估计误差小的坐标点，保证了算法的鲁棒性和精确性，所述测量***可以测量任意车型，无需对车辆停靠位置严格限定。

Description

面向自动化装车的车厢位姿测量***及方法
技术领域
本发明涉及车厢测量定位领域，具体涉及一种面向自动化装车的车厢位姿测量***及方法。
背景技术
人工进行货物装车，不仅工作繁重，而且易对身体造成损伤，特别是装载水泥、化肥等货物时，高密度粉尘、浓烈的异味等对人体危害很大。随着体力劳动人口减少，雇佣装车工人也越发困难。货物自动装车***可以避免人体伤害，提升工作效率，有效解决当前货物装车行业面临的困境。
实现货物自动装车的首要关键环节是确定货车车厢的长、宽、高等参数，以及车厢的停靠位置，即车厢位姿。当前自动装车***使用的车厢参数与位姿测量方法有两种，一是人工测量，需要车辆准确停靠到预定位置后，然后人工测量车厢长、宽、高，再将测量结果手动输入到自动装车***中；二是采用的是红外光电扫描技术，该技术利用跟踪扫描的方法，对机动车的长、宽、高三个方向上进行轮廓跟踪扫描，根据所得到的一系列三维数据，对机动车轮廓重构，得到车辆的长、宽、高等参数。
上述两种方法都对驾驶员驾车技术有非常高要求，需要准确停靠到指定位置或是沿着固定方向稳定行进，而当车辆位置或方向与预设值有偏差时，必然会导致测量误差；同时，上述第一种方法需要人工参与，缺乏灵活性，而第二种方法应用的红外传感器性能受环境光线、温度等影响较大，不利于室外使用。
发明内容
针对现有技术的不足，本发明提出一种面向自动化装车的车厢位姿测量***，包括控制***和测量机构，所述测量机构包括伸缩杆机构、360°激光扫描测距雷达、激光测距仪，360°激光扫描测距雷达安装在伸缩杆机构的正下方，激光测距仪固定在伸缩杆机构的侧面，360°激光扫描测距雷达和激光测距仪分别通过串口通信与控制***相连，测量待测车厢的位姿时，将测量机构竖直安装在被测车厢的正上方，测量机构的固定位置高于车厢的厢顶面，且满足测量机构竖直方向下移时不与车厢的厢壁发生干涉，通过控制***控制伸缩杆机构带动360°激光扫描测距雷达和激光测距仪下上移动；
所述伸缩杆机构用于带动360°激光扫描测距雷达和激光测距仪实现竖直方向的上下移动；
所述360°激光扫描测距雷达用于采集车厢的极坐标数据点集；
所述激光测距仪用于采集车厢的深度数据；
所述控制***用于根据采集到的极坐标数据点集识别车厢的存在，并估算车厢的位姿，所述车厢的位姿包括车厢的长度值、宽度值、车厢偏移角度值；
所述控制***还用于根据采集到的深度数据实现车厢深度的测量。
所述的伸缩杆机构包括伺服电机、电动缸固定座、电动缸推杆，伺服电机固定在竖直安装的电动缸固定座上，伺服电机的输出轴通过联轴器与电动缸推杆的输入轴相连，电动缸推杆的底端安装有360°激光扫描测距雷达，电动缸推杆的底部侧面安装有激光测距仪。
一种面向自动化装车的车厢位姿测量***的使用方法，包括如下步骤：
步骤1：将测量机构竖直安装在被测车厢的正上方，测量机构的固定位置高于车厢的厢顶面，且满足测量机构竖直方向下移时不与车厢的厢壁发生干涉；
步骤2：通过控制***控制伸缩杆机构下移，伸缩杆机构带动360°激光扫描测距雷达和激光测距仪下移，控制***根据360°激光扫描测距雷达采集的极坐标数据点集识别车厢的存在；
步骤3：当控制***识别到车厢的存在时，触发激光测距仪采集
Figure BDA0002475201990000024
次深度数据，来实现车厢深度的测量，
Figure BDA0002475201990000025
表示激光测距仪的预设采样次数；
步骤4：当控制***识别到车厢的存在后，控制***控制360°激光扫描测距雷达在车厢内移动，通过360°激光扫描测距雷达采集的极坐标数据点集实现车厢位姿的估算；
步骤5：通过控制***计算得到车厢的位姿数据以及深度值后，控制伸缩杆机构上移到初始位置，测量结束。
所述步骤2中的控制***根据360°激光扫描测距雷达采集的极坐标数据点集识别车厢的存在，具体表述为：
S2.1：当未检测到车厢的存在时，通过控制***设置360°激光扫描测距雷达的采样时间间隔△t1，360°激光扫描测距雷达和激光测距仪随着伸缩杆机构下移，360°激光扫描测距雷达每间隔△t扫描一次，并采集极坐标数据点集
Figure BDA0002475201990000021
其中θ表示测量点相对于360°激光扫描测距雷达自身朝向的夹角的角度值，r表示360°激光扫描测距雷达距离当前测量点之间的距离值，
Figure BDA0002475201990000022
表示第i次扫描时采集到的第m个数据点的角度值，
Figure BDA0002475201990000023
表示第i次扫描时采集到的第m个数据点的距离值，n表示360°激光扫描测距雷达第i次扫描时采集到的极坐标数据点的总个数；
S2.2：控制***实时读取360°激光扫描测距雷达采集的极坐标数据点集，存储在控制***中，并统计第i次采集的极坐标数据点集
Figure BDA0002475201990000031
中非(θm i,0)坐标点的个数Li，通过公式(1)计算非(θm i,0)坐标点所占比值ηi，其中j∈(1,n)；
Figure BDA0002475201990000032
S2.3：当控制***检测到，360°激光扫描测距雷达最新一次扫描时采集到的极坐标数据点集
Figure BDA0002475201990000033
中非(θm j,0)坐标点的个数Lj所占比值
Figure BDA0002475201990000034
时，则说明360°激光扫描测距雷达已经检测到车厢的存在，其中
Figure BDA0002475201990000035
表示非(θm j,0)坐标点个数的给定阈值，如果控制***未检测到车厢的存在，则继续控制伸缩杆机构下移，直至检测到车厢的存在时，控制***停止对伸缩杆机构下移的自动控制。
所述步骤3具体表述为：
S3.1：当识别到车厢存在时，通过控制***控制激光测距仪采集
Figure BDA0002475201990000036
次深度数据，记为
Figure BDA0002475201990000037
S3.2：计算
Figure BDA0002475201990000038
则Hh即为车厢的深度值。
所述步骤4具体表述为：
S4.1：当360°激光扫描测距雷达检测到车厢存在后，通过控制***控制伸缩杆机构下移到车厢内的某一给定高度H'；
S4.2：通过控制***控制360°激光扫描测距雷达在高度H'的位置进行一次扫描，记录采集到的极坐标数据点集为
Figure BDA0002475201990000039
S4.3：对所述极坐标数据点集
Figure BDA00024752019900000310
进行中值滤波处理，得到处理后的极坐标数据点集为
Figure BDA00024752019900000311
S4.4：对极坐标数据点集
Figure BDA00024752019900000312
中的n个极坐标数据点进行自适应筛选，得到一个新的极坐标数据点集{(θs,rs),(θe,re),(θ1,r1),(θ2,r2),(θ3,r3),(θ4,r4)}，所述自适应筛选是指对α随机选取不同的数值，其中α的取值范围为0＜α＜1；
S4.5：将新的极坐标数据点集{(θm,rm),(θs,rs),(θe,re),(θ1,r1),(θ2,r2),(θ3,r3),(θ4,r4)}中的极坐标表示的数据点转换为直角坐标系中的对应点坐标，转换后的直角坐标数据点集记为{(xm,ym),(xs,ys),(xe,ye),(x1,y1),(x2,y2),(x3,y3),(x4,y4)}，其中(xm,ym)为(θm,rm)在直角坐标系中对应点的坐标，(xs,ys)为(θs,rs)在直角坐标系中对应点的坐标，(xe,ye)为(θe,re)在直角坐标系中对应点的坐标，(x1,y1)为(θ1,r1)在直角坐标系中对应点的坐标，(x2,y2)为(θ2,r2)在直角坐标系中对应点的坐标，(x3,y3)为(θ3,r3)在直角坐标系中对应点的坐标，(x4,y4)为(θ4,r4)在直角坐标系中对应点的坐标；
S4.6：将(xm,ym)、(xs,ys)、(xs,ys)三点，利用线性回归方法拟合为一条拟合直线记为l，并计算出拟合直线l的斜率记为k；
S4.7：根据求取直线的点斜式方程，利用斜率k与点(x1,y1)、(x2,y2)、(x3,y3)、(x4,y4)分别求取四条直线l1、l2、l3、l4，如公式(2)～公式(5)所示，
l1:y-y1＝k(x-x1) (2)
Figure BDA0002475201990000041
l3:y-y3＝k(x-x3) (4)
Figure BDA0002475201990000042
S4.8：计算直线l1与直线l2的交点，记为A1,2＝(x1,2,y1,2)，计算直线l2与直线l3的交点，记为A2,3＝(x2,3,y2,3)，计算直线l3与直线l4的交点，记为A3,4＝(x3,4,y3,4)，计算直线l4与直线l1的交点，记为A4,1＝(x4,1,y4,1)，则得到一组顶点集{A1,2,A2,3,A3,4,A4,1}；
S4.9：重复S4.4进行自适应筛选，对α随机选取ψ个不同的数值{α1,α2,…,αψ}，计算得到θs的ψ个值，记为
Figure BDA0002475201990000043
计算得到θe的ψ个值，记为
Figure BDA0002475201990000044
然后在
Figure BDA0002475201990000045
中搜索出
Figure BDA0002475201990000046
中每个值所对应的距离值，记为
Figure BDA0002475201990000047
在
Figure BDA0002475201990000048
中搜索出
Figure BDA0002475201990000049
中每个值所对应的距离值，记为
Figure BDA00024752019900000410
则最后搜索得到ψ组新的极坐标数据点集，如公式(6)所示，
Figure BDA00024752019900000411
S4.10：重复S4.5～S4.8，对公式(6)所示的ψ组新的极坐标数据点集，分别通过求取直线、计算相邻两两直线的交点计算得到ψ组顶点集，如公式(7)所示，得到ψ个拟合直线，以及对应的ψ个斜率值，记为斜率值集{k1,k2,…,kψ}，
Figure BDA0002475201990000051
S4.11：1)对公式(7)给出的ψ组顶点集，1.1)计算
Figure BDA0002475201990000052
中各个坐标点的所有x坐标值的中值，以及所有y坐标值的中值，并将x坐标值的中值和y坐标值的中值组合为坐标点
Figure BDA0002475201990000053
1.2)计算
Figure BDA0002475201990000054
中各个坐标点的所有x坐标值的中值，以及所有y坐标值的中值，并将x坐标值的中值和y坐标值的中值组合为坐标点
Figure BDA0002475201990000055
1.3)计算
Figure BDA0002475201990000056
中各个坐标点的所有x坐标值的中值，以及所有y坐标值的中值，并将x坐标值的中值和y坐标值的中值组合为坐标点
Figure BDA0002475201990000057
1.4)计算
Figure BDA0002475201990000058
中各个坐标点的所有x坐标值的中值，以及所有y坐标值的中值，并将x坐标值的中值和y坐标值的中值组合为坐标点
Figure BDA0002475201990000059
1.5)将
Figure BDA00024752019900000510
组合为
Figure BDA00024752019900000511
即
Figure BDA00024752019900000512
为所在高度H'的估计顶点集，所述x坐标值是指坐标点在直角坐标系中对应的x轴方向的值，所述y坐标值是指坐标点在直角坐标系中对应的y轴方向的值；
2)计算斜率值集{k1,k2,…,kψ}中ψ个斜率值的平均值
Figure BDA00024752019900000513
S4.12：为了得到不同高度下的S组估计顶点集，通过控制***控制伸缩杆机构在车厢内移动S个不同的高度值，重复S4.1～S4.11，计算得到
Figure BDA00024752019900000514
的S组值，记为估计顶点集如公式(8)所示，计算得到
Figure BDA00024752019900000515
的S个值，记为估计斜率值集
Figure BDA00024752019900000516
Figure BDA00024752019900000517
S4.13：对于公式(8)给出的S组估计顶点集，利用公式(9)计算每个坐标点的S个x坐标值的平均值，利用公式(10)计算每个坐标点的S个y坐标值的平均值，得到最终估计顶点集
Figure BDA00024752019900000518
Figure BDA0002475201990000061
Figure BDA0002475201990000062
式中，u＝1,2,3,4，v＝1,2,3,…,S，u表示坐标点在估计顶点集中的位置，
Figure BDA0002475201990000063
表示第v个估计顶点集中第1个坐标点的x坐标值，
Figure BDA0002475201990000064
表示第v个估计顶点集中第1个坐标点的y坐标值，
Figure BDA0002475201990000065
表示第v个估计顶点集中第2个坐标点的x坐标值，
Figure BDA0002475201990000066
表示第v个估计顶点集中第2个坐标点的y坐标值，
Figure BDA0002475201990000067
表示第v个估计顶点集中第3个坐标点的x坐标值，
Figure BDA0002475201990000068
表示第v个估计顶点集中第3个坐标点的y坐标值，
Figure BDA0002475201990000069
表示第v个估计顶点集中第4个坐标点的x坐标值，
Figure BDA00024752019900000610
表示第v个估计顶点集中第4个坐标点的y坐标值，
Figure BDA00024752019900000611
表示第1个坐标点的S个x坐标值的平均值，
Figure BDA00024752019900000612
表示第1个坐标点的S个y坐标值的平均值，
Figure BDA00024752019900000613
表示第2个坐标点的S个x坐标值的平均值，
Figure BDA00024752019900000614
表示第2个坐标点的S个y坐标值的平均值，
Figure BDA00024752019900000615
表示第3个坐标点的S个x坐标值的平均值，
Figure BDA00024752019900000616
表示第3个坐标点的S个y坐标值的平均值，
Figure BDA00024752019900000617
表示第4个坐标点的S个x坐标值的平均值，
Figure BDA00024752019900000618
表示第4个坐标点的S个y坐标值的平均值；
S4.14：对于最终估计顶点集
Figure BDA00024752019900000619
计算最终估计顶点集中任意两两坐标点的距离，得到六个距离值，对所述六个距离值从大到小排序，其中长度最大的距离值即为车厢的对角线值，长度次之的距离值即为车厢的长度值，长度最小的距离值即为车厢的宽度值；
S4.15：计算估计斜率值集
Figure BDA00024752019900000620
中S个斜率值的平均值
Figure BDA00024752019900000621
则平均值
Figure BDA00024752019900000622
对应的角度值即为车厢相对于0°测量方向的偏移角度值，所述0°测量方向根据360°激光扫描测距雷达的安装方向定义。
所述的步骤S4.4具体表述为：
S4.4.1：搜索
Figure BDA00024752019900000623
的n个距离值中的最小值记为r1，定义最小值r1所对应的角度值为θ1，则搜索到的第一个极坐标数据点为(θ1,r1)；
S4.4.2：搜索
Figure BDA0002475201990000071
中角度值为θ1+0.5π的数据点记为(θ2,r2)，则满足θ2＝θ1+0.5π，r2为角度值θ1+0.5π所对应的距离值；搜索
Figure BDA0002475201990000072
中角度值为θ1+π的数据点记为(θ3,r3)，则满足θ3＝θ1+π，r3为角度值θ1+π所对应的距离值；搜索
Figure BDA0002475201990000073
中角度值为θ1-0.5π的数据点记为(θ4,r4)，则满足θ4＝θ1-0.5π，r4为角度值θ1-0.5π所对应的距离值；
S4.4.3：令x1＝r1*cos(θ1)-r2*sin(θ1)，y1＝r2*cos(θ1)+r1*sin(θ1)，a＝r4+r2，b＝r1+r3，利用公式(11)～公式(14)计算出四个采样角度值γ1,γ2,γ3,γ4；
Figure BDA0002475201990000074
Figure BDA0002475201990000075
Figure BDA0002475201990000076
Figure BDA0002475201990000077
S4.4.4：对所述四个采样角度值γ1,γ2,γ3,γ4从小到大进行排序，定义处在排序中第二个位置的采样角度值为中间值记为θm,θm∈{γ1,γ2,γ3,γ4}；
S4.4.5：定义θs＝θm-α×θm，θe＝θm+α×θm，对α随机选取一个数值，0＜α＜1，在
Figure BDA0002475201990000078
中搜索角度值为θm、θs、θe所对应的距离值，分别定义为rm、rs、re，则搜索出来的三个数据点分别为(θm,rm)、(θs,rs)、(θe,re)；
S4.4.6：将搜索出来的6个数据点组合为一个新的极坐标数据点集，记为{(θm,rm),(θs,rs),(θe,re),(θ1,r1),(θ2,r2),(θ3,r3),(θ4,r4)}。
本发明的有益效果是：
本发明提出了一种面向自动化装车的车厢位姿测量***及方法，其中测量机构包括伸缩杆机构、360°激光扫描测距雷达、激光测距仪，结构简单，室内外均可安装；通过360°激光扫描测距雷达和激光测距仪对车厢内部尺寸与偏移角度进行测量，激光测量稳定性好，受光线、温度等环境的影响较小，室内外均可应用，测量精度高；通过控制***实现测量机构的自由移动，方便实现全程自动化测量；通过360°激光扫描测距雷达扫描数据来计算车厢顶点坐标，确保了检测的实时性，此计算方法在不同高度下对车厢参数计算，并通过特定算法筛选出估计误差小的坐标点，保证了算法的鲁棒性和精确性；所述测量***可以测量任意车型，无需对车辆停靠位置严格限定。
附图说明
图1为本发明中的面向自动化装车的车厢位姿测量***的结构框图；
图2为本发明中的面向自动化装车的车厢位姿测量***的结构示意图；
图3为本发明中的面向自动化装车的车厢位姿测量***中的测量机构结构示意图；
图4为本发明中的面向自动化装车的车厢位姿测量***中的电动缸推杆局部剖视图；
图5为本发明中的面向自动化装车的车厢位姿测量***的控制程序编写流程图；
图6为本发明中的面向自动化装车的车厢位姿测量***的使用方法流程图；
图中，1、伺服电机，2、电动缸固定座，3、电动缸推杆，4、激光测距仪，5、360°激光扫描测距雷达，6、齿轮组传动，7、丝杠传动。
具体实施方式
下面结合附图和具体实施实例对发明做进一步说明。
如图1-3所示，一种面向自动化装车的车厢位姿测量***，包括控制***和测量机构，所述测量机构包括伸缩杆机构、360°激光扫描测距雷达5、激光测距仪4，360°激光扫描测距雷达5安装在伸缩杆机构的正下方，激光测距仪4固定在伸缩杆机构的侧面，360°激光扫描测距雷达5和激光测距仪4分别通过串口通信与控制***相连，测量待测车厢的位姿时，将测量机构竖直安装在被测车厢的正上方，测量机构的固定位置高于车厢的厢顶面，且满足测量机构竖直方向下移时不与车厢的厢壁发生干涉，通过控制***控制伸缩杆机构带动360°激光扫描测距雷达5和激光测距仪4下移，360°激光扫描测距雷达5在下移的过程中实时采集车厢的极坐标数据点集，并将采集到的数据实时传输给控制***，控制***根据360°激光扫描测距雷达5采集的极坐标数据点集识别车厢的存在，当识别到车厢存在时，通过控制***控制伸缩杆机构停止运动，并触发激光测距仪4采集
Figure BDA0002475201990000081
次车厢的深度数据，
Figure BDA0002475201990000082
表示激光测距仪的预设采样次数，当识别到车厢存在后，控制***控制360°激光扫描测距雷在车厢内移动，通过360°激光扫描测距雷达5采集的极坐标数据点集实现车厢位姿的估算，所述车厢的位姿包括车厢的长度值、车厢的宽度值、车厢的偏移角度值，具体控制程序编写流程图如图5所述，控制***采用PLC，包含有具体的用户界面，可以很好的实现与测量***的人机交互功能，采用的编程软件为C语言。
所述伸缩杆机构用于带动360°激光扫描测距雷达5和激光测距仪4实现竖直方向的上下移动；
所述360°激光扫描测距雷达5用于采集车厢的极坐标数据点集；
所述激光测距仪4用于采集车厢的深度数据；
所述控制***用于根据采集到的极坐标数据点集识别车厢的存在，并估算车厢的位姿，所述车厢的位姿包括车厢的长度值、宽度值、车厢偏移角度值；
所述控制***还用于根据采集到的深度数据实现车厢深度的测量。
所述的伸缩杆机构包括伺服电机1、电动缸固定座2、电动缸推杆3，伺服电机1固定在竖直安装的电动缸固定座2上，伺服电机1的输出轴通过联轴器与电动缸推杆3的输入轴相连，电动缸推杆3的底端安装有360°激光扫描测距雷达5，电动缸推杆3的底部侧面安装有激光测距仪4。
如图4所示，伺服电机1通过齿轮组传动6和丝杠传动7来驱动电动缸推杆3直线移动，检测时电动缸保持与地面垂直，激光测距仪4朝向车厢。
如图2所示建立坐标系，以电动缸推杆3的底部为原点构建坐标系，坐标系的x轴为360度激光扫描测距雷达5的0度扫描方向，坐标系的y轴为360度激光扫描测距雷达5的90度扫描方向，坐标系的z轴为激光测距仪4的测量方向。
所述控制***根据360°激光扫描测距雷达采集的极坐标数据点集识别车厢的存在，具体表述为：
S2.1：当未检测到车厢的存在时，通过控制***设置360°激光扫描测距雷达的采样时间间隔△t1，360°激光扫描测距雷达和激光测距仪随着伸缩杆机构下移，360°激光扫描测距雷达每间隔△t扫描一次，并采集极坐标数据点集
Figure BDA0002475201990000091
其中θ表示测量点相对于360°激光扫描测距雷达自身朝向的夹角的角度值，r表示360°激光扫描测距雷达距离当前测量点之间的距离值，
Figure BDA0002475201990000092
表示第i次扫描时采集到的第m个数据点的角度值，
Figure BDA0002475201990000093
表示第i次扫描时采集到的第m个数据点的距离值，n表示360°激光扫描测距雷达第i次扫描时采集到的极坐标数据点的总个数；
S2.2：控制***实时读取360°激光扫描测距雷达采集的极坐标数据点集，存储在控制***中，并统计第i次采集的极坐标数据点集
Figure BDA0002475201990000094
中非(θm i,0)坐标点的个数Li，通过公式(1)计算非(θm i,0)坐标点所占比值ηi，其中j∈(1,n)；
Figure BDA0002475201990000101
S2.3：当控制***检测到，360°激光扫描测距雷达最新一次扫描时采集到的极坐标数据点集
Figure BDA0002475201990000102
中非(θm j,0)坐标点的个数Lj所占比值
Figure BDA0002475201990000103
时，则说明360°激光扫描测距雷达已经检测到车厢的存在，其中
Figure BDA0002475201990000104
表示非(θm j,0)坐标点个数的给定阈值，如果控制***未检测到车厢的存在，则继续控制伸缩杆机构下移，直至检测到车厢的存在时，控制***停止对伸缩杆机构下移的自动控制。
所述当识别到车厢存在后，控制***控制360°激光扫描测距雷在车厢内移动，通过360°激光扫描测距雷达采集的极坐标数据点集实现车厢位姿的估算，具体表述为：
S4.1：当360°激光扫描测距雷达检测到车厢存在后，通过控制***控制伸缩杆机构下移到车厢内的某一给定高度H'；
S4.2：通过控制***控制360°激光扫描测距雷达在高度H'的位置进行一次扫描，记录采集到的极坐标数据点集为
Figure BDA0002475201990000105
S4.3：对所述极坐标数据点集
Figure BDA0002475201990000106
进行中值滤波处理，得到处理后的极坐标数据点集为
Figure BDA0002475201990000107
S4.4：对极坐标数据点集
Figure BDA0002475201990000108
中的n个极坐标数据点进行自适应筛选，得到一个新的极坐标数据点集{(θs,rs),(θe,re),(θ1,r1),(θ2,r2),(θ3,r3),(θ4,r4)}，所述自适应筛选是指对α随机选取不同的数值，其中α的取值范围为0＜α＜1，具体表述为：
S4.4.1：搜索
Figure BDA0002475201990000109
的n个距离值中的最小值记为r1，定义最小值r1所对应的角度值为θ1，则搜索到的第一个极坐标数据点为(θ1,r1)；
S4.4.2：搜索
Figure BDA00024752019900001010
中角度值为θ1+0.5π的数据点记为(θ2,r2)，则满足θ2＝θ1+0.5π，r2为角度值θ1+0.5π所对应的距离值；搜索
Figure BDA00024752019900001011
中角度值为θ1+π的数据点记为(θ3,r3)，则满足θ3＝θ1+π，r3为角度值θ1+π所对应的距离值；搜索
Figure BDA00024752019900001012
中角度值为θ1-0.5π的数据点记为(θ4,r4)，则满足θ4＝θ1-0.5π，r4为角度值θ1-0.5π所对应的距离值；
S4.4.3：令x1＝r1*cos(θ1)-r2*sin(θ1)，y1＝r2*cos(θ1)+r1*sin(θ1)，a＝r4+r2，b＝r1+r3，利用公式(11)～公式(14)计算出四个采样角度值γ1,γ2,γ3,γ4；
Figure BDA0002475201990000111
Figure BDA0002475201990000112
Figure BDA0002475201990000113
Figure BDA0002475201990000114
S4.4.4：对所述四个采样角度值γ1,γ2,γ3,γ4从小到大进行排序，定义处在排序中第二个位置的采样角度值为中间值记为θm,θm∈{γ1,γ2,γ3,γ4}；
S4.4.5：定义θs＝θm-α×θm，θe＝θm+α×θm，对α随机选取一个数值，0＜α＜1，在
Figure BDA0002475201990000115
中搜索角度值为θm、θs、θe所对应的距离值，分别定义为rm、rs、re，则搜索出来的三个数据点分别为(θm,rm)、(θs,rs)、(θe,re)；
S4.4.6：将搜索出来的6个数据点组合为一个新的极坐标数据点集，记为{(θm,rm),(θs,rs),(θe,re),(θ1,r1),(θ2,r2),(θ3,r3),(θ4,r4)}。
S4.5：将新的极坐标数据点集{(θm,rm),(θs,rs),(θe,re),(θ1,r1),(θ2,r2),(θ3,r3),(θ4,r4)}中的极坐标表示的数据点转换为直角坐标系中的对应点坐标，转换后的直角坐标数据点集记为{(xm,ym),(xs,ys),(xe,ye),(x1,y1),(x2,y2),(x3,y3),(x4,y4)}，其中(xm,ym)为(θm,rm)在直角坐标系中对应点的坐标，(xs,ys)为(θs,rs)在直角坐标系中对应点的坐标，(xe,ye)为(θe,re)在直角坐标系中对应点的坐标，(x1,y1)为(θ1,r1)在直角坐标系中对应点的坐标，(x2,y2)为(θ2,r2)在直角坐标系中对应点的坐标，(x3,y3)为(θ3,r3)在直角坐标系中对应点的坐标，(x4,y4)为(θ4,r4)在直角坐标系中对应点的坐标；
S4.6：将(xm,ym)、(xs,ys)、(xs,ys)三点，利用线性回归方法拟合为一条拟合直线记为l，并计算出拟合直线l的斜率记为k；
S4.7：根据求取直线的点斜式方程，利用斜率k与点(x1,y1)、(x2,y2)、(x3,y3)、(x4,y4)分别求取四条直线l1、l2、l3、l4，如公式(2)～公式(5)所示，
l1:y-y1＝k(x-x1) (2)
Figure BDA0002475201990000121
l3:y-y3＝k(x-x3) (4)
Figure BDA0002475201990000122
S4.8：计算直线l1与直线l2的交点，记为A1,2＝(x1,2,y1,2)，计算直线l2与直线l3的交点，记为A2,3＝(x2,3,y2,3)，计算直线l3与直线l4的交点，记为A3,4＝(x3,4,y3,4)，计算直线l4与直线l1的交点，记为A4,1＝(x4,1,y4,1)，则得到一组顶点集{A1,2,A2,3,A3,4,A4,1}；
S4.9：重复S4.4进行自适应筛选，对α随机选取ψ个不同的数值{α1,α2,…,αψ}，如α＝0.1,0.2,0.3,…具体数值，计算得到θs的ψ个值，记为
Figure BDA0002475201990000123
计算得到θe的ψ个值，记为
Figure BDA0002475201990000124
然后在
Figure BDA0002475201990000125
中搜索出
Figure BDA0002475201990000126
中每个值所对应的距离值，记为
Figure BDA0002475201990000127
在
Figure BDA0002475201990000128
中搜索出
Figure BDA0002475201990000129
中每个值所对应的距离值，记为
Figure BDA00024752019900001210
则最后搜索得到ψ组新的极坐标数据点集，如公式(6)所示，
Figure BDA00024752019900001211
S4.10：重复S4.5～S4.8，对公式(6)所示的ψ组新的极坐标数据点集，分别通过求取直线、计算相邻两两直线的交点计算得到ψ组顶点集，如公式(7)所示，得到ψ个拟合直线，以及对应的ψ个斜率值，记为斜率值集{k1,k2,…,kψ}，
Figure BDA00024752019900001212
S4.11：1)对公式(7)给出的ψ组顶点集，1.1)计算
Figure BDA00024752019900001213
中各个坐标点的所有x坐标值的中值，以及所有y坐标值的中值，并将x坐标值的中值和y坐标值的中值组合为坐标点
Figure BDA0002475201990000131
1.2)计算
Figure BDA0002475201990000132
中各个坐标点的所有x坐标值的中值，以及所有y坐标值的中值，并将x坐标值的中值和y坐标值的中值组合为坐标点
Figure BDA0002475201990000133
1.3)计算
Figure BDA0002475201990000134
中各个坐标点的所有x坐标值的中值，以及所有y坐标值的中值，并将x坐标值的中值和y坐标值的中值组合为坐标点
Figure BDA0002475201990000135
1.4)计算
Figure BDA0002475201990000136
中各个坐标点的所有x坐标值的中值，以及所有y坐标值的中值，并将x坐标值的中值和y坐标值的中值组合为坐标点
Figure BDA0002475201990000137
1.5)将
Figure BDA0002475201990000138
组合为
Figure BDA0002475201990000139
即
Figure BDA00024752019900001310
为所在高度H'的估计顶点集，所述x坐标值是指坐标点在直角坐标系中对应的x轴方向的值，所述y坐标值是指坐标点在直角坐标系中对应的y轴方向的值；
2)计算斜率值集{k1,k2,…,kψ}中ψ个斜率值的平均值
Figure BDA00024752019900001311
S4.12：为了得到不同高度下的S组估计顶点集，通过控制***控制伸缩杆机构在车厢内移动S个不同的高度值，重复S4.1～S4.11，计算得到
Figure BDA00024752019900001312
的S组值，记为估计顶点集如公式(8)所示，计算得到
Figure BDA00024752019900001313
的S个值，记为估计斜率值集
Figure BDA00024752019900001314
Figure BDA00024752019900001315
S4.13：对于公式(8)给出的S组估计顶点集，利用公式(9)计算每个坐标点的S个x坐标值的平均值，利用公式(10)计算每个坐标点的S个y坐标值的平均值，得到最终估计顶点集
Figure BDA00024752019900001316
Figure BDA00024752019900001317
Figure BDA00024752019900001318
式中，u＝1,2,3,4，v＝1,2,3,…,S，u表示坐标点在估计顶点集中的位置，
Figure BDA00024752019900001319
表示第v个估计顶点集中第1个坐标点的x坐标值，
Figure BDA00024752019900001320
表示第v个估计顶点集中第1个坐标点的y坐标值，
Figure BDA00024752019900001321
表示第v个估计顶点集中第2个坐标点的x坐标值，
Figure BDA00024752019900001322
表示第v个估计顶点集中第2个坐标点的y坐标值，
Figure BDA0002475201990000141
表示第v个估计顶点集中第3个坐标点的x坐标值，
Figure BDA0002475201990000142
表示第v个估计顶点集中第3个坐标点的y坐标值，
Figure BDA0002475201990000143
表示第v个估计顶点集中第4个坐标点的x坐标值，
Figure BDA0002475201990000144
表示第v个估计顶点集中第4个坐标点的y坐标值，
Figure BDA0002475201990000145
表示第1个坐标点的S个x坐标值的平均值，
Figure BDA0002475201990000146
表示第1个坐标点的S个y坐标值的平均值，
Figure BDA0002475201990000147
表示第2个坐标点的S个x坐标值的平均值，
Figure BDA0002475201990000148
表示第2个坐标点的S个y坐标值的平均值，
Figure BDA0002475201990000149
表示第3个坐标点的S个x坐标值的平均值，
Figure BDA00024752019900001410
表示第3个坐标点的S个y坐标值的平均值，
Figure BDA00024752019900001411
表示第4个坐标点的S个x坐标值的平均值，
Figure BDA00024752019900001412
表示第4个坐标点的S个y坐标值的平均值；
S4.14：对于最终估计顶点集
Figure BDA00024752019900001413
计算最终估计顶点集中任意两两坐标点的距离，得到六个距离值，对所述六个距离值从大到小排序，其中长度最大的距离值即为车厢的对角线值，长度次之的距离值即为车厢的长度值，长度最小的距离值即为车厢的宽度值；
S4.15：计算估计斜率值集
Figure BDA00024752019900001414
中S个斜率值的平均值
Figure BDA00024752019900001415
则平均值
Figure BDA00024752019900001416
对应的角度值即为车厢相对于0°测量方向的偏移角度值，所述0°测量方向根据360°激光扫描测距雷达的安装方向定义。
所述当识别到车厢存在时，通过控制***触发激光测距仪采集
Figure BDA00024752019900001417
次车厢的深度数据，来实现车厢深度的测量，具体表述为：
S3.1：当识别到车厢存在时，通过控制***控制激光测距仪采集
Figure BDA00024752019900001418
次深度数据，记为H1,H2,H3；
S3.2：计算
Figure BDA00024752019900001419
则Hh即为车厢的深度值。
一种面向自动化装车的车厢位姿测量***的使用方法，如图6所述，包括如下步骤：
步骤1：将测量机构竖直安装在被测车厢的正上方，测量机构的固定位置高于车厢的厢顶面，且满足测量机构竖直方向下移时不与车厢的厢壁发生干涉；
步骤2：通过控制***控制伸缩杆机构下移，伸缩杆机构带动360°激光扫描测距雷达和激光测距仪下移，控制***根据360°激光扫描测距雷达采集的极坐标数据点集识别车厢的存在，具体表述为：
S2.1：当未检测到车厢的存在时，通过控制***设置360°激光扫描测距雷达的采样时间间隔△t1，360°激光扫描测距雷达和激光测距仪随着伸缩杆机构下移，360°激光扫描测距雷达每间隔△t扫描一次，并采集极坐标数据点集
Figure BDA0002475201990000151
其中θ表示测量点相对于360°激光扫描测距雷达自身朝向的夹角的角度值，r表示360°激光扫描测距雷达距离当前测量点之间的距离值，
Figure BDA0002475201990000152
表示第i次扫描时采集到的第m个数据点的角度值，
Figure BDA0002475201990000153
表示第i次扫描时采集到的第m个数据点的距离值，n表示360°激光扫描测距雷达第i次扫描时采集到的极坐标数据点的总个数；
S2.2：控制***实时读取360°激光扫描测距雷达采集的极坐标数据点集，存储在控制***中，并统计第i次采集的极坐标数据点集
Figure BDA0002475201990000154
中非
Figure BDA0002475201990000155
坐标点的个数Li，通过公式(1)计算非(θm i,0)坐标点所占比值ηi，其中j∈(1,n)；
Figure BDA0002475201990000156
S2.3：当控制***检测到，360°激光扫描测距雷达最新一次扫描时采集到的极坐标数据点集
Figure BDA0002475201990000157
中非(θm j,0)坐标点的个数Lj所占比值
Figure BDA0002475201990000158
时，则说明360°激光扫描测距雷达已经检测到车厢的存在，其中
Figure BDA0002475201990000159
表示非(θm j,0)坐标点个数的给定阈值，如果控制***未检测到车厢的存在，则继续控制伸缩杆机构下移，直至检测到车厢的存在时，控制***停止对伸缩杆机构下移的自动控制。
步骤3：当控制***识别到车厢的存在时，触发激光测距仪采集
Figure BDA00024752019900001510
次深度数据，来实现车厢深度的测量，
Figure BDA00024752019900001511
表示激光测距仪的预设采样次数，具体表述为：
S3.1：当识别到车厢存在时，通过控制***控制激光测距仪采集
Figure BDA00024752019900001512
次深度数据，记为H1,H2,H3；
S3.2：计算
Figure BDA00024752019900001513
则Hh即为车厢的深度值。
步骤4：当控制***识别到车厢的存在后，控制***控制360°激光扫描测距雷达在车厢内移动，通过360°激光扫描测距雷达采集的极坐标数据点集实现车厢位姿的估算，具体表述为：
S4.1：当360°激光扫描测距雷达检测到车厢存在后，通过控制***控制伸缩杆机构下移到车厢内的某一给定高度H'；
S4.2：通过控制***控制360°激光扫描测距雷达在高度H'的位置进行一次扫描，记录采集到的极坐标数据点集为
Figure BDA0002475201990000161
S4.3：对所述极坐标数据点集
Figure BDA0002475201990000162
进行中值滤波处理，得到处理后的极坐标数据点集为
Figure BDA0002475201990000163
S4.4：对极坐标数据点集
Figure BDA0002475201990000164
中的n个极坐标数据点进行自适应筛选，得到一个新的极坐标数据点集{(θs,rs),(θe,re),(θ1,r1),(θ2,r2),(θ3,r3),(θ4,r4)}，所述自适应筛选是指对α随机选取不同的数值，其中α的取值范围为0＜α＜1，具体表述为：
S4.4.1：搜索
Figure BDA0002475201990000165
的n个距离值中的最小值记为r1，定义最小值r1所对应的角度值为θ1，则搜索到的第一个极坐标数据点为(θ1,r1)；
S4.4.2：搜索
Figure BDA0002475201990000166
中角度值为θ1+0.5π的数据点记为(θ2,r2)，则满足θ2＝θ1+0.5π，r2为角度值θ1+0.5π所对应的距离值；搜索
Figure BDA0002475201990000167
中角度值为θ1+π的数据点记为(θ3,r3)，则满足θ3＝θ1+π，r3为角度值θ1+π所对应的距离值；搜索
Figure BDA0002475201990000168
中角度值为θ1-0.5π的数据点记为(θ4,r4)，则满足θ4＝θ1-0.5π，r4为角度值θ1-0.5π所对应的距离值；
S4.4.3：令x1＝r1*cos(θ1)-r2*sin(θ1)，y1＝r2*cos(θ1)+r1*sin(θ1)，a＝r4+r2，b＝r1+r3，利用公式(11)～公式(14)计算出四个采样角度值γ1,γ2,γ3,γ4；
Figure BDA0002475201990000169
Figure BDA00024752019900001610
Figure BDA00024752019900001611
Figure BDA00024752019900001612
S4.4.4：对所述四个采样角度值γ1,γ2,γ3,γ4从小到大进行排序，定义处在排序中第二个位置的采样角度值为中间值记为θm,θm∈{γ1,γ2,γ3,γ4}；
S4.4.5：定义θs＝θm-α×θm，θe＝θm+α×θm，对α随机选取一个数值，0＜α＜1，在
Figure BDA0002475201990000171
中搜索角度值为θm、θs、θe所对应的距离值，分别定义为rm、rs、re，则搜索出来的三个数据点分别为(θm,rm)、(θs,rs)、(θe,re)；
S4.4.6：将搜索出来的6个数据点组合为一个新的极坐标数据点集，记为{(θm,rm),(θs,rs),(θe,re),(θ1,r1),(θ2,r2),(θ3,r3),(θ4,r4)}。
S4.5：将新的极坐标数据点集{(θm,rm),(θs,rs),(θe,re),(θ1,r1),(θ2,r2),(θ3,r3),(θ4,r4)}中的极坐标表示的数据点转换为直角坐标系中的对应点坐标，转换后的直角坐标数据点集记为{(xm,ym),(xs,ys),(xe,ye),(x1,y1),(x2,y2),(x3,y3),(x4,y4)}，其中(xm,ym)为(θm,rm)在直角坐标系中对应点的坐标，(xs,ys)为(θs,rs)在直角坐标系中对应点的坐标，(xe,ye)为(θe,re)在直角坐标系中对应点的坐标，(x1,y1)为(θ1,r1)在直角坐标系中对应点的坐标，(x2,y2)为(θ2,r2)在直角坐标系中对应点的坐标，(x3,y3)为(θ3,r3)在直角坐标系中对应点的坐标，(x4,y4)为(θ4,r4)在直角坐标系中对应点的坐标；
S4.6：将(xm,ym)、(xs,ys)、(xs,ys)三点，利用线性回归方法拟合为一条拟合直线记为l，并计算出拟合直线l的斜率记为k；
S4.7：根据求取直线的点斜式方程，利用斜率k与点(x1,y1)、(x2,y2)、(x3,y3)、(x4,y4)分别求取四条直线l1、l2、l3、l4，如公式(2)～公式(5)所示，
l1:y-y1＝k(x-x1) (2)
Figure BDA0002475201990000172
l3:y-y3＝k(x-x3) (4)
Figure BDA0002475201990000173
S4.8：计算直线l1与直线l2的交点，记为A1,2＝(x1,2,y1,2)，计算直线l2与直线l3的交点，记为A2,3＝(x2,3,y2,3)，计算直线l3与直线l4的交点，记为A3,4＝(x3,4,y3,4)，计算直线l4与直线l1的交点，记为A4,1＝(x4,1,y4,1)，则得到一组顶点集{A1,2,A2,3,A3,4,A4,1}；
S4.9：重复S4.4进行自适应筛选，对α随机选取ψ个不同的数值{α1,α2,…,αψ}，如α＝0.1,0.2,0.3,…具体数值，计算得到θs的ψ个值，记为
Figure BDA0002475201990000181
计算得到θe的ψ个值，记为
Figure BDA0002475201990000182
然后在
Figure BDA0002475201990000183
中搜索出
Figure BDA0002475201990000184
中每个值所对应的距离值，记为
Figure BDA0002475201990000185
在
Figure BDA0002475201990000186
中搜索出
Figure BDA0002475201990000187
中每个值所对应的距离值，记为
Figure BDA0002475201990000188
则最后搜索得到ψ组新的极坐标数据点集，如公式(6)所示，
Figure BDA0002475201990000189
S4.10：重复S4.5～S4.8，对公式(6)所示的ψ组新的极坐标数据点集，分别通过求取直线、计算相邻两两直线的交点计算得到ψ组顶点集，如公式(7)所示，得到ψ个拟合直线，以及对应的ψ个斜率值，记为斜率值集{k1,k2,…,kψ}，
Figure BDA00024752019900001810
S4.11：1)对公式(7)给出的ψ组顶点集，1.1)计算
Figure BDA00024752019900001811
中各个坐标点的所有x坐标值的中值，以及所有y坐标值的中值，并将x坐标值的中值和y坐标值的中值组合为坐标点
Figure BDA00024752019900001812
1.2)计算
Figure BDA00024752019900001813
中各个坐标点的所有x坐标值的中值，以及所有y坐标值的中值，并将x坐标值的中值和y坐标值的中值组合为坐标点
Figure BDA00024752019900001814
1.3)计算
Figure BDA00024752019900001815
中各个坐标点的所有x坐标值的中值，以及所有y坐标值的中值，并将x坐标值的中值和y坐标值的中值组合为坐标点
Figure BDA00024752019900001816
1.4)计算
Figure BDA00024752019900001817
中各个坐标点的所有x坐标值的中值，以及所有y坐标值的中值，并将x坐标值的中值和y坐标值的中值组合为坐标点
Figure BDA00024752019900001818
1.5)将
Figure BDA00024752019900001819
组合为
Figure BDA00024752019900001820
即
Figure BDA00024752019900001821
为所在高度H'的估计顶点集，所述x坐标值是指坐标点在直角坐标系中对应的x轴方向的值，所述y坐标值是指坐标点在直角坐标系中对应的y轴方向的值；
2)计算斜率值集{k1,k2,…,kψ}中ψ个斜率值的平均值
Figure BDA0002475201990000191
S4.12：为了得到不同高度下的S组估计顶点集，通过控制***控制伸缩杆机构在车厢内移动S个不同的高度值，重复S4.1～S4.11，计算得到
Figure BDA0002475201990000192
的S组值，记为估计顶点集如公式(8)所示，计算得到
Figure BDA0002475201990000193
的S个值，记为估计斜率值集
Figure BDA0002475201990000194
Figure BDA0002475201990000195
S4.13：对于公式(8)给出的S组估计顶点集，利用公式(9)计算每个坐标点的S个x坐标值的平均值，利用公式(10)计算每个坐标点的S个y坐标值的平均值，得到最终估计顶点集
Figure BDA0002475201990000196
Figure BDA0002475201990000197
Figure BDA0002475201990000198
式中，u＝1,2,3,4，v＝1,2,3,…,S，u表示坐标点在估计顶点集中的位置，
Figure BDA0002475201990000199
表示第v个估计顶点集中第1个坐标点的x坐标值，
Figure BDA00024752019900001910
表示第v个估计顶点集中第1个坐标点的y坐标值，
Figure BDA00024752019900001911
表示第v个估计顶点集中第2个坐标点的x坐标值，
Figure BDA00024752019900001912
表示第v个估计顶点集中第2个坐标点的y坐标值，
Figure BDA00024752019900001913
表示第v个估计顶点集中第3个坐标点的x坐标值，
Figure BDA00024752019900001914
表示第v个估计顶点集中第3个坐标点的y坐标值，
Figure BDA00024752019900001915
表示第v个估计顶点集中第4个坐标点的x坐标值，
Figure BDA00024752019900001916
表示第v个估计顶点集中第4个坐标点的y坐标值，
Figure BDA00024752019900001917
表示第1个坐标点的S个x坐标值的平均值，
Figure BDA00024752019900001918
表示第1个坐标点的S个y坐标值的平均值，
Figure BDA00024752019900001919
表示第2个坐标点的S个x坐标值的平均值，
Figure BDA00024752019900001920
表示第2个坐标点的S个y坐标值的平均值，
Figure BDA00024752019900001921
表示第3个坐标点的S个x坐标值的平均值，
Figure BDA00024752019900001922
表示第3个坐标点的S个y坐标值的平均值，
Figure BDA00024752019900001923
表示第4个坐标点的S个x坐标值的平均值，
Figure BDA00024752019900001924
表示第4个坐标点的S个y坐标值的平均值；
S4.14：对于最终估计顶点集
Figure BDA0002475201990000201
计算最终估计顶点集中任意两两坐标点的距离，得到六个距离值，对所述六个距离值从大到小排序，其中长度最大的距离值即为车厢的对角线值，长度次之的距离值即为车厢的长度值，长度最小的距离值即为车厢的宽度值；
S4.15：计算估计斜率值集
Figure BDA0002475201990000202
中S个斜率值的平均值
Figure BDA0002475201990000203
则平均值
Figure BDA0002475201990000204
对应的角度值即为车厢相对于0°测量方向的偏移角度值，所述0°测量方向根据360°激光扫描测距雷达的安装方向定义。
步骤5：通过控制***计算得到车厢的位姿数据以及深度值后，控制伸缩杆机构上移到初始位置，测量结束。

Claims (7)

1.一种面向自动化装车的车厢位姿测量***，其特征在于，包括控制***和测量机构，所述测量机构包括伸缩杆机构、360°激光扫描测距雷达、激光测距仪，360°激光扫描测距雷达安装在伸缩杆机构的正下方，激光测距仪固定在伸缩杆机构的侧面，360°激光扫描测距雷达和激光测距仪分别通过串口通信与控制***相连，测量待测车厢的位姿时，将测量机构竖直安装在被测车厢的正上方，测量机构的固定位置高于车厢的厢顶面，且满足测量机构竖直方向下移时不与车厢的厢壁发生干涉，通过控制***控制伸缩杆机构带动360°激光扫描测距雷达和激光测距仪下上移动；
所述伸缩杆机构用于带动360°激光扫描测距雷达和激光测距仪实现竖直方向的上下移动；
所述360°激光扫描测距雷达用于采集车厢的极坐标数据点集；
所述激光测距仪用于采集车厢的深度数据；
所述控制***用于根据采集到的极坐标数据点集识别车厢的存在，并估算车厢的位姿，所述车厢的位姿包括车厢的长度值、宽度值、车厢偏移角度值；
所述控制***还用于根据采集到的深度数据实现车厢深度的测量。
2.根据权利要求1所述的一种面向自动化装车的车厢位姿测量***，其特征在于，所述的伸缩杆机构包括伺服电机、电动缸固定座、电动缸推杆，伺服电机固定在竖直安装的电动缸固定座上，伺服电机的输出轴通过联轴器与电动缸推杆的输入轴相连，电动缸推杆的底端安装有360°激光扫描测距雷达，电动缸推杆的底部侧面安装有激光测距仪。
3.一种权利要求1或2所述的面向自动化装车的车厢位姿测量***的使用方法，其特征在于，包括如下步骤：
步骤1：将测量机构竖直安装在被测车厢的正上方，测量机构的固定位置高于车厢的厢顶面，且满足测量机构竖直方向下移时不与车厢的厢壁发生干涉；
步骤2：通过控制***控制伸缩杆机构下移，伸缩杆机构带动360°激光扫描测距雷达和激光测距仪下移，控制***根据360°激光扫描测距雷达采集的极坐标数据点集识别车厢的存在；
步骤3：当控制***识别到车厢的存在时，触发激光测距仪采集
Figure FDA0002475201980000011
次深度数据，来实现车厢深度的测量，
Figure FDA0002475201980000012
表示激光测距仪的预设采样次数；
步骤4：当控制***识别到车厢的存在后，控制***控制360°激光扫描测距雷达在车厢内移动，通过360°激光扫描测距雷达采集的极坐标数据点集实现车厢位姿的估算；
步骤5：通过控制***计算得到车厢的位姿数据以及深度值后，控制伸缩杆机构上移到初始位置，测量结束。
4.根据权利要求3所述的一种面向自动化装车的车厢位姿测量***的使用方法，其特征在于，所述步骤2中的控制***根据360°激光扫描测距雷达采集的极坐标数据点集识别车厢的存在，具体表述为：
S2.1：当未检测到车厢的存在时，通过控制***设置360°激光扫描测距雷达的采样时间间隔△t1，360°激光扫描测距雷达和激光测距仪随着伸缩杆机构下移，360°激光扫描测距雷达每间隔△t扫描一次，并采集极坐标数据点集
Figure FDA0002475201980000021
其中θ表示测量点相对于360°激光扫描测距雷达自身朝向的夹角的角度值，r表示360°激光扫描测距雷达距离当前测量点之间的距离值，
Figure FDA0002475201980000022
表示第i次扫描时采集到的第m个数据点的角度值，
Figure FDA0002475201980000023
表示第i次扫描时采集到的第m个数据点的距离值，n表示360°激光扫描测距雷达第i次扫描时采集到的极坐标数据点的总个数；
S2.2：控制***实时读取360°激光扫描测距雷达采集的极坐标数据点集，存储在控制***中，并统计第i次采集的极坐标数据点集
Figure FDA0002475201980000024
中非(θm i,0)坐标点的个数Li，通过公式(1)计算非(θm i,0)坐标点所占比值ηi，其中j∈(1,n)；
Figure FDA0002475201980000025
S2.3：当控制***检测到，360°激光扫描测距雷达最新一次扫描时采集到的极坐标数据点集
Figure FDA0002475201980000026
中非(θm j,0)坐标点的个数Lj所占比值
Figure FDA0002475201980000027
时，则说明360°激光扫描测距雷达已经检测到车厢的存在，其中
Figure FDA0002475201980000028
表示非(θm j,0)坐标点个数的给定阈值，如果控制***未检测到车厢的存在，则继续控制伸缩杆机构下移，直至检测到车厢的存在时，控制***停止对伸缩杆机构下移的自动控制。
5.根据权利要求3所述的一种面向自动化装车的车厢位姿测量***的使用方法，其特征在于，所述步骤3具体表述为：
S3.1：当识别到车厢存在时，通过控制***控制激光测距仪采集
Figure FDA0002475201980000029
次深度数据，记为
Figure FDA00024752019800000210
S3.2：计算
Figure FDA00024752019800000211
则Hh即为车厢的深度值。
6.根据权利要求3所述的一种面向自动化装车的车厢位姿测量***的使用方法，其特征在于，所述步骤4具体表述为：
S4.1：当360°激光扫描测距雷达检测到车厢存在后，通过控制***控制伸缩杆机构下移到车厢内的某一给定高度H'；
S4.2：通过控制***控制360°激光扫描测距雷达在高度H'的位置进行一次扫描，记录采集到的极坐标数据点集为
Figure FDA0002475201980000031
S4.3：对所述极坐标数据点集
Figure FDA0002475201980000032
进行中值滤波处理，得到处理后的极坐标数据点集为
Figure FDA0002475201980000033
S4.4：对极坐标数据点集
Figure FDA0002475201980000034
中的n个极坐标数据点进行自适应筛选，得到一个新的极坐标数据点集{(θs,rs),(θe,re),(θ1,r1),(θ2,r2),(θ3,r3),(θ4,r4)}，所述自适应筛选是指对α随机选取不同的数值，其中α的取值范围为0＜α＜1；
S4.5：将新的极坐标数据点集{(θm,rm),(θs,rs),(θe,re),(θ1,r1),(θ2,r2),(θ3,r3),(θ4,r4)}中的极坐标表示的数据点转换为直角坐标系中的对应点坐标，转换后的直角坐标数据点集记为{(xm,ym),(xs,ys),(xe,ye),(x1,y1),(x2,y2),(x3,y3),(x4,y4)}，其中(xm,ym)为(θm,rm)在直角坐标系中对应点的坐标，(xs,ys)为(θs,rs)在直角坐标系中对应点的坐标，(xe,ye)为(θe,re)在直角坐标系中对应点的坐标，(x1,y1)为(θ1,r1)在直角坐标系中对应点的坐标，(x2,y2)为(θ2,r2)在直角坐标系中对应点的坐标，(x3,y3)为(θ3,r3)在直角坐标系中对应点的坐标，(x4,y4)为(θ4,r4)在直角坐标系中对应点的坐标；
S4.6：将(xm,ym)、(xs,ys)、(xs,ys)三点，利用线性回归方法拟合为一条拟合直线记为l，并计算出拟合直线l的斜率记为k；
S4.7：根据求取直线的点斜式方程，利用斜率k与点(x1,y1)、(x2,y2)、(x3,y3)、(x4,y4)分别求取四条直线l1、l2、l3、l4，如公式(2)～公式(5)所示，
l1:y-y1＝k(x-x1) (2)
Figure FDA0002475201980000035
l3:y-y3＝k(x-x3) (4)
Figure FDA0002475201980000036
S4.8：计算直线l1与直线l2的交点，记为A1,2＝(x1,2,y1,2)，计算直线l2与直线l3的交点，记为A2,3＝(x2,3,y2,3)，计算直线l3与直线l4的交点，记为A3,4＝(x3,4,y3,4)，计算直线l4与直线l1的交点，记为A4,1＝(x4,1,y4,1)，则得到一组顶点集{A1,2,A2,3,A3,4,A4,1}；
S4.9：重复S4.4进行自适应筛选，对α随机选取ψ个不同的数值{α1,α2,…,αψ}，计算得到θs的ψ个值，记为
Figure FDA0002475201980000041
计算得到θe的ψ个值，记为
Figure FDA0002475201980000042
然后在
Figure FDA0002475201980000043
中搜索出
Figure FDA0002475201980000044
中每个值所对应的距离值，记为
Figure FDA0002475201980000045
在
Figure FDA0002475201980000046
中搜索出
Figure FDA0002475201980000047
中每个值所对应的距离值，记为
Figure FDA0002475201980000048
则最后搜索得到ψ组新的极坐标数据点集，如公式(6)所示，
Figure FDA0002475201980000049
S4.10：重复S4.5～S4.8，对公式(6)所示的ψ组新的极坐标数据点集，分别通过求取直线、计算相邻两两直线的交点计算得到ψ组顶点集，如公式(7)所示，得到ψ个拟合直线，以及对应的ψ个斜率值，记为斜率值集{k1,k2,…,kψ}，
Figure FDA00024752019800000410
S4.11：1)对公式(7)给出的ψ组顶点集，1.1)计算
Figure FDA00024752019800000411
中各个坐标点的所有x坐标值的中值，以及所有y坐标值的中值，并将x坐标值的中值和y坐标值的中值组合为坐标点
Figure FDA00024752019800000412
1.2)计算
Figure FDA00024752019800000413
中各个坐标点的所有x坐标值的中值，以及所有y坐标值的中值，并将x坐标值的中值和y坐标值的中值组合为坐标点
Figure FDA00024752019800000414
1.3)计算
Figure FDA00024752019800000415
中各个坐标点的所有x坐标值的中值，以及所有y坐标值的中值，并将x坐标值的中值和y坐标值的中值组合为坐标点
Figure FDA00024752019800000416
1.4)计算
Figure FDA00024752019800000417
中各个坐标点的所有x坐标值的中值，以及所有y坐标值的中值，并将x坐标值的中值和y坐标值的中值组合为坐标点
Figure FDA00024752019800000418
1.5)将
Figure FDA00024752019800000419
组合为
Figure FDA00024752019800000420
即
Figure FDA00024752019800000421
为所在高度H'的估计顶点集，所述x坐标值是指坐标点在直角坐标系中对应的x轴方向的值，所述y坐标值是指坐标点在直角坐标系中对应的y轴方向的值；
2)计算斜率值集{k1,k2,…,kψ}中ψ个斜率值的平均值
Figure FDA00024752019800000522
S4.12：为了得到不同高度下的S组估计顶点集，通过控制***控制伸缩杆机构在车厢内移动S个不同的高度值，重复S4.1～S4.11，计算得到
Figure FDA0002475201980000051
的S组值，记为估计顶点集如公式(8)所示，计算得到
Figure FDA0002475201980000052
的S个值，记为估计斜率值集
Figure FDA0002475201980000053
Figure FDA0002475201980000054
S4.13：对于公式(8)给出的S组估计顶点集，利用公式(9)计算每个坐标点的S个x坐标值的平均值，利用公式(10)计算每个坐标点的S个y坐标值的平均值，得到最终估计顶点集
Figure FDA0002475201980000055
Figure FDA0002475201980000056
Figure FDA0002475201980000057
式中，u＝1,2,3,4，v＝1,2,3,…,S，u表示坐标点在估计顶点集中的位置，
Figure FDA0002475201980000058
表示第v个估计顶点集中第1个坐标点的x坐标值，
Figure FDA0002475201980000059
表示第v个估计顶点集中第1个坐标点的y坐标值，
Figure FDA00024752019800000510
表示第v个估计顶点集中第2个坐标点的x坐标值，
Figure FDA00024752019800000511
表示第v个估计顶点集中第2个坐标点的y坐标值，
Figure FDA00024752019800000512
表示第v个估计顶点集中第3个坐标点的x坐标值，
Figure FDA00024752019800000513
表示第v个估计顶点集中第3个坐标点的y坐标值，
Figure FDA00024752019800000514
表示第v个估计顶点集中第4个坐标点的x坐标值，
Figure FDA00024752019800000515
表示第v个估计顶点集中第4个坐标点的y坐标值，
Figure FDA00024752019800000516
表示第1个坐标点的S个x坐标值的平均值，
Figure FDA00024752019800000517
表示第1个坐标点的S个y坐标值的平均值，
Figure FDA00024752019800000518
表示第2个坐标点的S个x坐标值的平均值，
Figure FDA00024752019800000519
表示第2个坐标点的S个y坐标值的平均值，
Figure FDA00024752019800000520
表示第3个坐标点的S个x坐标值的平均值，
Figure FDA00024752019800000521
表示第3个坐标点的S个y坐标值的平均值，
Figure FDA0002475201980000061
表示第4个坐标点的S个x坐标值的平均值，
Figure FDA0002475201980000062
表示第4个坐标点的S个y坐标值的平均值；
S4.14：对于最终估计顶点集
Figure FDA0002475201980000063
计算最终估计顶点集中任意两两坐标点的距离，得到六个距离值，对所述六个距离值从大到小排序，其中长度最大的距离值即为车厢的对角线值，长度次之的距离值即为车厢的长度值，长度最小的距离值即为车厢的宽度值；
S4.15：计算估计斜率值集
Figure FDA0002475201980000064
中S个斜率值的平均值
Figure FDA0002475201980000065
则平均值
Figure FDA0002475201980000066
对应的角度值即为车厢相对于0°测量方向的偏移角度值，所述0°测量方向根据360°激光扫描测距雷达的安装方向定义。
7.根据权利要求6所述的一种面向自动化装车的车厢位姿测量***的使用方法，其特征在于，所述的步骤S4.4具体表述为：
S4.4.1：搜索
Figure FDA0002475201980000067
的n个距离值中的最小值记为r1，定义最小值r1所对应的角度值为θ1，则搜索到的第一个极坐标数据点为(θ1,r1)；
S4.4.2：搜索
Figure FDA0002475201980000068
中角度值为θ1+0.5π的数据点记为(θ2,r2)，则满足θ2＝θ1+0.5π，r2为角度值θ1+0.5π所对应的距离值；搜索
Figure FDA0002475201980000069
中角度值为θ1+π的数据点记为(θ3,r3)，则满足θ3＝θ1+π，r3为角度值θ1+π所对应的距离值；搜索
Figure FDA00024752019800000610
中角度值为θ1-0.5π的数据点记为(θ4,r4)，则满足θ4＝θ1-0.5π，r4为角度值θ1-0.5π所对应的距离值；
S4.4.3：令x1＝r1*cos(θ1)-r2*sin(θ1)，y1＝r2*cos(θ1)+r1*sin(θ1)，a＝r4+r2，b＝r1+r3，利用公式(11)～公式(14)计算出四个采样角度值γ1,γ2,γ3,γ4；
Figure FDA00024752019800000611
Figure FDA00024752019800000612
Figure FDA00024752019800000613
Figure FDA0002475201980000071
S4.4.4：对所述四个采样角度值γ1,γ2,γ3,γ4从小到大进行排序，定义处在排序中第二个位置的采样角度值为中间值记为θm,θm∈{γ1,γ2,γ3,γ4}；
S4.4.5：定义θs＝θm-α×θm，θe＝θm+α×θm，对α随机选取一个数值，0＜α＜1，在
Figure FDA0002475201980000072
中搜索角度值为θm、θs、θe所对应的距离值，分别定义为rm、rs、re，则搜索出来的三个数据点分别为(θm,rm)、(θs,rs)、(θe,re)；
S4.4.6：将搜索出来的6个数据点组合为一个新的极坐标数据点集，记为{(θm,rm),(θs,rs),(θe,re),(θ1,r1),(θ2,r2),(θ3,r3),(θ4,r4)}。
CN202010361420.8A 2020-04-30 2020-04-30 面向自动化装车的车厢位姿测量***及方法 Active CN111366086B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010361420.8A CN111366086B (zh) 2020-04-30 2020-04-30 面向自动化装车的车厢位姿测量***及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010361420.8A CN111366086B (zh) 2020-04-30 2020-04-30 面向自动化装车的车厢位姿测量***及方法

Publications (2)

Publication Number Publication Date
CN111366086A true CN111366086A (zh) 2020-07-03
CN111366086B CN111366086B (zh) 2021-07-06

Family

ID=71207606

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010361420.8A Active CN111366086B (zh) 2020-04-30 2020-04-30 面向自动化装车的车厢位姿测量***及方法

Country Status (1)

Country Link
CN (1) CN111366086B (zh)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1877253A (zh) * 2005-06-09 2006-12-13 山东科技大学 车载式近景目标三维测量***及方法
CN202350736U (zh) * 2011-09-29 2012-07-25 凯迈（洛阳）测控有限公司 一种对行驶车辆体积测试的装置
CN102679889A (zh) * 2012-05-30 2012-09-19 北京恒华伟业科技股份有限公司 车辆尺寸的测量方法和装置
CN204421853U (zh) * 2015-03-06 2015-06-24 中国人民解放军军械工程学院 车轮定位参数测量装置
CN204421838U (zh) * 2015-01-20 2015-06-24 吉林大学 一种汽车基本参数测量装置
CN108445505A (zh) * 2018-03-29 2018-08-24 南京航空航天大学 线环境下基于激光雷达的特征显著性检测方法
CN109282755A (zh) * 2018-11-14 2019-01-29 吉林大学 一种车辆外廓尺寸自动测量装置及方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1877253A (zh) * 2005-06-09 2006-12-13 山东科技大学 车载式近景目标三维测量***及方法
CN202350736U (zh) * 2011-09-29 2012-07-25 凯迈（洛阳）测控有限公司 一种对行驶车辆体积测试的装置
CN102679889A (zh) * 2012-05-30 2012-09-19 北京恒华伟业科技股份有限公司 车辆尺寸的测量方法和装置
CN204421838U (zh) * 2015-01-20 2015-06-24 吉林大学 一种汽车基本参数测量装置
CN204421853U (zh) * 2015-03-06 2015-06-24 中国人民解放军军械工程学院 车轮定位参数测量装置
CN108445505A (zh) * 2018-03-29 2018-08-24 南京航空航天大学 线环境下基于激光雷达的特征显著性检测方法
CN109282755A (zh) * 2018-11-14 2019-01-29 吉林大学 一种车辆外廓尺寸自动测量装置及方法

Also Published As

Publication number Publication date
CN111366086B (zh) 2021-07-06

Similar Documents

Publication Publication Date Title
Kim et al. SLAM-driven robotic mapping and registration of 3D point clouds
CN105806344B (zh) 一种基于局部地图拼接的栅格地图创建方法
CN103257342B (zh) 三维激光传感器与二维激光传感器的联合标定方法
CN104062973A (zh) 一种基于图像标志物识别的移动机器人slam方法
US7321841B2 (en) Three-dimensional shape measuring method and measuring apparatus thereof
CN110455187B (zh) 一种基于三维视觉的盒体工件焊缝的检测方法
US8462208B2 (en) System and method for tridimensional cartography of a structural surface
EP3799790B1 (en) Method for tracking location of two-dimensional non-destructive inspection scanner on target object using scanned structural features
CN110646231B (zh) 一种扫地机器人测试方法和装置
Kim et al. Autonomous mobile robot localization and mapping for unknown construction environments
Kim et al. As-is geometric data collection and 3D visualization through the collaboration between UAV and UGV
CN116593589A (zh) 大体积混凝土结构三维超声波智能检测方法
CN114964213A (zh) 基于姿态感知和视觉扫描的建筑工程施工定位***及方法
CN114749848A (zh) 一种基于3d视觉引导的钢筋焊接自动化***
CN112975907B (zh) 一种对弧状焊缝、胶条的视觉检测方法
CN112508933B (zh) 一种基于复杂空间障碍物定位的柔性机械臂运动避障方法
CN111366086B (zh) 面向自动化装车的车厢位姿测量***及方法
Suzuki et al. SLAM using ICP and graph optimization considering physical properties of environment
CN116465335A (zh) 基于点云匹配的自动测厚方法和***
CN115205397A (zh) 一种基于计算机视觉和位姿估计的车辆时空信息识别方法
Bao et al. Robotic 3D plant perception and leaf probing with collision-free motion planning for automated indoor plant phenotyping
CN114918916A (zh) 基于智能制造的生产监测方法
CN117029721A (zh) 一种土木工程内部空间轮廓扫描测绘方法及装置
Skrzypczyński 2D and 3D world modelling using optical scanner data
US11961305B2 (en) Enhanced vehicle navigation using non-destructive inspection one-dimensional sensor arrays

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