全自主扫雪机器人车体运动控制算法研究

邓健，李长勇，智泉卿

(新疆大学机械工程学院，新疆 乌鲁木齐 830047)

1 引言

冬季，我国北部地区降雪频繁，从1995−2017年从受灾牲畜、受灾面积、经济损失和受灾频次统计表明[1]，大雪对人民日常出行带来极大不便，随着人民生活水平的提高，人工扫雪逐渐被大型机械化除雪设备取代。

但对于一些狭窄的区域，采用大型机械化除雪存在扫雪环境区域较单一，使用全自主扫雪机器人来进行除雪可以有效清除狭窄庭院分布不规则积雪。通过对全自主扫雪机器人车体运动控制算法进行研究和实验表明，扫雪机器人的上位机视觉传感器融合超声波传感器和下位机多方向覆盖算法相结合的方法进行扫雪，相对传统的控制算法可以有效提高行走偏差率、扫雪覆盖率和减小除雪重复率。其次设计的全自主扫雪机器人内部设置有多信息运算处理的控制器中心，可以在寒冷的冬季有效降低环卫工人的扫雪劳动强度。

除此之外，通过对全自主扫雪机器人除雪的调查研究发现，扫雪机器人的智能化水平相对移动机器人技术偏低，造成在作业过程中除雪不彻底和不干净等现象。针对此现象，扫雪机的智能化研究对扫雪机器人技术的发展具有重要的意义。

2 扫雪机器人工作环境

全自主扫雪机器人工作环境，如图1所示。庭院积雪分布情况为无规则状，道路部分积雪分布符合“曼哈顿街道”模型，通过对常规的大型除雪设备进行综合评价，发现存在着扫雪机械设备笨重，扫雪效果不理想和成本较高现象。

图1 扫雪区域场景Fig.1 Scene of the Snow Clearing Area

全自主微型扫雪机体积相对较小，对庭院狭窄地区除雪具有针对性特点。针对此问题提出了全自主扫雪机器人控制算法研究，此控制算法可以通过视觉传感器提取出复杂环境中的积雪分布，把干扰点进行淡化处理，同时采用超声波传感器进行测距，使用多方向覆盖算法进行下位机优先级分选执行。

3 扫雪机器人系统

3.1 扫雪机器人车身机构

扫雪机器人主要由推雪机构、左右转动推板机构、推板升降机构、视觉部分、行走机构、浮雪升降机构、浮雪机构等组成，如图2所示。

图2 扫雪机器人车身机构Fig.2 Body Mechanism of the Snowplow Robot

扫雪机器人以车体中心为圆心，圆心向推板定义为X方向，平行地面且垂直X轴定义为Y轴方向，分别垂直X轴和Y轴且向上定义为Z轴。其中推雪机构可在−Y0X左右45°转动和X0Y左右45°转动，可以把推板上的积雪推向左右侧面；视觉传感器下面装有二轴云台，云台可在XY轴构建的平面绕Z轴175°转动，同理也可以YZ轴构建的平面绕X轴左右175°转动，视觉可以在多方位采集场景图像信息。

扫雪机器人根据机构设计原理以及实际扫雪环境需求，采取最优化设计的主要性能参数，如表1所示。

表1 扫雪机器人性能参数Tab.1 Snowplow Robot Performance Parameters

3.2 扫雪机器人控制算法

3.2.1 控制算法设计

扫雪机器人分为上位机和下位机两部分，其中主要由通讯设置图像处理、信息采集和驱动执行三个方面。控制算法流程，如图3所示。

图3 控制系统流程Fig.3 Process Control System

通讯设置根据用户需求先选取网络通讯或者usart232串口通信，然后使用视觉传感器进行图片采集，通过对图像的处理可以采集到扫雪目标位置信息，最后完成上位机操控和下位机动作执行程序接口对接完成交互。其中图像处理程序采用Microsoft Visual Studio2015且配置OpenCV2.44进行编写。信息采集使用STM32F103zet6芯片为核心的单片机，主要负责采集环境信息和接收上位机下达的指令，然后传输给下位机。驱动执行部分是由下位机驱动执行，当下位机的行走驱动电机接收到上位机界面发出的A、E、G、C、Y时，分别执行前、后、左、右、后和停止动作。当执行远程操控时当上位机界面充当采用TCP Client时，首先连接服务器分配的IP地址和端口号，其次进行远程指令下达给下位机控制板，进行发送给具体的对应的执行机构。

3.2.2 人机交互界面

扫雪机器人上位机界面程序和下位机控制程序相互独立运行，通过usart232或TCP/IP通讯建立程序交互，当不需要远程通讯时，可使用usart232通过hc−05进行通讯，如图4（a）所示。当需要进行远程通讯时可使用TCP/IP技术[2]，连接对应服务器分配的IP 和端口号进行通讯，如图4（b）所示。上位机界面采用Micro⁃soft Visual Basic 6.0开发，其中主要功能包括视觉部分、手动按键操控、语音控制、远程操控等，如图4所示。

图4 人机交互界面Fig.4 Interactive Interface of Human−Computer

扫雪机器人下位机程序在Keil uVision5 上编写，其中常编写的程序有串口通讯、PWM脉冲宽度、定时器、协议、SPI通讯和主函数具体功能实现等，可以通过设置中断优先级顺序进行依次响应。

3.2.3 不同控制算法比较

在扫雪机器人在进行扫雪时会遇到各种复杂多变障碍物环境，简化为二维平面点的集合并且选用最有效的直接搜索方法及A*算法进行仿真[3]:

式中：f(k)—在k位置对象启动点与终点的代价估计；

g(k)—在k位置对象启动点与终点实际代价估计；

h(k)—在k位置启动点与终点最佳代价估计。

实际扫雪机器人在运输过程中可能遇到多通道路况择优问题，A*算法则会预先启动式多通道路径选出一条最优通道。如图5所示。通过对A*算法的研究发现，当遇到交叉路口或者障碍物时可以选择路径的优化和避障，选取出最优一条道路。当A*算法使用在扫雪路径优化中，面临着扫雪面积覆盖率低等问题。

图5 A*算法Fig.5 A* Algorithm

采用启发式搜索算法在路径规划过程中，通过对全局面积划分为各个局部子区域，然后在子区域内建立启发式沿边清扫，可使覆盖率维持在95%，同时重复率维持在25%以下［4］。在启发式搜索算法和A*算法的基础上提出了一种新的路径−方向解析算法，扫雪机器人的扫雪路线结合最优算法对局部环境的避障分析，如图6所示。

图6 避障控制算法原理Fig.6 Algorithm Principle of Obstacle Avoidance Control

扫雪机器人坐标系为x0y，障碍物1的坐标为（x1，y1），障碍物2的坐标为（x2，y2），障碍物3的坐标为（x3，y3），目标点位置为（x4，y4），出发点位置为（m，n）。其中出发点离障碍物（1～3）的距离依次为a、b和c，距离目标点距离为d，出发点与目标点之间距离线段与机器人坐标系x轴之间夹角为i，角度j则为世界坐标系与搬运机器人坐标系下夹角。最后将目标点坐标转换为在世界坐标系下的坐标，进行优化算法分析和求解。通过对覆盖算法［5−6］和A*算法的研究发现，采用分级优先顺序方法可以降低扫雪重复率，通过设定右下角W为扫雪规划起始点，从路径规划起始点到1的方向定义为第一优先级，从起始点到2的位置为第二优先级，其中先后优先级顺序设置在下位机程序内部，通过抢占优先级和响应优先级的分配进行，当遇到障碍物时可以借助视觉传感器融合超声波测距进行优化处理，上下位机通过usart232串口实时通讯，最终通过实验过程可使重复率控制在18%内，覆盖率维持在95%以上。

首先以世界坐标系下物体位置为绝对坐标系建立扫雪机器人相对坐标，从而可以计算物体在相对坐标系下对应的绝对坐标。

如世界坐标系下目标点位置坐标公式：

同理可求出障碍物1、2、3坐标公式。

3.2.4 视觉处理

由于扫雪环境和光照强度影响，需要对视觉传感器采集图片进行滤波处理，为了能准确获取扫雪目标信息，需要对采集照片进行灰度处理→二值化→降噪［7］，如图7所示。

图7 扫雪环境处理前后Fig.7 Before and After Snow Removal Environment Treatment

在扫雪场景中需要对积雪覆盖的区域进行提取，并设置灰度值为R=0.3a+0.59b+0.11c[8]突出目标A，设定为白色，同时把背景淡化并设置为黑色。

视觉远距离存在着畸变现象，并且远离与畸变成正比，可以用以下公式来表示[9−10]:

在畸变变化允许范围内进行处理可以有效降低定位误差。

4 实验测试数据

实验过程进行50次，实验中扫雪机运行的路线场景为学校篮球场内，任取一块（20×10)m直线路面进行实验，如表2所示。首先设定起点和终点后，从中均匀截取10个部分，从履带在雪地的压痕坐标可计算出第一部分偏移量误差和角度。然后第二部分继续测出偏移量误差和角度，依次递增，按照同样方法和步骤测完剩下的8部分偏移量误差和角度，实验中每部分进行5次，取失误次数相对少的三次运行轨迹作为实验样本，求取平均值和方差。

表2 试验路径数据Tab.2 Test Path Data

起点偏差角度和起点偏移距离，如图8所示。从图中可以看出在100cm内，起点偏移距离维持在（±2.17）%内，起点偏差角度维持在（±0.37）%内，根据得出结论可合理定制扫雪机器人带有障碍物的扫雪路线且符合扫雪机器人扫雪的精确控制要求。通过对实验数据的分析发现产生误差原因主要有雪面和履带之间产生的打滑和人工测量精确度误差，其中起点偏角为了避免误差产生，本实验采取大量重复实验作为预备实验，最终完成实验数据精确度的要求。

5 结论

在现有的全自主扫雪机器人控制算法上进行优化融合，采用图像处理融合超声波传感器测距和下位机路径—方向解析算法结合，最终使机器人在雪地场景100cm内，起点偏移距离维持在（±2.17）%内，起点偏差角度维持在（±0.37）%内，扫雪覆盖率维持在95%以上和重复率维持在20%以下，满足一定范围全自主除雪需求。