基于扩展卡尔曼滤波的5-Dof圆位姿估计算法_吴仰玉.pdf

第 36 卷 第 2 期2023 年 2 月传 感 技 术 学 报CHINESE JOUNAL OF SENSOS AND ACTUATOSVol.36No.2Feb 2023项目来源:国家自然科学基金项目(11961001);宁夏自然科学基金项目(2018AAC03126);宁夏高等学校一流学科建设(数学学科)(NXYLXK2017B09);北方民族大学重大专项项目(ZDZX201801);宁夏智能信息与大数据处理重点实验室开放基金(2019KLBD004)收稿日期:20210918修改日期:202205235-Dof Circular Feature s Pose Variables Estimation AlgorithmBased on Extend Kalman Filtering*WU Yangyu1*，LI Cui2(1Library North Minzu University，Yinchuan Ningxia 750021，China;2College of Mechanical Engineering，Beijing Institute of Technology，Beijing 100081，China)Abstract:A drawback of the existing circular feature s pose variables estimation algorithms is that input frames are processed independ-ently，so，valuable dynamic information of target is ignored，and the pose estimation accurate can still be improved A high accuracy 5-Dof circular feature s pose variables estimation method is proposed by using Extend Kalman filtering，and circular feature s pose is re-presented by a 5-DoF(degree of freedom)vector of=(X，Y，Z，)T In this method，bayesian framework is introduced to capture thetime information of continuous video frames，and the circular pose estimation system is optimized Firstly，the proposed algorithm givesprojection representation of 3D object circular feature to interact with the ellipse contour in the image by using，and then the non-linearmeasurement function is obtained Later，the measurement function and Extend Kalman filtering are combined for pose optimization Inaddition，Kalman filtering is utilized to correct pose estimation result，which provides a robust method for object 2D3D pose estimationby using circular feature in a challenging environment The experimental results show that the precision of circular feature s pose estima-tion is effectively improved by utilizing the time correlated information for image sequence with noises of different square errorKey words:pose estimation;circular feature;projection model of 3d object circular feature;extend Kalman filteringEEACC:7230doi:103969/jissn10041699202302017基于扩展卡尔曼滤波的 5-Dof 圆位姿估计算法*吴仰玉1*，李翠2(1北方民族大学图书馆，宁夏 银川 750021;2北京理工大学机电学院，北京 100081)摘要:现有圆位姿估计方法对输入帧进行独立处理，忽略了有价值的目标动态信息，圆位姿估计精度有提升空间，提出一种基于 EKF 的高精度 5-Dof 圆位姿估计方法，圆位姿由 5 自由度向量=(X，Y，Z，)T表示。该方法引入贝叶斯框架捕获视频连续帧的时间信息，优化圆位姿估计系统。首先，为了与 2D 椭圆轮廓交互，算法构造出 5 自由度向量 表示的空间圆投影轮廓 5-Dof 模型，进而设计非线性测量函数。其次，将该测量函数与扩展卡尔曼滤波(EKF)算法相结合，用于圆位姿优化。此外，使用简单的线性卡尔曼滤波算法(KF)对圆位姿估计值进行修正。实验表明，针对含有不同方差噪声的图像序列，算法利用图像序列的时间相关性，有效提高圆位姿估计精度。关键词:位姿估计;圆特征;空间圆投影轮廓模型;扩展卡尔曼滤波中图分类号:TP39141文献标识码:A文章编号:10041699(2023)02028707自主交会对接控制技术需要实时、高精度地测量航天器之间的相对位置、相对速度、相对姿态以完成航天器姿态轨道的高精度控制13。单圆特征普遍存在于航天器上，且易于检测和拟合，几何建模简单，被广泛应用于单目视觉目标位姿估计45。目前，单目图像圆位姿估计算法聚焦于两个方向:利用额外辅助特征，解决图像圆位姿估计的对偶解问题57;建立误差模型，通过最小化误差项提高圆位姿估计值的精度67。现有圆位姿估计方法通常把视频看作独立图像，将图像圆位姿估计模型应用于视频圆位姿估计，这些算法将圆位姿解析解作为位姿估计值，固有的精度以及帧间信息的缺失均会导致这些方法很难获得准确的位姿估计结果。现有文献以贝叶斯定理为基本框架，一致调和前序帧与当前帧信息，用于目标位姿估计，包含两类主流方法:一种是提供目标坐标系统，提取点特征或传感技术学报chinatransducersseueducn第 36 卷线特征信息，建立 2D 与 3D 特征信息的匹配模型，返回目标位姿12，810;另一种是比较真实图像和不同位姿 3D 模型合成图像的相似性，真实位姿获得最高的相似性评分1112。两类方法有效提高目标位姿估计精度，后者更适应于复杂多变的环境，但无法直接用于含圆特征且中心轴具有无限阶旋转对称性的 3D 目标位姿估计。为获的准确有效的圆位姿估计结果，我们提出一种高精度 5-Dof 圆位姿估计方法，该方法主要特点如下:考虑图像特征噪声分布特性，以贝叶斯定理作为基本框架，捕获视频连续帧的时间相关性，融合前序帧提供的先验信息和当前帧的后验信息，以贝叶斯解代替解析解，理论上保证了位姿估计结果的精确性;设计空间圆投影轮廓 5-Dof 模型，构建似然函数，通过扩展卡尔曼滤波(EKF)算法获的圆位姿估计值;使用简单的线性卡尔曼滤波算法(KF)对圆位姿估计值进行修正。1基于 EKF 的 5-Dof 圆位姿估计目标函数建立11空间圆 5-Dof 位姿参数表示如图 1(a)所示，半径为 的空间圆在摄像机坐标系 OcXcYcZc中的位置和姿态分别由圆心 G=(X0，Y0，Z0)T和法向量 GP0=(l，m，n)T的方向角表示。如图 1(b)，在 OcXcYcZc中，方向角 表示GP0在平面 OCXCZC的投影 GP0与 OcXc轴所成角，GP0逆时针旋转为正角;表示 GP0与轴 OcZc的夹角;|GP0|=1 且 GP0=(sincos，sinsin，cos)T。综上，空间圆位姿参数表示为:X0，Y0，Z0，。图 1空间圆的位姿表示12空间圆 5-Dof 位姿估计目标函数贝叶斯定理基本框架，一致调和前序帧与当前帧信息，用于捕获视频连续帧的时间相关性，保证位姿估计值的精确性。视频圆位姿估计的目的是:给定 1到 k 时刻的图像序列，从观测序列数据 z1:k=z1，z2，zk 推断未知圆位姿参数 xk的估计值xk，使得期望E(xkxk)2)最小。又 min E(xkxk)2 xk=xkp(xk|z1:k)dxk。至此视频圆位姿估计问题转化为:递推估计后验概率密度 p(xk|z1:k)进而求解条件期望E(xk|z1:k)。文章将圆位姿参数建模为事件 x，观测数据建模为事件 x。给定事件 z，事件 x 的条件概率为:p(x|z)=p(z|x)p(x)p(z)(1)p(z|x)是 x 的似然函数，p(x)表示事件 x 的先验分布。结合实际应用场景，空间圆运动连续平缓，系统状态满足一阶 Markov 假设。k 时刻位姿 xk基于观测数据 z1:k的后验概率密度函数为1314:p(xk|z1:k)=1p(zk|z1:k1)p(zk|xk)p(xk|z1:k1)=1p(zk|z1:k)p(zk|xk)p(xk|xk1)p(xk1|z1:k1)dxk1(2)式中:p(xk|xk1)p(xk1|z1:k1)dxk1表示给定 1 到k1 时刻观测数据与位姿估计值后，对当前位姿的预测状态;p(xk|xk1)是转移密度;p(xk1|z1:k1)表示k1 时刻状态向量的后验概率分布;p(zk|xk)表示给定 k 时刻位姿状态情况下观测数据 zk的概率。进一步，文章使用动态系统模型(3)描述观测数据与位姿变量间的因果关系，解释不同时间位姿的变化:xk=f(xk1)+kK(a)zk=h(xk)+kKK(b)(3)式中:k，k分别表示系统噪声和观测噪声。联合式(1)，式(2)变为式(4)14:p(xk|z1:k)=pk(zk h(xk)pvk(zk h(xk)p(xk|z1:k1)dxkpk xk f(xk1)p(xk1|z1:k1)dxk1pvk zk h(xk)p(xk|z1:k1)dxk(4)式(4)归纳了圆位姿估计背景下的贝叶斯解递推形式。显然，动态系统模型噪声和观测噪声分布的多样化使得直接求后验分布比较困难，文章假设系统噪声和观测噪声服从高斯分布，由扩展卡尔曼滤波方程 10 给出圆位姿估计的贝叶斯解。882第 2 期吴仰玉，李翠:基于扩展卡尔曼滤波的 5-Dof 圆位姿估计算法2基于 EKF 的 5-Dof 圆位姿估计算法设计算法采用一种有效的基于 EKF 和空间圆投影轮廓匹配策略，目的是在噪声、复杂背景环境中产生高精度圆位姿估计值。为更新位姿预测值，我们设计空间圆投影轮廓 5-Dof 似然函数模型，联合 EKF 更新位姿预测值。算法过程见图 2，其中空间圆投影轮廓5-Dof 似然函数模型设计是算法的主要内容。图 2算法流程21空间圆投影轮廓 5-Dof 似然函数模型建立一般位姿空间圆的 2D 投影为椭圆，参数方程 15 为:x=acoscosbsinsin+x0y=acossin+bcossin+y0式中:a，b 分别为椭圆的半长轴、半短轴，为倾斜角，(x0，y0)表示椭圆中心，离心角(0，2)。空间圆投影轮廓 5-Dof 模型描述空间圆位姿参数与其2D 投影轮廓的关系。一般位姿空间圆在 2D像平面的投影为椭圆，模型建立过程如下:如图 1(a)所示，投影椭圆 g，由圆心为 G 的空间圆和投影中心 Oc构成的圆锥面 Scone与像平面Z=f 相