[发明专利]基于激光测距成像仪的相对位姿确定方法

权利要求书

1.一种基于激光测距成像仪的相对位姿确定方法，其特征在于包括以下步骤：

(1)、激光测距成像仪获取到三个特征测量点p_i的矢量其中i＝1，2，3

(2)、构建参考坐标系o_rx_ry_rz_r

(3)、相对位置和姿态的构建

相对位置：Rr→=-L1→]]>

相对姿态矩阵：

定义ψ，θ，φ是体固联坐标系相对与参考坐标系的姿态角，即相对姿态角，假设按照3(ψ)-2(θ)-1(φ)顺序旋转可以得到姿态矩阵：

A=cosθcosψcosθsinψ-sinθsinφsinθcosψ-cosφsinψsinφsinθsinψ+cosφcosψsinφcosθcosφsinθcosψ+sinφsinψcosφsinθsinψ-sinφcosψcosφcosθ]]>

进而可以得到相对姿态角：

ψ=arctan(A(1,2)A(1,1))]]>

θ＝-arcsin(A(1，3))