智能搬运机器人系列之使用旭日X3派实现机器人防脱轨功能

准备工作

（1）旭日X3派

本摄像头小车上位机采用旭日X3派开发板。开发环境为Ubuntu系统下的opencv-python环境。通过HDMI外接显示器实现对两个车载USB摄像头的监测与开发，进而感知周边地图环境，通过UART与下位机的通信实现对下位机的控制。

（2）Aduino Mega 2560 Pro 开发板+IO拓展板

搬运小车的下位机使用的是以开源开发板Arduino ATmega2560 为原型改进的YWRduino mega 2560 pro开发板，它可以满足搬运小车的基本设计需求。开发环境使用的是Arduino官方提供的IDE和基于VS CODE的Platform IDE。

（3）罗技C170网络摄像头

此摄像头是用于车身位置引导和视觉循迹的摄像头，对分辨率和视野范围有较为特殊的需求。使用视频流时分辨率可以达到640*480像素，每帧图像的品质最高可以达到500万像素，在视频流获取过程中可以实现自动变焦。

（4）免驱动USB摄像头

此摄像头用于颜色识别，其镜头焦距为6.0MM，调焦范围在20MM至极远之内，视像解析度为640*480。摄像头长约3.8CM，宽1.5CM，高约3CM，配有可伸缩线圈，最长可以65CM。

（5）补光灯

补光灯使用的是自行设计的双档可调补光灯，由8个发光二极管并联而成。通过GPIO口控制三极管的通断及发光二极管的通断，实现亮度调节的目的，以满足在不同的环境条件下摄像头对环境光的需求，进而提高颜色识别的精准程度。

轨道状态

（导航地图）

搬运机器人在地图上是这样一个状态

由于拍实体机器人看起来不是很生动（主要是机器人做的太丑了），为了便于生动理解原理我使用以下图片解释。搬运机器人在每次转弯以后，摄像头正对的地方都是一条长直线（生动一点解释就是以下图二）。

（模拟车身在导航线上）

当搬运机器人中轴线跟导航线中轴线刚好重合，也就是搬运机器人正居导航线正中的时候，这种状况就是最良好的导航状况——搬运机器人正在路的正中，正上方摄像头拍到的导航线状况如下：

（导航线居于图片正中）

当搬运机器人左右发生偏移的时候，机器人与正上方摄像头拍到的导航线状况如下

（模拟车身在导航线偏右）

（模拟车身在导航线偏左）

由此可见，旭日X3派要做的事情就是，通过摄像头实时拍摄导航线的图片，并且经过一系列算法处理，得出搬运机器人相对导航线的偏离状况，并且把状况实时反馈给下位机。而下位机要做的就是接收到偏离信息，通过姿态调整算法，把搬运机器人调整回到轨道正中。

实现原理

先把拍摄到的导航线图片进行中值滤波，去除噪点。接着转化成灰度图，再把灰度图片进行二值化处理，处理到这里的时候，二值化结果中就只有0——白色的地图；255——导航线，从而使用数学方式找到导航线的中轴线。

由于摄像头跟车身位置固定，所以摄像头拍到图片的中位线就是车身中轴线所在位置，把两条中轴线的坐标还有斜率进行比对，就可以分析出搬运机器人车身相对中轴线的姿态，进而可以把分析结果通过串口通信方式发送给下位机。

导入需要用的库：

import cv2 as cv

设置串口各种参数，波特率设置为115200，使用40PIN中的UART3：

os.system('ls /dev/tty[a-zA-Z]*')

选择8号相机用作视频获取：

cap_follow = cv.VideoCapture(8)

设置两个变量分别用作记录两个中轴线的坐标：

line_1 = 0

获取一帧图片并进行中值滤波：

#第二个参数frame:表示截取到一帧的图片

（此处要解释一些为什么采用中值滤波：均值滤波、方框滤波、高斯滤波，都是线性滤波方式。由于线性滤波的结果是所有像素值的线性组合，因此含有噪声的像素也会被考虑进去，噪声不会被消除，而是以更柔和的方式存在，这时使用非线性滤波效果可能会更好。中值滤波与前面介绍的滤波方式不同，不再采用加权求均值的方式计算滤波结果，它用邻域内所有像素值的中间值来替代当前像素点的像素值。）

剪切获取到的图像，只显示和处理一正中小块（可参考下面图片，由于处理整张图片占用算力而结果跟局部处理一样，所以先裁剪图片到一小块有用区域，然后进行灰度、二值化处理）：

ROI = blur[0:210, 345:605]  # try to limit the ROI

找出搬运机器人中轴线和导航线中轴线：

n = [len(dst[:, 0]), len(dst[0, :])]

把线标注在处理后的图片上，以便调试时观察：

ROI = cv.line(ROI, (int(n[1] / 2), 0), (int(n[1] / 2), n[0]), (0, 0, 255), 2)

在电脑上显示处理后的灰度图片和标注完中轴线的灰度图便于调试时候观察（旭日X3派上的代码以删除了显示功能，因为用的是serve版本没有桌面，使用HDMI显示起来又挺麻烦而且占用算力）：

cv.imshow("gray", gray)

在调试窗口输出两个中轴线坐标差值和斜率差值：

print(int(  (n[1] / 2 - p1(n[0]))/20  ))

把两个差值处理成0-9十个梯度通过串口传送给下位机：

if line_1 < -5 and line_2 > 141:

运行效果

为了方便展示使用电脑模拟搬运机器人（自己写了一个processing程序用作模拟），主要是搬运机器人一打开电源，跑的速度很快，发生偏移和脱轨后的纠正几乎都发生在一瞬间，为了方便展示，自己写了一个模拟的小程序（搬运机器人实跑视频见文末）。

用于仿真的processing程序

此处不一一对processing的程序进行讲解，这个程序实现的功能，仿真出导航线，并且仿真出下位机。这个仿真程序可以通过串口接收到来自旭日X3派反馈的姿态数据，并且根据数据调整搬运机器人的姿态，使导航线重新回到摄像头拍摄图片正中间，即搬运机器人中轴线跟导航线中轴线重合。

import processing.serial.*;

使用电脑运行程序（因为电脑可以看到调试窗口）

右上角的两个小窗口，不带两条直线的是截取后的原始灰度图片，另外一个就是仿真出两条中轴线的模拟图。视频主体模拟的是摄像头右移，即模拟搬运机器人右移，从带有中轴线的调试窗口可看出导航线出现在了图片左边；下位机模拟程序调整搬运机器人车身，可看到屏幕上的中轴线也跟着右移，证明数据反馈和姿态调整算法发生作用。

操作旭日X3派

连接usb摄像头到旭日X3派、用usb转ttl下载器把旭日X3派跟电脑连接在一起：

上电，使用ssh登录，在线操作：

在app文件夹里面创建了一个user的文件夹，把自己写的用于测试的代码都放在这里：

在命令串口执行代码，并且打开电脑端写的仿真软件：

python3 /app/user/xunxian_pi.py

用手控制摄像头先往右移动，即模拟机器人车身偏右；再往左移，即模拟偏左，可从视频中看出，都可以很快调整回去，黑色的导航线紧跟着摄像头移动。命令串口不断反馈数值和指令，left表示向左调整，right表示向左调整，go stright表示不需要调整（指令下面两行数字分别是坐标调整值和角度值）：

原作者：衣柜旁的小明
原链接：详见地平线开发者社区（ https://developer.horizon.ai/forumDetail/106482341031035950