基于三轴云台的目标凝视与扫视控制系统的制作方法
【专利摘要】本发明提供一种基于三轴云台的目标凝视与扫视控制系统，包括三维坐标传感模块、三轴电子罗盘模块和控制模块，控制模块依据三维坐标传感模块提供的云台的经度、纬度和高度与三轴电子罗盘模块提供云台当前指向与磁北的方位偏角η，确定云台与目标点的相对方位角和俯仰角，实现目标的凝视或扫视。该系统基于三轴自稳云台系统，在载具三维运动或静止条件下，可实现对地面任意地理三维坐标点或合作(三维坐标已知)动目标的凝视，并可对两个任意地理坐标点连成的直线实现固定偏角的扫视。
【专利说明】
基于三轴云台的目标凝视与扫视控制系统
技术领域
[0001] 本发明涉及一种基于三轴云台的目标凝视与扫视控制系统。
【背景技术】
[0002] 目前需要一种控制系统解决在载具三维运动或静止条件下，实现对地面任意地理 三维坐标点或合作动目标即三维坐标已知的凝视。但并未有相关专利文件或其他公开文献 公开该控制系统。
[0003] 此外，由于AMR磁阻传感器本身的制造、安装位置和环境因素的影响，电子罗盘的 测量会出现误差，从而影响到精度。罗差主要是指由电子罗盘载体周围存在的铁磁材料的 影响而造成的误差，是地磁场测量时所特有的一种误差。它对电子罗盘的精度影响最大，可 达十几甚至几十度，是电子罗盘误差的主要来源。因此，应当对测量进行校准。
[0004] 上述问题是在三轴云台的目标凝视与扫视过程中应当予以考虑并解决的问题。
【发明内容】
[0005] 本发明的目的是提供一种基于三轴云台的目标凝视与扫视控制系统解决在载具 三维运动或静止条件下，实现对地面任意地理三维坐标点或合作动目标即三维坐标已知的 凝视的问题。
[0006] 本发明的技术解决方案是：
[0007] 一种基于三轴云台的目标凝视与扫视控制系统，包括三维坐标传感模块、三轴电 子罗盘模块和控制模块，
[0008] 三维坐标传感模块:提供云台的经度、炜度和高度；
[0009] 三轴电子罗盘模块:提供云台当前指向与磁北的方位偏角n，包括三轴加速度传感 器和三轴磁阻传感器，三轴加速度传感器提供云台的三维倾角包括俯仰角#和翻滚角V， 三轴磁阻传感器测量三轴方向的磁场强度H x，Hy，HZ;
[0010] 控制模块：依据三维坐标传感模块提供的云台的经度、炜度和高度与三轴电子罗 盘模块提供云台当前指向与磁北的方位偏角n，确定云台与目标点的相对方位角和俯仰角， 实现目标的凝视或扫视。
[0011] 进一步地，控制模块中，为实现目标的凝视，确定云台F与目标点P的相对方位角， 即云台从当前指向偏转到目标点P的方位角为：
[0012] totctte _azimuih _ang - rj + iz + ep (4)
[0013] 其中，n为云台当前指向与磁北的夹角，a为云台F与目标点P的直线FP在XOY平面的 投影直线F'P'与真北的夹角，供称为磁偏角为磁北与真北之间的夹角。
[0014] 进一步地，控制模块中，为实现固定夹角为
的目标扫视，确定云台F与目 标点P的方位角为：
[0015] rotate_azimuth_cmg -d + r] + <p-^p (I5)
[0016] 其中，a为云台F与目标点P的直线FP在X0Y平面的投影直线F' P'与真北的夹角，
n为云台当前指向与磁北的夹角，称为磁偏角为磁北与真北之间的夹角。
[0017] 进一步地，根据测得的三轴方向的磁场强度Hx，Hy，Hz，俯仰角勿和翻滚角V，并对 磁阻传感器的测量值校准后，求出校准后的云台当前指向与磁北的夹角n。
[0018] 进一步地，求出校准后的云台当前指向与磁北的夹角n具体为：
[0020] 其中/_/￥是罗差校准后的值，由式(24)知，对磁阻传感器的测量值校准后并分别 求式(24)中的比值即可得到校准后的方向夹角n。
[0021] 进一步地，对磁阻传感器的测量值校准，具体为:在某方向电子罗盘采样的数据Hx 和Hy对应椭圆上一点的坐标，该椭圆的方程为：
[0022] AHx2+BHy2+CHxHy+DHx+EHy = F (19)
[0023] 其中，F常数且大小与地磁场大小有关，求参数A、B、C、D、E确定该椭圆方程；
[0024]通过下式将该椭圆变为圆：
[0025] h=K-1 ? 〇-HHi-B) (20) Ht eosf> sinf? 1 kr bt
[o026]其中，拓'd秦具卞
[0027] 式(21)和(22)中，压为测量值，H为校准值，而奶H A A可由下式导出：
[0029]进一步地，控制模块中，为实现目标的凝视或扫视，确定云台F与目标点P的相对俯 仰角0为：
[0031]其中，V和h分别为云台与目标点的海拔高度，V为云台F与目标点P在X0Y平面的 投影K和K的距离。
[0032] 进一步地，还包括无线数传模块，无线数传模块:实现与地面站的通信，任务模式 和参数由地面站设置并通过无线数传模块模块上传，云台的实时工作状态通过无线数传模 块模块下载至地面站。
[0033] 进一步地，通过无线数传模块箱地面站申请执行磁阻传感器校准，在允许校准磁 阻传感器后，对磁阻传感器进行校准，校准成功后，向地面站发送校准成功信息。
[0034] 进一步地，控制模块还对落架的放下或收起进行控制，实现降落安全保护，具体 为:在云台高低小于安全高度时，放下落架;在云台高度大于或等于安全高度时，允许收起 落架。
[0035]本发明的有益效果是:该种基于三轴云台的目标凝视与扫视控制系统，在载具三 维运动或静止条件下，可实现对地面任意地理三维坐标点或合作(三维坐标已知)动目标的 凝视，并可对两个任意地理坐标点连成的直线实现固定偏角
的扫视。
【附图说明】
[0036]图1是本发明实施例基于三轴云台的目标凝视与扫视控制系统的说明框图。
[0037]图2是实施例中云台与坐标点的几何关系示意图。
[0038]图3是实施例中云台与坐标直线的几何关系示意图。
[0039] 图4是实施例中罗盘任意姿态示意图。
[0040] 图5是实施例基于三轴云台的目标凝视与扫视控制系统的初始化流程示意图。 [0041]图6是基于三轴云台的目标凝视与扫视控制系统的通信流程示意图。
[0042]图7是基于三轴云台的目标凝视与扫视控制系统的任务流程示意图。
[0043]图8是基于三轴云台的目标凝视与扫视控制系统的降落安全保护流程示意图。 [0044]图9是基于三轴云台的目标凝视与扫视控制系统的任务动态添加流程示意图。 [0045]图10是基于三轴云台的目标凝视与扫视控制系统的任务动态删除流程示意图。
【具体实施方式】
[0046]下面结合附图详细说明本发明的优选实施例。
[0047] 实施例
[0048] 一种基于三轴云台的目标凝视与扫视控制系统，如图1，包括三维坐标传感模块、 三轴电子罗盘模块和控制模块。
[0049]三维坐标传感模块:提供云台的经度、炜度和高度;用以提供参数:F_lat，F_long， M，该模块由GPS/北斗模块和气压计模块组成。理想条件下，现阶段GPS/BeiDou模块的经度 和炜度的定位精度为3m~5m，而高度定位精度约为20m~50m。为提高俯仰角0的估计精度， 实施例引入了微型气压计模块，理想条件下，气压计模块的测高精度可达l〇cm，满足设计要 求。
[0050]三轴电子罗盘模块:提供云台当前指向的方位偏角n，包括三轴加速度传感器和三 轴磁阻传感器，由于本系统针对的是三轴云台系统，因此，云台任意姿态下，电子罗盘必须 能够提供准确方位偏角。三轴加速度传感器提供云台的三维倾角包括俯仰角Y和翻滚角 0'，三轴磁阻传感器测量三轴方向的磁场强度Hx，Hy，Hz。
[0051]控制模块：依据三维坐标传感模块提供的云台的经度、炜度和高度与三轴电子罗 盘模块提供云台当前指向与磁北的方位偏角n，确定云台与目标点的相对方位角和俯仰角， 实现目标的凝视或扫视。
[0052]还包括无线数传模块:本系统与地面站之间的通信主要依靠无线数传模块，该模 块由半双工无线数传模块和功放模块构成。任务模式和参数可由地面站设置并通过该模块 上传，云台和设备的实时工作状态也可通过该模块下载至地面站。
[0053]实施例的基于三轴云台的目标凝视与扫视控制系统，初始化流程如图5。控制模块 通过无线数传模块箱地面站申请执行磁阻传感器校准，在允许校准磁阻传感器后，对磁阻 传感器进行校准，校准成功后，向地面站发送校准成功信息。
[0054] 控制模块还对落架的放下或收起进行控制，如图8，实现降落安全保护，具体为:在 云台高低小于安全高度时，放下落架;在云台高度大于或等于安全高度时，允许收起落架。
[0055] 还包括控制信号输入输出端口，实施例系统与地面站通信流程如图6。系统执行任 务流程如图7,任务动态添加流程如图9,任务动态删除流程如图10。系统提供手动控制和智 能控制两种工作方式，系统提供占空比可调的PWM信号输入输出端口。其中手动控制方式的 输出信号即输入信号的延迟，而智能控制方式的输出信号是输入信号与自动控制输出信号 的线性叠加。
[0056]系统凝视三维空间坐标点的工作原理
[0057]任意地理坐标点都可以由经度、炜度和高度准确描述，为实现凝视的功能，需要确 定云台与坐标点的相对方位角和俯仰角。本系统基于大地坐标系建立的云台F与目标坐标 点P的几何关系如图2所示。其中0XYZ坐标系中，X轴指示炜线指向西，Y轴指示经线指向真 北，Z轴指示海拔高度。图中K和K分别为F和P在X0Y平面的投影，由图中几何关系可得：
[0058] x = 3i*E_R/180*cos(P_lat/180*3T)* | P_l〇ng_F_long | (1)
[0059] y= |P_lat_F_lat|*100000 (2)
[0060] 式（1)中E_R为地球半径的均值，这里取值为6371111，？_1&丨和?_1&丨分别为目标点炜 度和云台炜度，而?_1〇即和?_1〇即分别为目标点经度和云台经度，这里经度和炜度值都用 "度"格式表示。由此可知，x和y分别为经度和炜度的差值，进一步有：
(3)
[0062] 式⑶中a为直线FY与真北的夹角。云台当前指向、真北和磁北的关系如图1所 示，其中云台当前指向与磁北的夹角为n，而磁北与真北之间的夹角识称为磁偏角。由此可 得，云台从当前指向偏转到目标点P的方位角为：
[0063] rotate ^azimuth _ang = rj + a + p (4)
[0064]下面将介绍两者相对俯仰角的关系，根据勾股定理，K和K的距离为：
[0065] r - ^Jxz + v (5)
[0066] 假设云台与目标点的海拔高度分别为V和h，如图1所示，两者相对俯仰角为： (6)
[0068] 进一步的，两者径向距离为：
[0069] r - ^Jr'" +(h! -h)~ (7)
[0070] 至此，云台与目标点的方位与俯仰角都已求出。只要能够实时获得云台与目标点 的三维坐标信息，即可利用上述原理可实现云台对任意目标点的凝视。
[0071] 系统扫视三维空间直线的工作原理
[0072] 为实现系统以固定夹角"扫视"三维空间直线的功能，需要系统运动且其运动轨迹 不能与目标直线垂直。三维空间中的直线是由三维坐标点构成的，由上述可知，确定云台与 直线上各坐标点的相对方位角和俯仰角，并结合云台与目标直线的相对运动，即可实现"扫 视"。
[0073] 以起点B和终点A描述目标直线，基于大地坐标系建立的云台F、目标直线起点和终 点的几何关系如图2所示。假设要求云台以固定的夹角扫视直线AB，首先来讨论方位角a
的计算方。将F、A和B点分别投影到X0Y平面内，得到了 W和V点，为方便描述直角夹角的 "扫视"几何关系，添加一条与直线# B'垂直的辅助线K B〃。
[0074]根据勾股定理，可得：
(H)
[0076]上式中《表示直线AV与炜线的夹角，K_long和Y_long分别为云台F经度和A点 经度。表示直线# K的长度，下同。
[0077] = ^(.-fjong-F'Jongf +(A' lat-F'(9)
[0078]上式中Kjat和y_lat分别为云台F炜度和A点炜度。根据三角函数，进一步可得：
[0079] 1 = arccos (Zp,： + Wf',： -ArB'1)/(2>JF,>WF,) (l〇)
[0080] 9 - arccos (^，2 + W'2 - )/( ) (11)
[0081 ] 式(10)中A表示直线AV与BV的夹角，式(11)中0表示直线BV与AW的夹角。
[0082] 由图3中几何关系可得：
(12)
[0084] a = 〇 -X-e (13)
[0085] 上式中e表示直线与rF'的夹角。
[0086]最后，得到的方位角为：
[0088]如果要求的固定夹角为
，则最终的方位角为： (14) (1力
[0090] 其中，a为云台F与目标点P的直线FP在X0Y平面的投影直线F' P'与真北的夹角，
n为云台当前指向与磁北的夹角，^称为磁偏角为磁北与真北之间的夹角。
[0091] 至此，得到了云台的方位偏角，而俯仰角的计算方法与前述相同。
[0092]云台指向与磁北夹角估计的方法
[0093] 利用三轴电子罗盘（即三轴磁阻传感器和三轴加速度传感器的合称)估计云台指 向与磁北夹角n的方法。设罗盘平面三个方向的磁场强度分别为&，^，&，而地球表面三个 方向的磁场强度为出，扮，出，版为前进方向，如图4所示小/-3/表示磁南北极轴线，#表示 俯仰角，V表示翻滚角，这两者可由加速度传感器提供，n为云台指向与磁北夹角。三轴电子 罗盘就是根据测得的&，馬具，俯仰角，和翻滚角0 /求得11。
[0094] 根据坐标投影关系，当测得仏具具，俯仰角￥和翻滚角0/时，折算到地球平面磁 场强度Hx，Hy的计算公式为：
[0095] 汉尤:=cos < + 汉sin g'sin 免,-.笔.cos# sin# (1.6.)
[0096] Hy=Hy cosQ7+Hzsin9/ (17)
[0097]接下来可以计算云台指向与磁北夹角n:
[0098] q = arctan(HY/Hx) (18)
[0099] AMR磁阻传感器的罗差校正
[0100]罗差的影响可认为是传感器测出的磁场在水平方向上两个分量的合成向量的顶 点在平面上画出的一个椭圆，而用于三轴电子罗盘上时，罗差的影响可认为是传感器测出 的三个磁场分量的合成向量的顶点在空间画出了一个椭球面，当罗差精确校正后，该合成 向量的顶点在空间则可画出一个正球。
[0101] 针对上述现象和原理，实施例利用云台系统提出了一种罗差校正方法，下面详细 介绍。根据椭圆假设，在某方向电子罗盘采样的数据Hx和Hy对应椭圆上一点的坐标，该椭圆 的方程可表不为一个椭圆的标准方程为：
[0102] AHx2+BHy2+CHxHy+DHx+EHy = F (19)
[0103]其中，F大小与地磁场大小有关，可作为一个常数。由此可见，只要在X0Y平面0度到 360度范围内的5个方向上测出五组数据Hx和Hy，即可根据式（19)求出5个参数六、8、(：、04，这 样就可以确定该椭圆方程。接下来，可通过下式将椭圆变为圆：
[0104] h=K-1 ? 〇-HHi-B) (20)
[0105] 其中 H ~\ 「cost?? sin<p . 「女「k
[0106] 珲= ，?= . B二 (21) \_tiy] sm炉 cos炉」 L^V」 L^i-
[0107] 式(20)和(21)中，压为测量值，H为校正值，而朽1^义，1^，1^可由下式导出：
[0109] 通过上述方法，可以将椭圆变为圆。
[0110] 设0XYZ为一个三维直角坐标系，有以上论述可知，测量值Hx、Hy和Hz的合成向量的 顶点在该坐标系内呈现椭球状。因此，将三轴云台系统分别围绕X、Y、Z轴精确转动，可分别 得到Y0Z、X0Z和X0Y平面内的3个椭圆。进一步的，可以用上述方法校正罗差，使得各平面内 的椭圆变为圆。值得注意的是，将式(16)和(17)带入式(18)可以得到下式，
[0112]其中好是罗差校正后的值，由式（23)中可知，只需要在上述三个平面内对磁阻传 感器的测量值校正并求它们的比值即可得到校准后的方向夹角n。
【主权项】
1. 一种基于三轴云台的目标凝视与扫视控制系统，其特征在于：包括三维坐标传感模 块、三轴电子罗盘模块和控制模块， 三维坐标传感模块:提供云台的经度、炜度和高度； 三轴电子罗盘模块:提供云台当前指向与磁北的方位偏角n，包括三轴加速度传感器和 三轴磁阻传感器，三轴加速度传感器提供云台的三维倾角包括俯仰角f'和翻滚角V，三轴 磁阻传感器测量三轴方向的磁场强度H x，Hy，Hz; 控制模块:依据三维坐标传感模块提供的云台的经度、炜度和高度与三轴电子罗盘模 块提供云台当前指向与磁北的方位偏角n，确定云台与目标点的相对方位角和俯仰角，实现 目标的凝视或扫视。2. 如权利要求1所述的基于三轴云台的目标凝视与扫视控制系统，其特征在于，控制模 块中，为实现目标的凝视，确定云台F与目标点P的相对方位角，即云台从当前指向偏转到目 标点P的方位角为：(4) 其中，η为云台当前指向与磁北的夹角，α为云台F与目标点P的直线FP在XOY平面的投影 直线F'P'与真北的夹角，F称为磁偏角为磁北与真北之间的夹角。3. 如权利要求2所述的基于三轴云台的目标凝视与扫视控制系统，其特征在于，控制模 块中，为实现固定夹角为P e[^，?)的目标扫视，确定云台F与目标点P的方位角为：(15) 其中，α为云台F与目标点P的直线FP在XOY平面的投影直线F7 P7与真北的夹角， a = & + i η为云台当前指向与磁北的夹角，供称为磁偏角为磁北与真北之间的夹角。4. 如权利要求1-3任一项所述的基于三轴云台的目标凝视与扫视控制系统，其特征在 于:根据测得的三轴方向的磁场强度&，^，^，俯仰角#和翻滚角0'，并对磁阻传感器的测 量值校准后，求出校准后的云台当前指向与磁北的夹角η。5. 如权利要求4所述的基于三轴云台的目标凝视与扫视控制系统，其特征在于，求出校 准后的云台当前指向与磁北的夹角η具体为：(23) 其中是罗差校准后的值，由式（24)知，对磁阻传感器的测量值校准后并分别求式 (24)中的比值即可得到校准后的方向夹角η。6. 如权利要求5所述的基于三轴云台的目标凝视与扫视控制系统，其特征在于，对磁阻 传感器的测量值校准，具体为:在某方向电子罗盘采样的数据Hx和Hy对应椭圆上一点的坐 标，该椭圆的方程为： AHx2+BHy2+CHxHy+DHx+EHy=F (19) 其中，F常数且大小与地磁场大小有关，求参数A、B、C、D、E确定该椭圆方程； 通过下式将该椭圆变为圆：7. 如权利要求1-3任一项所述的基于三轴云台的目标凝视与扫视控制系统，其特征在 于，控制模块中，为实现目标的凝视或扫视,确宙云台F与目标点P的相对俯仰角β为：(6) 其中，V和h分别为云台与目标点的海拔高度，V为云台F与目标点P在XOY平面的投影 FlW的距离。8. 如权利要求1-3任一项所述的基于三轴云台的目标凝视与扫视控制系统，其特征在 于:还包括无线数传模块，无线数传模块:实现与地面站的通信，任务模式和参数由地面站 设置并通过无线数传模块模块上传，云台的实时工作状态通过无线数传模块模块下载至地 面站。9. 如权利要求8所述的基于三轴云台的目标凝视与扫视控制系统，其特征在于:通过无 线数传模块箱地面站申请执行磁阻传感器校准，在允许校准磁阻传感器后，对磁阻传感器 进行校准，校准成功后，向地面站发送校准成功信息。10. 如权利要求1-3任一项所述的基于三轴云台的目标凝视与扫视控制系统，其特征在 于:控制模块还对落架的放下或收起进行控制，实现降落安全保护，具体为:在云台高低小 于安全高度时，放下落架;在云台高度大于或等于安全高度时，允许收起落架。
【文档编号】G01C17/38GK105892498SQ201610202303
【公开日】2016年8月24日
【申请日】2016年4月1日
【发明人】俞翔
【申请人】南京工程学院