基于S＋R-CKF的相对方位多机器人协同定位算法.pdfVIP

第３０卷　第７期 电子测量与仪器学报 Ｖｏｌ３０　Ｎｏ７ ２０１６年７ 月 ＪＯＵＲＮＡＬ ＯＦＥＬＥＣＴＲＯＮＩＣＭＥＡＳＵＲＥＭＥＮＴＡＮＤ ＩＮＳＴＲＵＭＥＮＴＡＴＩＯＮ 　·１１０７· ＤＯＩ：１０．１３３８２／ｊ．ｊｅｍｉ．２０１６．０７．０１７ 基于ＳＲＣＫＦ 的相对方位多机器人协同定位算法 李朕阳　郎　朗　陈孟元 （安徽工程大学　安徽省电气传动与控制重点实验室　芜湖　２４１０００） 摘　要：针对单移动机器人在探索未知复杂环境时，存在鲁棒性较差、效率较低等问题以及现有多机器人协同定位算法实 时性较差、数值不稳定和定位精度较差等缺陷，提出基于平方根容积卡尔曼滤波的相对方位多机器人协同定位算法。通过 建立机器人运动方程和观测方程，利用相对方位作为测量值，进一步得到多机器人协同定位的动态模型。在更新过程中直 接传递目标状态均值和协方差矩阵的平方根因子，降低了计算的复杂度。仿真结果表明：相比基于相对方位的ＥＫＦ、ＵＫＦ 协同定位算法，本文提出的协同定位算法均方根误差降低了８７．０４％和５２．１０％，运行时间比ＵＫＦ协同定位算法减少了 １４５％，表明该算法在协同定位性能上更优越。 关键词：多机器人；相对方位；容积卡尔曼滤波；平方根滤波；协同定位 中图分类号：ＴＰ２４；ＴＮ８３０．１　　文献标识码：Ａ　　国家标准学科分类代码：５１０．８０ Ｒｅｌａｔｉｖｅ ｐｏｓｉｔｉｏｎ ｃｏｏｐｅｒａｔｉｖｅ ｌｏｃａｌｉｚａｔｉｏｎ ａｌｇｏｒｉｔｈｍ ｏｆｍｕｌｔｉｒｏｂｏｔ ｂａｓｅｄ ｏｎ ＳＲＣＫＦ Ｌｉ Ｚｈｅｎｙａｎｇ　 Ｌａｎｇ Ｌａｎｇ　 Ｃｈｅｎ Ｍｅｎｇｙｕａｎ （Ａｎｈｕｉ Ｐｏｌｙｔｅｃｈｎｉｃ Ｕｎｉｖｅｒｓｉｔｙ，Ａｎｈｕｉ Ｋｅｙ Ｌａｂｏｒａｔｏｒｙ ｏｆＥｌｅｃｔｒｉｃ Ｄｒｉｖｅ ａｎｄ Ｃｏｎｔｒｏｌ，Ｗｕｈｕ２４１０００，Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ｗｈｅｎ ｅｘｐｌｏｒｉｎｇｔｈｅｕｎｋｎｏｗｎｃｏｍｐｌｅｘｅｎｖｉｒｏｎｍｅｎｔ，ｔｈｅ ｓｉｎｇｌｅ ｍｏｂｉｌｅｒｏｂｏｔｈａｓｍａｎｙｐｒｏｂｌｅｍｓｉｎｃｌｕｄｉｎｇ ｐｏｏｒ ｒｏｂｕｓｔｎｅｓｓ，ｌｏｗ ｅｆｆｉｃｉｅｎｃｙ，ａｓ ｗｅｌｌ ａｓ ｔｈｅ ｄｅｆｅｃｔｓ ｓｕｃｈ ａｓ ｂａｄ ｒｅａｌｔｉｍｅ ｐｅｒｆｏｒｍａｎｃｅ，ｕｎｓｔａｂｌｅ ｎｕｍｅｒｉｃａｌ ｖａｌｕｅ，ａｎｄ ｐｏｏｒ ｐｏｓｉｔｉｏｎｉｎｇ ａｃｃｕｒａｃｙ ｉｎ ｔｈｅ ｍｕｌｔｉｒｏｂｏｔ ｃｏｏｐｅｒａｔｉｖｅ ｌｏｃａｌｉｚａｔｉｏｎ ａｌｇｏｒｉｔｈｍ．Ｔｈｅｒｅｆｏｒｅ，ａ ｃｏｏｐｅｒａｔｉｖｅ ｌｏｃａｌｉｚａｔｉｏｎ ａｌｇｏｒｉｔｈｍ ｂａｓｅｄ ｏｎ ｓｑｕａｒｅ ｒｏｏｔ ｃｕｂａｔｕｒｅ Ｋａｌｍａｎ ｆｉｌｔｅｒｉｓｐｒｏｐｏｓｅｄ ｉｎ ｔｈｉｓｐａｐｅｒ．Ｔｈｉｓ ａｌｇｏｒｉｔｈｍ ｕｔｉｌｉｚｅｓ ｔｈｅ ｒｅｌａｔｉｖｅ ｐｏｓｉｔｉｏｎ ａｓ ｔｈｅ ｍｅａｓｕｒｅｄ ｖａｌｕｅ ｔｏ ｏｂｔａｉｎ ｔｈｅ ｄｙｎａｍｉｃ ｍｏｄｅｌ ｏｆ ｍｕｌｔｉｒｏｂｏｔ ｃｏｏｐｅｒａｔｉｖｅ ｌｏｃａｌｉｚａｔｉｏｎ ｔｈｒｏｕｇｈ ｅｓｔａｂｌｉｓｈｉｎｇｍｏｔｉｏｎｅｑｕａｔｉｏｎａｎｄｔｈｅ ｏｂｓｅｒｖａｔｉｏｎｅｑｕａｔｉｏｎｏｆｒｏｂｏｔ．Ｆｕｒｔｈｅｒｍｏｒｅ，ｉｎｔｈｅｕｐｄａｔｅｐｒｏｃｅｓｓ，ｔｈｅ ｓｑｕａｒｅ ｒｏｏｔ ｏｆ ｔｈｅ ｍｅａｎ ａｎｄ ｃｏｖａｒｉａｎｃｅ ｍａｔｒｉｘ ｆｒｏｍ ｔｈｅ ｔａｒｇｅｔ ｓｔａｔｅ ｉｓ ｔｒａｎｓｍｉｔｔｅｄ ｄｉｒｅｃｔｌｙ，ｗｈｉｃｈ ｒｅｄｕｃｅｓ ｔｈｅ ｃｏｍｐｕｔａｔｉｏｎａｌ ｃｏｍｐｌｅｘｉｔｙ．Ｔｈｅ ｓｉｍｕｌａｔｉｏｎ ｒｅｓｕｌｔｓ ｓｈｏｗ ｔｈａｔ ｃｏｍｐａｒｅｄ ｔｏ ｔｈｅ ＥＫＦ，ＵＫＦ ｃｏｏｐｅｒａｔｉｖｅ ｌｏｃａｔｉｏｎ ａｌｇｏｒｉｔｈｍ ｂａｓｅｄ ｏｎ ｔｈｅ ｒｅｌａｔｉｖｅ ｏｒｉｅｎｔａｔｉｏｎ，ｔｈｅ ｅｒｒｏｒ ｆｒｏｍ ｔｈｅ ｃｏｏｐｅｒａｔｉｖｅ ｌｏｃａｌｉｚａｔｉｏｎ ａｌｇｏｒｉｔｈｍ ｉｎ ｔｈｉｓ ｐａｐｅｒ ｒｅｄｕｃｅ