一种空中机器人自主协助地面盲车集群导航的方法

本发明属于多机器人协同领域，尤其涉及一种空中机器人自主协助地面盲车集群导航的方法。

背景技术：

1、如今，盲无人车和空中机器人被广泛应用于许多领域，如监视、农业、搜救、运输等领域，几乎都依赖于各种外部传感器来感知环境，如激光雷达、摄像头、雷达等。感知能力是在障碍物密集的未知区域实现自主导航的关键功能之一，也是后续导航模块(如建图、规划和高层策略)的条件。对于一个感知受限的机器人来说，安全导航是一项挑战，尤其是当它缺乏探测环境的传感器时，就会出现本发明所描述的“盲”机器人。此外，由于缺乏感知，当机器人群组存在潜在的机器人间碰撞时，导航就会变得更加困难。当机器人群的规模增大时，为所有机器人配备昂贵的感知或slam系统的成本会很高。盲无人车具有低成本、高负载能力和低能耗等显著优势。由于这些优势，盲无人车经常被用作任务执行单元，并以蜂群的形式工作，例如多机器人协同运输。为所有机器人配备传感器不仅成本高昂，而且地面视角也无法充分利用这些昂贵的传感器。而空中机器人在三维空间中具有超强的机动性，因此有更大的感知范围。将它们的优势结合起来，形成一个由盲无人车和空中机器人组成的协作系统，可以低成本高效率地完成高要求的任务。

2、对异构机器人系统，尤其是空地协作机器人系统，已有大量研究。大量研究集中于结合两种机器人协同执行任务，如探索和重建，充分利用每种机器人的互补优势。这些研究旨在解决不同类型机器人在整体性能和能力方面固有的局限性。

3、一些研究涉及利用空中机器人协助盲无人车移动，利用空中机器人更宽广的视场为盲无人车提供更多环境信息。《j.delmerico,e.mueggler,j.nitsch,and d.scaramuzza,“active autonomous aerial exploration for ground robot path planning,”ieeerobotics and automation letters,vol.2,no.2,pp.664-671,2017.》和《j.peterson,h.chaudhry,k.abdelatty,j.bird,and k.kochersberger,“online aerial terrainmapping for ground robot navigation,”sensors,vol.18,no.2,p.630,2018.》将图像组装成正交马赛克，然后使用经典计算机视觉方法对其进行分类，为盲无人车生成初始轨迹。《i.d.miller,f.cladera,t.smith,c.j.taylor,and v.kumar,“stronger together:air-ground robotic collaboration using semantics,”ieee robotics and automationletters,vol.7,no.4,pp.9643-9650,2022.》利用航空和地面之间的语义信息实现相对位姿估计，并与多盲无人车实时运行。这种协作使盲无人车能够从空中机器人的空中视角中获益，收集更全面的态势感知。

4、有些研究去掉了盲无人车携带的环境感知传感器，利用空中机器人与盲无人车之间的相对位姿估计来获得盲无人车的定位。《m.cognetti,g.oriolo,p.peliti,l.rosa,andp.stegagno,“cooperative control of a heterogeneous multi-robot system basedon relative localization,”in 2014ieee/rsj international conference onintelligent robots and systems.ieee,2014,pp.350-356.》使用一个带相机的空中机器人为多个盲无人车提供定位，并开发了一种协同控制方案，使盲无人车处于相机视场内。《f.gu′erin,f.guinand,j.-f.breth′e,h.pelvillain et al.,“uavugv cooperation forobjects transportation in an industrial area,”in 2015ieee internationalconference on industrial technology(icit).ieee,2015,pp.547-552.》使用空中机器人上的视觉舵机使领头的盲无人车保持在图像平面的中心，以实现盲无人车的定位，导航航点由人类操作员给出。《e.mueggler,m.faessler,f.fontana,and d.scaramuzza,“aerialguided navigation of a ground robot among movable obstacles,”in2014ieee international symposium on safety,security,and rescue robotics(2014).ieee,2014,pp.1-8.》用空中机器人的摄像头可以检测到的二维码对盲无人车进行定位，以补偿盲无人车车轮里程计的漂移。这些方法在盲无人车上安装视觉设备为盲无人车提供关键的定位信息，这需要空中机器人的持续观测。

5、这不仅效率低下，而且限制了空中机器人的机动性，然而如果没有观测，盲无人车的定位就会面临漂移和不确定性。

6、在盲车导航中，针对盲无人车，本发明设计了一种严格安全的路径规划策略，其中包括基于不确定性传播的轨迹生成和碰撞预测；针对空中机器人，本发明设计了一种制定为带时间窗的车辆路径规划的调度方法，以引导空中机器人为盲无人车提供支持。

技术实现思路

1、本技术实施例的目的是提供一种空中机器人自主协助地面盲车集群导航的方法，针对盲无人车，设计了一种严格安全的路径规划策略，其中包括基于不确定性传播的轨迹生成和碰撞预测；针对空中机器人，设计了一种制定为带时间窗的车辆路径规划的调度方法，以引导空中机器人为盲无人车提供支持。

2、根据本技术实施例的第一方面，提供一种空中机器人自主协助地面盲车集群导航的方法，包括：

3、(1)空中机器人通过激光雷达slam获取空中机器人里程计和地图，并与盲无人车进行相对位姿估计；

4、(2)盲无人车获取自身的里程计，结合空中机器人里程计和相对位姿估计结果，利用扩展卡尔曼滤波预测自身的位姿信息；

5、(3)盲无人车基于空中机器人共享的地图，规划自身的轨迹并预测因里程计估计的不确定性和地图中的未知区域而发生的碰撞，将预测到的碰撞信息发送至空中机器人；

6、(4)空中机器人接收所有盲无人车发送的碰撞信息，基于带时间窗的车辆路径规划方法规划自身的飞行路径，为可能发生碰撞的盲无人车提供新的相对位姿估计和地图信息，以使得盲无人车重新进行轨迹规划，避免碰撞。

7、进一步地，步骤(2)中，盲无人车利用扩展卡尔曼滤波的预测模型，根据自身的里程计来预测自身的位姿信息；

8、若盲无人车与空中机器人之间的距离在可进行相对位姿估计的范围内，则盲无人车与空中机器人进行相对位姿估计，并利用扩展卡尔曼滤波的观测模型，根据空中机器人里程计和相对位姿估计结果，得到盲无人车位姿信息的测量值，并对盲无人车利用扩展卡尔曼滤波预测模型得到的位姿信息进行修正。

9、进一步地，扩展卡尔曼滤波的预测模型为

10、

11、其中，p为盲无人车在世界坐标系中的位置，θ为盲无人车的偏航角，v为盲无人车的速度，ω为盲无人车的角速度，δt为预测步骤的时间间隔，r{θ}为盲无人车的旋转矩阵；

12、扩展卡尔曼滤波预测步骤为

13、

14、其中f为预测模型f(x,u)对x的雅可比矩阵，b为f(x,u)对u的雅可比矩阵，p为x的协方差矩阵，q为控制噪声的协方差矩阵。

15、进一步地，扩展卡尔曼滤波的观测模型为

16、

17、wrg＝wraarg

18、wpg＝wpa+wraapg

19、其中，wrg和wpg是盲无人车在世界坐标系中的旋转矩阵和位置，wra和wpa是空中机器人在世界坐标系中的旋转矩阵和位置、arg和apg是空中机器人在盲无人车系中的旋转矩阵和位置，w、a、g分别是世界坐标系，空中机器人机体坐标系，盲无人车机体坐标系，x{p}表示只取三维位置的x轴分量，y{p}表示只取三维位置的y轴分量，yaw{r}表示只取三维旋转的偏航角；

20、扩展卡尔曼滤波的观测步骤为

21、

22、pk|k＝(i-kkhk)pk|k-1

23、其中h为观测模块h(x)对x的雅可比矩阵，k为卡尔曼增益矩阵，r为观测噪声的协方差矩阵。

24、进一步地，利用扩展卡尔曼滤波来表示盲无人车位姿估计的不确定性，通过扩展卡尔曼滤波的预测步骤沿规划轨迹按一定距离步长计算预测步骤，以传播盲无人车的不确定性。

25、进一步地，步骤(3)中，在碰撞地图中进行基于不确定性的碰撞预测，其中所述碰撞地图为将未知栅格视为占用栅格的地图，碰撞预测过程为：

26、计算扩展卡尔曼滤波协方差矩阵的特征值和特征向量

27、q-1pxyq＝λ＝diag(λ1,λ2)

28、q＝[v1v2]

29、其中，pxy是扩展卡尔曼滤波在xy平面上的协方差矩阵，λ是pxy的特征值，v1和v2是pxy的特征向量，λ1、λ2分别为协方差矩阵的第一大特征值、第二大特征值，q为控制噪声的协方差矩阵；

30、计算3σ椭圆

31、

32、其中a和b是椭圆的半主轴和半次轴的长度，p是在椭圆中的栅格的位置，pg是盲无人车的位置；

33、沿着规划的轨迹遍历椭圆所占据的栅格，并找到盲无人车的碰撞椭圆中心ppc和碰撞时间tpc，选择ppc按照规划速度方向偏移预定距离的预期支持点pps，将pps和tpc合并为碰撞信息。

34、进一步地，步骤(4)包括：

35、对于接收到的n个碰撞点，空中机器人计算时间成本矩阵mvrptw和时间窗矩阵tvrptw：

36、

37、mvrptw(k，0)＝0

38、tvrptw(k，0)＝0

39、

40、其中，pa是空中机器人的位置，tpc为盲无人车的预测碰撞时间，pps为盲无人车的预期支持点，是指从到的时间成本；

41、考虑空中机器人速度的连续性，通过以下方式计算空中机器人的所述时间成本：

42、pdis＝pa-ppsk，vtan＝pdis/||pdis||·va

43、disa＝(vmax2-vd2)/(2amax)

44、

45、其中，pdis为空中机器人到预期支持点的位置向量，vtan为空中机器人速度在空中机器人到预期支持点的位置向量方向上的分量，disa为空中机器人由当前速度加速至最大速度所需的距离；

46、根据计算得到的时间成本矩阵mvrptw和时间窗矩阵tvrptw，通过or-tools进行空中机器人的调度。

47、根据本技术实施例的第二方面，提供一种空中机器人自主协助地面盲车集群导航的系统，包括：

48、相对位姿估计模块，空中机器人通过激光雷达slam获取空中机器人里程计和地图，并与盲无人车进行相对位姿估计；

49、盲无人车位姿预测模块，盲无人车获取自身的里程计，结合空中机器人里程计和相对位姿估计结果，利用扩展卡尔曼滤波预测自身的位姿信息；

50、碰撞信息预测模块，盲无人车基于空中机器人共享的地图，规划自身的轨迹并预测因里程计估计的不确定性和地图中的未知区域而发生的碰撞，将预测到的碰撞信息发送至空中机器人；

51、空中机器人调度模块，空中机器人接收所有盲无人车发送的碰撞信息，基于带时间窗的车辆路径规划方法规划自身的飞行路径，为可能发生碰撞的盲无人车提供新的相对位姿估计和地图信息，以使得盲无人车重新进行轨迹规划，避免碰撞。

52、根据本技术实施例的第三方面，提供一种电子设备，包括：

53、一个或多个处理器；

54、存储器，用于存储一个或多个程序；

55、当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如第一方面所述的方法。

56、根据本技术实施例的第四方面，提供一种计算机可读存储介质，其上存储有计算机指令，该指令被处理器执行时实现如第一方面所述方法的步骤。

57、本技术的实施例提供的技术方案可以包括以下有益效果：

58、由上述实施例可知，本技术为盲无人车设计的路径规划策略，激进地规划轨迹，保守地预测定位不确定性可能导致的盲无人车碰撞，从而提高盲无人车的运行效率，并且能够保障盲无人车的安全；在碰撞预测的基础上，为空中机器人的调度构建带时间窗的动态车辆路径规划问题，可以在盲无人车到达预测碰撞位置之前按顺序为盲无人车的运动提供支持，优化了空中机器人的飞行轨迹和盲无人车的等待时间。

59、应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本技术。

技术特征：

1.一种空中机器人自主协助地面盲车集群导航的方法，其特征在于，包括：

2.根据权利要求1所述的方法，其特征在于，步骤(2)中，盲无人车利用扩展卡尔曼滤波的预测模型，根据自身的里程计来预测自身的位姿信息；

3.根据权利要求2所述的方法，其特征在于，扩展卡尔曼滤波的预测模型为

4.根据权利要求2所述的方法，其特征在于，扩展卡尔曼滤波的观测模型为

5.根据权利要求1所述的方法，其特征在于，利用扩展卡尔曼滤波来表示盲无人车位姿估计的不确定性，通过扩展卡尔曼滤波的预测步骤沿规划轨迹按一定距离步长计算预测步骤，以传播盲无人车的不确定性。

6.根据权利要求1所述的方法，其特征在于，步骤(3)中，在碰撞地图中进行基于不确定性的碰撞预测，其中所述碰撞地图为将未知栅格视为占用栅格的地图，碰撞预测过程为：

7.根据权利要求1所述的方法，其特征在于，步骤(4)包括：

8.一种空中机器人自主协助地面盲车集群导航的系统，其特征在于，包括：

9.一种电子设备，其特征在于，包括：

10.一种计算机可读存储介质，其上存储有计算机指令，其特征在于，该指令被处理器执行时实现如权利要求1-7中任一项所述方法的步骤。

技术总结
本发明公开了一种空中机器人自主协助地面盲车集群导航的方法，空中机器人获取空中机器人里程计和地图，并与盲无人车进行相对位姿估计；盲无人车获取自身的里程计，结合空中机器人里程计和相对位姿估计结果，利用扩展卡尔曼滤波预测自身的位姿信息；盲无人车规划自身的轨迹并预测因里程计估计的不确定性和地图中的未知区域而发生的碰撞，将预测到的碰撞信息发送至空中机器人；空中机器人接收所有盲无人车发送的碰撞信息，基于带时间窗的车辆路径规划方法规划自身的飞行路径，为可能发生碰撞的盲无人车提供新的相对位姿估计和地图信息，以使得盲无人车重新进行轨迹规划，避免碰撞，该方法优化了空中机器人的飞行轨迹和盲无人车的等待时间。

技术研发人员：曹燕军,李哲晗,毛睿,许超
受保护的技术使用者：浙江大学
技术研发日：
技术公布日：2024/9/23