楼主解决这个问题了吗？我现在也在做六自由度机器人位姿监控，感觉网上资料很少。