买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

龙图腾网获悉浙江大学申请的专利一种基于多传感器融合的6自由度相对位姿估计系统获国家发明授权专利权，本发明授权专利权由国家知识产权局授予，授权公告号为：CN115930957B 。

龙图腾网通过国家知识产权局官网在2025-09-16发布的发明授权授权公告中获悉：该发明授权的专利申请号/专利号为：202211566069.1，技术领域涉及：G01C21/16；该发明授权一种基于多传感器融合的6自由度相对位姿估计系统是由高飞;寻芝任;曹燕军设计研发完成，并于2022-12-07向国家知识产权局提交的专利申请。

本一种基于多传感器融合的6自由度相对位姿估计系统在说明书摘要公布了：本发明公开了一种基于多传感器融合的6自由度相对位姿估计系统，应用于机器人B，包括红外LED模块，UWB模块，红外鱼眼相机、IMU模块和处理器：所述红外LED模块用于通过LED闪烁的占空比来编码自身的ID；所述红外鱼眼相机用于拍摄自身的视野图像；所述UWB模块用于提供与所述机器人的相对距离；所述IMU模块用于获取自身的3轴加速度、3轴角速度和重力方向，所述处理器用于利用所述视野图像用于识别机器人A的ID和像素坐标，根据所述像素坐标、相对距离和重力方向进行初步相对位姿解算，利用所述3轴加速度和3轴角速度，基于改进误差状态卡尔曼滤波器对计算得到的初步相对位姿进行滤波，从而实现相对位姿估计。

本发明授权一种基于多传感器融合的6自由度相对位姿估计系统在权利要求书中公布了：1.一种基于多传感器融合的6自由度相对位姿估计系统，应用于机器人B，其特征在于，包括红外LED模块，UWB模块，红外鱼眼相机、IMU模块和处理器： 所述红外LED模块用于通过LED闪烁的占空比来编码自身的ID； 所述红外鱼眼相机用于拍摄自身的视野图像； 所述UWB模块用于提供与所述机器人的相对距离； 所述IMU模块用于获取自身的3轴加速度、3轴角速度和重力方向， 所述处理器用于利用所述视野图像用于识别机器人A的ID和像素坐标，根据所述像素坐标、相对距离和重力方向进行初步相对位姿解算，利用所述3轴加速度和3轴角速度，基于改进误差状态卡尔曼滤波器对计算得到的初步相对位姿进行滤波，从而实现相对位姿估计； 其中，在所述处理器中，基于改进误差状态卡尔曼滤波器对计算得到的初步相对位姿进行滤波，包括： 1预测模型： 获取在固定参考系w下机器人κ的位置，速度和旋转四元数wpk，wvk，wqk，其中k代表机器人A或者B； 机器人A和B之间的相对位姿通过下面公式计算： p＝RT{wqB}wpA-wpB v＝RT{wqB}wvA-wvB 其中表示对应的旋转矩阵，表示四元数或者旋转角，RT表示旋转矩阵R的逆矩阵，BqA为旋转矩阵BRA对应的四元数，q*表示四元素q的共轭四元数； 2测量更新： 将初步相对位姿解算的结果作为相对位姿的观测值z， 其中是旋转矩阵对应的四元数，观测值和真实状态满足如下观测方程： z＝hxt+ε 其中

如需购买、转让、实施、许可或投资类似专利技术，可联系本专利的申请人或专利权人浙江大学，其通讯地址为：310058 浙江省杭州市西湖区余杭塘路866号；或者联系龙图腾网官方客服，联系龙图腾网可拨打电话0551-65771310或微信搜索“龙图腾网”。