基于三轴云台的目标凝视与扫视控制系统

摘要

本发明提供一种基于三轴云台的目标凝视与扫视控制系统，包括三维坐标传感模块、三轴电子罗盘模块和控制模块，控制模块依据三维坐标传感模块提供的云台的经度、纬度和高度与三轴电子罗盘模块提供云台当前指向与磁北的方位偏角η，确定云台与目标点的相对方位角和俯仰角，实现目标的凝视或扫视。该系统基于三轴自稳云台系统，在载具三维运动或静止条件下，可实现对地面任意地理三维坐标点或合作(三维坐标已知)动目标的凝视，并可对两个任意地理坐标点连成的直线实现固定偏角的扫视。

说明书

技术领域

本发明涉及一种基于三轴云台的目标凝视与扫视控制系统。

背景技术

目前需要一种控制系统解决在载具三维运动或静止条件下，实现对地面任意地理三维坐标点或合作动目标即三维坐标已知的凝视。但并未有相关专利文件或其他公开文献公开该控制系统。

此外，由于AMR磁阻传感器本身的制造、安装位置和环境因素的影响，电子罗盘的测量会出现误差，从而影响到精度。罗差主要是指由电子罗盘载体周围存在的铁磁材料的影响而造成的误差，是地磁场测量时所特有的一种误差。它对电子罗盘的精度影响最大，可达十几甚至几十度，是电子罗盘误差的主要来源。因此，应当对测量进行校准。

上述问题是在三轴云台的目标凝视与扫视过程中应当予以考虑并解决的问题。

发明内容

本发明的目的是提供一种基于三轴云台的目标凝视与扫视控制系统解决在载具三维运动或静止条件下，实现对地面任意地理三维坐标点或合作动目标即三维坐标已知的凝视的问题。

本发明的技术解决方案是：

一种基于三轴云台的目标凝视与扫视控制系统，包括三维坐标传感模块、三轴电子罗盘模块和控制模块，

三维坐标传感模块：提供云台的经度、纬度和高度；

三轴电子罗盘模块：提供云台当前指向与磁北的方位偏角η，包括三轴加速度传感器和三轴磁阻传感器，三轴加速度传感器提供云台的三维倾角包括俯仰角和翻滚角θ′，三轴磁阻传感器测量三轴方向的磁场强度H_x,H_y,H_z；

控制模块：依据三维坐标传感模块提供的云台的经度、纬度和高度与三轴电子罗盘模块提供云台当前指向与磁北的方位偏角η，确定云台与目标点的相对方位角和俯仰角，实现目标的凝视或扫视。

进一步地，控制模块中，为实现目标的凝视，确定云台F与目标点P的相对方位角，即云台从当前指向偏转到目标点P的方位角为：

其中，η为云台当前指向与磁北的夹角，α为云台F与目标点P的直线FP在XOY平面的投影直线F′P′与真北的夹角，称为磁偏角为磁北与真北之间的夹角。

进一步地，控制模块中，为实现固定夹角为的目标扫视，确定云台F与目标点P的方位角为：

其中，α为云台F与目标点P的直线FP在XOY平面的投影直线F′P′与真北的夹角，η为云台当前指向与磁北的夹角，称为磁偏角为磁北与真北之间的夹角。

进一步地，根据测得的三轴方向的磁场强度H_x,H_y,H_z，俯仰角和翻滚角θ′，并对磁阻传感器的测量值校准后，求出校准后的云台当前指向与磁北的夹角η。

进一步地，求出校准后的云台当前指向与磁北的夹角η具体为：

其中是罗差校准后的值，由式(24)知，对磁阻传感器的测量值校准后并分别求式(24)中的比值即可得到校准后的方向夹角η。

进一步地，对磁阻传感器的测量值校准，具体为：在某方向电子罗盘采样的数据H_x和H_y对应椭圆上一点的坐标，该椭圆的方程为：

AH_x²+BH_y²+CH_xH_y+DH_x+EH_y＝F>

其中，F常数且大小与地磁场大小有关，求参数A、B、C、D、E确定该椭圆方程；

通过下式将该椭圆变为圆：

H＝K^-1·Φ^-1(H₁-B)>

其中，

式(21)和(22)中，H₁为测量值，H为校准值，而可由下式导出：

进一步地，控制模块中，为实现目标的凝视或扫视，确定云台F与目标点P的相对俯仰角β为：

$\beta =arctan\left(\frac{h^{\prime }-h}{r^{\prime }}\right)---\left(6\right)$

其中，h′和h分别为云台与目标点的海拔高度，r′为云台F与目标点P在XOY平面的投影F′和P′的距离。

进一步地，还包括无线数传模块，无线数传模块：实现与地面站的通信，任务模式和参数由地面站设置并通过无线数传模块模块上传，云台的实时工作状态通过无线数传模块模块下载至地面站。

进一步地，通过无线数传模块箱地面站申请执行磁阻传感器校准，在允许校准磁阻传感器后，对磁阻传感器进行校准，校准成功后，向地面站发送校准成功信息。

进一步地，控制模块还对落架的放下或收起进行控制，实现降落安全保护，具体为：在云台高低小于安全高度时，放下落架；在云台高度大于或等于安全高度时，允许收起落架。

本发明的有益效果是：该种基于三轴云台的目标凝视与扫视控制系统，在载具三维运动或静止条件下，可实现对地面任意地理三维坐标点或合作(三维坐标已知)动目标的凝视，并可对两个任意地理坐标点连成的直线实现固定偏角的扫视。

附图说明

图1是本发明实施例基于三轴云台的目标凝视与扫视控制系统的说明框图。

图2是实施例中云台与坐标点的几何关系示意图。

图3是实施例中云台与坐标直线的几何关系示意图。

图4是实施例中罗盘任意姿态示意图。

图5是实施例基于三轴云台的目标凝视与扫视控制系统的初始化流程示意图。

图6是基于三轴云台的目标凝视与扫视控制系统的通信流程示意图。

图7是基于三轴云台的目标凝视与扫视控制系统的任务流程示意图。

图8是基于三轴云台的目标凝视与扫视控制系统的降落安全保护流程示意图。

图9是基于三轴云台的目标凝视与扫视控制系统的任务动态添加流程示意图。

图10是基于三轴云台的目标凝视与扫视控制系统的任务动态删除流程示意图。

具体实施方式

下面结合附图详细说明本发明的优选实施例。

实施例

一种基于三轴云台的目标凝视与扫视控制系统，如图1，包括三维坐标传感模块、三轴电子罗盘模块和控制模块。

三维坐标传感模块：提供云台的经度、纬度和高度；用以提供参数：F_lat,F_long,h′，该模块由GPS/北斗模块和气压计模块组成。理想条件下，现阶段GPS/BeiDou模块的经度和纬度的定位精度为3m～5m，而高度定位精度约为20m～50m。为提高俯仰角β的估计精度，实施例引入了微型气压计模块，理想条件下，气压计模块的测高精度可达10cm，满足设计要求。

三轴电子罗盘模块：提供云台当前指向的方位偏角η，包括三轴加速度传感器和三轴磁阻传感器，由于本系统针对的是三轴云台系统，因此，云台任意姿态下，电子罗盘必须能够提供准确方位偏角。三轴加速度传感器提供云台的三维倾角包括俯仰角和翻滚角θ′，三轴磁阻传感器测量三轴方向的磁场强度H_x,H_y,H_z。

控制模块：依据三维坐标传感模块提供的云台的经度、纬度和高度与三轴电子罗盘模块提供云台当前指向与磁北的方位偏角η，确定云台与目标点的相对方位角和俯仰角，实现目标的凝视或扫视。

还包括无线数传模块：本系统与地面站之间的通信主要依靠无线数传模块，该模块由半双工无线数传模块和功放模块构成。任务模式和参数可由地面站设置并通过该模块上传，云台和设备的实时工作状态也可通过该模块下载至地面站。

实施例的基于三轴云台的目标凝视与扫视控制系统，初始化流程如图5。控制模块通过无线数传模块箱地面站申请执行磁阻传感器校准，在允许校准磁阻传感器后，对磁阻传感器进行校准，校准成功后，向地面站发送校准成功信息。

控制模块还对落架的放下或收起进行控制，如图8，实现降落安全保护，具体为：在云台高低小于安全高度时，放下落架；在云台高度大于或等于安全高度时，允许收起落架。

还包括控制信号输入输出端口，实施例系统与地面站通信流程如图6。系统执行任务流程如图7，任务动态添加流程如图9，任务动态删除流程如图10。系统提供手动控制和智能控制两种工作方式，系统提供占空比可调的PWM信号输入输出端口。其中手动控制方式的输出信号即输入信号的延迟，而智能控制方式的输出信号是输入信号与自动控制输出信号的线性叠加。

系统凝视三维空间坐标点的工作原理

任意地理坐标点都可以由经度、纬度和高度准确描述，为实现凝视的功能，需要确定云台与坐标点的相对方位角和俯仰角。本系统基于大地坐标系建立的云台F与目标坐标点P的几何关系如图2所示。其中OXYZ坐标系中，X轴指示纬线指向西，Y轴指示经线指向真北，Z轴指示海拔高度。图中F′和P′分别为F和P在XOY平面的投影，由图中几何关系可得：

x＝π*E_R/180*cos(P_lat/180*π)*|P_long-F_long| (1)

y＝|P_lat-F_lat|*100000 (2)

式(1)中E_R为地球半径的均值，这里取值为6371m，P_lat和F_lat分别为目标点纬度和云台纬度，而P_long和F_long分别为目标点经度和云台经度，这里经度和纬度值都用“度”格式表示。由此可知，x和y分别为经度和纬度的差值，进一步有：

$\begin{array}{c}\alpha =\pi -{\alpha }^{\prime }\\ =\pi -\arctan \left(\frac{x}{y}\right)\end{array}---\left(3\right)$

式(3)中α为直线F′P′与真北的夹角。云台当前指向、真北和磁北的关系如图1所示，其中云台当前指向与磁北的夹角为η，而磁北与真北之间的夹角称为磁偏角。由此可得，云台从当前指向偏转到目标点P的方位角为：

下面将介绍两者相对俯仰角的关系，根据勾股定理，F′和P′的距离为：

$r^{\prime }=\sqrt{x^2+y^2}---\left(5\right)$

假设云台与目标点的海拔高度分别为h′和h，如图1所示，两者相对俯仰角为：

$\beta =arctan\left(\frac{h^{\prime }-h}{r^{\prime }}\right)---\left(6\right)$

进一步的，两者径向距离为：

$r=\sqrt{r^{\prime 2}+{(h^{\prime }-h)}^2}---\left(7\right)$

至此，云台与目标点的方位与俯仰角都已求出。只要能够实时获得云台与目标点的三维坐标信息，即可利用上述原理可实现云台对任意目标点的凝视。

系统扫视三维空间直线的工作原理

为实现系统以固定夹角“扫视”三维空间直线的功能，需要系统运动且其运动轨迹不能与目标直线垂直。三维空间中的直线是由三维坐标点构成的，由上述可知，确定云台与直线上各坐标点的相对方位角和俯仰角，并结合云台与目标直线的相对运动，即可实现“扫视”。

以起点B和终点A描述目标直线，基于大地坐标系建立的云台F、目标直线起点和终点的几何关系如图2所示。假设要求云台以固定的夹角扫视直线AB，首先来讨论方位角α的计算方。将F、A和B点分别投影到XOY平面内，得到了F′A′和B′点，为方便描述直角夹角的“扫视”几何关系，添加一条与直线A′B′垂直的辅助线F′B″。

根据勾股定理，可得：

$\omega =arccos\left(\frac{|A^{\prime }\_long-F^{\prime }\_long|}{\overline{A^{\prime }{F}^{\prime }}}\right)---\left(8\right)$

上式中ω表示直线A′F′与纬线的夹角，F′_long和A′_long分别为云台F经度和A点经度。表示直线A′F′的长度，下同。

$\overline{A^{\prime }{F}^{\prime }}=\sqrt{{(A^{\prime }\_long-F^{\prime }\_long)}^2+{(A^{\prime }\_lat-F^{\prime }\_lat)}^2}---\left(9\right)$

上式中F′_lat和A′_lat分别为云台F纬度和A点纬度。根据三角函数，进一步可得：

$\lambda =arccos[({\overline{A^{\prime }{F}^{\prime }}}^2+{\overline{B^{\prime }{F}^{\prime }}}^2-{\overline{A^{\prime }{B}^{\prime }}}^2)/(2·\overline{A^{\prime }{F}^{\prime }}·\overline{B^{\prime }{F}^{\prime }})]---\left(10\right)$

$\theta =arccos[({\overline{A^{\prime }{B}^{\prime }}}^2+{\overline{B^{\prime }{F}^{\prime }}}^2-{\overline{A^{\prime }{F}^{\prime }}}^2)/(2·\overline{A^{\prime }{B}^{\prime }}·\overline{B^{\prime }{F}^{\prime }})]---\left(11\right)$

式(10)中λ表示直线A′F′与B′F′的夹角，式(11)中θ表示直线B′F′与A′B′的夹角。

由图3中几何关系可得：

$ϵ=\frac{\pi }{2}-(\pi -\theta )=\theta -\frac{\pi }{2}---\left(12\right)$

α＝ω-λ-ε (13)

上式中ε表示直线B′F′与B″F′的夹角。

最后，得到的方位角为：

如果要求的固定夹角为则最终的方位角为：

其中，α为云台F与目标点P的直线FP在XOY平面的投影直线F′P′与真北的夹角，η为云台当前指向与磁北的夹角，称为磁偏角为磁北与真北之间的夹角。

至此，得到了云台的方位偏角，而俯仰角的计算方法与前述相同。

云台指向与磁北夹角估计的方法

利用三轴电子罗盘(即三轴磁阻传感器和三轴加速度传感器的合称)估计云台指向与磁北夹角η的方法。设罗盘平面三个方向的磁场强度分别为H_x,H_y,H_z，而地球表面三个方向的磁场强度为H_X,H_Y,H_Z，H_X为前进方向，如图4所示，N′--S′表示磁南北极轴线，表示俯仰角，θ′表示翻滚角，这两者可由加速度传感器提供，η为云台指向与磁北夹角。三轴电子罗盘就是根据测得的H_x,H_y,H_z，俯仰角和翻滚角θ′求得η。

根据坐标投影关系，当测得H_x,H_y,H_z，俯仰角和翻滚角θ′时,折算到地球平面磁场强度H_X,H_Y的计算公式为：

H_Y＝H_y>zsinθ′>

接下来可以计算云台指向与磁北夹角η:

η＝arctan(H_Y/H_X)>

AMR磁阻传感器的罗差校正

罗差的影响可认为是传感器测出的磁场在水平方向上两个分量的合成向量的顶点在平面上画出的一个椭圆，而用于三轴电子罗盘上时，罗差的影响可认为是传感器测出的三个磁场分量的合成向量的顶点在空间画出了一个椭球面，当罗差精确校正后，该合成向量的顶点在空间则可画出一个正球。

针对上述现象和原理，实施例利用云台系统提出了一种罗差校正方法，下面详细介绍。根据椭圆假设，在某方向电子罗盘采样的数据H_x和H_y对应椭圆上一点的坐标，该椭圆的方程可表示为一个椭圆的标准方程为：

AH_x²+BH_y²+CH_xH_y+DH_x+EH_y＝F>

其中，F大小与地磁场大小有关，可作为一个常数。由此可见，只要在XOY平面0度到360度范围内的5个方向上测出五组数据H_x和H_y，即可根据式(19)求出5个参数A、B、C、D、E，这样就可以确定该椭圆方程。接下来，可通过下式将椭圆变为圆：

H＝K^-1·Φ^-1(H₁-B)>

其中

式(20)和(21)中，H₁为测量值，H为校正值，而k_x,k_y,b_x,b_y可由下式导出：

通过上述方法，可以将椭圆变为圆。

设OXYZ为一个三维直角坐标系，有以上论述可知，测量值H_x、H_y和H_z的合成向量的顶点在该坐标系内呈现椭球状。因此，将三轴云台系统分别围绕X、Y、Z轴精确转动，可分别得到YOZ、XOZ和XOY平面内的3个椭圆。进一步的，可以用上述方法校正罗差，使得各平面内的椭圆变为圆。值得注意的是，将式(16)和(17)带入式(18)可以得到下式，

其中是罗差校正后的值，由式(23)中可知，只需要在上述三个平面内对磁阻传感器的测量值校正并求它们的比值即可得到校准后的方向夹角η。