项目背景

  • Robocon全国大学生机器人大赛
  • 2019年快马加鞭主题，MR1全向轮底盘机器人

  • 半自动流程控制

    任务目标

  • 设计路线绕过障碍，符合比赛规则要求

  • 路径自动生成
  • 寻路径移动
  • 场地地图

image.pngimage.png

项目效果

“追风号”半自动四轮机器人 (1).gif

基本思想

  • 五段法生成一组路线
    • 起始直线
    • 起始圆弧
    • 中间直线
    • 终点圆弧
    • 终点直线
  • 实现绕过两个障碍物的功能（S型路径）
  • 完整路径由多个五段组成

image.png

工程组成

【路径规划】路径生成 - 图5

数学库

  • 在路径规划中用到的数学知识均与平面解析几何相关

    矢量运算

  • 这里仅用到平面矢量，将矢量定义为一个二维数组

  • 而有向线段，使用一个起点坐标和矢量作为成员的结构体定义
  • 提供的函数接口如下 ```c void VectorReset(double thi);//重置参数数组为0 void VectorCopy(double thi, double tha);//向量拷贝 void VectorPandP(double thi, Point_int32 StartPoint, Point_int32 EndPoint);//向量以点到点格式 void VectorNandA(double* thi, double norm, double argument);//向量以模和辐角格式

bool VectorisZero(double thi);//零向量判断 int VectorgetQuadrants(double thi);//获得向量的所在象限 double VectorgetAngle(double thi, double tha);//计算两向量夹角 double VectorgetArgument(double thi);//计算向量的辐角(与基向量的夹角) double VectorgetLength(double thi);//获得向量长度

double VectorInnerproduct(double thi, double tha);//向量点乘(内积) void VectorReverse(double result, double thi);//向量反向 void VectorgetDirectionVector(doubleresult, Line line);//以向量记录直线方向 void VectorAdd(doubleresult, double thi, double tha);//向量相加 void VectorMul(doubleresult, double Num, double tha);//向量数乘

  1. <a name="Sm6g8"></a>
  2. ## 解析几何
  4. - 这里描述了圆、直线的几何关系，方程求解
  5. - 圆通过成员为圆心和半径的结构体定义
  6. - 直线通过成员为直线三参数，斜率的结构体定义
  7. - 提供的函数接口如下
  8. ```c
  9. //三点生成一个圆
  10. Circle CreateCircle(Point_int32 point1, Point_int32 point2, Point_int32 point3);
  12. //以点向式生成直线解析式(bx - ay + (ay0 - bx0) = 0) <点(x0, y0), 向(a, b)>
  13. Line CreateLineByPandV(Point_int32 StartPoint, double *Vector);
  14. //以点斜式生成直线解析式(kx - y + (y1 - kx1) = 0)
  15. Line CreateLineByPandK(Point_int32 point, double k, bool isK_NotExist); 
  16. //以两点式生成直线解析式
  17. Line CreateLineByPandP(Point_int32 StartPoint, Point_int32 EndPiont);
  18. //生成与已知直线垂直的直线解析式
  19. Line getVerticalLine(Point_int32 point, Line line);
  21. //生成两点连线的中垂线
  22. bool getMidperpendicular(Point_int32 point1, Point_int32 point2, Line* line);
  23. //生成过确定点，针对目标圆的切线
  24. bool getTangentLine(Point_int32 P, Circle C, Line* lines, Point_int32* Points);
  25. //生成两圆的外公切线
  26. bool getExternalCommonTangent(Circle C1, Point_int32* C1_Points, Circle C2, 
  27.                               Point_int32* C2_Points, Line* lines);
  28. //生成两圆的内公切线
  29. bool getInternalCommonTangent(Circle C1, Point_int32* C1_Points, Circle C2, 
  30.                               Point_int32* C2_Points, Line* lines);
  31. //获取两直线的交点
  32. bool GetIntersection(Line line1, Line line2, Point_int32* result);
  33. //生成由一个有向线段确定的一个圆的外切圆
  34. bool getExcircle(Point_int32 P, double *Vec, Circle C, Circle excircle);
  35. //生成角内内切圆（内切圆半径由边点1决定）
  36. bool CreateIncircle(Point_int32 PeakPoint, Point_int32 DecisionPoint, 
  37.                     Point_int32 SidePoint, Circle result);
  39. //获取两点连线的中点
  40. Point_int32 GetMidpoint(Point_int32 point1, Point_int32 point2);
  41. //获得两个向量的逆时针旋向的夹角（向量1逆时针转多少度就能转到向量2的位置）
  42. double getAnticlockAngle(double *StartVec, double *EndVec);
  43. //获得某点到某直线的距离
  44. double getDistanceToLine(Point_int32 point, Line line);
  45. //获得两点间的距离
  46. double GetDistanceToPoint(Point_int32 point1, Point_int32 point2);
  47. //获得顶点与另两点之间的夹角
  48. double GetAngleToLineByPangP(Point_int32 Origin_Point, Point_int32 Point1, Point_int32 Point2);
  49. //获得两直线的夹角
  50. double GetAngleToLineByLandL(Line line1, Line line2, bool isGetSupplementary);
  52. //判断两条射线是否相交
  53. bool isIntersectant(DSegment Radial1, DSegment Radial2);
  54. //判断两个圆是否相同
  55. bool isSameCircle(Circle C1, Circle C2);
  56. //转换值
  57. Point_int32 TransformToInt32(Point_float Point);

轨迹描述

  • 这里指生成的轨迹的存放形式

  • 由结构体的嵌套组成各段路径的关键参数 【路径规划】路径生成 - 图6

    轨迹生成

  • 将数学库有效的利用起来，结合设定信息，选择符合要求的路径

  • 主要利用的是解析几何中直线与圆弧的相切关系，再做取舍
  • 提供的函数接口如下 ```c void PositionConstParamsInit(void); bool CreateStart_EndLine(struct MoveInLine *Track, DSegment Start, Circle CalcuCircle, 
    1.                    DSegment barrier, bool isPointToCircle, DSegment Otherbarrier)
    bool CreatMiddleLine(struct MoveInLine *Track,
    1.               Circle C1, DSegment Barrier1, struct MoveInLine *Startline,
    2.               Circle C2, DSegment Barrier2, struct MoveInLine *Endline,
    3.               Point_int32 C2_EndPoint)
    bool CreatStart_EndCircle(struct MoveInCircle *Track, Point_int32 StartPoint, Point_int32 EndPoint, 
    1.                   double *SVec, double *EVec, Circle circle)
    bool CreatPointToPointMotionTrack(MoveTrack_PointToPoint *Track,
    1.                             DSegment Start, DSegment S_barrier, double S_Protected_R,
    2.                             DSegment End, DSegment E_barrier, double E_Protected_R);

```