[发明专利]一种非平行光轴双目测距方法

权利要求书

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°。