Webots——Fianl大作业

Webots期末大作业的实验报告

仓库地址：github

一、实验工作

本次实验我们根据要求并尝试拓展，我们完成了以下部分工作：

• 自己设计了一套基于Lidar的感知方法，基本思想是通过延迟统计，将累计击中一定次数的点判断为障碍物，以消除局部畸变带来的影响（没有直接使用助教提供的感知方法）；
• 动态路径规划部分采用了D*算法，在能完成动态路径规划的基本功能外考虑时间空间开销，使每次规划只需要考虑变化的局部情况，达到更快更好的相应效果；
• 运动控制部分采用了PID闭环控制，并尝试了多种PID，包括单环PID（位置环PID、速度环PID），以及双环PID（位置-速度串级PID）。在比较之后以小车当前位姿信息为反馈控制量构成闭环 PID 控制算法。

二、实验环境

• OS：Windows 11、MacOS
• 仿真平台：Webots 2021a
• 编程语言：C++

三、Webots元件

小车采用麦克纳姆轮，一种全向轮的方式前进，避免了小车旋转行为，简化了雷达建模操作。

小车用到的元件有：

• 麦克纳姆轮，提供全方位的行走
• GPS，提供小车的位置，便于在离散化的图中定位以及解析雷达数据
• inertial，提供小车的方向，便于判断和保持小车方向不变
• lidar，感知周围障碍物的距离，建立局部地图
• display，用于显示小车感知的障碍物（红色），小车的轨迹（蓝色），小车目前搜寻的最优路径（绿色）

四、感知部分

本次实验采用激光雷达对环境进行感知。

4.1 算法介绍

感知算法参考的是SLAM技术原理。

这里利用的是webots的lidar元件，设置为全向感知，感知层为1，去除了高度信息，以生成一张二维地图。设置项horizontalResolution表示感知的精度，maxrange表示感知的范围。

lidar的返回值可以通过函数const float *dis = lidar->getRangeImage();获得，其数组表示的是雷达检测到的各方向的距离障碍物的距离。

如图所示，dis[0]表示的值为箭头方向，然后随着下标变大，表示的方向为顺时针方向变化。其中如果某方向没有障碍物的话，其距离值同maxrange。

但由于雷达数据不稳定以及运动会造成雷达数据的畸变，我们采取了如下操作来生成一张较为完好的图。

• 保证小车的方向不变。因为雷达数据的解析是基于上述小车，一旦小车方向发生偏转，其解析的雷达数据的方位也发生相应的偏转，因此首要做的是保证小车的方向稳定。

const double *yaw = inertial->getRollPitchYaw();

if (fabs(yaw[0] - front) >= 0.05)
{
    int dir = (yaw[0] < front && yaw[0] > -front ? -1 : 1);
    for (int i = 0; i < 4; ++i)
    {
        speed3[i] = speed_rightCircle[i] * dir;
        motors[i]->setVelocity(speed3[i]);
    }
    continue;
}
else
{
    for (int i = 0; i < 4; ++i)
        speed3[i] = 0;
}

通过inertial获取小车当前的角度yaw，一旦与我们预设的角度front发生了偏转，则停止当前所有的行为，优先对小车进行旋转，姿态调整回原来的状态，并且会根据当前偏离角度选择最快的方向调整回来。

• 保证小车方向不变后，对于每次时间戳，我们解析雷达数据，并转换到离散坐标系下，但此时并不作障碍物的判断，仅仅增加某位置障碍物的计数。

count++;
const double *pos = gps->getValues();
const float *dis = lidar->getRangeImage();

// 计算障碍的计数
slam.add(lidar_point_count, dis, pos[0], pos[1]);

...

// 增加计数, 计数大于一定阈值才判定为障碍
void add(int total, const float *dis, double x, double y)
{
    for (int i = 0; i < total; ++i)
    {
        if (sign(dis[i] - max_range) == 0)// 此处无障碍
            continue;
        auto [ny, nx] = real_to_pic(x + dis[i] * cos(st - i * unit),
                                    y + dis[i] * sin(st - i * unit));
        if (nx >= 0 && nx < n && ny >= 0 && ny < m)
            cnt[nx][ny]++;
    }
}

• 每隔一定时间update_up后，再更新地图。在这里是每采取update_up次雷达数据后，对某位置有up次障碍物记录的位置才标记为障碍物，并在display中绘制出来。

if (count == update_up)
{
    count = 0;
    slam.update(display);// 更新地图
    pair<int, int> tmp = slam.real_to_pic(pos[0], pos[1]);// 当前位置
    dstar.update(tmp.first, tmp.second, display);// 重新规划路径

    loadPath(&dstar, &path);

    idx = 0; // 下标重置
    pos_pid.init();
}

...

// 通过计数更新障碍
void update(Display *display)
{
    display->setColor(RED);
    for (int i = 0; i < n; ++i)
    {
        for (int j = 0; j < m; ++j)
        {
            if (cnt[i][j] >= up)// 当前位置超过up次记录有障碍物
            {
                occupy[i][j] = 1;
                display->drawPixel(j, i);
            }
        }
    }

    for (int i = 0; i < n; ++i)
        memset(cnt[i], 0, sizeof(int) * m);
}

绘制出的示例图如下。

五、规划部分

5.1 算法介绍

D* 算法全称为 dYnamic A* 算法, 最初由 AnthonY Stentz 于论文 ““Optimal and Efficient Path Planning for PartiallY-Known Environments”” 中提出. D* 算法是一种启发式的路径搜索算法,适合面对周围环境未知或者周围环境存在动态变化的场景, 所以它特别契合于本次实验的要求.

具体介绍 D* 算法的论文链接为: 点击

与 Dijkstra 的算法和 A* 一样,D* 维护着一个要评估的节点列表,称为"OPEN list". 它是按照节点到达终点的代价来进行排序的, 节点到达终点的代价越小, 优先级越大. 节点被标记为具有以下几种状态之一：

• NEW,这意味着它从未被列入开放列表
• OPEN,表示它当前在 OPEN 列表中
• CLOSED,这意味着它已经进入 OPEN 列表并被 OPEN 列表所移除

D* 算法基于 A* 算法进行规划, 工作原理是每一轮从 OPEN 列表中迭代选择一个优先级最高, 也就是代价最低的节点并对其进行评估和邻居的拓展.

与 A* 算法不同的是, D* 算法是从终点开始进行反向搜索的, 这也体现了为什么 D* 算法适用于感知周围未知环境的原因. D* 首先会进行一条初始路径的找寻, 并对于每个节点都维护一条指向终点的路径. 具体来说, 从作为根节点的终点出发, 每个节点都指向其到达终点路径上的下一个节点作为其父节点. 这个过程基于 A* 算法的思路, 我们将到达终点的欧几里得距离作为启发函数, 并最终搜索到起点为止.

之后小车按照初始路径进行行进, 行进过程中不断感知周围的环境. 在周围环境发生变化需要重新规划的时候, 也就是小车发现当前节点指向的下一个节点是一个无法逾越的障碍时, 它就会在局部寻找其他的能够到达终点的节点, 重新更改它为当前节点指向的父节点, 这个过程是递归的过程, 会像一条波浪一样重新影响到整一条路径, 并最终到达终点, 从而实现了路径的重新规划操作.

根据节点状态的变化情况具体来说, 当其 cost 高于上次在 OPEN 列表中时设其为 RAISE 状态, 表示其 cost 小于等于上次在 OPEN 列表中时设其为 LOWER 状态. 当沿预期路径检测到障碍物时,所有受影响的点将再次放置在 OPEN 列表中,这次标记为 RAISE.但是,在 RAISE 节点成本增加之前,该算法会检查其邻居并检查它是否可以降低节点的成本.如果不是,则将 RAISE 状态传播到所有节点的后代,即具有指向它的到达终点路径上指向的节点.然后对这些节点进行评估,并将 RAISE 状态传递,形成波浪.当可以减少 RAISE 节点时,将更新其路径上的父节点,并将 LOWER 状态传递给其邻居.这些上升和降低状态的波浪是 D* 的核心.此时,一系列其他点被海浪"触及".因此,该算法仅适用于受成本变化影响的点.

D* 算法和 A* 算法另一个区别就是 D* 算法能够快速的进行重新规划. 在小车遇到障碍后, A* 算法需要进行全局的重新规划, 而 D* 算法只需要进行局部的更改就能够生成新的路径, D* 算法的效率较高. 美国火星探测器核心的寻路算法就是采用的 D* 算法.

5.2 算法详细说明

5.2.1 符号及函数说明

D* 算法维护一个 OPEN list, 它是一个可以用来做广度优先搜索的队列. 由于我们从终点开始反向搜索, 所以一开始 OPEN list 中只有一个终点.

节点的标识tag分为三类：没有加入过open表的(new)、在open表的(open)、曾经在open表但现在已经被移走的 (closed). 一开始所有的节点都设置为 new 状态.

每个节点 $X$ 到终点 $G$ 的代价为函数 $h(X)$ 所计.两个节点之间的开销为 $cost(X,Y)$,节点 $X$ 到终点的代价 $h(X)$为上一个节点(父节点)$h(Y)$ 到终点的代价加上两个节点之间的开销. 所以我们有下列式子成立.

$$h(X)=h(Y)+cost(X,Y)$$

对于每个节点来说, 我们除了维护它的实际代价, 还需要维护它的的最小代价, 用 $k$ 进行表示. 更准确的说, $k(X)$ 表示了节点 $X$ 在全图环境中到目标 $G$ 点的最小代价.

在计算和更新过程中,

• 对于标识为 new 的点,我们设置 k 与 h 相等, 即 $k=h$.
• 对于标识为 open 的点,对于新到来的 $h$ 值 $h_{new}$, 我们设置 k 为 h 的历史最小值, 即 $k=min(k,h_{new})$.
• 对于closed的点,对于新到来的 $h$ 值 $h_{new}$, 我们设置 k 为原来 h 和 $h_{new}$ 的较小值, 我们有 $k=min(h,h_{new})$.

之后会把受影响的节点状态设置为 OPEN, 并放到 OPEN list 中. 这个过程通过下列函数实现.

// 向 OPEN list 插入更新的节点
void insert(int x, int Y, double h)
{
    double k;
    if (state[Y][x] == NEW)
    {
        k = h;
    }
    if (state[Y][x] == OPEN)
    {
        k = min(kscore[Y][x], h);
    }
    if (state[Y][x] == CLOSED)
    {
        k = min(hscore[Y][x], h);
    }
    kscore[Y][x] = k;
    hscore[Y][x] = h;
    state[Y][x] = OPEN;
    OPEN list.push_back(make_pair(Y, x));
}

算法最主要的是函数是 process_state 函数,前者用于计算到目标G的最优路径. 它的具体细节我们会在后面介绍.

5.2.2 首次搜索

将终点置于 OPEN list 中,采用 A* 算法进行搜索,直到搜索到了起点,整个过程结束.搜索结束后,每个搜索过的节点标识为closed,每个节点的 $k=h$,其父节点为邻域中 $k$ 值最小的那个.

当搜索结束后,可以通过从起点开始找父节点,一路搜索到终点.若搜索结束时 OPEN list 不为空,则里头的节点 $h$ 的值必然不必起点的 $h$ 值小. 也就是说, 搜索到起点离开 OPEN list时, 首次搜索就已经结束了, 我们得到了一条从起点到达终点的路径. 但是 OPEN list 中还可能存在着代价比起点更高的节点仍未被拓展.

5.2.3 碰到障碍

若当前点的下一个点（父节点）为障碍,修改其 $h$ 值并将其放入 OPEN list 中(如果是墙的话就修改为墙的 $h$ 值,比如无穷大),但其$k$ 值仍旧不变,即 $k=min(k,h_{new})$,所以该点会被优先取出并且扩散展开.

扩散过程需要利用到论文中的伪代码 process-state,直到 $h\le k_{min}$ . 也就是如果扩散到某个节点的时候,计算后的h值不比其上次的 $k$ 值小,该点就可以结束对外扩散了.

5.2.4 整体框架

D* 算法实现的整体框架如下所示.

class Dstar
{
    public:
    int start_x, start_y, end_x, end_y, cur_x, cur_y;// 开始位置, 结束位置, 当前位置

    int m, n;// 地图大小

    int stay, flag, turn;// 用来解决卡墙问题的变量

    int **state, **nxt_x, **nxt_y;// 节点当前状态

    double **hscore, **kscore;// h值和k值

    vector<pair<int, int>> openlist;// OPEN list

    // 八个方向的邻居
    int dx[8] = {-1, -1, -1, 0, 0, 1, 1, 1},
    dy[8] = {-1, 0, 1, -1, 1, -1, 0, 1};

    vector<pair<int, int>> path;// 记录路径

    // 内存初始化
    Dstar(int start_x, int start_y, int end_x, int end_y, int m, int n);

    // 回收内存
    ~Dstar();

    // 得到OPEN list中最小k值
    double get_mink();

    // 得到OPEN list中最小k值对应的节点
    pair<int, int> get_min_state();

    // 向 OPEN list 插入更新的节点
    void insert(int x, int y, double h);

    // 节点处理函数
    int process_state();

    // 绘制路径
    void draw(Display *display);

    // 清除上次绘制的路径, 实现路径的动态变化
    void drawclear(Display *display);

    // 生成路径
    void generate_path(Display *display);

    // 初始化路径
    void work(Display *display);

    // 获取路径
    vector<pair<int, int>> get_path();

    // 遇到障碍物时进行路径更新
    void update(int cur_x, int cur_y, Display *display);

    // 重新计算节点cost
    void modify_cost(int x, int y);

    // 重新计算节点cost, 并递归更新受影响的其他节点
    void modify(int x, int y);

    // 判断一个点是否在障碍上
    int obstacle(int x, int y);

    // 计算两点之间的距离
    double calDis(int x1, int y1, int x2, int y2);
};

5.2.5 process_state 函数

process_state 函数是 D* 算法中的关键函数. 它用于处理节点之间的状态改变, 分支中包含了首次搜索以及遇到障碍物时的节点处理操作. 伪代码如下所示.

一开始从 OPEN list 中获取$k$值最小的节点,并移除该点.对该点进行分类处理,遍历其邻域,看是否需要修改父节点、$h$ 值及添加到 OPEN list 中,分类大体如下：
首先进行一次 $k_{old}$ 和 $h(X)$ 的判断,看 $X$ 的 $h$ 值是否需要调整：如果 $k_{old}<h(X)$ : 说明该节点受到障碍的影响,$X$ 处于 RAISE 状态,可以设想 $X$ 突变为墙时 $h$ 值上升,或者其父节点受到突变为墙的节点影响,导致 $h$ 值升高,最后影响到了它. 然后遍历其邻域,如果 $Y$ 点的 $h$ 值没有上升,并且 $X$ 的 $h$ 值能通过该点变得更小.上述情况,那就修改 $X$ 的父节点为 $Y$, 重置其 $h$ 的值.

然后再重新判断,看 $Y$ 的 $h$ 值是否需要调整：

$k_{old}=h(X)$: 处于第一遍遍历的阶段,或者该节点x并没有受到障碍影响. 然后遍历其邻域,if 后面的判断代表：

• $Y$ 第一次被遍历到；
• 或者 $Y$ 的父节点是 $X$,但是 $h(Y)$ 和 $h(X)+cost(X,Y)$ 值却不等, 由于 $k_{old}=h(X)$ ,这说明 $h(Y)$ 被更改了,但是$h(X)$ 还没有变；
• 又或者 $Y$ 的父节点不是 $X$, 但是如果让X成为其父节点将拥有更小的 $h(Y)$ 值.

上述三种情况都应该根据 $X$ 的 $h$ 值调整 $Y$ 的 $h(Y)$ 值,并将 $X$ 作为 $Y$ 的父节点,并将 $Y$ 添加到 OPEN list中.

$k_{old}\neq h(X)$: 说明该节点受到影响,遍历其邻域.

• 如果 $Y$ 为第一次遍历到的点；或者 $X$ 是 $Y$ 的父节点, 但是 $h(Y)$ 和 $h(X)+cost(X,Y)$ 值却不等, 这说明 $h(X)$ 被更改了,但是 $h(Y)$ 还没有变；

上述情况应该根据 $X$ 的 $h$ 值调整 $Y$ 的 $h(Y)$ 值,并将 $X$ 作为 $Y$ 的父节点,并将 $Y$ 添加到 OPEN list中.

• 如果 Y 的父节点不是X,但是让X成为其父节点将拥有更小的h(Y)值.上述情况应该应该调整x的h值,并将x添加到open表中.
• 如果 $Y$ 的父节点不是 $X$,但是让Y成为X父节点, X将拥有更小的h(x)值,并且Y被已经被open表移除,且h(Y)值在上升(即Y受到影响).

上述情况应该应该调整Y的h值,并将Y添加到open表中.

具体代码如下所示.

// 节点处理函数
int process_state()
{
    // 取所有节点中 k 值最小的
    pair<int, int> tmp = get_min_state();
    int Y = tmp.first, x = tmp.second;
    // OPEN list 为空,结束
    if (Y == -1 && x == -1)
        return -1;

    //获得该点k值,并将该点弹出Openlist队列
    double h = hscore[Y][x], k = kscore[Y][x];
    state[Y][x] = CLOSED;

    // 该点 X 处于 RAISE 状态
    if (k < h)
    {
        //遍历邻域节点 Y
        for (int i = 0; i < 8; i++)
        {
            if (x + dx[i] < 0 || x + dx[i] >= m)
                continue;
            if (Y + dY[i] < 0 || Y + dY[i] >= n)
                continue;
            int extend_x = x + dx[i], extend_Y = Y + dY[i];
            double cost = calDis(x, Y, extend_x, extend_Y);

            // 遇到障碍重新设置其 h 值和 k 值
            if (stop[extend_Y][extend_x] || obstacle(extend_x, extend_Y))
            {
                hscore[extend_Y][extend_x] = kscore[extend_Y][extend_x] =
                    1e6;
                stop[extend_Y][extend_x] = 1;
            }

            //Y 不处于RAISE 状态,且用其更新x后,X 的 h 值更小, 那么就更新 X 的父节点及其 h 值
            if (hscore[extend_Y][extend_x] <= k &&
                hscore[Y][x] > hscore[extend_Y][extend_x] + cost)
            {
                nxt_x[Y][x] = extend_x, nxt_Y[Y][x] = extend_Y;
                hscore[Y][x] = hscore[extend_Y][extend_x] + cost;
            }
        }
    }

    // 该点 X 处于 LOWER 状态
    if (k == h)
    {
        // 遍历邻域节点Y
        for (int i = 0; i < 8; i++)
        {
            if (x + dx[i] < 0 || x + dx[i] >= m)
                continue;
            if (Y + dY[i] < 0 || Y + dY[i] >= n)
                continue;
            int extend_x = x + dx[i], extend_Y = Y + dY[i];
            int back_flag = (nxt_x[extend_Y][extend_x] == x &&
                             nxt_Y[extend_Y][extend_x] == Y);
            double cost = calDis(x, Y, extend_x, extend_Y);

            // 分以下三种情况
            // Y是首次添加到OPEN list并处理
            // Y是X的子节点,并且其h值不需要改变
            // Y不是X的子节点,并且其h值可以变得更小
            if (state[extend_Y][extend_x] == NEW ||
                (back_flag &&
                 hscore[extend_Y][extend_x] != hscore[Y][x] + cost) ||
                (!back_flag &&
                 hscore[extend_Y][extend_x] > hscore[Y][x] + cost))
            {
                // 将X作为Y的父节点,修改其h值,并将Y点添加到Openlist中
                nxt_x[extend_Y][extend_x] = x,
                nxt_Y[extend_Y][extend_x] = Y;
                insert(extend_x, extend_Y, hscore[Y][x] + cost);
            }
        }
    }
    else
    {
        // 遍历邻域节点Y
        for (int i = 0; i < 8; i++)
        {
            if (x + dx[i] < 0 || x + dx[i] >= m)
                continue;
            if (Y + dY[i] < 0 || Y + dY[i] >= n)
                continue;
            int extend_x = x + dx[i], extend_Y = Y + dY[i];
            int back_flag = (nxt_x[extend_Y][extend_x] == x &&
                             nxt_Y[extend_Y][extend_x] == Y);
            double cost = calDis(x, Y, extend_x, extend_Y);

            // 分以下两种情况
            // Y是首次添加到OPEN list并处理
            // Y是X的子节点,并且其h值不需要改变
            if (state[extend_Y][extend_x] == NEW ||
                (back_flag &&
                 hscore[extend_Y][extend_x] != hscore[Y][x] + cost))
            {
                // 将X作为Y的父节点,修改其h值,并将Y点添加到Openlist中
                nxt_x[extend_Y][extend_x] = x,
                nxt_Y[extend_Y][extend_x] = Y;
                insert(extend_x, extend_Y, hscore[Y][x] + cost);
            }
            else
            {
                //Y不是X的子节点,并且其h值可以变得更小
                if (!back_flag &&
                    hscore[extend_Y][extend_x] > hscore[Y][x] + cost)
                {
                    //修改X的h值,并将其点添加到Openlist中
                    insert(x, Y, hscore[Y][x]);
                }
                //Y不是X的子节点,并且其h值可以变得更小, 并且Y不在Openlist中,且h值处于上升状态
                else if (!back_flag &&
                         hscore[Y][x] >
                         hscore[extend_Y][extend_x] + cost &&
                         state[extend_Y][extend_x] == CLOSED &&
                         hscore[extend_Y][extend_x] > k)
                {
                    //修改Y的h值,并将其点添加到Openlist中
                    insert(extend_x, extend_Y, hscore[extend_Y][extend_x]);
                }
            }
        }
    }
    //返回该点k值
    return get_mink();
}

其他的函数详见代码注释.

经过遇到障碍时对障碍节点的 h 和 k 值的设定为无限大以及状态处理函数的处理, 小车在遇到障碍时就能够快速的重新规划出路径.

六、运动控制部分

6.1 需求分析

运动控制部分的工作目标是通过获取规划部分得到的规划后路径，设计控制算法，使小车沿着期望的规划路径行驶。那么为了达到较好的控制效果，势必要采取闭环控制系统，PID就是老师不断推荐的一个很好的闭环控制算法，但PID本身也有很多分支，单环双环等，因此我们都实现了一下择效果最好的选用。

6.2 算法分析

6.2.1 PID原理分析

PID控制器（比例-积分-微分控制器），由比例单元（Proportional）、积分单元（Integral）和微分单元（Derivative）组成。可以通过调整这三个单元的增益$K_P、K_i、K_d$来调定其特性。PID控制器主要适用于基本上线性，且动态特性不随时间变化的系统。

PID控制器是一个在工业控制应用中常见的反馈回路部件。这个控制器把收集到的数据和一个参考值进行比较，然后把这个差别用于计算新的输入值，这个新的输入值的目的是可以让系统的数据达到或者保持在参考值。PID控制器可以根据历史数据和差别的出现率来调整输入值，使系统更加准确而稳定。 PID控制器的比例单元(P)、积分单元(I)和微分单元(D)分别对应目前误差、过去累计误差及未来误差。若是不知道受控系统的特性，一般认为PID控制器是最适用的控制器。借由调整PID控制器的三个参数，可以调整控制系统，设法满足设计需求。控制器的响应可以用控制器对误差的反应快慢、控制器过冲的程度及系统震荡的程度来表示。不过使用PID控制器不一定保证可达到系统的最佳控制，也不保证系统稳定性。 有些应用只需要PID控制器的部分单元，可以将不需要单元的参数设为零即可。因此PID控制器可以变成PI控制器、PD控制器、P控制器或I控制器。其中又以PI控制器比较常用，因为D控制器对回授噪声十分敏感，而若没有I控制器的话，系统不会回到参考值，会存在一个误差量。

6.2.2 串级PID

串级PID相比于普通PID，多了一个环的考虑，因而产生了内环外环的区分。

两个个PID控制器可以组合在一起，得到较佳的效果，这称为串级PID控制。以两个PID控制器组成的串级PID控制为例，其中一个PID控制器在外回路，控制像液面高度或是速度等主要的物理量，另一个PID控制器是内回路，以外回路PID控制器的输出做为其目标值，一般是控制较快速变化的参数，例如流量或加速度等。若利用串级PID控制，可以增加控制器的工作频率，并降低其时间常数。

例如一个温控的循环水浴设备有二个串级的PID控制器，分别有各自的热电偶温度感测器。外回路的控制器控制水温，其感测器距加热器很远，直接量测整体水温，其误差量是理想水温及整体水温的差值。外回路PID控制器的输出即为内回路控制器的目标值，内回路控制器控制加热器，其感测器是在加热器上，其误差量是加热器的理想温度及量测到温度的差值，其输出会使加热器维持在设定值附近。 内外回路控制器的参数可能会差很多，外回路的PID控制器有较长的时间常数，对应所有的水加热或是冷却需要的时间。内回路的PID控制器反应会比较快。每个控制器可以调整到符合其真正控制的系统，例如水槽中所有的水，或是加热器本身。

6.3 实验思路与方案选择

6.3.1 PID分析

实验中会从规划部分得到一系列期望路径上的点，因此每次控制的最终目标其实都是小车的位置，因此存在一个位置环。但程序对小车赋值控制其运动的又是小车四个轮子的速度，因此这里有个速度环。其实驱动小车轮子转动应该还有个电流环，但电流环的工作Webots已经帮我们做好了，因此无需考虑这一环，因此就存在下面三种设计方式：

• 采用基于位置环的单环PID，控制小车到达期望位置，并以每次的当前位置进行修正
• 采用基于速度环的单环PID，控制小车达到期望速度，以每次当前速度进行修正
• 采用速度环+位置环的双环PID，速度环做外环，位置环做内环，速度的变化会导致位置的变化，形成一个串级PID

分别尝试了三种实践后发现，当加入速度环的考量有个问题就是，走的过程会时而很快时而很慢。因为路径并不是连续的，二是一个一个离散的点多次追寻的结果，因此如果想使每一次到达目标位置具有目标速度(比如0)，那么在离目标位置远的时候速度会很快，到目标位置近的时候速度会降下来，但接下来又要去一个新的目标位置了，因此产生了时而很快时而很慢的效果。而采用位置环进行单环PID没有这个缺陷且能达到较好效果。因此我们最后采用基于位置环的单环PID进行实验。

6.3.2 PID参数分析

本系统采用 PID 算法来控制小车达到期望位置。小车行驶过程中，gps模块不断采集小车自身位置信息，并与期望位置进行比较使得小车的位置趋向目标位置，PID参数在本实验的含义大致如下：

• P：对小车当前的位置偏差进行调整，系数越大调节速度越快，减小误差，但是过大的比例，会造成小车速度状态的突变，从而导致小车状态不稳定，变得帕金森。
• I：加入积分调节，可以消除系统的稳态误差，提高无误差度。但系统的稳定性下降，动态响应变慢。
• D：微分调节反应的是小车的速度，可以预见偏差变化的趋势具有可预见性因而可以产生超前调节，加入微分调节可以改善系统的动态性能。

6.4 具体设计

6.4.1 PID类的封装

根据前面的算法分析和实验思路，封装了一个PID控制器类，代码及注释如下：

class PID
{
public:
    Coordinates SetPoint; //  设定目标 Desired Value
    Coordinates SumError; //	误差累计

    double Proportion; //  比例常数 Proportional Const
    double Integral;   //  积分常数 Integral Const
    double Derivative; //  微分常数 Derivative Const

    Coordinates LastError; //  Error[-1]
    Coordinates PrevError; //  Error[-2]

    PID()
    {
        Proportion = Integral = Derivative = SetPoint.x = SetPoint.y =
            SumError.x = SumError.y = LastError.x = LastError.y = PrevError.x =
                PrevError.y = 0;
    }

    /**
     * @brief 设置期望点
     * 
     * @param setpoint 期望点
     */
    void PID_SetPoint(Coordinates *setpoint)
    {
        SetPoint.x = setpoint->x;
        SetPoint.y = setpoint->y;
    }
    void init()
    {
        SetPoint.x = SetPoint.y = SumError.x = SumError.y = LastError.x =
            LastError.y = PrevError.x = PrevError.y = 0;

        Proportion = 0.97;
        Integral = 0.05;
        Derivative = 50;
    }

6.4.2 PID反馈量计算函数设计

因为小车的位置是个二维坐标，因此x轴方向和y轴方向均可分别看作一个PID，分别进行反馈量计算，具体函数实现及注释如下：

/**
 * @brief x方向PID算法
 *
 * @param CurPoint 当前位置
 * @return double PID反馈量
 */
double PID_x_PosLocCalc(Coordinates* CurPoint) {
    double iError_x, dError_x;

    iError_x = SetPoint.x - CurPoint->x;  // 偏差
    SumError.x += iError_x;               // //累计偏差，用于积分
    if (SumError.x > 10.0)                //积分限幅10
        SumError.x = 10.0;
    else if (SumError.x < -10.0)
        SumError.x = -10.0;
    dError_x = iError_x - LastError.x;  // 当前微分
    LastError.x = iError_x;

    return (double)(Proportion * iError_x    // 比例项
                    + Integral * SumError.x  // 积分项
                    + Derivative * dError_x);
}

/**
 * @brief y方向PID算法
 *
 * @param CurPoint 当前位置
 * @return double PID反馈量
 */
double PID_y_PosLocCalc(Coordinates* CurPoint) {
    double iError_y, dError_y;

    iError_y = SetPoint.y - CurPoint->y;  // 偏差
    SumError.y += iError_y;               // //累计偏差，用于积分
    if (SumError.y > 10.0)                //积分限幅10
        SumError.y = 10.0;
    else if (SumError.y < -10.0)
        SumError.y = -10.0;
    dError_y = iError_y - LastError.y;  // 当前微分
    LastError.y = iError_y;

    return (double)(Proportion * iError_y    // 比例项
                    + Integral * SumError.y  // 积分项
                    + Derivative * dError_y);
}

6.4.3 运动控制部分整体设计

首先，对于每个时间戳，通过gps获取小车当前位置，再调用PID函数，根据当前位置作为反馈量，得到应该到达的位置：

// 设置PID目标值
pos_pid.PID_SetPoint(path[idx]);

double cx = pos[0];
double cy = pos[1];

// 记录当前位置
auto [curx, cury] = slam.real_to_pic(pos[0], pos[1]);

Coordinates cur(curx, cury);

// 根据当前位置作为反馈量进行PID控制计算，并加到当前位置中
double pid_x = pos_pid.PID_x_PosLocCalc(&cur) + cur.x;
double pid_y = pos_pid.PID_y_PosLocCalc(&cur) + cur.y;

计算得到PID的期望位置后，控制速度去逼近期望位置

// 根据PID反馈量进行决策
cout << "cur: " << curx << " " << cury << endl;
keyValue1 = action_x(pid_x, curx);
keyValue2 = action_y(pid_y, cury);

char action_x(double pid, int cur) {
    double d = pid - cur;
    if (abs(d) <= 1)  return 'N';
    char ret;
    ret = d > 0 ? 'S' : 'W';
    return ret;
}

char action_y(double pid, int cur) {
    double d = pid - cur;
    if (abs(d) <= 1)  return 'N';
    char ret;
    ret = d > 0 ? 'A' : 'D';
    return ret;
}

对于每达到一个update_up，要重新规划并重置PID：

// 进行地图的更新和路径的重新规划
if (count == update_up) {
    count = 0;
    slam.update(display);                                   // 更新地图
    pair<int, int> tmp = slam.real_to_pic(pos[0], pos[1]);  // 当前位置
    dstar.update(tmp.first, tmp.second, display);  // 重新规划路径

    // 转储Path
    loadPath(&dstar, &path);

    idx = 0;         // 下标重置
    pos_pid.init();  // 初始化PID控制器
}

转储函数：

/**
 * @brief 将目标路径存储到Path中，传给运动控制部分
 * 
 * @param dstar 规划控制器
 * @param path 路径存储结构
 */
void loadPath(Dstar *dstar, vector<Coordinates *> *path)
{
    (*path).clear();
    for (int i = 0; i < dstar->path.size(); ++i)
    {
        Coordinates *ls =
            new Coordinates(dstar->path[i].first, dstar->path[i].second);
        (*path).push_back(ls);
    }
}

七、实验结果

演示视频：bilibili

八、改进空间

限于时间，本次实验对于门的处理比较粗糙，如果检测到了关上的门，那么门重新打开不会做“障碍清除”操作，这里我们简要讲述一下我们的改进思路：

感知部分可以通过实时感知和简单记忆知道某些点是原本为障碍但后面变成非障碍了，感知部分可以记录这些变成非障碍的点，并传给路径规划模块，路径规划重新调整这些点的h和k值并做D*算法的局部调整，重新规划路径，即可解决门的动态开合。

九、实验感想

本次实验作为机器人导论的最后大作业，花的时间不少，学到的东西也挺多。之前基本没有接触过这种综合性的机器人实践项目，从无到有逐渐学习，先进行需求分析，然后进行算法基本思想设计，最后通过自己实践编程完成基本功能，再针对项目的不足不断优化补充，最终取得了比较理想的结果。过程是一个不断学习的过程，在完成实践的过程中，也理解温习了老师课上讲到的内容，作为一次终期课程设计起到了很好的效果。

总的来说收获颇丰，本学期的机器人导论课程学到了不少东西，让我们这些之前对机器人、控制系统几乎一无所知的小白对这方面有了一定的理解，激发了兴趣、开阔了视野，希望以后能再接再厉。