深入理解现代摄像机聚焦与变焦系统：从光学原理到代码实现

前言

最近在做一个水下ROV的视觉系统，需要实现自动对焦和电动变焦功能。查了不少资料，发现网上讲这块的文章要么太理论化，要么代码不完整。干脆自己整理一篇，把光学原理和工程实现都讲清楚。

本文会从最基础的透镜成像讲起，逐步深入到现代摄像机的AF（自动对焦）和Zoom（变焦）系统实现，最后给出完整的控制代码。

一、光学成像基础

1.1 薄透镜成像公式

在讨论聚焦之前，必须先搞清楚透镜成像的基本规律。对于理想薄透镜，成像遵循高斯公式：

1f=1u+1v\frac{1}{f} = \frac{1}{u} + \frac{1}{v}f1​=u1​+v1​

其中：

• fff为透镜焦距
• uuu为物距（物体到透镜的距离）
• vvv为像距（像到透镜的距离）

这个公式看着简单，但它是整个聚焦系统的理论基础。当物体距离变化时，要想在固定位置的传感器上成清晰的像，就必须调整透镜位置或改变透镜组的等效焦距。

1.2 弥散圆与景深

实际成像中，点光源不可能成完美的点，而是会形成一个小圆斑，称为弥散圆（Circle of Confusion, CoC）。当弥散圆直径小于人眼分辨极限（通常取传感器像素尺寸的2-3倍）时，我们认为图像是清晰的。

景深（Depth of Field, DoF）就是基于弥散圆定义的：

DoF=2u2Ncf2f4−N2c2u2DoF = \frac{2u^2 N c f^2}{f^4 - N^2 c^2 u^2}DoF=f4−N2c2u22u2Ncf2​

简化后的近似公式：

DoFnear=u⋅HH+uDoF_{near} = \frac{u \cdot H}{H + u}DoFnear​=H+uu⋅H​
DoFfar=u⋅HH−uDoF_{far} = \frac{u \cdot H}{H - u}DoFfar​=H−uu⋅H​

其中H=f2N⋅cH = \frac{f^2}{N \cdot c}H=N⋅cf2​为超焦距，NNN为光圈F值，ccc为容许弥散圆直径。

/** * 景深计算模块 * 用于计算给定参数下的景深范围 */#include<math.h>typedefstruct{floatfocal_length;// 焦距 (mm)floataperture;// 光圈值 F/xfloatfocus_distance;// 对焦距离 (mm)floatcoc;// 容许弥散圆直径 (mm)}LensParams;typedefstruct{floatnear_limit;// 景深近界floatfar_limit;// 景深远界floattotal_dof;// 总景深floathyperfocal;// 超焦距}DOFResult;DOFResultcalculate_dof(constLensParams*params){DOFResult result;// 计算超焦距floatH=(params->focal_length*params->focal_length)/(params->aperture*params->coc);result.hyperfocal=H;floatu=params->focus_distance;// 景深近界result.near_limit=(u*(H-params->focal_length))/(H+u-2*params->focal_length);// 景深远界（可能为无穷大）if(u>=H){result.far_limit=INFINITY;}else{result.far_limit=(u*(H-params->focal_length))/(H-u);}// 总景深if(isinf(result.far_limit)){result.total_dof=INFINITY;}else{result.total_dof=result.far_limit-result.near_limit;}returnresult;}// 使用示例voiddof_example(void){LensParams lens={.focal_length=50.0f,// 50mm镜头.aperture=2.8f,// F2.8.focus_distance=3000.0f,// 对焦3米.coc=0.03f// 全画幅标准CoC};DOFResult dof=calculate_dof(&lens);printf("超焦距: %.2f m\n",dof.hyperfocal/1000);printf("景深范围: %.2f m - %.2f m\n",dof.near_limit/1000,dof.far_limit/1000);printf("总景深: %.2f m\n",dof.total_dof/1000);}

二、聚焦系统原理与实现

2.1 聚焦的本质

聚焦的核心任务就是调整光学系统，使目标物体成像在传感器平面上。根据成像公式，当物距uuu确定时，像距vvv也就确定了。现代摄像机实现聚焦的方式主要有两种：

1. 整组移动式：移动整个镜头组，改变镜头到传感器的距离
2. 内对焦式：移动镜头内部的对焦镜组，改变等效焦距

内对焦式是目前主流方案，优点是对焦速度快、镜头长度不变、密封性好。

2.2 对焦马达类型

常见的对焦马达有三种：

2.3 音圈马达(VCM)控制

VCM的工作原理是利用安培力。线圈在永磁体产生的磁场中通电，产生轴向推力，驱动镜片移动。推力公式：

F=BILF = BILF=BIL

其中BBB为磁感应强度，III为电流，LLL为线圈有效长度。

VCM的控制核心是位置伺服。通常采用霍尔传感器检测镜片位置，然后用PID控制器调节驱动电流。

/** * VCM音圈马达控制器 * 采用位置式PID控制 */#include<stdint.h>#include<stdbool.h>#defineVCM_POS_MIN0#defineVCM_POS_MAX1023#defineVCM_DAC_BITS10typedefstruct{// PID参数floatkp;floatki;floatkd;// 状态变量floatintegral;floatprev_error;floatoutput;// 限幅floatintegral_max;floatoutput_max;}PIDController;typedefstruct{PIDController pid;uint16_tcurrent_pos;uint16_ttarget_pos;bool is_moving;// 硬件接口函数指针void(*set_dac)(uint16_tvalue);uint16_t(*read_hall)(void);}VCMController;voidpid_init(PIDController*pid,floatkp,floatki,floatkd){pid->kp=kp;pid->ki=ki;pid->kd=kd;pid->integral=0;pid->prev_error=0;pid->output=0;pid->integral_max=500.0f;pid->output_max=1023.0f;}floatpid_compute(PIDController*pid,floaterror){// 比例项floatp_term=pid->kp*error;// 积分项（带抗饱和）pid->integral+=error;if(pid->integral>pid->integral_max){pid->integral=pid->integral_max;}elseif(pid->integral<-pid->integral_max){pid->integral=-pid->integral_max;}floati_term=pid->ki*pid->integral;// 微分项floatd_term=pid->kd*(error-pid->prev_error);pid->prev_error=error;// 输出限幅pid->output=p_term+i_term+d_term;if(pid->output>pid->output_max){pid->output=pid->output_max;}elseif(pid->output<-pid->output_max){pid->output=-pid->output_max;}returnpid->output;}voidvcm_init(VCMController*vcm,void(*set_dac)(uint16_t),uint16_t(*read_hall)(void)){pid_init(&vcm->pid,0.8f,0.05f,0.1f);vcm->set_dac=set_dac;vcm->read_hall=read_hall;vcm->current_pos=read_hall();vcm->target_pos=vcm->current_pos;vcm->is_moving=false;}voidvcm_set_position(VCMController*vcm,uint16_ttarget){if(target>VCM_POS_MAX)target=VCM_POS_MAX;vcm->target_pos=target;vcm->is_moving=true;}/** * VCM控制主循环，需要定时调用（建议1kHz） * 返回值：true表示仍在移动，false表示到达目标位置 */boolvcm_update(VCMController*vcm){// 读取当前位置vcm->current_pos=vcm->read_hall();// 计算误差floaterror=(float)(vcm->target_pos-vcm->current_pos);// 到位判断（死区）if(fabsf(error)<3.0f){vcm->is_moving=false;returnfalse;}// PID计算floatoutput=pid_compute(&vcm->pid,error);// 转换为DAC值并输出// VCM通常是双向驱动，需要中点偏置uint16_tdac_value=(uint16_t)(512+output);if(dac_value>VCM_POS_MAX)dac_value=VCM_POS_MAX;vcm->set_dac(dac_value);returntrue;}// 获取当前位置的距离映射（需要标定）floatvcm_pos_to_distance(uint16_tpos,float*calib_table,inttable_size){// 简单线性插值floatratio=(float)pos/VCM_POS_MAX;intidx=(int)(ratio*(table_size-1));if(idx>=table_size-1)idx=table_size-2;floatt=ratio*(table_size-1)-idx;returncalib_table[idx]*(1-t)+calib_table[idx+1]*t;}

2.4 自动对焦算法

现代摄像机的自动对焦(AF)主要分为两大类：

1. 对比度检测AF（CDAF）

通过分析图像的对比度/清晰度来判断是否合焦。原理是当图像清晰时，边缘锐利，高频分量多，对比度高。

常用的清晰度评价函数：

• Laplacian方差
• Tenengrad梯度
• Brenner梯度
• 高频能量比

/** * 图像清晰度评价函数集合 * 用于对比度检测自动对焦 */#include<stdint.h>#include<math.h>// 图像结构定义typedefstruct{uint8_t*data;intwidth;intheight;intstride;}GrayImage;/** * Laplacian方差法 * 计算图像的拉普拉斯算子响应的方差 */doublefocus_laplacian_var(constGrayImage*img){int64_tsum=0;int64_tsum_sq=0;intcount=0;// 3x3 Laplacian核// [0 1 0]// [1 -4 1]// [0 1 0]for(inty=1;y<img->height-1;y++){for(intx=1;x<img->width-1;x++){intlap=-4*img->data[y*img->stride+x]+img->data[(y-1)*img->stride+x]+img->data[(y+1)*img->stride+x]+img->data[y*img->stride+x-1]+img->data[y*img->stride+x+1];sum+=lap;sum_sq+=(int64_t)lap*lap;count++;}}doublemean=(double)sum/count;doublevariance=(double)sum_sq/count-mean*mean;returnvariance;}/** * Tenengrad梯度法 * 基于Sobel算子计算梯度幅值 */doublefocus_tenengrad(constGrayImage*img,intthreshold){doublesum=0;for(inty=1;y<img->height-1;y++){for(intx=1;x<img->width-1;x++){// Sobel X方向intgx=-img->data[(y-1)*img->stride+x-1]+img->data[(y-1)*img->stride+x+1]-2*img->data[y*img->stride+x-1]+2*img->data[y*img->stride+x+1]-img->data[(y+1)*img->stride+x-1]+img->data[(y+1)*img->stride+x+1];// Sobel Y方向intgy=-img->data[(y-1)*img->stride+x-1]-2*img->data[(y-1)*img->stride+x]-img->data[(y-1)*img->stride+x+1]+img->data[(y+1)*img->stride+x-1]+2*img->data[(y+1)*img->stride+x]+img->data[(y+1)*img->stride+x+1];intgrad_sq=gx*gx+gy*gy;if(grad_sq>threshold*threshold){sum+=grad_sq;}}}returnsum;}/** * Brenner梯度法 * 计算水平方向上间隔一个像素的差分 */doublefocus_brenner(constGrayImage*img){doublesum=0;for(inty=0;y<img->height;y++){for(intx=0;x<img->width-2;x++){intdiff=img->data[y*img->stride+x+2]-img->data[y*img->stride+x];sum+=(double)diff*diff;}}returnsum;}/** * 归一化方差法 * 对光照变化有更好的鲁棒性 */doublefocus_normalized_var(constGrayImage*img){int64_tsum=0;intcount=img->width*img->height;// 计算均值for(inty=0;y<img->height;y++){for(intx=0;x<img->width;x++){sum+=img->data[y*img->stride+x];}}doublemean=(double)sum/count;// 计算归一化方差doublevar_sum=0;for(inty=0;y<img->height;y++){for(intx=0;x<img->width;x++){doublediff=img->data[y*img->stride+x]-mean;var_sum+=diff*diff;}}// 归一化：除以均值return(var_sum/count)/(mean+1e-6);}

2. 相位检测AF（PDAF）

通过分析来自透镜不同区域的光线相位差来直接计算离焦量和方向。速度比CDAF快得多，因为它能一次判断出对焦方向和距离，不需要反复搜索。

/** * 相位检测AF简化模型 * 实际PDAF需要专用传感器支持 */typedefstruct{int16_t*left_pixels;// 左遮蔽像素数据int16_t*right_pixels;// 右遮蔽像素数据intnum_pixels;intbaseline;// 基线距离（像素）}PDAFData;typedefstruct{floatphase_diff;// 相位差（像素）floatconfidence;// 置信度floatdefocus_amount;// 离焦量（与标定相关）}PDAFResult;/** * 相位差计算 - SAD匹配法 * 通过滑动窗口找最小SAD位置来确定相位差 */PDAFResultpdaf_compute_phase(constPDAFData*data,intsearch_range){PDAFResult result={0};intmin_sad=INT32_MAX;intbest_shift=0;// 遍历搜索范围for(intshift=-search_range;shift<=search_range;shift++){intsad=0;intvalid_count=0;for(inti=0;i<data->num_pixels;i++){intj=i+shift;if(j>=0&&j<data->num_pixels){sad+=abs(data->left_pixels[i]-data->right_pixels[j]);valid_count++;}}if(valid_count>0){sad=sad/valid_count;// 归一化if(sad<min_sad){min_sad=sad;best_shift=shift;}}}// 亚像素精度优化（抛物线拟合）if(best_shift>-search_range&&best_shift<search_range){// 计算相邻位置的SADintsad_left=0,sad_right=0;intcount=0;for(inti=0;i<data->num_pixels;i++){intjl=i+best_shift-1;intjr=i+best_shift+1;if(jl>=0&&jr<data->num_pixels){sad_left+=abs(data->left_pixels[i]-data->right_pixels[jl]);sad_right+=abs(data->left_pixels[i]-data->right_pixels[jr]);count++;}}sad_left/=count;sad_right/=count;// 抛物线插值floatdenom=2.0f*(sad_left+sad_right-2*min_sad);if(fabsf(denom)>1e-6f){floatsub_shift=(float)(sad_left-sad_right)/denom;result.phase_diff=best_shift+sub_shift;}else{result.phase_diff=best_shift;}}else{result.phase_diff=best_shift;}// 计算置信度（基于SAD最小值）result.confidence=1.0f/(1.0f+min_sad/100.0f);// 离焦量计算（需要标定系数）// defocus = phase_diff * k，k与镜头参数相关result.defocus_amount=result.phase_diff*2.5f;// 示例系数returnresult;}

2.5 爬山搜索算法

CDAF需要一个搜索策略来找到最佳对焦位置。经典的爬山算法虽然简单，但容易陷入局部最优。实际应用中通常采用改进的策略：

/** * 自适应爬山对焦算法 * 支持粗搜+细搜两阶段 */typedefenum{AF_STATE_IDLE,AF_STATE_COARSE_SEARCH,AF_STATE_FINE_SEARCH,AF_STATE_FOCUSED,AF_STATE_FAILED}AFState;typedefstruct{AFState state;// 搜索参数intcoarse_step;intfine_step;intsearch_range;// 当前状态intcurrent_pos;intsearch_dir;// 1或-1// 评价值记录doubleprev_focus_val;doublepeak_focus_val;intpeak_position;// 下降计数（用于检测过峰）intdecline_count;// 硬件接口VCMController*vcm;double(*evaluate_focus)(void);}AutoFocusController;voidaf_init(AutoFocusController*af,VCMController*vcm,double(*evaluate)(void)){af->state=AF_STATE_IDLE;af->coarse_step=50;af->fine_step=5;af->search_range=900;af->vcm=vcm;af->evaluate_focus=evaluate;af->decline_count=0;}voidaf_start(AutoFocusController*af){af->state=AF_STATE_COARSE_SEARCH;af->current_pos=VCM_POS_MAX/2;// 从中间开始af->search_dir=1;af->prev_focus_val=0;af->peak_focus_val=0;af->peak_position=af->current_pos;af->decline_count=0;vcm_set_position(af->vcm,af->current_pos);}/** * 自动对焦状态机，需要周期性调用 * 返回当前状态 */AFStateaf_update(AutoFocusController*af){// 等待VCM到位if(vcm_update(af->vcm)){returnaf->state;// 仍在移动，继续等待}// 延迟一帧让图像稳定staticintsettle_count=0;if(settle_count<2){settle_count++;returnaf->state;}settle_count=0;// 获取当前对焦评价值doublefocus_val=af->evaluate_focus();switch(af->state){caseAF_STATE_COARSE_SEARCH:if(focus_val>af->peak_focus_val){// 找到更好的位置af->peak_focus_val=focus_val;af->peak_position=af->current_pos;af->decline_count=0;}elseif(focus_val<af->prev_focus_val*0.95){// 明显下降af->decline_count++;}af->prev_focus_val=focus_val;// 判断是否过峰if(af->decline_count>=3){// 进入细搜阶段af->state=AF_STATE_FINE_SEARCH;af->current_pos=af->peak_position-af->coarse_step;af->search_dir=1;af->peak_focus_val=0;af->decline_count=0;}else{// 继续粗搜af->current_pos+=af->search_dir*af->coarse_step;// 边界检测if(af->current_pos>VCM_POS_MAX||af->current_pos<VCM_POS_MIN){// 反向搜索af->search_dir=-af->search_dir;af->current_pos+=af->search_dir*af->coarse_step*2;}}vcm_set_position(af->vcm,af->current_pos);break;caseAF_STATE_FINE_SEARCH:if(focus_val>af->peak_focus_val){af->peak_focus_val=focus_val;af->peak_position=af->current_pos;af->decline_count=0;}else{af->decline_count++;}// 细搜完成条件if(af->decline_count>=2){// 移动到最佳位置vcm_set_position(af->vcm,af->peak_position);af->state=AF_STATE_FOCUSED;}else{af->current_pos+=af->search_dir*af->fine_step;vcm_set_position(af->vcm,af->current_pos);}break;caseAF_STATE_FOCUSED:// 已对焦，可以在这里监测焦点漂移break;default:break;}returnaf->state;}

三、变焦系统原理与实现

3.1 光学变焦原理

变焦镜头通过移动内部透镜组来改变系统的等效焦距。一个典型的变焦镜头包含：

1. 前固定组：负责接收景物光线
2. 变倍组：移动以改变放大倍率
3. 补偿组：移动以补偿像面位置，保持成像清晰
4. 后固定组：将光线聚焦到传感器

变倍组和补偿组的运动轨迹是非线性的，需要精确的凸轮曲线设计或电子补偿。

3.2 变焦与对焦的耦合

这是变焦镜头设计中的一个关键问题。当焦距改变时，对焦位置也会变化（所谓的"呼吸效应"）。解决方案：

1. 机械补偿：通过凸轮曲线将变倍和补偿动作机械耦合
2. 电子补偿：实时计算补偿量，用马达分别控制

/** * 变焦补偿计算 * 基于标定表进行插值 */#defineZOOM_CALIB_POINTS16#defineFOCUS_CALIB_POINTS8typedefstruct{// 变焦位置uint16_tzoom_pos;// 该变焦位置下的对焦补偿曲线// focus_offset[i] = 在focus_distance[i]距离时需要的对焦位置偏移floatfocus_distances[FOCUS_CALIB_POINTS];floatfocus_offsets[FOCUS_CALIB_POINTS];}ZoomCalibPoint;typedefstruct{ZoomCalibPoint points[ZOOM_CALIB_POINTS];intnum_points;}ZoomFocusCalibration;/** * 计算变焦补偿后的对焦位置 */uint16_tzoom_compensate_focus(constZoomFocusCalibration*calib,uint16_tzoom_pos,uint16_tbase_focus_pos,floatfocus_distance){// 找到相邻的变焦标定点intidx1=0,idx2=0;for(inti=0;i<calib->num_points-1;i++){if(zoom_pos>=calib->points[i].zoom_pos&&zoom_pos<calib->points[i+1].zoom_pos){idx1=i;idx2=i+1;break;}}// 变焦位置插值系数floatzoom_t=(float)(zoom_pos-calib->points[idx1].zoom_pos)/(calib->points[idx2].zoom_pos-calib->points[idx1].zoom_pos);// 在每个变焦标定点，根据对焦距离插值获得偏移量floatoffset1=interpolate_focus_offset(&calib->points[idx1],focus_distance);floatoffset2=interpolate_focus_offset(&calib->points[idx2],focus_distance);// 最终偏移量floatoffset=offset1*(1-zoom_t)+offset2*zoom_t;// 应用补偿intcompensated=(int)base_focus_pos+(int)offset;if(compensated<VCM_POS_MIN)compensated=VCM_POS_MIN;if(compensated>VCM_POS_MAX)compensated=VCM_POS_MAX;return(uint16_t)compensated;}floatinterpolate_focus_offset(constZoomCalibPoint*point,floatdistance){// 找到相邻的对焦距离标定点for(inti=0;i<FOCUS_CALIB_POINTS-1;i++){if(distance>=point->focus_distances[i]&&distance<point->focus_distances[i+1]){floatt=(distance-point->focus_distances[i])/(point->focus_distances[i+1]-point->focus_distances[i]);returnpoint->focus_offsets[i]*(1-t)+point->focus_offsets[i+1]*t;}}// 超出范围，使用边界值if(distance<point->focus_distances[0]){returnpoint->focus_offsets[0];}returnpoint->focus_offsets[FOCUS_CALIB_POINTS-1];}

3.3 电动变焦控制

现代摄像机的变焦马达通常是步进电机或直流电机配合光电编码器。这里给出一个完整的步进电机变焦控制实现：

/** * 步进电机变焦控制器 * 支持加减速曲线、位置反馈、软限位 */#include<stdint.h>#include<stdbool.h>// 步进电机微步设置#defineMICROSTEP_DIV16#defineSTEPS_PER_REV(200*MICROSTEP_DIV)typedefenum{ZOOM_STATE_IDLE,ZOOM_STATE_ACCELERATING,ZOOM_STATE_CONSTANT,ZOOM_STATE_DECELERATING,ZOOM_STATE_HOMING}ZoomState;typedefstruct{// 位置（微步数）int32_tcurrent_pos;int32_ttarget_pos;int32_tmin_pos;int32_tmax_pos;// 速度控制（微步/秒）floatcurrent_speed;floattarget_speed;floatmax_speed;floatmin_speed;floatacceleration;// 状态ZoomState state;intdirection;// 1或-1// 定时器相关uint32_tstep_interval_us;uint32_tlast_step_time;// 硬件接口void(*step_pulse)(void);void(*set_direction)(intdir);bool(*read_home_sensor)(void);int(*read_encoder)(void);// 可选的光电编码器}ZoomController;voidzoom_init(ZoomController*zoom){zoom->current_pos=0;zoom->target_pos=0;zoom->min_pos=0;zoom->max_pos=STEPS_PER_REV*10;// 假设10圈行程zoom->current_speed=0;zoom->min_speed=100;zoom->max_speed=3000;zoom->acceleration=5000;zoom->state=ZOOM_STATE_IDLE;}voidzoom_home(ZoomController*zoom){zoom->state=ZOOM_STATE_HOMING;zoom->direction=-1;zoom->set_direction(-1);zoom->current_speed=zoom->min_speed;}voidzoom_goto(ZoomController*zoom,int32_ttarget){// 限位if(target<zoom->min_pos)target=zoom->min_pos;if(target>zoom->max_pos)target=zoom->max_pos;zoom->target_pos=target;// 计算方向if(target>zoom->current_pos){zoom->direction=1;}elseif(target<zoom->current_pos){zoom->direction=-1;}else{return;// 已在目标位置}zoom->set_direction(zoom->direction);zoom->state=ZOOM_STATE_ACCELERATING;zoom->current_speed=zoom->min_speed;}/** * 计算梯形加减速曲线 * 返回当前应该的速度 */staticfloatcalc_speed_profile(ZoomController*zoom){int32_tremaining=(zoom->target_pos-zoom->current_pos)*zoom->direction;if(remaining<=0){return0;// 到达目标}// 计算减速所需的距离// v^2 = v0^2 + 2*a*s => s = v^2 / (2*a)floatdecel_distance=(zoom->current_speed*zoom->current_speed)/(2*zoom->acceleration);if(remaining<=decel_distance){// 需要开始减速zoom->state=ZOOM_STATE_DECELERATING;floatnew_speed=zoom->current_speed-zoom->acceleration*(zoom->step_interval_us/1e6f);if(new_speed<zoom->min_speed)new_speed=zoom->min_speed;returnnew_speed;}elseif(zoom->current_speed<zoom->max_speed){// 加速阶段zoom->state=ZOOM_STATE_ACCELERATING;floatnew_speed=zoom->current_speed+zoom->acceleration*(zoom->step_interval_us/1e6f);if(new_speed>zoom->max_speed)new_speed=zoom->max_speed;returnnew_speed;}else{// 匀速阶段zoom->state=ZOOM_STATE_CONSTANT;returnzoom->max_speed;}}/** * 变焦控制主循环 * 需要在高优先级定时器中断或高频循环中调用 */voidzoom_update(ZoomController*zoom,uint32_tcurrent_time_us){if(zoom->state==ZOOM_STATE_IDLE){return;}// 归零模式if(zoom->state==ZOOM_STATE_HOMING){if(zoom->read_home_sensor()){zoom->current_pos=0;zoom->state=ZOOM_STATE_IDLE;return;}// 按固定速度归零if(current_time_us-zoom->last_step_time>=zoom->step_interval_us){zoom->step_pulse();zoom->current_pos+=zoom->direction;zoom->last_step_time=current_time_us;}return;}// 正常移动模式zoom->current_speed=calc_speed_profile(zoom);if(zoom->current_speed<zoom->min_speed){// 到达目标zoom->state=ZOOM_STATE_IDLE;zoom->current_pos=zoom->target_pos;return;}// 计算步进间隔zoom->step_interval_us=(uint32_t)(1e6f/zoom->current_speed);// 执行步进if(current_time_us-zoom->last_step_time>=zoom->step_interval_us){zoom->step_pulse();zoom->current_pos+=zoom->direction;zoom->last_step_time=current_time_us;}}// 将步进位置转换为焦距(mm)floatzoom_pos_to_focal_length(ZoomController*zoom,float*fl_table,inttable_size){floatratio=(float)zoom->current_pos/zoom->max_pos;intidx=(int)(ratio*(table_size-1));if(idx>=table_size-1)idx=table_size-2;floatt=ratio*(table_size-1)-idx;returnfl_table[idx]*(1-t)+fl_table[idx+1]*t;}

四、综合应用：一个完整的摄像机控制系统

把前面的模块整合起来，实现一个完整的摄像机镜头控制系统：

/** * 摄像机镜头控制系统 * 整合AF、Zoom、光圈控制 */#include<stdint.h>#include<stdbool.h>typedefstruct{// 子系统VCMController focus_motor;ZoomController zoom_motor;AutoFocusController af;// 状态bool af_enabled;bool zoom_tracking;// 变焦跟焦使能floatcurrent_focus_distance;// 标定数据ZoomFocusCalibration zoom_calib;floatfocus_distance_table[32];floatfocal_length_table[32];}CameraLensController;/** * 初始化摄像机控制系统 */voidcamera_init(CameraLensController*cam){// 初始化对焦马达vcm_init(&cam->focus_motor,hal_vcm_set_dac,hal_vcm_read_hall);// 初始化变焦马达zoom_init(&cam->zoom_motor);cam->zoom_motor.step_pulse=hal_zoom_step;cam->zoom_motor.set_direction=hal_zoom_dir;cam->zoom_motor.read_home_sensor=hal_zoom_home_sensor;// 初始化自动对焦af_init(&cam->af,&cam->focus_motor,image_calc_focus_value);cam->af_enabled=false;cam->zoom_tracking=true;}/** * 开始自动对焦 */voidcamera_start_af(CameraLensController*cam){cam->af_enabled=true;af_start(&cam->af);}/** * 手动设置对焦位置 */voidcamera_set_focus(CameraLensController*cam,uint16_tpos){cam->af_enabled=false;// 如果开启了变焦跟焦，需要补偿if(cam->zoom_tracking){pos=zoom_compensate_focus(&cam->zoom_calib,cam->zoom_motor.current_pos,pos,cam->current_focus_distance);}vcm_set_position(&cam->focus_motor,pos);}/** * 设置变焦位置 */voidcamera_set_zoom(CameraLensController*cam,int32_tpos){uint16_told_focus=cam->focus_motor.current_pos;zoom_goto(&cam->zoom_motor,pos);// 变焦跟焦补偿if(cam->zoom_tracking&&!cam->af_enabled){// 预测变焦后的对焦位置uint16_tnew_focus=zoom_compensate_focus(&cam->zoom_calib,pos,old_focus,cam->current_focus_distance);vcm_set_position(&cam->focus_motor,new_focus);}}/** * 主控制循环 * 建议以1kHz频率调用 */voidcamera_update(CameraLensController*cam,uint32_ttime_us){// 更新变焦电机zoom_update(&cam->zoom_motor,time_us);// 更新对焦电机/自动对焦if(cam->af_enabled){AFState state=af_update(&cam->af);if(state==AF_STATE_FOCUSED){cam->af_enabled=false;// 记录对焦距离（用于变焦跟焦）cam->current_focus_distance=vcm_pos_to_distance(cam->focus_motor.current_pos,cam->focus_distance_table,32);}}else{vcm_update(&cam->focus_motor);}}/** * 获取当前镜头状态 */voidcamera_get_status(CameraLensController*cam,float*focal_length,float*focus_distance,bool*is_focused){*focal_length=zoom_pos_to_focal_length(&cam->zoom_motor,cam->focal_length_table,32);*focus_distance=vcm_pos_to_distance(cam->focus_motor.current_pos,cam->focus_distance_table,32);*is_focused=(cam->af.state==AF_STATE_FOCUSED)||(!cam->af_enabled&&!cam->focus_motor.is_moving);}

协议层：VISCA/Pelco-D控制

专业摄像机通常支持VISCA或Pelco-D协议进行远程控制。这里给出VISCA协议的对焦变焦命令解析：

/** * VISCA协议解析器（部分） * 针对对焦/变焦控制 */#defineVISCA_HEADER0x81#defineVISCA_TERMINATOR0xFF// 命令类别#defineVISCA_CAT_INTERFACE0x00#defineVISCA_CAT_CAMERA0x04#defineVISCA_CAT_PAN_TILT0x06// 摄像机命令#defineVISCA_CMD_ZOOM0x07#defineVISCA_CMD_FOCUS0x08#defineVISCA_CMD_AF_MODE0x38#defineVISCA_CMD_ZOOM_DIRECT0x47#defineVISCA_CMD_FOCUS_DIRECT0x48typedefstruct{uint8_tbuffer[16];intlength;}VISCAPacket;typedefenum{VISCA_ZOOM_STOP=0,VISCA_ZOOM_TELE,// 长焦方向VISCA_ZOOM_WIDE// 广角方向}VISCAZoomAction;typedefenum{VISCA_FOCUS_STOP=0,VISCA_FOCUS_FAR,VISCA_FOCUS_NEAR}VISCAFocusAction;/** * 解析VISCA数据包 */boolvisca_parse(constuint8_t*data,intlen,VISCAPacket*pkt){if(len<3)returnfalse;if(data[0]!=VISCA_HEADER)returnfalse;// 查找结束符intend=-1;for(inti=1;i<len;i++){if(data[i]==VISCA_TERMINATOR){end=i;break;}}if(end<0)returnfalse;pkt->length=end+1;memcpy(pkt->buffer,data,pkt->length);returntrue;}/** * 处理VISCA命令 */voidvisca_process_command(CameraLensController*cam,constVISCAPacket*pkt){if(pkt->length<4)return;uint8_tcategory=pkt->buffer[1]&0x0F;if(category==VISCA_CAT_CAMERA){uint8_tcmd=pkt->buffer[2];switch(cmd){caseVISCA_CMD_ZOOM:// 变焦速度控制: 8x 01 04 07 Vp FF{uint8_taction=pkt->buffer[3]&0x0F;uint8_tspeed=(pkt->buffer[3]>>4)&0x07;if(action==0x00){// 停止cam->zoom_motor.target_pos=cam->zoom_motor.current_pos;cam->zoom_motor.state=ZOOM_STATE_IDLE;}elseif(action==0x02){// 长焦zoom_goto(&cam->zoom_motor,cam->zoom_motor.max_pos);cam->zoom_motor.max_speed=500+speed*400;}elseif(action==0x03){// 广角zoom_goto(&cam->zoom_motor,cam->zoom_motor.min_pos);cam->zoom_motor.max_speed=500+speed*400;}}break;caseVISCA_CMD_ZOOM_DIRECT:// 变焦直接位置: 81 01 04 47 0p 0q 0r 0s FF// 位置 = pqrs (16位){uint16_tpos=((pkt->buffer[3]&0x0F)<<12)|((pkt->buffer[4]&0x0F)<<8)|((pkt->buffer[5]&0x0F)<<4)|(pkt->buffer[6]&0x0F);int32_ttarget=(pos*cam->zoom_motor.max_pos)/0xFFFF;zoom_goto(&cam->zoom_motor,target);}break;caseVISCA_CMD_FOCUS:// 手动对焦: 81 01 04 08 Vp FF{cam->af_enabled=false;uint8_taction=pkt->buffer[3]&0x0F;uint8_tspeed=(pkt->buffer[3]>>4)&0x07;intstep=10+speed*20;if(action==0x00){// 停止}elseif(action==0x02){// 远焦intpos=cam->focus_motor.current_pos+step;if(pos>VCM_POS_MAX)pos=VCM_POS_MAX;vcm_set_position(&cam->focus_motor,pos);}elseif(action==0x03){// 近焦intpos=cam->focus_motor.current_pos-step;if(pos<VCM_POS_MIN)pos=VCM_POS_MIN;vcm_set_position(&cam->focus_motor,pos);}}break;caseVISCA_CMD_FOCUS_DIRECT:// 对焦直接位置{cam->af_enabled=false;uint16_tpos=((pkt->buffer[3]&0x0F)<<12)|((pkt->buffer[4]&0x0F)<<8)|((pkt->buffer[5]&0x0F)<<4)|(pkt->buffer[6]&0x0F);uint16_ttarget=(pos*VCM_POS_MAX)/0xFFFF;vcm_set_position(&cam->focus_motor,target);}break;caseVISCA_CMD_AF_MODE:// 自动对焦模式: 81 01 04 38 0p FF{uint8_tmode=pkt->buffer[3];if(mode==0x02){// AF开camera_start_af(cam);}elseif(mode==0x03){// MFcam->af_enabled=false;}elseif(mode==0x04){// 单次AFcamera_start_af(cam);}}break;}}}/** * 生成VISCA查询响应 */intvisca_build_zoom_pos_response(constCameraLensController*cam,uint8_t*out){// 响应格式: 90 50 0p 0q 0r 0s FFuint16_tpos=(uint16_t)((float)cam->zoom_motor.current_pos/cam->zoom_motor.max_pos*0xFFFF);out[0]=0x90;out[1]=0x50;out[2]=(pos>>12)&0x0F;out[3]=(pos>>8)&0x0F;out[4]=(pos>>4)&0x0F;out[5]=pos&0x0F;out[6]=0xFF;return7;}

五、图像处理辅助：基于OpenCV的对焦评价

前面的嵌入式代码更适合资源受限的场景。如果是在Linux或Windows平台，可以用OpenCV实现更强大的对焦评价：

/** * OpenCV对焦评价工具 * C++实现，支持多种评价算法 */#include<opencv2/opencv.hpp>#include<vector>classFocusEvaluator{public:enumMethod{LAPLACIAN_VAR,TENENGRAD,SOBEL_VAR,MODIFIED_LAPLACIAN,FFT_HIGH_FREQ};// 设置ROI区域voidsetROI(constcv::Rect&roi){roi_=roi;use_roi_=true;}voidclearROI(){use_roi_=false;}// 计算对焦值doubleevaluate(constcv::Mat&frame,Method method=LAPLACIAN_VAR){cv::Mat gray;if(frame.channels()==3){cv::cvtColor(frame,gray,cv::COLOR_BGR2GRAY);}else{gray=frame;}// 应用ROIif(use_roi_&&!roi_.empty()){gray=gray(roi_);}switch(method){caseLAPLACIAN_VAR:returnlaplacianVariance(gray);caseTENENGRAD:returntenengrad(gray);caseSOBEL_VAR:returnsobelVariance(gray);caseMODIFIED_LAPLACIAN:returnmodifiedLaplacian(gray);caseFFT_HIGH_FREQ:returnfftHighFreq(gray);default:returnlaplacianVariance(gray);}}private:// Laplacian方差doublelaplacianVariance(constcv::Mat&gray){cv::Mat lap;cv::Laplacian(gray,lap,CV_64F);cv::Scalar mean,stddev;cv::meanStdDev(lap,mean,stddev);returnstddev[0]*stddev[0];}// Tenengrad梯度doubletenengrad(constcv::Mat&gray,intthreshold=0){cv::Mat gx,gy;cv::Sobel(gray,gx,CV_64F,1,0,3);cv::Sobel(gray,gy,CV_64F,0,1,3);cv::Mat fm;cv::magnitude(gx,gy,fm);// 阈值过滤if(threshold>0){cv::threshold(fm,fm,threshold,0,cv::THRESH_TOZERO);}returncv::mean(fm.mul(fm))[0];}// Sobel方差doublesobelVariance(constcv::Mat&gray){cv::Mat gx,gy;cv::Sobel(gray,gx,CV_64F,1,0,3);cv::Sobel(gray,gy,CV_64F,0,1,3);cv::Mat grad=cv::abs(gx)+cv::abs(gy);cv::Scalar mean,stddev;cv::meanStdDev(grad,mean,stddev);returnstddev[0]*stddev[0];}// 修正的LaplaciandoublemodifiedLaplacian(constcv::Mat&gray){cv::Mat M=(cv::Mat_<double>(3,1)<<-1,2,-1);cv::Mat Lx,Ly;cv::filter2D(gray,Lx,CV_64F,M);cv::filter2D(gray,Ly,CV_64F,M.t());cv::Mat FM=cv::abs(Lx)+cv::abs(Ly);returncv::sum(FM)[0];}// FFT高频分量doublefftHighFreq(constcv::Mat&gray){cv::Mat padded;intm=cv::getOptimalDFTSize(gray.rows);intn=cv::getOptimalDFTSize(gray.cols);cv::copyMakeBorder(gray,padded,0,m-gray.rows,0,n-gray.cols,cv::BORDER_CONSTANT,cv::Scalar::all(0));cv::Mat planes[]={cv::Mat_<float>(padded),cv::Mat::zeros(padded.size(),CV_32F)};cv::Mat complex;cv::merge(planes,2,complex);cv::dft(complex,complex);cv::split(complex,planes);cv::Mat mag;cv::magnitude(planes[0],planes[1],mag);// 中心化intcx=mag.cols/2;intcy=mag.rows/2;cv::Matq0(mag,cv::Rect(0,0,cx,cy));cv::Matq1(mag,cv::Rect(cx,0,cx,cy));cv::Matq2(mag,cv::Rect(0,cy,cx,cy));cv::Matq3(mag,cv::Rect(cx,cy,cx,cy));cv::Mat tmp;q0.copyTo(tmp);q3.copyTo(q0);tmp.copyTo(q3);q1.copyTo(tmp);q2.copyTo(q1);tmp.copyTo(q2);// 高通滤波：抑制低频intradius=std::min(cx,cy)/4;cv::Mat mask=cv::Mat::ones(mag.size(),CV_32F);cv::circle(mask,cv::Point(cx,cy),radius,cv::Scalar(0),-1);cv::Mat filtered;cv::multiply(mag,mask,filtered);returncv::sum(filtered)[0]/(mag.rows*mag.cols);}cv::Rect roi_;booluse_roi_=false;};/** * 自动对焦控制器（OpenCV版） */classAutoFocusCV{public:AutoFocusCV(intmin_pos,intmax_pos):min_pos_(min_pos),max_pos_(max_pos){}// 粗搜+细搜intfindBestFocus(std::function<cv::Mat(int)>capture_at_pos,intcoarse_step=50,intfine_step=5){// 粗搜std::vector<double>coarse_values;std::vector<int>coarse_positions;for(intpos=min_pos_;pos<=max_pos_;pos+=coarse_step){cv::Mat frame=capture_at_pos(pos);doubleval=evaluator_.evaluate(frame,FocusEvaluator::TENENGRAD);coarse_positions.push_back(pos);coarse_values.push_back(val);std::cout<<"Coarse: pos="<<pos<<", focus="<<val<<std::endl;}// 找粗搜峰值automax_it=std::max_element(coarse_values.begin(),coarse_values.end());intpeak_idx=std::distance(coarse_values.begin(),max_it);intpeak_pos=coarse_positions[peak_idx];// 细搜范围intfine_start=std::max(min_pos_,peak_pos-coarse_step);intfine_end=std::min(max_pos_,peak_pos+coarse_step);doublebest_val=0;intbest_pos=peak_pos;for(intpos=fine_start;pos<=fine_end;pos+=fine_step){cv::Mat frame=capture_at_pos(pos);doubleval=evaluator_.evaluate(frame,FocusEvaluator::TENENGRAD);if(val>best_val){best_val=val;best_pos=pos;}std::cout<<"Fine: pos="<<pos<<", focus="<<val<<std::endl;}returnbest_pos;}// 设置ROIvoidsetROI(constcv::Rect&roi){evaluator_.setROI(roi);}private:FocusEvaluator evaluator_;intmin_pos_;intmax_pos_;};// 使用示例intmain(){cv::VideoCapturecap(0);if(!cap.isOpened()){std::cerr<<"Cannot open camera"<<std::endl;return-1;}FocusEvaluator evaluator;// 实时显示对焦值while(true){cv::Mat frame;cap>>frame;if(frame.empty())break;// 计算对焦值doublefocus=evaluator.evaluate(frame,FocusEvaluator::LAPLACIAN_VAR);// 显示std::string text="Focus: "+std::to_string(focus);cv::putText(frame,text,cv::Point(10,30),cv::FONT_HERSHEY_SIMPLEX,1,cv::Scalar(0,255,0),2);cv::imshow("Focus Evaluation",frame);if(cv::waitKey(1)==27)break;}return0;}

六、实际工程中的注意事项

做了几个项目后，总结了一些实际中容易踩的坑：

1. VCM的磁滞问题

音圈马达在正反方向移动时存在磁滞，同样的电流不一定能到达同样的位置。解决方法是增加闭环控制的增益，或者在对焦算法中始终从同一方向逼近目标位置。

2. 振动导致的对焦抖动

在无人机等振动环境下，对焦值波动很大。需要在评价函数前加时域滤波，或者用连续多帧平均值来判断。

3. 低光环境下AF失效

光线不足时图像信噪比低，对焦评价值可信度下降。可以切换到更鲁棒的评价函数，或者放宽对焦精度要求。

4. 变焦呼吸效应的主观感受

技术上把呼吸效应消除得很完美了，但视频拍摄时用户仍然能感知到微小的焦点漂移。实际上需要做到像变焦后人眼感觉焦点"完全不动"，补偿精度要达到1/4景深以内。

总结

本文从光学基础讲起，详细介绍了现代摄像机聚焦和变焦系统的工作原理，并给出了完整的嵌入式控制代码和PC端图像处理代码。主要内容包括：

1. 薄透镜成像公式和景深计算
2. VCM音圈马达的PID控制
3. 对比度检测AF和相位检测AF的原理与实现
4. 爬山搜索自动对焦算法
5. 变焦马达控制和焦点补偿
6. VISCA协议解析
7. OpenCV对焦评价工具

这套代码在我的ROV项目上跑得挺稳定，希望能给做类似项目的朋友一点参考。有问题欢迎评论区讨论。

如果觉得有帮助，点个赞收藏一下吧~