[发明专利]利用红外吊灯和摄像头测量机器人位置的方法与系统

权利要求书

1.一种利用红外吊灯和摄像头测量机器人位置的方法，其特征在于，包括：

步骤一、将红外吊灯吊装在屋顶上，通过所述红外吊灯对地面进行红外照明，同时在屋顶上散布红外光斑；

步骤二、将红外摄像头设置于机器人的头部、用以捕捉所述红外吊灯的图像以及屋顶散布的所述红外光斑的图像，同时获取所述红外吊灯到所述红外摄像头所处平面的垂直距离作为高度信息；

步骤三、将所述机器人置于所述红外吊灯的正下方、并指定所述机器人的初始朝向和位置，利用所述机器人的头部所设置的所述红外摄像头捕捉所述红外吊灯及所述红外摄像头视野内的所述红外光斑的图像；

步骤四、所述机器人先移动一小段距离、并确保所述红外吊灯一直处于所述红外摄像头的视野中，结合所述高度信息、基于所述机器人的室内平面移动(即二维矢量)约束及所述红外摄像头的仿射变换原理进行计算获得所述机器人的朝向和位置变化，同时计算出所述红外摄像头视野中捕获的所述红外光斑的高度信息和坐标值；

步骤五、在步骤四的基础上，所述机器人开始全面自主移动，直到所述红外摄像头视野内的所述红外吊灯离开所述红外摄像头视野，在所述机器人移动过程期间所述红外摄像头不断捕获新的所述红外光斑图像，同时利用已经计算出的被捕获的所述红外光斑的高度信息和位置坐标，计算所述机器人朝向和位置的变化，同时计算新捕获的所述红外光斑的高度信息，用于在所述机器人的后续移动过程中持续计算出所述机器人的位置和朝向。

2.根据权利要求1所述的利用红外吊灯和摄像头测量机器人位置的方法，其特征在于，在所述步骤四中，获得所述机器人的朝向和位置变化的具体算法如下：首先结合所述红外摄像头垂直方向上安装姿态，根据仿射变换几何原理，有

式中I_P(U_P，V_P)为所述机器人上方某一点P在所述红外摄像头视野中的像素坐标，其中，u₀为成像中心横坐标、v₀为成像中心纵坐标、k为放大系数，h_P表示点P的高度信息即点P到所述红外摄像头平面的垂直距离，(^Cx_P,^Cy_P)表示所述红外摄像头的中心到点P的射线在所述红外摄像头平面中的投影矢量，所述投影矢量由以下公式计算而出

其中(x,y)表示当前时刻所述机器人及其所述红外摄像头的位置坐标，θ表示所述机器人及所述红外摄像头的朝向；

在初始时刻，所述机器人位置被初始为(x₀，y₀)，朝向被初始设置为θ₀，吊灯底部到所述红外摄像头平面的距离为h_D；

在所述红外摄像头的视野中，在视野范围内获得的所述红外吊灯图像的中心像素坐标为(U_D，V_D)，所捕获的所述红外光斑的中心像素坐标为G₁(U_g1,V_g1)；

在所述机器人移动后，在所述红外摄像头的视野中所述红外吊灯中心像素坐标变化为所述红外光斑的中心像素坐标变化为则可通过以下平面移动约束及所述红外摄像头仿射变换几何关系来计算所述机器人移动后的位置(x₁，y₁)和新朝向θ₁；

结合上述计算公式和变量定义,可计算出所述机器人的平面位移矢量为(x₁-x₀,y₁-y₀)；

变形后可得：

在此基础上，可计算出机器人移动后的朝向为：

其中

继而可以计算出所述机器人移动后的位置为(x₁，y₁)；

继而可计算出所述红外光斑Gi到所述红外摄像头平面的垂直高度距离为：