[发明专利]一种模块化舞台激光定位方法

摘要

本发明公开了一种模块化舞台激光定位方法，包括以下步骤1)在舞台周围布置反光柱，以舞台中心为原点建立全局坐标系，使用全站仪获取各个反光柱的位置，建立地图；2)使用激光传感器扫描反光柱，获取局部坐标系下位置，并使用静态定位方式确定模块化舞台的初始位姿；3)使用动态定位方式对反光柱进行匹配，并根据反光柱的位置使用最小二乘法实时计算出模块化舞台位姿。本发明对针对模块化舞台的定位具有实时性强，定位精度高的特点。

主权项

一种模块化舞台激光定位方法，所述方法包括以下步骤：1)：建立地图在舞台周边放置M根适当直径、高度的反光柱；反光柱个数大于等于3，小于20；以舞台中心作为坐标原点建立全局的直角坐标系，各反光柱在全局直角坐标系中的坐标用集合GF＝{FGi(XGi,YGi)|i＝1,2,…,M}表示，各反光柱之间的距离用集合GD＝{DGij|i,j＝1,2,…,M}表示；集合GF和集合GD中的数据即为地图数据；2)：使用静态定位方式确定模块化舞台位姿激光传感器的静态定位方式不依赖于上一时刻的位姿，可在任意时刻开始定位，适合用于确定模块化舞台初始位姿；模块化舞台上的激光传感器扫描周围一周，以模块化舞台中心为坐标原点，激光传感器0度方向为Y轴正方向建立局部坐标系；利用激光原始数据计算出各个反光柱在局部坐标系中的坐标，用集合LF＝{FLi(XLi,YLi)|i＝1,2,…,n；n≤M}表示；计算局部坐标系中的各个反光柱之间距离，用集合LD＝{DLij|i,j＝1,2,…,n；n≤M}表示；设模块化舞台在全局坐标系中的位姿为Pk(Xk,Yk,θk)；将集合GD的距离值与集合LD中的距离值作比较，获得局部坐标系中的反光柱坐标与全局坐标系中的反光柱坐标的对应关系用集合GLF＝{Cij|i,j＝1,2,…,n；n≤M}，其中i,j分别代表局部坐标系中第i个反光柱和全局坐标系中第j个反光柱；利用基于最小二乘法的位姿计算公式得Pk；计算公式参见式(1)：其中，矩阵A中数据为局部坐标系中反光柱坐标，向量b中数据为与局部坐标系中的反光柱相对应的全局坐标系中的坐标；利用最小二乘法求解公式获得矩阵H的值，求解公式参见式(2)：H＝[cosθk sinθk Xk Yk]T＝(AT·A)‑1·b   (2)其中，矩阵H中含有模块化舞台的位姿数据，根据矩阵H求得舞台位姿Pk(Xk,Yk,θk)；基于最小二乘法的位姿计算公式相对于三点定位法定位更加精确；该公式可以利用多个反光柱坐标进行位姿计算，实现误差的平方和最小化，而三点定位法只能使用三个反光柱的坐标计算位姿，误差较大；3)采用动态定位方式确定模块化舞台位姿a.估计下一时刻舞台位姿及反光柱在局部坐标系中的位置模块化舞台的动态定位方式需要获取前一时刻的位姿以及舞台的运动状态；根据前两次的位置估算出下一时刻的位姿，位姿估计公式参见式(3)：根据估算的位姿以及平面直角坐标系坐标转换矩阵，获得全局坐标系中的反光柱在下一时刻局部坐标系中的坐标，坐标转换公式参见式(4):其中，为全局坐标系中的第i个反光柱在局部坐标系中的估计坐标，矩阵R为直角坐标系的旋转平移矩阵，[XGi YGi]T为在全局坐标系中第i个反光柱的坐标，为模块化舞台在全局坐标系中的估计坐标，为模块化舞台在全局坐标系中的估计角度；b.使用反光柱估计的位置与扫描到的位置进行匹配反光柱在局部坐标系中的估计位置用集合表示；使用激光传感器进行一次扫描，获得反光柱在局部坐标系中的实际位置，用集合LF＝{FLi(XLi,YLi)|i＝1,2,…,n；n≤M}表示，匹配集合LE和集合LF中的反光柱位置，当两个集合中的反光柱位置差距小于一定阈值，则代表两反光柱匹配成功；匹配判断公式参见式(5)其中，FLi为采集到的反光柱在局部坐标系中的实际位置，为估计的反光柱在局部坐标系中的位置，ε为阈值；采用动态匹配可以将干扰反射源剔除，提高定位稳定性和精度；因为需要将采集到的数据与估计值匹配，而干扰反射源与估计值差距较大无法匹配成功，所以可以剔除干扰反射源；c.更新模块化舞台位姿及反光柱坐标经过匹配，筛选出有效的反光柱位置，使用前文基于最小二乘法的位姿计算公式计算获得模块化舞台的位姿。