无人物流车的单目视觉测距测速方法

1.本发明设涉及一种无人物流车的单目视觉测距测速方法，属于无人汽车领域。


背景技术：

2.随着自动驾驶技术的不断发展，越来越多的行业正受到其影响，并且实现产业化落地。无人配送物流车是自动驾驶技术在限定场景下的成功应用之一。无人配送物流车能够自动将货物投递到收货人手中，节约人工成本，并且能够全天候工作，提高配送效率。如果在无人配送过程中能够实时规划配送路径，则一方面可以提高配送效率，另一方面可以使得配送服务更加灵活；如果收发货交接过程中能够实现人车交互，则会大大提高用户体验，使得配送服务更加便利、更加人性化。在进行无人出的路径规划时，无人车的所需要配备测距测试的功能。但现有的测距测试大部分采用的是毫米波雷达的方式，这种测距测试成本昂贵，而且横向距离探测准确性相对较差。


技术实现要素：

3.本发明的目的在于，提供一种无人物流车的单目视觉测距测速方法。本发明可以通过单视觉进行测距测速，具有距离和速度测量准确的优点。
4.本发明的技术方案：无人物流车的单目视觉测距测速方法，包括如下步骤：
5.s1、将世界坐标系依次按序转换成相机坐标系、图像坐标系与像素坐标系，获得世界坐标系与像素坐标系的转换矩阵；
6.s2、基于目标检测算法获取目标物体在图像坐标系中的检测框坐标值，以检测框底边中点作为测距像素点，代入转换矩阵中求解出目标物体在世界坐标系中的横向和纵向距离；
7.s3、基于多目标跟踪算法获取目标物体在图像连续帧中的匹配关系，根据预设间隔时间值对间隔时间前后两帧中匹配上的目标物体的位置进行计算得到目标物体速度。
8.上述的无人物流车的单目视觉测距测速方法，步骤s1中，所述世界坐标系与像素坐标系的转换矩阵如下：
[0009][0010]
其中：zc为对应物体深度信息；[u,v]表示图像像素坐标；d
x
，dy分别表示一个像素在图像坐标系x、y两个坐标轴方向的物理尺寸；(u0,v0)表示相机光心在图像坐标系中的位置；f表示相机的焦距；[x,y,z]为世界坐标；(u,v)表示以像素为单位的像素坐标系的坐标；r矩阵是大小为3行3列的旋转矩阵，t矩阵为3行1列的平移矩阵。
[0011]
前述的无人物流车的单目视觉测距测速方法，步骤s1中，世界坐标系和相机坐标
系的转换矩阵如下：
[0012][0013]
式中(x,y,z)表示世界坐标系，(xc,yc,zc)表示相机坐标系，r矩阵是大小为3行3列的旋转矩阵，t矩阵为3行1列的平移矩阵。
[0014]
前述的无人物流车的单目视觉测距测速方法，步骤s1中，相机坐标系和图像坐标系的转换是将三维场景o-xcyczc映射成一个二维平面o
1-xy，其中点m为相机坐标系中的的点，其坐标为(xc,yc,zc)，映射之后对应的点为m，其坐标为(x,y)，相机坐标系和图像坐标系之间转换通过透视投影实现，其中相机坐标系和图像坐标系的转换矩阵如下：
[0015][0016]
式中：即为zcx＝fxc，zcy＝fyc，f为相机的焦距。
[0017]
前述的无人物流车的单目视觉测距测速方法，步骤s1中，相机坐标系和图像坐标系的转换是以(u,v)表示以像素为单位的像素坐标系的坐标，(x,y)表示以mm为单位的图像坐标系的坐标；
[0018]
假设每一个像素在x轴与y轴方向上的物理尺寸为dx和dy，则图像任意一个像素在两个坐标系下的坐标有如下所示关系式：
[0019][0020]
其中，(u0,v0)＝xy(0,0)；
[0021]
根据图像任意一个像素在两个坐标系下的坐标的关系式得到相机坐标系和图像坐标系的转换矩阵：
[0022][0023]
前述的无人物流车的单目视觉测距测速方法，步骤s2中，求解出目标物体在世界坐标系中的横向和纵向距离的过程具体是:
[0024]
将世界坐标系与像素坐标系的转换矩阵进行合并：
[0025][0026]
将物体深度zc，旋转矩阵r和平移矩阵t进行展开，分别得到：
[0027]
zc＝r7x+r8y+r9z+t3；
[0028][0029][0030]
从而得到最后的转换矩阵方程：
[0031][0032]
令z＝0，则使得未知量个数只有2个，即(x,y))，将转换矩阵方程展开：
[0033][0034][0035]
令：
[0036]
[0037]
对公式进行替换后则有：
[0038][0039]
计算后可求出：
[0040][0041]
目标物体的横向距离x、纵向距离y由图像坐标系中的检测框的底边中点坐标(u,v)计算得到。
[0042]
前述的无人物流车的单目视觉测距测速方法，步骤s3中，所述目标物体的速度及其方向角计算公式如公式下：
[0043][0044][0045]
式中：(x,y)为前一帧的目标物体在图像坐标系的坐标，(x',y')为后一帧的目标物体在图像坐标系的坐标。
[0046]
与现有技术比较，本发明通过将世界坐标系依次按序转换成相机坐标系、图像坐标系与像素坐标系，获得世界坐标系与像素坐标系的转换矩阵；再基于目标检测算法获取目标物体在图像坐标系中的检测框坐标值，以检测框底边中点作为测距像素点，代入转换矩阵中求解出目标物体在世界坐标系中的横向和纵向距离；最后基于多目标跟踪算法获取目标物体在图像连续帧中的匹配关系，根据预设间隔时间值对间隔时间前后两帧中匹配上的目标物体的位置进行计算得到物体速度。由此，本发明有效的进行目标物体的横向距离以及纵向距离的测量，而且还能准确的测量目标物体的速度以及方向角，具有测量过程方便，测量结果准确的优点。
附图说明
[0047]
图1是本发明的流程示意图；
[0048]
图2是本发明世界坐标系转换成相机坐标系的示意图；
[0049]
图3是本发明相机坐标系转化为图像坐标系的示意图；
[0050]
图4是本发明图像坐标系转化为像素坐标系的示意图；
[0051]
图5是目标物体速度及其方向角计算示意图；
[0052]
图6是目标物体速度及其方向正负定义示意图；
[0053]
图7是本发明所实现的测距示意图；
[0054]
图8是本发明所实现的测距测速示意图。
具体实施方式
[0055]
下面结合附图和实施例对本发明作进一步的说明，但并不作为对本发明限制的依据。
[0056]
实施例：无人物流车的单目视觉测距测速方法，包括如下步骤：
[0057]
s1、将世界坐标系依次按序转换成相机坐标系、图像坐标系与像素坐标系，获得世界坐标系与像素坐标系的转换矩阵；
[0058]
如图1所示，用(x,y,z)表示世界坐标系，(xc,yc,zc)表示相机坐标系，世界坐标系与相机坐标系转换过程是由一个旋转矩阵变换r以及一个位移矩阵变换t组成，因此世界坐标系和相机坐标系的转换矩阵如下：
[0059][0060]
式中(x,y,z)表示世界坐标系，(xc,yc,zc)表示相机坐标系，r矩阵是大小为3行3列的旋转矩阵，t矩阵为3行1列的平移矩阵。
[0061]
相机坐标系与图像坐标系之间的关系是通过相机成像模型来建立的，通常采用的是针孔相机模型。如图3所示：相机坐标系和图像坐标系的转换是将三维场景o-xcyczc映射成一个二维平面o
1-xy，其中点m为相机坐标系中的的点，其坐标为(xc,yc,zc)，映射之后对应的点为m，其坐标为(x,y)，相机坐标系和图像坐标系之间转换通过透视投影实现，其中相机坐标系和图像坐标系的转换矩阵如下：
[0062][0063]
式中：根据三角形相似原理，即为zcx＝fxc，zcy＝fyc，f为相机的焦距。
[0064]
图像坐标系已经和像素坐标系重合，但是图像坐标系的原点位于相机的光心处，并且其坐标轴单位是物理单位mm，而像素坐标系坐标单位是像素。像素只能表示位于图像中的位置，即列数与行数，并没有使用物理单位表示该像素在图像中的位置，所以需要再建立以物理单位(mm)表示的像素坐标系，如图4所示。因此在途4中，相机坐标系和图像坐标系的转换是以(u,v)表示以像素为单位的像素坐标系的坐标，(x,y)表示以mm为单位的图像坐标系的坐标；
[0065]
假设每一个像素在x轴与y轴方向上的物理尺寸为dx和dy，则图像任意一个像素在
两个坐标系下的坐标有如下所示关系式：
[0066][0067]
其中，(u0,v0)＝xy(0,0)；
[0068]
根据图像任意一个像素在两个坐标系下的坐标的关系式得到相机坐标系和图像坐标系的转换矩阵：
[0069][0070]
根据上述各坐标系之间的转换关系，所述世界坐标系与像素坐标系的转换矩阵如下：
[0071][0072]
其中：zc为对应物体深度信息；[u,v]表示图像像素坐标；d
x
，dy分别表示一个像素在图像坐标系x、y两个坐标轴方向的物理尺寸；(u0,v0)表示相机光心在图像坐标系中的位置；f表示相机的焦距；[x,y,z]为世界坐标；(u,v)表示以像素为单位的像素坐标系的坐标；r矩阵是大小为3行3列的旋转矩阵，t矩阵为3行1列的平移矩阵。
[0073]
以上转换过程中涉及到的一些参数，皆可以通过对相机进行标定获得，其内部参数包括相机的焦距、传感器每个单元的尺寸以及光心的位置；外部参数则包括相机在三个维度上的旋转角度和平移量。
[0074]
s2、基于目标检测算法(例如yolo算法，ssd算法等，上述算法属于本领域技术人员所常规的常规技术手段，为此本发明不在具体赘述)获取目标物体在图像坐标系中的检测框坐标值，以检测框底边中点作为测距像素点，代入转换矩阵中求解出目标物体在世界坐标系中的横向和纵向距离，具体过程是:
[0075]
将世界坐标系与像素坐标系的转换矩阵进行合并：
[0076][0077]
将物体深度zc，旋转矩阵r和平移矩阵t进行展开，分别得到：
[0078]
zc＝r7x+r8y+r9z+t3；
[0079][0080][0081]
从而得到最后的转换矩阵方程：
[0082][0083]
从上式可知，在相机标定完成后，转换矩阵中未知量只有(x,y,z,u,v)，即可以在已知物体空间坐标(x,y,z)的情况下推导出位于图像中的(u,v)。
[0084]
但是反过来，从图像中的像素点坐标(u,v)推导空间坐标(x,y,z)，由于存在三个未知量，而参与计算的方程只有两个，存在无穷解。则必须作出先验性假设，因为只考虑与地面接触点的空间位置，所以令z＝0，则使得未知量个数只有2个，即(x,y))，将转换矩阵方程展开：
[0085][0086][0087]
令：
[0088][0089]
对公式进行替换后则有：
[0090][0091]
计算后可求出：
[0092][0093]
由于a、b、c、d、e、f中涉及到的参数皆已通过相机标定工作获得，所以目标物体的横向距离x、纵向距离y由图像坐标系中的检测框的底边中点坐标(u,v)计算得到。
[0094]
s3、基于多目标跟踪算法(例如deepsort算法，上述算法属于本领域技术人员所掌握的常规技术收单，为此本发明不在具体赘述)获取目标物体在图像连续帧中的匹配关系，根据预设间隔时间值对间隔时间前后两帧中匹配上的目标物体的位置进行计算得到目标物体速度。
[0095]
即本发明考虑到无人物流车中自动驾驶任务需要获取各个目标物体的速度情况用于后续决策规划任务，可以结合多目标跟踪算法(例如deepsort算法)，对图像流中连续帧里的各个目标物体进行匹配以实现跟踪，从而可以记录这些连续帧中目标物体的位置轨迹信息。
[0096]
通过设置固定的间隔时间(例如，低速情况下可以设置为t＝1s)，对于其中一个目标物体来说，用当前帧得到的位置和t时刻前的帧得到的位置进行计算即可得到该时间段内的平均速度值，同时其方向角也可以确定，因此如图5所示，所述目标物体的速度及其方向角计算公式如公式下：
[0097][0098][0099]
式中：(x,y)为前一帧的目标物体在图像坐标系的坐标，(x',y')为后一帧的目标物体在图像坐标系的坐标。
[0100]
如图6所示，为了更好地表示目标物体的轨迹方向，可以将目标物体的速度及其方向的正负情况规定如下：
[0101]
目标物体远离自车(无人物流车)：
[0102]
速度v设定为正值；
[0103]
对应的速度方向θ根据向左、向右分别设定为负值、正值；
[0104]
2.目标物体靠近自车(无人物流车)：
[0105]
速度v设定为负值；
[0106]
对应的速度方向θ根据向左、向右分别设定为负值、正值。
[0107]
经过实际试验，本发明实施例所实现的测距功能如图7所示，测速和测距同时进行的功能如图8所示。图7和图8中目标物体只涉及到行人，其中s表示物体的速度大小，d表示速度的方向，第二行的数字1代表该物体的id(由目标跟踪算法输出)，person表示物体的类别，0.90表示该物体为person类别的置信度，l表示物体检测框左下角、右下角的两个点所代表的横向距离范围，v表示物体检测框底边中点所代表的纵向距离。需要说明的是，检测到的物体类别种类数量可以通过目标检测算法制作数据集训练进行拓展，本发明所保护的是如何结合目标检测、跟踪算法完成对目标物体的测距、测速功能。
[0108]
综上所述，本发明通过将世界坐标系依次按序转换成相机坐标系、图像坐标系与像素坐标系，获得世界坐标系与像素坐标系的转换矩阵；再基于目标检测算法获取目标物体在图像坐标系中的检测框坐标值，以检测框底边中点作为测距像素点，代入转换矩阵中求解出目标物体在世界坐标系中的横向和纵向距离；最后基于多目标跟踪算法获取目标物体在图像连续帧中的匹配关系，根据预设间隔时间值对间隔时间前后两帧中匹配上的目标物体的位置进行计算得到物体速度。由此，本发明有效的进行目标物体的横向距离以及纵向距离的测量，而且还能准确的测量目标物体的速度以及方向角，具有测量过程方便，测量结果准确的优点。