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

权利要求书

1.一种模块化舞台激光定位方法，所述方法包括以下步骤：

1)：建立地图

在舞台周边放置M根反光柱；反光柱个数大于等于3，小于20；以舞台中心作为坐标原点建立全局的直角坐标系，各反光柱在全局直角坐标系中的坐标用集合GF＝{F_Gi(X_Gi,Y_Gi)|i＝1,2,…,M}表示，各反光柱之间的距离用集合GD＝{D_Gij|i,j＝1,2,…,M}表示；集合GF和集合GD中的数据即为地图数据；

2)：使用静态定位方式确定模块化舞台位姿

激光传感器的静态定位方式不依赖于上一时刻的位姿，在任意时刻开始定位，用于确定模块化舞台初始位姿；模块化舞台上的激光传感器扫描周围一周，以模块化舞台中心为坐标原点，激光传感器0度方向为Y轴正方向建立局部坐标系；利用激光原始数据计算出各个反光柱在局部坐标系中的坐标，用集合LF＝{F_Li(X_Li,Y_Li)|i＝1，2，…,n；n≤M}表示；计算局部坐标系中的各个反光柱之间距离，用集合LD＝{D_Lij|i,j＝1,2,…,n；n≤M}表示；设模块化舞台在全局坐标系中的位姿为P_k(X_k,Y_k,θ_k)；将集合GD的距离值与集合LD中的距离值作比较，获得局部坐标系中的反光柱坐标与全局坐标系中的反光柱坐标的对应关系用集合GLF＝{C_ij|i,j＝1,2,…,n；n≤M}；利用基于最小二乘法的位姿计算公式得P_k；计算公式参见式(1)：

其中，矩阵A中数据为局部坐标系中反光柱坐标，矩阵b中数据为与局部坐标系中的反光柱相对应的全局坐标系中的坐标；

利用最小二乘法求解公式获得矩阵H的值，求解公式参见式(2)：

H＝[cosθ_k sinθ_k X_k Y_k]^T＝(A^T·A)^-1·A^T·b (2)

其中，矩阵H中含有模块化舞台的位姿数据，根据矩阵H求得舞台位姿P_k(X_k,Y_k,θ_k)；

基于最小二乘法的位姿计算公式相对于三点定位法定位更加精确，该公式利用多个反光柱坐标进行位姿计算，实现误差的平方和最小化；

3)采用动态定位方式确定模块化舞台位姿

a.估计下一时刻舞台位姿及反光柱在局部坐标系中的位置

模块化舞台的动态定位方式需要获取前一时刻的位姿以及舞台的运动状态；根据前两次的位置估算出下一时刻的位姿，位姿估计公式参见式(3)：

根据估算的位姿以及平面直角坐标系坐标转换矩阵，获得全局坐标系中的反光柱在下一时刻局部坐标系中的坐标，坐标转换公式参见式(4):

其中，为全局坐标系中的第i个反光柱在局部坐标系中的估计坐标，矩阵R为直角坐标系的旋转平移矩阵，[X_Gi Y_Gi]^T为在全局坐标系中第i个反光柱的坐标，为模块化舞台在全局坐标系中的估计坐标，为模块化舞台在全局坐标系中的估计角度；

b.使用反光柱估计的位置与扫描到的位置进行匹配

反光柱在局部坐标系中的估计位置用集合表示；使用激光传感器进行一次扫描，获得反光柱在局部坐标系中的实际位置，用集合LF＝{F_Li(X_Li,Y_Li)|i＝1,2,…,n；n≤M}表示，匹配集合LE和集合LF中的反光柱位置，当两个集合中的反光柱位置差距小于阈值，则代表两反光柱匹配成功；匹配判断公式参见式(5)

其中，F_Li为采集到的反光柱在局部坐标系中的实际位置，为估计的反光柱在局部坐标系中的位置，ε为阈值；

采用动态匹配将干扰反射源剔除，提高定位稳定性和精度；因为需要将采集到的数据与估计值匹配，而干扰反射源与估计值差距较大无法匹配成功，所以可以剔除干扰反射源；

c.更新模块化舞台位姿及反光柱坐标

经过匹配，筛选出有效的反光柱位置，使用基于最小二乘法的位姿计算公式计算获得模块化舞台的位姿。

2.根据权利要求1所述的一种模块化舞台激光定位方法，其特征在于，模块化舞台的定位过程，包括以下步骤：

1)：建立地图，在舞台周边布置反光柱，以舞台中心为坐标原点建立直角坐标系；使用全站仪获取反光柱在全局坐标系中的坐标，将反光柱位置数据输入底层控制计算机并计算各反光柱之间的距离，保存反光柱位置数据和距离数据，建图完毕；

2)：初始化位姿，在上位机控制软件中点击“初始化位姿”按钮，模块化舞台中的控制计算机启动激光传感器，获取反光柱位置数据，并计算反光柱之间的距离；将获取的反光柱位置数据和计算得到的距离数据与之前保存的地图数据匹配；使用基于最小二乘法的位姿计算公式计算匹配的数据，获得模块化舞台的位姿；

3)：实时定位，当确定模块化舞台位姿后，点击上位机中“实时定位”按钮；模块化舞台底层控制计算机使用动态定位算法实时处理激光传感器的数据，计算舞台位姿并将位姿上传到上位机软件中显示。