CN107817500B - 一种模块化舞台激光定位方法 - Google Patents

Abstract

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

Description

一种模块化舞台激光定位方法

技术领域

本发明应用于模块化舞台领域，涉及到模块化舞台的实时定位问题。

背景技术

近年来，随着物质、文化生活水平的提高，广大人民群众对艺术表演的需求迅速增长，文化设施建设的重要性日渐凸显，这对剧场等演艺场所的科技发展提出了更高的要求。而随着现代科技的进步，越来越多的高新科技在演艺场所中得到了运用，既提高了艺术表演的观赏性，也丰富了演艺场所艺术的表现手段。模块化舞台在表演中的应用，实现了快速切换场景和迁移演员的功能。现有的模块化舞台有轨道式和遥控式。轨道式模块化舞台只能在固定轨道上运动，运动轨迹虽然精确但是局限较大，只能做一维运动，并且需要铺设轨道，影响舞台美观。遥控式模块化舞台没有轨道限制，可以执行复杂的运动，但运动轨迹和运动速度需要人工实时控制。表演时无法保证模块化舞台的位置与预期的位置相同，并且运动轨迹精度随着操作者操作水平的不同而差距较大。

发明内容

为了克服已有模块化舞台的局限性，本发明采用激光定位技术实现模块化舞台的实时定位功能，提高模块化舞台运动轨迹的精度，为模块化舞台的自动化控制提供精确定位数据。

本发明解决其技术问题所采用的技术方案是：

一种模块化舞台激光定位方法，所述方法包括以下过程：

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}，其中i,j分别代表局部坐标系中第i个反光柱和全局坐标系中第j个反光柱。利用基于最小二乘法的位姿计算公式得P_k。计算公式参见式(1)：

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

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

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

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

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

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

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

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

根据估算的位姿

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

其中，

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

为模块化舞台在全局坐标系中的估计坐标，

为模块化舞台在全局坐标系中的估计角度。

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

反光柱在局部坐标系中的估计位置用集合

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

其中，F_Li为采集到的反光柱在局部坐标系中的实际位置，

为估计的反光柱在局部坐标系中的位置，ε为阈值。

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

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

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

本发明的技术构思为：针对模块化舞台的移动定位问题，建立特征地图，使用静态与动态相结合的激光定位算法，实现模块化舞台的实时精确定位,为模块化舞台的自动化控制提供位姿数据。

全套模块化舞台定位方法可以在模块化舞台中的计算机中完成，上位机监控软件只提供指令发送操作和位姿实时显示，不参与具体数据的获取和计算。

模块化舞台定位过程具体操作过程如下：

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

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

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

与传统的模块化舞台相比，本发明给出的模块化舞台激光定位方法最大的特点是定位精确，可与相应的轨迹规划程序相结合实现模块化舞台的全自动化。

附图说明

图1为模块化舞台及全局坐标系、局部坐标系示意图；

图2为模块化舞台动态定位示意图。

具体实施方式

为了使本发明的技术方案、设计思路能更加清晰，下面结合附图再进行详尽的描述。

本发明的模块化舞台激光定位方法，所述方法包括以下过程：

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}，其中i,j分别代表局部坐标系中第i个反光柱和全局坐标系中第j个反光柱。利用基于最小二乘法的位姿计算公式得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)采用动态定位方式确定模块化舞台位姿

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

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

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

其中

为全局坐标系中的第i个反光柱在局部坐标系中的坐标，矩阵R为直角坐标系的旋转平移矩阵，[X_Gi Y_Gi]^T为反光柱在全局坐标系中的坐标，

为模块化舞台在全局坐标系中的估计坐标，

为模块化舞台在全局坐标系中的估计角度。

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

反光柱在局部坐标系中的估计位置用集合

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

其中，F_Li为采集到的反光柱在局部坐标系中的实际位置，

为估计的反光柱在局部坐标系中的位置，ε为阈值。

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

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

本实施例为模块化舞台激光定位过程，具体操作过程如下：

1)：建立地图

在舞台四周布置4个反光柱，使用全站仪获得各个反光柱的位置分别为：F_G1(-1298，2434)，F_G2(-2338，-310),F_G3(1262，-1123)，F_G4(2187,1198)，并构成集合GF＝{F_G1，F_G2，F_G3，F_G4}。计算获得反光柱之间距离分别为D_G12＝2936,D_G13＝4384,D_G14＝3699,D_G23＝3692,D_G24＝4771,D_G34＝2499,并构成集合GD＝{D_G12，D_G13，D_G14，D_G23，D_G24，D_G34}。集合GF和集合GD中的数据即为地图数据。

2)：在软件操作界面中点击“初始化位姿”按钮，激光传感器扫描周围环境，进行静态定位。以模块化舞台中心为原点，激光0度方向为y轴正方向建立局部直角坐标系，扫描到的反光柱在局部坐标系中的位置分别为：F_L1(-1297，2436)，F_L2(-2335，-312),F_L3(1260，-1122)，F_L4(2185,1200),并构成集合LF＝{F_L1,F_L2,F_L3,F_L4},计算局部坐标系中各反光柱之间的距离得：D_L12＝2937,D_L13＝4381,D_L14＝3694,D_L23＝3685,D_L24＝4766,D_G34＝2500,并构成集合LD＝{D_L12，D_L13，D_L14，D_L23，D_L24，D_L34}。比较集合GD和LD中的距离值，获得局部坐标系中的反光柱坐标与全局坐标系中的反光柱坐标的对应关系，并用集合GLF＝{C₁₁,C₂₂,C₃₃,C₄₄}表示。利用位姿计算公式(1)计算模块化舞台的位姿，如下：

根据公式(2)求得模块化舞台的位姿为：P₁(1,0,0.004)。

当确定模块化舞台初始位姿后，在软件界面中点击“实时定位”按钮，使用动态定位方式进行定位。模块化舞台的动态定位方式需要获取前一时刻的位姿以及舞台的运动状态。根据前两次的位置估算出下一时刻的位姿，位姿估计公式如下：

计算获得下一时刻模块化舞台的位姿：

激光传感器扫描到的反光柱位置为：F_L1(-1713，1912),F_L2(135，-1279),F_L3(2464，-417),F_L4(1072，2995)。根据公式(3)，计算获得全局坐标系中的各个反光柱在局部坐标系中的估计位置：

匹配集合LE和集合LF中的反光柱位置，当两个集合中的反光柱位置差距小于阈值100时，则代表两反光柱匹配成功。匹配结果为GLF＝{C₁₂,C₂₃,C₃₄,C₄₁}。再根据位姿计算公式(1)计算得模块化舞台的位姿为：P_k+1(219，-353,0.8345)。重复动态定位过程，实现了模块化舞台的实时定位，从而为模块化舞台的自动化提供定位精度上的保证。

以上阐述的是本发明给出的一个实施例所表现出优良性能的模块化移动舞台激光精确定位效果。需要指出，上述实施例用来解释说明本发明，而不是对本发明进行限制，在本发明的精神和权利要求的保护范围内，对本发明做出的任何修改，都落入本发明的保护范围。

Claims (2)

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

Images