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

摘要

一种基于激光测距成像仪的相对位姿确定方法，本发明属于导航定位技术领域。其特征在于包括以下步骤：(1)激光测距成像仪获取到三个特征测量点p_i的矢量(距离+方向)；(2)构建参考坐标系o_rx_ry_rz_r；(3)构建相对位置和相对姿态。本方法可以有效克服传统的基于光学CCD相机的位姿确定算法中存在的计算量大、收敛速度慢、导航精度低等缺点。

主权项

1、一种基于激光测距成像仪的相对位姿确定方法，其特征在于包括以下步骤：(1)、激光测距成像仪获取到三个特征测量点p_i的矢量(距离+方向)其中i＝1，2，3p1p2→=L2→-L1→]]>p1p3→=L3→-L1→;]]>(2)、构建参考坐标系o_rx_ry_rz_rorxr→=p1p2→||p1p2→||=L2→-L1→||L2→-L1→||]]>orzr→=orxr→×p1p3→||p1p3→||=orxr→×L3→-L1→||L3→-L1→||]]>oryr→=orzr→×orxr→;]]>(3)、相对位置和姿态的构建相对位置：Rr→=-L1→]]>相对姿态矩阵(坐标变换矩阵)：A=orxr→oryr→orzr→T]]>定义ψ，θ，φ是体固连坐标系相对与参考坐标系的姿态角，即相对姿态角，假设按照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))φ=arctan(A(2,3)A(3,3)).]]>