位置视觉伺服运动工件分拣系统设计

视觉伺服

---

1 研究背景与意义

2 实验目标

以实验室装配机器人及其零件运动为例，给定某传输带上零件运动视频，要求
（1）提取零件特征，包括位置、速度、轨迹等。给出编程实例和实验。（25分）
（2）根据零件的运动特征，设计装配机器人的抓取视觉伺服控制器，给出仿真结果。（25分）

3 设计策略与原理

3.1 前景提取与目标检测

$\qquad$当观察场景的是一个固定的相机，背景几乎保持不变。在这种情况下，感兴趣的元素是在场景中运动的物体。为了提取出这些前景物体，我们需要对背景建模，然后将当前帧的模型与背景模型进行比较，以检测前景物体。前景提取是智能监控应用的基础步骤。
$\qquad$常用的目标检测方法为帧间差分和背景减除:
背景减除法：
$\qquad$背景减去法的含义是构建一个参考图片（纯背景），将每个新的视频帧同参考图片相减，并进行二值化，从而得到运动前景的方法。这也就意味着，背景减去法的结果是一个对非静态区域的一个高亮表示。构建参考图片最简单的方法就是将时间上连续的一系列背景图片进行平均。这种方法有很多问题，同时，它还需要一个没有前景出现，仅仅有纯背景的一段图片序列来作为训练样本。训练之后，背景的运动以及训练阶段前景的静止不运动，都被当做运动目标。另外，该方法无法应对场景中光线的逐渐变化。
$\qquad$背景减除方法的关键在于建立一个鲁棒的背景模型（背景图像），常用的建立背景模型方法有：均值法；中值法；滑动平均滤波法；单高斯；混合高斯模型；codebook等。
$\qquad$OpenCV的video module中包含了几种较为常用的背景减除方法，其中混合高斯模型（Gaussian of Mixture Models, GMM）方法效果较好。

混合高斯模型的原理:
$\qquad$混合高斯背景建模是基于像素样本统计信息的背景表示方法，利用像素在较长时间内大量样本值的概率密度等统计信息(如模式数量、每个模式的均值和标准差)表示背景，然后使用统计差分(如3σ原则)进行目标像素判断，可以对复杂动态背景进行建模，计算量较大。
$\qquad$在混合高斯背景模型中，认为像素之间的颜色信息互不相关，对各像素点的处理都是相互独立的。对于视频图像中的每一个像素点，其值在序列图像中的变化可看作是不断产生像素值的随机过程，即用高斯分布来描述每个像素点的颜色呈现规律。
$\qquad$对于多峰高斯分布模型，图像的每一个像素点按不同权值的多个高斯分布的叠加来建模，每种高斯分布对应一个可能产生像素点所呈现颜色的状态，各个高斯分布的权值和分布参数随时间更新。当处理彩色图像时，假定图像像素点R、G、B三色通道相互独立并具有相同的方差。

3.2 基于Lukas-Kanade跟踪器的特征点跟踪(光流法)原理

3.3 流程图

4 算法分析与步骤

4.1 前景提取与目标检测实用算法

$\qquad$如果有不含前景物体的背景图片，提取前景的工作相对容易，只需要比对当前帧和背景图片的不同，调用函数absdiff即可实现。但是大多数情况，获得背景图片是不可能的，比如在复杂的场景下，或者有光线条件的变化。因此，就需要动态的变换背景。一种简单的办法是对所观察到的图片取平均，但这样做也有很多弊端，首先，这种办法在计算背景图片的前需要输入大量的图片，其次我们进行取平均的过程中不能有前景物体进入。所以一种相对好的办法是动态建立背景图片并实时更新。
基于阈值图像差分的前景提取算法步骤：
$\qquad$具体的实现过程主要分为两部分：一部分是调用absdiff函数找出当前图片和背景图片的区别，这之中使用了threshold函数去除为前景，当前图片像素与背景图片像素变化超过一定阈值的时候才认定其为前景；另一个工作是更新背景图片，调用函数accumulateWeighted，根据权重参数可以调整背景更新的速度，将当前图片更新到背景中，这里巧妙利用得到的前景提取结果作为mask，在更新背景图片的过程中避免了前景的干扰。
$\qquad$这种定期更新背景模型的方法，可以通过计算滑动平均值，即对时序信号计算均值时考虑接收到最新值。如果$p_t$是t时刻的像素值，$\mu_{t-1}$是当前的平均值，那么新的平均值将是:

$$\mu_t=(1-\alpha)*\mu_{t-1}+\alpha*p_t$$
$\qquad$其中参数$\alpha$被称为学习率，它定义了当前值对平均值的影响程度。该值越大，滑动平均值对观察值的适应速度更快。为了构建背景模型，需要对图像帧的每个像素计算滑动平均数。判断前景要素的依据是当前图像与背景模型之间的差值，这种算法的主要难点在于找到合适的阈值。

//基于阈值图像差分的前景提取算法
class BGFGSegmentor : public FrameProcessor {
    Mat gray;//当前帧灰度图  
    Mat background;//背景图，格式为32位浮点  
    Mat backImage;//CV_8U格式背景图  
    Mat foreground;//前景图  
    double learningRate;//学习率  
    int threshold;//阈值，滤去扰动  
public:
    BGFGSegmentor() :threshold(30), learningRate(0.6) {}
    //帧处理函数  
    void process(Mat &frame, Mat &output) {
        //转化为灰度图  
        cvtColor(frame, gray, CV_BGR2GRAY);
        if (background.empty())
            //第一帧  
            gray.convertTo(background, CV_32F);
        //背景转为CV_8U格式以便求取和当前帧差的绝对值  
        background.convertTo(backImage, CV_8U);
        //求当前帧与背景的差别  
        absdiff(backImage, gray, foreground);
        //过滤掉前景中与背景差别不大的扰动点  
        cv::threshold(foreground, output, threshold, 255, THRESH_BINARY_INV);
        //更新背景，output作为掩码  
        accumulateWeighted(gray, background, learningRate, output);
    }
};

$\qquad$虽然可以调整阈值参数和权重更新速度调节前景提取的结果，但从原理上分析不难发现，若第一帧就带有前景内容就会对结果产生干扰，因为后续更新背景都是对前景mask后对背景进行更新的，所以第一帧的前景部分对背景的影响因子很难被更新掉，另外在光照不均匀的坏境下拍摄就会出现杂点难以去除。如果采用混合高斯模型。可以使一个像素具有更多的信息，这样可以有效的减少由于光照不均匀等环境因素造成对前景的干扰。
混合高斯模型的算法步骤:
$\qquad$首先，混合高斯算法对每个像素维护多个模型（即多个滑动平均值）。因此，如果背景像素在两个值之间来回变化，那么将存储两个滑动平均值。如果新的像素值不属于其中之一，则认为它是前景值。
$\qquad$其次，不仅仅保存滑动平均值，还保存滑动方差。它的计算方式为：

$$\sigma^2_1=(1-\sigma)*\sigma^2_{t-1}+\sigma(p_t-\mu_t)^2$$
$\qquad$计算得到的均值和方差形成了一个高斯模型，通过它我们可以获知某个像素值属于该模型的概率。它使得确定合适的阈值变得简单，因为现在它表现为一个概率而非差异的绝对值。同时，背景中变化较明显的区域，前景物体需要的差值将大很多。
$\qquad$最后，当高斯模型的击中频率不够频繁，它会从当前的模型中移除。反之，如果一个像素值位于当前维护的背景模型之外（即它是一个前景像素），那么新的高斯模型将被创建。如果之后它被频繁击中，那么会成为背景的一部分。
$\qquad$这个精密的算法相比之前的简单方法要复杂很多，不易实现。还好，OpenCV已经为我们做好了相关工作，将其封装在类BackgroundSubtractorMOG，使用起来非常方便。实现代码如下所示：

class GS : public FrameProcessor {
    Mat gray;               //当前帧灰度图 
    Mat foreground;         // 前景图片  
    BackgroundSubtractorMOG mog; //混合高斯模型 实例
public:
    void process(Mat &frame, Mat &output) {
        cvtColor(frame, gray, CV_BGR2GRAY);//转化为灰度图
        mog(gray, foreground, 0.01);
        threshold(foreground, output, 128, 255, THRESH_BINARY_INV);
    }
};

$\qquad$考虑到获取的视频已经包含了外界环境设备干扰，可在已经得到的前景中定义感兴趣区域并通过白色掩膜掩盖外围设备干扰，在一定程度上把运动物块提取出来，方便后续对其的特征点跟踪。

output.convertTo(output, CV_8U);
ROI = output(Range(240, 480),Range(200, 640));//设置感兴趣区域
//因已知传送带在视频中的大致位置，故而只需添加上下两块三角掩膜
/*****************下三角********************/
for (int j = 0; j < ROI.rows; j++) {
    for (int i = 0; i < ROI.cols; i++) {
        if((ROI.rows*1.0/ROI.cols*i)>ROI.rows-j)
        ROI.ptr<uchar>(j)[i] = saturate_cast<uchar>(255);
    }
}
/*****************上三角********************/
for (int j = 0; j < ROI.rows*1.0/2; j++) {
    for (int i = 0; i < ROI.cols*1.0 / 2; i++) {
        if ((ROI.rows*1.0 / ROI.cols*i)<ROI.rows*1.0/2 - j)
            ROI.ptr<uchar>(j)[i] = saturate_cast<uchar>(255);
    }
}
imshow("ROI", ROI);

4.2 基于Lukas-Kanade跟踪器的特征点跟踪(光流法)

$\qquad$先前我们已经获取了视频中的前景，前景的有效获得可以帮助我们更好的检测运动物体的特征点。为了在不同帧之间跟踪特征点，我们必须定位特征点在随后的图像帧中的新位置。如果我们假设特征点的强度在相邻帧之间不发生变化，那么我们要找到一个偏移量$（u,v）:$

$$I_t(x,y)=I_{t+1}(x+u,y+v)$$
$\qquad$其中$I_t$和$I_{t+1}$分别是当前帧以及下一时刻的值。这个强度不变的假设对于拍摄时间相近的图像中的微小偏移量而言都是成立的。我们可以使用泰勒展开式来近似得到这个方程式，新的方程式将设计图像的倒数：
$$I_{t+1}(x+u,y+v)\approx I_t(x,y)+\frac{dI}{dx}u+\frac{dI}{dy}v+\frac{dI}{dt}$$
$\qquad$如果强度不变假设成立，可以推出另一个方程式：
$$\frac{dI}{dx}u+\frac{dI}{dy}v=-\frac{dI}{dt}$$
$\qquad$这个著名的约束就是基础的光流约束方程式，在引入另一个假设后，它被使用于Lukas-Kanade特征跟踪算法，即特征点附近所有点的偏移量都是一致的。因此我们对于拥有独特未知偏移量(u,v)的点都利用光流约束。这给予我们超过未知数的数量（两个）的方程式，我们可以利用均方法进行求解。在实践中它通过迭代的方式进行求解，OpenCV实现了在不同分辨率下进行估算求解的函数，使得可以更有效地进行搜索，同时也能适应较大的偏差。

//Harris算子提取角点
class HarrisDetector {
private:
    //表示角点强度的32位浮点图像
    Mat cornerStrength;
    //表示阈值化后角度的32位浮点图像
    Mat cornerTh;
    //局部极大值图像（内部）
    Mat localMax;
    //导数平滑的相邻像素的尺寸
    int neighbourhood;
    //梯度计算的孔径大小
    int aperture;
    //Harris参数
    double k;
    //阈值计算的最大强度
    double maxStrength;
    //计算得到的阈值（内部）
    double threshold;
    //非极大值抑制的相邻像素的尺寸
    int nonMaxSize;
    //非极大值抑制的核
    Mat kernel;
    void applyThreshold(Mat &result,int threshold) {
        if (threshold > 0)
            cv::threshold(result, result, threshold, 255, THRESH_BINARY);
    }
public:
    HarrisDetector() :neighbourhood(3), 
                    aperture(3), k(0.01), 
                    maxStrength(0.0), 
                    threshold(0.01), 
                    nonMaxSize(3)
    {
        //创建非极大值抑制的核
        setLocalMaxWindowSize(nonMaxSize);
    }
    //创建非极大值抑制的核
    void setLocalMaxWindowSize(int size){
        nonMaxSize = size;
        kernel.create(nonMaxSize, nonMaxSize, CV_8U);
    }
    //计算Harris的角点
    void detect(const Mat &image) {
        cornerHarris(image, cornerStrength,
            neighbourhood,      //neighborhood size
            aperture,           //滤波器的孔径大小
            k);                 //Harris参数
        //内部阈值计算
        double minStrength;     //未使用
        minMaxLoc(cornerStrength, &minStrength, &maxStrength);
        /*Mat result1; 
        cornerStrength.copyTo(result1);
        applyThreshold(result1,0.0001);
        imshow("cornerStrength", result1);*/
        cout << "Min = " << minStrength << " Max = " << maxStrength << " localMax = " << localMax << endl;
        //局部极大值检测
        Mat dilated;//临时图像
        dilate(cornerStrength, dilated, Mat());
        compare(cornerStrength, dilated, localMax, CMP_EQ);
    }
    //由Harris值获取角点图
    Mat getCornerMap(double qualityLevel) {
        Mat cornerMap;
        //对角点图像进行阈值化
        threshold = qualityLevel*maxStrength;
        cv::threshold(cornerStrength, cornerTh, threshold, 255, THRESH_BINARY);
        //转换为8位图像
        cornerTh.convertTo(cornerMap, CV_8U);
        //非极大值抑制
        bitwise_and(cornerMap, localMax, cornerMap);
        return cornerMap;
    }
    //由Harris值得到特征点
    void getCorners(vector<Point>&points, double qualityLevel) {
        //得到角点图
        Mat cornerMap = getCornerMap(qualityLevel);
        //得到角点
        getCorners(points, cornerMap);
    }
    //由角点图获取特征点
    void getCorners(vector<Point>&points, const Mat &cornerMap) {
        //遍历像素得到所有特征
        for (int y = 0; y < cornerMap.rows; y++) {
            const uchar*rowPtr = cornerMap.ptr<uchar>(y);
            for (int x = 0; x < cornerMap.cols; x++) {
                //如果是特征点
                if (rowPtr[x]) {
                    points.push_back(Point(x, y));
                }
            }
        }
    }
    //在特征点的位置绘制图
    void drawOnImage(Mat image, 
                    const vector<Point>&points, 
                    Scalar color = Scalar(0, 0, 255),
                    int radius = 3, 
                    int thickness = 2) {
        vector<Point>::const_iterator it = points.begin();
        //对于所有角点
        while (it != points.end()) {
            //绘制一个图
            circle(image, *it, radius, color, thickness);
            ++it;
        }
    }
};
//主函数调用
HarrisDetector harris;          //创建Harris检测器的对象
harris.detect(frame);           //计算Harris值
vector<Point>pts;               //检测Harris角点
harris.getCorners(pts, 0.01);   //绘制Harris角点
harris.drawOnImage(frame,pts);
namedWindow("harris");
imshow("harris", frame);

//SURT特征提取
//主函数调用
    std::vector<cv::KeyPoint> keypoint1;// 设置用于存放特征点的向量
    // 构造SURF特征检测器
    cv::SurfFeatureDetector surf(3000);     // 阈值
    surf.detect(frame, keypoint1);          // 对两幅图分别检测SURF特征
    cv::Mat imageSURF;                      //输出带有详细特征点信息的两幅图像
    cv::drawKeypoints(frame, keypoint1,
        imageSURF,
        cv::Scalar(0, 0, 255),
        cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
    cv::namedWindow("SURF Features");
    cv::imshow("SURF Features", imageSURF);

//基于Lukas-Kanade跟踪器的特征点跟踪
class FeatureTracker : public FrameProcessor {
    Mat gray;  //当前灰度图
    Mat gray_prev;  //之前的灰度图
    vector<Point2f> points[2];//前后两帧的特征点
    vector<Point2f> initial;//初始特征点
    vector<Point2f> features;//检测到的特征
    int max_count; //要跟踪特征的最大数目
    double qlevel; //特征检测的指标
    double minDist;//特征点之间最小容忍距离
    vector<uchar> status; //特征跟踪状态
    vector<float> err; //跟踪时的错误 
public:
    FeatureTracker() :max_count(500), qlevel(0.01), minDist(10.) {}
    void process(Mat &frame, Mat &output) {
        //得到灰度图
        cvtColor(frame, gray, CV_BGR2GRAY);
        frame.copyTo(output);
        //1.如果需要添加新的特征点
        //特征点太少了，重新检测特征点
        if (addNewPoint()) {
            detectFeaturePoint();
            //插入检测到的特征点
            points[0].insert(points[0].end(), features.begin(), features.end());
            initial.insert(initial.end(), features.begin(), features.end());
        }
        //第一帧
        if (gray_prev.empty()) {
            gray.copyTo(gray_prev);
        }
        //2.跟踪特征点
        //根据前后两帧灰度图估计前一帧特征点在当前帧的位置
        //默认窗口是15*15
        calcOpticalFlowPyrLK(
            gray_prev,//前一帧灰度图
            gray,//当前帧灰度图
            points[0],//前一帧特征点位置
            points[1],//当前帧特征点位置
            status,//特征点被成功跟踪的标志
        err,
        Size(20,20));//前一帧特征点点小区域和当前特征点小区域间的差，根据差的大小可删除那些运动变化剧烈的点
        int k = 0;
        //去除那些未移动的特征点
        for (int i = 0; i<points[1].size(); i++) {
            if (acceptTrackedPoint(i)) {
                initial[k] = initial[i];
                points[1][k++] = points[1][i];
            }
        }
        points[1].resize(k);
        initial.resize(k);
        //标记被跟踪的特征点
        handleTrackedPoint(frame, output);
        //4.当前帧的点和图像变为前一帧的点和图像
        //为下一帧跟踪初始化特征点集和灰度图像
        std::swap(points[1], points[0]);
        cv::swap(gray_prev, gray);
    }
    void detectFeaturePoint() {
        goodFeaturesToTrack(gray,//图片
            features,//输出特征点
            max_count,//特征点最大数目
            qlevel,//质量指标
            minDist);//最小容忍距离
    }
    bool addNewPoint() {
        //若特征点数目少于10，则决定添加特征点
        return points[0].size() <= 10;
    }
    //若特征点在前后两帧移动了，则认为该点是目标点，且可被跟踪
    bool acceptTrackedPoint(int i) {
        return status[i] &&
            (abs(points[0][i].x - points[1][i].x) +
                abs(points[0][i].y - points[1][i].y) >2);
    }
    //画特征点
    void  handleTrackedPoint(Mat &frame, Mat &output) {
        //cout << "Size=" << points[1].size() << endl;
        for (int i = 0; i<points[1].size(); i++) {
            //当前特征点到初始位置用直线表示
            line(output, initial[i], points[1][i], Scalar(255, 255, 255));
            //line(output, initial[i], points[1][i], Scalar::all(0));
            //当前位置用圈标出
            circle(output, points[1][i], 3, Scalar(255, 255, 255), (-1));
            //circle(output, points[1][i], 3, Scalar::all(0), (-1));
        }
    }
};

5 效果展示与结论

5.1 背景提取与目标检测效果图

基于阈值图像差分的前景提取算法

$\qquad$从上图中我们可以看出黑色物块即为我们所要抓取的目标长方体。理想的效果为运动物块呈现黑色（即为前景），静止物体呈现为白色（即为背景），但是从上述图像（得到的前景图）中我们可以发现前景图参杂了除传送带以外的设备轮廓。这些轮廓出现的原因是因为这些设备受到的光照强度不同所引起的，因此在工业设计过程中我们相机的摆放位置应尽量避免拍摄到除目标系统以外的设备，并在光照均匀的场合下进行数据采集。
$\qquad$另外由于视频中拍摄者并没有固定相机的位置，所以会造成前景图获取时出现重影等不利于提取真实前景的情况，如下图所示：

混合高斯模型的前景提取算法


$\qquad$显然通过混合高斯模型提取到的前景比用前者的效果提升了不少，然而它同样不能解决相机移动造成背景移动的问题，在拍摄者移动相机时仍然可以明显的看到获取到的前景图中出现重影，如下所示：（这也正说明了固定相机的重要性）

$\qquad$通过上述效果图的对比与分析，我们可以相信如果摄像机的有效拍摄范围仅为受均匀光照的移动传送带，那么两种算法均可以得到理想的前景图。在实验中，我们也发现混合高斯模型算法因其计算量庞大，运算速度慢并不适合实时在线控制系统，因此在实际操作中我们会采用简单可行的基于阈值图像差分的前景提取算法。
定义感兴趣并添加掩膜后效果图：

$\qquad$根据此效果图展示，我们有理由相信在固定相机的情况下，是可以有效去除由于背景移动造成对前景提取的干扰。

5.2 特征点跟踪

Harris算子提取角点效果图

SURF特征提取算法效果图

基于Lukas-Kanade跟踪器的流光法跟踪效果图
$\qquad$从上图所示，基于Lukas-Kanada跟踪器的流光法具有较好的特征点跟踪效果，可以获得运动物体在相机坐标系的位置、速度、轨迹。如果相机的标定参数已知，就可以通过其内外参数矩阵将相机坐标系对应的世界坐标系求得，在此基础上获得运动物体的实际位置、速度、轨迹亦不难，后续可对其进行控制算法设计（可先尝试用PID实现）。