CN115728730B - 一种多感知融合定位系统构建与坐标标定方法 - Google Patents
一种多感知融合定位系统构建与坐标标定方法 Download PDFInfo
- Publication number
- CN115728730B CN115728730B CN202211496101.3A CN202211496101A CN115728730B CN 115728730 B CN115728730 B CN 115728730B CN 202211496101 A CN202211496101 A CN 202211496101A CN 115728730 B CN115728730 B CN 115728730B
- Authority
- CN
- China
- Prior art keywords
- radar
- laser
- positioning
- laser reflection
- coordinate
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 94
- 230000004927 fusion Effects 0.000 title claims abstract description 52
- 238000010276 construction Methods 0.000 title claims abstract description 17
- 239000002131 composite material Substances 0.000 claims abstract description 61
- 238000004891 communication Methods 0.000 claims abstract description 7
- 239000011159 matrix material Substances 0.000 claims description 51
- 230000009466 transformation Effects 0.000 claims description 48
- 150000001875 compounds Chemical class 0.000 claims description 30
- 238000004364 calculation method Methods 0.000 claims description 22
- 238000013519 translation Methods 0.000 claims description 6
- 238000005259 measurement Methods 0.000 claims description 5
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 230000000694 effects Effects 0.000 claims description 3
- 230000007613 environmental effect Effects 0.000 claims description 3
- 238000005457 optimization Methods 0.000 claims description 3
- 230000008569 process Effects 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 238000013473 artificial intelligence Methods 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000004146 energy storage Methods 0.000 description 1
- 238000009776 industrial production Methods 0.000 description 1
- 239000003550 marker Substances 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
Landscapes
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本发明公开了一种多感知融合定位系统构建与坐标标定方法,属于人工智能领域。首先在工作区域构建由复合标定器、定位基站计算机及通信网络组成的UWB定位系统,并建立UWB定位坐标系;其次采用UWB定位系统标定方法获取并存储UWB基站坐标数据;再次在复合标定器旁建立雷达定位系统,并构建雷达定位坐标系;之后提出雷达定位系统粗标定与精标定方法对主、从激光雷达进行坐标标定;最后构建包括UWB基站和激光雷达的多感知融合定位系统,提出多感知融合定位系统联合标定方法,完成整个定位系统的坐标统一。本发明实现了多感知融合定位系统的系统构建与坐标标定,为提高多感知融合定位的精确性和可靠性奠定了良好的基础。
Description
技术领域
本发明涉及人工智能领域,具体涉及一种多感知融合定位系统构建与坐标标定方法。
背景技术
随着人工智能和传感器技术的发展,移动机器人在仓储物流、工业生产等领域的应用愈加广泛,面临的工作场景也更加复杂。移动机器人定位导航是其实现自主移动的核心技术,而定位精度直接影响了其精细作业的操控能力。实际使用中,传统定位方法由于定位原理单一难以满足机器人定位的高精度、高稳定性要求,需要以多传感器融合的方式提高定位性能。
多传感器坐标标定是定位系统多感知融合的基础,系统标定精度直接影响运动目标定位的精度。目前应用较多的标定仪器有工业相机、经纬仪、激光跟踪仪等。工业相机标定精度高,然而其视场角和测量范围较小,系统搭建成本高,操作较为复杂。经纬仪精度较高、适用范围广,但其测量标定速度慢,操作成本较高。激光跟踪仪精度较高、测量量程大,但其设备昂贵,测量效率较低。如公开号为CN114994648A的中国发明专利提出的一种直线运动机构上2D激光雷达的外参标定方法,利用标志物构建大场景的高精度点云,但是没有解决多传感器联合标定的问题;公开号为CN114544881A的中国发明专利提出的一种应用于储能电池安全检测的多气体传感器标定的设备,有效解决了多传感器联合标定的问题,但是其针对的是气体传感器,具有一定局限性。本发明面向移动目标集群的定位系统,提出一种多感知融合定位系统构建与坐标标定方法,通过UWB定位系统部署及标定、雷达定位系统部署及标定、多感知融合定位系统联合标定等,完成UWB基站-激光雷达定位坐标系的坐标统一。
发明内容
针对于多感知融合定位系统,为满足坐标标定需求,并有效控制成本,本发明提出了一种多感知融合定位系统构建与坐标标定方法,通过多传感器协同与迭代最近点配准实现系统标定统一,在解决了现有技术在面对部分标定场景时精度性能不足问题的同时,有效的控制了系统标定的成本。
本发明通过以下技术方案实现:
多感知融合定位系统构建与坐标标定方法,包括步骤:
步骤1:UWB定位系统部署:所述UWB定位系统包括复合标定器,所述复合标定器包括上部的UWB基站、中部的支撑架和下部的激光反射柱,复合标定器上的UWB基站具有主基站和从基站两类模式;根据UWB定位系统部署方法,通过激光雷达扫描复合标定器下部的激光反射柱,获得激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,获得激光反射柱的中心点坐标,进而在工作区域建立由4个复合标定器、定位基站计算机及通信网络组成的UWB定位系统;进而在工作区域建立1个主UWB基站和3个从UWB基站的UWB定位坐标系ΣXuOuYu。
进一步地,所述激光反射柱表面贴有激光反射膜,所述支撑架的中心与激光反射柱的轴线重合,所述UWB基站安装于支撑架顶部的中心位置,所述UWB基站的天线中心与激光反射柱的轴线重合。
进一步地,所述激光反射柱中心点计算方法是根据激光雷达扫描的激光反射柱轮廓数据,计算激光反射柱的中心点坐标数据(x o, y o, α o),包括以下步骤:
步骤1.1.1:假设激光雷达扫描的起始角为θ S,终止角为θ E,激光雷达正面前向的扫描角为θ F,激光雷达扫描所获得的目标点是以极坐标形式表达的点集:Q = { PQ j } = {(ρ j , θ j ) },j = 1, 2, …, w,w为激光雷达扫描一次的点数,将激光点云数据集Q转换为直角坐标形式:PQ j = (x j , y j , α j );
步骤1.1.2:如果激光雷达仅扫描1个激光反射柱,则将所有激光点云数据作为1组,SQ = Q;否则,如果激光雷达扫描n个激光反射柱,则采用Kmeans聚类算法将激光点云数据集Q分为n组,Q = { MQ k }, k = 1, 2, …, n;
步骤1.1.3:针对每个激光点云数据组SQ或MQ k ,采用Hough变换或最小二乘法将数据组内的激光点拟合为圆弧,求取圆弧的圆心坐标,即为激光反射柱的中心坐标数据(x o,y o, α o)。
进一步地,所述UWB定位系统部署方法,包括以下步骤:
步骤1.2.1:选取定位性能要求高的工作区域附近的角点为第一角点,选取定位性能要求高的方向为第一参考方向,与第一参考方向垂直的方向为第二参考方向,选取通过第一角点的矩形工作区域的对角线为第一对角线,而矩形工作区域的另一条对角线为第二对角线;
步骤1.2.2:在第一角点处精确设置激光雷达,保证激光雷达正面前向与第一参考方向重合,沿第一参考方向放置第二复合标定器;通过激光雷达扫描第二复合标定器下部的激光反射柱,获得激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,计算激光反射柱的中心点坐标;通过调节第二复合标定器的位置保证激光反射柱的中心点位于激光雷达正面前向上且到激光雷达的距离为l a,此时固定第二复合标定器的位置;
步骤1.2.3:在第一角点处精确调整激光雷达,保证激光雷达正面前向与第二参考方向重合,沿第二参考方向放置第三复合标定器;通过激光雷达扫描第三复合标定器下部的激光反射柱,获得激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,计算激光反射柱的中心点坐标;通过调节第三复合标定器的位置保证激光反射柱的中心点位于激光雷达正面前向上且到激光雷达的距离为l b,此时固定第三复合标定器的位置;
步骤1.2.4:在第一角点处再次精确调整激光雷达,保证激光雷达正面前向与第一对角线方向重合,沿第一对角线方向放置第四复合标定器;通过激光雷达扫描第四复合标定器下部的激光反射柱,获得激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,计算激光反射柱的中心点坐标;通过调节第四复合标定器的位置保证激光反射柱的中心点位于激光雷达正面前向上且到激光雷达的距离为l c,此时固定第四复合标定器的位置;
步骤1.2.5:从第一角点处移除激光雷达,再精确放置第一复合标定器,第一复合标定器上部为主UWB基站,并保证所有复合标定器上部的UWB基站在竖直方向上的高度差≤20cm;
步骤1.2.6:以主UWB基站位置为原点Ou,以第一参考方向作为Xu轴,选取经过原点、垂直于Xu轴并指向另外两个从UWB基站的方向作为Yu轴,建立UWB定位坐标系ΣXuOuYu。
步骤2:UWB定位系统标定:采用UWB定位系统标定方法,获得主UWB基站和从UWB基站在UWB定位坐标系下的坐标数据,形成复合标定器在坐标系ΣXuOuYu下的中心坐标数据集AU,并存储于定位基站计算机。
进一步地,所述UWB定位系统标定方法是通过建立并求解相对距离与从UWB基站坐标间的几何方程组FG来获得UWB基站的坐标数据;主UWB基站为原点Ou,通过主从UWB基站测距,获得Xu轴上的第二从UWB基站到主UWB基站的距离为d0,则所述从UWB基站的坐标为(d0,0);第三、第四从UWB基站到主UWB基站的距离分别测定为d3,1和d4,1,到Xu轴上的第二从UWB基站距离分别测定为d3,2和d4,2,第三、第四从UWB基站的坐标分别为(x 3, y 3)和(x 4, y 4),则几何方程组FG为
(1)
通过求解几何方程组FG获得第三、第四从UWB基站在坐标系ΣXuOuYu下的坐标数据,而主UWB基站与从UWB基站的坐标也即复合标定器在坐标系ΣXuOuYu下的中心点坐标,以此建立复合标定器的中心点坐标数据集AU。
步骤3:雷达定位系统部署:采用雷达定位系统部署方法,在复合标定器旁建立由主激光雷达、从激光雷达、定位基站计算机及通信网络组成的雷达定位系统;在工作区域建立主激光雷达的定位坐标系ΣXL0OL0YL0,以及第i个从激光雷达的定位坐标系ΣXLi OLi YLi 。
步骤4:雷达定位系统粗标定:采用雷达定位系统粗标定方法,以激光反射柱标识从激光雷达的部署位置,通过主激光雷达扫描激光反射柱,获得激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,获得激光反射柱的中心点坐标,进而计算第i从雷达定位坐标系ΣXLi OLi YLi 相对于主雷达定位坐标系ΣXL0OL0YL0的粗略坐标变换矩阵Tc(i),并存储于定位基站计算机。
进一步地,所述雷达定位系统粗标定方法与雷达定位系统部署方法交叉融合进行,包括以下步骤:
步骤4.1:在第一复合标定器旁设置主激光雷达,保证主激光雷达正面前向与第一对角线方向重合;选定主激光雷达中心作为主雷达定位坐标系的原点OL0,过原点OL0且沿激光雷达正面前向为XL0轴,垂直于X L0轴向左作射线为YL0轴,建立主雷达定位坐标系ΣXL0OL0YL0;
步骤4.2:在第四复合标定器旁设置激光反射柱,以激光反射柱标识第一从激光雷达的部署位置,通过主激光雷达扫描激光反射柱,获得激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,计算激光反射柱的中心点坐标;
步骤4.3:通过调节激光反射柱的位置保证激光反射柱的中心点位于主激光雷达正面前向上且到主激光雷达的距离为l c;利用主激光雷达测定激光反射柱的中心点在主雷达定位坐标系ΣXL0OL0YL0下的坐标数据(x s, y s, α s),在此时激光反射柱处放置第一从激光雷达,保证第一从激光雷达正面前向与第一对角线方向重合;
步骤4.4:选定第一从激光雷达中心作为从雷达定位坐标系的原点OL1,过原点OL1且沿激光雷达正面前向为XL1轴,垂直于X L1轴向左作射线为YL1轴,建立从雷达定位坐标系ΣXL1OL1YL1;
步骤4.5:针对第一从激光雷达建立第一从雷达定位坐标系ΣXL1OL1YL1,计算第一从雷达定位坐标系ΣXL1OL1YL1相对于主雷达定位坐标系ΣXL0OL0YL0的粗略位姿偏移量Tc(1)为:
(2)
进一步地,所述雷达定位系统粗标定方法还可根据环境特点及雷达角度需求,部署并标定第二从激光雷达及第三从激光雷达,包括以下步骤:
在第二复合标定器旁设置激光反射柱,以激光反射柱标识第二从激光雷达的部署位置,通过主激光雷达扫描激光反射柱,获得激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,计算激光反射柱的中心点坐标;通过调节激光反射柱的位置保证激光反射柱的中心点位于主激光雷达正面前向上且到主激光雷达的距离为l a;利用主激光雷达测定激光反射柱的中心点在主雷达定位坐标系ΣXL0OL0YL0下的坐标数据(x s2, y s2, α s2),在此时激光反射柱处放置第二从激光雷达,保证第二从激光雷达正面前向与第二对角线方向重合,建立第二从雷达定位坐标系ΣXL2OL2YL2,计算第二从雷达定位坐标系ΣXL2OL2YL2相对于主雷达定位坐标系ΣXL0OL0YL0的粗略位姿偏移量Tc(2)为
(3)
在第三复合标定器旁设置激光反射柱,以激光反射柱标识第三从激光雷达的部署位置,通过主激光雷达扫描激光反射柱,获得激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,计算激光反射柱的中心点坐标;通过调节激光反射柱的位置保证激光反射柱的中心点位于主激光雷达正面前向上且到主激光雷达的距离为l b;利用主激光雷达测定激光反射柱的中心点在主雷达定位坐标系ΣXL0OL0YL0下的坐标数据(x s3, y s3, α s3),在此时激光反射柱处放置第三从激光雷达,保证第三从激光雷达正面前向与第二对角线方向重合,建立第三从雷达定位坐标系ΣXL3OL3YL3,计算第三从雷达定位坐标系ΣXL3OL3YL3相对于主雷达定位坐标系ΣXL0OL0YL0的粗略位姿偏移量Tc(3)为
(4)
步骤5:雷达定位系统精标定:采用雷达定位系统精标定方法,在工作区域布置n个激光反射柱,通过主、从激光雷达扫描激光反射柱,获得多个激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,获得多个激光反射柱的中心点坐标数据集;根据点云配准算法配准不同中心点坐标数据集,计算第i从雷达定位坐标系ΣXLi OLi YLi 相对于主雷达定位坐标系ΣXL0OL0YL0的精确坐标变换矩阵Ta(i),并存储于定位基站计算机。
进一步地,所述雷达定位系统精标定方法包括以下步骤:
步骤5.1.1:在工作区域随机布置n个激光反射柱,其中n≥3,激光反射柱分布于工作区域中的各个部分,且相互之间无遮挡,保证每个激光雷达都能扫描到每个激光反射柱;
步骤5.1.2:通过主激光雷达扫描激光反射柱,获得n个激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,在主雷达定位坐标系ΣXL0OL0YL0下获得n个激光反射柱的中心点坐标数据集A0,并存储于定位基站计算机;
步骤5.1.3:通过第i从激光雷达扫描激光反射柱,获得n个激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,在第i从雷达定位坐标系ΣXLi OLi YLi 下获得n个激光反射柱的中心点坐标数据集B i ,并存储于定位基站计算机;
步骤5.1.4:针对第i从激光雷达,利用粗略坐标变换矩阵Tc(i),将从雷达定位坐标系ΣXLi OLi YLi 下的数据集B i ,变换到主雷达定位坐标系ΣXL0OL0YL0下的数据集A i ,并存储于定位基站计算机;
步骤5.1.5:在主雷达定位坐标系ΣXL0OL0YL0下,采用点云配准算法配准数据集A i 和A0,获取第i从雷达定位坐标系ΣXLi OLi YLi 相对于主雷达定位坐标系ΣXL0OL0YL0的精确坐标变换矩阵Ta(i),并存储于定位基站计算机。
进一步地,所述点云配准算法包括以下步骤:
步骤5.2.1:对应点关联:假设数据集DM = { PD m },DT = { PD t },m = 1, 2, …,S,t = 1, 2, …, R,S和R分别为数据集DM和DT中数据的数量;针对数据集DM中的任一点PD m ,选择数据集DT中离PD m 的最近点PD t 为点PD m 在数据集DT中的对应点PD t (m),完成数据集DM和DT之间的对应点关联;
步骤5.2.2:配准变换定义:假设数据集DM和DT之间的3×3坐标变换矩阵为Tp,2×2旋转矩阵为Rp,平移列向量为tp,则存在如下关系:
(5)
步骤5.2.3:误差方程建立:假设数据集DM = { PD m }经过Tp变换之后与数据集DT中对应点PD t (m)的误差和为E(Rp,tp),则构建评价配准效果的误差方程:
(6)
步骤5.2.4:方程优化求解:先求取数据集DM的质心为pM,DT数据集的质心为pT,可对E(Rp,tp)做出处理并简化可得:
(7)
再设数据集DM和数据集DT中每个数据点的去质心坐标分别为q m 、q t ,对应点PD t (m)的去质心坐标为q t (m),则目标旋转矩阵为:
(8)
为求解Rp可定义3×3的矩阵W,并对W进行奇异值分解(SVD):
,/> (9)
其中,Σ为奇异值组成的对角矩阵,U和V为正交矩阵,当W满秩时解得:
,/> (10)
求得旋转矩阵Rp与平移列向量tp后,即可按式(7)构造任意两个数据集DM和DT之间的坐标变换矩阵。
步骤6:多感知融合定位系统构建:上述UWB定位系统与雷达定位系统共同组成多感知融合定位系统。
步骤7:多感知融合定位系统联合标定:采用多感知融合定位系统联合标定方法,通过主激光雷达分别扫描复合标定器下部的激光反射柱,获得多个激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,获得多个激光反射柱的中心点坐标数据集AL,存储于定位基站计算机;根据点云配准算法配准数据集AU和AL,计算UWB定位坐标系ΣXuOuYu相对于主雷达定位坐标系ΣXL0OL0YL0的坐标变换矩阵Tc,并存储于定位基站计算机。
进一步地,所述雷达定位系统联合标定方法包括以下步骤:
步骤7.1:通过主激光雷达分别扫描第二、第三、第四复合标定器下部的激光反射柱,获得激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,计算第二、第三、第四复合标定器在坐标系ΣXL0OL0YL0下的中心点坐标数据集AL,并存储于定位基站计算机;
步骤7.2:从定位基站计算机中读取第二、第三、第四复合标定器在UWB定位坐标系ΣXuOuYu下的中心点坐标数据集AU,再读取第二、第三、第四复合标定器在主雷达定位坐标系ΣXL0OL0YL0下的中心点坐标数据集AL,采用点云配准算法配准数据集AU和AL,计算UWB定位坐标系ΣXuOuYu相对于主雷达定位坐标系ΣXL0OL0YL0的坐标变换矩阵Tc,并存储于定位基站计算机。
步骤8:多感知融合定位系统坐标统一:针对任一目标点,假设该目标点在坐标系ΣXuOuYu下的坐标为Pu,在坐标系ΣXL0OL0YL0下的坐标为PL0,在坐标系ΣXLi OLi YLi 下的坐标为PLi ,则建立如下所示的坐标变换模型:
PL0 = Ta(i) ×PLi (11)
PL0 = Tc×Pu (12)
与现有技术相比,本发明至少具有下述的有益效果或优点:
在复杂的环境中,多传感器融合定位的方式在提高定位性能的同时带来了标定的困难,传统标定方法无法兼顾移动目标的高精度定位与大范围运动、定位系统的高性能表现与低成本约束之间的诸多要求。
本发明针对多感知融合定位系统的特点,提出一种多感知融合定位系统构建与坐标标定方法的创新方法,相较于传统的系统构建与标定方式,通过多感知融合点云配准算法在大量程下有效提高了坐标标定精度,降低了定位系统标定的人力和时间成本,为多感知融合定位系统的高精度定位奠定了良好的技术基础。
附图说明
图1为本发明的多感知融合定位系统构建与坐标标定方法流程图;
图2为UWB定位系统部署示意图;
图3为多感知融合定位系统部署示意图;
图4为多感知融合定位系统坐标系示意图。
具体实施方式
以下结合附图和发明人依据本发明提供的技术方案所完成的具体实施例,对本发明做进一步的详细描述,但本实施例并不用于限制本发明,凡是采用本发明的相似方法及其相似变化,均应列入本发明的保护范围。
本发明所述的多感知融合定位系统构建与坐标标定方法,如图1所示,具体包括如下步骤:
步骤1:UWB定位系统部署:所述UWB定位系统包括复合标定器,所述复合标定器包括上部的UWB基站、中部的支撑架和下部的激光反射柱,复合标定器上的UWB基站具有主基站和从基站两类模式;根据UWB定位系统部署方法,通过激光雷达扫描复合标定器下部的激光反射柱,获得激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,获得激光反射柱的中心点坐标,进而在工作区域建立由4个复合标定器、定位基站计算机及通信网络组成的UWB定位系统;在工作区域建立1个主UWB基站和3个从UWB基站的UWB定位坐标系ΣXuOuYu。
进一步地,所述激光反射柱表面贴有激光反射膜,所述支撑架的中心与激光反射柱的轴线重合,所述UWB基站安装于支撑架顶部的中心位置,所述UWB基站的天线中心与激光反射柱的轴线重合。
进一步地,所述激光反射柱中心点计算方法是根据激光雷达扫描的激光反射柱轮廓数据,计算激光反射柱的中心点坐标数据(x o, y o, α o),包括以下步骤:
步骤1.1.1:假设激光雷达扫描的起始角为θ S,终止角为θ E,激光雷达正面前向的扫描角为θ F,激光雷达扫描所获得的目标点是以极坐标形式表达的点集:Q = { PQ j } = {(ρ j , θ j ) },j = 1, 2, …, w,w为激光雷达扫描一次的点数,将激光点云数据集Q转换为直角坐标形式:PQ j = (x j , y j , α j );
步骤1.1.2:如果激光雷达仅扫描1个激光反射柱,则将所有激光点云数据作为1组,SQ = Q;否则,如果激光雷达扫描n个激光反射柱,则采用Kmeans聚类算法将激光点云数据集Q分为n组,Q = { MQ k }, k = 1, 2, …, n;
步骤1.1.3:针对每个激光点云数据组SQ或MQ k ,采用Hough变换或最小二乘法将数据组内的激光点拟合为圆弧,求取圆弧的圆心坐标,即为激光反射柱的中心坐标数据(x o,y o, α o)。
进一步地,所述UWB定位系统部署方法,包括以下步骤:
步骤1.2.1:如图3所示,选取定位性能要求高的工作区域附近的角点为第一角点,在第一角点处设置十字标记点PU0,作为安装主UWB基站所属的复合标定器的参考点;选取定位性能要求高的方向为第一参考方向,与第一参考方向垂直的方向为第二参考方向,保证十字标记点PU0的第一标记线LU1沿第一参考方向,第二标记线LU2沿第二参考方向;假设矩形工作区域的长度为l a,宽度为l b,对角线长度为l c,选取通过第一角点的矩形工作区域的对角线为第一对角线,而矩形工作区域的另一条对角线为第二对角线,定义第一对角线方向为第三参考方向,沿第三参考方向设置第三标记线LU3;
步骤1.2.2:如图3所示,在十字标记点PU0处精确放置激光雷达,保证激光雷达的中心与十字标记点PU0的中心重合,激光雷达正面前向与第一标记线LU1重合,沿第一标记线LU1方向放置激光反射柱,通过调节激光反射柱的位置保证激光反射柱的中心点位于激光雷达正面前向上且到激光雷达的距离为l a,此时在激光反射柱处设置圆形标记PU1,并在圆形标记PU1圆心处放置第二复合标定器;
步骤1.2.3:如图3所示,再次在十字标记点PU0处精确调整激光雷达,保证激光雷达的中心与十字标记点PU0的中心重合,激光雷达正面前向与第二标记线LU2重合,沿第二标记线LU2方向放置激光反射柱,通过调节激光反射柱的位置保证激光反射柱的中心点位于激光雷达正面前向上且到激光雷达的距离为l b,此时在激光反射柱处设置圆形标记PU2,并在圆形标记PU2圆心处放置第三复合标定器;
步骤1.2.4:如图3所示,第三次在十字标记点PU0处精确调整激光雷达,保证激光雷达的中心与十字标记点PU0的中心重合,激光雷达正面前向与第三标记线LU3重合,沿第三标记线LU3方向放置激光反射柱,通过调节激光反射柱的位置保证激光反射柱的中心点位于激光雷达正面前向上且到激光雷达的距离为l c,此时在激光反射柱处设置圆形标记PU3,并在圆形标记PU3圆心处放置第四复合标定器;
步骤1.2.5:从十字标记点PU0处移除激光雷达,再精确放置第一复合标定器,第一复合标定器上部为主UWB基站,并保证所有复合标定器上部的UWB基站在竖直方向上的高度差≤20cm;
步骤1.2.6:如图4所示,以主UWB基站位置为原点Ou,以第一参考方向作为Xu轴,选取经过原点、垂直于Xu轴并指向另外两个从UWB基站的方向作为Yu轴,建立UWB定位坐标系ΣXuOuYu,如图2所示即为UWB理想定位坐标系。
步骤2:UWB定位系统标定:采用UWB定位系统标定方法,获得主UWB基站和从UWB基站在UWB定位坐标系下的坐标数据,形成复合标定器在坐标系ΣXuOuYu下的中心坐标数据集AU,并存储于定位基站计算机。
进一步地,所述UWB定位系统标定方法是通过建立并求解相对距离与从UWB基站坐标间的几何方程组FG来获得UWB基站的坐标数据;主UWB基站为原点Ou,通过主从UWB基站测距,获得Xu轴上的第二从UWB基站到主UWB基站的距离为d0,则所述从UWB基站的坐标为(d0,0);第三、第四从UWB基站到主UWB基站的距离分别测定为d3,1和d4,1,到Xu轴上的第二从UWB基站距离分别测定为d3,2和d4,2,第三、第四从UWB基站的坐标分别为(x 3, y 3)和(x 4, y 4),则几何方程组FG为
(1)
通过求解几何方程组FG获得第三、第四从UWB基站在坐标系ΣXuOuYu下的坐标数据,而主UWB基站与从UWB基站的坐标也即复合标定器在坐标系ΣXuOuYu下的中心点坐标,以此建立复合标定器的中心点坐标数据集AU。
步骤3:雷达定位系统部署:采用雷达定位系统部署方法,在复合标定器旁建立由主激光雷达、从激光雷达、定位基站计算机及通信网络组成的雷达定位系统;在工作区域建立主激光雷达的定位坐标系ΣXL0OL0YL0,以及第i个从激光雷达的定位坐标系ΣXLi OLi YLi 。
步骤4:雷达定位系统粗标定:采用雷达定位系统粗标定方法,以激光反射柱标识从激光雷达的部署位置,通过主激光雷达扫描激光反射柱,获得激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,获得激光反射柱的中心点坐标,进而计算第i从雷达定位坐标系ΣXLi OLi YLi 相对于主雷达定位坐标系ΣXL0OL0YL0的粗略坐标变换矩阵Tc(i),并存储于定位基站计算机。
进一步地,所述雷达定位系统粗标定方法与雷达定位系统部署方法交叉融合进行,包括以下步骤:
步骤4.1:如图3所示,在主UWB基站复合标定器旁设置部署主激光雷达的十字标记点PR0,保证十字标记点PR0的第一标记线LR1沿矩形工作区域的第一对角线方向,在十字标记点PR0处精确设置主激光雷达,保证主激光雷达的中心与十字标记点PR0的中心重合,主激光雷达正面前向与第一标记线LR1重合,建立主雷达定位坐标系ΣXL0OL0YL0,如图4所示;
步骤4.2:在第四复合标定器旁设置激光反射柱,通过调节激光反射柱的位置保证激光反射柱的中心点位于主激光雷达正面前向上且到主激光雷达的距离为l c,利用主激光雷达测定激光反射柱的中心点在主雷达定位坐标系ΣXL0OL0YL0下的坐标数据(x s, y s, α s),此时在激光反射柱处设置圆形标记PR1,如图3所示;
步骤4.3:在圆形标记PR1处移除激光反射柱,再精确放置第一从激光雷达,保证第一从激光雷达的中心与圆形标记PR1的圆心重合,第一从激光雷达的正面前向沿矩形工作区域的第一对角线方向,建立第一从雷达定位坐标系ΣXL1OL1YL1,如图4所示,计算第一从雷达定位坐标系ΣXL1OL1YL1相对于主雷达定位坐标系ΣXL0OL0YL0的粗略位姿偏移量Tc(1)为:
(2)
进一步地,所述雷达定位系统粗标定方法还可根据环境特点及雷达角度需求,部署并标定第二从激光雷达及第三从激光雷达,包括以下步骤:
如图3所示,在第二复合标定器旁设置激光反射柱,在十字标记点PR0处精确调整主激光雷达,保证主激光雷达的中心与十字标记点PR0的中心重合,主激光雷达正面前向与第一参考方向重合,沿第一参考方向上在从UWB基站附近放置激光反射柱,通过调节激光反射柱的位置保证激光反射柱的中心位于激光雷达正面前向上且到激光雷达的距离为l a,利用主激光雷达测定激光反射柱的中心点在主雷达定位坐标系ΣXL0OL0YL0下的坐标数据(x s2,y s2, α s2),此时在激光反射柱处设置圆形标记PR2;在圆形标记PR2处移除激光反射柱,再精确放置第二从激光雷达,保证第二从激光雷达的中心与圆形标记PR2的圆心重合,第二从激光雷达的正面前向沿矩形工作区域的第二对角线方向,建立第二从雷达定位坐标系ΣXL2OL2YL2,计算第二从雷达定位坐标系ΣXL2OL2YL2相对于主雷达定位坐标系ΣXL0OL0YL0的粗略位姿偏移量Tc(2)为
(3)
如图3所示,在第三复合标定器旁设置激光反射柱,通过调节激光反射柱的位置保证激光反射柱的中心点位于第二从激光雷达正面前向上且到激光雷达的距离为l b,利用主激光雷达测定激光反射柱的中心点在主雷达定位坐标系ΣXL0OL0YL0下的坐标数据(x s3,y s3, α s3),此时在激光反射柱处设置圆形标记PR3。再在圆形标记PR3处移除激光反射柱,再精确放置第三从激光雷达,保证第三从激光雷达的中心与圆形标记PR3的圆心重合,第三从激光雷达的正面前向沿矩形工作区域的第二对角线方向,建立第三从雷达定位坐标系ΣXL3OL3YL3,计算第三从雷达定位坐标系ΣXL3OL3YL3相对于主雷达定位坐标系ΣXL0OL0YL0的粗略位姿偏移量Tc(3)为
(4)
步骤5:雷达定位系统精标定:采用雷达定位系统精标定方法,在工作区域布置n个激光反射柱,通过主、从激光雷达扫描激光反射柱,获得多个激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,获得多个激光反射柱的中心点坐标数据集;根据点云配准算法配准不同中心点坐标数据集,计算第i从雷达定位坐标系ΣXLi OLi YLi 相对于主雷达定位坐标系ΣXL0OL0YL0的精确坐标变换矩阵Ta(i),并存储于定位基站计算机。
进一步地,所述雷达定位系统精标定方法包括以下步骤:
步骤5.1.1:在工作区域随机布置n个激光反射柱,其中n≥3,激光反射柱分布于工作区域中的各个部分,且相互之间无遮挡,保证每个激光雷达都能扫描到每个激光反射柱;
步骤5.1.2:通过主激光雷达扫描激光反射柱,获得n个激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,在主雷达定位坐标系ΣXL0OL0YL0下获得n个激光反射柱的中心点坐标数据集A0,并存储于定位基站计算机;
步骤5.1.3:通过第i从激光雷达扫描激光反射柱,获得n个激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,在第i从雷达定位坐标系ΣXLi OLi YLi 下获得n个激光反射柱的中心点坐标数据集B i ,并存储于定位基站计算机;
步骤5.1.4:针对第i从激光雷达,利用粗略坐标变换矩阵Tc(i),将从雷达定位坐标系ΣXLi OLi YLi 下的数据集B i ,变换到主雷达定位坐标系ΣXL0OL0YL0下的数据集A i ,并存储于定位基站计算机;
步骤5.1.5:在主雷达定位坐标系ΣXL0OL0YL0下,采用点云配准算法配准数据集A i 和A0,获取第i从雷达定位坐标系ΣXLi OLi YLi 相对于主雷达定位坐标系ΣXL0OL0YL0的精确坐标变换矩阵Ta(i),并存储于定位基站计算机。
进一步地,所述点云配准算法包括以下步骤:
步骤5.2.1:对应点关联:假设数据集DM = { PD m },DT = { PD t },m = 1, 2, …,S,t = 1, 2, …, R,S和R分别为数据集DM和DT中数据的数量;针对数据集DM中的任一点PD m ,选择数据集DT中离PD m 的最近点PD t 为点PD m 在数据集DT中的对应点PD t (m),完成数据集DM和DT之间的对应点关联;
步骤5.2.2:配准变换定义:假设数据集DM和DT之间的3×3坐标变换矩阵为Tp,2×2旋转矩阵为Rp,平移列向量为tp,则存在如下关系:
(5)
步骤5.2.3:误差方程建立:假设数据集DM = { PD m }经过Tp变换之后与数据集DT中对应点PD t (m)的误差和为E(Rp,tp),则构建评价配准效果的误差方程:
(6)
步骤5.2.4:方程优化求解:先求取数据集DM的质心为pM,DT数据集的质心为pT,可对E(Rp,tp)做出处理并简化可得:
(7)
再设数据集DM和数据集DT中每个数据点的去质心坐标分别为q m 、q t ,对应点PD t (m)的去质心坐标为q t (m),则目标旋转矩阵为:
(8)
为求解Rp可定义3×3的矩阵W,并对W进行奇异值分解(SVD):
,/> (9)
其中,Σ为奇异值组成的对角矩阵,U和V为正交矩阵,当W满秩时解得:
,/> (10)
求得旋转矩阵Rp与平移列向量tp后,即可按式(7)构造任意两个数据集DM和DT之间的坐标变换矩阵。
步骤6:多感知融合定位系统构建:上述UWB定位系统与雷达定位系统共同组成多感知融合定位系统。
步骤7:多感知融合定位系统联合标定:采用多感知融合定位系统联合标定方法,通过主激光雷达分别扫描复合标定器下部的激光反射柱,获得多个激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,获得多个激光反射柱的中心点坐标数据集AL,存储于定位基站计算机;根据点云配准算法配准数据集AU和AL,计算UWB定位坐标系ΣXuOuYu相对于主雷达定位坐标系ΣXL0OL0YL0的坐标变换矩阵Tc,并存储于定位基站计算机。
进一步地,所述雷达定位系统联合标定方法包括以下步骤:
步骤7.1:通过主激光雷达分别扫描第二、第三、第四复合标定器下部的激光反射柱,获得激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,计算第二、第三、第四复合标定器在坐标系ΣXL0OL0YL0下的中心点坐标数据集AL,并存储于定位基站计算机;
步骤7.2:从定位基站计算机中读取第二、第三、第四复合标定器在UWB定位坐标系ΣXuOuYu下的中心点坐标数据集AU,再读取第二、第三、第四复合标定器在主雷达定位坐标系ΣXL0OL0YL0下的中心点坐标数据集AL,采用点云配准算法配准数据集AU和AL,计算UWB定位坐标系ΣXuOuYu相对于主雷达定位坐标系ΣXL0OL0YL0的坐标变换矩阵Tc,并存储于定位基站计算机。
步骤8:多感知融合定位系统坐标统一:针对任一目标点,假设该目标点在坐标系ΣXuOuYu下的坐标为Pu,在坐标系ΣXL0OL0YL0下的坐标为PL0,在坐标系ΣXLi OLi YLi 下的坐标为PLi ,则建立如下所示的坐标变换模型:
PL0 = Ta(i) ×PLi (11)
PL0 = Tc×Pu (12)。
以上所述仅是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下还可以做出若干改进,这些改进也应视为本发明的保护范围。
Claims (10)
1.一种多感知融合定位系统构建与坐标标定方法,其特征在于,包括以下步骤:
步骤1:UWB定位系统部署:所述UWB定位系统包括复合标定器,所述复合标定器包括上部的UWB基站、中部的支撑架和下部的激光反射柱,复合标定器上的UWB基站具有主基站和从基站两类模式;根据UWB定位系统部署方法,通过激光雷达扫描复合标定器下部的激光反射柱,获得激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,获得激光反射柱的中心点坐标,进而在工作区域建立由4个复合标定器、定位基站计算机及通信网络组成的UWB定位系统;进而在工作区域建立1个主UWB基站和3个从UWB基站的UWB定位坐标系ΣXuOuYu;
步骤2:UWB定位系统标定:采用UWB定位系统标定方法,获得主UWB基站和从UWB基站在UWB定位坐标系下的坐标数据,形成复合标定器在坐标系ΣXuOuYu下的中心坐标数据集AU,并存储于定位基站计算机;
步骤3:雷达定位系统部署:采用雷达定位系统部署方法,在复合标定器旁建立由主激光雷达、从激光雷达、定位基站计算机及通信网络组成的雷达定位系统;在工作区域建立主激光雷达的定位坐标系ΣXL0OL0YL0,以及第i从激光雷达的定位坐标系ΣXLi OLi YLi ;
步骤4:雷达定位系统粗标定:采用雷达定位系统粗标定方法,以激光反射柱标识从激光雷达的部署位置,通过主激光雷达扫描激光反射柱,获得激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,获得激光反射柱的中心点坐标,进而计算第i从雷达定位坐标系ΣXLi OLi YLi 相对于主雷达定位坐标系ΣXL0OL0YL0的粗略坐标变换矩阵Tc(i),并存储于定位基站计算机;
步骤5:雷达定位系统精标定:采用雷达定位系统精标定方法,在工作区域布置n个激光反射柱,通过主、从激光雷达扫描激光反射柱,获得多个激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,获得多个激光反射柱的中心点坐标数据集;根据点云配准算法配准主、从激光雷达所扫描的中心点坐标数据集,计算第i从雷达定位坐标系ΣXLi OLi YLi 相对于主雷达定位坐标系ΣXL0OL0YL0的精确坐标变换矩阵Ta(i),并存储于定位基站计算机;
步骤6:多感知融合定位系统构建:上述UWB定位系统与雷达定位系统共同组成多感知融合定位系统;
步骤7:多感知融合定位系统联合标定:采用多感知融合定位系统联合标定方法,通过主激光雷达分别扫描复合标定器下部的激光反射柱,获得多个激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,获得多个激光反射柱的中心点坐标数据集AL,存储于定位基站计算机;根据点云配准算法配准数据集AU和AL,计算UWB定位坐标系ΣXuOuYu相对于主雷达定位坐标系ΣXL0OL0YL0的坐标变换矩阵Tc,并存储于定位基站计算机;
步骤8:多感知融合定位系统坐标统一:针对任一目标点,假设该目标点在坐标系ΣXuOuYu下的坐标为Pu,在坐标系ΣXL0OL0YL0下的坐标为PL0,在坐标系ΣXLi OLi YLi 下的坐标为PLi ,则建立如下所示的坐标变换模型:
PL0 = Ta(i) ×PLi (1)
PL0 = Tc×Pu (2)。
2.根据权利要求1所述的多感知融合定位系统构建与坐标标定方法,其特征在于,所述激光反射柱的表面贴有激光反射膜,所述支撑架的中心与激光反射柱的轴线重合,所述UWB基站安装于支撑架顶部的中心位置,所述UWB基站的天线中心与激光反射柱的轴线重合。
3. 根据权利要求1所述的多感知融合定位系统构建与坐标标定方法,其特征在于,所述激光反射柱中心点计算方法是根据激光雷达扫描的激光反射柱轮廓数据,计算激光反射柱的中心点坐标数据(x o, y o, α o),包括以下步骤:
步骤1.1.1:假设激光雷达扫描的起始角为θ S,终止角为θ E,激光雷达正面前向的扫描角为θ F,激光雷达扫描所获得的目标点是以极坐标形式表达的点集:Q = { PQ j } = { (ρ j ,θ j ) },j = 1, 2, …, w,w为激光雷达扫描一次的点数,将激光点云数据集Q转换为直角坐标形式:PQ j = (x j , y j , α j );
步骤1.1.2:如果激光雷达仅扫描1个激光反射柱,则将所有激光点云数据作为1组,SQ= Q;否则,如果激光雷达扫描n个激光反射柱,则采用Kmeans聚类算法将激光点云数据集Q分为n组,Q = { MQ k }, k = 1, 2, …, n;
步骤1.1.3:针对每个激光点云数据组SQ或MQ k ,采用Hough变换或最小二乘法将数据组内的激光点拟合为圆弧,求取圆弧的圆心坐标,即为激光反射柱的中心坐标数据(x o, y o,α o)。
4.根据权利要求1所述的多感知融合定位系统构建与坐标标定方法,其特征在于,所述UWB定位系统部署方法包括以下步骤:
步骤1.2.1:选取定位性能要求高的工作区域附近的角点为第一角点,选取定位性能要求高的方向为第一参考方向,与第一参考方向垂直的方向为第二参考方向,选取通过第一角点的矩形工作区域的对角线为第一对角线,而矩形工作区域的另一条对角线为第二对角线;
步骤1.2.2:在第一角点处精确设置激光雷达,保证激光雷达正面前向与第一参考方向重合,沿第一参考方向放置第二复合标定器;通过激光雷达扫描第二复合标定器下部的激光反射柱,获得激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,计算激光反射柱的中心点坐标;通过调节第二复合标定器的位置保证激光反射柱的中心点位于激光雷达正面前向上且到激光雷达的距离为l a,此时固定第二复合标定器的位置;
步骤1.2.3:在第一角点处精确调整激光雷达,保证激光雷达正面前向与第二参考方向重合,沿第二参考方向放置第三复合标定器;通过激光雷达扫描第三复合标定器下部的激光反射柱,获得激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,计算激光反射柱的中心点坐标;通过调节第三复合标定器的位置保证激光反射柱的中心点位于激光雷达正面前向上且到激光雷达的距离为l b,此时固定第三复合标定器的位置;
步骤1.2.4:在第一角点处再次精确调整激光雷达,保证激光雷达正面前向与第一对角线方向重合,沿第一对角线方向放置第四复合标定器;通过激光雷达扫描第四复合标定器下部的激光反射柱,获得激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,计算激光反射柱的中心点坐标;通过调节第四复合标定器的位置保证激光反射柱的中心点位于激光雷达正面前向上且到激光雷达的距离为l c,此时固定第四复合标定器的位置;
步骤1.2.5:从第一角点处移除激光雷达,再精确放置第一复合标定器,第一复合标定器上部为主UWB基站,并保证所有复合标定器上部的UWB基站在竖直方向上的高度差≤20cm;
步骤1.2.6:以主UWB基站位置为原点Ou,以第一参考方向作为Xu轴,选取经过原点、垂直于Xu轴并指向另外两个从UWB基站的方向作为Yu轴,建立UWB定位坐标系ΣXuOuYu。
5. 根据权利要求1所述的多感知融合定位系统构建与坐标标定方法,其特征在于,所述UWB定位系统标定方法是通过建立并求解相对距离与从UWB基站坐标间的几何方程组FG来获得UWB基站的坐标数据;主UWB基站为原点Ou,通过主从UWB基站测距,获得Xu轴上的第二从UWB基站到主UWB基站的距离为d0,则所述从UWB基站的坐标为(d0, 0);第三、第四从UWB基站到主UWB基站的距离分别测定为d3,1和d4,1,到Xu轴上的第二从UWB基站距离分别测定为d3,2和d4,2,第三、第四从UWB基站的坐标分别为(x 3, y 3)和(x 4, y 4),则几何方程组FG为
(3)
通过求解几何方程组FG获得第三、第四从UWB基站在坐标系ΣXuOuYu下的坐标数据,而主UWB基站与从UWB基站的坐标也即复合标定器在坐标系ΣXuOuYu下的中心点坐标,以此建立复合标定器的中心点坐标数据集AU。
6.根据权利要求1所述的多感知融合定位系统构建与坐标标定方法,其特征在于,所述雷达定位系统粗标定方法与雷达定位系统部署方法交叉融合进行,包括以下步骤:
步骤4.1:在第一复合标定器旁设置主激光雷达,保证主激光雷达正面前向与第一对角线方向重合;选定主激光雷达中心作为主雷达定位坐标系的原点OL0,过原点OL0且沿激光雷达正面前向为XL0轴,垂直于X L0轴向左作射线为YL0轴,建立主雷达定位坐标系ΣXL0OL0YL0;
步骤4.2:在第四复合标定器旁设置激光反射柱,以激光反射柱标识第一从激光雷达的部署位置,通过主激光雷达扫描激光反射柱,获得激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,计算激光反射柱的中心点坐标;
步骤4.3:通过调节激光反射柱的位置保证激光反射柱的中心点位于主激光雷达正面前向上且到主激光雷达的距离为l c;利用主激光雷达测定激光反射柱的中心点在主雷达定位坐标系ΣXL0OL0YL0下的坐标数据(x s, y s, α s),在此时激光反射柱处放置第一从激光雷达,保证第一从激光雷达正面前向与第一对角线方向重合;
步骤4.4:选定第一从激光雷达中心作为从雷达定位坐标系的原点OL1,过原点OL1且沿激光雷达正面前向为XL1轴,垂直于X L1轴向左作射线为YL1轴,建立从雷达定位坐标系ΣXL1OL1YL1;
步骤4.5:针对第一从激光雷达建立第一从雷达定位坐标系ΣXL1OL1YL1,计算第一从雷达定位坐标系ΣXL1OL1YL1相对于主雷达定位坐标系ΣXL0OL0YL0的粗略位姿偏移量Tc(1)为:
(4)。
7.根据权利要求6所述的多感知融合定位系统构建与坐标标定方法,其特征在于,所述雷达定位系统粗标定方法还可根据环境特点及雷达角度需求,部署并标定第二从激光雷达及第三从激光雷达。
8.根据权利要求1所述的多感知融合定位系统构建与坐标标定方法,其特征在于,所述雷达定位系统精标定方法包括以下步骤:
步骤5.1.1:在工作区域随机布置n个激光反射柱,其中n≥3,激光反射柱分布于工作区域中的各个部分,且相互之间无遮挡,保证每个激光雷达都能扫描到每个激光反射柱;
步骤5.1.2:通过主激光雷达扫描激光反射柱,获得n个激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,在主雷达定位坐标系ΣXL0OL0YL0下获得n个激光反射柱的中心点坐标数据集A0,并存储于定位基站计算机;
步骤5.1.3:通过第i从激光雷达扫描激光反射柱,获得n个激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,在第i从雷达定位坐标系ΣXLi OLi YLi 下获得n个激光反射柱的中心点坐标数据集B i ,并存储于定位基站计算机;
步骤5.1.4:针对第i从激光雷达,利用粗略坐标变换矩阵Tc(i),将从雷达定位坐标系ΣXLi OLi YLi 下的数据集B i ,变换到主雷达定位坐标系ΣXL0OL0YL0下的数据集A i ,并存储于定位基站计算机;
步骤5.1.5:在主雷达定位坐标系ΣXL0OL0YL0下,采用点云配准算法配准数据集A i 和A0,获取第i从雷达定位坐标系ΣXLi OLi YLi 相对于主雷达定位坐标系ΣXL0OL0YL0的精确坐标变换矩阵Ta(i),并存储于定位基站计算机。
9.根据权利要求1所述的多感知融合定位系统构建与坐标标定方法,其特征在于,所述点云配准算法包括以下步骤:
步骤5.2.1:对应点关联:假设数据集DM = { PD m },DT = { PD t },m = 1, 2, …, S,t= 1, 2, …, R,S和R分别为数据集DM和DT中数据的数量;针对数据集DM中的任一点PD m ,选择数据集DT中离PD m 的最近点PD t 为点PD m 在数据集DT中的对应点PD t (m),完成数据集DM和DT之间的对应点关联;
步骤5.2.2:配准变换定义:假设数据集DM和DT之间的3×3坐标变换矩阵为Tp,2×2旋转矩阵为Rp,平移列向量为tp,则存在如下关系:
(5)
步骤5.2.3:误差方程建立:假设数据集DM = { PD m }经过Tp变换之后与数据集DT中对应点PD t (m)的误差和为E(Rp,tp),则构建评价配准效果的误差方程:
(6)
步骤5.2.4:方程优化求解:先求取数据集DM的质心为pM,DT数据集的质心为pT,可对E(Rp,tp)做出处理并简化可得:
(7)
再设数据集DM和数据集DT中每个数据点的去质心坐标分别为q m 、q t ,对应点PD t (m)的去质心坐标为q t (m),则目标旋转矩阵为:
(8)
为求解Rp可定义3×3的矩阵W,并对W进行奇异值分解(SVD):
,/> (9)
其中,Σ为奇异值组成的对角矩阵,U和V为正交矩阵,当W满秩时解得:
,/> (10)
求得旋转矩阵Rp与平移列向量tp后,即可按式(5)构造任意两个数据集DM和DT之间的坐标变换矩阵。
10.根据权利要求1所述的多感知融合定位系统构建与坐标标定方法,其特征在于,所述雷达定位系统联合标定方法包括以下步骤:
步骤7.1:通过主激光雷达分别扫描第二、第三、第四复合标定器下部的激光反射柱,获得激光反射柱的轮廓数据;利用激光反射柱中心点计算方法,计算第二、第三、第四复合标定器在坐标系ΣXL0OL0YL0下的中心点坐标数据集AL,并存储于定位基站计算机;
步骤7.2:从定位基站计算机中读取第二、第三、第四复合标定器在UWB定位坐标系ΣXuOuYu下的中心点坐标数据集AU,再读取第二、第三、第四复合标定器在主雷达定位坐标系ΣXL0OL0YL0下的中心点坐标数据集AL,采用点云配准算法配准数据集AU和AL,计算UWB定位坐标系ΣXuOuYu相对于主雷达定位坐标系ΣXL0OL0YL0的坐标变换矩阵Tc,并存储于定位基站计算机。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211496101.3A CN115728730B (zh) | 2022-11-25 | 2022-11-25 | 一种多感知融合定位系统构建与坐标标定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211496101.3A CN115728730B (zh) | 2022-11-25 | 2022-11-25 | 一种多感知融合定位系统构建与坐标标定方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115728730A CN115728730A (zh) | 2023-03-03 |
CN115728730B true CN115728730B (zh) | 2024-06-07 |
Family
ID=85298482
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211496101.3A Active CN115728730B (zh) | 2022-11-25 | 2022-11-25 | 一种多感知融合定位系统构建与坐标标定方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115728730B (zh) |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN209230639U (zh) * | 2018-11-07 | 2019-08-09 | 成都方德尔科技有限公司 | 一种基于激光导航的坐标定位装置 |
CN111336915A (zh) * | 2018-12-18 | 2020-06-26 | 莱卡地球系统公开股份有限公司 | 在基于激光跟踪仪的工业对象测量期间粗略定位可移动协作目标的系统 |
CN113158387A (zh) * | 2021-03-17 | 2021-07-23 | 南京航空航天大学 | 一种基于激光雷达栅格地图耦合的视觉靶点布置方法 |
CN115183762A (zh) * | 2022-08-02 | 2022-10-14 | 上海于万科技有限公司 | 一种机场仓库内外建图方法、系统、电子设备及介质 |
-
2022
- 2022-11-25 CN CN202211496101.3A patent/CN115728730B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN209230639U (zh) * | 2018-11-07 | 2019-08-09 | 成都方德尔科技有限公司 | 一种基于激光导航的坐标定位装置 |
CN111336915A (zh) * | 2018-12-18 | 2020-06-26 | 莱卡地球系统公开股份有限公司 | 在基于激光跟踪仪的工业对象测量期间粗略定位可移动协作目标的系统 |
CN113158387A (zh) * | 2021-03-17 | 2021-07-23 | 南京航空航天大学 | 一种基于激光雷达栅格地图耦合的视觉靶点布置方法 |
CN115183762A (zh) * | 2022-08-02 | 2022-10-14 | 上海于万科技有限公司 | 一种机场仓库内外建图方法、系统、电子设备及介质 |
Non-Patent Citations (1)
Title |
---|
室内环境下基于UWB和激光雷达的移动机器人二维定位的研究;王昭东;《中国优秀硕士学位论文全文数据库 信息科技辑》;20220615;第2022卷(第6期);I140-184 * |
Also Published As
Publication number | Publication date |
---|---|
CN115728730A (zh) | 2023-03-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
TWI752447B (zh) | 超寬頻輔助精確定位方法 | |
CN103323855B (zh) | 一种基线动态测量系统的精度获取方法 | |
CN111968177B (zh) | 一种基于固定摄像头视觉的移动机器人定位方法 | |
CN103895023A (zh) | 一种基于编码方位装置的机械臂末端跟踪测量系统及方法 | |
CN110142805A (zh) | 一种基于激光雷达的机器人末端校准方法 | |
CN109238247B (zh) | 一种面向大空间复杂现场的六自由度测量方法 | |
CN101539397B (zh) | 物体三维姿态的精密光学测量方法 | |
CN106226780A (zh) | 基于激光扫描雷达的多旋翼室内定位系统及实现方法 | |
CN111595238B (zh) | 一种基于多站法的激光跟踪仪精度现场评价系统 | |
CN110926479B (zh) | 自动生成室内三维导航地图模型的方法和系统 | |
CN111273312A (zh) | 一种智能车辆定位与回环检测方法 | |
CN101900552A (zh) | 经纬像机摄像测量方法及系统 | |
CN115027482A (zh) | 智能驾驶中的融合定位方法 | |
CN115876197A (zh) | 一种系留升空光电成像目标定位方法 | |
CN110160503B (zh) | 一种顾及高程的无人机景观匹配定位方法 | |
CN115728730B (zh) | 一种多感知融合定位系统构建与坐标标定方法 | |
CN117928491A (zh) | 一种多站组网视觉跟踪的监测系统及监测方法 | |
CN112859052A (zh) | 基于重叠航带共轭基元的机载激光雷达系统集成误差检校方法 | |
CN117310627A (zh) | 一种应用于车路协同路侧感知系统的联合标定方法 | |
CN116679314A (zh) | 融合点云强度的三维激光雷达同步建图与定位方法及系统 | |
CN115932774A (zh) | 基于转换矩阵的雷达目标的融合仿真方法及其装置 | |
Wang et al. | A mobile stereo vision system with variable baseline distance for three-dimensional coordinate measurement in large FOV | |
CN115932926A (zh) | 一种缩短多传感器融合定位初始化时间的方法 | |
CN115307547A (zh) | 一种激光跟踪仪结合视觉系统的组网测试方法 | |
CN114004949A (zh) | 机载点云辅助的移动测量系统安置参数检校方法及系统 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant |