基于图模型的小尺寸飞行器地面试验中位姿估计方法

Research on posture estimation method of small-size vehicle in the ground test based on the graph optimal model

在多飞行器地面试验位姿估计中,由于跟踪算法导致的跟踪轨迹不连续会使得位姿估计产生累积误差,为了实现位姿的精确估计,提出了一种基于图模型的全局位姿估计非线性优化方法。首先,建立了一个飞行器地面视觉位姿估计系统。然后根据飞行器上特征点的数目提出了一种向量交叉式的飞行器位姿解算方法,求解得到数据已关联飞行器位姿估计值。利用中介坐标系法求解得到轨迹段初始位姿节点在测量坐标系下的值,最后,在图模型基础上下,对整个量测过程中飞行器的位姿估计结果进行非线性全局优化减小线性算法的累积误差,并通过仿真与实际实验对飞行器位姿估计算法的可行性与精度进行验证。实验结果表明:在测量范围为6 000 mm×6 000 mm×3 000 mm的范围内,飞行器尺寸约为400 mm,特征点三维定位精度为2.9 mm的条件下,基于非线性优化的飞行器位姿估计算法的理论精度分别可达0.5°(3σ)与3 mm (3σ),实际绝对测量精度分别可达1.3°(3σ)与4 mm (3σ),基本满足地面试验对多飞行器编队算法开发以及制导控制系统性能长时间评估稳定可靠、精度高、抗干扰能力强等要求。

In the ground test of multi-vehicle, the global motion estimation method based on graph optimization model was proposed in order to reduce the cumulative error of posture estimation due to the discontinuity of tracking trajectory caused by tracking algorithm. Firstly, a ground test vision motion estimation system for vehicle was established. Then, according to the number of feature points on the vehicle, a vector crossover method was proposed to solve the position of vehicle posture, and the estimation value of vehicle posture with good data correlation was obtained. The intermediate coordinate system method was used to solve the value of the initial position node of the track segment under the measuring coordinate system, and finally, under the framework of graph optimization theory, the nonlinear global optimization of the motion estimation results of the vehicle during the whole measurement process was carried out to reduce the cumulative error of the linear algorithm, and the feasibility and precision of the vehicle motion estimation algorithm were verified by simulation and practical experiments. The experimental results show that the accuracy of the proposed vehicle attitude estimation algorithm can reach 0.5°(3σ) and 3 mm(3σ) respectively, and the actual absolute measurement accuracy can reach 1.3°(3σ) and 4 mm(3σ) respectively, the size of the vehicle is 400 mm in the range of the measuring range of 6 000 mm×6 000 mm×3 000 mm and the three-dimensional positioning accuracy of the feature points is 2.9 mm. It basically meets the requirements of ground test for the development of multi-vehicle formation algorithm and the performance evaluation of guidance control system, stable and reliable, high precision and strong antijamming ability.