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

沈阳注册公司seo网络优化软件

沈阳注册公司,seo网络优化软件,做网站人员配置,网站建设供需在 ego-planner 中,在进行了之前的初始轨迹采样,Astar 绕障轨迹,障碍物边界的二次计算后,会有两组点,一组是二次计算后的障碍物边界轨迹点,一组是 Astar 绕障轨迹点,(可以查看之前的 ego 文章)。…

在 ego-planner 中,在进行了之前的初始轨迹采样,Astar 绕障轨迹,障碍物边界的二次计算后,会有两组点,一组是二次计算后的障碍物边界轨迹点,一组是 Astar 绕障轨迹点,(可以查看之前的 ego 文章)。

初始轨迹采样:https://blog.csdn.net/a_xiaoning/article/details/151158675?spm=1001.2014.3001.5501

Astar绕障碍:https://blog.csdn.net/a_xiaoning/article/details/151722705?spm=1001.2014.3001.5501

障碍物边界二次计算:https://blog.csdn.net/a_xiaoning/article/details/151800448?spm=1001.2014.3001.5501

为了给避障留有足够的空间,ego 的做法是会将 Astar 的路径控制点进一步推离障碍物

计算交点,避障基准点与排斥方向的步骤

障碍段轨迹点初始标记

首先,进行障碍段轨迹点的初始标记,这个障碍段是指在上一步进行了障碍物边界点扩张后的点,在 ego 中,存储在 final_segment_ids 中,对应上示意图中的绿色点:

      // final_segment_ids 为当前障碍物段的所有可调控制点  cps_是所有轨迹控制点  在初始时,会将所有可调控制点标记为 falsefor (int j = final_segment_ids[i].first; j <= final_segment_ids[i].second; ++j)cps_.flag_temp[j] = false;

计算交点,避障基准点与扩张方向

1. 首先计算当前轨迹点的切线方向,也就是向量上一个轨迹点->下一个轨迹点,记作 ctrl_pts_law:

// 计算控制点 Qj 的法向方向:Q(j-1) -> Q(j+1) 的垂直方向
Eigen::Vector3d ctrl_pts_law(cps_.points.col(j + 1) - cps_.points.col(j - 1)), intersection_point;

2. 从 Astar 路径的中点开始搜索垂直于向量 ctrl_pts_law 的法线向量,这里使用了向量点积的概念。

可以回顾一下点积的概念,结合一下 ego 的场景,两个向量点积就是一个向量在另一个向量的投影,当点积 > 0 时,说明两个向量同向,点积 < 0 时,说明两个向量反向。

那么在 ego 中,就利用这个性质,来在 Astar 的路径点中,寻找与当前控制点的切线向量垂直的控制点(也就是点积异号的时候)

// 从 Astar 路径中点开始搜索
int Astar_id = a_star_pathes[i].size() / 2, last_Astar_id; // Let "Astar_id = id_of_the_most_far_away_Astar_point" will be better, but it needs more computation
// 计算 Astar 路径与初始 B 样条控制点方向的点积,用以确定寻找 Astar 路径点的方向
// 点积的意义表示 Astar 路径上某点与控制点控制点Qj组成的向量 在 ctrl_pts_law 上的投影,如果 val > 0 则表示与 ctrl_pts_law 同向,否则反向
double val = (a_star_pathes[i][Astar_id] - cps_.points.col(j)).dot(ctrl_pts_law), last_val = val;// 沿 Astar 路径搜索,找到与 ctrl_pts_law 方向的交点(即障碍物基准点方向)
while (Astar_id >= 0 && Astar_id < (int)a_star_pathes[i].size())
{last_Astar_id = Astar_id;if (val >= 0)--Astar_id;else++Astar_id;val = (a_star_pathes[i][Astar_id] - cps_.points.col(j)).dot(ctrl_pts_law);// 检测到交点(点积符号变化,说明跨越ctrl_pts_law方向)if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0)) // val = last_val = 0.0 is not allowed{

3. 在找到 Astar 对应的交点标号后,再在两个交点之间进行插值计算准确的交点坐标:

  // 检测到交点(点积符号变化,说明跨越ctrl_pts_law方向)if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0)) // val = last_val = 0.0 is not allowed{// 线性插值计算精确交点坐标(intersection_point)intersection_point =a_star_pathes[i][Astar_id] +((a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id]) *(ctrl_pts_law.dot(cps_.points.col(j) - a_star_pathes[i][Astar_id]) / ctrl_pts_law.dot(a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id])) // = t);//cout << "i=" << i << " j=" << j << " Astar_id=" << Astar_id << " last_Astar_id=" << last_Astar_id << " intersection_point = " << intersection_point.transpose() << endl;got_intersection_id = j;  // 标记找到交点的控制点break;}

4. 基于交点坐标计算障碍物基准点与排斥方向。

障碍物基准点:就是在控制点与交点连线上,离障碍物最近的点,在寻找时,使用障碍物网格地图的分辨率在连线上逐次寻找即可

排斥方向:向量 控制点->交点

// 基于交点计算障碍物基准点与排斥方向
if (got_intersection_id >= 0)
{cps_.flag_temp[j] = true;   // 标记该控制点已分配约束double length = (intersection_point - cps_.points.col(j)).norm(); // 控制点与交点的距离if (length > 1e-5){// 在控制点与交点的连线上寻找离障碍物最近的安全点 根据网格地图的分辨率来逐次移动for (double a = length; a >= 0.0; a -= grid_map_->getResolution()){// 在控制点与交点的连线上寻找离障碍物最近的安全点occ = grid_map_->getInflateOccupancy((a / length) * intersection_point + (1 - a / length) * cps_.points.col(j));if (occ || a < grid_map_->getResolution()){// 若安全点在障碍物内,则回退一个栅格分辨率if (occ)a += grid_map_->getResolution();// 基准点:避开障碍物的最近安全点cps_.base_point[j].push_back((a / length) * intersection_point + (1 - a / length) * cps_.points.col(j));// 排斥方向:从控制点指向基准点(归一化)cps_.direction[j].push_back((intersection_point - cps_.points.col(j)).normalized());break;}}}
}

5.循环计算在障碍物段内所有轨迹控制点对应的障碍物基准点与排斥方向

计算不出来的点直接分配

为剩下计算不出来交点,障碍物基准点,排斥方向的控制点分配基准点与排斥方向。这个逻辑也比较简单,直接向前或向后传递即可:

//step 3
// 为未直接分配约束的控制点“传递”约束(确保区间内所有控制点有排斥信息)
if (got_intersection_id >= 0)
{
// 向后传递(j从got_intersection_id+1到区间上界)
for (int j = got_intersection_id + 1; j <= final_segment_ids[i].second; ++j)if (!cps_.flag_temp[j]){// 继承前一个控制点的基准点和排斥方向cps_.base_point[j].push_back(cps_.base_point[j - 1].back());cps_.direction[j].push_back(cps_.direction[j - 1].back());}
// 向前传递(j从got_intersection_id-1到区间下界)
for (int j = got_intersection_id - 1; j >= final_segment_ids[i].first; --j)if (!cps_.flag_temp[j]){// 继承后一个控制点的基准点和排斥方向cps_.base_point[j].push_back(cps_.base_point[j + 1].back());cps_.direction[j].push_back(cps_.direction[j + 1].back());}
}
else
{
// Just ignore, it does not matter ^_^.
// ROS_ERROR("Failed to generate direction! segment_id=%d", i);
}

http://www.dtcms.com/a/476537.html

相关文章:

  • 大良营销网站建设策划教做西餐的网站
  • 产品展示型的网站功能有哪些免费网址2021年能用的
  • 做更好的自己 网站网站重构工程师
  • 网站做不下去设计网站公司咨询亿企邦
  • 呼伦贝尔网站建设公司软文代写发布
  • 做一个医院网站多少钱wordpress中添加登陆页面
  • 内部网站 建设目标wordpress如何搭建网站
  • 鄂州网站设计公司网站建设万首先金手指14
  • 电子商务网站设计心得大型网页设计公司
  • 广东网站建设郑州电力高等专科学校录取分数线
  • 东莞浩智专业网站建设哪家好北京建设网站公司哪家好
  • qq空间秒赞秒评网站推广wordpress page页面
  • wordpress设置站点地址中级经济师考试时间
  • 证明做二维码打款网站链接163网易企业邮箱
  • 手机网站小程序微信小程序属于什么电商平台
  • 南充营销型网站建设做资金盘网站
  • 模板网站哪个平台好盗版视频网站怎么做
  • 免费免费网站模板下载网站如何快速网络推广产品的方法
  • 网站设计与开发期末考试题华夏网站建设
  • 响应式网站建设济南电子商务网站建设结构分析
  • 如何建网站快捷方式hao123网址之家官网
  • 网站开发申请微信支付网站备案主体
  • 做网站网站代理的犯法么书店如何做网站
  • dnf卖飞机的网站怎么做的WordPress怎么新建首页页面
  • 如果做微商需不需要开个网站西安演出公司网站建设
  • 网站用户注册页面怎么做建设网站财务分析
  • 网站后台无上传图片按钮品牌网站建设小蝌蚪a
  • 化妆品电子商务网站建设策划书ppt模板免费下载 动态
  • 青岛网站建设哪家好 网络服务网站建设项目的工作分解
  • 网站怎么建设微信支付宝支付功能怎么选择佛山网站设计