带有运动跟踪和激光雷达的机器人汽车路径规划

描述

该项目是 UIUC SE423 的最终项目。

该项目的目标是让机器人汽车在建造的轨道上导航，同时避开途中的随机障碍物。这个项目有几个主要组成部分。

• 控制机器人小车并使用航位推算获得机器人小车的位姿
• 使用 F28379D Launchpad 处理激光雷达读数
• 使用激光雷达识别障碍物
• 使用 A* 算法查找路径
• 与运动跟踪系统通信以在路径中导航

控制机器人汽车

我们使用 F28379D 启动板的增强型正交编码器脉冲 (eQEP) 外围设备来获取我们机器人汽车中使用的电机的角度。有了这个，很容易得到机器人汽车的速度与采样时间和行驶距离。然后，我们使用 PI 控制器来控制机器人汽车。理想情况下，假设车轮和地板之间没有滑动，我们可以通过航位推算来计算机器人汽车的姿态。

处理激光雷达数据

对于这个项目，便宜的 YDLidar X2 型号效果很好。它可以以 360° 视角感应0.10 到 8.0m 的范围。这款激光雷达的一大优点是它会在开机时自动开始收集数据

激光雷达可以通过串口传输数据。在这个项目中，SCID用于接受数据，激光雷达的数据通过launchpad的pin 9进来。

我使用 8 状态状态机来解释和存储激光雷达数据。得到的数据是一个包含 360 个条目的数组形式，每个条目包含激光雷达检测到的点的距离和时间戳信息，这个数组的索引是收集数据的角度。

使用激光雷达检测障碍物

得到上一步的激光雷达数据后，我们得到了机器人看到的信息：机器人在中心，有一段距离对应从0到359°的所有整数角。因此，极坐标图足以让我们可视化结果。

在我们的项目中，我们在机电实验室里搭建了一个12*12英尺的轨道，供机器人小车在里面跑，障碍物是2*2英尺的木块，只能放在轨道的整数格子上。

这种设置极大地简化了我们的寻障程序：根据收集到的数据，我们可以在给定机器人车的初始条件（pose_x、pose_y、pose_theta）的情况下，通过坐标变换获得轨道的“全局视图”。然后，将轨道范围内的障碍物读数四舍五入应该足以让我们得到障碍物的边缘，因为障碍物只能出现在整数网格上。下面的初始条件对应上图第二个，其中pose_x是-4，pose_y是6，pose_theta是30°。

//initial condition
double pose_x = -4; //x_position of the robot car
double pose_y = 6; //x_position of the robot car
//need to put negative initial angle because the Lidar is rotating CW
double pose_theta = 0; //angle of robot car
double pose_rad; //angle in rad

坐标变换在 main 函数内的 while 循环中完成。乒乓缓冲区用于确保有足够的时间来处理所有数据。在下面的代码中，我首先遍历缓冲区数组中的所有点。如果激光雷达 0.1 m 范围内的障碍物（激光雷达无法检测到，因此距离为 0），我将其放在原点，该原点对应于全局原点。

if (pingpts[i].distance ==0) {
        x_f[i].distance = 0;
        y_f[i].distance = 0;
        x_f[i].timestamp = pingpts[i].timestamp;
        y_f[i].timestamp = pingpts[i].timestamp;
}

否则，我首先将极坐标转换为笛卡尔坐标，并使用基方程的变化来获得检测点的“全局”位置。x_ori和y_ori是极坐标到笛卡尔坐标的直接结果，而x_f和y_f是变化基和平移的最终结果。

else {
        x_ori[i] = pingpts[i].distance*cos(i*0.01745329);//0.017453292519943 is pi/180
        y_ori[i] = pingpts[i].distance*sin(i*0.01745329);
        x_f[i].distance = (x_ori[i])*cos(pose_rad)+(y_ori[i])*sin(pose_rad)+pose_x; //change basis
        y_f[i].distance = -(x_ori[i])*sin(pose_rad)+(y_ori[i])*cos(pose_rad)+pose_y; //change basis

最后，通过仅考虑范围内的障碍物的简单标准，我们可以在阵列x:(-6,6), y:(0,12),上将检测到的点标记为障碍物“x” 。mapCourseStart这mapCourseStart是一个包含 176 (11*16) 个元素的字符数组，用于存储轨道网格交叉点的地图。它在初始化期间包含所有“0”，这意味着还没有障碍物。

char mapCourseStart[176] =      //16x11
{   '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0'   };

机器人 1 格内的点不被考虑，因为它可能是一些随机数据收集错误。mapCourseStart障碍物的全局位置(x, y) 与数组的索引之间存在双射对应关系，11*(11-y)+5-x.这里(x, y) 是经过坐标变换和平移后激光雷达读数的位置。

// code to process the obstacle
        if ((round(x_f[i].distance) >-6) && (round(x_f[i].distance) <6) ) { // within x range
            if ((round(y_f[i].distance) > 0)&& round(y_f[i].distance) < 12) { //within y range
                if ((fabs(x_f[i].distance-pose_x) > 1) && (fabs(y_f[i].distance-pose_y) > 1)) { //neglect points that are too close to the robot car
                    mapCourseStart[(11*(11-(int)(round(y_f[i].distance))))+5+(int)(round(x_f[i].distance))] = 'x'; //set 'x' at correct location
                }
            }
        }
        x_f[i].timestamp = pingpts[i].timestamp;
        y_f[i].timestamp = pingpts[i].timestamp;
    }
}

下图是mapCourseStart用网格覆盖阵列的图。的点位于mapCourseStart每个图块的中心。

因此，在初始条件pose_x= -4、pose_y= 6 和= 30° 的机器人上看，数组pose_theta中索引为 18、29、40、41、42 的点应该从“0”变为“x” mapCourseStart.

从 Code Composer Studio 的表达式窗口检查mapCourseStart数组的结果，我们从代码中得到预期的结果，索引 18、29、40、41、42 是障碍物 ('x')。

下一步是使用路径规划算法来找到机器人要走的路径。

使用对角线步骤实现 A* 算法

A* 是一种路径规划算法，它同时考虑了行进距离（g）和从当前点到目的地的距离（启发式值，h）。然后它会选择去g+h值最小的点。更详细的解释可以在 https://www.redblobgames.com/pathfinding/a-star/introduction.html 找到。

前几个学期的代码只考虑了水平和垂直方向的移动，这里的启发值是由 给出的曼哈顿距离|x1-x2|+|y1-y2|。但是，如果我们考虑对角线移动，它会更有效。下面是我添加到getNeighborsA* 算法函数中的代码。要进行对角线移动，我们需要确保相邻的垂直或水平邻居是可达的。也就是说，如果我们想去左上邻居，我们需要确保我们可以同时到达左上邻居。

//top left corner, only can travel when both top and left are reachable
	if ((canTravel(rowCurr - 1, colCurr) == 1) && (canTravel(rowCurr, colCurr - 1) == 1)) {
		if (canTravel(rowCurr - 1, colCurr - 1) == 1) {
			nodeToAdd.row = rowCurr - 1;
			nodeToAdd.col = colCurr - 1;
			neighbors[numNeighbors] = nodeToAdd;
			numNeighbors++;
		}
	}
	//top right, need top and right reachable
	if ((canTravel(rowCurr - 1, colCurr) == 1) && (canTravel(rowCurr, colCurr + 1) == 1)) {
		if (canTravel(rowCurr - 1, colCurr + 1) == 1) {
			nodeToAdd.row = rowCurr - 1;
			nodeToAdd.col = colCurr + 1;
			neighbors[numNeighbors] = nodeToAdd;
			numNeighbors++;
		}
	}
	//bottom left, need bottom and left reachable
	if ((canTravel(rowCurr + 1, colCurr) == 1) && (canTravel(rowCurr, colCurr - 1) == 1)) {
		if (canTravel(rowCurr + 1, colCurr - 1) == 1) {
			nodeToAdd.row = rowCurr + 1;
			nodeToAdd.col = colCurr - 1;
			neighbors[numNeighbors] = nodeToAdd;
			numNeighbors++;
		}
	}
	//bottom right, need bottom and right reachable
	if ((canTravel(rowCurr + 1, colCurr) == 1) && (canTravel(rowCurr, colCurr + 1) == 1)) {
		if (canTravel(rowCurr + 1, colCurr + 1) == 1) {
			nodeToAdd.row = rowCurr + 1;
			nodeToAdd.col = colCurr + 1;
			neighbors[numNeighbors] = nodeToAdd;
			numNeighbors++;
		}
	}

如果我们可以对角线旅行，则启发式值更接近欧几里得距离而不是曼哈顿距离。因此，我更新了启发式值函数，并将所有与距离相关的值从整数类型更改为双精度类型。

double heuristic(int rowCurr, int colCurr, int rowGoal, int colGoal)
{
	int rowDiff = rowCurr - rowGoal;
	int colDiff = colCurr - colGoal;
	return sqrt(rowDiff * rowDiff + colDiff * colDiff);
}

最后，对角线行进的距离是 sqrt(2)，而不是之前的垂直和水平移动中的 1。所以我更新了行进距离计算。

if (abs(next.row - minDistNode.row) + abs(next.col - minDistNode.col) == 2) {
    next.distTravelFromStart = currDist + sqrt(2);
} else {
    next.distTravelFromStart = currDist + 1;
}

以下是小型、中型和大型地图路径规划的测试用例。我们可以看到，对角线行驶确实可以节省一些步数（pathLen）和行驶距离（next.distTravelFromStart值），因此效率更高。

A*算法中的点在nodeTrack数组中被跟踪，调用函数后，我们可以得到从起点到终点的正确顺序的reconstructPath路径。pathRowpathCol

从点到点的导航是通过提供的xy_control函数完成的。我们需要将具有正确索引的pathRowand值传递给函数。pathCol

与运动跟踪系统通信

我们可以使用航位推算来获取我们机器人汽车的位姿数据。但是，由于车轮在运行过程中可能会打滑，因此航位推算会出现一些误差，这些误差可能会随着时间的推移而累积。因此，为了确保机器人汽车按预期运行，我们使用机电一体化实验室设置的 Optitrack 系统来获得我们的机器人汽车更精确的姿势。

为了使 Optitrack 系统正常工作，我们首先需要将机器人小车设置为 0°，机器人的坐标方向与全局坐标对齐。在Motive软件中选择设置在机器人车顶部的反光球，并运行通过连接实验室计算机的路由器发送数据的脚本后，我们只需要找到一种接收数据的方法即可。在这里，我使用了 Wiznet W5500 芯片来实现这一点。W5500 芯片是一款硬连线 TCP/IP 嵌入式以太网控制器，可通过 SPI（串行外设接口）为嵌入式系统实现更轻松的互联网连接。

W5500接GPIO122、123、124、125，分别对应launchpad的12、13、17、18脚。数据接收采用UDP协议，机器人小车作为UDP服务器，与Optitrack连接的计算机作为UDP客户端。

我使用的以太网转换器模块为我提供了将路由器连接到机器人汽车的能力。将路由器设置为客户端模式（通常用于没有wifi功能但有以太网端口的设备的模式）并将其连接到配置网页中发送数据的路由器，机器人小车可以从Optitrack系统中获取数据.

为了避免launchpad的CPU1太忙而搞砸了，我在这个项目中使用了CPU2来设置UDP服务器，接收和处理来自OptiTrack的数据。一般来说，要使用CPU2，我们首先需要确保CPU2中使用的GPIO在CPU1初始化GPIO时，其所有权设置为CPU2。这里，我们需要在CPU1的main函数中设置GPIO 0、122、123、124对CPU2的所有权。

//wiznet reset
    GPIO_SetupPinMux(0, GPIO_MUX_CPU2, 0);
    GPIO_SetupPinOptions(0, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPASET.bit.GPIO0 = 1;

    GPIO_SetupPinMux(125, GPIO_MUX_CPU2, 0); // wiznet cs
    GPIO_SetupPinOptions(125, GPIO_OUTPUT, GPIO_PUSHPULL); // Make GPIO2 an Output Pin
    GpioDataRegs.GPDSET.bit.GPIO125 = 1; //Initially Set GPIO2/SS High so wiznet is not selected
    GPIO_SetupPinMux(122, GPIO_MUX_CPU2, 6); //SPISIMOC/WIZNET
    GPIO_SetupPinMux(123, GPIO_MUX_CPU2, 6); //SPISOMIC/WIZNET
    GPIO_SetupPinMux(124, GPIO_MUX_CPU2, 6); //SPICLKC/WIZNET

然后我们需要在DevCfgRegs寄存器中将我们使用的外设设置为 1。这里我们使用 SPIC 进行通信，因此我们使用CPUSEL6.

EALLOW;
DevCfgRegs.CPUSEL6.bit.SPI_C = 1;
EDIS;

在 CPU1 中进行基本设置后，我们可以放心地将所有 UDP 服务器代码放入 CPU2 的函数中。我在这里为 CPU2 使用了一个单独的项目。

对于 Optitrack 数据，它采用 x、y、theta、刚体数和帧数的形式。这些数据每个是 2 个字节，但我们每次只从客户端接收 1 个字节。因此，我们需要将数据组合在一起以进行整个阅读。这里我使用了一个 union 来存储从 Optitrack 接收到的一组数据，它有一个rawData类型为 uint16_t 的Data数组和一个类型为 float 的数组。

typedef union optiData_s {
    uint16_t rawData[10];
    float Data[5];
} optiData_t;

然后通过组合接收到的数据，我们得到每个 2 字节的原始数据，并将这 2 字节的数据组合为 4 字节的浮点数，我们从 Optitrack 恢复数据。最后，通过将 CPU2 中的 IPC 寄存器设置为 1，我们中断 CPU1 接收数据。

recvsize = recvfrom(0, (uint8_t *)buffer, len, recvfrom_ip, &recvfrom_port);
mydata.rawData[0] = buffer[0] | (buffer[1] << 8);
mydata.rawData[1] = buffer[2] | (buffer[3] << 8);
mydata.rawData[2] = buffer[4] | (buffer[5] << 8);
mydata.rawData[3] = buffer[6] | (buffer[7] << 8);
mydata.rawData[4] = buffer[8] | (buffer[9] << 8);
mydata.rawData[5] = buffer[10] | (buffer[11] << 8);
mydata.rawData[6] = buffer[12] | (buffer[13] << 8);
mydata.rawData[7] = buffer[14] | (buffer[15] << 8);
mydata.rawData[8] = buffer[16] | (buffer[17] << 8);
mydata.rawData[9] = buffer[18] | (buffer[19] << 8);
recvcount += 1;
cpu2tocpu1[0] = mydata.Data[0];
cpu2tocpu1[1] = mydata.Data[1];
cpu2tocpu1[2] = mydata.Data[2];
cpu2tocpu1[3] = mydata.Data[3];
cpu2tocpu1[4] = mydata.Data[4];
IpcRegs.IPCSET.bit.IPC0 = 1;

下图是从 CPU2 读取的结果。

然后，我们为两个 CPU 创建了一个浮点数组cpu2tocpu1来存储来自 Optitrack 的数据。然后我们在 cmd 文件中找到cpu2tocpu1ram 的位置，将两者设置在 CPU1 和 CPU2 中的相同位置，然后我们可以将值写入 CPU2 中的这个数组并在 CPU1 中检索它。

//IPC
__interrupt void CPU2toCPU1IPC0(void){
    GpioDataRegs.GPBTOGGLE.bit.GPIO52 = 1;
    x = cpu2tocpu1[0];
    y = cpu2tocpu1[1];
    theta = cpu2tocpu1[2];
    number = cpu2tocpu1[3];
    framecount = cpu2tocpu1[4];
    OPTITRACKps = UpdateOptitrackStates(ROBOTps);
    ROBOTps.x = OPTITRACKps.x;
    ROBOTps.y = OPTITRACKps.y;
    ROBOTps.theta = OPTITRACKps.theta;
    UARTPrint = 1;
    IpcRegs.IPCACK.bit.IPC0 = 1;
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}

要运行它，我们首先需要build 使用 CPU2 的项目和debug CPU1 项目。CPU1 运行后，右键单击 cpu2 和connect target. run菜单下 ， load CPU2.out 文件。最后，运行 CPU1，然后运行 ​​CPU2。

有了 Optitrack 数据，我们可以使用它来控制xy_control功能。下面是一个演示视频，展示了机器人汽车通过运动跟踪系统到达几个预设点并告诉它它的位置。

结合障碍物检测和 A*

把东西放在一起后，我们可以让我们的机器人汽车自动行驶到目的地，避开障碍物。

下面是机器人避开障碍物并前往所需点的视频。

它也可以穿过一个小迷宫。

学分

感谢 Dan Block 教授帮助我解决了我在这个项目中遇到的所有问题并提供了必要的入门代码。

感谢 TA Hang Cui 对 UDP 通信的帮助和 CPU2 的入门代码，

感谢 Scott Manhart 设计激光雷达支架。