[发明专利]一种两轴四框架光电吊舱的伺服控制方法及系统

摘要

本发明属于机载光电吊舱的伺服控制技术，具体涉及一种两轴四框架光电吊舱的平台稳定技术以及高精度光电跟踪控制技术。本发明首先设计了一种内外框架联动平台稳定控制技术使该吊舱具有很好的平台稳定性能，该平台稳定控制技术可保证内方位框架与内俯仰框架始终互相垂直，减小了吊舱框架的几何约束耦合，使光电吊舱在过顶时也具有很好的平台稳定性能；在此基础上，本发明又设计了一种内外框联动光电跟踪控制技术以提高光电跟踪精度，最终使该光电吊舱实现该吊舱的高精度与高稳定度的光电跟踪控制。本发明作为可以应用于两轴四框架光电吊舱的关键伺服控制技术，具有较好的应用前景，能产生较大经济效益和军事效益。

主权项

一种两轴四框架机载光电吊舱的伺服控制方法，光电吊舱由方位轴与俯仰轴两个轴系组成，方位轴包括内方位框架与外方位框架，俯仰轴包括内俯仰框架与外俯仰框架；其特征在于，该光电吊舱在载机机身振动以及外部风力干扰状况下通过下述方法实现高精度高稳定光电跟踪控制：（1）内框架执行所设计的内框架位置环跟踪控制算法获得内框架速度环控制量，将速度环控制量代入陀螺速度环稳定回路获得相应的DA口电压控制量，通过驱动器将DA口电压控制量功率放大后驱动内框架电机带动内框架作出跟踪控制运动；具体包括以下步骤：（1.1）获取脱靶量数据，进行脱靶量数据处理，并计算分段PID控制量；（1.1.1）获取脱靶量数据，进行脱靶量数据处理；通过DSP处理器的通讯接口单元每20ms读取一次内方位的脱靶数据Δθ_nfw，当前1ms内方位目标给定位置Posr_nfw如下式所示：Posr_nfw＝Pos_nfw+Δθ_nfw/20；其中，Posr_nfw为每1ms当前内方位目标给定位置，Pos_nfw为当前1ms内方位码盘当前测量的值；通过DSP处理器的通讯接口单元每20ms读取一次内俯仰的脱靶数据Δθ_nfy，当前1ms内俯仰目标给定位置Posr_nfy如下式所示：Posr_nfy=Pos_nfy+△θ_nfy/20；其中，Posr_nfy为每1ms当前内俯仰目标给定位置，Pos_nfy为当前俯仰码盘当前测量的值；（1.1.2）将经过上一步骤处理后的脱靶量代入到分段PID控制计算得到分段PID控制量；根据上一步骤中获得的当前1ms方位目标位置值Posr_nfw与当前1ms内方位码盘当前测量的值Pos_ní，计算当前1ms的内方位等效目标偏差值Err_nfw=Posr_nfw‑Pos_nfw，内方位误差的积分ErrI_nfw=ΣErr_nfw，内方位误差的微分ErrD_nfw=（当前1ms的Err_nfw‑上1ms的Err_nfw）×1000，将上述参数代入分段PID控制中获得内方位分段变参数PID控制量PIDspeedOut_nfw；内方位分段变参数PID控制量PIDspeedOut_nfw的公式如下式所示：PIDspeedOut_nfw=nfwKp×Err_nfw+nfwKi×ErrI_nfw+nfwKd×ErrD_nfw，其中：nfwKp为内方位比例系数，nfwKi为内方位积分系数，nfwKd为内方位微分系数；根据上一步骤中获得的当前1ms俯仰目标位置值Posr_nfy与当前1ms内俯仰码盘当前测量的值Pos_n，计算当前1ms的内俯仰等效目标偏差值Err_nfy=Posr_nfy‑Pos_nfy，内俯仰误差的积分ErrI_nfy=ΣErr_nfy，内俯仰误差的微分ErrD_nfy=（当前1ms的Err_nfy‑上1ms的Err_nfy）×1000，将上述参数代入分段PID控制中获得内俯仰分段变参数PID控制量PIDspeedOut_nfy；内俯仰分段变参数PID控制量PIDspeedOut_nfy的公式如下式所示：PIDspeedOut_nfw=nfyKp×Err_nfy+nfyKi×ErrI_nfy+nfyKd×ErrD_nfy，其中：nfyKp为内俯仰比例系数，nfyKi为内俯仰积分系数，nfyKd为内俯仰微分系数；（1.2）获得加速度滞后补偿控制量，并与上述步骤(1.1)中分段PID控制量控制量相加获得位置环总控制量；（1.2.1）获取陀螺速度滞后补偿控制量通过DSP伺服控制器的陀螺读数单元获得经过滤波处理过的光电载荷相对内方位轴的惯性空间速度gyrospeed_nfw，与内方位滞后补偿系数nfwK1相乘得内方位跟踪速度滞后补偿控制量gyrospeedOut_nfw=gyrospeed_nfw×nfwK1；通过DSP伺服控制器的陀螺读数单元获得经过滤波处理过的光电载荷相对内俯仰轴的惯性空间速度gyrospeed_nfy，与内俯仰滞后补偿系数nfyK1相乘得内俯仰跟踪速度滞后补偿控制量gyrospeedOut_nfy=gyrospeed_nfy×nfyK1；（1.2.2）获取陀螺加速度滞后补偿控制量将步骤（1.2.1）中测量的内方位惯性空间速度值gyrospeed_nfw微分后获得内方位惯性空间加速度值gyroAccspeed_nfw，与内方位滞后补偿系数nfwK2相乘得内方位速度滞后补偿控制量gyroAccSpeedOut_nfw=gyroAccSpeed_nfw×nfwK2；将步骤（1.2.1）中测量的内俯仰惯性空间速度值gyrospeed_nfy微分后获得内俯仰惯性空间加速度值gyroAccspeed_nfy，与内俯仰滞后补偿系数nfyK2相乘得内俯仰速度滞后补偿控制量gyroAccSpeedOut_nfy=gyroAccSpeed_nfy×nfyK2；（1.2.3）获取总的速度给定控制量；将步骤（1.1.2）中内方位分段PID控制量PIDspeedOut_nfw、步骤（1.2.1）中内方位陀螺速度滞后补偿控制量gyrospeedOut_nfw、步骤（1.2.2）中内方位陀螺加速度滞后补偿控制量gyroAccSpeedOut_nfw相加作为内方位速度给定控制量SpeedOut_nfw=PIDspeedOut_nfw+gyrospeedOut_nfw+gyroAccSpeedOut_nfw；将步骤（1.1.2）中内俯仰分段PID控制量PIDspeedOut_nfy、步骤（1.2.1）中内俯仰陀螺速度滞后补偿控制量gyrospeedOut_nfy、步骤（1.2.2）中内俯仰陀螺加速度滞后补偿控制量gyroAccSpeedOut_nfy相加作为内俯仰速度给定控制量SpeedOut_nfy=PIDspeedOut_nfy+gyrospeedOut_nfy+gyroAccSpeedOut_nfy；（1.3）将步骤(1.2)中位置环总控制量代入陀螺速度环稳定回路获得相应的DA口电压控制量；（1.3.1）获取陀螺速度偏差值DSP控制器每1ms将读取的内方位速度给定控制量speedOut_nfw，减去光电载荷相对内方位轴的惯性空间速度gyrospeed_nfw，获得内方位陀螺速度偏差值SpeedErr_nfw=SpeedOut_nfw‑gyrospeed_nfw；DSP控制器每1ms将读取的内俯仰速度给定控制量speedOut_nfy，减去光电载荷相对内俯仰轴的惯性空间速度gyrospeed_nfy获得内俯仰陀螺速度偏差值SpeedErr_nfy=SpeedOut_nfy‑gyrospeed_nfy；（1.3.2）获取控制电机运动所需要的DA口电压控制量；根据步骤（1.3.1）中得到的内方位陀螺速度偏差值SpeedErr_nfw计算DA口内方位电压控制量Ut_nfw=SpeedErr_nfw×nfwspeedKp+speedErrI_nfw×nfwspeedKi，其中：内方位陀螺速度偏差值的积分speedErrI_nfw=ΣspeedErr_nfw，nfwspeedKi为内方位速度比例系数，nfwspeedKi为内方位速度积分系数；根据步骤（1.3.1）中得到的内俯仰陀螺速度偏差值SpeedErr_nfy计算DA口内俯仰电压控制量，Ut_nfy=SpeedErr_nfy×nfyspeedKp+speedErrI_nfy×nfyspeedKi，其中：内俯仰陀螺速度偏差值的积分speedErrI_nfy=ΣspeedErr_nfy，nfyspeedKi为内俯仰速度比例系数，nfyspeedKi为内俯仰速度积分系数；（1.4）内框架驱动器将步骤(1.3)中的DA口电压控制量功率放大后驱动内框架电机带动内框架作出相应的平台稳定与跟踪控制运动；内方位驱动器将DA口电压控制量Ut_nfw进行功率放大后，驱动内方位力矩电机带动光电吊舱内方位框架运动；内俯仰驱动器将DA口电压控制量Ut_nfy进行功率放大后，驱动内俯仰力矩电机带动光电吊舱内俯仰框架运动；（2）外框架以内框架码盘值作为控制指令，执行外框架位置环控制算法获得速度环控制量，将速度环控制量代入外框架速度环；（2.1）将步骤(1.1)中读取的内框架码盘数据代入到分段滑模控制中获得外框架位置环控制量；取外方位框架的数学模型为得到本吊舱外方位滑模控制的位置环控制量输出函数如下：其中，外方位框架目标值r＝内方位码盘值Pos_nfw+外方位码盘值Pos_wfw，滑模控制切换函数取s₁＝0.3×Pos_nfw+(当前1ms的Pos_nfw‑上1ms的Pos_nfw)×1000，滑模控制趋近律取k₁为比例系数，ε₁为速度系数；外俯仰框架的数学模型为得到本吊舱外俯仰滑模控制的位置环控制量输出函数如下：其中，外俯仰目标值r＝内俯仰码盘值Pos_nfy+外俯仰码盘值Pos_wfy，滑模控制切换函数取s₂＝0.3×Pos_nfy+(当前1ms的Pos_nfy‑上1ms的Pos_nfy)×1000，滑模控制趋近律取k₂为比例系数，ε₂为速度系数；（2.2）将上述步骤(2.1)获得的外框架位置环控制量代入到外框架码盘速度环回路获得相应的DA口电压控制量；（2.2.1）获得外框架码盘速度值将外方位码盘值微分后获得外方位码盘速度Speed_wfw，公式如下：Speed_wfw=(当前1ms外方位码盘值Pos_wfw‑上1ms外方位码盘值Err_nfw)×1000；将外俯仰位置值微分后获得外俯仰码盘速度Speed_wfw，公式如下：Speed_wfw=(当前1ms外俯仰码盘值Pos_wfw‑上1ms外俯仰码盘值Err_nfw)×1000；（2.2.2）获取码盘速度偏差值DSP控制器每1ms将读取的外方位速度给定控制量speedOut_wfw减去外方位码盘速度Speed_wfw，得到外方位码盘速度偏差值：SpeedErr_wfw=SpeedOut_wfw‑speed_wfw；DSP控制器1每1ms将读取的外俯仰速度给定控制量speedOut_wfw减去外俯仰码盘速度Speed_wfw，得到外俯仰码盘速度偏差值SpeedErr_nfw：SpeedErr_wfy=SpeedOut_wfy‑speed_wfy；（2.2.3）获取控制电机运动所需要的DA口电压控制量；根据步骤（2.2.2）中得到的外方位码盘速度偏差值SpeedErr_nfw计算出DA口外方位电压控制量Ut_wfw，公式如下所示：Ut_wfw=SpeedErr_wfw×wfwspeedKp+speedErrI_wfw×wfwspeedKi，其中：外方位码盘速度偏差值的积分speedErrI_wfw=ΣspeedErr_wfw，wfwspeedKi为外方位速度比例系数，wfwspeedKi为外方位速度积分系数；根据步骤（2.2.2）中得到的外俯仰码盘速度偏差值SpeedErr_nfw计算出DA口外俯仰电压控制量Ut_wfw，公式如下所示：Ut_wfy=SpeedErr_wfy×wfyspeedKp+speedErrI_wfy×wfyspeedKi，其中：外俯仰码盘速度偏差值的积分speedErrI_wfy=ΣspeedErr_wfy，wfyspeedKi为外俯仰速度比例系数，wfyspeedKi为外俯仰速度积分系数；（2.3）外框架驱动器将步骤(2.2)中DA口电压控制量功率放大驱动外框架电机带动外框架跟随内框架作出相应的运动；外方位驱动器将DA口电压控制量Ut_wfw进行功率放大后，驱动外方位力矩电机带动光电吊舱外方位框架运动；外俯仰驱动器将DA口电压控制量Ut_wfy进行功率放大后，驱动外俯仰力矩电机带动光电吊舱外俯仰框架11运动。