一种高精度Mark点的注册方法与流程

本发明涉及mark定位技术领域，具体涉及一种高精度mark点的注册方法。

背景技术

灯塔技术就是利用灯塔作为基站，发射信号与被测设备通信，从而完成定位的技术。

每个灯塔可以发出水平和垂直两道平面红外激光，进行扫描，这两道激光可以扫描覆盖空间的所有物体。请求定位的设备安装若干红外传感器，这些红外传感器之间的空间位置是固定的。此时从灯塔角度看，可以等效为透视投影，利用透视投影变换可以得到所有传感器的平面图像。

灯塔开始扫描时发出同步信号，传感器得到同步信号后开始计时，此时时间为t1，当某个传感器检测到激光后，时间记为t2，激光扫描是匀速进行的，设角速度为ω，那么可通过θ＝ω×(t2-t1)求得扫描角度，假设扫描中心(等效为摄像头光心)距离成像面为l，l为常量，则可通过下公式计算出被测物体上所有传感器在成像面上的坐标，即它们透视投影后的坐标值。

此时，传统方法对求解位置的问题变为perspective-n-point，即通过透视投影得到的二维坐标值和透视投影前的对应三维坐标值来评估物体的位姿的方法，该技术可以对宏观物体进行高速、高精度的定位，但对于较小的物体，如mark点，它本身的大小和传感器是一个量级的，因此便不能通过perspective-n-point估算位姿。

技术实现要素：

本发明实施例的目的在于提供一种高精度mark点的注册方法，用以解决传统灯塔技术不能定位微小目标的问题。

为实现上述目的，本发明实施例提供一种：

一种高精度mark点的注册方法，包括定位空间，所述定位空间内设有至少3个安装在被检测物体上的mark贴、安装在工具上的第一定位传感器、安装在mark贴上的第二定位传感器及两个灯塔激光定位器，所述至少3个安装在被检测物体上的mark贴不能处于同一平面内，所述定位空间内设有相互独立的虚拟坐标系、工具坐标系及mark坐标系。

获取第一定位传感器位于工具坐标系内的工具坐标系坐标。

灯塔激光定位器采集第一定位传感器的虚拟坐标系坐标，根据所述工具坐标系坐标与第一定位传感器的虚拟坐标系坐标计算工具坐标系与虚拟坐标系的映射关系，得到第一映射关系。

获取至少3个mark贴位于mark坐标系内的mark点坐标，灯塔激光定位器采集第二定位传感器的虚拟坐标系坐标，根据所述mark点坐标与第二定位传感器的虚拟坐标系坐标计算工具坐标系与虚拟坐标系的映射关系，得到第二映射关系。

根据第一映射关系与第二映射关系计算工具坐标系与mark坐标系的映射关系，得到第三映射关系。

本发明实施例具有如下优点：

本发明实施例为了克服传统灯塔技术不能定位微小目标的缺陷，对定位原理、使用方式提出了更改，并提出了虚拟坐标系的概念，可以利用灯塔技术固有的优点，快速得到被测物体的虚拟坐标。而且在定位速度、范围没有明显衰减的情况下，可以对微小物体进行定位；可以对无法进行精确传感器安装的物体进行定位。

附图说明

图1为本发明实施例提供的一种高精度mark点的注册方法的整体结构图。

图2为本发明实施例提供的一种高精度mark点的注册方法的位于xy平面坐标系中设备结构图；

图3为本发明实施例提供的一种高精度mark点的注册方法的位于yz平面坐标系中设备结构图。

具体实施方式

以下由特定的具体实施例说明本发明的实施方式，熟悉此技术的人士可由本说明书所揭露的内容轻易地了解本发明的其他优点及功效。

须知，本说明书所附图式所绘示的结构、比例、大小等，均仅用以配合说明书所揭示的内容，以供熟悉此技术的人士了解与阅读，并非用以限定本发明可实施的限定条件，故不具技术上的实质意义，任何结构的修饰、比例关系的改变或大小的调整，在不影响本发明所能产生的功效及所能达成的目的下，均应仍落在本发明所揭示的技术内容得能涵盖的范围内。同时，本说明书中所引用的如“上”、“下”、“左”、右”、“中间”等的用语，亦仅为便于叙述的明了，而非用以限定本发明可实施的范围，其相对关系的改变或调整，在无实质变更技术内容下，当亦视为本发明可实施的范畴。

如图1所示，一种高精度mark点的注册方法，包括定位空间，所述定位空间内设有至少3个安装在被检测物体上的mark贴、安装在工具上的第一定位传感器、安装在mark贴上的第二定位传感器及两个灯塔激光定位器，所述灯塔激光定位器的激光束使用50％占空比且频率为2mhz的pwm进行调制。所述至少3个安装在被检测物体上的mark贴不能处于同一平面内，所述定位空间内设有相互独立且相互叠加的虚拟坐标系、工具坐标系及mark坐标系。由于虚拟坐标系、工具坐标系及mark坐标系之间相互独立且来源不同，该三者坐标系之间的尺度不同，因此需要通过以下方法可得到工具坐标系与mark坐标系之间的映射关系。

获取第一定位传感器位于工具坐标系内的工具坐标系坐标。在工具上设定至少3个工具映射坐标点，并在该工具映射坐标点上安装第一定位传感器，该工具映射坐标点为第一定位传感器的工具坐标系坐标。

第一映射关系

灯塔激光定位器采集第一定位传感器的虚拟坐标系坐标，根据所述工具坐标系坐标与第一定位传感器的虚拟坐标系坐标计算工具坐标系与虚拟坐标系的映射关系，得到第一映射关系。所述第一映射关系的计算方法如下：

设全部工具映射坐标点的坐标集合c1，设第一定位传感器位于虚拟坐标系内的坐标集合c2，设rtv为m×n阶旋转矩阵、ttv为m×n阶转换矩阵，则有方程：

c2＝rtv*c1+ttv；

使用奇异值分解svd(singularvaluedecomposition)对上式进行计算；

其中，centroidc1为c1的坐标平均值，centroidc2为c2的坐标平均值，h的奇异值分解得到矩阵u、σ、v；

[u,σ,v]＝svd(h)；

其中，u是m×m阶酉矩阵，σ是半正定m×n阶对角矩阵，而v，即rtv的共轭转置，是n×n阶酉矩阵；

得到，

rtv＝vu^t，其中t代表ttv；

ttv＝-rtv*centroidc1+centroidc2；

灯塔无法进行绝对定位，所以引入了虚拟坐标系，虚拟坐标系跟真实世界坐标有一个比例关系，即虚拟坐标系与工具坐标系的尺度不同，假设尺度倍数为λ1，则有

简化后得到

其中，

旋转矩阵

转换矩阵

第二映射关系

获取至少3个mark贴位于mark坐标系内的mark点坐标。灯塔激光定位器采集第二定位传感器的虚拟坐标系坐标，根据所述mark点坐标与第二定位传感器的虚拟坐标系坐标计算工具坐标系与虚拟坐标系的映射关系，得到第二映射关系。所述第二映射关系的计算方法如下：

在被检测物体上设定至少3个物体映射坐标点，并在该物体映射坐标点上安装第二定位传感器，该物体映射坐标点为第二定位传感器的mark点坐标；

设全部物体映射坐标点的坐标集合d1，设第二定位传感器位于虚拟坐标系内的坐标集合d2，设rmv为m×n阶旋转矩阵、tmv为m×n阶转换矩阵，则有方程：

d2＝rmv*d1+tmv；

使用奇异值分解svd(singularvaluedecomposition)对上式进行计算；

其中，centroidd1为d1的坐标平均值，centroidd2为d2的坐标平均值，h的奇异值分解得到矩阵u、σ、v；

[u,σ,v]＝svd(h)；

其中，u是m×m阶酉矩阵，σ是半正定m×n阶对角矩阵，而v，即rmv的共轭转置，是n×n阶酉矩阵；

得到，

rmv＝vu^t，其中t代表tmv；

tmv＝-rmv*centroidd1+centroidd2；

灯塔无法进行绝对定位，所以引入了虚拟坐标系，虚拟坐标系跟真实世界坐标有一个比例关系，即虚拟坐标系与maek坐标系的尺度不同，假设尺度倍数为λ2，假设尺度倍数为λ2，则有

简化后得到

其中，

旋转矩阵

转换矩阵

第三映射关系

根据第一映射关系与第二映射关系计算工具坐标系与mark坐标系的映射关系，得到第三映射关系。所述第三映射关系的计算方法如下：

设工具坐标系到虚拟坐标系的转换矩阵为ttv，其逆矩阵为tvt；

设mark坐标系到虚拟坐标系的转换矩阵为tmv，其逆矩阵为tvm；

取任意工具映射坐标点为pt，其在mark坐标系中的对应点为pm，则有

pm＝tvm*(ttv*pt)；

pt＝tvt*(tmv*pm)；

根据矩阵运算的结合律，则可将以上公式简写为

tmt＝tvm*ttv；

tvm＝tvt*tmv；

pm＝tmt*pt；

pt＝tvm*pm。

采集定位传感器的虚拟坐标值的具体方法

所述灯塔激光定位器包括第一灯塔定位器与第二灯塔定位器，所述第一灯塔定位器与第二灯塔定位器具有严格的角色定义，二者不能替换，二者的激光扫描点一致且扫描方向相反，灯塔激光定位器采集定位传感器的虚拟坐标系坐标的具体方法如下，其中定位传感器为第一定位传感器或第二定位传感器：

第一灯塔定位器与第二灯塔定位器相对设置且轴心合一，得到间距l；

定位传感器坐标x值的计算方法：

xy平面坐标系中，如图2所示，l与定位传感器之间的夹角，得到第一灯塔定位器的扫描角度φ0与第二灯塔定位器的扫描角度φ1；

根据角φ0与角φ1得到定位传感器到达第一灯塔定位器的连线a0与到达第二灯塔定位器的连线b0之间的夹角β，计算公式为

β＝180°-φ0-φ1；

计算a0与b0，根据正弦定理得到

定位传感器与l之间的距离即x的坐标值，x的方向由第一灯塔定位器的扫描角度决定，当扫描角度为[0,90]之间时为正，扫描角度为(90，180]时为负

x0＝a0×sinφ0；

x1＝b0×sinφ1；

由x0与x1求得融合公式

x＝c*x0+(1-c)*x1；

其中，c的取值范围为[0,1]；

定位传感器坐标z值的计算方法

yz平面坐标系中，如图3所示，其计算原理与坐标x同理，得到

z＝c*z0+(1-c)*z1；

其中，z0为定位传感器到达第一灯塔定位器的连线距离；

z1为定位传感器到达第二灯塔定位器的连线距离；

c的取值范围为[0,1]；

定位传感器坐标y值的计算方法

xy平面坐标系中

y0＝a0×cosφ0；

y1＝b0×cosφ1；

其中y0为第一灯塔定位器与定位传感器之间的距离值；

y1为第二灯塔定位器与定位传感器之间的距离值；

将y0与y1的公式融合，得到第一融合公式

y00＝c0*y0+(1-c0)*y1；

yz平面坐标系中

y2＝a1×cosψ0；

y3＝b1×cosψ1；

其中y2为第一灯塔定位器与定位传感器之间的距离值；

y3为第二灯塔定位器与定位传感器之间的距离值；

将y2与y3的公式融合，得到第二融合公式

y11＝c1*y2+(1-c1)*y3；

将第一融合公式与第二融合公式融合，得到y值计算公式

y＝c2*y00+(1-c2)*y11；

其中c0、c1、c2取值范围为[0,1]；

综上得到定位传感器的虚拟坐标值(x，y，z)。

所述灯塔激光定位器采集定位传感器的旋转角度计算方法为：

灯塔激光定位器在扫描开始时发出同步信号，定位传感器接收到同步信号后开始计时t1，扫描方式为匀速扫描，其扫描角速度为ω；当定位传感器检测到扫描激光后计时t2，得到扫描角度θ＝ω(t2-t1)。

测量角度误差处理

电机在转动过程中不可避免会产生速度差和相位差。速度差和相位差难以具体估测，因此采取一个统计性的补偿，这对结果至少可产生有益的影响。所述扫描角度的角度误差补偿公式如下：

θ＝(t2-t1)(ω+δω)-δφ；

其中，δω是速度偏差补偿；

δφ是相位偏差补偿。

抖震误差处理

每个灯塔当中安装有两个电机，电机以3600rpm的速度运转，不可避免会带来微小的震动。对准装置中的光导航元件用以捕捉这种抖动。光导航原件的输出以自身为参考，得到的偏差等效于从站相对于主站的微小抖动。该装置可以捕获x和z方向上的抖动，对于y方向上的抖动，采用x和z方向的混合值。

y＝c×x+(1-c)×z

校准定位系统时，在对准激光线上放置传感器，此时系统处于奇异状态，对于传感器而言，两个灯塔测得的角度理论值都为0。但实际上系统存在偏差，系统此时采集测量值和光导航元件的输出，建立映射表。因为表并不能一一对应，故对于所有的输入值采用最近邻取值法。例如：

系统正常工作中扫描得到某传感器的ψ0值，此时使用采集到的光导航原件输出在查找表中查找最相近的值，这个值即是一个合理的偏差，将这个偏差补偿到ψ0，作为最后的有效值。

光干扰误差处理

根据理论计算，激光信号随着定位装置和灯塔之间的距离线性衰减(实际上，通过非理想双凸透镜的光的衰减速度更快)，另一方面，考虑到安全性，不允许使用太强的激光，因此，为了区分扫描信号和环境干扰光(定位装置不是理想的光敏元件，如荧光光源会误触发)，对激光束使用50％占空比频率为2mhz的pwm进行调制。

虽然，上文中已经用一般性说明及具体实施例对本发明作了详尽的描述，但在本发明基础上，可以对之作一些修改或改进，这对本领域技术人员而言是显而易见的。因此，在不偏离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。