水平稳定平台的姿态解算与控制策略

李凌云，芦海超，李芳芳，吴秦英

（西安石油大学电子工程学院，西安710065）

0 引言

稳定平台可分为单轴稳定平台、双轴稳定平台以及三轴（俯仰轴、滚转轴、方位轴）稳定平台。其中忽略俯仰通道和滚转通道——仅考虑绕方位轴的两自由度水平稳定平台具有结构简单、使用灵活等优点，因此具有广泛的应用背景。在石油工业中，出现了利用全景图像捕捉设备进行油气井探测获得岩层、裂缝等重要信息，进而完成油气井三维重建的图形图像新技术。其工作过程中要求承载图像捕捉设备的稳定云台要能够克服井下恶劣条件及随机扰动等因素的影响，保持稳定的姿态以获取高质量的原始图像信息；此外，稳定平台也被广泛应用在水平导向钻井中：钻具姿态的实时测量是实现导向的关键因素，这些姿态信息包括工具面向角、倾斜角和方位角，它们是由安装在钻具内的随钻测量单元获得的，稳定平台提供的方位角正好可以获得钻具的姿态信息[1]；在随钻测量技术中，要在钻进的同时实现各个参数的连续测量，获得准确的工程参数，稳定平台的作用必不可少。在安防监管、交通监控等民用领域中，随着被监控对象的多样化，现有的固定监控设备已不能很好的满足人们对动态监控的需求[2]，可移动监控设备将是未来的重要发展方向。可移动的设备如何克服运动过程中振动等外来扰动因素带来的影响，始终保持监控画质清晰和精准对焦，也需要应用高性能的稳定平台来解决[3]。

1 平台的姿态解算算法

目前载体的姿态解算方法主要有：欧拉法、方向余弦法与四元数法[4]。由于欧拉法的每个计算方程中均含有三角函数，并且会出现“奇点”。故常用后两种方法来解算出载体的姿态角度。

1.1 方向余弦法

刚体在三维空间中的运动可逆向分解为依次绕三个旋转轴的旋转，各轴旋转角（欧拉角）分别为：俯仰角θ、横滚角φ 和方位角ψ，其分别绕各自的旋转轴旋转后的旋转向量分别为Tx、Ty与Tz：

方向余弦矩阵可由三维空间中的任意向量R( x,y,z )按照Z-Y-X 的顺序旋转后得到：

稳定平台在三维空间中的姿态可用方向余弦或者欧拉角表示，用后者表示的方法具有非常直观的意义。但从式（2）看出，方向余弦矩阵中的9 个元素均为三角函数，在直接参与运算的情况下，其计算量非常大，严重影响控制器的工作效率，并不适合在工程实际中应用。相比而言，四元数具有运算量小、计算精度高、不存在奇异性等优点[5]，可预先求取四元数，然后再计算四元数旋转矩阵与方向余弦矩阵之间的对等关系来提取平台的欧拉角。

1.2 四元数法及四元数更新

四元数由一个实数和三个虚数构成[5-6]：

由文献[7]可知，捷联姿态矩阵与四元数有如下关系：

其中M 称为四元数的旋转矩阵，可以看出，矩阵M 与方向余弦矩阵有一一对应的关系，这样就建立起四元数组成的矩阵M 与方向余弦矩阵之间的关系。由式（2）和式（4）可以通过计算反三角函数求取出三个欧拉角，如下：

故只需知道四元数的四个参数就能解算出平台最后的姿态角度。

然而平台时刻处于动态之中，故四元数是一个随时间变化的函数。建立四元数微分方程如下：

其中，w 为载体坐标系中绕轴向旋转的角速率，将式（8）展开：

式（10）中，T、T+ΔT 分别表示姿态解算的当前时刻和下一时刻。从上式可看出，理论上只需得到3 个绕轴角速率便可通过四元数法求出平台的欧拉角。然而，选用的陀螺仪存在积分累积误差，随着运行时间的加长，将产生严重的漂移，使计算的姿态角存在较大的误差，最终导致平台失控。利用加速度计对陀螺仪进行补偿可以很好地解决这个问题。

将重力加速度从大地坐标系旋转到平台的载体坐标系中，再将其与归一化的加速度计值做外积[9]，将得到外积误差融合到陀螺仪值中便可实现对陀螺仪的积分累积误差进行补偿。归一化后重力加速度ĝb如下：

再与加速度做外积，得外积误差c→：

其中，加速度计在载体坐标系中的加速度计值分别用abx、aby、abz表示。重力加速度旋转到载体坐标系中的值分别用gbx、gby、gbz表示。由于加速度计与陀螺仪均固定在同一个载体上，最终将该误差融合到陀螺仪的值上更新四元数即可实现对陀螺仪积分累积误差的补偿。

2 执行机构的控制策略

平台选用的执行机构为180 度的数字舵机，其采用PWM 信号进行控制。高电平脉宽T 与舵机目标角γ 之间有如下关系：

舵机本身为伺服机构，当它没有转到指定位置时，将会全速向目标位置转动。当其到达了指定位置，将会自动保持该位置，除非指定位置发生改变，否则舵机将会一直保持当前的位置不变，所以对舵机而言，PWM信号提供的是指定位置，跟踪运动则要靠舵机本身。但仅仅知道式（13）是行不通的，因为舵机是按照PWM信号来实现转动，所以必须要找到高电平脉冲宽度T与PWM 控制信号之间的对应关系。规定PWM 周期为20ms，将20ms 分为14400 份，则将式（13）转换为：

此处的γ 是经过增量型PID 算法的输出值，PWMvalue 为当前的PWM 值，即舵机现在位置对应的PWM 值，初始值设定为1080（对应1.5ms），至于为什么不用PWMvalue=1080+γ*8 现解释如下：

因为舵机与平台固联，而平台是处在动态当中，所以PWM 的值必定是上一个时刻的值。此处的γ 是经过PID 增量式算法的输出值。

稳定平台在水平面内的运动分为原地旋转运动与水平平移运动。在此，仅对原地旋转运动进行分析。

如图1（a）所示，将姿态检测模块放于舵机转轴上，舵机与平台固联，假设舵机转轴基准位置设在1.5ms处，将点划线部分看成是一个随平台运动的基准坐标系。当底座左旋+45°时（b），姿态模块检测出此时的姿态角，将其与期望角度做差，该偏差值送给控制器进行增量型PID 算法后，得出的控制量驱动舵机转轴右旋补偿平台底座的左旋，使舵机转轴始终指向期望位置。

3 系统测试

为验证本文所采用的姿态解算算法以及控制策略的有效性，对水平稳定平台的原地旋转运动进行测试。主控制器选用STM32f013C8T6；姿态检测单元选用MPU6050；SR-1501 型数字舵机作为执行机构。利用串口将姿态数据导入电脑后，用MATLAB 绘制平台响应曲线并对其进行分析。

（1）定值控制时的系统响应

假设上电后舵机处在初始位置（方位角0°），以表1 期望角度为例（仅给出期望角度为+30°的响应曲线如图2 所示）。

表1 定值控制系统响应情况记录表

从表1 可以看出，在给定期望角度的情况下，除了在期望角度为60°时，系统的调节时间过长，其他给定的期望角度情况下系统上电后从基准位置转到期望位置的时间短、超调量小、稳态误差低。

图1 平台底座旋转示意图

图2 期望角度为+30°

（2）人为干扰时的系统响应

在给定期望角度后，并随机加入人为干扰，平台响应情况如表2 所示。仅给出期望角度为+30°干扰响应曲线如图3 所示。

图3 期望角度为+30°时的干扰响应曲线

表2 人为干扰时系统响应情况记录表

从表2 可以看出，在给定期望角度的情况下，人为转动平台，会给平台带来干扰，但平台最终能够凭借自身的旋转回到期望位置，克服了人为干扰，基本满足系统控制要求。

4 结语

水平稳定平台一直是随动跟踪系统的重要研究方向之一，而平台的姿态解算与控制策略是其重要组成部分。本文分析了方向余弦矩阵与四元数之间一一对应的关系，用四元数法解算出平台的姿态角；研究了平台执行机构的控制策略，从平台的响应时间可以看出，无论是在定值下还是加入人为干扰，平台都能回到指定位置，实现对目标的稳定跟踪，证明了姿态解算算法以及控制策略的有效性。但是，执行机构的响应时间是制约平台快速性的主要因素之一，下一步主要的研究工作是如何选取性能优良的执行机构来减少系统的响应时间。