基于Coppeliasim的仿真流水线协作机器人

一、机器人简介

基于Coppeliasim仿真软件，流水线协作机器人实现了将不同颜色物料的分拣投放至目标位置。传送带上分布着不等距的物料，传送带的末端有一个用来计数和检测物料是否到达的传感器，若有物料靠近传送带就会停止传送，等待JAKA12机械臂进行抓取物料。另外，根据传送带侧方的摄像头识别物料的颜色，紫色物料需要放置到地面上的目标位置，橙色物料先用机械臂“井”型堆叠放置在youBot物料小车上，然后物料小车通过轨迹跟随到达目标物料池前方，根据物料池前方安装的激光测距传感器检测物料小车与物料池的距离，当距离小于阈值时物料小车停止运动。最后物料小车首先将小车上原本有的浅蓝色方形物料放到物料池侧边平台上，物料小车上的其它物料由youBot上的机械臂推送到物料池中，整个流水线仿真场景结束。

二、遇到的问题

2.1同时安装不同版本coppeliasim

由于coppeliasim版本之间变化比较大，有时候在新版本找不到替代的方法，就要安装多版本的coppelisim，直接安装的话会直接替换掉老版本，这时候可以去官网下载二进制包，这样不安装就能运行coppeliasim，多版本就不在话下啦~
官网：[https://www.coppeliarobotics.com/downloads]

2.2youBot机器人在仿真时会滑动

虽然将机器人的速度都设为0并且增大摩擦力，但是机器人还是会在平面上滑动！这时候可以试试更改仿真的环境。

2.3youBot机器人路径跟随时不稳定、抖动

可以试着降低仿真的dt~

2.4仿真过程中出现闪退

可以检查一下是否把创建窗口或辅助控制台的代码写到了sysCall_actuation（）函数中，这样软件就会不停创建窗口，直至软件崩溃闪退。

三、关键算法

3.1视觉处理

视觉处理的摄像头采用了Vision sensor，其得到的图像可以关联到Floating view，从而实时进行观测。图像处理采用了simVision库函数，其是用于执行简单图像处理和处理特殊类型视觉算法的API函数，足够满足处理本项目的图像。

1. Step1：将摄像头获得的图像句柄用simVision.sensorImgToWorkImg()转移到工作空间，从而对图像进行进一步操作。
2. Step2：通过simVision.selectiveColorOnWorkImg() 将颜色阈值内的紫色物料分割出来，并且用simVision.blobDetectionOnWorkImg() 将物料识别出来，阈值外的颜色存储到Buffer1中。
3. Step3：用simVision.workImgToBuffer2() 函数将工作空间图像转移到Buffer2中，再用 simVision.swapWorkImgWithBuffer1() 函数将工作空间和Buffer1中的图像互换，重复Step2将橙色物料识别出来。
4. Step4：使用simVision.swapBuffers()将Buffer中的图像互换，最后用simVision.addBuffer1ToWorkImg()函数将Buffer1中的图像加到工作空间图像上，得到最终图像处理的结果。
5. Setp5：将得到的物料位置和颜色信息显示在辅助控制台窗口上并通过sim.writeCustomDataBlock()将上述信息存储起来，供其它协作机器人获取，实现机器人之间信息的传递。

function sysCall_init() 
    -- Prepare a floating view with the camera views:
    cam=sim.getObject('.')
    sim.writeCustomDataBlock(cam,'purple',0)
    out = sim.auxiliaryConsoleOpen("debug",8,1,{0,50},{200,200})
end

function sysCall_cleanup() 
    sim.auxiliaryConsoleClose(out)
end 

function sysCall_vision(inData)
    local beforedata={0,0.0}
    simVision.sensorImgToWorkImg(inData.handle)
    simVision.selectiveColorOnWorkImg(inData.handle,{1.0,0.09,0.89},{0.5,0.4,0.5},true,true,true)
    local trig,packedPacket=simVision.blobDetectionOnWorkImg(inData.handle,0.100000,0.000000,false,{1.0,1.0,0})
    resBlob = sim.unpackFloatTable(packedPacket)
    simVision.workImgToBuffer2(inData.handle)
    simVision.swapWorkImgWithBuffer1(inData.handle)
    simVision.selectiveColorOnWorkImg(inData.handle,{1.0,0.52,0.18},{0.5,0.5,0.3},true,true,false)
    local trig2,packedPacket2=simVision.blobDetectionOnWorkImg(inData.handle,0.100000,0.000000,false,{0,1.0,1.0})
    resBlob2 = sim.unpackFloatTable(packedPacket2)
    simVision.swapBuffers(inData.handle)
    simVision.addBuffer1ToWorkImg(inData.handle)
    simVision.workImgToSensorImg(inData.handle)
    for i=1,resBlob[1],1 do
        xtarget = resBlob[6*i-1]
        ytarget = resBlob[6*i]
        sim.auxiliaryConsolePrint(out,string.format("\n purple X: %0.2f",xtarget))
    end
    
    for i=1,resBlob2[1],1 do
        xtarget = resBlob2[6*i-1]
        ytarget = resBlob2[6*i]
        sim.auxiliaryConsolePrint(out,string.format("\n orange X: %0.2f",xtarget))
    end
    
    sim.writeCustomDataBlock(cam,'number',resBlob[1]+resBlob2[1])
    
    for i=1,8-resBlob[1]-resBlob2[1]-1,1 do
        sim.auxiliaryConsolePrint(out,string.format("\n"))
    end
    if resBlob[1]==0 then
        sim.writeCustomDataBlock(cam,'purple',0)
    else
        sim.writeCustomDataBlock(cam,'purple',resBlob[6*resBlob[1]-1])
    end
end

3.2基于Dummy和Path的轨迹规划

在Coppeliasim中，Path是Scence的一种“实体”，与model一样，用户可以使用Path来自定义各种运动路径，简单来说，从Path上我们可以给定某一个时刻机器人运动到某一点的位置以及其姿态，定义机器人在整个路径中每一时刻的运动过程，Coppliasim中默认的Path是通过给定一些关键的控制点，并在这些控制点中间采用贝塞尔曲线插值来得到光滑路径。Dummy常用来与Path相结合，Dummy可以沿着Path向前运动，实时获取当前时刻在Path上的位置和姿态，作为路径上的一个“领航者”。

1. Step1：获得Dummy和Path的句柄。
2. Step2：获得视觉传感器DataBlock上存储的物料信息，若传送带上无物料，则调用sim.followPath()函数，使得Dummy顺着Path的轨迹以一定的速度运动。
3. Step3：youBot获得Dummy句柄，并得到Dummy的位姿信息。
4. Step4：youBot用PD算法跟随Dummy位姿进行运动，实现轨迹的跟随。
   

工程文件：https://github.com/TycoonL/AssemblyLineRobot-Coppeliasim.git

参考文档：

1.regular API: https://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm
2.simVision API: https://www.coppeliarobotics.com/helpFiles/en/simVision.htm