CN115728730B - 一种多感知融合定位系统构建与坐标标定方法 - Google Patents

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定位坐标系ΣX_uO_uY_u。

进一步地，所述激光反射柱表面贴有激光反射膜，所述支撑架的中心与激光反射柱的轴线重合，所述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基站位置为原点O_u，以第一参考方向作为X_u轴，选取经过原点、垂直于X_u轴并指向另外两个从UWB基站的方向作为Y_u轴，建立UWB定位坐标系ΣX_uO_uY_u。

步骤2：UWB定位系统标定：采用UWB定位系统标定方法，获得主UWB基站和从UWB基站在UWB定位坐标系下的坐标数据，形成复合标定器在坐标系ΣX_uO_uY_u下的中心坐标数据集A_U，并存储于定位基站计算机。

进一步地，所述UWB定位系统标定方法是通过建立并求解相对距离与从UWB基站坐标间的几何方程组F_G来获得UWB基站的坐标数据；主UWB基站为原点O_u，通过主从UWB基站测距，获得X_u轴上的第二从UWB基站到主UWB基站的距离为d₀，则所述从UWB基站的坐标为(d₀,0)；第三、第四从UWB基站到主UWB基站的距离分别测定为d_3,1和d_4,1，到X_u轴上的第二从UWB基站距离分别测定为d_3,2和d_4,2，第三、第四从UWB基站的坐标分别为(x ₃, y ₃)和(x ₄, y ₄)，则几何方程组F_G为

(1)

通过求解几何方程组F_G获得第三、第四从UWB基站在坐标系ΣX_uO_uY_u下的坐标数据，而主UWB基站与从UWB基站的坐标也即复合标定器在坐标系ΣX_uO_uY_u下的中心点坐标，以此建立复合标定器的中心点坐标数据集A_U。

步骤3：雷达定位系统部署：采用雷达定位系统部署方法，在复合标定器旁建立由主激光雷达、从激光雷达、定位基站计算机及通信网络组成的雷达定位系统；在工作区域建立主激光雷达的定位坐标系ΣX_L0O_L0Y_L0，以及第i个从激光雷达的定位坐标系ΣX_Li O_Li Y_Li 。

步骤4：雷达定位系统粗标定：采用雷达定位系统粗标定方法，以激光反射柱标识从激光雷达的部署位置，通过主激光雷达扫描激光反射柱，获得激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，获得激光反射柱的中心点坐标，进而计算第i从雷达定位坐标系ΣX_Li O_Li Y_Li 相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的粗略坐标变换矩阵T_c(i)，并存储于定位基站计算机。

进一步地，所述雷达定位系统粗标定方法与雷达定位系统部署方法交叉融合进行，包括以下步骤：

步骤4.1：在第一复合标定器旁设置主激光雷达，保证主激光雷达正面前向与第一对角线方向重合；选定主激光雷达中心作为主雷达定位坐标系的原点O_L0，过原点O_L0且沿激光雷达正面前向为X_L0轴，垂直于X _L0轴向左作射线为Y_L0轴，建立主雷达定位坐标系ΣX_L0O_L0Y_L0；

步骤4.2：在第四复合标定器旁设置激光反射柱，以激光反射柱标识第一从激光雷达的部署位置，通过主激光雷达扫描激光反射柱，获得激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，计算激光反射柱的中心点坐标；

步骤4.3：通过调节激光反射柱的位置保证激光反射柱的中心点位于主激光雷达正面前向上且到主激光雷达的距离为l _c；利用主激光雷达测定激光反射柱的中心点在主雷达定位坐标系ΣX_L0O_L0Y_L0下的坐标数据(x _s, y _s, α _s)，在此时激光反射柱处放置第一从激光雷达，保证第一从激光雷达正面前向与第一对角线方向重合；

步骤4.4：选定第一从激光雷达中心作为从雷达定位坐标系的原点O_L1，过原点O_L1且沿激光雷达正面前向为X_L1轴，垂直于X _L1轴向左作射线为Y_L1轴，建立从雷达定位坐标系ΣX_L1O_L1Y_L1；

步骤4.5：针对第一从激光雷达建立第一从雷达定位坐标系ΣX_L1O_L1Y_L1，计算第一从雷达定位坐标系ΣX_L1O_L1Y_L1相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的粗略位姿偏移量T_c(1)为：

(2)

进一步地，所述雷达定位系统粗标定方法还可根据环境特点及雷达角度需求，部署并标定第二从激光雷达及第三从激光雷达，包括以下步骤：

在第二复合标定器旁设置激光反射柱，以激光反射柱标识第二从激光雷达的部署位置，通过主激光雷达扫描激光反射柱，获得激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，计算激光反射柱的中心点坐标；通过调节激光反射柱的位置保证激光反射柱的中心点位于主激光雷达正面前向上且到主激光雷达的距离为l _a；利用主激光雷达测定激光反射柱的中心点在主雷达定位坐标系ΣX_L0O_L0Y_L0下的坐标数据(x _s2, y _s2, α _s2)，在此时激光反射柱处放置第二从激光雷达，保证第二从激光雷达正面前向与第二对角线方向重合，建立第二从雷达定位坐标系ΣX_L2O_L2Y_L2，计算第二从雷达定位坐标系ΣX_L2O_L2Y_L2相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的粗略位姿偏移量T_c(2)为

(3)

在第三复合标定器旁设置激光反射柱，以激光反射柱标识第三从激光雷达的部署位置，通过主激光雷达扫描激光反射柱，获得激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，计算激光反射柱的中心点坐标；通过调节激光反射柱的位置保证激光反射柱的中心点位于主激光雷达正面前向上且到主激光雷达的距离为l _b；利用主激光雷达测定激光反射柱的中心点在主雷达定位坐标系ΣX_L0O_L0Y_L0下的坐标数据(x _s3, y _s3, α _s3)，在此时激光反射柱处放置第三从激光雷达，保证第三从激光雷达正面前向与第二对角线方向重合，建立第三从雷达定位坐标系ΣX_L3O_L3Y_L3，计算第三从雷达定位坐标系ΣX_L3O_L3Y_L3相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的粗略位姿偏移量T_c(3)为

(4)

步骤5：雷达定位系统精标定：采用雷达定位系统精标定方法，在工作区域布置n个激光反射柱，通过主、从激光雷达扫描激光反射柱，获得多个激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，获得多个激光反射柱的中心点坐标数据集；根据点云配准算法配准不同中心点坐标数据集，计算第i从雷达定位坐标系ΣX_Li O_Li Y_Li 相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的精确坐标变换矩阵T_a(i)，并存储于定位基站计算机。

进一步地，所述雷达定位系统精标定方法包括以下步骤：

步骤5.1.1：在工作区域随机布置n个激光反射柱，其中n≥3，激光反射柱分布于工作区域中的各个部分，且相互之间无遮挡，保证每个激光雷达都能扫描到每个激光反射柱；

步骤5.1.2：通过主激光雷达扫描激光反射柱，获得n个激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，在主雷达定位坐标系ΣX_L0O_L0Y_L0下获得n个激光反射柱的中心点坐标数据集A₀，并存储于定位基站计算机；

步骤5.1.3：通过第i从激光雷达扫描激光反射柱，获得n个激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，在第i从雷达定位坐标系ΣX_Li O_Li Y_Li 下获得n个激光反射柱的中心点坐标数据集B _i ，并存储于定位基站计算机；

步骤5.1.4：针对第i从激光雷达，利用粗略坐标变换矩阵T_c(i)，将从雷达定位坐标系ΣX_Li O_Li Y_Li 下的数据集B _i ，变换到主雷达定位坐标系ΣX_L0O_L0Y_L0下的数据集A _i ，并存储于定位基站计算机；

步骤5.1.5：在主雷达定位坐标系ΣX_L0O_L0Y_L0下，采用点云配准算法配准数据集A _i 和A₀，获取第i从雷达定位坐标系ΣX_Li O_Li Y_Li 相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的精确坐标变换矩阵T_a(i)，并存储于定位基站计算机。

进一步地，所述点云配准算法包括以下步骤：

步骤5.2.1：对应点关联：假设数据集D_M = { PD _m }，D_T = { PD _t }，m = 1, 2, …,S，t = 1, 2, …, R，S和R分别为数据集D_M和D_T中数据的数量；针对数据集D_M中的任一点PD _m ，选择数据集D_T中离PD _m 的最近点PD _t 为点PD _m 在数据集D_T中的对应点PD _t (m)，完成数据集D_M和D_T之间的对应点关联；

步骤5.2.2：配准变换定义：假设数据集D_M和D_T之间的3×3坐标变换矩阵为T_p，2×2旋转矩阵为R_p，平移列向量为t_p，则存在如下关系：

(5)

步骤5.2.3：误差方程建立：假设数据集D_M = { PD _m }经过T_p变换之后与数据集D_T中对应点PD _t (m)的误差和为E(R_p,t_p)，则构建评价配准效果的误差方程：

(6)

步骤5.2.4：方程优化求解：先求取数据集D_M的质心为p_M，D_T数据集的质心为p_T，可对E(R_p,t_p)做出处理并简化可得：

(7)

再设数据集D_M和数据集D_T中每个数据点的去质心坐标分别为q _m 、q _t ，对应点PD _t (m)的去质心坐标为q _t (m)，则目标旋转矩阵为：

(8)

为求解R_p可定义3×3的矩阵W，并对W进行奇异值分解（SVD）：

，/> (9)

其中，Σ为奇异值组成的对角矩阵，U和V为正交矩阵，当W满秩时解得：

，/> (10)

求得旋转矩阵R_p与平移列向量t_p后，即可按式(7)构造任意两个数据集D_M和D_T之间的坐标变换矩阵。

步骤6：多感知融合定位系统构建：上述UWB定位系统与雷达定位系统共同组成多感知融合定位系统。

步骤7：多感知融合定位系统联合标定：采用多感知融合定位系统联合标定方法，通过主激光雷达分别扫描复合标定器下部的激光反射柱，获得多个激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，获得多个激光反射柱的中心点坐标数据集A_L，存储于定位基站计算机；根据点云配准算法配准数据集A_U和A_L，计算UWB定位坐标系ΣX_uO_uY_u相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的坐标变换矩阵T_c，并存储于定位基站计算机。

进一步地，所述雷达定位系统联合标定方法包括以下步骤：

步骤7.1：通过主激光雷达分别扫描第二、第三、第四复合标定器下部的激光反射柱，获得激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，计算第二、第三、第四复合标定器在坐标系ΣX_L0O_L0Y_L0下的中心点坐标数据集A_L，并存储于定位基站计算机；

步骤7.2：从定位基站计算机中读取第二、第三、第四复合标定器在UWB定位坐标系ΣX_uO_uY_u下的中心点坐标数据集A_U，再读取第二、第三、第四复合标定器在主雷达定位坐标系ΣX_L0O_L0Y_L0下的中心点坐标数据集A_L，采用点云配准算法配准数据集A_U和A_L，计算UWB定位坐标系ΣX_uO_uY_u相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的坐标变换矩阵T_c，并存储于定位基站计算机。

步骤8：多感知融合定位系统坐标统一：针对任一目标点，假设该目标点在坐标系ΣX_uO_uY_u下的坐标为P_u，在坐标系ΣX_L0O_L0Y_L0下的坐标为P_L0，在坐标系ΣX_Li O_Li Y_Li 下的坐标为P_Li ，则建立如下所示的坐标变换模型：

P_L0 = T_a(i) ×P_Li (11)

P_L0 = T_c×P_u (12)

与现有技术相比，本发明至少具有下述的有益效果或优点：

在复杂的环境中，多传感器融合定位的方式在提高定位性能的同时带来了标定的困难，传统标定方法无法兼顾移动目标的高精度定位与大范围运动、定位系统的高性能表现与低成本约束之间的诸多要求。

本发明针对多感知融合定位系统的特点，提出一种多感知融合定位系统构建与坐标标定方法的创新方法，相较于传统的系统构建与标定方式，通过多感知融合点云配准算法在大量程下有效提高了坐标标定精度，降低了定位系统标定的人力和时间成本，为多感知融合定位系统的高精度定位奠定了良好的技术基础。

附图说明

图1为本发明的多感知融合定位系统构建与坐标标定方法流程图；

图2为UWB定位系统部署示意图；

图3为多感知融合定位系统部署示意图;

图4为多感知融合定位系统坐标系示意图。

具体实施方式

以下结合附图和发明人依据本发明提供的技术方案所完成的具体实施例，对本发明做进一步的详细描述，但本实施例并不用于限制本发明，凡是采用本发明的相似方法及其相似变化，均应列入本发明的保护范围。

本发明所述的多感知融合定位系统构建与坐标标定方法，如图1所示，具体包括如下步骤：

步骤1：UWB定位系统部署：所述UWB定位系统包括复合标定器，所述复合标定器包括上部的UWB基站、中部的支撑架和下部的激光反射柱，复合标定器上的UWB基站具有主基站和从基站两类模式；根据UWB定位系统部署方法，通过激光雷达扫描复合标定器下部的激光反射柱，获得激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，获得激光反射柱的中心点坐标，进而在工作区域建立由4个复合标定器、定位基站计算机及通信网络组成的UWB定位系统；在工作区域建立1个主UWB基站和3个从UWB基站的UWB定位坐标系ΣX_uO_uY_u。

进一步地，所述激光反射柱表面贴有激光反射膜，所述支撑架的中心与激光反射柱的轴线重合，所述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所示，选取定位性能要求高的工作区域附近的角点为第一角点，在第一角点处设置十字标记点PU₀，作为安装主UWB基站所属的复合标定器的参考点；选取定位性能要求高的方向为第一参考方向，与第一参考方向垂直的方向为第二参考方向，保证十字标记点PU₀的第一标记线LU₁沿第一参考方向，第二标记线LU₂沿第二参考方向；假设矩形工作区域的长度为l _a，宽度为l _b，对角线长度为l _c，选取通过第一角点的矩形工作区域的对角线为第一对角线，而矩形工作区域的另一条对角线为第二对角线，定义第一对角线方向为第三参考方向，沿第三参考方向设置第三标记线LU₃；

步骤1.2.2：如图3所示，在十字标记点PU₀处精确放置激光雷达，保证激光雷达的中心与十字标记点PU₀的中心重合，激光雷达正面前向与第一标记线LU₁重合，沿第一标记线LU₁方向放置激光反射柱，通过调节激光反射柱的位置保证激光反射柱的中心点位于激光雷达正面前向上且到激光雷达的距离为l _a，此时在激光反射柱处设置圆形标记PU₁，并在圆形标记PU₁圆心处放置第二复合标定器；

步骤1.2.3：如图3所示，再次在十字标记点PU₀处精确调整激光雷达，保证激光雷达的中心与十字标记点PU₀的中心重合，激光雷达正面前向与第二标记线LU₂重合，沿第二标记线LU₂方向放置激光反射柱，通过调节激光反射柱的位置保证激光反射柱的中心点位于激光雷达正面前向上且到激光雷达的距离为l _b，此时在激光反射柱处设置圆形标记PU₂，并在圆形标记PU₂圆心处放置第三复合标定器；

步骤1.2.4：如图3所示，第三次在十字标记点PU₀处精确调整激光雷达，保证激光雷达的中心与十字标记点PU₀的中心重合，激光雷达正面前向与第三标记线LU₃重合，沿第三标记线LU₃方向放置激光反射柱，通过调节激光反射柱的位置保证激光反射柱的中心点位于激光雷达正面前向上且到激光雷达的距离为l _c，此时在激光反射柱处设置圆形标记PU₃，并在圆形标记PU₃圆心处放置第四复合标定器；

步骤1.2.5：从十字标记点PU₀处移除激光雷达，再精确放置第一复合标定器，第一复合标定器上部为主UWB基站，并保证所有复合标定器上部的UWB基站在竖直方向上的高度差≤20cm；

步骤1.2.6：如图4所示，以主UWB基站位置为原点O_u，以第一参考方向作为X_u轴，选取经过原点、垂直于X_u轴并指向另外两个从UWB基站的方向作为Y_u轴，建立UWB定位坐标系ΣX_uO_uY_u，如图2所示即为UWB理想定位坐标系。

步骤2：UWB定位系统标定：采用UWB定位系统标定方法，获得主UWB基站和从UWB基站在UWB定位坐标系下的坐标数据，形成复合标定器在坐标系ΣX_uO_uY_u下的中心坐标数据集A_U，并存储于定位基站计算机。

进一步地，所述UWB定位系统标定方法是通过建立并求解相对距离与从UWB基站坐标间的几何方程组F_G来获得UWB基站的坐标数据；主UWB基站为原点O_u，通过主从UWB基站测距，获得X_u轴上的第二从UWB基站到主UWB基站的距离为d₀，则所述从UWB基站的坐标为(d₀,0)；第三、第四从UWB基站到主UWB基站的距离分别测定为d_3,1和d_4,1，到X_u轴上的第二从UWB基站距离分别测定为d_3,2和d_4,2，第三、第四从UWB基站的坐标分别为(x ₃, y ₃)和(x ₄, y ₄)，则几何方程组F_G为

(1)

通过求解几何方程组F_G获得第三、第四从UWB基站在坐标系ΣX_uO_uY_u下的坐标数据，而主UWB基站与从UWB基站的坐标也即复合标定器在坐标系ΣX_uO_uY_u下的中心点坐标，以此建立复合标定器的中心点坐标数据集A_U。

步骤3：雷达定位系统部署：采用雷达定位系统部署方法，在复合标定器旁建立由主激光雷达、从激光雷达、定位基站计算机及通信网络组成的雷达定位系统；在工作区域建立主激光雷达的定位坐标系ΣX_L0O_L0Y_L0，以及第i个从激光雷达的定位坐标系ΣX_Li O_Li Y_Li 。

步骤4：雷达定位系统粗标定：采用雷达定位系统粗标定方法，以激光反射柱标识从激光雷达的部署位置，通过主激光雷达扫描激光反射柱，获得激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，获得激光反射柱的中心点坐标，进而计算第i从雷达定位坐标系ΣX_Li O_Li Y_Li 相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的粗略坐标变换矩阵T_c(i)，并存储于定位基站计算机。

进一步地，所述雷达定位系统粗标定方法与雷达定位系统部署方法交叉融合进行，包括以下步骤：

步骤4.1：如图3所示，在主UWB基站复合标定器旁设置部署主激光雷达的十字标记点PR₀，保证十字标记点PR₀的第一标记线LR₁沿矩形工作区域的第一对角线方向，在十字标记点PR₀处精确设置主激光雷达，保证主激光雷达的中心与十字标记点PR₀的中心重合，主激光雷达正面前向与第一标记线LR₁重合，建立主雷达定位坐标系ΣX_L0O_L0Y_L0，如图4所示；

步骤4.2：在第四复合标定器旁设置激光反射柱，通过调节激光反射柱的位置保证激光反射柱的中心点位于主激光雷达正面前向上且到主激光雷达的距离为l _c，利用主激光雷达测定激光反射柱的中心点在主雷达定位坐标系ΣX_L0O_L0Y_L0下的坐标数据(x _s, y _s, α _s)，此时在激光反射柱处设置圆形标记PR₁，如图3所示；

步骤4.3：在圆形标记PR₁处移除激光反射柱，再精确放置第一从激光雷达，保证第一从激光雷达的中心与圆形标记PR₁的圆心重合，第一从激光雷达的正面前向沿矩形工作区域的第一对角线方向，建立第一从雷达定位坐标系ΣX_L1O_L1Y_L1，如图4所示，计算第一从雷达定位坐标系ΣX_L1O_L1Y_L1相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的粗略位姿偏移量T_c(1)为：

(2)

进一步地，所述雷达定位系统粗标定方法还可根据环境特点及雷达角度需求，部署并标定第二从激光雷达及第三从激光雷达，包括以下步骤：

如图3所示，在第二复合标定器旁设置激光反射柱，在十字标记点PR₀处精确调整主激光雷达，保证主激光雷达的中心与十字标记点PR₀的中心重合，主激光雷达正面前向与第一参考方向重合，沿第一参考方向上在从UWB基站附近放置激光反射柱，通过调节激光反射柱的位置保证激光反射柱的中心位于激光雷达正面前向上且到激光雷达的距离为l _a，利用主激光雷达测定激光反射柱的中心点在主雷达定位坐标系ΣX_L0O_L0Y_L0下的坐标数据(x _s2,y _s2, α _s2)，此时在激光反射柱处设置圆形标记PR₂；在圆形标记PR₂处移除激光反射柱，再精确放置第二从激光雷达，保证第二从激光雷达的中心与圆形标记PR₂的圆心重合，第二从激光雷达的正面前向沿矩形工作区域的第二对角线方向，建立第二从雷达定位坐标系ΣX_L2O_L2Y_L2，计算第二从雷达定位坐标系ΣX_L2O_L2Y_L2相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的粗略位姿偏移量T_c(2)为

(3)

如图3所示，在第三复合标定器旁设置激光反射柱，通过调节激光反射柱的位置保证激光反射柱的中心点位于第二从激光雷达正面前向上且到激光雷达的距离为l _b，利用主激光雷达测定激光反射柱的中心点在主雷达定位坐标系ΣX_L0O_L0Y_L0下的坐标数据(x _s3,y _s3, α _s3)，此时在激光反射柱处设置圆形标记PR₃。再在圆形标记PR₃处移除激光反射柱，再精确放置第三从激光雷达，保证第三从激光雷达的中心与圆形标记PR₃的圆心重合，第三从激光雷达的正面前向沿矩形工作区域的第二对角线方向，建立第三从雷达定位坐标系ΣX_L3O_L3Y_L3，计算第三从雷达定位坐标系ΣX_L3O_L3Y_L3相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的粗略位姿偏移量T_c(3)为

(4)

步骤5：雷达定位系统精标定：采用雷达定位系统精标定方法，在工作区域布置n个激光反射柱，通过主、从激光雷达扫描激光反射柱，获得多个激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，获得多个激光反射柱的中心点坐标数据集；根据点云配准算法配准不同中心点坐标数据集，计算第i从雷达定位坐标系ΣX_Li O_Li Y_Li 相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的精确坐标变换矩阵T_a(i)，并存储于定位基站计算机。

进一步地，所述雷达定位系统精标定方法包括以下步骤：

步骤5.1.1：在工作区域随机布置n个激光反射柱，其中n≥3，激光反射柱分布于工作区域中的各个部分，且相互之间无遮挡，保证每个激光雷达都能扫描到每个激光反射柱；

步骤5.1.2：通过主激光雷达扫描激光反射柱，获得n个激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，在主雷达定位坐标系ΣX_L0O_L0Y_L0下获得n个激光反射柱的中心点坐标数据集A₀，并存储于定位基站计算机；

步骤5.1.3：通过第i从激光雷达扫描激光反射柱，获得n个激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，在第i从雷达定位坐标系ΣX_Li O_Li Y_Li 下获得n个激光反射柱的中心点坐标数据集B _i ，并存储于定位基站计算机；

步骤5.1.4：针对第i从激光雷达，利用粗略坐标变换矩阵T_c(i)，将从雷达定位坐标系ΣX_Li O_Li Y_Li 下的数据集B _i ，变换到主雷达定位坐标系ΣX_L0O_L0Y_L0下的数据集A _i ，并存储于定位基站计算机；

步骤5.1.5：在主雷达定位坐标系ΣX_L0O_L0Y_L0下，采用点云配准算法配准数据集A _i 和A₀，获取第i从雷达定位坐标系ΣX_Li O_Li Y_Li 相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的精确坐标变换矩阵T_a(i)，并存储于定位基站计算机。

进一步地，所述点云配准算法包括以下步骤：

步骤5.2.1：对应点关联：假设数据集D_M = { PD _m }，D_T = { PD _t }，m = 1, 2, …,S，t = 1, 2, …, R，S和R分别为数据集D_M和D_T中数据的数量；针对数据集D_M中的任一点PD _m ，选择数据集D_T中离PD _m 的最近点PD _t 为点PD _m 在数据集D_T中的对应点PD _t (m)，完成数据集D_M和D_T之间的对应点关联；

步骤5.2.2：配准变换定义：假设数据集D_M和D_T之间的3×3坐标变换矩阵为T_p，2×2旋转矩阵为R_p，平移列向量为t_p，则存在如下关系：

(5)

步骤5.2.3：误差方程建立：假设数据集D_M = { PD _m }经过T_p变换之后与数据集D_T中对应点PD _t (m)的误差和为E(R_p,t_p)，则构建评价配准效果的误差方程：

(6)

步骤5.2.4：方程优化求解：先求取数据集D_M的质心为p_M，D_T数据集的质心为p_T，可对E(R_p,t_p)做出处理并简化可得：

(7)

再设数据集D_M和数据集D_T中每个数据点的去质心坐标分别为q _m 、q _t ，对应点PD _t (m)的去质心坐标为q _t (m)，则目标旋转矩阵为：

(8)

为求解R_p可定义3×3的矩阵W，并对W进行奇异值分解（SVD）：

，/> (9)

其中，Σ为奇异值组成的对角矩阵，U和V为正交矩阵，当W满秩时解得：

，/> (10)

求得旋转矩阵R_p与平移列向量t_p后，即可按式(7)构造任意两个数据集D_M和D_T之间的坐标变换矩阵。

步骤6：多感知融合定位系统构建：上述UWB定位系统与雷达定位系统共同组成多感知融合定位系统。

步骤7：多感知融合定位系统联合标定：采用多感知融合定位系统联合标定方法，通过主激光雷达分别扫描复合标定器下部的激光反射柱，获得多个激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，获得多个激光反射柱的中心点坐标数据集A_L，存储于定位基站计算机；根据点云配准算法配准数据集A_U和A_L，计算UWB定位坐标系ΣX_uO_uY_u相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的坐标变换矩阵T_c，并存储于定位基站计算机。

进一步地，所述雷达定位系统联合标定方法包括以下步骤：

步骤7.1：通过主激光雷达分别扫描第二、第三、第四复合标定器下部的激光反射柱，获得激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，计算第二、第三、第四复合标定器在坐标系ΣX_L0O_L0Y_L0下的中心点坐标数据集A_L，并存储于定位基站计算机；

步骤7.2：从定位基站计算机中读取第二、第三、第四复合标定器在UWB定位坐标系ΣX_uO_uY_u下的中心点坐标数据集A_U，再读取第二、第三、第四复合标定器在主雷达定位坐标系ΣX_L0O_L0Y_L0下的中心点坐标数据集A_L，采用点云配准算法配准数据集A_U和A_L，计算UWB定位坐标系ΣX_uO_uY_u相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的坐标变换矩阵T_c，并存储于定位基站计算机。

步骤8：多感知融合定位系统坐标统一：针对任一目标点，假设该目标点在坐标系ΣX_uO_uY_u下的坐标为P_u，在坐标系ΣX_L0O_L0Y_L0下的坐标为P_L0，在坐标系ΣX_Li O_Li Y_Li 下的坐标为P_Li ，则建立如下所示的坐标变换模型：

P_L0 = T_a(i) ×P_Li (11)

P_L0 = T_c×P_u (12)。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下还可以做出若干改进，这些改进也应视为本发明的保护范围。

Claims (10)

1.一种多感知融合定位系统构建与坐标标定方法，其特征在于，包括以下步骤：

步骤1：UWB定位系统部署：所述UWB定位系统包括复合标定器，所述复合标定器包括上部的UWB基站、中部的支撑架和下部的激光反射柱，复合标定器上的UWB基站具有主基站和从基站两类模式；根据UWB定位系统部署方法，通过激光雷达扫描复合标定器下部的激光反射柱，获得激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，获得激光反射柱的中心点坐标，进而在工作区域建立由4个复合标定器、定位基站计算机及通信网络组成的UWB定位系统；进而在工作区域建立1个主UWB基站和3个从UWB基站的UWB定位坐标系ΣX_uO_uY_u；

步骤2：UWB定位系统标定：采用UWB定位系统标定方法，获得主UWB基站和从UWB基站在UWB定位坐标系下的坐标数据，形成复合标定器在坐标系ΣX_uO_uY_u下的中心坐标数据集A_U，并存储于定位基站计算机；

步骤3：雷达定位系统部署：采用雷达定位系统部署方法，在复合标定器旁建立由主激光雷达、从激光雷达、定位基站计算机及通信网络组成的雷达定位系统；在工作区域建立主激光雷达的定位坐标系ΣX_L0O_L0Y_L0，以及第i从激光雷达的定位坐标系ΣX_Li O_Li Y_Li ；

步骤4：雷达定位系统粗标定：采用雷达定位系统粗标定方法，以激光反射柱标识从激光雷达的部署位置，通过主激光雷达扫描激光反射柱，获得激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，获得激光反射柱的中心点坐标，进而计算第i从雷达定位坐标系ΣX_Li O_Li Y_Li 相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的粗略坐标变换矩阵T_c(i)，并存储于定位基站计算机；

步骤5：雷达定位系统精标定：采用雷达定位系统精标定方法，在工作区域布置n个激光反射柱，通过主、从激光雷达扫描激光反射柱，获得多个激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，获得多个激光反射柱的中心点坐标数据集；根据点云配准算法配准主、从激光雷达所扫描的中心点坐标数据集，计算第i从雷达定位坐标系ΣX_Li O_Li Y_Li 相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的精确坐标变换矩阵T_a(i)，并存储于定位基站计算机；

步骤6：多感知融合定位系统构建：上述UWB定位系统与雷达定位系统共同组成多感知融合定位系统；

步骤7：多感知融合定位系统联合标定：采用多感知融合定位系统联合标定方法，通过主激光雷达分别扫描复合标定器下部的激光反射柱，获得多个激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，获得多个激光反射柱的中心点坐标数据集A_L，存储于定位基站计算机；根据点云配准算法配准数据集A_U和A_L，计算UWB定位坐标系ΣX_uO_uY_u相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的坐标变换矩阵T_c，并存储于定位基站计算机；

步骤8：多感知融合定位系统坐标统一：针对任一目标点，假设该目标点在坐标系ΣX_uO_uY_u下的坐标为P_u，在坐标系ΣX_L0O_L0Y_L0下的坐标为P_L0，在坐标系ΣX_Li O_Li Y_Li 下的坐标为P_Li ，则建立如下所示的坐标变换模型：

P_L0 = T_a(i) ×P_Li (1)

P_L0 = T_c×P_u (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基站位置为原点O_u，以第一参考方向作为X_u轴，选取经过原点、垂直于X_u轴并指向另外两个从UWB基站的方向作为Y_u轴，建立UWB定位坐标系ΣX_uO_uY_u。

5. 根据权利要求1所述的多感知融合定位系统构建与坐标标定方法，其特征在于，所述UWB定位系统标定方法是通过建立并求解相对距离与从UWB基站坐标间的几何方程组F_G来获得UWB基站的坐标数据；主UWB基站为原点O_u，通过主从UWB基站测距，获得X_u轴上的第二从UWB基站到主UWB基站的距离为d₀，则所述从UWB基站的坐标为(d₀, 0)；第三、第四从UWB基站到主UWB基站的距离分别测定为d_3,1和d_4,1，到X_u轴上的第二从UWB基站距离分别测定为d_3,2和d_4,2，第三、第四从UWB基站的坐标分别为(x ₃, y ₃)和(x ₄, y ₄)，则几何方程组F_G为

(3)

通过求解几何方程组F_G获得第三、第四从UWB基站在坐标系ΣX_uO_uY_u下的坐标数据，而主UWB基站与从UWB基站的坐标也即复合标定器在坐标系ΣX_uO_uY_u下的中心点坐标，以此建立复合标定器的中心点坐标数据集A_U。

6.根据权利要求1所述的多感知融合定位系统构建与坐标标定方法，其特征在于，所述雷达定位系统粗标定方法与雷达定位系统部署方法交叉融合进行，包括以下步骤：

步骤4.1：在第一复合标定器旁设置主激光雷达，保证主激光雷达正面前向与第一对角线方向重合；选定主激光雷达中心作为主雷达定位坐标系的原点O_L0，过原点O_L0且沿激光雷达正面前向为X_L0轴，垂直于X _L0轴向左作射线为Y_L0轴，建立主雷达定位坐标系ΣX_L0O_L0Y_L0；

步骤4.2：在第四复合标定器旁设置激光反射柱，以激光反射柱标识第一从激光雷达的部署位置，通过主激光雷达扫描激光反射柱，获得激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，计算激光反射柱的中心点坐标；

步骤4.3：通过调节激光反射柱的位置保证激光反射柱的中心点位于主激光雷达正面前向上且到主激光雷达的距离为l _c；利用主激光雷达测定激光反射柱的中心点在主雷达定位坐标系ΣX_L0O_L0Y_L0下的坐标数据(x _s, y _s, α _s)，在此时激光反射柱处放置第一从激光雷达，保证第一从激光雷达正面前向与第一对角线方向重合；

步骤4.4：选定第一从激光雷达中心作为从雷达定位坐标系的原点O_L1，过原点O_L1且沿激光雷达正面前向为X_L1轴，垂直于X _L1轴向左作射线为Y_L1轴，建立从雷达定位坐标系ΣX_L1O_L1Y_L1；

步骤4.5：针对第一从激光雷达建立第一从雷达定位坐标系ΣX_L1O_L1Y_L1，计算第一从雷达定位坐标系ΣX_L1O_L1Y_L1相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的粗略位姿偏移量T_c(1)为：

(4)。

7.根据权利要求6所述的多感知融合定位系统构建与坐标标定方法，其特征在于，所述雷达定位系统粗标定方法还可根据环境特点及雷达角度需求，部署并标定第二从激光雷达及第三从激光雷达。

8.根据权利要求1所述的多感知融合定位系统构建与坐标标定方法，其特征在于，所述雷达定位系统精标定方法包括以下步骤：

步骤5.1.1：在工作区域随机布置n个激光反射柱，其中n≥3，激光反射柱分布于工作区域中的各个部分，且相互之间无遮挡，保证每个激光雷达都能扫描到每个激光反射柱；

步骤5.1.2：通过主激光雷达扫描激光反射柱，获得n个激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，在主雷达定位坐标系ΣX_L0O_L0Y_L0下获得n个激光反射柱的中心点坐标数据集A₀，并存储于定位基站计算机；

步骤5.1.3：通过第i从激光雷达扫描激光反射柱，获得n个激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，在第i从雷达定位坐标系ΣX_Li O_Li Y_Li 下获得n个激光反射柱的中心点坐标数据集B _i ，并存储于定位基站计算机；

步骤5.1.4：针对第i从激光雷达，利用粗略坐标变换矩阵T_c(i)，将从雷达定位坐标系ΣX_Li O_Li Y_Li 下的数据集B _i ，变换到主雷达定位坐标系ΣX_L0O_L0Y_L0下的数据集A _i ，并存储于定位基站计算机；

步骤5.1.5：在主雷达定位坐标系ΣX_L0O_L0Y_L0下，采用点云配准算法配准数据集A _i 和A₀，获取第i从雷达定位坐标系ΣX_Li O_Li Y_Li 相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的精确坐标变换矩阵T_a(i)，并存储于定位基站计算机。

9.根据权利要求1所述的多感知融合定位系统构建与坐标标定方法，其特征在于，所述点云配准算法包括以下步骤：

步骤5.2.1：对应点关联：假设数据集D_M = { PD _m }，D_T = { PD _t }，m = 1, 2, …, S，t= 1, 2, …, R，S和R分别为数据集D_M和D_T中数据的数量；针对数据集D_M中的任一点PD _m ，选择数据集D_T中离PD _m 的最近点PD _t 为点PD _m 在数据集D_T中的对应点PD _t (m)，完成数据集D_M和D_T之间的对应点关联；

步骤5.2.2：配准变换定义：假设数据集D_M和D_T之间的3×3坐标变换矩阵为T_p，2×2旋转矩阵为R_p，平移列向量为t_p，则存在如下关系：

(5)

步骤5.2.3：误差方程建立：假设数据集D_M = { PD _m }经过T_p变换之后与数据集D_T中对应点PD _t (m)的误差和为E(R_p,t_p)，则构建评价配准效果的误差方程：

(6)

步骤5.2.4：方程优化求解：先求取数据集D_M的质心为p_M，D_T数据集的质心为p_T，可对E(R_p,t_p)做出处理并简化可得：

(7)

再设数据集D_M和数据集D_T中每个数据点的去质心坐标分别为q _m 、q _t ，对应点PD _t (m)的去质心坐标为q _t (m)，则目标旋转矩阵为：

(8)

为求解R_p可定义3×3的矩阵W，并对W进行奇异值分解（SVD）：

，/> (9)

其中，Σ为奇异值组成的对角矩阵，U和V为正交矩阵，当W满秩时解得：

，/> (10)

求得旋转矩阵R_p与平移列向量t_p后，即可按式(5)构造任意两个数据集D_M和D_T之间的坐标变换矩阵。

10.根据权利要求1所述的多感知融合定位系统构建与坐标标定方法，其特征在于，所述雷达定位系统联合标定方法包括以下步骤：

步骤7.1：通过主激光雷达分别扫描第二、第三、第四复合标定器下部的激光反射柱，获得激光反射柱的轮廓数据；利用激光反射柱中心点计算方法，计算第二、第三、第四复合标定器在坐标系ΣX_L0O_L0Y_L0下的中心点坐标数据集A_L，并存储于定位基站计算机；

步骤7.2：从定位基站计算机中读取第二、第三、第四复合标定器在UWB定位坐标系ΣX_uO_uY_u下的中心点坐标数据集A_U，再读取第二、第三、第四复合标定器在主雷达定位坐标系ΣX_L0O_L0Y_L0下的中心点坐标数据集A_L，采用点云配准算法配准数据集A_U和A_L，计算UWB定位坐标系ΣX_uO_uY_u相对于主雷达定位坐标系ΣX_L0O_L0Y_L0的坐标变换矩阵T_c，并存储于定位基站计算机。