当前位置: 首页 > news >正文

ORCA-3D避障算法解析

二维ORCA原理参考:
https://zhuanlan.zhihu.com/p/669426124

ORCA原理图解

1. 找到避障速度增量 u

碰撞处理分为三种情况:
(1)没有发生碰撞,且相对速度落在小圆里
(2)没有发生碰撞,且相对速度落在圆锥里
(3)发生碰撞,马上做出反应
请添加图片描述
timeStep 决定了仿真每一步的时间更新间隔,是系统的时间推进基础。较小的 timeStep 可以提高仿真的精度,但会增加计算量。

timeHorizon 决定了智能体在进行避障计算时预测的时间范围。较大的 timeHorizon 值使得智能体可以更早预测潜在碰撞,但会减少它的速度选择自由度。

timeStep 是碰撞时需要计算的调整u所需的时间
timeHorizon 是未发生碰撞时,需要计算的u所化的时间,他是一种提前预测

2. 添加速度障碍平面

表示一个平面需要法向量和平面上的点
请添加图片描述

1和2对应代码如下

	// 它使用ORCA(Optimal Reciprocal Collision Avoidance)方法来计算智能体之间的避碰行为void Agent::computeNewVelocity(){orcaPlanes_.clear();				  // 清空ORCA平面列表const float invTimeHorizon = 1.0f / timeHorizon_; // 计算时间视野的倒数/* 创建智能体的ORCA平面 */for (size_t i = 0; i < agentNeighbors_.size(); ++i){								       // 遍历每个邻居智能体const Agent *const other = agentNeighbors_[i].second;	       // 获取邻居智能体指针//这里的position_是在rvo->updateState添加的当前agent的位置// 改这块就好了===============================const Vector3 relativePosition = other->position_ - position_; // 计算相对位置const Vector3 relativeVelocity = velocity_ - other->velocity_; // 计算相对速度// const Vector3 relativePosition = relative_position_; // 计算相对位置// const Vector3 relativeVelocity = relative_velocity_; // 计算相对速度const float distSq = absSq(relativePosition);		       // 计算相对位置的平方距离const float combinedRadius = radius_ + other->radius_;	       // 计算合并半径const float combinedRadiusSq = sqr(combinedRadius);	       // 计算合并半径的平方Plane plane; // 定义一个平面对象Vector3 u;   // 定义速度调整向量if (distSq > combinedRadiusSq){										// 如果没有发生碰撞// w表示给定时间视野TimeHorizon内,两个智能题之间的相对速度偏移量const Vector3 w = relativeVelocity - invTimeHorizon * relativePosition; // 计算从截断中心到相对速度的向量const float wLengthSq = absSq(w);					// 计算w向量的平方长度const float dotProduct = w * relativePosition; // 计算w向量和相对位置的点积// 1. 如果投影在截断圆上// dotProduct表示相差的速度和相差的位置的点乘,要是点乘小于0,表示在靠近if (dotProduct < 0.0f && sqr(dotProduct) > combinedRadiusSq * wLengthSq){						    const float wLength = std::sqrt(wLengthSq); // 计算w向量的长度const Vector3 unitW = w / wLength;	    // 计算w向量的单位向量plane.normal = unitW;					 // 设置平面的法向量u = (combinedRadius * invTimeHorizon - wLength) * unitW; // 计算速度调整向量}// 2. 如果投影在圆锥上else{																  const float a = distSq;													  // 设置系数aconst float b = relativePosition * relativeVelocity;									  // 设置系数bconst float c = absSq(relativeVelocity) - absSq(cross(relativePosition, relativeVelocity)) / (distSq - combinedRadiusSq); // 设置系数c// t表示圆锥中心线到斜线的距离 对于 半径的倍数const float t = (b + std::sqrt(sqr(b) - a * c)) / a;									  // 计算t值const Vector3 w = relativeVelocity - t * relativePosition;								  // 计算w向量const float wLength = abs(w);												  // 计算w向量的长度const Vector3 unitW = w / wLength;											  // 计算w向量的单位向量plane.normal = unitW;			    // 设置平面的法向量u = (combinedRadius * t - wLength) * unitW; // 计算速度调整向量}}// 3. 如果发生碰撞else{									     const float invTimeStep = 1.0f / sim_->timeStep_;		     // 计算时间步长的倒数const Vector3 w = relativeVelocity - invTimeStep * relativePosition; // 计算w向量const float wLength = abs(w);					     // 计算w向量的长度const Vector3 unitW = w / wLength;				     // 计算w向量的单位向量plane.normal = unitW;				      // 设置平面的法向量u = (combinedRadius * invTimeStep - wLength) * unitW; // 计算速度调整向量}// 有多少个neighbor,就有多少个orca平面plane.point = velocity_ + 0.5f * u; // 计算平面上的点orcaPlanes_.push_back(plane);	    // 将平面添加到ORCA平面列表中}const size_t planeFail = linearProgram3(orcaPlanes_, maxSpeed_, prefVelocity_, false, newVelocity_); // 计算新的速度,如果失败返回失败的平面索引if (planeFail < orcaPlanes_.size()){									 // 如果存在失败的平面linearProgram4(orcaPlanes_, planeFail, maxSpeed_, newVelocity_); // 调用备用算法处理失败的平面}}

3. 线性规划求解出最优速度

linearProgram几个函数实现了一套线性规划(Linear Programming, LP)求解方法,目的是在有多个平面约束(即避障条件)的情况下找到最优的速度向量,以确保多个智能体不会发生碰撞。

linearProgram1():寻找线与圆形区域的交点
linearProgram2():解决单个平面约束的最优速度
linearProgram3():求解所有平面的初步速度
linearProgram4():处理多个平面之间的约束冲突

http://www.lryc.cn/news/437489.html

相关文章:

  • CentOS 7停更官方yum源无法使用,更换阿里源
  • Introduction结构
  • 基于SpringBoot实现SpringMvc上传下载功能实现
  • vue 控制组件是否显示
  • 生产部门不给力?精益化生产管理咨询公司为您出谋划策
  • HTML+CSS - 网页布局之网格布局
  • 基于51单片机的16X16点阵显示屏proteus仿真
  • 【目标检测数据集】厨房常见的水果蔬菜调味料数据集4910张39类VOC+YOLO格式
  • 在Python中统计字符串中每个字符出现的次数
  • 关于 vue/cli 脚手架实现项目编译运行的源码解析
  • C++笔记---继承(上)
  • 气膜体育馆:为学校打造智能化运动空间—轻空间
  • JVM 调优篇5 jvm性能监控
  • 一. Unity实现虚拟摇杆及屏幕自适应功能
  • 【达梦数据库】mysql 和达梦 tinyint 与 bit 返回值类型差异
  • VUE工程中axios基本使用
  • 跨服务器执行PowerShell脚本
  • linux_L2_linux删除文件
  • 系统架构设计师 - 项目管理
  • Spring Boot基础
  • C语言 | Leetcode C语言题解之第402题移掉K位数字
  • 使用Visual Studio Code配置C/C++开发环境的全面指南
  • 算法练习题26——多项式输出(模拟)
  • 卷积神经网络经典模型架构简介
  • 【Kubernetes】常见面试题汇总(十三)
  • 嵌入式QT开发:构建高效智能的嵌入式系统
  • Linux抢占调度
  • k8s中,为什么把pod的服务以deployment的形式通过nodeport对外发布,以及容器和虚拟机的一些区别
  • PMP--一模--解题--41-50
  • Kafka启动关闭及其相关命令kafka启动、状态监控、日常操作