CN103673882A - 一种三维空间坐标寻迹方法及其数据采集装置 - Google Patents
一种三维空间坐标寻迹方法及其数据采集装置 Download PDFInfo
- Publication number
- CN103673882A CN103673882A CN201310698879.7A CN201310698879A CN103673882A CN 103673882 A CN103673882 A CN 103673882A CN 201310698879 A CN201310698879 A CN 201310698879A CN 103673882 A CN103673882 A CN 103673882A
- Authority
- CN
- China
- Prior art keywords
- plane
- laser
- time
- spike
- equivalent rotary
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Landscapes
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明公开了一种三维空间坐标寻迹方法及其数据采集装置,本发明的数据采集装置通过设置指引激光以及在受测对象上安装平面示踪板和图像传感器,使得指引激光在平面示踪板形成三个示踪点,从而能够记录下包含有受测对象三维运动姿态信息的所述各个示踪点在平面示踪板上的二维运动轨迹;本发明的三维空间坐标寻迹方法通过对上述数据采集装置获得的二维运动轨迹进行计算,能够在一次测量中同时得出受测对象在运动过程中的旋转四元数和x、y轴平移量。本发明具有测量距离远、测量速度快、测量精度高的优点。
Description
技术领域
本发明涉及一种用于三维空间坐标寻迹的数据采集装置,本发明还涉及应用该装置的三维空间坐标寻迹方法。
背景技术
目前的目标循迹方法包含独立和非独立两种。在测绘领域一般采用非独立式依靠外部基准进行测距定位循迹的方法,例如GPS、卫星轨道测量、经纬仪全站仪地形测绘,可以测得受测目标的三维位置。一般在飞机及潜艇的使用上采用惯性导航的独立测量方法,以获得受测目标的三维旋转的四元数。
现有技术在中高速ms级别、中远距离百米级、高精度mm级测绘的技术领域及同时需要旋转姿态和相对偏移的场合往往缺乏必要的技术手段。
发明内容
本发明所要解决的技术问题是:提供一种用于三维空间坐标寻迹的数据采集装置,能够记录下包含有受测对象三维运动姿态信息的所述各个示踪点在平面示踪板上的二维运动轨迹。
本发明进一步要解决的技术问题是:提供一种应用上述数据采集装置的三维空间坐标寻迹方法,具有测量距离远、测量速度快、测量精度高的特点。
解决上述技术问题,本发明所采用的技术方案如下:
一种用于三维空间坐标寻迹的数据采集装置,其特征在于:所述的数据采集装置包括指引激光、平面示踪板和图像传感器;所述指引激光设有三束相互平行且不共面的激光束,指引激光安装在固定位置上并指向固定方向,其中,第一激光束与第二激光束所在的平面与第一激光束与第三激光束所在的平面相垂直,所述平面示踪板安装在受测对象上,并分别与水平面、竖直平面形成不为0°且不为90°的夹角,所述第一至第三激光束依次在平面示踪板上形成第一至第三示踪点,所述图像传感器安装在受测对象上,用以记录下包含有受测对象三维运动姿态信息的所述各个示踪点在平面示踪板上的二维运动轨迹。
作为本发明的一种改进,所述第一激光束与第二激光束所在的平面平行于水平面,第一激光束与第三激光束所在的平面平行于竖直平面。
为了使得数据采集装置的灵敏度和测量范围的综合实用性最高,作为本发明的优选实施方式,作为本发明的一种改进,所述平面示踪板分别与水平面、竖直平面形成的夹角接近于45°。
作为本发明的一种改进,所述的指引激光还设有一束或者以上所述的激光束。
作为本发明的一种改进,所述的数据采集装置还包括支架;所述指引激光通过该支架安装在固定位置上,所述支架能够调节指引激光的指向。
一种应用上述数据采集装置的三维空间坐标寻迹方法,包括以下步骤:
步骤一:
用上述数据采集装置对受测对象进行测量,以获取在受测对象的运动过程中所述第一至第三示踪点在平面示踪板上的二维运动轨迹;
步骤二:
将所述平面示踪板由运动前时刻至运动后时刻的旋转运动等效分解成第一次等效旋转和第二次等效旋转;
其中,第一次等效旋转为:以运动前时刻的所述第一示踪点作为等效旋转点,将运动前时刻的平面示踪板绕第一等效旋转轴转过第一等效旋转角到达第一中间平面,使得所述第一中间平面的法线与运动后时刻的所述平面示踪板的法线相平行,所述第一等效旋转轴即为所述平面示踪板在运动前、后时刻的经过所述等效旋转点的法线公垂线,第一等效旋转角等于平面示踪板在运动前、后时刻的法线夹角,所述第一示踪点在第一次等效旋转前后位置不变;
所述第二次等效旋转为:将所述位于第一中间平面的平面示踪板绕第二等效旋转轴转过第二等效旋转角到达第二中间平面,其中所述第二等效旋转轴为第一中间平面经过所述等效旋转点的法线,第二等效旋转角等于第一示踪点与第二示踪点在第一等效旋转后的连线与运动后时刻的连线的空间夹角,也等于第一示踪点与第三示踪点在第一等效旋转后的连线与运动后时刻的连线的空间夹角,使得所述第一示踪点与第二示踪点在第二次等效旋转后的连线与在运动后时刻的连线相平行,所述第一示踪点与第三示踪点在第二次等效旋转后的连线与在运动后时刻的连线相平行,所述第一示踪点在第二次等效旋转前后位置不变;
从而根据所述步骤一获得的二维运动轨迹,计算出所述平面示踪板的第一次等效旋转的规范化旋转四元数;
步骤三:
计算出所述平面示踪板的第二次等效旋转的规范化旋转四元数;
步骤四:
将步骤二、三计算出的所述平面示踪板的第一次等效旋转和第二次等效旋转的旋转四元数相乘,计算得到从运动前时刻的平面示踪板到所述第二中间平面的旋转四元数,该计算结果即为受测对象从所述运动前时刻到所述运动后时刻的旋转四元数。
作为本发明的一种改进,所述的三维空间坐标寻迹方法还包括:
步骤五:
以所述第一激光束到第二激光束的指向作为x轴,以所述第一激光束到第三激光束的指向作为y轴,将所述平面示踪板由运动前时刻至运动后时刻的平移运动等效分解成沿所述x轴和y轴的平移运动以及沿所述第一激光束指向的平移运动;计算出受测对象从所述运动前时刻到所述运动后时刻的分别沿x轴和y轴的平移量。
作为本发明的一种实施方式,所述步骤一中,从所述二维运动轨迹获得:
1)所述三束激光束的三维空间方程参数:
第一激光束的三维方程
第二激光束的三维方程
第三激光束的三维方程
其中,Lx为第一、第二激光束之间的距离,Ly为第一、第三激光束之间的距离;
2)受测对象的运动前时刻即所述运动过程的初始时刻,所述第一至第三示踪点在平面示踪板上的二维坐标依次为:
Po0=(Po0(1),Po0(2));
Px0=(Px0(1),Px0(2));
Py0=(Py0(1),Py0(2));
受测对象的运动后时刻即所述运动过程的除初始时刻外的任意时刻,所述第一至第三示踪点在平面示踪板上的二维坐标依次为:
Pon=(Pon(1),Pon(2));
Pxn=(Pxn(1),Pxn(2));
Pyn=(Pyn(1),Pyn(2));
所述步骤二中,计算出所述平面示踪板的第一次等效旋转的规范化旋转四元数的步骤如下:
1)计算出运动前时刻,所述第一示踪点与第二示踪点之间的距离为:
计算出运动前时刻,所述第一示踪点与第三示踪点之间的距离:
计算出运动前时刻,所述平面示踪板的点法式方程系数为:
m0k=[-1/(tan(arcsin(Lx/Lx0))) -1/(tan(asin(Ly/Ly0))) 1] ……2.3)
2)计算出第一次等效旋转后,所述第一示踪点与第二示踪点之间的距离为:
计算出第一次等效旋转后,所述第一示踪点与第三示踪点之间的距离:
计算出第一次等效旋转后,所述平面示踪板的点法式方程系数为:
mnk=[-1/(tan(arcsin(Lx/Lxn))) -1/(tan(asin(Ly/Lyn))) 1] ……2.6)
3)将所述等效旋转点定义为三维坐标原点,从而计算出:
在运动前时刻以及在第二次等效旋转后,所述第一示踪点的三维坐标均为:
Pow0=Pow1=(0,0,0);
在运动前时刻,所述第二示踪点和第三示踪点的三维坐标依次为:
Pxw0=(Lx,0,-Lx*m0k(1)/m0k(3)) ……2.7)
Pyw0=(0,Ly,-Ly*m0k(2)/m0k(3)) ……2.8)
在第二次等效旋转后,所述第二示踪点和第三示踪点的三维坐标依次为:
Pxwn=(Lx,0,-Lx*mnk(1)/mnk(3)) ……2.9)
Pywn=(0,Ly,-Ly*mnk(2)/mnk(3)) ……2.10)
4)得出所述第一等效旋转轴的三维矢量为:
5)得出所述第一次等效旋转的转动角度为:
ai1=-arccos((m0k·mnk)/(|m0k|*|mnk|)) ……3.2)
6)计算出所述平面示踪板的第一次等效旋转的非规范化(四元数平方和不为1)旋转四元数为:
ci1t=[cos(ai1/2) Ai1*sin(ai1/2)]; ……3.3)
7)对所述3.3)式进行规范化处理,使得四元数平方和为1,得出所述平面示踪板的第一次等效旋转的规范化旋转四元数为:
ci1=[cos(ai1/2) Ai1*sin(ai1/2)*(kfq)]; ……3.4)
其中,kfq=((1-ci1t(1)^2)/sum(ci1t(2:4).^2))^0.5;
8)计算出第一次等效旋转后,所述第二、第三示踪点的三维坐标依次为:
其中,
所述步骤三中,计算所述平面示踪板的第二次等效旋转的规范化旋转四元数的步骤如下:
1)所述第二等效旋转轴的三维矢量与第一中间平面的法线表达式相同,为所述2.6)式;
2)计算出所述第二次等效旋转的转动角度为:
a12=-arccos((Pxw1·Pxwn)/(|Pxw1|*|Pxwn|)); ……5.1)
3)计算出所述平面示踪板(Board)的第二次等效旋转的非规范化旋转四元数为:
c12t=[cos(a12/2) mnk*sin(a12/2)]; ……5.3)
4)对所述5.3)式进行规范化处理,使得四元数平方和为1,得出所述所述平面示踪板的第二次等效旋转的规范化旋转四元数为:
c12=[cos(a12/2) mnk*sin(a12/2)*(kfq2)] ……5.4)
其中,kfq2=((1-c12t(1)^2)/sum(c12t(2:4).^2))^0.5;
所述步骤四中,计算得到的从运动前时刻的平面示踪板到所述第二中间平面的旋转四元数为:
作为本发明的一种实施方式,所述步骤五中,计算受测对象从所述运动前时刻到所述运动后时刻的沿x轴和y轴的平移量的步骤如下:
1)计算出第一运动轨迹的斜率,即所述平面示踪板沿所述x轴平移时,所述第一示踪点在平面示踪板上二维移动轨迹的斜率:
kx=(Pxn(2)-Pon(2))/(Pxn(1)-Pon(1)); ……7.1)
计算出第二运动轨迹的斜率,即所述平面示踪板沿所述y轴平移时,所述第一示踪点在平面示踪板上二维移动轨迹的斜率:
ky=(Pyn(2)-Pon(2))/(Pyn(1)-Pon(1)); ……7.2)
2)获得所述第一运动轨迹的方程系数:
klxn=[kx -1 0]; ……7.3)
获得所述第二运动轨迹的方程系数:
klyn=[ky -1 0]; ……7.4)
3)计算出第一直线的方程系数,该第一直线经过所述运动前时刻的第一示踪点且平行于所述第一运动轨迹,其与所述第二运动轨迹相交于第二交点:
klxno=[kx -1 Po0(2)-kx*Po0(1)]; ……8.1)
计算出第二直线的方程系数,该第二直线经过所述运动前时刻的第一示踪点且平行于所述第二运动轨迹,其与所述第一运动轨迹相交于第一交点:
klyno=[ky -1 Po0(2)-ky*Po0(1)]; ……8.2)
根据所述8.1)式、8.2)式以及7.3)式、7.4)式解出所述第一交点和第二交点的坐标,由此计算出所述第一交点到运动后时刻的第一示踪点的长度:
dx0=norm(px); ……8.3)
并计算出所述第二交点到运动后时刻的第一示踪点的长度:
dy0=norm(py); ……8.4)
最后,根据所述8.3)式和2.6)计算出受测对象从所述运动前时刻到所述运动后时刻的沿x轴的平移量,并根据下式判断受测对象在x轴上的平移方向:
af=atan(mnk(3)/mnk(2));
dy=dy*sin(af);
根据所述8.4)式和2.6)计算出受测对象从所述运动前时刻到所述运动后时刻的沿y轴的平移量,并根据下式判断受测对象在y轴上的平移方向:
af=atan(mnk(3)/mnk(1));
dx=dx*sin(af);
与现有技术相比,本发明具有以下有益效果:
第一,本发明的数据采集装置通过设置指引激光以及在受测对象上安装平面示踪板和图像传感器,使得指引激光在平面示踪板形成三个示踪点,从而能够记录下包含有受测对象三维运动姿态信息的所述各个示踪点在平面示踪板上的二维运动轨迹;
第二,本发明的三维空间坐标寻迹方法通过对上述数据采集装置获得的二维运动轨迹进行计算,能够在一次测量中同时得出受测对象在运动过程中的旋转四元数和x、y轴平移量;
第三,本发明适用范围广,可以广泛使用在检测距离0到1km内,平移距离分辨率精度在0.05~50mm,旋转角度分辨率精度在0.01度以上的各种姿态循迹的场合;
第四,本发明与独立测量的惯性导航相比具有完全不存在时飘问题,即测量结果与测量持续的时间无关的优点;
因此,本发明具有测量距离远、测量速度快、测量精度高的优点。
附图说明
下面结合附图和具体实施例对本发明作进一步的详细说明:
图1为本发明的数据采集装置的示意图;
图2为三个示踪点于运动前时刻以及运动后时刻在平面示踪板上的位置示意图;
图3为平面示踪板于运动前时刻以及运动后时刻的位置示意图;
图4为平面示踪板于第一次等效旋转后的位置示意图;
图5为平面示踪板于第二次等效旋转后的位置示意图;
图6为平面示踪板于等效平移后的位置示意图;
图7为步骤五中x、y轴平移量计算的示意图之一;
图8为步骤五中x、y轴平移量计算的示意图之二;
图中,Board_0-运动前时刻的平面示踪板,Board_1-第一次等效旋转后的平面示踪板,Board_2-第二次等效旋转后的平面示踪板,Board_3-等效平移后的平面示踪板,Board_4-运动后时刻的平面示踪板,L_0-运动前时刻平面示踪板的法线,L_1-第一次等效旋转后的平面示踪板的法线,L_2-第二次等效旋转后的平面示踪板的法线,L_3-等效平移后的平面示踪板的法线,L_4-运动后时刻平面示踪板的法线。
具体实施方式
如图1所示,本发明的用于三维空间坐标寻迹的数据采集装置,包括指引激光、平面示踪板Board和图像传感器;所述指引激光设有三束相互平行且不共面的激光束Laser_o,Laser_x和Laser_y,指引激光安装在固定位置上并指向固定方向,为了使得数据采集装置的连续测量范围更大,该指引激光的固定指向应该大致指向受测对象的运动方向,其中,第一激光束Laser_o与第二激光束Laser_x所在的平面平行于水平面,第一激光束Laser_o与第三激光束Laser_y所在的平面平行于竖直平面,以使得第一激光束Laser_o与第二激光束Laser_x所在的平面与第一激光束Laser_o与第三激光束Laser_y所在的平面相垂直,所述平面示踪板Board安装在受测对象上,并分别与水平面、竖直平面形成不为0°且不为90°的夹角α和β,在该两个夹角α和β接近于45°时,数据采集装置的灵敏度和测量范围的综合实用性最高,所述第一至第三激光束Laser_o,Laser_x和Laser_y依次在平面示踪板Board上形成第一至第三示踪点Po,Px和Py,以使得受测对象产生相对于x轴、y轴、z轴旋转以及相对于x轴、y轴平移的三维运动姿态变化时,上述三个示踪点能够在平面示踪板Board上产生相应的运动轨迹变化,而上述平面示踪板Board应具有合适的面积,以保证在受测对象定向运动的一定距离内,所有的激光束均能在平面示踪板Board上形成示踪点,所述图像传感器安装在受测对象上,用以记录下包含有受测对象三维运动姿态信息的所述各个示踪点在平面示踪板Board上的二维运动轨迹。
本发明的三维空间坐标寻迹方法是基于以下原理实现的:
1坐标系刚性旋转。
基本原理:参见《惯性导航》秦永元295页,原文如下:
以q0、q1、q2、q3构造四元数:
四元数描述了刚体的定点旋转动,既当只关心b系相对于a系的角度位置时,可以认为b系是由R系经过无中间过程的一次性等效旋转而成的,Q包含了等效旋转的全部信息:uR为旋转轴和旋转方向,θ为转过的角度。
其中,坐标系是在此表现为刚体,即轴之间的相对位置及单位长度无论是否旋转都保持不变。
根据以上理论,结合本案实际应用:
真实空间有一个点,在某个正交参照系下,其坐标记做:
而在另一个同原点正交参照系下,其坐标标记为:
两个参照系是按照:以“uR为旋转轴和旋转方向,θ为转过的角度”重合的。
定义
则有
2坐标系平移。
当坐标系仅仅发生平移时,其原点的示碂点偏移可独立互不影响的分解为绝对坐标系下x和y两个非正交的投影。并且与示踪板与x=0平面和y=0平面交线的斜率线性。可以根据示踪板的方程求出交线斜率解算及绝对坐标系下原点的平移。
因此,本发明的三维空间坐标寻迹方法中,平面示踪板Board由受测对象运动前的初始时刻到受测对象运动后的任意时刻,其无中间过程的一次性等效旋转可以分解为第一次等效旋转和第二次等效旋转,其中,第一次等效旋转为:绕垂直于两个法线的轴旋转两个法线夹角的类似合页的旋转;第二次等效旋转为:为了使旋转前后坐标系完全重合而绕旋转后法线旋转角度差的类似于陀螺的旋转。将前后两次等效旋转的四元数顺序相乘,即能得到新旧坐标系旋转变换的四元数。
如图1至图8所示,本发明应用上述数据采集装置的三维空间坐标寻迹方法,包括以下步骤:
步骤一:
用上述数据采集装置对受测对象进行测量,以获取在受测对象的运动过程中所述第一至第三示踪点Po、Px和Py在平面示踪板Board上的二维运动轨迹,从而获得:
1)三束激光束的三维空间方程参数:
第一激光束Laser_o的三维方程
第二激光束Laser_x的三维方程
第三激光束Laser_y的三维方程
其中,Lx为第一、第二激光束Laser_o和Laser_x之间的距离,Ly为第一、第三激光束Laser_o和Laser_y之间的距离,它们由上述数据采集装置的指引激光在生产、制造和安装时确定;
2)受测对象的运动前时刻即所述运动过程的初始时刻,所述第一至第三示踪点Po、Px和Py在平面示踪板Board上的二维坐标依次为:
Po0=(Po0(1),Po0(2));
Px0=(Px0(1),Px0(2));
Py0=(Py0(1),Py0(2));
受测对象的运动后时刻即所述运动过程的除初始时刻外的任意时刻,也就是本方法的待测量时刻,所述第一至第三示踪点Po、Px和Py在平面示踪板Board上的二维坐标依次为:
Pon=(Pon(1),Pon(2));
Pxn=(Pxn(1),Pxn(2));
Pyn=(Pyn(1),Pyn(2));
其中,为了保证测量的精确度,上述受测对象在空间三维旋转的偏角应在正负45度之内;
步骤二:
将所述平面示踪板Board由运动前时刻至运动后时刻的旋转运动等效分解成第一次等效旋转和第二次等效旋转;
其中,参见图4,第一次等效旋转为:以运动前时刻的所述第一示踪点Po作为等效旋转点,将运动前时刻的平面示踪板Board绕第一等效旋转轴转过第一等效旋转角到达第一中间平面,使得所述第一中间平面的法线与运动后时刻的所述平面示踪板Board的法线相平行,所述第一等效旋转轴即为所述平面示踪板Board在运动前、后时刻的经过所述等效旋转点的法线公垂线,第一等效旋转角等于平面示踪板Board在运动前、后时刻的法线夹角,所述第一示踪点Po在第一次等效旋转前后位置不变;
参见图5,所述第二次等效旋转为:将所述位于第一中间平面的平面示踪板Board转过第二等效旋转角到达第二中间平面,其中所述第二等效旋转轴为第一中间平面经过所述等效旋转点的法线,第二等效旋转角等于第一示踪点Po与第二示踪点Px在第一等效旋转后的连线与运动后时刻的连线的空间夹角,也等于第一示踪点Po与第三示踪点Py在第一等效旋转后的连线与运动后时刻的连线的空间夹角,使得所述第一示踪点Po与第二示踪点Px在第二次等效旋转后的连线与在运动后时刻的连线相平行,所述第一示踪点Po与第三示踪点Py在第二次等效旋转后的连线与在运动后时刻的连线相平行,所述第一示踪点Po在第二次等效旋转前后位置不变;
从而:
1)计算出运动前时刻,所述第一示踪点Po与第二示踪点Px之间的距离为:
计算出运动前时刻,所述第一示踪点Po与第三示踪点Py之间的距离:
计算出运动前时刻,所述平面示踪板Board的点法式方程系数为:
m0k=[-1/(tan(arcsin(Lx/Lx0))) -1/(tan(asin(Ly/Ly0))) 1] ……2.3)
2)计算出第一次等效旋转后,所述第一示踪点Po与第二示踪点Px之间的距离为:
计算出第一次等效旋转后,所述第一示踪点Po与第三示踪点Py之间的距离:
计算出第一次等效旋转后,所述平面示踪板Board的点法式方程系数为:
mnk=[-1/(tan(arcsin(Lx/Lxn))) -1/(tan(asin(Ly/Lyn))) 1] ……2.6)
说明:由于平面示踪板Board所在平面可以表示成点法式方程:
A(x-x0)+B(y-y0)+C(z-z0)=0,其中第一示踪点Po定义为三维原点。作为三维空间的原点,x0、y0、z0都为0,并且此点也在平面示踪板Board上,满足示踪板平面的方程。则有在示踪板上的本文以平面方程系数[A B C]代表平面的方程;
3)将所述等效旋转点定义为三维坐标原点,从而计算出:
在运动前时刻以及在第二次等效旋转后,所述第一示踪点Po的三维坐标均为:
Pow0=Pow1=0,0,0;
在运动前时刻,所述第二示踪点Px和第三示踪点Py的三维坐标依次为:
Pxw0=(Lx,0,-Lx*m0k(1)/m0k(3)) ……2.7)
Pyw0=(0,Ly,-Ly*m0k(2)/m0k(3)) ……2.8)
在第二次等效旋转后,所述第二示踪点Px和第三示踪点Py的三维坐标依次为:
Pxwn=(Lx,0,-Lx*mnk(1)/mnk(3)) ……2.9)
Pywn=(0,Ly,-Ly*mnk(2)/mnk(3)) ……2.10)
4)得出所述第一等效旋转轴的三维矢量为:
5)得出所述第一次等效旋转的转动角度为:
ai1=-arccos((m0k·mnk)/(|m0k|*|mnk|)) ……3.2)
6)计算出所述平面示踪板Board的第一次等效旋转的非规范化(四元数平方和不为1)旋转四元数为:
ci1t=[cos(ai1/2) Ai1*sin(ai1/2)]; ……3.3)
7)对所述3.3)式进行规范化处理,使得四元数平方和为1,得出所述所述平面示踪板Board的第一次等效旋转的规范化旋转四元数为:
ci1=[cos(ai1/2) Ai1*sin(ai1/2)*(kfq)]; ……3.4)
其中,kfq=((1-ci1t(1)^2)/sum(ci1t(2:4).^2))^0.5;
8)计算出第一次等效旋转后,所述第二、第三示踪点Px和Py的三维坐标依次为:
其中,
步骤三:
计算出所述平面示踪板Board的第二次等效旋转的旋转四元数,包括:
1)所述第二等效旋转轴的三维矢量与第一中间平面的法线表达式相同,为所述2.6)式;
2)计算出所述第二次等效旋转的转动角度为:
a12=-arccos((Pxw1·Pxwn)/(|Pxw1|*|Pxwn|)); ……5.1)
另外,所述第二次等效旋转的转动角度还可按下式进行计算:
a12=-arccos((Pyw1·Pywn)/(|Pyw0|*|Pywn|)); ……5.2)
为了提高计算精度,可以将5.1)式和5.2)式的计算结果取平均值作为所述第二次等效旋转的转动角度;
3)计算出所述平面示踪板Board的第二次等效旋转的非规范化旋转四元数为:
c12t=[cos(a12/2) mnk*sin(a12/2)]; ……5.3)
4)对所述5.3)式进行规范化处理,使得四元数平方和为1,得出所述所述平面示踪板Board的第二次等效旋转的规范化旋转四元数为:
c12=[cos(a12/2) mnk*sin(a12/2)*(kfq2)] ……5.4)
其中,kfq2=((1-c12t(1)^2)/sum(c12t(2:4).^2))^0.5;
步骤四:
将所述平面示踪板Board的第一次等效旋转和第二次等效旋转的旋转四元数相乘,计算得到从运动前时刻的平面示踪板Board到所述第二中间平面的旋转四元数:
该计算结果即为受测对象从所述运动前时刻到所述运动后时刻的旋转四元数。
步骤五:
参见图6至图8,以所述第一激光束Laser_o到第二激光束Laser_x的指向作为x轴,以所述第一激光束Laser_o到第三激光束Laser_y的指向作为y轴,将所述平面示踪板Board由运动前时刻至运动后时刻的平移运动等效分解成沿所述x轴和y轴的平移运动以及沿所述第一激光束Laser_o指向的平移运动;从而:
1)计算出第一运动轨迹Pon至Pxn的斜率,即所述平面示踪板Board沿所述x轴平移时,所述第一示踪点Po在平面示踪板Board上二维移动轨迹的斜率:
kx=(Pxn(2)-Pon(2))/(Pxn(1)-Pon(1)); ……7.1)
计算出第二运动轨迹Pon至Pyn的斜率,即所述平面示踪板Board沿所述y轴平移时,所述第一示踪点Po在平面示踪板Board上二维移动轨迹的斜率:
ky=(Pyn(2)-Pon(2))/(Pyn(1)-Pon(1)); ……7.2)
2)获得所述第一运动轨迹Pon至Pxn的方程系数:
klxn=[kx -1 0]; ……7.3)
获得所述第二运动轨迹Pon至Pyn的方程系数:
klyn=[ky -1 0]; ……7.4)
说明:当平面示踪板Board和受测对象沿上述x轴移动时,第一示踪点Po会沿着示踪板坐标系内的第一示踪点Po与第二示踪点Px的连线(下面简称Pon至Pxn直线)方向移动。由此可以计算Pon至Pxn直线的二维斜率并和Pon组成二维点斜式方程。点斜式方程可以转化为一般方程A×x+B×y+C=0;本文用三下标数组[A B C]表示此方程。
此处斜率为kx;
[A B C]=[kx -1 0];
当平面示踪板Board和受测对象沿上述y轴移动时,第一示踪点Po会沿着示踪板坐标系内的第一示踪点Po与第三示踪点Py的连线(下面简称Pon至Pyn直线)方向移动。由此可以计算Pon至Pyn直线的二维斜率并和Pon组成二维点斜式方程。点斜式方程可以转化为一般方程A×x+B×y+C=0;本文用三下标数组[A B C]表示此方程。
此处斜率为ky;
[A B C]=[ky -1 0];
3)计算出第一直线Po0至py的方程系数,该第一直线Po0至py经过所述运动前时刻的第一示踪点Po0且平行于所述第一运动轨迹Pon至Pxn,其与所述第二运动轨迹Pon至Pyn相交于第二交点py:
klxno=[kx -1 Po0(2)-kx*Po0(1)]; ……8.1)
计算出第二直线Po0至px的方程系数,该第二直线Po0至px经过所述运动前时刻的第一示踪点Po0且平行于所述第二运动轨迹Pon至Pyn,其与所述第一运动轨迹Pon至Pxn相交于第一交点px:
klyno=[ky -1 Po0(2)-ky*Po0(1)]; ……8.2)
根据所述8.1)式、8.2)式以及7.3)式、7.4)式解出所述第一交点px和第二交点py的坐标,由此计算出所述第一交点px到运动后时刻的第一示踪点Pon的长度:
dx=norm(px); ……8.3)
并计算出所述第二交点py到运动后时刻的第一示踪点Pon的长度:
dy=norm(py); ……8.4)
最后,根据所述8.3)式和2.6)计算出受测对象从所述运动前时刻到所述运动后时刻的沿x轴的平移量,并根据下式判断受测对象在x轴上的平移方向:
af=atan(mnk(3)/mnk(2));
dy=dy*sin(af);
if py(2)<0;dy=-dy; ……9.1)
根据所述8.4)式和2.6)计算出受测对象从所述运动前时刻到所述运动后时刻的沿y轴的平移量,并根据下式判断受测对象在y轴上的平移方向:
af=atan(mnk(3)/mnk(1));
dx=dx*sin(af);
if px(1)<0;dx=-dx; ……9.2)
说明:示踪板在空间上正交的x、y三维平移,Po点会在示踪板上沿Pon至Pxn和Pon至Pyn两条非正交的直线上平移。
Po点的平移按照沿Pon Pxn和Pon Pyn方向矢量分解得到Po点平移在示踪板上沿Pon Pxn和Pon Pyn方向的距离。
过Po0与Pon Pyn平行的直线与Pon Pxn的交点px坐标由以下过程求出。
A(1,:)=klxn(1:2);
A(2,:)=klyno(1:2);
B=-[klxn(3);klyno(3)];
px=A\B;
过Po0与Pon Pxn平行的直线与Pon Pyn的交点py坐标由以下过程求出。
A(1,:)=klyn(1:2);
A(2,:)=klxno(1:2);
B=-[klyn(3);klxno(3)];
py=A\B。
最后,受测对象沿z轴的平移可以通过现有的测量方法实现。
另外,本发明数据采集装置的指引激光还可以增设一束或者以上所述的激光束,通过在四束或者以上的激光束中抽取其中三束,分别用上述三维空间坐标寻迹方法进行计算,即可通过重复测量的方式提高本发明获得的受测对象的旋转四元数以及x轴、y轴平移量的测量精确度。
本发明的指引激光也可以通过支架安装在上述固定位置上,当受测对象移动至将近偏离指引激光,即有一个示踪点将近超出平面示踪板Board时,可以让受测对象停止下来,通过上述支架调节指引激光的指向,使得各个示踪点重新回到平面示踪板的合适位置,并记录下支架调节指引激光指向的方位角及俯仰角,然后再次启动受测对象,继续对受测对象进行测量,实现对受测对象的连续寻迹。
作为本发明装置的一个应用案例,本发明装置可以使用在在先申请《一种基于视觉测量的隧道断面轮廊测量装置》的姿态循迹上,指引激光及经纬调节支架安装于轨道上,示踪板安装于《一种基于视觉测量的隧道断面轮廊测量装置》上。并跟随检测车装置一起运动,通过本装置,获得检测车沿着x、y、z三轴旋转的角度和沿着x、y轴平移的位移。
本发明不局限与上述具体实施方式,根据上述内容,按照本领域的普通技术知识和惯用手段,在不脱离本发明上述基本技术思想前提下,本发明还可以做出其它多种形式的等效修改、替换或变更,均落在本发明的保护范围之中。例如,仅要求第一激光束Laser_o与第二激光束Laser_x所在的平面与第一激光束Laser_o与第三激光束Laser_y所在的平面相垂直,而不必平行于水平面和竖直平面。
Claims (7)
1.一种用于三维空间坐标寻迹的数据采集装置,其特征在于:所述的数据采集装置包括指引激光、平面示踪板(Board)和图像传感器;所述指引激光设有三束相互平行且不共面的激光束(Laser_o,Laser_x和Laser_y),指引激光安装在固定位置上并指向固定方向,其中,第一激光束(Laser_o)与第二激光束(Laser_x)所在的平面与第一激光束(Laser_o)与第三激光束(Laser_y)所在的平面相垂直,所述平面示踪板(Board)安装在受测对象上,并分别与水平面、竖直平面形成不为0°且不为90°的夹角(α和β),所述第一至第三激光束(Laser_o,Laser_x和Laser_y)依次在平面示踪板(Board)上形成第一至第三示踪点(Po,Px和Py),所述图像传感器安装在受测对象上,用以记录下包含有受测对象三维运动姿态信息的所述各个示踪点在平面示踪板(Board)上的二维运动轨迹。
2.根据权利要求1所述的数据采集装置,其特征在于:所述第一激光束(Laser_o)与第二激光束(Laser_x)所在的平面平行于水平面,第一激光束(Laser_o)与第三激光束(Laser_y)所在的平面平行于竖直平面。
3.根据权利要求2所述的数据采集装置,其特征在于:所述平面示踪板(Board)分别与水平面、竖直平面形成的夹角(α和β)接近于45°。
4.根据权利要求1至3任意一项所述的数据采集装置,其特征在于:所述的指引激光还设有一束或者以上所述的激光束。
5.根据权利要求4所述的数据采集装置,其特征在于:所述的数据采集装置还包括支架;所述指引激光通过该支架安装在固定位置上,所述支架能够调节指引激光的指向。
6.一种应用权利要求1至5任意一项所述数据采集装置的三维空间坐标寻迹方法,包括以下步骤:
步骤一:
用权利要求1至5任意一项所述的数据采集装置对受测对象进行测量,以获取在受测对象的运动过程中所述第一至第三示踪点(Po、Px和Py)在平面示踪板(Board)上的二维运动轨迹;
步骤二:
将所述平面示踪板(Board)由运动前时刻至运动后时刻的旋转运动等效分解成第一次等效旋转和第二次等效旋转;
其中,第一次等效旋转为:以运动前时刻的所述第一示踪点(Po)作为等效旋转点,将运动前时刻的平面示踪板(Board)绕第一等效旋转轴转过第一等效旋转角到达第一中间平面,使得所述第一中间平面的法线与运动后时刻的所述平面示踪板(Board)的法线相平行,所述第一等效旋转轴即为所述平面示踪板(Board)在运动前、后时刻的经过所述等效旋转点的法线公垂线,所述第一示踪点(Po)在第一次等效旋转前后位置不变;
所述第二次等效旋转为:将所述位于第一中间平面的平面示踪板(Board)绕第二等效旋转轴转过第二等效旋转角到达第二中间平面,其中所述第二等效旋转轴为第一中间平面经过所述等效旋转点的法线,使得所述第一示踪点(Po)与第二示踪点(Px)在第二次等效旋转后的连线与在运动后时刻的连线相平行,所述第一示踪点(Po)与第三示踪点(Py)在第二次等效旋转后的连线与在运动后时刻的连线相平行,所述第一示踪点(Po)在第二次等效旋转前后位置不变;
从而根据所述步骤一获得的二维运动轨迹,计算出所述平面示踪板(Board)的第一次等效旋转的规范化旋转四元数;
步骤三:
计算出所述平面示踪板(Board)的第二次等效旋转的规范化旋转四元数;
步骤四:
将步骤二、三计算出的所述平面示踪板(Board)的第一次等效旋转和第二次等效旋转的旋转四元数相乘,计算得到从运动前时刻的平面示踪板(Board)到所述第二中间平面的旋转四元数,该计算结果即为受测对象从所述运动前时刻到所述运动后时刻的旋转四元数。
7.根据权利要求6所述的三维空间坐标寻迹方法,其特征在于:所述的三维空间坐标寻迹方法还包括:
步骤五:
以所述第一激光束(Laser_o)到第二激光束(Laser_x)的指向作为x轴,以所述第一激光束(Laser_o)到第三激光束(Laser_y)的指向作为y轴,将所述平面示踪板(Board)由运动前时刻至运动后时刻的平移运动等效分解成沿所述x轴和y轴的平移运动以及沿所述第一激光束(Laser_o)指向的平移运动;计算出受测对象从所述运动前时刻到所述运动后时刻的分别沿x轴和y轴的平移量。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310698879.7A CN103673882B (zh) | 2012-12-27 | 2013-12-18 | 一种三维空间坐标寻迹方法及其数据采集装置 |
Applications Claiming Priority (4)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201210581525.X | 2012-12-27 | ||
CN201210581525X | 2012-12-27 | ||
CN201210581525 | 2012-12-27 | ||
CN201310698879.7A CN103673882B (zh) | 2012-12-27 | 2013-12-18 | 一种三维空间坐标寻迹方法及其数据采集装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103673882A true CN103673882A (zh) | 2014-03-26 |
CN103673882B CN103673882B (zh) | 2016-05-18 |
Family
ID=50312166
Family Applications (2)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201320841786.0U Expired - Lifetime CN204007506U (zh) | 2012-12-27 | 2013-12-18 | 一种用于三维空间坐标寻迹的数据采集装置 |
CN201310698879.7A Expired - Fee Related CN103673882B (zh) | 2012-12-27 | 2013-12-18 | 一种三维空间坐标寻迹方法及其数据采集装置 |
Family Applications Before (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201320841786.0U Expired - Lifetime CN204007506U (zh) | 2012-12-27 | 2013-12-18 | 一种用于三维空间坐标寻迹的数据采集装置 |
Country Status (1)
Country | Link |
---|---|
CN (2) | CN204007506U (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104237380A (zh) * | 2014-10-10 | 2014-12-24 | 北京理工大学 | 复杂构件机械手扫查位姿的四元数转换方法 |
CN106092069A (zh) * | 2016-08-24 | 2016-11-09 | 湖南科天健光电技术有限公司 | 一种地下工程施工中的导向和位移监测的装置 |
CN110030932A (zh) * | 2019-05-24 | 2019-07-19 | 广东嘉腾机器人自动化有限公司 | Agv偏移测量方法及agv偏移测量装置 |
CN111121735A (zh) * | 2020-01-03 | 2020-05-08 | 中国船舶重工集团公司第七0七研究所 | 一种隧道、地铁和矿井开挖掘进自主定位定向***及方法 |
CN112964245A (zh) * | 2021-02-24 | 2021-06-15 | 中国人民解放军92728部队 | 一种利用光电经纬仪定位的方法 |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN204007506U (zh) * | 2012-12-27 | 2014-12-10 | 广州市地下铁道总公司 | 一种用于三维空间坐标寻迹的数据采集装置 |
CN104634249B (zh) * | 2015-02-06 | 2017-08-29 | 南京理工大学 | 推进剂加注过程中运载火箭运动信号探测定位方法 |
CN109115172B (zh) * | 2018-08-23 | 2020-09-08 | 西安科技大学 | 基于机器视觉的采煤机机身位姿检测方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH09212621A (ja) * | 1996-01-30 | 1997-08-15 | Fujitsu Ltd | 動画像による運動計測装置 |
CN1240270A (zh) * | 1999-07-02 | 2000-01-05 | 清华大学 | 目标空间位置及姿态激光跟踪测量***及其测量方法 |
CN101526337A (zh) * | 2009-04-21 | 2009-09-09 | 吴晓军 | 三维图像扫描***及三维图像扫描方法 |
EP2343685A1 (en) * | 2008-10-01 | 2011-07-13 | Sony Computer Entertainment Inc. | Information processing device, information processing method, program, and information storage medium |
CN204007506U (zh) * | 2012-12-27 | 2014-12-10 | 广州市地下铁道总公司 | 一种用于三维空间坐标寻迹的数据采集装置 |
-
2013
- 2013-12-18 CN CN201320841786.0U patent/CN204007506U/zh not_active Expired - Lifetime
- 2013-12-18 CN CN201310698879.7A patent/CN103673882B/zh not_active Expired - Fee Related
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH09212621A (ja) * | 1996-01-30 | 1997-08-15 | Fujitsu Ltd | 動画像による運動計測装置 |
CN1240270A (zh) * | 1999-07-02 | 2000-01-05 | 清华大学 | 目标空间位置及姿态激光跟踪测量***及其测量方法 |
EP2343685A1 (en) * | 2008-10-01 | 2011-07-13 | Sony Computer Entertainment Inc. | Information processing device, information processing method, program, and information storage medium |
CN101526337A (zh) * | 2009-04-21 | 2009-09-09 | 吴晓军 | 三维图像扫描***及三维图像扫描方法 |
CN204007506U (zh) * | 2012-12-27 | 2014-12-10 | 广州市地下铁道总公司 | 一种用于三维空间坐标寻迹的数据采集装置 |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104237380A (zh) * | 2014-10-10 | 2014-12-24 | 北京理工大学 | 复杂构件机械手扫查位姿的四元数转换方法 |
CN104237380B (zh) * | 2014-10-10 | 2017-01-18 | 北京理工大学 | 复杂构件机械手扫查位姿的四元数转换方法 |
CN106092069A (zh) * | 2016-08-24 | 2016-11-09 | 湖南科天健光电技术有限公司 | 一种地下工程施工中的导向和位移监测的装置 |
CN110030932A (zh) * | 2019-05-24 | 2019-07-19 | 广东嘉腾机器人自动化有限公司 | Agv偏移测量方法及agv偏移测量装置 |
CN110030932B (zh) * | 2019-05-24 | 2020-12-15 | 广东嘉腾机器人自动化有限公司 | Agv偏移测量方法及agv偏移测量装置 |
CN111121735A (zh) * | 2020-01-03 | 2020-05-08 | 中国船舶重工集团公司第七0七研究所 | 一种隧道、地铁和矿井开挖掘进自主定位定向***及方法 |
CN112964245A (zh) * | 2021-02-24 | 2021-06-15 | 中国人民解放军92728部队 | 一种利用光电经纬仪定位的方法 |
CN112964245B (zh) * | 2021-02-24 | 2024-03-29 | 中国人民解放军92728部队 | 一种利用光电经纬仪定位的方法 |
Also Published As
Publication number | Publication date |
---|---|
CN103673882B (zh) | 2016-05-18 |
CN204007506U (zh) | 2014-12-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103673882A (zh) | 一种三维空间坐标寻迹方法及其数据采集装置 | |
Chen et al. | A railway track geometry measuring trolley system based on aided INS | |
Kennie | Engineering surveying technology | |
CN103323855B (zh) | 一种基线动态测量***的精度获取方法 | |
CN106052716B (zh) | 惯性系下基于星光信息辅助的陀螺误差在线标定方法 | |
CN101655343B (zh) | 电子经纬仪空间坐标测量***校准用靶标、底座和基准尺 | |
CN103063203B (zh) | 大地测量***和操作大地测量***的方法 | |
CN106767930A (zh) | 一种捷联惯导与对准棱镜安装偏角测量方法 | |
CN107202582A (zh) | 静止卫星在轨实时成像导航与配准方法 | |
CN101523154A (zh) | 用于确定细长物体的定向参数的装置和方法 | |
CN101169320B (zh) | 运动物体的瞬时姿态测量装置 | |
CN103234496B (zh) | 一种三坐标测量机二维平台误差的高精度校正方法 | |
CN102207386A (zh) | 基于方位效应误差补偿的寻北方法 | |
Li et al. | Laser-aided INS and odometer navigation system for subway track irregularity measurement | |
CN101625235B (zh) | 基于多个分体惯性基准的轨道半径曲线测量*** | |
Zhu et al. | A calibration method of USBL installation error based on attitude determination | |
CN104535042B (zh) | 基于非正交轴系激光经纬仪的测量方法 | |
CN105004269A (zh) | 一种用于激光跟踪仪的四象限传感器光斑偏移量测量方法 | |
CN105953795A (zh) | 一种用于航天器表面巡视的导航装置及方法 | |
CN103162645A (zh) | 一种基于椭偏度测量的滚转角误差测量方法和装置 | |
CN102087117A (zh) | 飞船交会对接用测距敏感器精度的地面测量方法 | |
Zhang et al. | A calibration method of ultra-short baseline installation error with large misalignment based on variational Bayesian unscented Kalman filter | |
Ghosh | Fundamentals of computational photogrammetry | |
CN101118705B (zh) | 模拟gps卫星定位实验装置及实验方法 | |
Xu et al. | In-motion coarse alignment method for SINS/GPS integration in polar region |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C53 | Correction of patent of invention or patent application | ||
CB03 | Change of inventor or designer information |
Inventor after: Liu Guangwu Inventor after: Zhu Maozhi Inventor after: Shi Lei Inventor before: Zhu Maozhi Inventor before: Shi Lei |
|
COR | Change of bibliographic data |
Free format text: CORRECT: INVENTOR; FROM: ZHU MAOZHI SHI LEI TO: LIU GUANGWU ZHU MAOZHI SHI LEI |
|
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20160518 Termination date: 20191218 |
|
CF01 | Termination of patent right due to non-payment of annual fee |