一种三轴稳定伺服平台的姿态解算及补偿方法
【技术领域】
[0001 ]本发明属于伺服稳定平台控制领域，涉及姿态解算和补偿方法，具体地说，是一种 三轴稳定伺服平台的姿态解算及补偿方法。
【背景技术】
[0002] 机载光电平台是无人机上用于跟踪和捕获目标的平台，具有机动灵活、适应性强、 实时准确等特点。作为无人机的重要任务设备，平台上通常搭载有可见光、红外和激光等适 用于不同环境的任务载荷，是跟踪过程中实现视轴指向变化和视轴陀螺稳定的执行机构， 使得视轴在飞机姿态发生变化时保持稳定，同时可以进行图象数据的实时压缩，通过无线 传输将信号返回地面。因此对于三轴稳定平台的设计与控制就尤为重要了。
【发明内容】
[0003] 为了克服现有技术的缺点，本发明提供一种三轴稳定伺服平台的姿态解算及补偿 方法。它结构简单，机动灵活，适应性强，实时准确。
[0004] 本发明解决其技术问题所采取的技术方案是:包括如下步骤：
[0005] 步骤一、定义无人机姿态角、负载框架角和负载姿态角；
[0006] 步骤二、建立坐标变换矩阵；
[0007] 步骤三、计算负载姿态角；
[0008] 步骤四、稳定伺服平台的补偿方法；
[0009] 步骤五、通过控制系统，调整稳定伺服平台，保持天线稳定。
[0010] 本发明结构简单，机动灵活，适应性强，实时准确。
【附图说明】
[0011]下面结合附图和实施例对本发明进一步说明。
[0012] 图1为本发明无人机坐标系示意图；
[0013] 图2为本发明三轴稳定平台示意图；
[0014]图3为本发明负载坐标系不意图；
[0015]图4为本发明设备连接方块图；
[0016]图5为本发明惯导输出角曲线图；
[0017] 图6为本发明负载框架角曲线图；
[0018] 图7为本发明负载姿态角曲线图。
【具体实施方式】
[0019] 本发明包括下列步骤：
[0020] 步骤一、定义无人机姿态角、负载框架角和负载姿态角：
[0021] 无人机姿态角，即无人机与大地坐标系的夹角。包括无人机航向角、无人机纵摇角 和无人机横摇角，无人机坐标系如图1所示，具体定义如下：
[0022] 无人机航向角：从真北方向开始顺时针到无人机机首方向的夹角，在水平面内测 量，顺时针为正，通过惯导系统测量，用Φ表示，范围〇°~360°。
[0023] 无人机纵摇角:从无人机首尾线(纵轴0Y)与水平面的夹角，在铅垂面内测量，机首 在水平面上方为正，通过惯导系统测量，用Θ表示，范围-90°~90°。
[0024] 无人机横摇角:为无人机绕首尾线(纵轴0Y)旋转的角度，即无人机横剖面(Χ0Ζ)和 水平面的交线与无人机横轴(0Y)之间的夹角，测量平面垂直于无人机机翼平面且垂直于首 尾线，右翼下倾为正，通过惯导系统测量，用r表示，范围-180°~180°。
[0025] 负载框架角包括方位框架角、滚动框架角和俯仰框架角，三轴稳定平台示意图如 图2所示，具体定义如下：
[0026]方位框架角:在三轴稳定平台中，外框为方位框，绕方位轴旋转，顺时针为正，由旋 变测量得到，用A表示，范围-170°~170°。
[0027]滚动框架角:在三轴稳定平台中，中框为滚动框，绕滚动轴旋转，右弦向下为正，由 旋变测量得到，用R表示，范围-8°~8°。
[0028]俯仰框架角:在三轴稳定平台中，内框为俯仰框，绕俯仰轴旋转，抬头为正，由旋变 测量得到，用E表示，范围-8°~-75°。
[0029] 负载姿态角，即视轴与大地坐标系的夹角。包括负载方位姿态角、负载滚动姿态角 和负载俯仰姿态角，负载坐标系如图3所示，具体定义如下：
[0030] 负载方位角:正北与视轴指向在水平面投影线的夹角，在水平面内测量，顺时针为 正，用α表示，范围0°~360°。
[0031] 负载俯仰角:为视轴指向与其在水平面投影线的夹角，在铅垂面内测量，波束指向 在水平面之上为正，用β表示，范围-90°~90°。
[0032] 负载滚动角：为绕视轴指向旋转的角度，即视轴横剖面和水平面的交线与视轴横 轴(0Χ)之间的夹角，测量平面垂直于横轴平面(Χ0Ζ)且垂直于首尾线，右翼下倾为正，用γ 表示，范围-180°~180°。
[0033] 步骤二、建立坐标变换矩阵：
[0034] 负载框架角为负载坐标系与无人机坐标系的夹角，因此负载坐标系到无人机坐标 系的方位轴、横滚轴和俯仰轴转换矩阵分别为：
[0035]
[0036]由从内到外顺序，可得转换矩阵r1:
[0037]
[0038] 同理，按滚动、俯仰、航向的顺序，可得到无人机坐标系到大地坐标系的转换矩阵 r1：
[0039]
[0040] 步骤三、计算负载姿态角：
[0041] 视轴与负载坐标系的Y轴重合，可以取单位向量[Ο,Ι,ΟΥ，经坐标变换，可以得到 在大地坐标系下的坐标：
[0042]
[0043] 由姿态角定义，负载的方位姿态角和俯仰姿态角分别为：
[0044] a = arctan(xid/yid)
[0045] 0 = arcsin(zid)
[0046] 视轴不能表征滚动姿态的变化，因此取垂直于波束的单位向量[0，0，1 ]7和[1，0， 0 ]'，经坐标变换，得到在大地坐标系下的坐标：
[0047]
[0048] 可以得到负载的滚动角：
[0049] γ =-arctan(z2d/z3d)
[0050] 步骤四、稳定伺服平台的补偿方法：
[0051 ]大地坐标系到无人机坐标系变换，负载坐标系至无人机坐标系变换。两边求等式， 进而求解补偿的方位框架角、俯仰框架角、滚动框架角。
[0052] (1)滚动框架角：
[0053] -sinR = T3i*cos( γ )cos(-c〇+T32*cos( γ )sin(-c〇_T33sin( γ )
[0054] 其中α、γ为步骤二所求得的负载方位角与滚动角，T31、T32和T33为步骤二中T(与Γ 1 互逆)的元素。
[0055]可以求解出三轴稳定平台的滚动框架角为R，则中框的补偿角度为R