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

摘要

本发明公开了一种航空相机稳定平台无超调伺服控制方法，属于自动控制技术领域。其主要特点是，对横滚通道的位置指令进行规划，得到一条从初始姿态角度按照1/4正旋曲线过渡到位置指令角度的曲线，由此将相机控制系统给出的阶跃位置指令转化成平滑过渡曲线；通过姿态解算算法解算出瞄准线的姿态角，由此获得稳定平台在地理坐标系下的角位置。本发明解决了已有稳定平台位置阶跃过程中所出现的超调问题，大大提高了高空拍照的效率；同时，能够控制相机稳定平台的视线在地理坐标系下精确指向目标航道。本发明所用曲线规划算法和姿态解算算法简单，实现方便，移植性好，因而本发明具有更广阔的应用前景。

主权项

一种航空相机稳定平台无超调伺服控制方法，其特征在于：该方法由置有伺服控制模块的DSP实现，伺服控制模块含有横滚通道和俯仰通道，当DSP上电后，伺服控制模块执行以下操作步骤：第一步，判断是否接收到航空相机系统控制器发出的“开始工作”指令，若为否，等待，若为是，转入第二步；第二步，接收航空相机系统控制器给出的横滚通道位置指令θ_{ro_cmd}；将俯仰通道位置指令置为零，即令θ_{el_cmd}＝0；令变量i＝1；第三步，采集垂直陀螺输出的载机初始横滚角θ_{ro_v0}和初始俯仰角θ_{el_v0}，采集横滚旋转变压器输出的横滚外框架初始角位置信号θ_{ro_r0}和俯仰旋转变压器输出的俯仰内框架初始角位置信号θ_{el_r0}，并根据下式计算：${\mathrm{\θ}}_{ro\_los0}=\arctan \left(\frac{a_{11}{\mathrm{cos\θ}}_{el\_r0}{\mathrm{sin\θ}}_{ro\_r0}+a_{12}{\mathrm{sin\θ}}_{el\_r0}+a_{13}{\mathrm{cos\θ}}_{el\_r0}{\mathrm{cos\θ}}_{ro\_r0}}{a_{31}{\mathrm{cos\θ}}_{el\_r0}{\mathrm{sin\θ}}_{ro\_r0}+a_{32}{\mathrm{sin\θ}}_{el\_r0}+a_{33}{\mathrm{cos\θ}}_{el\_r0}{\mathrm{cos\θ}}_{ro\_r0}}\right)$a₁₁＝cosθ_{ro_v0}a₁₂＝0a₁₃＝sinθ_{ro_v0}a₃₁＝‑sinθ_{ro_v0}cosθ_{el_v0}a₃₂＝sinθ_{el_v0}a₃₃＝cosθ_{ro_v0}cosθ_{el_v0}式中，θ_{ro_los0}为相机瞄准线的初始横滚姿态角；第四步，生成横滚通道的规划位置控制曲线：${\mathrm{\θ}}_{ro\_cmd\_i}=\[Sin(2\mathrm{\π}\×(\frac{T\×i}{4\×T_s}\left)\right)\]\×({\mathrm{\θ}}_{ro\_cmd}-{\mathrm{\θ}}_{ro\_los0})+{\mathrm{\θ}}_{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})${\mathrm{\θ}}_{ro\_los}=\arctan \left(\frac{a_{11}{\mathrm{cos\θ}}_{el\_r}{\mathrm{sin\θ}}_{ro\_r}+a_{12}{\mathrm{sin\θ}}_{el\_r}+a_{13}{\mathrm{cos\θ}}_{el\_r}{\mathrm{cos\θ}}_{ro\_r}}{a_{31}{\mathrm{cos\θ}}_{el\_r}{\mathrm{sin\θ}}_{ro\_r}+a_{32}{\mathrm{sin\θ}}_{el\_r}+a_{33}{\mathrm{cos\θ}}_{el\_r}{\mathrm{cos\θ}}_{ro\_r}}\right)$a₁₁＝cosθ_{ro_v}a₁₂＝0a₁₃＝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进行位置回路解算：${\mathrm{\ω}}_{ro\_cmd}=k_{pr}\frac{s/{\mathrm{\ω}}_{pr}+1}{s}{p}_{ro\_err}$${\mathrm{\ω}}_{el\_cmd}=k_{pe}\frac{s/{\mathrm{\ω}}_{pe}+1}{s}{p}_{el\_err}$式中，ω_{ro_cmd}为横滚通道速率回路控制量，ω_{el_cmd}为俯仰通道速率回路控制量，k_pr为PI调节器1横滚通道的增益系数，k_pe为PI调节器1俯仰通道的增益系数，ω_pr为PI调节器1横滚通道的积分器控制参数，ω_pe为PI调节器1俯仰通道的积分器控制参数；第十步，进行横滚通道和俯仰通道速率回路运算：10.1采集双轴速率陀螺当前输出的横滚外框架的角速率信号ω_ro和俯仰内框架的角速率信号ω_el，并采用一阶低通滤波器对双轴速率陀螺的输出信号进行滤波：${\mathrm{\ω}}_{ro1}=\frac{1}{1+S/{\mathrm{\ω}}_{lp\_ro}}{\mathrm{\ω}}_{ro}$${\mathrm{\ω}}_{el1}=\frac{1}{1+S/{\mathrm{\ω}}_{lp\_el}}{\mathrm{\ω}}_{el}$式中，ω_ro1为横滚通道滤波信号，ω_el1为俯仰通道滤波信号，ω_{lp_ro}为一阶低通滤波器横滚通道的转折频率，ω_{lp_e1}为一阶低通滤波器俯仰通道的转折频率；10.2根据以下公式计算速率误差量：ω_{ro_err}＝ω_{ro_cmd}－ω_ro1ω_{el_err}＝ω_{el_cmd}－ω_el1式中，ω_{ro_err}为横滚通道的速率误差量，ω_{el_err}为俯仰通道的速率误差量；10.3采用PI调节器2进行速率回路解算：$I_{ro\_cmd}=k_{gr}\frac{s/{\mathrm{\ω}}_{gr}+1}{s}{\mathrm{\ω}}_{ro\_err}$$I_{e1\_cmd}=k_{ge}\frac{s/{\mathrm{\ω}}_{ge}+1}{s}{\mathrm{\ω}}_{el\_err}$式中，I_{ro_cmd}为横滚通道PWM功率放大器的控制量，I_{el_cmd}为俯仰通道PWM功率放大器的控制量，k_gr为PI调节器2横滚通道的增益系数，k_ge为PI调节器2俯仰通道的增益系数，ω_gr为PI调节器2横滚通道积分器控制参数，ω_ge为PI调节器2俯仰通道的积分器控制参数；第十一步，将横滚通道PWM功率放大器的控制量I_{ro_cmd}施加给横滚通道功率放大器，将俯仰通道PWM功率放大器的控制量I_{el_cmd}施加给俯仰通道功率放大器；第十二步，令变量i加1，即i＝i+1；第十三步，判断是否i＞T_c/T，若为是，转入第十四步，若为否，转入第十五步，T_c为稳定平台阶跃响应指标；第十四步，通知相机启动拍照；第十五步，判断是否i＞T_p/T，若为是，转入第十六步，若为否，返回第五步，T_p为横滚通道位置指令的时间间隔；第十六步，是否接到工作结束指令，如果为否，则返回第二步，如果为是，则伺服控制模块停止运行。