Teach-Repeat-Replan: A Complete and Robust System for Aggressive Flight in Complex Environments

本文提出了一种名为Teach-Repeat-Replan的完整鲁棒运动规划系统,用于自主四旋翼飞行器在复杂环境中的飞行。系统能将不稳定的人工教学轨迹转换为安全的重复轨迹,并通过滑动窗口局部感知和重新规划应对未知或移动障碍。研究中,作者改进了飞行走廊生成方法,采用凸聚类膨胀和多面体空间表示,优化了轨迹优化过程,确保平滑、安全和动力学可行性。

作者: 19级 lz

日期:2021-11-3

论文:《Teach-Repeat-Replan:A Complete and Robust System for Aggressive Flight in Complex Environments》



一、背景:

本文针对自主四旋翼飞行器的飞行问题,提出了一个完整的鲁棒运动规划系统。该方法建立在经典的教学重复框架上,广泛应用于基础设施检测、空中运输和搜救。
在这些应用中,由人类完全决定无人机飞行轨迹的拓扑结构。然而,糟糕的引导轨迹和不断变化的环境阻碍了一个简单的引导-重复(teach-repeat)系统的较好的应用。
在本文中,我们提出了一种方法,能够自动转换一个人的任意不稳定的驾驶轨迹到一个拓扑等价的轨迹,而不是命令无人机精确地跟随人的导航轨迹。生成的轨迹保证平滑、安全、动力学可行。此外,为了避免飞行过程中未映射的或动态的障碍,我们在系统中引入了滑动窗口局部感知和重新规划方法,以生成安全的机载局部轨迹。我们将我们的系统命名为teach-repeat-replan。它可以捕捉用户对飞行任务的意图,将任意不稳定的教学路径转换为平滑的相同轨迹,并生成安全的局部重新规划,以避免未建图或移动的障碍。



二、有关工作:

引导-重复(teach-repeat)系统:

近年来,许多机器人引导-重复工作已经发表,特别是移动机器人。它们大多侧重于提高运算符重复/跟随路径的准确性或健壮性,这与本文的动机有根本的不同。

轨迹优化:

轨迹优化是从较差的教学轨迹生成安全、可执行的重复轨迹的关键。本文首先寻找一种安全的几何路径来指导轨迹的生成。通过在路径中迭代地添加中间路径点,多次求解minimum-snap问题,最终生成安全轨迹。

飞行通道优化:

我们全球规划的第一步是围绕引导轨迹建立一个飞行走廊,用于时空轨迹优化。在我们之前的工作中,我们通过寻找一系列轴向的立方体来构建飞行走廊,这可能会牺牲很多空间,特别是在高度非凸的环境中。
在这里插入图片描述

在这里插入图片描述

其中凸多面体比简单的立方体获得了更多的自由空间。使用简单的轴向立方体极大地限制了轨迹优化的求解空间,可能会导致较差的解。更重要的是,在自由空间非常有限的情况下,例如飞行通过一个非常狭窄的圆,一个基于立方体的走廊甚至可能不能覆盖所有的教学轨迹,导致走廊中不存在任何解决方案。因此,为了更充分地利用自由空间并适应即使是极其杂乱的映射,我们提出了一种基于凸集聚类生成一般、自由、大凸多面体的方法。



三、研究过程及结果:


凸簇膨胀

凸簇定义:如果连接S中任意两点的线段完全位于S,则向量空间中的该合集称为凸集。
体素被标记为被占用或自由

算法1:

在这里插入图片描述

define:
C:代表目标凸簇
C+:是迭代中尝试添加到C的体素集
C*:包含新添加的体素,保持了凸簇性。

(1)簇膨胀首先加入种子体素体素
(2)加入p体
This should not happen. [ERROR] [1756358664.429886169, 3672.712000000]: Ran out of pool, index=100 61 26 [ERROR] [1756358664.429899881, 3672.712000000]: Unable to handle the initial or end point, force return! [ERROR] [1756358664.429913079, 3672.712000000]: a star error iter=5,time(ms)=0,rebound. [ERROR] [1756358664.429953628, 3672.712000000]: ERROR! the drone is in obstacle. This should not happen. [ERROR] [1756358664.429974423, 3672.712000000]: Ran out of pool, index=100 62 26 [ERROR] [1756358664.429987496, 3672.712000000]: Unable to handle the initial or end point, force return! [ERROR] [1756358664.430000376, 3672.712000000]: a star error iter=5,time(ms)=0,rebound. [ERROR] [1756358664.430036663, 3672.712000000]: ERROR! the drone is in obstacle. This should not happen. [ERROR] [1756358664.430052404, 3672.712000000]: Ran out of pool, index=100 60 26 [ERROR] [1756358664.430065756, 3672.712000000]: Unable to handle the initial or end point, force return! [ERROR] [1756358664.430083399, 3672.712000000]: a star error iter=5,time(ms)=0,rebound. [ERROR] [1756358664.430115088, 3672.712000000]: ERROR! the drone is in obstacle. This should not happen. [ERROR] [1756358664.430131173, 3672.712000000]: Ran out of pool, index=100 62 26 [ERROR] [1756358664.430149486, 3672.712000000]: Unable to handle the initial or end point, force return! [ERROR] [1756358664.430162703, 3672.712000000]: a star error iter=5,time(ms)=0,rebound. [ERROR] [1756358664.430235732, 3672.712000000]: ERROR! the drone is in obstacle. This should not happen. [ERROR] [1756358664.430268589, 3672.712000000]: Ran out of pool, index=100 60 26 [ERROR] [1756358664.430290943, 3672.712000000]: Unable to handle the initial or end point, force return! [ERROR] [1756358664.430306426, 3672.712000000]: a star error iter=5,time(ms)=0,rebound. [ERROR] [1756358664.430344630, 3672.712000000]: ERROR! the drone is in obstacle. This should not happen. [ERROR] [1756358664.430366767, 3672.712000000]: Ran out of pool, index=100 61 26 [ERROR] [1756358664.430384512, 3672.712000000]: Unable to handle the initial or end point, force return! [ERROR] [1756358664.430403036, 3672.712000000]: a star error iter=5,time(ms)=0,rebound. [ERROR] [1756358664.430435337, 3672.712000000]: ERROR! the drone is in obstacle. This should not happen. [ERROR] [1756358664.430456490, 3672.712000000]: Ran out of pool, index=100 60 26 [ERROR] [1756358664.430474754, 3672.712000000]: Unable to handle the initial or end point, force return! [ERROR] [1756358664.430488073, 3672.712000000]: a star error iter=5,time(ms)=0,rebound. -0.0924 0.573 -0.0117 -0.0929 0.573 -0.0114 -0.22 0.545 0.0515 -0.507 0.508 0.193 [WARN] [1756358664.430557163, 3672.712000000]: First 3 control points in obstacles! return false, t=0.000000 first_optimize_step_success=0 final_plan_success=0 [FSM]: from GEN_NEW_TRAJ to GEN_NEW_TRAJ [rebo replan]: -------------------------------------1361 start: -0.0919 0.574 -0.012, 0 0 0 goal:-2.99 3.55 1, 0 0 0 -0.092 0.57 -0.012 -0.092 0.57 -0.011 -0.15 0.56 0.13 -0.29 0.6 0.38 [WARN] [1756358664.430915790, 3672.712000000]: First 3 control points in obstacles! return false, t=0.000000 first_optimize_step_success=0 final_plan_success=0 [FSM]: from GEN_NEW_TRAJ to GEN_NEW_TRAJ [rebo replan]: -------------------------------------1362 start: -0.0919 0.574 -0.012, 0 0 0 goal:-2.99 3.55 1, 0 0 0 -0.092 0.57 -0.012 -0.092 0.57 -0.012 -0.13 0.67 0.11 -0.25 0.86 0.29 [WARN] [1756358664.455403672, 3672.728000000]: First 3 control points in obstacles! return false, t=0.000000 first_optimize_step_success=0 final_plan_success=0 [FSM]: from GEN_NEW_TRAJ to GEN_NEW_TRAJ [rebo replan]: -------------------------------------1363 start: -0.0919 0.574 -0.012, 0 0 0 goal:-2.99 3.55 1, 0 0 0 [ERROR] [1756358664.491644334, 3672.732000000]: Ran out of pool, index=100 23 26 [ERROR] [1756358664.491745147, 3672.732000000]: Unable to handle the initial or end point, force return! [ERROR] [1756358664.491804661, 3672.732000000]: a star error, force return! [ERROR] [1756358664.491914622, 3672.732000000]: ERROR! the drone is in obstacle. This should not happen. [ERROR] [1756358664.492054906, 3672.732000000]: Ran out of pool, index=100 23 26 [ERROR] [1756358664.492110003, 3672.732000000]: Unable to handle the initial or end point, force return! [ERROR] [1756358664.492137217, 3672.732000000]: a star error iter=5,time(ms)=0,rebound. [ERROR] [1756358664.492199114, 3672.732000000]: ERROR! the drone is in obstacle. This should not happen. [ERROR] [1756358664.492255540, 3672.732000000]: Ran out of pool, index=100 23 26 [ERROR] [1756358664.492307602, 3672.732000000]: Unable to handle the initial or end point, force return! [ERROR] [1756358664.492328030, 3672.732000000]: a star error -0.0922 0.574 -0.0119 -0.0924 0.574 -0.0117 -0.161 0.609 0.0238 -0.328 0.704 0.107 [WARN] [1756358664.492497334, 3672.732000000]: First 3 control points in obstacles! return false, t=0.000000 first_optimize_step_success=0 final_plan_success=0 [FSM]: from GEN_NEW_TRAJ to GEN_NEW_TRAJ [rebo replan]: -------------------------------------1364 start: -0.0849 0.58 -0.00768, 0 0 0 goal:-2.99 3.55 1, 0 0 0 [ERROR] [1756358664.500685781, 3672.744000000]: Ran out of pool, index=100 40 27 [ERROR] [1756358664.500790285, 3672.744000000]: Unable to handle the initial or end point, force return! [ERROR] [1756358664.500841994, 3672.744000000]: a star error, force return! [ERROR] [1756358664.500946032, 3672.744000000]: ERROR! the drone is in obstacle. This should not happen. [ERROR] [1756358664.501024860, 3672.744000000]: Ran out of pool, index=100 40 27 [ERROR] [1756358664.501101501, 3672.744000000]: Unable to handle the initial or end point, force return! [ERROR] [1756358664.501179159, 3672.744000000]: a star error iter=5,time(ms)=0,rebound. [ERROR] [1756358664.501306925, 3672.744000000]: ERROR! the drone is in obstacle. This should not happen. [ERROR] [1756358664.501378965, 3672.744000000]: Ran out of pool, index=100 40 27 [ERROR] [1756358664.501428880, 3672.744000000]: Unable to handle the initial or end point, force return! [ERROR] [1756358664.501500788, 3672.744000000]: a star error iter=5,time(ms)=0,rebound. -0.0852 0.58 -0.00752 -0.0856 0.58 -0.00734 -0.173 0.595 0.0351 -0.381 0.644 0.133 [WARN] [1756358664.501692241, 3672.744000000]: First 3 control points in obstacles! return false, t=0.000000 first_optimize_step_success=0 final_plan_success=0 [FSM]: from GEN_NEW_TRAJ to GEN_NEW_TRAJ [rebo replan]: -------------------------------------1365 start: -0.0849 0.58 -0.00768, 0 0 0 goal:-2.99 3.55 1, 0 0 0 -0.085 0.58 -0.0073 -0.085 0.58 -0.007 -0.11 0.58 0.15 -0.17 0.68 0.4 [WARN] [1756358664.507642672, 3672.752000000]: First 3 control points in obstacles! return false, t=0.000000 first_optimize_step_success=0 final_plan_success=0 [FSM]: from GEN_NEW_TRAJ to GEN_NEW_TRAJ [rebo replan]: -------------------------------------1366 start: -0.0849 0.58 -0.00768, 0 0 0 goal:-2.99 3.55 1, 0 0 0 -0.085 0.58 -0.0078 -0.084 0.58 -0.008 -0.25 0.71 0.055 -0.54 0.98 0.15 [WARN] [1756358664.516889884, 3672.764000000]: First 3 control points in obstacles! return false, t=0.000000 first_optimize_step_success=0 final_plan_success=0 [FSM]: from GEN_NEW_TRAJ to GEN_NEW_TRAJ [rebo replan]: -------------------------------------1367 start: -0.0849 0.58 -0.00768, 0 0 0 goal:-2.99 3.55 1, 0 0 0 -0.085 0.58 -0.0075 -0.084 0.58 -0.0074 -0.11 0.63 0.16 -0.19 0.77 0.44 [WARN] [1756358664.522673491, 3672.772000000]: First 3 control points in obstacles! return false, t=0.000000 first_optimize_step_success=0 final_plan_success=0 [FSM]: from GEN_NEW_TRAJ to GEN_NEW_TRAJ [rebo replan]: -------------------------------------1368 start: -0.0849 0.58 -0.00768, 0 0 0 goal:-2.99 3.55 1, 0 0 0 -0.085 0.58 -0.0074 -0.086 0.58 -0.0071 -0.11 0.61 0.15 -0.24 0.68 0.4 [WARN] [1756358664.555013954, 3672.788000000]: First 3 control points in obstacles! return false, t=0.000000 first_optimize_step_success=0 final_plan_success=0 [FSM]: from GEN_NEW_TRAJ to GEN_NEW_TRAJ [rebo replan]: -------------------------------------1369 start: -0.0849 0.58 -0.00768, 0 0 0 goal:-2.99 3.55 1, 0 0 0 -0.085 0.58 -0.0079 -0.085 0.58 -0.0082 -0.19 0.66 0.12 -0.39 0.82 0.29 [WARN] [1756358664.561977705, 3672.792000000]: First 3 control points in obstacles! return false, t=0.000000 first_optimize_step_success=0 final_plan_success=0 [FSM]: from GEN_NEW_TRAJ to GEN_NEW_TRAJ [rebo replan]: -------------------------------------1370 start: -0.0849 0.58 -0.00768, 0 0 0 goal:-2.99 3.55 1, 0 0 0 -0.085 0.58 -0.0079 -0.085 0.58 -0.008 -0.22 0.65 0.096 -0.47 0.8 0.23 [WARN] [1756358664.576167822, 3672.804000000]: First 3 control points in obstacles! return false, t=0.000000 first_optimize_step_success=0 final_plan_success=0 [FSM]: from GEN_NEW_TRAJ to GEN_NEW_TRAJ [rebo replan]: -------------------------------------1371 start: -0.0849 0.58 -0.00768, 0 0 0 goal:-2.99 3.55 1, 0 0 0 -0.085 0.58 -0.0074 -0.084 0.58 -0.0071 -0.094 0.59 0.14 -0.15 0.68 0.4 [WARN] [1756358664.585455552, 3672.812000000]: First 3 control points in obstacles! return false, t=0.000000 first_optimize_step_success=0 final_plan_success=0 [FSM]: from GEN_NEW_TRAJ to GEN_NEW_TRAJ [rebo replan]: -------------------------------------1372 start: -0.0849 0.58 -0.00768, 0 0 0 goal:-2.99 3.55 1, 0 0 0 [ERROR] [1756358664.645594342, 3672.824000000]: Ran out of pool, index=100 3 10 [ERROR] [1756358664.645641009, 3672.824000000]: Unable to handle the initial or end point, force return! [ERROR] [1756358664.645659474, 3672.824000000]: a star error, force return! [ERROR] [1756358664.645708497, 3672.824000000]: ERROR! the drone is in obstacle. This should not happen. [ERROR] [1756358664.645728158, 3672.824000000]: Ran out of pool, index=100 3 10 [ERROR] [1756358664.645742831, 3672.824000000]: Unable to handle the initial or end point, force return! [ERROR] [1756358664.645756807, 3672.824000000]: a star error iter=5,time(ms)=0,rebound. [ERROR] [1756358664.645796326, 3672.824000000]: ERROR! the drone is in obstacle. This should not happen. [ERROR] [1756358664.645815082, 3672.824000000]: Ran out of pool, index=100 3 10 [ERROR] [1756358664.645835116, 3672.824000000]: Unable to handle the initial or end point, force return! [ERROR] [1756358664.645849149, 3672.824000000]: a star error -0.0851 0.58 -0.00751 -0.0852 0.581 -0.00732 -0.137 0.629 0.0375 -0.266 0.754 0.141 [WARN] [1756358664.645906536, 3672.824000000]: First 3 control points in obstacles! return false, t=0.000000 first_optimize_step_success=0 final_plan_success=0 [FSM]: from GEN_NEW_TRAJ to GEN_NEW_TRAJ [rebo replan]: -------------------------------------1373 start: -0.085 0.579 -0.00401, 0 0 0 goal:-2.99 3.55 1, 0 0 0 iter(+1)=24,time(ms)=0.000,total_t(ms)=0.000,cost=0.053 first_optimize_step_success=1 Need to reallocate time. total time:0,optimize:0,refine:0 final_plan_success=1 [FSM]: from GEN_NEW_TRAJ to EXEC_TRAJ [FSM]: state: EXEC_TRAJ [rebo replan]: -------------------------------------1374 start: -0.0872 0.578 -0.00241, -0.00322 -0.000964 0.00222 goal:-2.99 3.55 1, 0 0 0 iter(+1)=21,time(ms)=0.000,total_t(ms)=0.000,cost=0.028 first_optimize_step_success=1 total time:0,optimize:0,refine:0 final_plan_success=1 [SAFETY]: from EXEC_TRAJ to EXEC_TRAJ [FSM]: state: EXEC_TRAJ [FSM]: state: EXEC_TRAJ [FSM]: state: EXEC_TRAJ [FSM]: state: EXEC_TRAJ [FSM]: state: EXEC_TRAJ [FSM]: state: EXEC_TRAJ [FSM]: state: EXEC_TRAJ [FSM]: from EXEC_TRAJ to REPLAN_TRAJ [rebo replan]: -------------------------------------1375 start: -1.38 1.07 0.73, -0.228 0.188 0.1 goal:-2.99 3.55 1, 0 0 0 iter(+1)=15,time(ms)=0.000,total_t(ms)=0.000,cost=0.013 first_optimize_step_success=1 total time:0,optimize:0,refine:0 final_plan_success=1 [FSM]: from REPLAN_TRAJ to EXEC_TRAJ [FSM]: state: EXEC_TRAJ [FSM]: state: EXEC_TRAJ [FSM]: state: EXEC_TRAJ [FSM]: state: EXEC_TRAJ [FSM]: state: EXEC_TRAJ [FSM]: state: EXEC_TRAJ [FSM]: state: EXEC_TRAJ [FSM]: state: EXEC_TRAJ [FSM]: state: EXEC_TRAJ [FSM]: state: EXEC_TRAJ [FSM]: state: EXEC_TRAJ [FSM]: state: EXEC_TRAJ [FSM]: from EXEC_TRAJ to WAIT_TARGET [FSM]: state: WAIT_TARGET [FSM]: state: WAIT_TARGET [FSM]: state: WAIT_TARGET [FSM]: state: WAIT_TARGET [FSM]: state: WAIT_TARGET [FSM]: state: WAIT_TARGET ^C[traj_server-2] killing on exit [waypoint_generator-3] killing on exit [ego_planner_node-1] killing on exit des manager shutting down processing monitor... ... shutting down processing monitor complete done
08-29
从日志信息来看,系统(可能是无人机或机器人路径规划系统)在尝试生成或执行轨迹时遇到了多个问题,主要问题包括: 1. **A* 算法错误**:系统多次提示 "a star error",这通常意味着路径规划算法未能找到从起点到终点的有效路径。 2. **节点池耗尽**:提示 "Ran out of pool",表明用于路径搜索的节点资源不足,可能由于地图复杂或内存配置不足。 3. **起点或终点无法处理**:"Unable to handle the initial or end point" 表示系统无法正确解析起点或目标点,可能由于其位于障碍物中。 4. **控制点位于障碍物中**:"First 3 control points in obstacles" 表明生成的轨迹控制点位于障碍物区域内,导致轨迹不可行。 5. **轨迹重规划失败**:多次尝试重规划轨迹("GEN_NEW_TRAJ")失败,直到最后几次尝试才成功。 ### 可能的原因 - 起点或目标点设置在障碍物内部。 - A* 算法的地图表示或障碍物检测机制存在问题。 - 轨迹优化模块未能有效规避障碍。 - 节点池配置过小,导致搜索失败。 ### 解决方案建议 1. **检查起点和目标点**:确保起点和终点不在障碍物内。 2. **调整路径规划参数**:增加节点池大小,提高搜索能力。 3. **改进障碍物检测逻辑**:确保轨迹生成后能正确检测并规避障碍。 4. **优化轨迹生成算法**:使用更鲁棒的轨迹优化方法,如结合 RRT 或 FMT。 ```python # 示例:简单检查点是否在障碍物中 def is_point_in_obstacle(point, obstacles, threshold=0.1): for obs in obstacles: if all(abs(p - o) < threshold for p, o in zip(point, obs)): return True return False # 示例使用 point = (-0.092, 0.573, -0.0117) obstacles = [(-0.092, 0.573, -0.0114), (-0.1, 0.5, 0)] print(is_point_in_obstacle(point, obstacles)) # 输出 True 或 False ``` ###
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

中南大学苹果实验室

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值