【路径规划】如何给路径增加运动对象

前言

在上一篇的路径规划文章中，使用多段五阶贝塞尔曲线优化RRT搜索后的路径，虽然视频上看起来还好，但还是不够好玩，为了让其有趣一些，突然想增加一个运动对象(例如一个三角形)，在生成平滑曲线后，沿着平滑曲线运动，这个是本期想要实现的目标，在实现之后，后面可以考虑再增加一些运动模型来使得这个运动对象符合生活中的一些运动对象。

画三角形

首先我们使用一个三角形来表示一个运动对象的话，需要找到一个画三角形的方法，这个需要借助 matlab 的 fill 函数，这个函数的作用大致是这样：给出几个点的横纵坐标，然后使用 fill 函数可以将这几个坐标点围成的封闭图形用颜色填充，并可以构造一个对象。

下面是我给出的一个示例程序：

它执行后的效果就是这样的：

上图中的左上角黄色大三角就是这个代码的效果。这里有一个我自编的生成三角形三个点坐标的函数 Triangle ，除了fill，就是这自编函数需要自行理解了，这里可以介绍一下生成坐标的基本原理，这个自编函数的代码如下：

%*************************************************************************%
                %pos：三角形底边中心坐标
                %yaw：三角形指向点
                %size：三角形大小
                
                %      (1)
                %       *
                %      * *
                %     *   *
                %    * * * *
                %  (2)     (3)
                %x1,x2,x3：生成的三角形的三个顶点的x坐标
                %y1,y2.y3: 生成的三角形的三个顶点的y坐标
%**********************************************************************%
function [x,y] = Triangle(pos, yaw, size)
% pos 定义三角形底边中心坐标
% vec 定义三角形方向
% dis 定义三角形大小
pos_yaw = [yaw(1) - pos(1), yaw(2) - pos(2)];  %求中线向量
pos_yaw_I = pos_yaw / sqrt(pos_yaw(1) * pos_yaw(1) + pos_yaw(2) * pos_yaw(2)); %求单位向量
 
P1 = pos + pos_yaw_I * 3 * size;   %求1点坐标
 
%求2点坐标
P2_P3 = [1,-pos_yaw_I(1) / pos_yaw_I(2)];  %求垂直于pos_yaw_I的向量
P2_I = P2_P3 / sqrt(P2_P3(1) * P2_P3(1) + P2_P3(2) * P2_P3(2));  %求单位向量
P3 = pos + P2_I * size;   %求P2点的坐标
%求3点坐标
P3_P2 = [-1,pos_yaw_I(1) / pos_yaw_I(2)];  %求垂直于pos_yaw_I的向量
P3_I = P3_P2 / sqrt(P3_P2(1) * P3_P2(1) + P3_P2(2) * P3_P2(2));  %求单位向量
P2 = pos + P3_I * size;   %求P3点的坐标
 
x = [P1(1),P2(1),P3(1)];
y = [P1(2),P2(2),P3(2)];

最开始我是想使用三角函数来求解的，加入我传进来的参数中 pos 代表的是三角形底边的中点，我们最后是要生成一个等腰三角形，中点距离两边顶点的距离都为 size，pos距离另一个顶点的距离为 3*size ，这个三角形的中线指向的方向是 pos - yaw，yaw点就是要指向的坐标点，示意图如下：

直接用三角函数的话，也可以求出来，但是当这个三角形的底边与X轴不平行的时候，求起来会特别麻烦，因此我就使用的向量法来求的坐标：

我们可以通过求向量 pos_yaw ，然后就可以求出 pos_yaw 的单位向量 pos_yaw_I，之后就可以通过pos的坐标加上单位向量乘以3*size长度的方式就求得了P1点的坐标，计算过程还是比较简单的，之后我们就可以求解P2，P3坐标，由于P2，P3与向量pos_P1垂直，因此求解过程需要绕一下，不过还是相对比较简单。

我们知道互相垂直的向量点乘为0，例如向量a与向量b垂直，那么a.x * b.x + a.y * b.y = 0成立，我们可以按照这个规律来求得P2与P3坐标：

动画

我们得到了计算三角形坐标的方法之后，就是制作动画，在上一期中的贝塞尔曲线生成的优化路径点，我们可以全部保存，之后就是让这个三角形动起来。还记得上面也说过一次吗？我们在使用fill函数时，是可以得到一个对象的，这个对象是可以使用 set 函数进行重置的，也就是可以只擦除这个封闭对象，然后重新画图，相当于是移动，代码如下：

pos = [route(route_count,1),route(route_count,2)];
yaw = [route(route_count-1,1),route(route_count-1,2)];
size = 8;
[x,y] = Triangle(pos, yaw, size);
triangle1 = fill(x,y,'y');   %定义一个三角对象triangle1
 
for i = route_count:-1: 2
    pos = [route(i,1),route(i,2)];   %读取路径点
    yaw = [route(i-1,1),route(i-1,2)];  %读取下一个路径点
    size = 8;
    [x,y] = Triangle(pos, pos + [1,1], size);  %得到三角形坐标
    set(triangle1,'XData',x,'YData',y); %%重置绘图对象
    hold on;
    pause(0.01); %暂停0.1s，使得RRT扩展过程容易观察    
end

之后做成的效果就是这样的：

是不是逐渐开始好玩了