CN108489382B - 一种基于空间多点约束的agv动态位姿测量方法 - Google Patents

Abstract

本发明公开了一种基于空间多点约束的AGV动态位姿测量方法，包括：(1)发射站和空间约束点的布置，发射站则固定在AGV车上，接收器的三维坐标信息已知；(2)获得各空间约束点在发射站坐标系和全局导航坐标系下的坐标；(3)结合空间约束点在全局导航坐标系的坐标和各空间约束点在发射站坐标系的坐标解算出发射站坐标系和全局导航坐标系之间的转换关系，解算出发射站在全局导航坐标系下的动态位姿，进而得到AGV的动态位姿。本发明主要是采用“后方交会”的原理对发射站的位姿进行测量，有效节约了成本，且不易丢失数据，保证了测量的连续性，省去了繁琐的各发射站之间的标定过程。

Description

一种基于空间多点约束的AGV动态位姿测量方法

技术领域

本发明涉及空间wMPS位姿测量领域，尤其涉及空间发射站动态位姿测量问题。

背景技术

随着工业自动化和柔性制造技术的发展，在仓库等地方对具有高精度和自动化的室内AGV需求越来越大，而对AGV的导航和动态位姿测量的需求更是不断增加。例如在汽车和航空航天领域的自动搬运和装卸过程中，需要通过实时获取AGV的位置和姿态信息，来精确完成装配或搬运过程。目前主流室内AGV定位方式主要分为两大类：航迹推测法(Deadreckoning)和绝对定位法(Absolute positioning)。航迹推测法是依靠内部传感器，给定初始状态来确定当前位姿，主要基于积分运算，具有能够不依赖外部设备及自主提供测量数据的优势，但其测量误差会随时间放大，存在数据漂移现象。而绝对定位法是在空间中布设发射基站和接受装置，再对信息进行结算从而得到AGV相对于导航坐标系的绝对定位，它的测量误差与时间无关，具有较高精度和稳定性，是目前用的最多的定位方法。而在目前众多室内导航定位方法之中，基于wMPS的定位与位姿测量方法具有测量精度很高、探测空间较大和数据更新较快的特点。wMPS系统由发射站、接收器、信号处理器及计算器这几部分组成，在空间中布设由多个发射站或接收器构成的测量场，基于约束点的发射站测量场是由已知精确三维坐标的各约束点解算出各个发射站之间的外参数得到的。

现在比较常见的位姿测量方法是将发射站固定在空间特定位置，然后将接收器固定在AGV的合适位置，随AGV移动并持续接收发射站发出的激光信号，并通过算法完成AGV的位姿测量。这种方法有灵活易实现的优点，但是当导航范围扩大时，就需要增加发射站的数量来完成测量要求，这样就会使测量成本急剧增加。而且考虑到现场环境的复杂程度，发射站固定在特定位置，其发出的激光信号容易被遮挡，导致测量数据的不连续。而为了节约定位和位姿测量成本，提高定位及位姿测量精度，在实际场合中可以减少发射站的数量，采用空间多点约束的单站测量模式来确定发射站在全局坐标系下的动态位置和姿态信息，进而得到AGV的实时位姿信息。

发明内容

本发明的目的是为了克服发射站固定模式中存在的问题，当发射站固定时，若要满足更大的AGV运动范围，则需要增加发射站的数量，成本较高，而且发射站固定时，它与AGV车体上的接收器之间不能有遮挡，否则会造成数据缺失，这样该模式的适应力和抗干扰能力不强。本发明将单发射站固定在AGV车体上，在四周的合适位置处布置多个接收器作为空间约束点，来进行AGV的动态位姿测量，当AGV的运动区域需要扩大时只需要增加约束点的数量即可，可以很大程度节约成本和增强系统的抗干扰能力。本发明提供一种基于空间多点约束的AGV动态位姿测量方法，采用“后方交会”的原理对发射站的位姿进行测量，有效节约了成本，且不易丢失数据，保证了测量的连续性，省去了繁琐的各发射站之间的标定过程。本发明接收器被固定在空间中的合适位置，发射站则固定在AGV车上，接收器的位姿信息已知，在AGV运动时，发射站不断发出激光信号，结合接收器的位置信息和发射站的内部参数，即可解算出发射站在全局坐标系下的动态位姿，进而得到AGV车的动态位姿。

为了解决上述技术问题，本发明提出的一种基于空间多点约束的AGV动态位姿测量方法，包括以下步骤：

步骤一、发射站和空间约束点的布置：测量系统由一个发射站和4个以上的空间约束点组成，所述多个空间约束点按照包络AGV运行轨迹且不在一个平面内布局，所述空间约束点均采用球形接收器并通过球座固定，

步骤二、获得各空间约束点在发射站坐标系的坐标：所述发射站和多个空间约束点之间具有两种约束，分别为光平面约束和距离约束；所述光平面约束指的是接收器接收到的发射站发出的激光扫描角度信息和发射站内部参数共同构建的平面方程，其数学表达式为：

式(1)中，；(A_ij B_ij C_ij D_ij)表示发射站激光平面的内参数；R_θij表示的是发射站转子绕转轴旋转的姿态变换矩阵；θ_ij指的是发射站扫描角度，θ_ij由接收器收到的激光信号间隔以及发射站转子的转速得到；R_i和T_i为发射站外参数，由全局定向得到；(X_i Y_i Z_i)^T为空间约束点在全局导航坐标系下的坐标，该(X_i Y_i Z_i)^T由激光跟踪仪测得；所述距离约束的数学表达式为：

式(7)中，多个空间约束点在发射站坐标系下的坐标R_n(X_n，Y_n，Z_n)由非线性最优化求得：

从而，求得各空间约束点在发射站坐标系的坐标；

步骤三、结合空间约束点在全局导航坐标系的坐标(X_i Y_i Z_i)^T和步骤二获得的各空间约束点在发射站坐标系的坐标解算出发射站坐标系和全局导航坐标系之间的转换关系：

设第i个约束点在发射站坐标系和全局导航坐标系下的坐标分别是P_i和Q_i，那么两个坐标系之间的坐标转换关系为：

式(8)中，R为旋转矩阵，T为平移矩阵；

R为正交矩阵，式(9)中各元素与发射站坐标系相对于全局导航坐标系的转动角度α，β，γ的关系如下：

T＝(x y z)^T，表示的是发射站坐标系原点到全局导航坐标系原点的平移量，

(α,β,γ,x,y,z)为发射站坐标系相对于全局导航坐标系之间的位姿。

进一步讲，本发明基于空间多点约束的AGV动态位姿测量方法，其中，步骤二中，由激光跟踪仪测得空间约束点在全局导航坐标系下的坐标(X_i Y_i Z_i)^T的具体内容如下：

使用激光跟踪仪在空间中设站测量各空间约束点的三维坐标，将所述激光跟踪仪依次移动到新的站位，再次测量各空间约束点的三维坐标，激光跟踪仪的站位摆放满足每个站位能观测到3个以上的空间约束点，且空间约束点的数目和激光跟踪仪的站位数满足测长误差方程的建立条件：mn>3(m+n)，其中，m是激光跟踪仪的测量站位数量，n是空间约束点的数量；

将激光跟踪仪某个站位下的测站坐标系定义为全局导航坐标系，其他站位下的测站坐标系相对于全局导航坐标系的关系为：

P_i＝RQ_i+T (2)

式(2)中，P_i为全局导航坐标系下空间约束点的三维坐标，Q_i为激光跟踪仪在其他站位下的测站坐标系下空间约束点的三维坐标测量值；

结合式(2)计算得到激光跟踪仪所有站位间的定位关系；

然后，利用跟踪仪高精度测距对全局约束点的三维坐标值进行改正优化，过程如下：

假设空间中存在n个全局约束点，三维坐标为(x_i,y_i,z_i),i＝1,2,...,n；激光跟踪仪在空间中通过m个站位对这n个约束点进行测量每个站位三维坐标为(X_k,Y_k,Z_k),k＝1,2,...,m；

在测量每个站位三维坐标每个站位三维坐标过程中，对于每个站位和全局约束点，它们之间的距离r_ik是激光跟踪仪高精度的干涉测距值，假设为真值，r_ik的实际测量值l_ik表示如下：

对式(6)进行线性化：

式(4)中，△x_i和△X_k就是全局约束点和跟踪仪站位坐标的优化改正值，初值

和

由之前激光跟踪仪站位之间的初步定向来确定，利用式(4)建立测长误差方程：

将式(5)转化为线性方程组的形式，如下：

V＝A△X-b (6)

式(6)中，矩阵A是由(3)泰勒展开的一阶求导项所组成的大型稀疏矩阵，

△X＝(△X₁,△Y₁,△Z₁,△X₂,△Y₂,△Z₂,...,△X_m,△Y_m,△Z_m,△x₁,△y₁,△z₁,△x₂,△y₂,△z₂,...,△x_n,△y_n,△z_n)^T

由于矩阵A是病态矩阵，条件数极大，因此，对矩阵A进行奇异值分解或者QR分解来求解方程组，结合相应的迭代条件得到最终的三维坐标优化改正值。

与现有技术相比，本发明的有益效果是：

本发明通过将发射站固定在AGV车体上，采用空间约束点的位置坐标信息和发射站内参以及接收器接收到的扫描光信号来解算出发射站在空间坐标系下的平移和旋转矩阵，即发射站的动态位姿。约束点的数量可随需求增加，极大降低了成本，而且不易丢失数据，位姿信息的获取更加完备，在实际应用场合中的抗干扰能力更强。

附图说明

图1为本发明测量方法的整体示意图；

图2为发射站的示意图；

图3为空间约束点的结构示意图；

图4为测量原理示意图；

图5为激光跟踪仪设站测空间约束点的三维坐标示意图。

附图标记：1-空间约束点，2-自动导引小车(AGV)，3-发射站，4-激光面一，5-激光面二，6-球形接收器，7-磁性球座，8-光电接收组件，P1～P7为空间约束点。

具体实施方式

下面结合附图和具体实施例对本发明技术方案作进一步详细描述，所描述的具体实施例仅对本发明进行解释说明，并不用以限制本发明。

本发明一种基于空间多点约束的AGV动态位姿测量方法，用于测量发射站在自动导引小车(AGV)上的动态位姿，由于发射站固定在AGV的特定位置，其相对于AGV的位姿固定且已知，可由测得的发射站的动态位姿解算得到AGV的动态位姿。本发明的特征在于所述发射站的数量为单个，发射站固定在AGV上随车体运动，所述发射站由旋转平台和固定底座组成，会发出两束扫描光和一束同步光，发射站相对于车体的位姿是固定的。在空间中的适合位置布置多个约束点，这些约束点的位置三维坐标已经测得，约束点能接收发射站发出的扫描光并将其转换成电信号传递给信号处理器，通过分类及识别得到约束点中心相对于发射站的扫描角度，将其传递给终端计算机，结合发射站的内部参数和已知的约束点位置坐标解算出发射站的动态位姿，进而解算出AGV的动态位姿。

空间约束点为球形接收器的中心，由于约束点的数量较多，使用激光跟踪仪单次测量所有约束点的三维坐标误差较大，可用单台激光跟踪仪通过多个站位获取空间约束点的冗余距离约束对各约束点的三维坐标进行优化，故球形接收器的几何尺寸要与激光跟踪仪兼容。空间约束点的建立是通过以下方法实现的：将激光跟踪仪首先在空间中设站测量空间约束点的三维坐标，然后将跟踪仪依次移动到新的站位，再次测量空间约束点的坐标，待所有测量任务完成之后，即可通过相应的算法对各个站位跟踪仪测得的全局约束点信息进行解算，得到激光跟踪仪在所有站位的方位定向。在测量过程中要保证每个站位能观测到3个以上的约束点，且约束点的数目和跟踪仪的站位数需要满足测长误差方程的建立条件：mn>3(m+n)，其中m是激光跟踪仪的测量站位数量，n是空间约束点的数量。由于激光跟踪仪的测距精度远高于测角精度，可以利用跟踪仪的每个站点和全局约束点之间的距离建立一个基于冗余长度约束方程，来对跟踪仪的测角误差进行优化，最后对测得的空间约束点的三维坐标进行优化修正。

所述的多个空间约束点分布在发射站的四周，即球形接收器通过球座固定在四周的墙壁，多个约束点的分布包络了AGV车体的运动路径，且不在一个平面内。空间约束点的数量需满足测长误差方程(5)的建立条件：mn>3(m+n)，其中m是激光跟踪仪的测量站位数量，n是空间约束点的数量。激光跟踪仪的测量站位和空间约束点构成一个空间三维控制网。

本发明提出的一种基于空间多点约束的AGV动态位姿测量方法，包括以下步骤：

步骤一、发射站和空间约束点的布置：

如图1所示，测量系统由一个发射站3和4个以上的空间约束点1组成，发射站3固定在自动导引小车(AGV)2上且随车体运动，所述多个空间约束点1按照包络AGV2运行轨迹且不在一个平面内布局，所述空间约束1点均采用球形接收器6并通过磁性球座7固定；

步骤二、获得各空间约束点在发射站坐标系和全局导航坐标系下的坐标：

如图2所示，所述发射站3由旋转平台和固定底座组成，会发出两束扫描光和一束同步光，扫描光的光平面为4和5。

如图3所示，球形接收器6的中心装有光电接收组件8，可以接收发射站3发出的两束扫描光4和5，并将光信号转换为脉冲信号，经由数据线传递给后续信号处理器，信号处理器对脉冲信号进行分类和处理之后得到光电接收组件8的中心与发射站3之间的约束。

所述发射站3和多个空间约束点1之间具有两种约束，分别为光平面约束和距离约束；

所述光平面约束指的是球形接收器6接收到的发射站3发出的激光扫描角度信息和发射站3内部参数共同构建的平面方程，其数学表达式为：

式(1)中，；(A_ij B_ij C_ij D_ij)表示发射站3激光平面的内参数；表示的是发射站3转子绕转轴旋转的姿态变换矩阵；θ_ij指的是发射站3扫描角度，θ_ij由接收器6收到的激光信号间隔以及发射站3转子的转速得到；R_i和T_i为发射站3外参数，由全局定向得到；(X_i Y_iZ_i)^T为空间约束点1在全局导航坐标系下的坐标，该(X_i Y_i Z_i)^T由激光跟踪仪测得，具体过程如下：

如图5所示，使用激光跟踪仪在空间中设站测量各空间约束点P1～P7的三维坐标，将所述激光跟踪仪依次移动到新的站位，再次测量各空间约束点P1～P7的三维坐标，激光跟踪仪的站位摆放满足每个站位能观测到3个以上的空间约束点，且空间约束点的数目和激光跟踪仪的站位数满足测长误差方程的建立条件：mn>3(m+n)，其中，m是激光跟踪仪的测量站位数量，n是空间约束点的数量；

将激光跟踪仪某个站位下的测站坐标系定义为全局导航坐标系，其他站位下的测站坐标系相对于全局导航坐标系的关系为：

P_i＝RQ_i+T (2)

式(2)中，P_i为全局导航坐标系下空间约束点1的三维坐标，Q_i为激光跟踪仪在其他站位下的测站坐标系下空间约束点1的三维坐标测量值；

结合配准算法计算得到激光跟踪仪所有站位间的定位关系，采用的配准算法属于本领域公知常识，在此不再赘述。

然后，利用跟踪仪高精度测距对全局约束点1的三维坐标值进行改正优化，过程如下：

假设空间中存在n个全局约束点，三维坐标为(x_i,y_i,z_i),i＝1,2,...,n；激光跟踪仪在空间中通过m个站位对这n个约束点进行测量每个站位三维坐标为(X_k,Y_k,Z_k),k＝1,2,...,m；

在每个站位测量三维坐标过程中，对于每个站位和全局约束点1，它们之间的距离r_ik是激光跟踪仪高精度的干涉测距值，假设为真值，r_ik的实际测量值l_ik表示如下：

对式(3)进行线性化：

式(4)中，△x_i和△X_k就是全局约束点1和跟踪仪站位坐标的优化改正值，初值

和

由之前激光跟踪仪站位之间的初步定向来确定，利用式(4)建立误差方程：

将式(5)转化为线性方程组的形式，如下：

V＝A△X-b (6)

式(6)中，矩阵A是由式(3)泰勒展开的一阶求导项所组成的大型稀疏矩阵，

△X＝(△X₁,△Y₁,△Z₁,△X₂,△Y₂,△Z₂,...,△X_m,△Y_m,△Z_m,△x₁,△y₁,△z₁,△x₂,△y₂,△z₂,...,△x_n,△y_n,△z_n)^T

由于矩阵A是病态矩阵，条件数极大，因此，对矩阵A进行奇异值分解或者QR分解来求解方程组，结合相应的迭代条件得到最终的三维坐标优化改正值。

如图4所示，所述距离约束的数学表达式为：

式(7)中，多个空间约束点1在发射站坐标系下的坐标R_n(X_n，Y_n，Z_n)由非线性最优化求得：

从而，求得各空间约束点1在发射站坐标系的坐标；

步骤三、结合空间约束点在全局导航坐标系的坐标(X_i Y_i Z_i)^T和步骤二获得的各空间约束点在发射站坐标系的坐标解算出发射站坐标系和全局导航坐标系之间的转换关系，

设第i个约束点在发射站坐标系和全局导航坐标系下的坐标分别是P_i和Q_i，那么两个坐标系之间的坐标转换关系为：

式(8)中，R为旋转矩阵，T为平移矩阵；

R为正交矩阵，式(9)中各元素与发射站坐标系相对于全局导航坐标系的转动角度α，β，γ的关系如下：

T＝(x y z)^T，表示的是发射站坐标系原点到全局导航坐标系原点的平移量；(α,β,γ,x,y,z)为发射站坐标系相对于全局导航坐标系之间的位姿。

本发明实施例对各器件的型号除做特殊说明的以外，其他器件的型号不做限制，只要能完成上述功能的器件均可。

尽管上面结合附图对本发明进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨的情况下，还可以做出很多变形，这些均属于本发明的保护之内。

Claims (2)

1.一种基于空间多点约束的AGV动态位姿测量方法，其特征在于，包括以下步骤：

步骤一、发射站和空间约束点的布置：

测量系统由一个发射站和4个以上的空间约束点组成，所述多个空间约束点按照包络AGV运行轨迹且不在一个平面内布局，所述空间约束点均采用球形接收器并通过球座固定，

步骤二、获得各空间约束点在发射站坐标系的坐标：

所述发射站和多个空间约束点之间具有两种约束，分别为光平面约束和距离约束；

所述光平面约束指的是接收器接收到的发射站发出的激光扫描角度信息和发射站内部参数共同构建的平面方程，其数学表达式为：

式(1)中， (A_ij B_ij C_ij D_ij)表示发射站激光平面的内参数；表示的是发射站转子绕转轴旋转的姿态变换矩阵；θ_ij指的是发射站扫描角度，θ_ij由接收器收到的激光信号间隔以及发射站转子的转速得到；R_i和T_i为发射站外参数，由全局定向得到；(X_i Y_i Z_i)^T为空间约束点在全局导航坐标系下的坐标，该(X_i Y_i Z_i)^T由激光跟踪仪测得；

所述距离约束的数学表达式为：

式(7)中，多个空间约束点在发射站坐标系下的坐标R_n(X_n，Y_n，Z_n)由非线性最优化求得：

从而，求得各空间约束点在发射站坐标系的坐标；

步骤三、结合空间约束点在全局导航坐标系的坐标(X_i Y_i Z_i)^T和步骤二获得的各空间约束点在发射站坐标系的坐标解算出发射站坐标系和全局导航坐标系之间的转换关系，

设第i个约束点在发射站坐标系和全局导航坐标系下的坐标分别是P_i和Q_i，那么两个坐标系之间的坐标转换关系为：

式(8)中，R为旋转矩阵，T为平移矩阵；

R为正交矩阵，式(9)中各元素与发射站坐标系相对于全局导航坐标系的转动角度α，β，γ的关系如下：

T＝(x y z)^T，表示的是发射站坐标系原点到全局导航坐标系原点的平移量，(α,β,γ,x,y,z)为发射站坐标系相对于全局导航坐标系之间的位姿。

2.根据权利要求1所述基于空间多点约束的AGV动态位姿测量方法，其特征在于，

步骤二中，由激光跟踪仪测得空间约束点在全局导航坐标系下的坐标(X_i Y_i Z_i)^T的具体内容如下：

使用激光跟踪仪在空间中设站测量各空间约束点的三维坐标，将所述激光跟踪仪依次移动到新的站位，再次测量各空间约束点的三维坐标，激光跟踪仪的站位摆放满足每个站位能观测到3个以上的空间约束点，且空间约束点的数目和激光跟踪仪的站位数满足测长误差方程的建立条件：mn＞3(m+n)，其中，m是激光跟踪仪的测量站位数量，n是空间约束点的数量；

将激光跟踪仪某个站位下的测站坐标系定义为全局导航坐标系，其他站位下的测站坐标系相对于全局导航坐标系的关系为：

P_i＝RQ_i+T (2)

式(2)中，P_i为全局导航坐标系下空间约束点的三维坐标，Q_i为激光跟踪仪在其他站位下的测站坐标系下空间约束点的三维坐标测量值；

结合配准算法计算得到激光跟踪仪所有站位间的定位关系；

然后，利用激光跟踪仪高精度测距对全局约束点的三维坐标值进行改正优化，过程如下：

假设空间中存在n个全局约束点，三维坐标为(x_i,y_i,z_i),i＝1,2,...,n；激光跟踪仪在空间中通过m个站位对这n个约束点进行测量每个站位三维坐标为(X_k,Y_k,Z_k),k＝1,2,...,m；

在测量每个站位三维坐标过程中，对于每个站位和全局约束点，它们之间的距离r_ik是激光跟踪仪高精度的干涉测距值，假设为真值，r_ik的实际测量值l_ik表示如下：

对式(3)进行线性化：

式(4)中，Δx_i和ΔX_k就是全局约束点和激光跟踪仪站位坐标的优化改正值，初值

和

由之前激光跟踪仪站位之间的初步定向来确定，利用式(4)建立测长误差方程：

将式(5)转化为线性方程组的形式，如下：

V＝AΔX-b (6)

式(6)中，矩阵A是由式(3)泰勒展开的一阶求导项所组成的大型稀疏矩阵，

ΔX＝(ΔX₁,ΔY₁,ΔZ₁,ΔX₂,ΔY₂,ΔZ₂,...,ΔX_m,ΔY_m,ΔZ_m,Δx₁,Δy₁,Δz₁,Δx₂,Δy₂,Δz₂,...,Δx_n,Δy_n,Δz_n)^T

由于矩阵A是病态矩阵，条件数极大，因此，对矩阵A进行奇异值分解或者QR分解来求解方程组，结合相应的迭代条件得到最终的三维坐标优化改正值。

Images