CN105468014A - 一种单自驾仪一体化的飞行器系统及其二维云台控制方法 - Google Patents

Abstract

一种单自驾仪一体化的飞行器系统及其二维云台控制方法，包括地面控制中心，无人机飞行器，二维云台、自驾仪、数据通信链路及图传链路；自驾仪同时实现飞行器控制与二维云台控制。二维云台控制时将自驾仪中的位置/姿态/航向传感器获取的无人机位置姿态信息、地面控制中心上传的目标点位置信息，映射到以无人机质心为原点的机体坐标系中；进一步通过二维云台以及无人机质心的几何关系，可以得到二维云台与目标点之间的相对位置关系，继而通过控制算法解算出云台的期望俯仰角、偏航角指令，以保证云台在无人机飞行器飞行过程中始终能正对目标点。同时综合考虑成像质量问题，对云台转动的角度、角速度进行约束，保证能获得清晰的实时图像。

Description

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

技术领域

本发明属于无人机控制系统设计领域，具体涉及一种单自驾仪一体化的飞行器系统及其二维云台控制方法。

背景技术

目前，云台摄像系统在无人机航拍领域应用越来越广泛。在无人机领域，固定翼类型的占主导地位，由于固定翼无人机速度快，无法悬停，当需要对一个目标进行定点观察时，就要采用云台控制机构。云台根据无人机的航迹和姿态实时对偏转角和滚转角进行控制，从而实现对目标点的持续观察。云台系统是固定翼无人机完成航拍和定点观察任务的关键因素。

现有的无人机云台控制系统大多采用独立的模块。这些模块可以解算出目标到云台的位置和姿态等信息，然后利用这些信息生成云台的控制指令，从而获得稳定的视频图像。由于云台是安装在无人机上，知道了无人机的位置和姿态等信息，就可以知道云台的位置和姿态等信息。而自动驾驶仪既可以获得无人机的位置和姿态等信息，也可以生成相关的指令，因此单独为云台系统配置一个控制器是不必要的，造成硬件上的浪费，同时使整个无人机系统变得冗余和臃肿。此外，这两个控制单元相对独立，导致整套系统无法很好的结合起来，自动驾驶仪控制系统无法在飞行航迹规划中考虑航迹变换对云台拍摄效果可能带来的影响，也无法根据拍摄要求设计更为合理的飞行航迹，影响最终任务完成的效果。

发明内容

针对上述技术问题，本发明的目的在于通过设计一种单自驾仪一体化的飞行器系统及其二维云台控制方法，提供一种整体结构科学简单、高度集成、安全可靠、可方便操作二维云台进行目标观测的单一自驾仪系统，以弥补现有携带云台的无人机系统冗余，操作复杂的不足。

本发明的技术方案为：

一种单自驾仪一体化的飞行器系统，包括地面控制中心，无人机飞行器，二维云台、自驾仪、一条上下行的数据通信链路及一条下行的图传链路；所述自驾仪同时实现飞行器控制以及二维云台控制，所述二维云台和自驾仪均固联于无人机飞行器上；二维云台具有遥控模式和云台自动指向模式两种控制方式，遥控模式时，所述地面控制中心通过数据通信链路与无人机飞行器上的自驾仪进行实时通信，实时监测和控制无人机的位置及姿态，并在遥控模式下控制二维云台的俯仰角、偏航角；云台自动指向模式时，二维云台控制的指令由自驾仪解算输出，地面控制中心实时进行监测，并在需要时切换至遥控模式，二维云台控制系统使用自驾仪控制系统中的位置/姿态/航向信息计算得到二维云台期望的俯仰角、偏航角指令。

一种单自驾仪一体化的飞行器系统的二维云台自动指向控制方法，首先将自驾仪中的位置/姿态/航向传感器获取的无人机位置、姿态信息、地面控制中心上传的目标点位置信息，映射到以无人机质心为原点的机体坐标系中；进一步通过二维云台以及无人机质心的几何关系，可以得到二维云台与目标点之间的相对位置关系，继而通过控制算法解算出云台的期望俯仰角、偏航角指令，以保证云台在无人机飞行器飞行过程中始终能正对目标点。同时综合考虑成像质量问题，对云台转动的角度、角速度进行约束，保证能获得清晰的实时图像。具体步骤如下：

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

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

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

[x_gy_gz_g]为目标点在惯性坐标系Ox_gy_gz_g下的位置坐标，[x_oy_oz_o]为飞机质心(即机体坐标系原点)在惯性坐标系Ox_gy_gz_g下的位置坐标，L_bg为由Ox_gy_gz_g到Ox_by_bz_b的坐标转换矩阵。从而可以计算出目标点在机体坐标系Ox_by_bz_b下的位置坐标[x_by_bz_b]：

$\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]=L_{bg}\left[\begin{array}{c}{x}_g-x_0\\ y_g-y_0\\ z_g-z_0\end{array}\right]=L_x\left(\mathrm{\φ}\right)L_y\left(\mathrm{\θ}\right)L_z\left(\mathrm{\ψ}\right)\left[\begin{array}{c}{x}_g-x_0\\ y_g-y_0\\ z_g-z_0\end{array}\right]$

$L_{bg}=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\θ}\\ \sin \mathrm{\θ}\sin \mathrm{\ψ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \sin \mathrm{\θ}\sin \mathrm{\φ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\cos \mathrm{\θ}\\ \sin \mathrm{\θ}\cos \mathrm{\φ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\cos \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$

具体角度定义如下：

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

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

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

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

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

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

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

$\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]=\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]-\left[\begin{array}{c}{x}_{bc}\\ y_{bc}\\ z_{bc}\end{array}\right]$

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

$\left\{\begin{array}{c}{\mathrm{\θ}}_{cmd}=arctan\left(\frac{z_p}{x_p^2+y_p^2}\right)\\ {\mathrm{\ψ}}_{cmd}=arctan\left(\frac{y_p}{x_p}\right)\end{array}\right.$

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

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

$\left\{\begin{array}{c}{\mathrm{\θ}}_{cmd}=Limitr({\mathrm{\θ}}_{cmd},{\mathrm{\θ}}_{\min },{\mathrm{\θ}}_{max})\\ {\mathrm{\ψ}}_{cmd}=Limitr({\mathrm{\ψ}}_{cmd},{\mathrm{\ψ}}_{min},{\mathrm{\ψ}}_{\max })\end{array}\right.$

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

$\left\{\begin{array}{c}{\mathrm{\θ}}_{cmd}=Limitr({\mathrm{\θ}}_{cmd},{\mathrm{\θ}}_{cur}-controlstep\[0\],{\mathrm{\θ}}_{cur}+controlstep\[0\])\\ {\mathrm{\ψ}}_{cmd}=Limitr({\mathrm{\ψ}}_{cmd},{\mathrm{\ψ}}_{cur}-controlstep\[1\],{\mathrm{\ψ}}_{cur}+controlstep\[1\])\end{array}\right.$

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

$\left\{\begin{array}{c}PWAU\[0\]=k_0*{\mathrm{\θ}}_{cmd}+{\mathrm{\θ}}_0\\ PWAU\[1\]=k_1*{\mathrm{\ψ}}_{cmd}+{\mathrm{\ψ}}_0\end{array}\right..$

本发明的优点是：

本发明简化了目前多种无人飞行器中自驾仪与云台控制机构分开的方法，将二维云台控制集成到无人机飞行器自驾仪中，真正实现了自驾仪的功能突破化发展，且避免了不同独立机构获取信息有可能带来的冲突，保证各部分功能的正常有序运行，提高整个系统的可靠性。同时，精简了系统组成，消除冗余，在提高自驾仪集成度的同时，使地面控制中心更易操作，飞行系统更加高效运行。

附图说明

图1为本发明单自驾仪一体化的飞行器系统的结构框图

图2为本发明的二维云台控制方法的流程图

具体实施方式

图1为本发明单自驾仪一体化的飞行器系统的结构框图，一种单自驾仪一体化的飞行器系统，包括地面控制中心，无人机飞行器，二维云台、自驾仪、一条上下行的数据通信链路及一条下行的图传链路；所述自驾仪同时实现飞行器及二维云台控制，所述二维云台和自驾仪均固联于无人机飞行器上；其中二维云台控制系统使用无人机自驾仪控制系统中的位置/姿态/航向信息计算得到二维云台期望的俯仰角、偏航角指令，而不再与自驾仪分开独立使用。具体地，二维云台具有遥控模式和云台自动指向模式两种控制方式，遥控模式时，所述地面控制中心通过数据通信链路与无人机飞行器上的自驾仪进行实时通信，实时监测和控制无人机的位置及姿态，并在遥控模式下控制二维云台的俯仰角、偏航角；云台自动指向模式时，二维云台控制的指令由自驾仪解算输出，地面控制中心仅实时监测，并在需要时切换至遥控模式。

参照图2，一种单自驾仪一体化的飞行器系统的二维云台自动指向控制方法，具体步骤如下：

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

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

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

[x_gy_gz_g]为目标点在惯性坐标系Ox_gy_gz_g下的位置坐标，[x_oy_oz_o]为飞机质心(即机体坐标系原点)在惯性坐标系Ox_gy_gz_g下的位置坐标，L_bg为由Ox_gy_gz_g到Ox_by_bz_b的坐标转换矩阵。从而可以计算出目标点在机体坐标系Ox_by_bz_b下的位置坐标[x_by_bz_b]：

$\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]=L_{bg}\left[\begin{array}{c}{x}_g-x_0\\ y_g-y_0\\ z_g-z_0\end{array}\right]=L_x\left(\mathrm{\φ}\right)L_y\left(\mathrm{\θ}\right)L_z\left(\mathrm{\ψ}\right)\left[\begin{array}{c}{x}_g-x_0\\ y_g-y_0\\ z_g-z_0\end{array}\right]$

$L_{bg}=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\θ}\\ \sin \mathrm{\θ}\sin \mathrm{\ψ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \sin \mathrm{\θ}\sin \mathrm{\φ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\cos \mathrm{\θ}\\ \sin \mathrm{\θ}\cos \mathrm{\φ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\cos \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$

具体角度定义如下：

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

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

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

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

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

因为二维云台坐标系的三个轴与机体坐标系的三个轴分别平行，因此将机体坐标系按照向量平移，即可得到二维云台坐标系Ox_py_pz_p。

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

$\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]=\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]-\left[\begin{array}{c}{x}_{bc}\\ y_{bc}\\ z_{bc}\end{array}\right]$

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

$\left\{\begin{array}{c}{\mathrm{\θ}}_{cmd}=arctan\left(\frac{z_p}{x_p^2+y_p^2}\right)\\ {\mathrm{\ψ}}_{cmd}=arctan\left(\frac{y_p}{x_p}\right)\end{array}\right.$

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

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

$\left\{\begin{array}{c}{\mathrm{\θ}}_{cmd}=Limitr({\mathrm{\θ}}_{cmd},{\mathrm{\θ}}_{\min },{\mathrm{\θ}}_{max})\\ {\mathrm{\ψ}}_{cmd}=Limitr({\mathrm{\ψ}}_{cmd},{\mathrm{\ψ}}_{min},{\mathrm{\ψ}}_{\max })\end{array}\right.$

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

$\left\{\begin{array}{c}{\mathrm{\θ}}_{cmd}=Limitr({\mathrm{\θ}}_{cmd},{\mathrm{\θ}}_{cur}-controlstep\[0\],{\mathrm{\θ}}_{cur}+controlstep\[0\])\\ {\mathrm{\ψ}}_{cmd}=Limitr({\mathrm{\ψ}}_{cmd},{\mathrm{\ψ}}_{cur}-controlstep\[1\],{\mathrm{\ψ}}_{cur}+controlstep\[1\])\end{array}\right.$

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

$\left\{\begin{array}{c}PWAU\[0\]=k_0*{\mathrm{\θ}}_{cmd}+{\mathrm{\θ}}_0\\ PWAU\[1\]=k_1*{\mathrm{\ψ}}_{cmd}+{\mathrm{\ψ}}_0\end{array}\right..$

Claims (2)

1.一种单自驾仪一体化的飞行器系统，包括地面控制中心，无人机飞行器，二维云台、自驾仪、一条上下行的数据通信链路及一条下行的图传链路；所述自驾仪同时实现飞行器控制以及二维云台控制，所述二维云台和自驾仪均固联于无人机飞行器上；二维云台具有遥控模式和云台自动指向模式两种控制方式，遥控模式时，所述地面控制中心通过数据通信链路与无人机飞行器上的自驾仪进行实时通信，实时监测和控制无人机的位置及姿态，并在遥控模式下控制二维云台的俯仰角、偏航角；云台自动指向模式时，二维云台控制的指令由自驾仪解算输出，地面控制中心实时进行监测，并在需要时切换至遥控模式，二维云台控制系统使用自驾仪控制系统中的位置/姿态/航向信息计算得到二维云台期望的俯仰角、偏航角指令。

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

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

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

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

[x_gy_gz_g]为目标点在惯性坐标系Ox_gy_gz_g下的位置坐标，[x_oy_oz_o]为飞机质心在惯性坐标系Ox_gy_gz_g下的位置坐标，L_bg为由Ox_gy_gz_g到Ox_by_bz_b的坐标转换矩阵，从而可以计算出目标点在机体坐标系Ox_by_bz_b下的位置坐标[x_by_bz_b]：

$\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]=L_{bg}\left[\begin{array}{c}{x}_g-x_0\\ y_g-y_0\\ z_g-z_0\end{array}\right]=L_x\left(\mathrm{\φ}\right)L_y\left(\mathrm{\θ}\right)L_z\left(\mathrm{\ψ}\right)\left[\begin{array}{c}{x}_g-x_0\\ y_g-y_0\\ z_g-z_0\end{array}\right]$

$L_{bg}=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\θ}\\ \sin \mathrm{\θ}\sin \mathrm{\φ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \sin \mathrm{\θ}\sin \mathrm{\φ}\cos \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\cos \mathrm{\θ}\\ \sin \mathrm{\θ}\cos \mathrm{\φ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\cos \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$

其中：

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

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

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

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

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

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

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

$\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]=\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]-\left[\begin{array}{c}{x}_{bc}\\ y_{bc}\\ z_{bc}\end{array}\right]$

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

$\left\{\begin{array}{c}{\mathrm{\θ}}_{cmd}=\arctan \left(\frac{z_p}{x_p^2+y_p^2}\right)\\ {\mathrm{\ψ}}_{cmd}=\arctan \left(\frac{y_p}{x_p}\right)\end{array}\right.$

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

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

$\left\{\begin{array}{c}{\mathrm{\θ}}_{cmd}=\lim itr({\mathrm{\θ}}_{cmd},{\mathrm{\θ}}_{\min },{\mathrm{\θ}}_{max})\\ {\mathrm{\ψ}}_{cmd}=\lim itr({\mathrm{\ψ}}_{cmd},{\mathrm{\ψ}}_{min},{\mathrm{\ψ}}_{\max })\end{array}\right.$

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

$\left\{\begin{array}{c}{\mathrm{\θ}}_{cmd}=Limitr({\mathrm{\θ}}_{cmd},{\mathrm{\θ}}_{cur}-controlstep\[0\],{\mathrm{\θ}}_{cur}+controlstep\[0\])\\ {\mathrm{\ψ}}_{cmd}=Limitr({\mathrm{\ψ}}_{cmd},{\mathrm{\ψ}}_{cur}-controlstep\[1\],{\mathrm{\θ}}_{cur}+controlstep\[1\])\end{array}\right.$

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

$\left\{\begin{array}{c}PWAU\[0\]=k_0*{\mathrm{\θ}}_{cmd}+{\mathrm{\θ}}_0\\ PWAU\[1\]=k_1*{\mathrm{\ψ}}_{cmd}+{\mathrm{\ψ}}_0\end{array}\right..$