一、颜色目标定位方法

定位是在识别目标区域后，获取其在图像坐标系或世界坐标系中的具体位置。

1. 图像坐标系定位

核心是获取目标区域的关键坐标，常用方式包括：

• 质心定位：计算目标掩码区域的像素重心 $(x, y)$。
  • 公式：$x = \frac{\sum x_i}{S}$，$y = \frac{\sum y_i}{S}$ （$S$ 为目标区域像素总数）。
  • 适用：规则或不规则目标。
• 边界框定位：获取目标区域的最小外接矩形，输出矩形的左上角 $(x_1, y_1)$ 和右下角 $(x_2, y_2)$ 坐标，便于快速确定目标范围。

2. 世界坐标系定位（可选进阶）

当需要获取目标在真实空间中的位置时，需通过相机标定实现：

• 相机标定：通过棋盘格等标定板，获取相机的内参（焦距、主点坐标）和外参（相机位置姿态）。
• 坐标转换：利用标定参数，将图像坐标系中的目标坐标，转换为世界坐标系（如米、厘米）的三维位置 $(x, y, z)$。

3. 定位精度优化

• 提高相机分辨率：增加像素密度，减少坐标计算的量化误差。
• 优化目标区域：通过形态学操作确保目标区域完整，避免因区域残缺导致质心偏移。
• 固定相机与目标距离：减少景深变化对目标成像大小的影响，提升定位一致性。

---

二、基础代码实现（仅定位与显示）

1. 函数头文件

#include <ros/docs.h>                  // ROS头文件
#include <cv_bridge/cv_bridge.h>      // 转换图像格式的头文件，将ROS的数据格式转换为OpenCV的数据格式
#include <sensor_msgs/image_encodings.h> // 图像编码格式的头文件
#include <opencv2/imgproc/imgproc.hpp>   // OpenCV的图像处理函数头文件
#include <opencv2/highgui/highgui.hpp>   // OpenCV的图形化显示函数头文件

2. 引入命名空间

using namespace cv;
using namespace std;

3. 定义与 HSV 相关变量

static int iLowH = 10;
static int iHighH = 40;
static int iLowS = 90; 
static int iHighS = 255;
static int iLowV = 1;
static int iHighV = 255;

4. 回调函数

void Cam_RGB_Callback(const sensor_msgs::Image msg)
{
    // ROS_INFO("Cam_RGB_Callback");
    cv_bridge::CvImagePtr cv_ptr; // 创建指针函数
    try
    {
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e) // 用于异常数据处理，防止程序卡死
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    Mat imgOriginal = cv_ptr->image; // RGB

    // 将RGB图片转换成HSV
    Mat imgHSV;
    vector<Mat> hsvSplit; 
    // 在 HSV 图像通道分割中，hsvSplit的作用是接收split函数的输出，存储拆分后的三个单通道图像
    
    cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV); 
    // cvtColor（全称：convert Color）是 OpenCV 提供的色彩空间转换函数
    
    // 在HSV空间做直方图均衡化
    split(imgHSV, hsvSplit); // 将 HSV 色彩空间的图像分割为三个独立的通道（Hue色调、Saturation饱和度、Value明度）
    equalizeHist(hsvSplit[2], hsvSplit[2]); // 对 HSV 的明度通道（V 通道）进行直方图均衡化，增强图像的对比度
    merge(hsvSplit, imgHSV); // 将分割后并处理完的三个通道重新合并为一幅 HSV 色彩空间的图像

    Mat imgThresholded; // 定义一个Mat类型的变量imgThresholded，用于存储后续阈值分割（二值化）的结果

    // 使用上面的Hue,Saturation和Value的阈值范围对图像进行二值化
    inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded); 
    // inRange 是 OpenCV 中用于色彩阈值分割的核心函数，imgThresholded存储分割结果（白色为目标区域，黑色为背景）

    // 开操作 (去除一些噪点)
    Mat element = getStructuringElement(MORPH_RECT, Size(5, 5)); 
    // 创建一个形态学操作的 “结构元素”（也称为 “核”），用于定义形态学运算的范围和形状
    morphologyEx(imgThresholded, imgThresholded, MORPH_OPEN, element); 
    // 执行开运算（Morphological Opening），这是一种基础的形态学操作，由 “腐蚀（Erosion）” 和 “膨胀（Dilation）” 两个步骤组成（先腐蚀后膨胀）

    // 闭操作 (连接一些连通域)
    morphologyEx(imgThresholded, imgThresholded, MORPH_CLOSE, element); 
    // 与开操作类似，不过顺序有所不同，他是先膨胀，后腐蚀

    // 遍历二值化后的图像数据
    int nTargetX = 0; // 目标区域x坐标总和
    int nTargetY = 0; // 目标区域y坐标总和
    int nPixCount = 0; // 目标区域的像素总数
    int nImgWidth = imgThresholded.cols; // 二值化图像的宽度（列数）
    int nImgHeight = imgThresholded.rows; // 二值化图像的高度（行数）
    int nImgChannels = imgThresholded.channels(); // 图像通道数（二值化图通常为1）

    for (int y = 0; y < nImgHeight; y++) // 遍历每一行（y为行坐标）
    {
        for(int x = 0; x < nImgWidth; x++) // 遍历每一列（x为列坐标）
        {
            if(imgThresholded.data[y*nImgWidth + x] == 255) // 判断当前像素是否为目标像素（二值化图中白色像素为255，代表目标）
            {
                nTargetX += x; // 累加x坐标
                nTargetY += y; // 累加y坐标
                nPixCount ++;  // 目标像素数+1
            }
        }
    }

    if(nPixCount > 0) // 若存在目标像素
    {
        nTargetX /= nPixCount; // 质心坐标 = 坐标总和 / 像素总数（平均坐标）
        nTargetY /= nPixCount; // 质心坐标 = 坐标总和 / 像素总数（平均坐标）
        printf("颜色质心坐标( %d , %d )  点数 = %d\n", nTargetX, nTargetY, nPixCount);

        // 在原始图像上画十字标记质心
        Point line_begin = Point(nTargetX-10, nTargetY); // 水平线段起点
        Point line_end = Point(nTargetX+10, nTargetY);   // 水平线段终点
        line(imgOriginal, line_begin, line_end, Scalar(255,0,0)); // 画水平线（蓝色）
        
        line_begin.x = nTargetX; line_begin.y = nTargetY-10; // 垂直线段起点
        line_end.x = nTargetX; line_end.y = nTargetY+10;     // 垂直线段终点
        line(imgOriginal, line_begin, line_end, Scalar(255,0,0)); // 画垂直线（蓝色）
    }
    else
    {
        printf("目标颜色消失...\n");
    }

    // 显示处理结果
    imshow("RGB", imgOriginal);      // 结果显示
    imshow("HSV", imgHSV);           // 结果显示
    imshow("Result", imgThresholded); // 结果显示
    cv::waitKey(5); // 等待一会儿，给程序一些处理时间
}

5. 主函数

int main(int argc, char **argv)
{
    ros::init(argc, argv, "demo_cv_hsv"); // 初始化
    ros::NodeHandle nh; // 召唤大管家
    
    // 订阅话题，配置分辨率，选择相机名称，配置接收消息的缓存长度
    ros::Subscriber rgb_sub = nh.subscribe("/kinect2/qhd/image_color_rect", 1, Cam_RGB_Callback); 
    
    ros::Rate loop_rate(30);

    // 生成图像显示和参数调节的窗口空间
    namedWindow("Threshold", WINDOW_AUTOSIZE);
    // 创建名为 “Threshold” 的窗口，WINDOW_AUTOSIZE 表示窗口大小随内容自动调整
    
    createTrackbar("LowH", "Threshold", &iLowH, 179);  // Hue (0 - 179)，这里不是360是因为这样写只需占用一个字节
    createTrackbar("HighH", "Threshold", &iHighH, 179);
    createTrackbar("LowS", "Threshold", &iLowS, 255);  // Saturation (0 - 255)
    createTrackbar("HighS", "Threshold", &iHighS, 255);
    createTrackbar("LowV", "Threshold", &iLowV, 255);  // Value (0 - 255)
    createTrackbar("HighV", "Threshold", &iHighV, 255);

    namedWindow("RGB"); 
    namedWindow("HSV"); 
    namedWindow("Result"); 

    while(ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

---

三、目标跟随

1. 原理

目标跟随是在目标定位基础上，通过控制执行机构（如移动机器人底盘、机械臂等）使执行机构与目标保持预设相对位置的过程。结合前文的颜色目标定位功能，可实现基于视觉的闭环控制跟随系统。

1.1 闭环控制逻辑

目标跟随的核心是偏差纠正：通过视觉识别获取目标在图像中的位置，与预设的 “期望位置”（如图像中心）比较得到偏差，再根据偏差计算控制量，驱动执行机构调整位置以消除偏差。

• 偏差定义： 设图像分辨率为 $(W, H)$，目标质心坐标为 $(x, y)$，期望位置为图像中心 $(W/2, H/2)$，则：

  • 水平偏差 $dx = x - W/2$ （正值表示目标在右侧，负值表示在左侧）
  • 垂直偏差 $dy = y - H/2$ （正值表示目标在下方，负值表示在上方）
  • 注：实际应用中可根据需求选择单轴或双轴控制，移动机器人通常优先控制水平偏差实现转向跟随。

• 控制策略： 将偏差转换为执行机构的控制指令（如速度、角度），常用比例控制（P 控制）：

  • 转向控制：angular_vel = Kp * dx （Kp 为比例系数，控制灵敏度）
  • 前进/后退控制：若需保持距离，可通过目标区域像素数 nPixCount 估算距离偏差，再计算线速度 linear_vel = Kp' * (nPixCount - target_count)。

1.2 坐标系映射关系

• 图像坐标系 → 控制量：图像中目标的位置偏差反映了真实世界中执行机构与目标的相对位姿关系。
  • 水平偏差 $dx$ 对应执行机构与目标的水平角度偏差，需通过转向运动消除；
  • 目标像素数 nPixCount 与目标距离成反比（距离越近，像素数越多），可间接反映距离偏差，通过线速度调整。
• 实时性要求：视觉处理和控制指令生成需在短时间内完成（通常 ≤ 100ms），避免因延迟导致跟随震荡或失焦。ROS 节点的回调机制和控制频率（如 loop_rate(30) 即 30Hz）需匹配相机帧率。

---

四、跟随功能代码实现片段

在回调函数中增加速度计算与发布逻辑：

    // ... (前文图像处理代码相同) ...

    if(nPixCount > 0)
    {
        nTargetX /= nPixCount; // 横坐标中点
        nTargetY /= nPixCount; // 纵坐标中点
        printf("颜色质心坐标( %d , %d )  点数 = %d\n", nTargetX, nTargetY, nPixCount);

        // 画坐标
        Point line_begin = Point(nTargetX-10, nTargetY); // 横坐标起始点
        Point line_end = Point(nTargetX+10, nTargetY);   // 横坐标终点
        line(imgOriginal, line_begin, line_end, Scalar(255,0,0), 3); // 画线
        
        line_begin.x = nTargetX; line_begin.y = nTargetY-10; // 纵坐标起始点
        line_end.x = nTargetX; line_end.y = nTargetY+10;     // 纵坐标终点
        line(imgOriginal, line_begin, line_end, Scalar(255,0,0), 3); // 画线

        // 计算机器人运动速度
        // 差值 * 比例系数
        float fVelFoward = (nImgHeight/2 - nTargetY) * 0.002;     
        float fVelTurn = (nImgWidth/2 - nTargetX) * 0.003;           

        vel_cmd.linear.x = fVelFoward;
        vel_cmd.linear.y = 0;
        vel_cmd.linear.z = 0;
        vel_cmd.angular.x = 0;
        vel_cmd.angular.y = 0;
        vel_cmd.angular.z = fVelTurn;
    }
    else
    {
        printf("目标颜色消失...\n");
        vel_cmd.linear.x = 0;
        vel_cmd.linear.y = 0;
        vel_cmd.linear.z = 0;
        vel_cmd.angular.x = 0;
        vel_cmd.angular.y = 0;
        vel_cmd.angular.z = 0;
    }

    // 显示处理结果
    imshow("RGB", imgOriginal);
    imshow("Result", imgThresholded);
    cv::waitKey(1);
    
    vel_pub.publish(vel_cmd);
    printf("机器人运动速度( linear.x= %.2f , angular.z= %.2f )\n", vel_cmd.linear.x, vel_cmd.angular.z);

---

五、完整跟随代码

#include <ros/docs.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <geometry_msgs/Twist.h>

using namespace cv;
using namespace std;

static int iLowH = 10;
static int iHighH = 40;
static int iLowS = 90; 
static int iHighS = 255;
static int iLowV = 1;
static int iHighV = 255;

geometry_msgs::Twist vel_cmd;   // 速度消息包
ros::Publisher vel_pub;         // 速度发送

void Cam_RGB_Callback(const sensor_msgs::Image msg)
{
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    Mat imgOriginal = cv_ptr->image;
    
    // 将RGB图片转换成HSV
    Mat imgHSV;
    vector<Mat> hsvSplit;
    cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV);
    
    // 在HSV空间做直方图均衡化
    split(imgHSV, hsvSplit);
    equalizeHist(hsvSplit[2], hsvSplit[2]);
    merge(hsvSplit, imgHSV);

    Mat imgThresholded;
    // 使用上面的Hue,Saturation和Value的阈值范围对图像进行二值化
    inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded); 
    
    // 开操作 (去除一些噪点)
    Mat element = getStructuringElement(MORPH_RECT, Size(5, 5));
    morphologyEx(imgThresholded, imgThresholded, MORPH_OPEN, element);
    
    // 闭操作 (连接一些连通域)
    morphologyEx(imgThresholded, imgThresholded, MORPH_CLOSE, element);

    // 遍历二值化后的图像数据
    int nTargetX = 0;
    int nTargetY = 0;
    int nPixCount = 0;
    int nImgWidth = imgThresholded.cols;
    int nImgHeight = imgThresholded.rows;
    int nImgChannels = imgThresholded.channels();
    
    printf("横向宽度= %d   纵向高度= %d \n", nImgWidth, nImgHeight);

    for (int y = 0; y < nImgHeight; y++)
    {
        for(int x = 0; x < nImgWidth; x++)
        {
            if(imgThresholded.data[y*nImgWidth + x] == 255)
            {
                nTargetX += x;
                nTargetY += y;
                nPixCount ++;
            }
        }
    }

    if(nPixCount > 0)
    {
        nTargetX /= nPixCount;
        nTargetY /= nPixCount;
        printf("颜色质心坐标( %d , %d )  点数 = %d\n", nTargetX, nTargetY, nPixCount);

        // 画坐标
        Point line_begin = Point(nTargetX-10, nTargetY);
        Point line_end = Point(nTargetX+10, nTargetY);
        line(imgOriginal, line_begin, line_end, Scalar(255,0,0), 3);
        
        line_begin.x = nTargetX; line_begin.y = nTargetY-10; 
        line_end.x = nTargetX; line_end.y = nTargetY+10; 
        line(imgOriginal, line_begin, line_end, Scalar(255,0,0), 3);

        // 计算机器人运动速度
        float fVelFoward = (nImgHeight/2 - nTargetY) * 0.002;     // 差值*比例
        float fVelTurn = (nImgWidth/2 - nTargetX) * 0.003;        // 差值*比例
        
        vel_cmd.linear.x = fVelFoward;
        vel_cmd.linear.y = 0;
        vel_cmd.linear.z = 0;
        vel_cmd.angular.x = 0;
        vel_cmd.angular.y = 0;
        vel_cmd.angular.z = fVelTurn;
    }
    else
    {
        printf("目标颜色消失...\n");
        vel_cmd.linear.x = 0;
        vel_cmd.linear.y = 0;
        vel_cmd.linear.z = 0;
        vel_cmd.angular.x = 0;
        vel_cmd.angular.y = 0;
        vel_cmd.angular.z = 0;
    }

    // 显示处理结果
    imshow("RGB", imgOriginal);
    imshow("Result", imgThresholded);
    cv::waitKey(1);
    
    vel_pub.publish(vel_cmd);
    printf("机器人运动速度( linear.x= %.2f , angular.z= %.2f )\n", vel_cmd.linear.x, vel_cmd.angular.z);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "demo_cv_follow");
   
    ros::NodeHandle nh;
    ros::Subscriber rgb_sub = nh.subscribe("kinect2/qhd/image_color_rect", 1, Cam_RGB_Callback);
    vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 30);
    
    ros::Rate loop_rate(30);

    // 生成图像显示和参数调节的窗口空间
    namedWindow("Threshold", WINDOW_AUTOSIZE);
    createTrackbar("LowH", "Threshold", &iLowH, 179); // Hue (0 - 179)
    createTrackbar("HighH", "Threshold", &iHighH, 179);
    createTrackbar("LowS", "Threshold", &iLowS, 255); // Saturation (0 - 255)
    createTrackbar("HighS", "Threshold", &iHighS, 255);
    createTrackbar("LowV", "Threshold", &iLowV, 255); // Value (0 - 255)
    createTrackbar("HighV", "Threshold", &iHighV, 255);

    namedWindow("RGB"); 
    namedWindow("Result"); 

    while(ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}