VREP中的二维激光雷达

　　目前，轮式机器人的研究中已经大量使用激光雷达辅助机器人的避障导航，考虑到使用成本，一般二维激光雷达使用较多，如下图。由于只能扫描一个平面，如果想用二维激光雷达获取环境三维点云，则需要通过移动机器人或加装机械结构提供第三个维度的支持。

　　激光雷达扫描时可以想象成将超声波传感器发出的声波替换为激光并高速回转扫描，如此就能大概构建出附近的物体轮廓，这个过程非常像潜艇上使用声纳探测周围物体。当然，由于激光雷达使用激光而不是声波，它的探测过程不仅极短，而且能弥补声波广角发散的缺点（激光不易发散，锥度角很小）。激光雷达工作时会先在当前位置发出激光并接收反射光束，解析得到距离信息，而后激光发射器会转过一个角度分辨率对应的角度再次重复这个过程。限于物理及机械方面的限制，激光雷达通常会有一部分“盲区”。使用激光雷达返回的数据通常可以描绘出一幅极坐标图，极点位于雷达扫描中心，0-360°整周圆由扫描区域及盲区组成。在扫描区域中激光雷达在每个角度分辨率对应位置解析出的距离值会被依次连接起来，这样，通过极坐标表示就能非常直观地看到周围物体的轮廓，激光雷达扫描范围示意图可以参见下图。

　　激光雷达通常有四个性能衡量指标：测距分辨率、扫描频率（有时也用扫描周期）、角度分辨率及可视范围。测距分辨率衡量在一个给定的距离下测距的精确程度，通常与距离真实值相差在5-20mm；扫描频率衡量激光雷达完成一次完整扫描的快慢，通常在10Hz及以上；角度分辨率直接决定激光雷达一次完整扫描能返回多少个样本点；可视范围指激光雷达完整扫描的广角，可视范围之外即为盲区。

• 使用视觉传感器模拟激光雷达

　　为了模拟二维激光雷达，将视觉传感器设为透视模型并只获取深度信息。near clipping plane和far clipping plane设置为激光雷达的最小/最大扫描距离；视场角Persp. angle设为雷达可视范围（角度）；将Y轴分辨力率设为1，X轴分辨率可根据需求设置（分辨率越大结果越精确，分辨率过低可能会造成某些尺寸小的物体无法被探测到）。

　　视觉传感器filter设置如下，其中加入Extract coordinates from work image用来提取深度图像中坐标信息，这一环节返回的数据可以通过函数simReadVisionSensor来获取。参数设置界面上的Point count along X/Y设为X/Y轴分辨率，即调用一次相关API函数可以读取包含256个像素点的坐标信息的数据：

　　为了显示激光雷达“看到”的环境，可以通过Graph来记录用户自定义数据流：按照下图所示在数据类型栏中选择user-defined，将自定义数据名分别修改为"x"和"y"用来记录障碍物相对于激光雷达的位置，并通过函数simSetGraphUserData(graphHandle, dataStreamName, data)来设置自定义数据的值。

　　Graph：

if (sim_call_type==sim_childscriptcall_initialization) then

    -- Put some initialization code here
    graphHandle = simGetObjectAssociatedWithScript(sim_handle_self)

end




if (sim_call_type==sim_childscriptcall_sensing) then

    -- Put your main SENSING code here
    simResetGraph(graphHandle)
    
    data = simGetStringSignal("UserData")
    if data then
        measuredData = simUnpackFloatTable(data)
        for i=1,#measuredData/2,1 do
            simSetGraphUserData(graphHandle,'x',measuredData[2*(i-1)+1])
            simSetGraphUserData(graphHandle,'y',measuredData[2*(i-1)+2])

            simHandleGraph(graphHandle,0) 
        end
    end

end



if (sim_call_type==sim_childscriptcall_cleanup) then

    -- Put some restoration code here
    simResetGraph(graphHandle)

end


--[[
Normally, simHandleGraph is called for you, in the main script. In that case, you can record one value in each 
simulation step. When you explicitely handle the graph, then you have the possibility to record more than one 
new graph point in each simulation step.
--]]

　　Vision_sensor：

if (sim_call_type==sim_childscriptcall_initialization) then

    -- Put some initialization code here
    visionSensor1Handle = simGetObjectHandle("Vision_sensor")
    red={1,0,0}
    points=simAddDrawingObject(sim_drawing_points,5,0,-1,1000,red,nil,red,red)
end


if (sim_call_type==sim_childscriptcall_sensing) then

    -- Put your main SENSING code here
    measureData = {}
    m1=simGetObjectMatrix(visionSensor1Handle,-1)
    r,t1,u1=simReadVisionSensor(visionSensor1Handle)
    
    if u1 then   

        for j=0,u1[2]-1,1 do      -- point count along Y
            for i=0,u1[1]-1,1 do  -- point count along X
                w=2+4*(j*u1[1]+i) -- index
                v1=u1[w+1]        -- x
                v2=u1[w+2]        -- y
                v3=u1[w+3]        -- z
                v4=u1[w+4]        -- dist

                if(v4 < 0.99) then
                   --[[
                    p={v1,v2,v3}
                    p=simMultiplyVector(m1,p) -- convert point from visionSensor1 coordinates to world coordinates
                    simAddDrawingObjectItem(points, p)
                    --]]
                    table.insert(measureData, v1)
                    table.insert(measureData, v3)
                end

            end
        end

        stringData = simPackFloats(measureData) -- Packs a table of floating-point numbers into a string
        simSetStringSignal("UserData", stringData)
    end

end


if (sim_call_type==sim_childscriptcall_cleanup) then

    -- Put some restoration code here
    simRemoveDrawingObject(points)

end

　　在场景中加入浮动窗口，并将Graph与窗口关联，切换到X-Y曲线显示（取消坐标轴自动缩放，坐标轴比例设为1：1）

• SICK TiM310

　　VREP模型浏览器的components/sensors目录中包含多种激光雷达，其中德国SICK传感器推出的迷你激光测量系统TiM310可视范围为270°，角度分辨率为1°，探测距离为0.05m~4m，扫描频率15Hz，功耗4W。

operating range diagram

　　由于是用视觉传感器来模拟激光雷达，因此270°的可视范围对于一般视觉传感器来说太大（普通广角镜头视角在60－84度，超广角镜头的视角为94-118度，鱼眼镜头视角在220-230度），可以拆分为2个视角135°的视觉传感器来凑成270°：

　　视角参数和远近剪切平面设置如下：

 　　激光雷达获取的位置数据要在参考坐标系SICK_TiM310_ref中描述，而simReadVisionSensor获取的像素点坐标是相对于视觉传感器坐标系的，因此需要使用矩阵变换将其转换到SICK_TiM310_ref参考系中。用画笔在场景中画出激光雷达的扫描线，需要得知线段起点和终点坐标，这一坐标是在世界坐标系下描述的，因此一共涉及3个坐标系之间的变换。

if (sim_call_type==sim_childscriptcall_initialization) then 
    visionSensor1Handle=simGetObjectHandle("SICK_TiM310_sensor1")
    visionSensor2Handle=simGetObjectHandle("SICK_TiM310_sensor2")
    joint1Handle=simGetObjectHandle("SICK_TiM310_joint1")
    joint2Handle=simGetObjectHandle("SICK_TiM310_joint2")
    sensorRefHandle=simGetObjectHandle("SICK_TiM310_ref")  -- the base of SICK LiDAR

    maxScanDistance=simGetScriptSimulationParameter(sim_handle_self,'maxScanDistance')
    if maxScanDistance>1000 then maxScanDistance=1000 end
    if maxScanDistance<0.1 then maxScanDistance=0.1 end
    simSetObjectFloatParameter(visionSensor1Handle,sim_visionfloatparam_far_clipping,maxScanDistance)
    simSetObjectFloatParameter(visionSensor2Handle,sim_visionfloatparam_far_clipping,maxScanDistance)
    maxScanDistance_=maxScanDistance*0.9999

    scanningAngle=simGetScriptSimulationParameter(sim_handle_self,'scanAngle')
    if scanningAngle>270 then scanningAngle=270 end
    if scanningAngle<2 then scanningAngle=2 end
    scanningAngle=scanningAngle*math.pi/180
    simSetObjectFloatParameter(visionSensor1Handle,sim_visionfloatparam_perspective_angle,scanningAngle/2)
    simSetObjectFloatParameter(visionSensor2Handle,sim_visionfloatparam_perspective_angle,scanningAngle/2)

    simSetJointPosition(joint1Handle,-scanningAngle/4)
    simSetJointPosition(joint2Handle,scanningAngle/4)
    red={1,0,0}
    lines=simAddDrawingObject(sim_drawing_lines,1,0,-1,1000,nil,nil,nil,red)

    if (simGetInt32Parameter(sim_intparam_program_version)<30004) then
        simDisplayDialog("ERROR","This version of the SICK sensor is only supported from V-REP V3.0.4 and upwards.&&nMake sure to update your V-REP.",sim_dlgstyle_ok,false,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
    end
end 

if (sim_call_type==sim_childscriptcall_cleanup) then 
    simRemoveDrawingObject(lines)
end 

if (sim_call_type==sim_childscriptcall_sensing) then 
    measuredData={}
    
    if notFirstHere then
        -- We skip the very first reading
        simAddDrawingObjectItem(lines,nil)
        showLines=simGetScriptSimulationParameter(sim_handle_self,'showLaserSegments')

        r,t1,u1=simReadVisionSensor(visionSensor1Handle)
        r,t2,u2=simReadVisionSensor(visionSensor2Handle)
    
        m1=simGetObjectMatrix(visionSensor1Handle,-1)
        m01=simGetInvertedMatrix(simGetObjectMatrix(sensorRefHandle,-1)) 
        m01=simMultiplyMatrices(m01,m1)  -- transformation matrix between base and visionSensor

        m2=simGetObjectMatrix(visionSensor2Handle,-1)
        m02=simGetInvertedMatrix(simGetObjectMatrix(sensorRefHandle,-1))
        m02=simMultiplyMatrices(m02,m2)

        if u1 then
            p={0,0,0}
            p=simMultiplyVector(m1,p) -- convert the origin of visionSensor1 coordinates to world coordinates
            t={p[1],p[2],p[3],0,0,0}

            for j=0,u1[2]-1,1 do      -- point count along Y
                for i=0,u1[1]-1,1 do  -- point count along X
                    w=2+4*(j*u1[1]+i) -- index
                    v1=u1[w+1]        -- coordinate x of detected point(Coordinates are relative to the vision sensor position/orientation.)
                    v2=u1[w+2]        -- coordinate y of detected point
                    v3=u1[w+3]        -- coordinate z of detected point
                    v4=u1[w+4]        -- distance to detected point
                    if (v4<maxScanDistance_) then
                        p={v1,v2,v3}
                        p=simMultiplyVector(m01,p) -- describe position in LiDAR base coordinates
                        table.insert(measuredData,p[1])
                        table.insert(measuredData,p[2])
                        table.insert(measuredData,p[3])
                    end
                    if showLines then  -- draw laser line
                        p={v1,v2,v3}
                        p=simMultiplyVector(m1,p) -- convert point from visionSensor1 coordinates to world coordinates
                        t[4]=p[1]
                        t[5]=p[2]
                        t[6]=p[3]
                        simAddDrawingObjectItem(lines,t)
                    end
                end
            end
        end
        if u2 then
            p={0,0,0}
            p=simMultiplyVector(m2,p)
            t={p[1],p[2],p[3],0,0,0}
            for j=0,u2[2]-1,1 do
                for i=0,u2[1]-1,1 do
                    w=2+4*(j*u2[1]+i)
                    v1=u2[w+1]
                    v2=u2[w+2]
                    v3=u2[w+3]
                    v4=u2[w+4]
                    if (v4<maxScanDistance_) then
                        p={v1,v2,v3}
                        p=simMultiplyVector(m02,p)
                        table.insert(measuredData,p[1])
                        table.insert(measuredData,p[2])
                        table.insert(measuredData,p[3])
                    end
                    if showLines then
                        p={v1,v2,v3}
                        p=simMultiplyVector(m2,p)
                        t[4]=p[1]
                        t[5]=p[2]
                        t[6]=p[3]
                        simAddDrawingObjectItem(lines,t)
                    end
                end
            end
        end
    end
    notFirstHere=true
    
    -- measuredData now contains all the points that are closer than the sensor range
    -- For each point there is the x, y and z coordinate (i.e. 3 number for each point)
    -- Coordinates are expressed relative to the sensor frame.
    -- You can access this data from outside via various mechanisms. The best is to first
    -- pack the data, then to send it as a string. For example:
    --
    -- 
    -- data=simPackFloatTable(measuredData)
    -- simSetStringSignal("measuredDataAtThisTime",data)
    --
    -- Then in a different location:
    -- data=simGetStringSignal("measuredDataAtThisTime")
    -- measuredData=simUnpackFloatTable(data)
    --
    --
    -- Of course you can also send the data via tubes, wireless (simTubeOpen, etc., simSendData, etc.)
    --
    -- Also, if you send the data via string signals, if you you cannot read the data in each simulation
    -- step, then always append the data to an already existing signal data, e.g.
    --
    -- 
    -- data=simPackFloatTable(measuredData)
    -- existingData=simGetStringSignal("measuredDataAtThisTime")
    -- if existingData then
    --     data=existingData..data
    -- end
    -- simSetStringSignal("measuredDataAtThisTime",data)
end 

　　获得测量数据后，可以使用函数将包含坐标值的一维列表打包成字符串，并设置字符串信号。在另一个需要使用传感器数据的脚本中读取字符串信号，然后将其解压成列表：

data = simPackFloatTable(measuredData)
simSetStringSignal("measuredDataAtThisTime", data)

-- Then in a different location:
data = simGetStringSignal("measuredDataAtThisTime")
measuredData = simUnpackFloatTable(data)

 

 

参考：

sick tim310激光安全扫描器

LabVIEW Robotics中如何借助二维激光雷达感知三维场景

Scatter Plot