cpp
复制代码
#include <opencv2/opencv.hpp>
#include <vector>
#include <map>
#include <chrono>
#include <thread>
using namespace cv;
using namespace std;
// 机械臂通信类（伪代码，需替换为实际SDK，如Modbus TCP/EtherCAT）
class RobotArm {
public:
bool connect(string ip, int port) { /* 连接机械臂，返回是否成功 */ return true; }
bool moveToHeight(double target_height) { /* 移动到指定高度，单位mm */ return true; }
double getCurrentHeight() { /* 获取当前高度 */ return 100.0; }
bool isBelowLimit(double limit_height) { /* 判断是否低于最低限位 */ return getCurrentHeight() > limit_height; }
void stop() { /* 停止移动 */ }
};
// 相机通信类（伪代码，需替换为实际相机SDK，如GigE Vision）
class Camera2D {
public:
bool init(int roi_x, int roi_y, int roi_w, int roi_h) { /* 初始化相机+ROI */ return true; }
Mat captureImage() { /* 采集单帧图像，返回ROI区域 */ return imread("temp.jpg"); }
};
// 清晰度评估器（拉普拉斯方差法）
class SharpnessEvaluator {
public:
SharpnessEvaluator(double threshold) : vol_threshold(threshold) {}
double calculateVoL(const Mat& image) {
Mat gray, blur_gray, laplacian;
cvtColor(image, gray, COLOR_BGR2GRAY);
GaussianBlur(gray, blur_gray, Size(3,3), 0); // 去噪
Laplacian(blur_gray, laplacian, CV_64F);
Scalar mean, stddev;
meanStdDev(laplacian, mean, stddev);
return stddev.val[0] * stddev.val[0]; // 方差=标准差²
}
bool isSharp(double vol) { return vol >= vol_threshold; }
private:
double vol_threshold; // 清晰阈值
};
// 主流程函数
void focusPositionDetection() {
// 1. 初始化参数
const double START_HEIGHT = 100.0; // 起始高度（mm）
const double MIN_LIMIT_HEIGHT = 5.0; // 最低限位（mm）
const double COARSE_STEP = 5.0; // 粗扫步长（mm）
const double FINE_STEP = 1.0; // 精扫步长（mm）
const int STABILIZE_TIME = 100; // 稳定等待时间（ms）
const double SHARP_THRESHOLD = 100.0; // 清晰阈值（预校准）
const Rect ROI(100, 100, 400, 300); // 图像ROI（x,y,w,h）
// 2. 初始化硬件与算法
RobotArm robot;
Camera2D camera;
SharpnessEvaluator evaluator(SHARP_THRESHOLD);
if (!robot.connect("192.168.1.100", 502)) { // 机械臂IP+端口
printf("机械臂连接失败！\n");
return;
}
if (!camera.init(ROI.x, ROI.y, ROI.width, ROI.height)) {
printf("相机初始化失败！\n");
return;
}
// 3. 安全校验
if (!robot.isBelowLimit(MIN_LIMIT_HEIGHT)) {
printf("当前高度超出最低限位！\n");
robot.stop();
return;
}
// 4. 粗扫描：找峰值区间
map<double, double> coarse_data; // 粗扫数据：高度→VoL
double current_height = START_HEIGHT;
double last_vol = 0.0;
bool is_rising = true;
double peak_start = 0.0, peak_end = 0.0;
while (is_rising && robot.isBelowLimit(MIN_LIMIT_HEIGHT)) {
// 移动机械臂
if (!robot.moveToHeight(current_height)) {
printf("机械臂移动失败！\n");
robot.stop();
return;
}
this_thread::sleep_for(chrono::milliseconds(STABILIZE_TIME)); // 稳定等待
// 采集图像+计算清晰度
Mat img = camera.captureImage();
double vol = evaluator.calculateVoL(img);
coarse_data[current_height] = vol;
printf("粗扫：高度=%.1fmm，VoL=%.2f\n", current_height, vol);
// 趋势判断
if (vol < last_vol && last_vol > 0) {
// VoL首次下降，确定峰值区间
peak_start = current_height + COARSE_STEP;
peak_end = current_height;
is_rising = false;
} else {
last_vol = vol;
current_height -= COARSE_STEP; // 继续向下移动
}
}
// 粗扫异常：无峰值区间
if (is_rising) {
printf("粗扫未找到清晰峰值区间！\n");
robot.stop();
return;
}
// 5. 精扫描：精准定位最优高度
map<double, double> fine_data; // 精扫数据：高度→VoL
current_height = peak_start;
double max_vol = 0.0;
double best_height = 0.0;
while (current_height >= peak_end) {
// 移动机械臂
if (!robot.moveToHeight(current_height)) {
printf("机械臂精扫移动失败！\n");
robot.stop();
return;
}
this_thread::sleep_for(chrono::milliseconds(STABILIZE_TIME));
// 采集图像+计算清晰度
Mat img = camera.captureImage();
double vol = evaluator.calculateVoL(img);
fine_data[current_height] = vol;
printf("精扫：高度=%.1fmm，VoL=%.2f\n", current_height, vol);
// 记录最大值
if (vol > max_vol) {
max_vol = vol;
best_height = current_height;
}
current_height -= FINE_STEP; // 继续向下精扫
}
// 精扫异常：最优高度不清晰
if (!evaluator.isSharp(max_vol)) {
printf("最优高度VoL=%.2f < 阈值%.2f，无清晰位置！\n", max_vol, SHARP_THRESHOLD);
robot.stop();
return;
}
// 6. 验证并停止
robot.moveToHeight(best_height);
this_thread::sleep_for(chrono::milliseconds(STABILIZE_TIME));
// 连续采集3帧验证
double avg_vol = 0.0;
for (int i=0; i<3; i++) {
Mat img = camera.captureImage();
avg_vol += evaluator.calculateVoL(img);
}
avg_vol /= 3.0;
if (evaluator.isSharp(avg_vol)) {
printf("聚焦清晰！最优高度=%.1fmm，平均VoL=%.2f\n", best_height, avg_vol);
robot.stop();
} else {
printf("验证失败，重新粗扫描！\n");
focusPositionDetection(); // 重新执行流程（可限制重试次数）
}
}
int main() {
focusPositionDetection();
return 0;
}