基于几何约束的相对姿态确定算法优化

本文采用领航—跟随（leader-follower）模式的双机编队控制策略，如图1所示。长机和僚机上都安装有视觉传感器以获取另一个飞行器及目标相对于自身的视线矢量，长机和僚机分别位于各自的体坐标系下，定义为L和F，I定义为惯性坐标系，每个飞行器的惯性姿态矩阵定义为，同理表示L坐标系到F坐标系的姿态转换矩阵，因此有

图1　双机编队模型

假设在长机和僚机之间引入平行光束，这样避免了由非平行光束产生的位置偏差，为求解长机和僚机之间的相对姿态，构建场景如图2所示。其中，w1表示由F指向L所在坐标系下的矢量，v1表示由F指向L所在坐标系下的矢量，w2表示由F指向目标F所在坐标系下的矢量，v2表示由L指向目标L所在坐标系下的矢量。w1与v1满足如下关系：

图2　几何约束矢量图

图2中，w1，w2，v1和v2均表示单位矢量。根据姿态确定原理，仅由两飞行器间的一组视线矢量值无法确定三轴相对姿态，还需要知道以视线矢量方向为轴旋转的角度。由图1可知，视线矢量构成了三角形的两条边，而且三角形内角和为180°，如果已知其中的两个角，便可求出第三个角。第三个角可以是到被观测目标的两条视线矢量构成的夹角。因此，对于双机编队相对姿态求解问题，目标对象可以是任一参考点，不需要知道目标的位置信息，只要飞行器之间以及到目标的视线矢量构成三角形约束，定义为

式中，θ1，θ2和θ3的定义如图1所示，对式（3）两边取余弦得

通过点乘和叉乘获得正弦值与余弦值，三角形约束方程定义为

从式（6）可以看出，三角形约束有效替代了角度观测值给出的信息。由于三角形约束不仅是参数而且是关于观测值的一个函数，因此，角度观测值可以改写为

式中，d为到目标的两条视线矢量夹角的余弦值。为求，需要找到一个姿态矩阵A和以参考方向为轴旋转的角度θ满足如下关系：

假设在惯性坐标系下w1与v1平行，设C为任一姿态矩阵，满足w1＝Cv1，有

设R（n2，θ）为以n2为轴，以θ为旋转角度的转换矩阵，其中0≤θ＜2π，则R（n2，θ）用欧拉方程表示为(www.xing528.com)

令n2＝w1，代入式（9），替换式（11）得

令w∗＝Cv2，代入三角形约束方程式（7）中得

由于式（13）方程左边第一项为零，所以，

根据cosθcosβ＋sinθsinβ＝－1，可以求出

最后推导出姿态矩阵A的求解公式：

由式（16）可以看出

将式（10）和w∗＝Cv2代入式（17）和式（18），得到和，其中，

令，式（11）可以写成

令，最终得到式（16）的简化形式如下：