[发明专利]一种单自驾仪一体化的飞行器系统及其二维云台控制方法

权利要求书

1.一种单自驾仪一体化的飞行器系统的二维云台自动指向控制方法，其特征在于，步骤如下：

(1)获取目标点与无人机的位置信息；

从自驾仪中的位置/姿态/航向传感器实时获取无人机的位置以及姿态信息，从地面控制中心获取当前目标点的位置信息；

(2)利用坐标转换矩阵，获得目标点在机体坐标系下的坐标；

[x_g y_g z_g]为目标点在惯性坐标系Ox_gy_gz_g下的位置坐标，[x_o y_o z_o]为飞机质心在惯性坐标系Ox_gy_gz_g下的位置坐标，L_bg为由Ox_gy_gz_g到Ox_by_bz_b的坐标转换矩阵，从而可以计算出目标点在机体坐标系Ox_by_bz_b下的位置坐标[x_b y_b z_b]：

其中：

偏航角ψ：机体轴Ox_b在Ox_gy_g水平面上的投影与Ox_g轴之间的夹角；飞机右偏航时形成的角度，规定为正；

俯仰角θ：机体轴Ox_b与Ox_gy_g水平面之间的夹角；当飞行器头部上仰时，规定为正；

滚转角φ：飞机对称平面与包含Ox_b轴的铅垂平面之间的夹角，飞行器向右滚转时形成的角度，规定为正；

(3)利用坐标平移，得到二维云台坐标系；

在二维云台安装的过程中，可以测得云台与飞行器质心的距离，即得到机体坐标系Ox_by_bz_b下的二维云台位置坐标[x_bc y_bc z_bc]；

定义二维云台坐标系，二维云台坐标系的三个轴与机体坐标系的三个轴分别平行，于是将机体坐标系按照向量平移，即可得到二维云台坐标系Ox_py_pz_p；

(4)利用坐标平移，计算目标点在二维云台坐标系下的坐标；由目标点在机体坐标系下的坐标[x_b y_b z_b]，通过坐标平移得到其在二维云台坐标系下的位置坐标[x_p y_p z_p]：

(5)根据相对位置信息计算云台期望俯仰角与偏航角指令：

(6)根据计算得到俯仰角、偏航角指令，考虑成像的质量问题，对控制指令的幅值、变化速率进行限制；

首先，考虑成像质量，对云台转动角度范围进行限制；其中θ_min,θ_max为俯仰角转动指令的边界值，Limitr()为限制函数，即输出的θ_cmd必须在θ_min与θ_max的区间内，ψ_min,ψ_max为偏航角转动指令的边界值；

其次，考虑图像的稳定性，对云台转动的速度进行限制；其中controlstep[0]为俯仰角转动指令的允许变化速率，θ_cur为上一时刻俯仰角转动指令，controlstep[1]为偏航角转动指令的允许变化速率，ψ_cur为上一时刻偏航角转动指令；

(7)通过预先标定好的角度与控制指令的转换参数k₀,θ₀,k₁,ψ₀，将俯仰角、偏航角指令转换为云台转动指令：