CN113848964A

CN113848964A - 一种非平行光轴双目测距方法

Info

Publication number: CN113848964A
Application number: CN202111051340.3A
Authority: CN
Inventors: 楼杰; 周俊阳; 董辉; 郭方洪; 吴祥; 张文安
Original assignee: Jinhua Zhejiang University Of Technology Innovation Joint Research Institute
Current assignee: Jinhua Zhejiang University Of Technology Innovation Joint Research Institute
Priority date: 2021-09-08
Filing date: 2021-09-08
Publication date: 2021-12-28

Abstract

本发明公开了一种非平行光轴双目测距方法，包括：S1、搭建双目系统模型，双目系统模型包括双目测距模块和IMU模块，双目测距模块包括左目相机和右目相机，IMU模块用于监测倾斜角度；S2、获取当前目标第一图像、目标第二图像和倾斜角度；S3、分别计算出目标图像中的目标位置；S4、获得初始测距结果，并判断当前倾斜角度是否低于预设角度阈值，若是，则根据初始测距结果驱动移动机器人向目标行进，否则，对初始测量结果进行矫正，以直立状态下目标到移动机器人的距离作为矫正测距结果，并根据矫正测距结果驱动移动机器人向目标行进。该方法可实时采集图像进行测距并监测机器人的倾斜状态进行测距结果矫正，大大提高测距精度。

Description

一种非平行光轴双目测距方法

技术领域

本发明属于机器视觉技术领域，具体涉及一种非平行光轴双目测距方法。

背景技术

双目测距原理根据左右相机采集的图像信息计算出对应的实际物体与相机的距离。根据左右相机的位置与姿态，结合相机成像原理推导出相应的测量目标物体到相机的距离。

但是传统的双目测距都是基于两台摄像机的像平面位于同一平面上、光轴互相平行以及焦距相同的理想条件下进行测距。而在实际情况下，采用平行光轴形成的视场角比较小，视野受限，且两台相机由于装配误差等原因难以保持在同一平面，导致光轴互不平行。此外，两轮机器人在行进时机身的晃动幅度会比较大，导致距离无法测量或测量结果出现跳变，误差较大，无法作为有效的测量结果。因此，使用传统的双目测距算法难以保证测距精度，且不利于扩大视野。本申请提出一种基于非平行光轴的双目测距方法。

发明内容

本发明的目的在于针对上述问题，提出一种非平行光轴双目测距方法，可实时采集图像计算目标到移动机器人的距离，并通过监测移动机器人的倾斜状态对超出倾斜范围的测量结果进行校正，解决了移动机器人行进时由于机身晃动导致测距不准的问题，能够显著提高测距精度。

为实现上述目的，本发明所采取的技术方案为：

本发明提出的一种非平行光轴双目测距方法，应用于移动机器人，包括：

S1、在移动机器人上搭建双目系统模型，双目系统模型包括双目测距模块和IMU模块，双目测距模块包括左目相机和右目相机，IMU模块用于监测移动机器人相对地面的倾斜角度；

S2、获取当前左目相机采集的目标第一图像、右目相机采集的目标第二图像和IMU模块监测的倾斜角度；

S3、分别计算出目标第一图像和目标第二图像中的目标位置；

S4、获得目标到移动机器人的初始测距结果，并判断当前倾斜角度是否低于预设角度阈值，若是，则根据初始测距结果驱动移动机器人向目标行进，否则，对初始测量结果进行矫正，以直立状态下目标到移动机器人的距离作为矫正测距结果，并根据矫正测距结果驱动移动机器人向目标行进。

优选地，步骤S4中，初始测距结果计算如下：

1)计算FK，公式如下：

∠FKJ＝180°-∠JFK-∠FJD

2)计算MG，公式如下：

∠GML＝180°-∠GLE-∠LGM

3)计算初始测距结果CN，公式如下：

PD＝EQ＝FDsinθ

PQ＝PD+DE+EQ

KM＝PQ-FK-MG

其中，C为目标位置，D为左目相机的光心，E为右目相机的光心，F为目标第一图像的中心，G为目标第二图像的中心，过G点作DE垂线交于Q，过F点作DE垂线交于P，J为目标位置C在目标第一图像中的点，L为目标位置C在目标第二图像中的点，K为CD与FG的交点，M为CE与FG的交点，过C点作DE的垂线分别交KM于O、交DE于N，θ为左目相机的光轴或右目相机的光轴与DE的垂线的夹角。

优选地，夹角θ为18°。

优选地，步骤S4中，矫正测距结果计算如下：

∠NRY＝arctan(NY/RY)

VU＝RZ＝RScos∠SRZ

SZ＝SRsin∠SRZ

SU＝SZ+ZU＝SZ+RV

当移动机器人为前倾状态时：

∠SRZ＝∠NRY-∠SRN

UT＝UV-TV

CT＝CU+UT

当移动机器人为后倾状态时：

∠SRZ＝∠NRY+∠SRN

TU＝TV-UV

CT＝CU-TU

矫正测距结果满足

其中，R为移动机器人的驱动轮转动中心，V为移动机器人的驱动轮与地面的接触点，C为目标位置，移动机器人直立状态下过C点作相机光心连线的垂线交于N，移动机器人前倾状态下或后倾状态下过C点作相机光心连线的垂线交于S，过N点作CV垂线交于T，过S点作CV垂线交于U，过R点作NT垂线交于Y，过R点作SU垂线交于Z，∠SRN为移动机器人相对地面的倾斜角度。

优选地，预设角度阈值为3°。

与现有技术相比，本发明的有益效果为：该方法可实时采集图像计算目标到移动机器人的距离并监测移动机器人的倾斜状态，根据不同的倾斜状态控制移动机器人运动，且当超过预设角度阈值时对初始测距结果进行矫正，解决了移动机器人行进时由于机身晃动导致测距不准的问题，能够显著提高测距精度。

附图说明

图1为本发明的非平行光轴双目测距方法流程图；

图2为本发明的移动机器人测距原理图；

图3为本发明的移动机器人直立状态工作示意图；

图4为本发明的移动机器人前倾状态工作示意图；

图5为本发明的移动机器人直立状态和前倾状态组合示意图；

图6为本发明的移动机器人前倾状态下矫正模型测距原理图；

图7为本发明的移动机器人后仰状态工作示意图；

图8为本发明的移动机器人直立状态和后仰状态组合示意图；

图9为本发明的移动机器人后仰状态下矫正模型测距原理图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

需要说明的是，除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是在于限制本申请。

如图1-9所示，一种非平行光轴双目测距方法，应用于移动机器人，包括：

S1、在移动机器人上搭建双目系统模型，双目系统模型包括双目测距模块和IMU模块，双目测距模块包括左目相机和右目相机，IMU模块用于监测移动机器人相对地面的倾斜角度。

其中，机器人类型可根据实际需求进行调整，本实施例中移动机器人采用消防机器人，且为两轮自平衡机器人，目标为火源，双目测距模块为两个热成像相机，如均采用Xmodule T3-317-68非制冷测温热像模组相机或Xmodule S0-212-68非制冷测温热像模组相机等，均用于采集火源图像，IMU模块用于监测移动机器人相对地面的倾斜角度。在移动机器人直立状态下，两个热成像相机光轴不平行，如在水平面上同时向外倾斜，可增加热成像相机的视野，相机光轴的夹角可根据实际需求进行调节，既可以增加视野，也保证了左右热成像相机仍能够看到正前方，中间无视野盲区，进一步扩大安全监测范围。

在检测到火源时，如火灾现场，获取火源的图像以及移动机器人相对地面的倾斜角度，其中，火源在目标第一图像和目标第二图像中的位置可以采用现有技术中直接获取目标像素点坐标。热成像相机在确定火源后对火源进行测距获得初始测距结果，根据火源和两个热成像相机的光心构成三角形，利用相似三角形的原理得出火源到移动机器人的距离即为初始测距结果。由于移动机器人采用两轮自平衡机构，运动时机身晃动幅度较大，会导致测距结果发生跳变。若移动机器人的当前倾斜角度不低于预设角度阈值，则判断为初始测距结果出现跳变，无法作为有效的测量结果，进行矫正，以矫正测距结果作为有效的测量结果，若低于预设角度阈值，则判断为初始测距结果为有效的测量结果。进一步地，为避免单次测量结果前后出现跳变，提高测距结果的准确性、有效性，当在预设角度阈值范围内时，可根据实际情况取n组数据求平均值作为初始测距结果，n可根据实际需求进行取值，如本实施例中取n＝10。

在一实施例中，步骤S4中，初始测距结果计算如下：

1)计算FK，公式如下：

∠FKJ＝180°-∠JFK-∠FJD

2)计算MG，公式如下：

∠GML＝180°-∠GLE-∠LGM

3)计算初始测距结果CN，公式如下：

PD＝EQ＝FDsinθ

PQ＝PD+DE+EQ

KM＝PQ-FK-MG

其中，如图2所示，DE为所述左目相机和右目相机的间距，AB为所述目标第一图像的水平投影，HI为所述目标第二图像的水平投影，DF为所述左目相机的焦距，GE为所述右目相机的焦距。DE、AB、HI、DF、GE、θ、J、L均已知，根据成像的原理，目标位置C、左目相机的光心D和右目相机的光心E形成三角形，因三角形CDE和三角形CKM相似，利用相似三角形原理求出CN，CN即火源到移动机器人的距离。

在一实施例中，夹角θ为18°。为增加热成像相机的视野，本实施例中夹角θ为18°，保证了左右热成像相机仍能够看到正前方，中间无视野盲区，进一步扩大安全监测范围。

在一实施例中，步骤S4中，矫正测距结果计算如下：

∠NRY＝arctan(NY/RY)

VU＝RZ＝RScos∠SRZ

SZ＝SRsin∠SRZ

SU＝SZ+ZU＝SZ+RV

当移动机器人为前倾状态时：

∠SRZ＝∠NRY-∠SRN

UT＝UV-TV

CT＝CU+UT

当移动机器人为后倾状态时：

∠SRZ＝∠NRY+∠SRN

TU＝TV-UV

CT＝CU-TU

矫正测距结果满足

其中，移动机器人处于前倾状态还是后仰状态，可以通过IMU模块反馈的倾斜角度来判断，如当倾斜角度为负值时，判断为后仰状态，当倾斜角度为正值时，判断为前倾状态。如图4-6所示，为移动机器人前倾状态，图6为图5去除移动机器人轮廓后的图。如图7-9所示，为移动机器人后仰状态，图9为图8去除移动机器人轮廓后的图。CS即为初始测距结果，可由初始测距结果CS求出矫正测距结果CN，移动机器人行进时会产生晃动使测量结果出现跳变，对误差较大的测量结果做矫正处理，即将跳变的测量结果矫正到直立时的测量结果，可以减少测量误差。具体地，RV垂直于CV，RV为移动机器人的驱动轮半径，NT垂直于CV，其中RV、RN、NT已知，RS＝RN，∠SRN为移动机器人相对地面的倾斜角度，可通过IMU模块获得。过R点作NT垂线交于Y，过R点作SU垂线交于Z，已知NR、TV，RY＝TV，获得∠NRY＝arctan(NY/RY)，即可代入公式获得对应倾斜状态下的矫正测距结果CN。以矫正测距结果CN替代初始测距结果CS可获得更加准确的测距结果。

在一实施例中，预设角度阈值为3°。

本实施例中，当倾斜角度小于3°时，判断为初始测距结果有效，根据初始测距结果驱动移动机器人向目标行进。当倾斜角度在3°以上时，对初始测距结果进行矫正，根据返回的矫正测距结果驱动移动机器人向目标行进，如当移动机器人行进至灭火范围时，可控制携带的消防设备工作，如喷水或灭火剂进行灭火等，设定灭火范围是4～5m。有助于保证测距准确性的同时提高工作效率。需要说明的是，预设角度阈值的取值可根据实际需求进行调整。

该方法可实时采集图像计算目标到移动机器人的距离并监测移动机器人的倾斜状态，根据不同的倾斜状态控制移动机器人运动，且当超过预设角度阈值时对初始测距结果进行矫正，解决了移动机器人行进时由于机身晃动导致测距不准的问题，能够显著提高测距的准确性。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请描述较为具体和详细的实施例，但并不能因此而理解为对申请专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims

1.一种非平行光轴双目测距方法，应用于移动机器人，其特征在于：所述非平行光轴双目测距方法，包括：

S1、在所述移动机器人上搭建双目系统模型，所述双目系统模型包括双目测距模块和IMU模块，所述双目测距模块包括左目相机和右目相机，所述IMU模块用于监测所述移动机器人相对地面的倾斜角度；

S2、获取当前所述左目相机采集的目标第一图像、所述右目相机采集的目标第二图像和所述IMU模块监测的倾斜角度；

S3、分别计算出所述目标第一图像和所述目标第二图像中的目标位置；

S4、获得所述目标到所述移动机器人的初始测距结果，并判断当前所述倾斜角度是否低于预设角度阈值，若是，则根据所述初始测距结果驱动所述移动机器人向所述目标行进，否则，对所述初始测量结果进行矫正，以直立状态下所述目标到所述移动机器人的距离作为矫正测距结果，并根据所述矫正测距结果驱动所述移动机器人向所述目标行进。

2.如权利要求1所述的非平行光轴双目测距方法，其特征在于：步骤S4中，所述初始测距结果计算如下：

1)计算FK，公式如下：

∠FKJ＝180°-∠JFK-∠FJD

2)计算MG，公式如下：

∠GML＝180°-∠GLE-∠LGM

3)计算所述初始测距结果CN，公式如下：

PD＝EQ＝FDsinθ

PQ＝PD+DE+EQ

KM＝PQ-FK-MG

其中，C为目标位置，D为所述左目相机的光心，E为所述右目相机的光心，F为所述目标第一图像的中心，G为所述目标第二图像的中心，过G点作DE垂线交于Q，过F点作DE垂线交于P，J为所述目标位置C在所述目标第一图像中的点，L为所述目标位置C在所述目标第二图像中的点，K为CD与FG的交点，M为CE与FG的交点，过C点作DE的垂线分别交KM于O、交DE于N，θ为所述左目相机的光轴或所述右目相机的光轴与DE的垂线的夹角。

3.如权利要求2所述的非平行光轴双目测距方法，其特征在于：所述夹角θ为18°。

4.如权利要求1所述的非平行光轴双目测距方法，其特征在于：步骤S4中，所述矫正测距结果计算如下：

∠NRY＝arctan(NY/RY)

VU＝RZ＝RScos∠SRZ

SZ＝SRsin∠SRZ

SU＝SZ+ZU＝SZ+RV

当所述移动机器人为前倾状态时：

∠SRZ＝∠NRY-∠SRN

UT＝UV-TV

CT＝CU+UT

当所述移动机器人为后倾状态时：

∠SRZ＝∠NRY+∠SRN

TU＝TV-UV

CT＝CU-TU

所述矫正测距结果满足

其中，R为所述移动机器人的驱动轮转动中心，V为所述移动机器人的驱动轮与地面的接触点，C为目标位置，所述移动机器人直立状态下过C点作相机光心连线的垂线交于N，所述移动机器人前倾状态下或后倾状态下过C点作相机光心连线的垂线交于S，过N点作CV垂线交于T，过S点作CV垂线交于U，过R点作NT垂线交于Y，过R点作SU垂线交于Z，∠SRN为所述移动机器人相对地面的倾斜角度。

5.如权利要求1所述的非平行光轴双目测距方法，其特征在于：所述预设角度阈值为3°。