相对位姿估计是机器人视觉领域的研究热点。通过两帧数据来估计相机的六自由度位姿变换。充分挖掘TOF相机优势，提出了多个有效算法，用以保证估计精度。采用迭代最近点(ICP)算法估计位姿变换，为了克服ICP算法迭代发散问题，利用尺度不变特征点对估计初始值。为了提取有效特征点，根据统计学原理尺度化灰度图像，提高图像对比度。为了提高相机的测量精度，根据曝光时间越长，测量精度越高的原理，提出了融合多帧数据算法，使得融合后的数据帧中每个像素值均是在最长合理曝光时间下采集得到。同时提出了度量两个六自由度位姿变换差异的算法，并首次利用其跟踪ICP迭代过程。实验证明提出的算法可以有效估计相机六自由度位姿变换。