基于多感知数据融合的多机器人接近对准定位控制方法

本发明属于智能制造与机器人，具体涉及一种基于多感知数据融合的多机器人接近对准定位控制方法。

背景技术：

1、随着机器人智能化的发展，机器人化智能制造将成为智能制造的主攻方向。随着航空航天制造业的快速发展，移动机器人所面临的问题也更加复杂，具体包括以下问题：

2、1.大型部件的尺寸大、重量大，对移动机器人的尺寸，载重能力要求更高。

3、2.大部件及小部件的多种类，多批次生产模式对移动机器人的协同性，易于重构性要求更高。

4、3.长时间的工作需求，复杂的工厂环境对移动机器人的鲁棒性要求更高。

5、针对以上问题，主要有两种解决方案。一是研发相应的重载移动机器人，但由于重载移动机器人尺寸较大因此灵活性不足。二是将移动机器人编组进行协同搬运，既可以协同搬运大型部件，也可以离组单独搬运小型部件。但机器人集群之间存在相互遮挡现象，对定位与控制要求较高。

6、移动机器人定位与控制技术是多移动机器人协同的关键技术。目前，用于移动机器人定位的传感器包括全球定位系统(gps)、超宽带(uwb)、惯性测量单元(imu)、以及激光雷达等。在室外环境中，gps能够提供高精度的位置数据，然而，在室内使用时，其信号会衰减甚至完全丧失。uwb通常用于室内定位场景，可以实现绝对定位，但其数据存在较大的噪声，因此无法实现高精度的定位。imu通常用于惯性导航定位系统，可获取目标的运动状态信息，但其误差会随着时间的推移而逐渐累积，最终导致定位失效。虽然激光雷达的定位精度相对较高，但其存在近距离盲区，并容易受到遮挡的影响，导致目标跟踪失效或定位不准确。且造价成本高。如公开号为cn116038713a的中国发明专利提出了一种保密环境下的无硬连接的多移动机器人协同搬运方法，通过激光测距雷达跟踪的方式实现协同搬运问题。然而当环境受到干扰或存在机器人集群之间的相互遮挡现象时，激光雷达在地面端定位方面的性能表现不佳。

技术实现思路

1、本发明的目的在于解决上述背景技术中所提出的问题，首先将多机器人系统根据移动机器人运行功能分为主移动机器人和从移动机器人，将多机器人编组集结分为快速接近、静态对准、动态跟随三个运行阶段；其次部署导航激光雷达全局定位系统、辅助激光雷达接近定位系统、激光测距传感器阵列相对定位系统以及编码器/imu自定位系统；再次，对上述定位系统获得多传感器数据进行处理获得移动机器人的位姿数据；再次，根据主从机器人运行阶段，采用与运行阶段相适应的数据融合方法；再次，设计了backstepping运动学控制器与自适应滑模动力学控制器；最后，根据运行阶段自适应调整运动学控制器和动力学控制器的控制参数，并使用多感知融合数据实现对移动机器人的定位与控制，本发明所设计的基于多感知数据融合的多机器人接近对准定位控制方法，有效的提高了在多种类、多尺寸部件的搬运中，多机器人定位与控制的精度和鲁棒性，提供一种基于多感知数据融合的多机器人接近对准定位控制方法；

2、为了实现上述目的，本发明采用以下技术方案：一种基于多感知数据融合的多机器人接近对准定位控制方法，包括：

3、步骤1：多机器人系统设计：根据移动机器人运行功能将多机器人系统中移动机器人分为主移动机器人和从移动机器人，且多机器人系统中移动机器人进行编组集结过程包括快速接近、静态对准以及动态跟随三个运行阶段；

4、步骤2：传感检测系统部署：在移动机器人端和地面端部署导航激光雷达全局定位系统，在移动机器人端部署辅助激光雷达接近定位系统、激光测距传感器阵列相对定位系统以及编码器/imu自定位系统；

5、步骤3：多传感器数据处理：在快速接近阶段，基于导航激光雷达全局定位数据处理和编码器/imu自定位数据处理，得到移动机器人的第一测量全局位姿和第一测量全局位姿变化量；

6、在静态对准阶段，基于辅助激光雷达接近定位数据处理和激光测距传感器阵列相对定位数据处理，得到从移动机器人相对主移动机器人的第一测量相对位姿偏差和第一测量相对位置偏差；

7、在动态跟随阶段，基于导航激光雷达全局定位数据处理和编码器/imu自定位数据处理，得到移动机器人的第二测量全局位姿和第二测量全局位姿变化量，同时，基于辅助激光雷达接近定位数据处理和激光测距传感器阵列相对定位数据处理，得到从移动机器人相对主移动机器人的第二测量相对位姿偏差和第二测量相对位置偏差；

8、步骤4：多感知数据融合定位：在快速接近阶段，将步骤3得到第一测量全局位姿和第一测量全局位姿变化量进行融合，得到第一偏差输入；

9、在静态对准阶段，将步骤3得到第一测量相对位姿偏差和第一测量相对位置偏差进行融合，得到第二偏差输入；

10、在动态跟随阶段，将步骤3得到第二测量全局位姿和第二测量全局位姿变化量进行融合，得到第三偏差输入，同时将步骤3得到第二测量相对位姿偏差和第二测量相对位置偏差进行融合，得到第四偏差输入；

11、步骤5：自适应接近对准控制：在快速接近阶段，采用自适应反演滑模控制处理第一偏差输入，得到快速接近阶段的控制信号，在静态对准阶段，采用自适应反演滑模控制处理第二偏差输入，得到静态对准阶段的控制信号，在动态跟随阶段，主移动机器人和从移动机器人分别采用自适应反演滑模控制处理第三偏差输入和第四偏差输入，得到动态跟随阶段的控制信号；

12、步骤6：控制器参数自调整：判断移动机器人运行阶段，根据运行阶段自适应调整步骤5中自适应反演滑模控制的控制参数。

13、进一步地，所述传感检测系统部署包括：

14、步骤2.1：导航激光雷达全局定位系统包括导航激光雷达和激光反射柱，在所有移动机器人前端部署导航激光雷达，在地面端以不对称原则部署多个激光反射柱作为移动机器人定位的全局参考地标，通过激光导航雷达扫描地面端激光反射柱，记录每个激光反射柱的全局位置坐标，以激光反射柱为全局参考地标建立全局定位坐标系与全局定位地图，完成导航激光雷达测量坐标系与移动机器人运动控制坐标系的坐标标定，获得第一坐标变换矩阵co；

15、步骤2.2：辅助激光雷达接近定位系统包括两个辅助激光雷达，在所有移动机器人对角位置安装两个辅助激光雷达，保证两辅助激光雷达扫描范围的中线与两辅助激光雷达中心之间的连线重合，完成辅助激光雷达测量坐标系与移动机器人运动控制坐标系的坐标标定，获得第二坐标变换矩阵ca；

16、步骤2.3：在移动机器人第一侧面中心部署非等腰直角三角形定位块，所述非等腰直角三角形定位块通过长直角边安装于移动机器人第一侧面，所述长直角边长度l1与传感器间距d之间具有d-tl≤l1＜d关系，tl为长度容差，长直角边与斜边之间的长边夹角为α，所述长边夹角α指向移动机器人前进方向，第一侧面中心的三角定位块高度为再在所有移动机器人第二侧面部署激光测距传感器阵列，所述激光测距传感器阵列包括第一激光测距传感器、第二激光测距传感器以及第三激光测距传感器，按间距d进行等间距排列形成传感器阵列，传感器阵列中间的第二激光测距传感器安装于移动机器人第二侧面的中点位置，第一激光测距传感器位于移动机器人前进方向一侧，第三激光测距传感器位于移动机器人后退方向一侧，由从移动机器人的激光测距传感器阵列和主移动机器人的直角三角形定位块构成上述主-从移动机器人的激光测距传感器阵列相对定位系统，完成激光测距传感器阵列测量坐标系与辅助激光雷达测量坐标系之间的坐标标定，获得第三坐标变换矩阵cl；

17、步骤2.4：编码器/imu自定位系统包括编码器和imu传感器，将编码器安装于移动机器人的车轮轴端，将imu传感器安装于移动机器人中心。

18、进一步地，所述导航激光雷达全局定位数据处理包括：

19、步骤a1：导航激光雷达数据采集：通过导航激光雷达扫描激光反射柱，在导航激光雷达极坐标系下获取离雷达最近的三个激光反射柱的极坐标位置并转化为笛卡尔坐标系下的直角坐标位置(xa,ya)、(xb,yb)、(xc,yc)；

20、步骤a2：三边定位全局位姿计算：在导航激光雷达笛卡尔坐标系下，根据三边定位原理计算移动机器人的全局位姿pd(xd,yd,θd)，其计算过程为：

21、

22、

23、进一步地，所述辅助激光雷达接近定位数据处理包括：

24、步骤b1：原始点云数据获取：从移动机器人使用辅助激光雷达采集主移动机器人的原始点云数据；

25、步骤b2：点云区域分割：以从移动机器人几何中心为原点，将半径为r的圆形区域作为辅助激光雷达接近定位的搜索区域；

26、步骤b3：最小矩形拟合：在从移动机器人辅助激光雷达的测量坐标系下，使用包围盒几何特征检测算法对主移动机器人的激光点云数据进行最小矩形拟合，确定主移动机器人两直角边交点在辅助激光雷达测量坐标系下的直角交点坐标；

27、步骤b4：相对位姿计算：根据主移动机器人的直角交点坐标计算从移动机器人相对于主移动机器人的相对位姿偏差

28、进一步地，所述激光测距传感器阵列相对定位数据处理包括：

29、步骤c1：传感器阵列数据采集：分别利用从移动机器人上的第一激光测距传感器、第二激光测距传感器、第三激光测距传感器采集从移动机器人相对于主移动机器人的测距数据d1、d2、d3；

30、步骤c2：机器人相对位姿状态分类：设距离容差为td，比较测距数据d1、d2、d3的差值，根据比较结果得到移动机器人之间相对位姿状态，具体包括：

31、若d1-td≤d2≤d1+td，则移动机器人之间相对位姿状态为第三传感器定位状态；

32、若d1-td≤d3≤d1+td，则移动机器人之间相对位姿状态为第二传感器定位状态；

33、若d2-td≤d1≤d2+td，则移动机器人之间相对位姿状态为第一传感器定位状态；

34、步骤c3：机器人相对位姿计算：根据移动机器人之间相对位姿状态计算从移动机器人相对于主移动机器人的相对位置偏差具体包括：

35、当移动机器人相对位姿状态为第三传感器定位状态：计算侧向相对位姿偏差为纵向相对位姿偏差为得到从移动机器人相对于主移动机器人的相对位置偏差

36、当移动机器人相对位姿状态为第二传感器定位状态：计算侧向相对位姿偏差为纵向相对位姿偏差为得到从移动机器人相对于主移动机器人的相对位置偏差

37、当移动机器人相对位姿状态为第一传感器定位状态：计算侧向相对位姿偏差为纵向相对位姿偏差为得到从移动机器人相对于主移动机器人的相对位置偏差

38、进一步地，所述编码器/imu自定位数据处理包括：

39、步骤d1：编码器/imu数据获取：使用编码器获得编码器脉冲数据：单圈总脉冲数为c，在时间t0内统计到的编码器脉冲数m0。以固定频率f2查询imu数据，获得机器人在第i个周期内沿x轴方向的加速度axi和沿y轴方向的加速度ayi以及i时刻机器人的航向角θi；

40、步骤d2：建立编码器位姿计算模型：建立机器人的运动学模型，根据编码器脉冲数据计算轮子转速和根据机器人的运动学模型计算机器人速度变化量(δvxi,δvyi)；

41、步骤d3：建立imu位姿计算模型：对传感器数据进行处理获得机器人在n时刻相较于起始点的速度变化量(δvxi,δvyi)和角度变化量δθi，其公式为：

42、

43、δθi＝θn-θ1

44、步骤d4：编码器/imu数据融合：采用扩展卡尔曼滤波算法对编码器模型获得的速度数据和imu模型获得的速度数据进行融合，获得机器人的可靠速度变化量(δvxe,δvye)，并结合编码器/imu查询频率f2计算机器人位移增量(δxe,δye)，其公式为；

45、

46、步骤d5：位姿变化量计算：编码器/imu融合位姿变化量(δxe,δye,δθe)通过坐标变换，将自身坐标系变换为全局坐标系，获得移动机器人全局位姿变化量δpe(δxe,δye,δθe)。

47、进一步地，所述步骤4具体包括：

48、在快速接近阶段，以频率f1获取第一测量全局位姿，以频率f2获取第一测量全局位姿变化量，利用扩展卡尔曼滤波法融合第一测量全局位姿与第一测量全局位姿变化量，获得所有移动机器人的第一全局位姿偏差数据，通过坐标变换矩阵co将第一全局位姿偏差数据变换到移动机器人运动控制坐标系下，得到第一偏差输入；

49、在静态对准阶段，以频率f3获取从移动机器人相对于主移动机器人的第一测量相对位姿偏差，以频率f4获取从移动机器人相对于主移动机器人的第一测量相对位置偏差，利用扩展卡尔曼滤波法融合第一测量相对位姿偏差与第一测量相对位置偏差，获得从移动机器人相对主移动机器人的第一相对位姿数据，通过坐标变换矩阵ca将第一相对位姿数据变换到移动机器人运动控制坐标系下，得到第二偏差输入；

50、在动态跟随阶段，针对主移动机器人，以频率f1获取主移动机器人的第二测量全局位姿，以频率f2获取主移动机器人的第二测量全局位姿变化量，利用扩展卡尔曼滤波法融合第二测量全局位姿与第二测量全局位姿变化量，获得主移动机器人的第二全局位姿偏差数据，并通过坐标变换矩阵co将第二全局位姿偏差数据变换到移动机器人运动控制坐标系下，得到第三偏差输入；

51、同时，针对从移动机器人，以频率f4获取从移动机器人相对于主移动机器人的第二测量相对位姿偏差，以频率f3获取从移动机器人相对于主移动机器人的第二测量相对位置偏差，利用扩展卡尔曼滤波法融合第二测量相对位姿偏差与第二测量相对位置偏差，获得从移动机器人相对主移动机器人的第二相对位姿数据，并通过坐标变换矩阵ca将第二相对位姿数据变换到移动机器人运动控制坐标系下，得到第四偏差输入。

52、进一步地，所述自适应接近对准控制包括：

53、步骤e1：基于运动学模型对backstepping运动学控制器进行设计，其表达式为：

54、

55、式中为机器人沿xl轴方向的速度，为机器人沿yl轴方向的速度，为机器人沿φl轴方向的旋转速度，k1、k2、k3为backstepping运动学控制器的可调整参数，为从机器人在主移动机器人运动控制坐标系下的偏差输入，分别为从机器人在世界坐标系下沿xw轴、yw轴方向的速度，为从机器人在主机器人局部坐标系下绕φl轴期望旋转速度，uc是控制器的输出；

56、步骤e2：构建移动机器人逆动力学模型：对移动机器人动力学模型求逆得到移动机器人逆动力学模型。

57、步骤e3：基于动力学模型对自适应滑模动力学控制器进行设计，其表达式为：

58、

59、自适应率设计为：

60、

61、式中p为切换函数的配置参数，c为滑模面的设计参数，为系统自适应律，σ为系统切换函数，r、β、a为自适应滑模动力学控制器的可调整参数，z1为机器人轮子的转角误差，为机器人轮子的转速误差，z2为滑模面，θd为机器人轮子的期望转角，为机器人轮子的期望转速，为机器人轮子的期望转加速度，mn为移动机器人转动惯量，μ为轮子与地面间的摩擦系数，u2为轮子的输入转矩。

62、进一步地，所述控制器参数自调整包括：

63、步骤6.1：控制器初始化：给定控制器参数初始值，其中k1、k2、k3为正实数，r、β、a为正对角矩阵，再设计移动机器人运行阶段估计器，接着设计移动机器人运动状态估计器。

64、步骤6.2：根据移动机器人运行阶段估计器判断机器人运行阶段；

65、若为接近阶段，则控制器参数调整为：增大k1、k2、k3的值到初始值的m1(m1>1)倍，减小r矩阵对角元素到初始值的n1(n1<1)倍，增大β矩阵对角元素的值到初始值的m2(m2>1)倍，a矩阵的值不变，基于移动机器人运动状态估计器对所有移动机器人运动状态进行判断，若所有移动机器人运动平稳，则继续以步长p1(p1>0)增大k1、k2、k3，减小r矩阵对角元素，增大β矩阵对角元素，若所有移动机器人开始发生摆动，则以步长p2(p1>p2>0)减小k1、k2、k3，增大r矩阵对角元素，减小β矩阵对角元素，直至所有移动机器人恢复平稳；

66、若为静态对准阶段，则控制器参数调整为：减小k1、k2、k3的值到初始值的n2(n2<1)倍，减小β矩阵对角元素到初始值的n3(n3<1)倍，增大r矩阵对角元素的值到初始值的m3(m3>1)倍，a矩阵的值不变，基于移动机器人运动状态估计器对所有移动机器人运动状态进行判断，若从移动机器人运动平稳，则继续以步长p1(p1>0)减小k1、k2、k3，减小β矩阵对角元素，增大r矩阵对角元素，若从移动机器人开始发生摆动，则以步长p2(p1>p2>0)增大k1、k2、k3，增大β矩阵对角元素，减小r矩阵对角元素，直至从移动机器人恢复平稳；

67、若为动态跟随阶段，则控制器参数调整为：增大a矩阵对角元素的值到初始值的m4(m4>1)倍，同时保持其他参数与静态对准阶段控制器参数一致，基于移动机器人运动状态估计器对所有移动机器人运动状态进行判断，若所有移动机器人运动平稳，则继续以步长p1(p1>0)增大a矩阵对角元素，若所有移动机器人开始发生摆动，则以步长p2(p1>p2>0)减小a矩阵对角元素，直至所有移动机器人恢复平稳。

68、进一步地，所述控制器初始化包括：

69、步骤6.1.1：给定控制器参数初始值，其中k1、k2、k3为正实数，r、β、a为正对角矩阵；

70、步骤6.1.2：设计移动机器人运行阶段估计器：在全局坐标系下设定集结点p0，以p0为中心以r0为半径设定为集结区域，将移动机器人靠近集结区域的过程定义为快速接近阶段，将移动机器人进入集结区域后，主移动机器人在集结点p0停止、从移动机器人获得与主移动机器人相对位姿期望值的过程定义为静态对准阶段，将静态对准后从移动机器人跟随主移动机器人运动的过程定义为动态跟随阶段；

71、步骤6.1.3：设计移动机器人运行状态估计器：将移动机器人在稳态时间t0内，偏差不超过e0定义为移动机器人运动平稳，将移动机器人在稳态时间t0内，偏差大于e0定义为移动机器人发生摆动。

72、有益效果：本发明针对多机器人编组集结的快速接近、静态对准、动态跟随三个运行阶段，部署了导航激光雷达全局定位系统、辅助激光雷达接近定位系统、激光测距传感器阵列相对定位系统以及编码器/imu自定位系统，并利用扩展卡尔曼滤波方法针对三个阶段进行数据融合。在有利发挥了各传感器的优点的同时显著提高了系统的准确性与可靠性。针对多机器人快速接近、静态对准、动态跟随三个运动阶段设计了自适应反演滑模控制器，并根据不同运行阶段自适应调节控制器参数，有效提高了多机器人在编组集结与协同运动过程中的准确性与抗干扰性，推动了移动机器人集群定位与控制技术的发展。