一种基于光电扫描的室内移动机器人位姿测量系统及测量方法，在移动机器人上设置有激光发射站，激光发射站的周边设置有至少3个用于接收激光发射站所发出的光信号的接收器，还设置有至少1个与接收器相连用于对接收器所接收的信号进行处理确定接收器在测量空间内激光发射站坐标系下的精确坐标的信号处理器，以及与信号处理器无线连接通过确定激光发射站到各个接收器之间的距离来确定移动机器人的姿态角度和移动机器人的位置的终端计算机。本发明操作者在测量时无需在空间中布设多个发射基站，亦无需经过繁琐的全局定向，依靠由光电接收器构成的多个引导信标及固定在移动机器人机身上的高速激光扫描转台，实现对室内移动机器人进行三维空间位置及姿态的实时测量。
1.一种基于光电扫描的室内移动机器人位姿测量系统的测量方法，基于光电扫描的室内移动机器人位姿测量系统，包括有移动机器人(1)，其特征在于，所述的移动机器人(1)上设置有激光发射站(2)，所述的激光发射站(2)的周边设置有至少3个用于接收激光发射站(2)所发出的光信号的接收器(3)，还设置有至少1个与所述的接收器(3)相连用于对所述的接收器(3)所接收的信号进行处理确定接收器(3)在测量空间内激光发射站坐标系下的精确坐标的信号处理器(4)，以及与所述的信号处理器(4)无线连接通过确定激光发射站到各个接收器之间的距离来确定移动机器人的姿态角度和移动机器人的位置的终端计算机(5)；其特征在于，方法包括如下步骤：
1)建立全局导航坐标系，并用激光跟踪仪或室内GPS精确测量出每个接收器在全局导航坐标系下的精确三维坐标；
2)每个接收器通过接收激光发射站红外激光器发出的扫描激光信号与同步脉冲激光器发出的同步脉冲激光信号，并发送给相连接的信号处理器，信号处理器分别计算相连接的每个接收器在测量空间内激光发射站坐标系下的精确坐标，包括两个扫描角θi1和θi2及接收器相对于激光发射站坐标系下的水平角αi和垂直角βi，并将计算结果存入终端计算机；
3)终端计算机根据不同接收器的精确坐标，分别计算出激光发射站到各个接收器之间的距离；
4)在求定激光发射站到每个接收器的距离真值di后，每个接收器在激光发射站坐标系下的近似坐标值表示为pi＝di(cosβicosαi,cosβisinαi,sinβi)，以近似坐标值作为迭代初始值，进而优化解算得出每个接收器在激光发射站坐标系下的精确坐标值；
5)通过步骤4)求得的接收器在激光发射站坐标系下的精确坐标和步骤1)建立的每个接收器在全局导航坐标系下的三维坐标，经过坐标系转换，得到激光发射站坐标系相对于全局导航坐标系的姿态矩阵及平移矩阵；
6)通过姿态矩阵获得移动机器人的姿态角度，通过平移矩阵获得移动机器人的位置。
2.根据权利要求1所述的基于光电扫描的室内移动机器人位姿测量系统的测量方法，其特征在于，步骤2)所述的两个扫描角θi1和θi2及水平角αi和垂直角βi是采用如下公式获得：θi1＝ω·ti1
其中，ω代表激光发射站转子的旋转角速度；ti1和ti2分别表示激光发射站的第1束和第
2束光平面分别扫过第i个接收器所用的时间；ni1和ni2分别表示激光发射站的第一束和第二束光平面分别扫过第i个接收器时光平面的法矢量，它们的叉乘运算结果li由(rix riy riz)表示。
3.根据权利要求1所述的基于光电扫描的室内移动机器人位姿测量系统的测量方法，其特征在于，步骤3)所述的计算激光发射站到各个接收器之间的距离是采用如下公式获得：通过任意两个接收器可以列出如下方程：若多于三个接收器收到发射站的激光信号，即可列出三个形如上式的方程，未知数和方程个数相同，进而可以求解出发射站分别到三个接收器的距离di,i＝1,2,3。其中，θij表示激光发射站与两个接收器i和j连线之间的夹角，可通过如下公式计算：
4.根据权利要求1所述的基于光电扫描的室内移动机器人位姿测量系统的测量方法，其特征在于，步骤5)中所述的姿态矩阵和平移矩阵可通过如下公式获得：其中， 代表姿态矩阵， 代表平移矩阵，假设测量空间中共有i个接收器收到了激光发射站的光信号，G与S分别为接收器在导航坐标系与激光发射站坐标系下的三维坐标点矩阵，表示为：求解如下最小二乘问题，即可的到 和 的最终解：
5.根据权利要求1所述的基于光电扫描的室内移动机器人位姿测量系统的测量方法，其特征在于，步骤6)所述的移动机器人的姿态角度是根据步骤5)所求出的姿态矩阵得出：其中，ψ表示航向角度，θ表示俯仰角度，γ表示横滚角度。
6.根据权利要求5所述的基于光电扫描的室内移动机器人位姿测量系统的测量方法，其特征在于，步骤6)所述的移动机器人的位置是直接通过平移矩阵获得，将平移矩阵 写成：则平移矩阵的三个分量即为移动机器人在导航坐标系下的三维坐标。
基于光电扫描的室内移动机器人位姿测量系统及测量方法
技术领域
[0001] 本发明涉及一种室内移动机器人位姿测量方法。特别是涉及一种基于光电扫描的室内移动机器人位姿测量系统及测量方法。
背景技术
[0002] 室内移动机器人(AGV、移动加工平台等)现已被广泛应用于工业现场大部件装配领域。在工作过程中，人们需要对其位置及姿态进行实时测量。目前，移动机器人的三维坐标及姿态测量方法主要分为两类：航迹推测法和绝对定位法。航迹推测主要依靠里程计、陀螺仪等内部本体感受传感器，通过给定初始状态，测量机器人相对于初始位姿的距离和方向来确定机器人的位姿；绝对定位主要采用引导信标、主动或被动标识、地图匹配或全球定位系统进行位姿测量。相比二者，航迹推测法具有能够不依赖外部设备自主提供测量数据的优势，使机器人本体与测量系统构成统一整体，从而连续稳定地输出机器人自身的位置及姿态信息，对本系统外的环境不造成任何的干扰。但由于航迹推测算法多基于积分运算，其测量误差会随时间的积累被逐步放大，并存在数据漂移现象；绝对定位法通常会在空间中布设一个或多个发射基站和接收装置，并以接收装置作为探测目标。通过发射基站向测量空间发送不同形式的信号(光，声，电磁等)，在接收到这些信号后，接收装置基于不同的物理原理将它们进行处理整合并转换为自身的位姿信息，或直接反射回发射基站由发射端处理解算。绝对定位法的测量精度与时间不相关联，没有误差累积及数据漂移现象，因此通常具有较高的精度及稳定性。但是鉴于移动机器人工作区域大，执行任务复杂，为了确保其高效流畅运行，需要在工作空间布设多个发射基站以保证测量范围，对现场环境要求较高。另外，在构建测量场的过程中，坐标系的建立及不同发射基站之间的全局定向步骤相对繁琐。
发明内容
[0003] 本发明所要解决的技术问题是，提供一种用于在无法进行多基站交会测量的工业环境中的移动机器人的基于光电扫描的室内移动机器人位姿测量系统及测量方法。
[0004] 本发明所采用的技术方案是：一种基于光电扫描的室内移动机器人位姿测量系统，包括有移动机器人，所述的移动机器人上设置有激光发射站，所述的激光发射站的周边设置有至少3个用于接收激光发射站所发出的光信号的接收器，还设置有至少1个与所述的接收器相连用于对所述的接收器所接收的信号进行处理确定接收器在测量空间内激光发射站坐标系下的精确坐标的信号处理器，以及与所述的信号处理器无线连接通过确定激光发射站到各个接收器之间的距离来确定移动机器人的姿态角度和移动机器人的位置的终端计算机。
[0005] 每一个信号处理器连接有3～8个接收器。
[0006] 一种基于光电扫描的室内移动机器人位姿测量系统的测量方法，包括如下步骤：
[0007] 1)建立全局导航坐标系，并用激光跟踪仪或室内GPS精确测量出每个接收器在全局导航坐标系下的精确三维坐标；
[0008] 2)每个接收器通过接收激光发射站红外激光器发出的扫描激光信号与同步脉冲激光器发出的同步脉冲激光信号，并发送给相连接的信号处理器，信号处理器分别计算相连接的每个接收器在测量空间内激光发射站坐标系下的精确坐标，包括两个扫描角θi1和θi2及接收器相对于激光发射站坐标系下的水平角αi和垂直角βi，并将计算结果存入终端计算机；
[0009] 3)终端计算机根据不同接收器的精确坐标，分别计算出激光发射站到各个接收器之间的距离；
[0010] 4)在求定激光发射站到每个接收器的距离真值di后，每个接收器在激光发射站坐标系下的近似坐标值表示为pi＝di(cosβi cosαi,cosβi sinαi,sinβi)，以近似坐标值作为迭代初始值，进而优化解算得出每个接收器在激光发射站坐标系下的精确坐标值；
[0011] 5)通过步骤4)求得的接收器在激光发射站坐标系下的精确坐标和步骤1)建立的每个接收器在全局导航坐标系下的三维坐标，经过坐标系转换，得到激光发射站坐标系相对于全局导航坐标系的姿态矩阵及平移矩阵；
[0012] 6)通过姿态矩阵获得移动机器人的姿态角度，通过平移矩阵获得移动机器人的位置。
[0013] 步骤2)所述的两个扫描角θi1和θi2及水平角αi和垂直角βi是采用如下公式获得：
[0014] θi1＝ω·ti1
[0015] θi2＝ω·ti2
[0016] li＝ni1×ni2＝(rix riy riz)
[0017]
[0018]
[0019] 其中，ω代表激光发射站转子的旋转角速度；ti1和ti2分别表示激光发射站的第1束和第2束光平面分别扫过第i个接收器所用的时间；ni1和ni2分别表示激光发射站的第一束和第二束光平面分别扫过第i个接收器时光平面的法矢量，它们的叉乘运算结果li由(rix riy riz)表示。
[0020] 步骤3)所述的计算激光发射站到各个接收器之间的距离是采用如下公式获得：通过任意两个接收器可以列出如下方程：
[0021]
[0022] 若多于三个接收器收到发射站的激光信号，即可列出三个形如上式的方程，未知数和方程个数相同，进而可以求解出发射站分别到三个接收器的距离di,i＝1,2,3。其中，θij表示激光发射站与两个接收器(i和j)连线之间的夹角，可通过如下公式计算：
[0023]
[0024] 步骤5)中所述的姿态矩阵和平移矩阵可通过如下公式获得：
[0025]
[0026] 其中， 代表姿态矩阵， 代表平移矩阵，假设测量空间中共有i个接收器收到了激光发射站的光信号，G与S分别为接收器在导航坐标系与激光发射站坐标系下的三维坐标点矩阵，表示为：
[0027]
[0028]
[0029] 求解如下最小二乘问题，即可的到 和 的最终解：
[0030]
[0031] 步骤6)所述的移动机器人的姿态角度是根据步骤5)所求出的姿态矩阵得出：
[0032]
[0033]
[0034]
[0035] 其中，ψ表示航向角度，θ表示俯仰角度，γ表示横滚角度。
[0036] 步骤6)所述的移动机器人的位置是直接通过平移矩阵获得，将平移矩阵 写成：
[0037]
[0038] 则平移矩阵的三个分量即为移动机器人在导航坐标系下的三维坐标。
[0039] 本发明的基于光电扫描的室内移动机器人位姿测量系统及测量方法，是一种高精度、高效率且便捷的位姿测量系统和方法。操作者在测量时无需在空间中布设多个发射基站，亦无需经过繁琐的全局定向，依靠由光电接收器构成的多个引导信标及固定在移动机器人机身上的高速激光扫描转台，实现对室内移动机器人进行三维空间位置及姿态的实时测量。本发明具有如下特点：
[0040] 1、系统结构简单：本发明所提供的系统是基于单一移动测量基站辅以引导信标的工作原理，比起其它需要多个测站的位姿测量系统，节约了成本，简化了工作步骤；
[0041] 2、高精度：本系统的坐标测量误差小于3mm，姿态角度测量误差小于0.1°。精度远高于激光雷达等其它基于引导信标的测量系统；
[0042] 3、受现场环境影响小：由于作为引导信标的接收器的体积很小，其可以根据测量任务被放置在任何通视条件较好的位置。另外，固定在机器人上并跟随其移动的发射站采用全空间旋转扫描的工作方式，可以探测360°视场范围，进一步放宽了接收器布设位置的要求。
附图说明
[0043] 图1是本发明的基于光电扫描的室内移动机器人位姿测量系统的结构示意图；
[0044] 图2是基于光电扫描的室内移动机器人位姿测量系统中激光发射站的结构示意图；
[0045] 图3是基于光电扫描的室内移动机器人位姿测量系统中接收器的结构示意图；
[0046] 图4是全局导航坐标系；
[0047] 图5是激光发射站坐标系；
[0048] 图6是待求解坐标系。
[0049] 图中
[0050] 1：移动机器人 2：激光发射站
[0051] 3：接收器 4：信号处理器
[0052] 5；终端计算机 21：光平面
[0053] 22：旋转轴 23：转子
[0054] 24：同步脉冲激光器 25：基座
[0055] 26：红外激光器 27：接收器外壳
[0056] 28：接收器光敏中心
具体实施方式
[0057] 下面结合实施例和附图对本发明的基于光电扫描的室内移动机器人位姿测量系统及测量方法做出详细说明。
[0058] 使用一台小型高速旋转平台(发射站)辅以一定数量的引导信标，以已知每个引导信标在全局坐标系下精确的三维坐标为前提，通过测量发射站相对于每个引导信标的空间扫描角度，结合发射站的精确内部参数完成自身的位姿测量。
[0059] 本发明，将一个可以向全空间发射激光信号的小型高速旋转平台(发射站)固定在移动机器人机体上，在跟随机器人移动的同时，其以扫描的方式持续将激光光信号发射到工作空间。与此同时，多个引导信标(接收器)被分散放置在发射站能够探测到的工作区域。当发射站发出的激光信号扫过引导信标时，信标内部的光电传感器，将激光信号转变成电信号，经后续处理转变成扫描角度信息。以扫描角度作为基本观测量，并根据发射站的内部参数及引导信标在全局坐标系下的精确坐标(导航任务开始前事先标定好)，最终优化求解发射站在全局坐标系下的三维坐标及姿态角度。
[0060] 如图1、图2所示，本发明的基于光电扫描的室内移动机器人位姿测量系统，包括有移动机器人1，所述的移动机器人1上设置有激光发射站2，所述的激光发射站2的周边设置有至少3个用于接收激光发射站2所发出的光信号的接收器3，还设置有至少1个与所述的接收器3相连用于对所述的接收器3所接收的信号进行处理确定接收器3在测量空间内激光发射站坐标系下的精确坐标的信号处理器4，以及与所述的信号处理器4无线连接通过确定激光发射站到各个接收器之间的距离来确定移动机器人的姿态角度和移动机器人的位置的终端计算机5。每一个信号处理器4可以连接有3～8个接收器3。
[0061] 本发明所述的激光发射站2和接收器3是采用申请号为201210126759.5中所公开的结构。其中：
[0062] 如图2所示，激光发射站主要由基座25和转子23两部分组成：转子23位于激光发射站顶部，可以高速旋转，转子上安装有两个线性红外激光器26，这样即可形成两束对测量空间高速扫描的扇形红外激光平面；基站上还安装有一圈同步脉冲激光器24，该激光器可在转子转过某一特定位置时发出脉冲激光，形成一个计时基准。这样，接收器通过接收同步脉冲激光与扇形扫描激光即可测出扫描激光扫过接收器时转子转过的角度θi1和θi2，并以此作为基本观测量用于后续计算。
[0063] 如图2所示，接收器选用快速光敏器件(如PIN光电二极管或雪崩光电二极管)作为传感器将基站发出的光信号转化为光电流信号。预处理电路将原始光电流信号进行放大并通过阈值判断将其二值化为逻辑脉冲。然后由光信号转化成的逻辑脉冲被输入计时电路中进行计时。接收器外壳27的尺寸与直径为1.5英寸的激光跟踪仪反射靶球(SMR)完全相同，且接收器光敏中心28与SMR角锥棱镜中心位置偏差小于0.1mm，可与激光跟踪仪或其他全局坐标测量系统进行精度比对。为增加系统的抗干扰能力，光敏单元的敏感波长与发射站射出的激光波长相同，且接收器末端末端的输出信号采用差分信号。
[0064] 本发明在双平面共轴旋转扫描测角方案的基础上提出一种基于光电扫描的室内移动机器人位姿测量系统的测量方法。该测量系统主要包括固定在移动机器人车身上的光电扫描发射站及位于已知点处的光信号接收器组成，同时每台接收器还配有负责坐标计算的计算机。系统采取发射站-接收器单向广播的工作方式进行测量，由使用双平面共轴旋转扫描原理的激光发射站向全空间广播带有角度信息的扫描光信号。最终系统整体的机器人位姿测量精度可达到2mm。
[0065] 为实现对移动机器人的高精度的位姿测量，需要确定引导信标的精确三维空间坐标。本发明采用激光跟踪仪或其他精密全局对引导信标的三维坐标进行精确测量，其三维坐标测量精度优于0.03mm，完全满足移动机器人毫米级的位置测量要求，及角分级的姿态测量要求。
[0066] 本发明的基于光电扫描的室内移动机器人位姿测量系统的测量方法，包括如下步骤：
[0067] 1)建立全局导航坐标系，并用激光跟踪仪或室内GPS精确测量出每个接收器在全局导航坐标系下的精确三维坐标；
[0068] 2)每个接收器通过接收激光发射站红外激光器发出的扫描激光信号与同步脉冲激光器发出的同步脉冲激光信号，并发送给相连接的信号处理器，信号处理器分别计算相连接的每个接收器在测量空间内激光发射站坐标系下的精确坐标，包括两个扫描角θi1和θi2及接收器相对于激光发射站坐标系下的水平角αi和垂直角βi，并将计算结果存入终端计算机；
[0069] 所述的两个扫描角θi1和θi2及水平角αi和垂直角βi是采用如下公式获得：
[0070] θi1＝ω·ti1
[0071] θi2＝ω·ti2
[0072] li＝ni1×ni2＝(rix riy riz)
[0073]
[0074]
[0075] 其中，ω代表激光发射站转子的旋转角速度；ti1和ti2分别表示激光发射站的第1束和第2束光平面分别扫过第i个接收器所用的时间；ni1和ni2分别表示激光发射站的第一束和第二束光平面分别扫过第i个接收器时光平面的法矢量，它们的叉乘运算结果li由(rix riy riz)表示。
[0076] 3)终端计算机根据不同接收器的精确坐标，分别计算出激光发射站到各个接收器之间的距离；
[0077] 所述的计算激光发射站到各个接收器之间的距离是采用如下公式获得：通过任意两个接收器可以列出如下方程：
[0078]
[0079] 若多于三个接收器收到发射站的激光信号，即可列出三个形如上式的方程，未知数和方程个数相同，进而可以求解出发射站分别到三个接收器的距离di,i＝1,2,3。其中，θij表示激光发射站与两个接收器(i和j)连线之间的夹角，可通过如下公式计算：
[0080]
[0081] 4)在求定激光发射站到每个接收器的距离真值di后，每个接收器在激光发射站坐标系下的近似坐标值表示为pi＝di(cosβi cosαi,cosβi sinαi,sinβi)，以近似坐标值作为迭代初始值，进而通过LM算法优化解算得出每个接收器在激光发射站坐标系下的精确坐标值；
[0082] 5)通过步骤4)求得的接收器在激光发射站坐标系下的精确坐标和步骤1)建立的每个接收器在全局导航坐标系下的三维坐标，经过坐标系转换，得到激光发射站坐标系相对于全局导航坐标系的姿态矩阵及平移矩阵；
[0083] 所述的姿态矩阵和平移矩阵可通过如下公式获得：
[0084]
[0085] 其中， 代表姿态矩阵， 代表平移矩阵，假设测量空间中共有i个接收器收到了激光发射站的光信号，G与S分别为接收器在导航坐标系与激光发射站坐标系下的三维坐标点矩阵，表示为：
[0086]
[0087]
[0088] 求解如下最小二乘问题，即可的到 和 的最终解：
[0089]
[0090] 6)通过姿态矩阵获得移动机器人的姿态角度，通过平移矩阵获得移动机器人的位置。
[0091] 所述的移动机器人的姿态角度是根据步骤5)所求出的姿态矩阵得出：
[0092]
[0093]
[0094]
[0095] 其中，ψ表示航向角度，θ表示俯仰角度，γ表示横滚角度。
[0096] 所述的移动机器人的位置是直接通过平移矩阵获得，将平移矩阵 写成：
[0097]
[0098] 则平移矩阵的三个分量即为移动机器人在导航坐标系下的三维坐标。
