[发明专利]航空相机稳定平台无超调伺服控制方法

权利要求书

1.一种航空相机稳定平台无超调伺服控制方法，其特征在于：该方法由置有伺服控制模块的DSP实现，伺服控制模块含有横滚通道和俯仰通道，当DSP上电后，伺服控制模块执行以下操作步骤：

第一步，判断是否接收到航空相机系统控制器发出的“开始工作”指令，若为否，等待，若为是，转入第二步；

第二步，接收航空相机系统控制器给出的横滚通道位置指令θ_{ro_cmd}；将俯仰通道位置指令置为零即令θ_{el_cmd}＝0；令变量i＝1；

第三步，采集垂直陀螺输出的载机初始横滚角θ_{ro_v0}和初始俯仰角θ_{el_v0}，采集横滚旋转变压器输出的横滚外框架初始角位置信号θ_{ro_r0}和俯仰旋转变压器输出的俯仰内框架初始角位置信号θ_{el_r0}，并根据下式计算：

θro_los0=arctan(a11cosθel_r0sinθro_r0+a12sinθel_r0+a13cosθel_r0cosθro_r0a31cosθel_r0sinθro_r0+a32sinθel_r00a33cosθel_r0cosθro_r0)]]>

a₁₁＝cosθ_{ro_v0}

a₁₂＝0

a₁₃＝sinθ_{ro_v0}

a₃₁＝-sinθ_{ro_v0}cosθ_{el_v0}

a₃₂＝sinθ_{el_v0}

a₃₃＝cosθ_{ro_v0}cosθ_{el_v0}

式中，θ_{ro_los0}为相机瞄准线的初始横滚姿态角；

第四步，生成横滚通道的规划位置指令曲线：

θro_cmd_i=[Sin(2π×(T×i4×Ts))](θro_cmd-θro_los0)+θro_los0]]>

式中，θ_{ro_cmd_i}表示与变量i对应的规划位置指令；T为采样周期；T_s为横滚通道位置调节时间；

第五步，采集垂直陀螺当前输出的载机横滚角θ_{ro_v}和载机俯仰角θ_{el_v}，采集横滚旋转变压器当前输出的横滚外框架的角位置信号θ_{ro_r}和俯仰旋转变压器当前输出的俯仰内框架的角位置信号θ_{el_r}，并根据以下一组公式计算：

θ_{el_los}＝arcsin(a₃₁cosθ_{el_r}sinθ_{ro_r}+a₃₂sinθ_{el_r}+a₃₃cosθ_{el_r}cosθ_{ro_r})

θro_los=arctan(a11cosθel_rsinθro_r+a12sinθel_r+a13cosθel_rcosθro_ra31cosθel_rsinθro_r+a32sinθel_r+a33cosθel_rcosθro_r)]]>

a₁₁＝cosθ_{ro_v}

a₁₂＝0

a₁₃＝sinθ_{ro_v}

a₃₁＝-sinθ_{ro_v}cosθ_{el_v}

a₃₂＝sinθ_{el_v}

a₃₃＝cosθ_{ro_v}cosθ_{el_v}

式中，θ_{el_los}为相机瞄准线的当前俯仰姿态角，θ_{ro_los}为相机瞄准线的当前横滚姿态角；

第六步，判断i＜T_s/T，若为是，转入第七步，若为否，转入第八步；

第七步，将i代入横滚通道的规划位置控制曲线，求出规划位置指令θ_{ro_cmd_i}，然后转入第九步；

第八步，将航空相机系统控制器给出的横滚通道位置指令θ_{ro_cmd}作为当前横滚通道的规划位置指令θ_{ro_cmd_i}，即令θ_{ro_cmd_i}＝θ_{ro_cmd}并转入第九步；

第九步，进行横滚通道和俯仰通道位置回路运算：

9.1根据以下公式计算位置误差：

p_{ro_err}＝θ_{ro_cmd_i}－θ_{ro_los}

p_{el_err}＝θ_{el_cmd}－θ_{el_los}

式中，p_{ro_err}为横滚位置误差量，p_{el_err}为俯仰位置误差量；

9.2采用PI调节器1进行位置回路解算：

ωro_cmd=kprs/ωpr+1spro_err]]>

ωel_cmd=kpes/ωpe+1spel_err]]>

式中，ω_{ro_cmd}为横滚通道速率回路控制量，ω_{el_cmd}为俯仰通道速率回路控制量，k_pr为PI调节器1横滚通道的增益系数，k_pe为PI调节器1俯仰通道的增益系数，ω_pr为PI调节器1横滚通道的积分器控制参数，ω_pe为PI调节器1俯仰通道的积分器控制参数；

第十步，进行横滚通道和俯仰通道速率回路运算：

10.1采集双轴速率陀螺当前输出的横滚外框架的角速率信号ω_ro和俯仰内框架的角速率信号ω_el，并采用一阶低通滤波器对双轴速率陀螺的输出信号进行滤波：

ωro1=11+S/ωlp_roωro]]>

ωel1=11+S/ωlp_elωel]]>

式中，ω_ro1为横滚通道滤波信号，ω_el1为俯仰通道滤波信号，ω_{lp_ro}为一阶低通滤波器横滚通道的转折频率，ω_{lp_e1}为一阶低通滤波器俯仰通道的转折频率；

10.2，根据以下公式计算速度误差量：

ω_{ro_err}＝ω_{ro_cmd}－ω_ro1

ω_{el_err}＝ω_{el_cmd}－ω_el1

式中，ω_{err_ro}为横滚通道的速度误差量，ω_{err_el}为俯仰通道的速度误差量；

10.3，采用PI调节器2进行速率回路解算：

Iro_cmd=kgrS/ωgr+1sωro_err]]>

Iel_cmd=kgeS/ωge+1sωel_err]]>

式中，I_{cmd_r}为横滚通道PWM功率放大器的控制量，I_{cmd_e}为俯仰通道PWM功率放大器的控制量，k_gr为PI调节器2横滚通道的增益系数，k_ge为PI调节器2俯仰通道的增益系数，ω_gr为PI调节器2横滚通道积分器控制参数，ω_ge为PI调节器2俯仰通道的积分器控制参数；

第十一步，将横滚通道PWM功率放大器的控制量I_{cmd_r}施加给横滚通道功率放大器，将俯仰通道PWM功率放大器的控制量I_{cmd_e}施加给俯仰通道功率放大器；

第十二步，令变量i加1，即i＝i+1；

第十三步，判断i＞T_c/T，若为是，转入第十四步，若为否，转入第十五步，T_c为稳定平台阶跃响应指标；

第十四步，通知相机启动拍照；

第十五步，判断i＞T_p/T，若为是，转入第十六步，若为否，返回第五步，T_p为横滚通道位置指令的时间间隔；

第十六步，是否接到工作结束指令，如果为否，则返回第二步，如果为是，则伺服控制模块停止运行。

2.根据权利要求1所述的航空相机稳定平台无超调伺服控制方法，其特征在于：采样周期T＝5ms；横滚通道位置调节时间T_s＝300ms；稳定平台阶跃响应指标T_c＝400ms；横滚通道位置指令的时间间隔T_p＝600ms；PI调节器1横滚通道增益系数k_pr＝0.8，PI调节器1俯仰通道增益系数，k_pe＝0.6；PI调节器1横滚通道的积分器控制参数ω_pr＝1.88，PI调节器1俯仰通道的积分器控制参数ω_pe＝1.88；一阶低通滤波器横滚通道的转折频率ω_{lp_ro}＝503，一阶低通滤波器俯仰通道的转折频率ω_{lp_e1}＝503；PI调节器2横滚通道增益系数k_gr＝8.6，PI调节器2俯仰通道增益系数k_ge＝6.3；PI调节器2横滚通道的积分器控制参数ω_gr＝62.8，PI调节器2俯仰通道的积分器控制参数ω_ge＝50.2。