基于下倾双目立体视觉的移动机器人前方坡度预测方法与流程

本发明涉及基于下倾双目立体视觉的移动机器人前方坡度预测方法，属于移动机器人控制领域。

背景技术：

室外移动机器人工作环境非常复杂，其中地面的起伏情况就多变，可能是平地，也可能是上坡或者下坡，那么移动机器人提前获知该信息非常重要，不然可能导致倾倒等意外情况。对于采集地面情况信息的摄像机，光轴可能与室外移动机器人的地盘平行，也可能下倾设置，这样可以将视野集中在地面，而不是天空及远处，但是也给计算带来一定难度。

技术实现要素：

针对上述问题，本发明提供基于下倾双目立体视觉的移动机器人前方坡度预测方法，为移动机器人检测前方地面的坡度信息。

本发明解决其技术问题所采用的技术方案是：

基于下倾双目立体视觉的移动机器人前方坡度预测方法，包括设置在所述的移动机器人前部的双目立体摄像机，焦距为f，基线宽度为b，高度为h，所述的双目立体摄像机的光轴与所述的移动机器人的底盘的夹角为β，还包括与所述的双目立体摄像机连接的处理器，所述的处理器设置前方坡度预测方法，包括以下步骤：

(1)所述的处理器获取所述的双目立体摄像机的图像对fl和fr，形成深度信息z=f(x,y)，x,y为像平面坐标，z为对应的深度；

(2)对于成像点a(x^a,y^a)，投影角度θ=arctan(y^a/f)，则对于平坦地面，成像点a对应的地面点a’的深度为z’，实际点a的深度为z^a=f(x^a,y^a)；

(3)角度δ=π/2-β-θ；深度z’=h*cosθ/cosδ；线段ao的长度l3=z^a/cosθ；实际点a到光心o所在铅垂线的垂直距离l2=l3*sinδ；

(4)如果实际深度z^a>z’，前方地面为下坡，实际点a到平坦地面的垂直距离l1=l3*cosδ-h，则坡度为α=arctan（l1/l2)；如果实际深度z^a<z’，地面为上坡，实际点a到平坦地面的垂直距离l1=h-l3*cosδ，则坡度为α=arctan(l1/l2)；如果实际深度z^a=z’，则地面为平面，角度为α=0。

本发明的有益效果主要表现在：1、可提前预测前方地面的坡度数值，为移动机器人运动控制提供环境信息。2、摄像机光轴下倾情况下，可精确计算前方地面的坡度数值。

附图说明

图1是移动机器人外形示意图；

图2是移动机器人前方坡度预测方法原理示意图1；

图3是移动机器人前方坡度预测方法原理示意图2。

具体实施方式

下面结合附图对本发明作进一步描述：

参照图1－3，基于下倾双目立体视觉的移动机器人前方坡度预测方法，包括设置在所述的移动机器人前部的双目立体摄像机，焦距为f，基线宽度为b，高度为h。

所述的双目立体摄像机的光轴与所述的移动机器人的底盘的夹角为β，这样视野主要集中在地面，有利于提取有用信息。还包括与所述的双目立体摄像机连接的处理器，所述的处理器设置前方坡度预测方法，包括以下步骤：

(1)所述的处理器获取所述的双目立体摄像机的图像对fl和fr，形成深度信息z=f(x,y)，x,y为像平面坐标，z为对应的深度；

所述的处理器是根据视差原理来计算深度信息，利用所述的双目立体摄像机的参数，可得z=f(x,y)=f*b/d，其中d是根据图像对fl和fr计算得到的位置(x,y)的视差。

(2)对于成像点a(x^a,y^a)，投影角度θ=arctan(y^a/f)，则对于平坦地面，成像点a对应的地面点a’的深度为z’，实际点a的深度为z^a=f(x^a,y^a)；

其中，深度z’和z^a分别是点a’和点a沿光轴到光心o的距离。

(3)角度δ=π/2-β-θ；深度z’=h*cosθ/cosδ；线段ao的长度l3=z^a/cosθ；实际点a到光心o所在铅垂线的垂直距离l2=l3*sinδ；

角度δ为直线ao与光心o所在铅垂线oo’的夹角，用于后续计算。首先，计算线段a’o长度，在三角形δoo’a’中，|a’o|=h/cosδ，所以深度z’=|a’o|*cosθ=h*cosθ/cosδ；然后，根据实际点a的深度z^a，采用三角形余弦公式，可计算线段ao的长度l3=z^a/cosθ；在三角形δoo"a中，可采用正弦公式，计算垂直距离l2=l3*sinδ。

(4)如果实际深度z^a>z’，前方地面为下坡，实际点a到平坦地面的垂直距离l1=l3*cosδ-h，则坡度为α=arctan（l1/l2)；如果实际深度z^a<z’，地面为上坡，实际点a到平坦地面的垂直距离l1=h-l3*cosδ，则坡度为α=arctan(l1/l2)；如果实际深度z^a=z’，则地面为平面，角度为α=0。

如图2，实际深度z^a>z’，则前方地面为下坡，首先计算实际点a到平坦地面的垂直距离l1=l3*cosδ-h，再根据三角形的正切函数关系得到α=arctan（l1/l2)；

如图3，实际深度z^a<z’，则地面为上坡，首先计算实际点a到平坦地面的垂直距离l1=h-l3*cosδ，再根据三角形的正切函数关系得到α=arctan（l1/l2)。

综上所述，通过上述方法可计算基于下倾双目立体视觉的移动机器人前方地面坡度，为移动机器人的导航提供必要信息。

技术特征：

技术总结
公开一种基于下倾双目立体视觉的移动机器人前方坡度预测方法，包括下倾设置在移动机器人前部的双目立体摄像机，还包括与双目立体摄像机连接的处理器，处理器设置前方坡度预测方法，包括步骤：1、获取双目立体摄像机的图像对fL和fR，形成深度信息z=f(x,y)；2、对于成像点a(xA,yA)，计算投影角度θ，则对于平坦地面，点a对应的地面点A’的深度为z’，实际点A的深度为zA=f(xA,yA)；3、计算角度δ，深度z’，线段AO的长度l3，点A到光心O所在铅垂线的垂直距离l2；4、如果深度zA>z’，前方地面为下坡，计算α；如果深度zAA=z’，则地面为平面。

技术研发人员：刘瑜
受保护的技术使用者：杭州晶一智能科技有限公司
技术研发日：2019.03.20
技术公布日：2019.07.09