CN103631276B - 一种基于旋转双棱镜的跟踪装置的控制方法 - Google Patents

Abstract

本发明提供了一种基于旋转双棱镜的跟踪装置的控制方法，可以对运动目标进行跟踪。该装置包括两个棱镜、两个电机、两个位置传感器、一个探测器和一个控制器。通过旋转双棱镜，扩大了探测器的视场；通过探测器和控制器可以计算目标位置；通过控制器控制棱镜旋转，可以对运动目标进行闭环跟踪，使目标的成像始终在探测器视场中间。本发明解决了旋转双棱镜用于目标跟踪的问题，通过探测器闭环，使其可应用于对运动目标的跟踪。本发明的装置结构紧凑、转动惯量低、响应迅速。

Description

一种基于旋转双棱镜的跟踪装置的控制方法

技术领域

本发明属于光电跟踪领域，涉及一种光电跟踪装置与控制方法。

背景技术

在光电跟踪领域，通常采用万向架形式的跟踪装置，将探测器安装在多轴万向架上，通过控制万向架回转运动来控制视轴的方向，此类机构体积较大、对振动敏感。另一种结构为摆镜结构，此类结构对机械误差敏感，一般用于小口径的光束偏转。

旋转双棱镜结构(Risley棱镜)可通过旋转共轴的两个楔形棱镜达到控制光束偏转的目的，具有结构紧凑、转动惯量低、响应迅速的特点。在先技术(参见云茂金、祖继峰等的专利：CN1256609C与专利：CN2655268)中提出采用该结构进行光束扫描，对基于旋转双棱镜的扫描装置和扫描算法进行了研究，但所提供的装置和方法不适用于目标跟踪。

扫描属于正向解算，算法较为简单，并且激光器发射的光线始终沿棱镜光轴方向，在先技术中推导的正向光束偏转公式只适用于入射光线沿棱镜光轴方向入射的情况。但目标跟踪时，同时需要正向解算和反向解算，且光束方向不一定沿棱镜光轴方向，算法较复杂，目前文献中并未发现解决此问题的相关方法。

发明内容

本发明的目的是克服现有技术的不足，解决旋转双棱镜用于目标跟踪的技术问题，提供一种可以用于目标跟踪的装置及其控制方法。

本发明的技术解决方案包括：一种基于旋转双棱镜的跟踪装置，包括：第一棱镜、第二棱镜、第一电机、第二电机、第一位置传感器、第二位置传感器、探测器和控制器，其中，两个棱镜、两个电机和探测器为同轴安装，第一棱镜和第二棱镜具有相同的顶角和折射率，第一电机和第二电机均为环形力矩电机，二者的转子分别与第一棱镜和第二棱镜直接相连；第一位置传感器测量第一棱镜绕转轴的旋转角度θ₁，并将θ₁送到控制器；第二位置传感器测量第二棱镜绕转轴的旋转角度θ₂，并将θ₂送到控制器；探测器能够测量得到目标在探测器上所成像点的方位角Θ和俯仰角Φ，并将其送到控制器，控制器接收第一棱镜的位置θ₁、第二棱镜的位置θ₂、探测器上所成像点的方位角Θ和俯仰角Φ，输出电压信号V₁至第一电机，输出电压信号V₂至第二电机。

进一步的，通过探测器闭环，使目标成像始终位于探测器视场中心附近。

另外提供一种利用上述的旋转双棱镜的跟踪装置的控制方法，该控制方法的包括以下步骤：

步骤1)、由探测器测量得到目标在探测器上所成像点的方位角Θ、俯仰角Φ；由第一位置传感器测量得到第一棱镜的位置θ₁，第二位置传感器测量得到第二棱镜的位置θ₂；由θ₁、θ₂、Θ、Φ计算得到目标的方位角Θ_t和俯仰角Φ_t；

计算过程如下：由目标发出的光在探测器上形成像点，由于光路是可逆的，那么由探测器出射的光束经过两个棱镜折射后的出射光线将到达目标点，此过程可以通过4次折射得到，如公式(1)所示：

其中，Tr_i表示依次经过的4次折射过程，i＝1,2,3,4；

设K_t，L_t，M_t表示矢量的方向余弦，则方位角Θ_t和俯仰角Φ_t由下面公式计算：

${\mathrm{\&Theta;}}_t=\left\{\begin{array}{c}arctan\left(\frac{L_t}{K_t}\right),K_t\&GreaterEqual;0\\ arctan\left(\frac{L_t}{K_t}\right)+\mathrm{\&pi;},K_t<0\end{array}\right.---\left(2\right)$

Φ_t＝arccos(M_t) (3)

步骤2)、由目标的方位角Θ_t和俯仰角Φ_t，计算使目标成像在探测器的视场中心时，两个棱镜需要旋转到的新位置θ₁ ^*、θ₂ ^*；

计算过程如下：由于光路是可逆的，为了使目标成像在视场中心，由探测器中心出射的光线(0,0,1)将到达目标点；

按两步法计算θ₁ ^*和θ₂ ^*：

第一步：由俯仰角Φ_t计算Δθ＝|θ₁-θ₂|，具体方法为保持第一棱镜的位置不动，采用优化方法得到Δθ＝|θ₁-θ₂|。优化方法的原理为：由于两个棱镜的夹角在0°～180°或180°～360°范围时，入射光线(0,0,1)对应的出射光线的俯仰角和方位角与棱镜夹角的大小是单调的，迭代优化算法也会很快收敛到全局极值，具体流程如下：

步骤1、取棱镜夹角为转步骤2；

步骤2、通过公式(1)-公式(3)计算得到俯仰角Φ_i，转步骤3；

步骤3、如果|Φ_i-Φ_t|＜ε(ε为设定的精度要求)，Δθ_i+1＝Δθ_i，流程结束；否则转步骤4；

步骤4、如果Φ_i＜Φ_t，取如果Φ_i＞Φ_t，取转步骤5；

步骤5、更新i＝i+1；转步骤2；

第二步：在第一步基础上，得到了棱镜夹角Δθ＝|θ₁-θ₂|，通过公式(1)-公式(3)，得到2个方位角Θ₀和Θ₀'，则两个棱镜需要同步旋转的角度为Θ_t-Θ₀和Θ_t-Θ₀'，则两个棱镜需要旋转到的最终位置有两组解：

或

步骤3)、通过控制器控制第一电机和第二电机运动，使其带动第一棱镜和第二棱镜旋转到位置θ₁ ^*、θ₂ ^*，目标将被锁定在探测器的视场中心。

进一步的，通过优化方法确定两个棱镜需要旋转到的新位置θ₁ ^*、θ₂ ^*，从而保证目标成像在探测器的视场中心。

本发明的具体原理在于：

结合图1-3，本发明一种基于旋转双棱镜的跟踪装置，包括：第一棱镜1、第二棱镜2、第一电机3、第二电机4、第一位置传感器5、第二位置传感器6、探测器7、控制器8。

其中，两个棱镜、两个电机和探测器为同轴安装。第一棱镜1和第二棱镜2具有相同的顶角和折射率。第一电机3和第二电机4均为环形力矩电机，二者的转子分别与第一棱镜和第二棱镜直接相连，省却了中间传动环节，具有响应快、刚度高的特点；第一位置传感器5测量第一棱镜1绕转轴的旋转角度θ₁，并将θ₁送到控制器8；第二位置传感器6测量第二棱镜2绕转轴的旋转角度θ₂，并将θ₂送到控制器8；探测器7可以测量得到目标在探测器7上所成像点的方位角Θ和俯仰角Φ。矢量的方位角和俯仰角的定义如图2所示：坐标系为直角坐标系0xyz，矢量与x轴正向的夹角为方位角Θ，与z轴正向的夹角为俯仰角Φ。控制器8接收第一棱镜的位置θ₁、第二棱镜的位置θ₂、探测器7上所成像点的方位角Θ和俯仰角Φ；输出电压信号V₁至第一电机3，输出电压信号V₂至第二电机4。

完成目标跟踪的过程如下：

1)由探测器7测量得到目标在探测器上所成像点的方位角Θ、俯仰角Φ；由第一位置传感器5测量得到第一棱镜1的位置θ₁，第二位置传感器6测量得到第二棱镜2的位置θ₂；由θ₁、θ₂、Θ、Φ计算得到目标的方位角Θ_t和俯仰角Φ_t。

计算过程如下：由目标发出的光在探测器上形成像点，由于光路是可逆的，那么由探测器出射的光束经过两个棱镜折射后的出射光线将到达目标点。此过程可以通过4次折射得到。如公式(1)所示：

其中，Tr_i(i＝1,2,3,4)表示依次经过的4次折射过程。

设(K_t，L_t，M_t表示矢量的方向余弦)，则方位角Θ_t和俯仰角Φ_t由下面公式计算：

${\mathrm{\&Theta;}}_t=\left\{\begin{array}{c}arctan\left(\frac{L_t}{K_t}\right),K_t\&GreaterEqual;0\\ arctan\left(\frac{L_t}{K_t}\right)+\mathrm{\&pi;},K_t<0\end{array}\right.---\left(2\right)$

Φ_t＝arccos(M_t) (3)

2)由目标的方位角Θ_t和俯仰角Φ_t，计算使目标成像在探测器的视场中心时，两个棱镜需要旋转到的新位置θ₁ ^*、θ₂ ^*。

计算过程如下：由于光路是可逆的，为了使目标成像在视场中心，由探测器中心出射的光线(0,0,1)将到达目标点。

按两步法计算θ₁ ^*和θ₂ ^*：

第一步：由俯仰角Φ_t计算Δθ＝|θ₁-θ₂|。具体方法为保持第一棱镜1的位置不动，采用优化方法得到Δθ＝|θ₁-θ₂|。优化方法的原理为：由于两个棱镜的夹角在0°～180°或180°～360°范围时，入射光线(0,0,1)对应的出射光线的俯仰角和方位角与棱镜夹角的大小是单调的，迭代优化算法也会很快收敛到全局极值。具体流程如图3所示：

步骤1、取棱镜夹角为转步骤2；

步骤2、通过公式(1)-公式(3)计算得到俯仰角Φ_i，转步骤3；

步骤3、如果|Φ_i-Φ_t|＜ε(ε为设定的精度要求)，Δθ_i+1＝Δθ_i，流程结束；否则转步骤4；

步骤4、如果Φ_i＜Φ_t，取如果Φ_i＞Φ_t，取转步骤5；

步骤5、更新i＝i+1；转步骤2。

第二步：在第一步基础上，得到了棱镜夹角Δθ＝|θ₁-θ₂|，通过公式(1)-公式(3)，得到2个方位角Θ₀和Θ₀'，则两个棱镜需要同步旋转的角度为Θ_t-Θ₀和Θ_t-Θ₀'，则两个棱镜需要旋转到的最终位置有两组解：

或

3)通过控制器8控制第一电机3和第二电机4运动，使其带动第一棱镜1和第二棱镜2旋转到位置θ₁ ^*、θ₂ ^*，目标将被锁定在探测器7的视场中心。

本发明与现有技术相比的优点在于：

1.本发明提出的控制算法解决了旋转双棱镜用于目标跟踪的问题，通过探测器闭环，使其可应用于对运动目标的跟踪。

2.此装置结构紧凑、转动惯量低、响应迅速。

附图说明

图1为本发明的装置结构图；

图2为系统的坐标示意图；

图3为优化流程图；

图4为目标方位角俯仰角曲线图

图5为跟踪过程中两棱镜的位置曲线图；

图6为目标在探测器7中的俯仰误差曲线图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。

首先结合图1介绍基于旋转双棱镜的跟踪装置。组成目标跟踪装置的主要部件有第一棱镜1、第二棱镜2、第一电机3、第二电机4、第一位置传感器5、第二位置传感器6、探测器7、控制器8。

其中第一棱镜1和第二棱镜2的顶角为7.5°，折射率为1.5；

第一电机3和第二电机4均为环形力矩电机，二者的转子分别与第一棱镜和第二棱镜直接相连，省却了中间传动环节，具有响应快、刚度高的特点；

第一位置传感器5、第二位置传感器6为圆光栅，具有精度高、重量轻的优点；第一位置传感器5测量第一棱镜1绕转轴的旋转角度θ₁，并将θ₁送到控制器8；第二位置传感器6测量第二棱镜2绕转轴的旋转角度θ₂，并将θ₂送到控制器8；

探测器7的本身视场设为0.5°，可以测量得到目标在探测器7上所成像点的方位角和俯仰角；矢量的方位角和俯仰角的定义如图2所示：坐标系为直角坐标系0xyz，矢量与x轴正向的夹角为方位角Θ，与z轴正向的夹角为俯仰角Φ。

控制器8接收第一棱镜的位置θ₁、第二棱镜的位置θ₂、探测器7上所成像点的方位角Θ和俯仰角Φ；输出电压信号V₁至第一电机3，输出电压信号V₂至第二电机4。

由于通过旋转棱镜，可以将视场扩大至约7.5°。设运动目标9以1°/s的速度经过视场，俯仰角最小设为0.5°(受棱镜死区限制)，目标的方位角和俯仰角曲线如图4所示，方位角范围为-85°到85°，俯仰角范围为0.5°到6°。

另外由于探测器本身视场有限(只有0.5°)，首先给定目标位置的引导数据，在目标成像在探测器7中后，按以下步骤完成目标的跟踪过程：

1)、由探测器7测量得到目标在探测器上所成像点的方位角Θ、俯仰角Φ；由第一位置传感器5测量得到第一棱镜1的位置θ₁，第二位置传感器6测量得到第二棱镜2的位置θ₂；由θ₁、θ₂、Θ、Φ计算得到目标的方位角Θ_t和俯仰角Φ_t。

计算过程如下：由目标发出的光在探测器上形成像点，由于光路是可逆的，那么由探测器出射的光束经过两个棱镜折射后的出射光线将到达目标点。此过程可以通过4次折射得到。如公式(1)所示：

其中，Tr_i(i＝1,2,3,4)表示依次经过的4次折射过程。

设(K_t，L_t，M_t表示矢量的方向余弦)，则方位角Θ_t和俯仰角Φ_t由下面公式计算：

${\mathrm{\&Theta;}}_t=\left\{\begin{array}{c}arctan\left(\frac{L_t}{K_t}\right),K_t\&GreaterEqual;0\\ arctan\left(\frac{L_t}{K_t}\right)+\mathrm{\&pi;},K_t<0\end{array}\right.---\left(2\right)$

Φ_t＝arccos(M_t) (3)

2)、由目标的方位角Θ_t和俯仰角Φ_t，计算使目标成像在探测器的视场中心时，两个棱镜需要旋转到的新位置θ₁ ^*、θ₂ ^*。

计算过程如下：由于光路是可逆的，为了使目标成像在视场中心，由探测器中心出射的光线(0,0,1)将到达目标点。

按两步法计算θ₁ ^*和θ₂ ^*：

第一步：由俯仰角Φ_t计算Δθ＝|θ₁-θ₂|。具体方法为保持第一棱镜1的位置不动，采用优化方法得到Δθ＝|θ₁-θ₂|。优化方法的原理为：由于两个棱镜的夹角在0°～180°或180°～360°范围时，入射光线(0,0,1)对应的出射光线的俯仰角和方位角与棱镜夹角的大小是单调的，迭代优化算法也会很快收敛到全局极值。具体流程如图3所示：

步骤1、取棱镜夹角为转步骤2；

步骤2、通过公式(1)-公式(3)计算得到俯仰角Φ_i，转步骤3；

步骤3、如果|Φ_i-Φ_t|＜ε(ε为设定的精度要求，此处设为5″)，Δθ_i+1＝Δθ_i，流程结束；否则转步骤4；

步骤4、如果Φ_i＜Φ_t，取如果Φ_i＞Φ_t，取转步骤5；

步骤5、更新i＝i+1；转步骤2。

第二步：在第一步基础上，得到了棱镜夹角Δθ＝|θ₁-θ₂|，通过公式(1)-公式(3)，得到2个方位角Θ₀和Θ₀'，则两个棱镜需要同步旋转的角度为Θ_t-Θ₀和Θ_t-Θ₀'，则两个棱镜需要旋转到的最终位置有两组解：

或

选择哪组解根据棱镜期望位置和当前位置之差决定，取其中较小的一组解。

3)、通过控制器8控制第一电机3和第二电机4运动，使其带动第一棱镜1和第二棱镜2旋转到位置θ₁ ^*、θ₂ ^*，控制算法采用PID算法控制棱镜的转动，在棱镜位置到达期望位置后，目标将被锁定在探测器7的视场中心。图5为跟踪过程中两棱镜旋转的位置曲线；图6为目标在探测器7中的俯仰误差曲线。可以看出除开始时的过渡过程外，其他时间的俯仰误差均小于0.1mrad。

以上所述，仅为本发明中的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉该技术的人在本发明所揭示的技术范围内，可理解想到的变换或替换，都应涵盖在本发明的包含范围之内。

Claims (2)

1.一种基于旋转双棱镜的跟踪装置的控制方法，所述的基于旋转双棱镜的跟踪装置包括：第一棱镜(1)、第二棱镜(2)、第一电机(3)、第二电机(4)、第一位置传感器(5)、第二位置传感器(6)、探测器(7)和控制器(8)，其中，两个棱镜旋转轴、两个电机转子旋转轴和探测器为同轴，第一棱镜(1)和第二棱镜(2)具有相同的顶角和折射率，第一电机(3)和第二电机(4)均为环形力矩电机，第一电机(3)的转子与第一棱镜(1)直接相连，第二电机(4)的转子与第二棱镜(2)直接相连；第一位置传感器(5)测量第一棱镜(1)绕转轴的旋转角度θ₁，并将θ₁送到控制器(8)；第二位置传感器(6)测量第二棱镜(2)绕转轴的旋转角度θ₂，并将θ₂送到控制器(8)；探测器(7)能够测量得到目标在探测器上所成像点的方位角Θ和俯仰角Φ，并将其送到控制器(8)，控制器(8)接收第一棱镜(1)的位置θ₁、第二棱镜(2)的位置θ₂、探测器(7)上所成像点的方位角Θ和俯仰角Φ，输出电压信号V₁至第一电机(3)，输出电压信号V₂至第二电机(4)，其特征在于，该控制方法包括以下步骤：

步骤1)、由探测器测量得到目标在探测器上所成像点的方位角Θ、俯仰角Φ；由第一位置传感器测量得到第一棱镜的位置θ₁，第二位置传感器测量得到第二棱镜的位置θ₂；由θ₁、θ₂、Θ、Φ计算得到目标的方位角Θ_t和俯仰角Φ_t；

计算过程如下：由目标发出的光在探测器上形成像点，由于光路是可逆的，那么由探测器出射的光束经过两个棱镜折射后的出射光线将到达目标点，此过程可以通过4次折射得到，如公式(1)所示：

其中，Tr_i表示依次经过的4次折射过程，i＝1,2,3,4；

设K_t，L_t，M_t表示矢量的方向余弦，则方位角Θ_t和俯仰角Φ_t由下面公式计算：

${\mathrm{\&Theta;}}_t=\left\{\begin{array}{c}\arctan \left(\frac{L_t}{K_t}\right),K_t\&GreaterEqual;0\\ \arctan \left(\frac{L_t}{K_t}\right)+\mathrm{\&pi;},K_t<0\end{array}\right.---\left(2\right)$

Φ_t＝arccos(M_t) (3)

步骤2)、由目标的方位角Θ_t和俯仰角Φ_t，计算使目标成像在探测器的视场中心时，两个棱镜需要旋转到的新位置θ₁ ^*、θ₂ ^*；

计算过程如下：由于光路是可逆的，为了使目标成像在视场中心，由探测器中心出射的光线(0,0,1)将到达目标点；

按两步法计算θ₁ ^*和θ₂ ^*：

第一步：由俯仰角Φ_t计算△θ＝|θ₁-θ₂|，具体方法为保持第一棱镜(1)的位置不动，采用优化方法得到△θ＝|θ₁-θ₂|；优化方法的原理为：由于两个棱镜的夹角在0°～180°或180°～360°范围时，入射光线(0,0,1)对应的出射光线的俯仰角和方位角与棱镜夹角的大小是单调的，迭代优化算法也会很快收敛到全局极值，具体流程如下：

步骤1、取棱镜夹角为i＝1，转步骤2；

步骤2、通过公式(1)-公式(3)计算得到俯仰角Φ_i，转步骤3；

步骤3、如果|Φ_i-Φ_t|<ε，ε为设定的精度要求，△θ_i+1＝△θ_i，流程结束；否则转步骤4；

步骤4、如果Φ_i<Φ_t，取如果Φ_i>Φ_t，取转步骤5；

步骤5、更新i＝i+1；转步骤2；

第二步：在第一步基础上，得到了棱镜夹角△θ＝|θ₁-θ₂|，通过公式(1)-公式(3)，得到2个方位角Θ₀和Θ₀'，则两个棱镜需要同步旋转的角度为Θ_t-Θ₀和Θ_t-Θ₀'，则两个棱镜需要旋转到的最终位置有两组解：

或

步骤3)、通过控制器控制第一电机和第二电机运动，使其带动第一棱镜和第二棱镜旋转到位置θ₁ ^*、θ₂ ^*，目标将被锁定在探测器(7)的视场中心。

2.根据权利要求1所述的控制方法，其特征在于通过优化方法确定两个棱镜需要旋转到的新位置θ₁ ^*、θ₂ ^*，从而保证目标成像在探测器的视场中心。