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

说明书

技术领域

本发明属于导航定位技术领域。

背景技术

相对位姿确定方法在机器人导航定位、航天器交会对接、编队飞行等众多领域有着极其重要的应用价值。目前，相对位姿确定方法使用较多的传感器主要是光学CCD导航相机。通过多个非共线视线矢量(单位方向矢量)的测量，通过近似假设得到相对位置和姿态的近似解析解(参考文献1：N.K Philip，Ananthasayanam M R.Relative position and attitude estimation and controlschemes for the final phase of an autonomous docking mission of spacecraft.Acta Astronautica，2003，52：511-522；参考文献2：王保丰，李广云，于志坚.飞行器自主交会对接逼近阶段单台CCD测量方法研究.宇航学报，2007，28(1)：22-27)。或者借助于最小二乘算法得到相对位姿的最小方差(或者其他非线性估计方法)意义下的解(参考文件3：JU-YOUNG DU.Vision based NavigationSystem for Autonomous Proximity Operations：An Experimental andAnalytical Study.Ph.D.dissertation，Texas A&M University，December2004)。上述相对位置和姿态的近似解析解求取的过程中由于近似假设的存在，位姿导航误差也会一直存在解析解中；而基于非线性估计的相对位姿确定方法存在着计算量大、导航收敛困难和精度低的缺点。同时，由于计算机视觉中的模式识别图像处理需要很大的计算资源，所有使用CCD相机作为导航传感器的基于视觉的导航算法都存在数据计算量大、数据更新率低的缺点，这些缺点有时候将会导致暂时的跟踪识别目标丢失、从而最终导致导航失败。

发明内容

为了克服传统的基于光学CCD相机的位姿确定算法中存在的计算量大、收敛速度慢、导航精度低等缺点，本发明给出了一种采用基于高采样速率的激光测距成像仪的相对位姿确定方法。

具体的位姿流程如下：

1)激光测距成像仪获取到三个特征测量点p_i(i＝1，2，3)的矢量(距离+方向)Li→(i=1,2,3);]]>

2)构建参考坐标系o_rx_ry_rz_r；

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

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

相对姿态矩阵(坐标变换矩阵)：

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