用于规范化农业种植场景的自主路径规划方法

REF：基于A* 与DWA算法的果园导航机器人研究

08797001d83cb1c24fb43bd50468f1ea.png

1. 全局路径规划

• 传统A*算法： 起始点坐标（xs,ys），当前节点坐标（xc,yc），目标点坐标（xg,yg）
  • 代价函数：f(n)=g(n)+h(n)f(n)=g(n)+h(n)f(n)=g(n)+h(n)
  • 实际代价：g(n)=(xs−xc)2+(ys−yc)2g(n) = \sqrt{(x_s-x_c)^2+(y_s-y_c)^2}g(n)=(xs−xc)2+(ys−yc)2
  • 预估代价：h(n)=(xg−xc)2+(yg−yc)2h(n)=\sqrt{(x_g-x_c)^2+(y_g-y_c)^2}h(n)=(xg−xc)2+(yg−yc)2
• 基于矩形场景的优化： 因为农场具备相对标准化的矩形场地，则使用矩形作为可行驶区域减少路径规划的计算量，下图为例，从起点使用不遇到障碍物为条件进行矩形区域扩展，直到目标点结束。

b23223d6b91ad74eca98660b0dcaa3ec.png

• 转折点选择： 为了方便机器人围绕农作物作业，横向选择靠近农作物的路径行走，为了方便机器人转弯，纵向选择可通行区域的居中位置。

26cc47e2aa452e450dfc334537692d41.png

• 转折点平滑优化： 使用相切圆的方式，进行圆弧轨迹的规划，使得圆周跟相邻线段相切，对所有转折点进行相切处理完成规划，若规划的圆弧路径出现障碍物导致阻挡，则沿着线段重新找新的圆心形成圆弧。

18fbf58fd4878f4724158d6700635101.png

2. 局部路径规划

• 传统DWA： 先根据当前机器人的位置和速度信息，在速度参数空间中生成多组线速度和角速度，将这些速度组合代入到运动模型中并模拟出移动轨迹，基于评价函数对预测轨迹进行评估，最后从所有预测轨迹中选择最佳轨迹所对应的速度组作为机器人的移动速度，如此循环直至机器人到达目标节点。
• 传统评价函数：
  • G(v,w)=σ(a⋅heading(v,w)+b⋅dist(v,w)+c⋅velocity(v,w))G(v,w)=\sigma(a \cdot heading(v,w) + b\cdot dist(v,w) + c\cdot velocity(v,w))G(v,w)=σ(a⋅heading(v,w)+b⋅dist(v,w)+c⋅velocity(v,w))
  • ℎ𝑒𝑎𝑑𝑖𝑛𝑔(𝑣, 𝜔)：方位角评价函数，当前选择的速度组预测的轨迹末端朝向与目标节点位置航向角之间的角度偏差，角度偏差越小则取值越大
  • 𝑑𝑖𝑠𝑡(𝑣, 𝜔)：障碍物评价函数，当前选择的速度组模拟的轨迹距离障碍物的最近距离，距离越大取值越大
  • 𝑣𝑒𝑙𝑜𝑐𝑖𝑡𝑦(𝑣, 𝜔)：速度评价函数，当前选择的速度组模拟的轨迹的速度大小，速度越大则取值越大；
  • 𝜎：为平滑系数，表示归一化处理；
  • 𝛼、𝛽、𝛾：相应评价函数的权重系数。
• 改进评价函数：
  • ℎ𝑒𝑎𝑑𝑖𝑛𝑔(𝑣, 𝜔)：修改为跟下一个路径点的角度差，代替原来所有都跟最终目标节点进行对比
  • 新增全局路径偏离距离评价函数：计算预测轨迹路线与全局规划路线之间的距离，确保在运动过程中机器人能更紧密地沿全局路径前进
  • 调整障碍物评价函数：区别处理静态已知障碍物和未知障碍物，降低相互干扰
  • G(v,w)=σ(a⋅heading(v,w)+b1⋅distoff(v,w)+b2⋅distk(v,w)+b3⋅distuk(v,w)+c⋅velocity(v,w))G(v,w)=\sigma(a \cdot heading(v,w) +b1\cdot dist_{off}(v,w) +b2\cdot dist_{k}(v,w) +b3\cdot dist_{uk}(v,w) + c\cdot velocity(v,w))G(v,w)=σ(a⋅heading(v,w)+b1⋅distoff(v,w)+b2⋅distk(v,w)+b3⋅distuk(v,w)+c⋅velocity(v,w))
  • 𝑑𝑖𝑠𝑡_{𝑜𝑓𝑓}(𝑣, 𝜔)：为路径偏离距离评价函数，表示预测轨迹路线与全局规划路线之间的距离，距离越小取值越大；
  • 𝑑𝑖𝑠𝑡_{k}(𝑣, 𝜔)：预测轨迹终点与已知障碍物之间的最近距离；
  • 𝑑𝑖𝑠𝑡_{uk}(𝑣, 𝜔)：预测轨迹终点与未知障碍物之间的最近距离

3. 算法融合