机器人（无人机）算法解析及优化_theta star a star

机器人（无人机）算法解析及优化

机器人算法通常包括定位、避障、平滑、目标检测、目标追踪。

1.定位

1.1里程计定位信息

定位算法重在精度、实时性和鲁棒性！！！

常用：外部设备定位UWB、Mocap、GPS、RTK（差分GPS），机载设备定位光流、摄像头（VIO技术）、激光雷达（LIO技术）。

以下介绍几种强大的三维激光雷达定位算法：

香港大学提出的FAST-LIO：
1)为了应对发生退化的快速运动，嘈杂或混乱的环境，采用了紧密耦合的迭代卡尔曼滤波器来融合LiDAR特征点和IMU测量值。
2）提出了一种形式的反向传播过程来补偿运动畸变。
3)为了降低大量激光雷达特征点带来的计算负荷，提出了一种新的卡尔曼增益计算公式，并证明了其与传统卡尔曼增益公式的等价性。新公式的计算复杂度取决于状态维而不是测量维。

香港大学提出的FAST-LIO2：

前端用了增量的kdtree（ikd-tree），后端用了迭代误差状态卡尔曼滤波iESKF，流程短，计算快。

高博和清华大学提出的Faster-LIO：

把ikd-tree替换成了iVox（增量稀疏体素结构），优化了代码逻辑，保持精度并提供约1.5-2 倍的速度提升。

以上三种是基于滤波的紧耦合LIO算法，接下来介绍两种基于voxelmap体素图的LIO算法。

PV-LIO基于概率体素图的LiDAR惯性里程计：

精度比FAST-LIO2高，利用 IKFoM 将激光雷达特征点与 IMU 数据融合在一起，从而在快速运动或狭窄的环境中实现稳健导航。PV-LIO 还支持在线 LiDAR-IMU 外参估计。

电子科大提出的VoxelMap++：

一种在线LiDAR（惯性）里程计的可合并体素映射方法，精度相当高。

1.2重定位

使用过move_base的小伙伴，也许曾对amcl这个导航技术栈可选节点产生过疑惑，其实这是用于预先建好全局地图的重定位，低频的重定位数据（map->odom）用以修正高频里程计信息的累积误差（odom->base_link）。

更加详细的解释可以看看B站UP机器人工匠阿杰的视频，链接给大家放在下面啦。

常用的两种导航定位方式：

1）预先建好全局地图，这样可以用重定位算法修正里程计累积误差，获得更高精度。

2）只利用里程计定位信息来导航，弊端是有累积误差，时间久了偏差较大。

在二维导航中常用的重定位算法：AMCL

在三维导航中常用的重定位算法：ICP、NDT、AMCL3d

2.避障

谨防思维误区：避障算法不一定需要地图，基于传感器数据即可避障！比如bug算法

2.1二维避障算法

常用的路径规划算法：

1）全局路径规划算法：Dijkstra、A-star、theta-star（ROS2）

2）局部路径规划算法：DWA、MPC、TEB、MPPI（ROS2）

在这里推荐一个宝藏资源ai-winter/ros_motion_planning，链接给大家放在下面啦！

不仅可以在自己电脑上跑仿真，也可以把其中的A*, JPS, D*, LPA*, D* Lite, Theta*, RRT, RRT*, RRT-Connect, Informed RRT*, ACO, PSO, Voronoi, PID, LQR, MPC, DWA, APF, Pure Pursuit应用在自己的机器人上哦！

全局路径规划算法：A-star和theta*的对比

Theta* 是一种基于 A* 搜索算法的任意角度路径规划算法。它可以找到运行时间与 A* 相当的接近最优路径。

A-star

theta*

2.2三维避障算法

浙大高飞实验室提出的ego-planner：

香港科技大学提出的FAST-Planner：

2023年香港大学提出的ROG-MAP：

无全局地图，启发式搜索替代全局路径规划，就像咱们出门去食堂吃饭也没有什么全局路径规划。这是一种基于均匀网格的OGM，它维护一个随着机器人移动的局部地图，以实现高效的地图操作和减少大型场景自主飞行的内存开销。此外，还提出了一种新颖的增量式障碍物膨胀方法，可以显着降低膨胀计算耗时。效果超强！！！

3.平滑

3.1路径平滑

在ros_motion_planning中，有一些常用的路径平滑算法。

比如：贝塞尔曲线、B样条、三次样条、dubins曲线、多项式回归曲线、reeds sheep曲线。

3.2速度平滑

常用的两个速度平滑包

4.目标检测

单阶段模型YOLO兼备实时性与准确性，加之发展地如火如荼，广泛应用于检测、分割、姿态估计、跟踪和分类，是目标检测的不二选择。
YOLO模型版本轨迹和技术演进
CVPR2024 | YOLO-World 检测一切对象模型
YOLOX

5.目标追踪

常见的opencv中的KCF等等，KF、MPC在目标追踪中的应用

基于图像的伺服控制算法

常见的例如：基于图像的视觉伺服PID控制，即根据目标检测框中心点与画面中心点的偏差，基于误差消除误差的思想，采用PID算法速度控制、位置控制机器人消除偏差来实现目标跟踪。
目标检测存在漏检情况，如何弥补基于目标检测的追踪漏检乱跑问题，后续补充。

此外，其是没有预见性的，MPC模型预测控制和KF的过程模型皆有预测性，可以往MPC方向发展，另外强化学习比较适合用于做目标追踪、导航避障。

例如：浙江大学提出的CoNi-MPC：基于非惯性系的空地协作模型预测控制

6.题外话

依据Kolmogorov-Arnold 表示定理 (KART)，即每个多元连续函数都可以表示为单变量连续函数的两层嵌套叠加，MIT等校提出了全新神经网络架构KAN。

全新神经网络架构KAN来了！
KAN与传统的MLP架构截然不同，且能用更少的参数在数学、物理问题上取得更高精度。

视频：

参考链接：

什么是里程计-CSDN博客

VIO、VSLAM、VINS的区别与联系-CSDN博客

bilibili机器人工匠阿杰之ROS快速入门教程。

DWA、TEB、MPC算法优劣对比-CSDN博客

ROS1各种路径规划算法ai-winter/ros_motion_planning

ros_motion_planning中的路径规划算法如何集成到自己的机器人上-CSDN博客

使用mid360从0开始搭建实物机器人入门级导航系统，基于Fast_Lio,Move_Base