基于openmv的人形机器人轨迹跟踪方法设计

于之洋，魏友业，张剑诠，陈鸿林，戴峻峰

（淮阴工学院 电子信息工程学院，江苏 淮安 223003）

0 引言

在当今的机器人技术领域内，研制出与人类具有着类似行走能力与思考能力且能够随着环境变化自行调整的双足形仿人行走机器人一直是学术界的焦点和大众关心的热点问题。而其中，仿人机器人的视觉轨迹跟踪方法一直是学术界研究的重点，虽然经过了多年的研究，但是当今的仿人机器人的寻迹方法在稳定性、自适应能力等方面仍有着极大的提升空间。这些能力的实现和提升还依赖于机械结构与算法设计的共同发展[1]。

本文提出了一种基于openmv的人形机器人轨迹跟踪方法设计（以下简称轨迹跟踪方法）。该轨迹跟踪方法以多帧图像为一个周期，根据工程实际对处理周期内各个帧的结果赋予不同的加权值的方法进行设计。首先，分析了现有的具有视觉识别功能的人形机器人所存在的缺陷问题，针对其缺陷改进了机器人结构，并设计了基于单目视觉环境的轨迹跟踪算法。

根据对工作环境信息掌握的程度不同，机器人路径规划可分为：环境信息完全已知的路径规划和环境信息完全未知或部分未知的路径规划[2]。目前，为实现人形机器人低速环境下的自主导航，学界内多半选用在环境信息完全已知的路径基础上进行局部避障的方式。而轨迹跟踪作为机器人的导航方式之一，能够保证机器人在规定时间运动在指定的状态附近[3]。

由于人形机器人在运动过程中视觉算法在不间断地工作，让机器人在一次运动过程中强行停止转为去做其他动作无疑是不现实的，故而视觉算法所得出的大部分运动指令为冗余数据。在文中选择将这些冗余数据通过加权计算的方式，对当前运动指令的正确性做出判断。若判断为错误，则直接将此次运动指令作废处理，此方法可极大提升资源利用率从而增加寻迹的准确性。

1 人形机器人轨迹跟踪方法

1.1 轨迹跟踪方法概述

双足仿人型机器人轨迹跟踪算法是机器人在具有引导线、环境较为稳定情况下进行行走的典型方法，轨迹跟踪算法由两个阶段组成：在第一阶段，根据所捕获的图像计算其所需跟踪的轨迹处于空间中的坐标位置；在第二阶段中，根据机器人当前所处状态以及在此之前一段工程周期所给出的行动指令综合判断给出最优的行动规划，以此实现机器人的轨迹跟踪算法。

如钱苏珂[4]等提出的一种基于双目视觉的移动机器人环境识别系统，采用逐帧分析图像，其根据当前环境规划机器人未来所需行驶的路线的方法。当算法计算出新的路线时，会将之前所得出的路线整个废弃，从而做到实时调整机器人的运动姿态。当机器人在实际的场景中运行时，算法运行所产生的结果大部分都为冗余数据，被程序直接废弃，图片的利用率极为低下并且轨迹跟踪的准确率不高，造成了资源的浪费以及无效计算。

本文所提出的轨迹跟踪方法中的最佳行动规划的判断是结合当前状态与一段周期内的算法结果所给出的，所以这种轨迹跟踪方法能够产生较为优秀的行动路径规划。就实验结果而言，所提出的轨迹跟踪方法能够较好地减小机器人在跟踪直线时左右摇摆而产生的资源浪费。同时，由于机器人轨迹跟踪需要对大量数据进行处理，需要高效的计算能力，所提出的轨迹跟踪方法能够较好地解决如何重复利用有限的图片资源及计算资源的问题。

1.2 系统功能描述

由于双足仿人型机器人主要是在人类所设计的较为稳定的环境中工作，整个轨迹跟踪方法基于一个由拥有19个机械自由度的双足仿人型机器人和openmv4单目摄像头视觉系统。视觉系统安装在机器人的胸腔部位，能够在机器人的运动过程中实时采集图像，机器人的双足共有10个自由度，能够完成基本的行走、转弯及爬坡等功能[5]。

轨迹跟踪算法程序位于以STM32H750VBT6芯片为核心并烧录openmv系统框架的视觉控制单元中，其控制单元作为本文所用的双足仿人型机器人的控制中枢，通过UART串口连接到机器人的舵机控制板实现对于机器人的行走姿态控制。

本文所提出的双足仿人型机器人轨迹跟踪系统结构框图如图1。

系统以围绕STM32H750VBT6芯片所设置的控制板为主控，在其内部搭建openmv框架对OV7725摄像头采集到的图像进行处理，通过LCD屏幕将处理后的图像及运算结果可视化显示出来[6]，多个部位相结合从而构成了本文所提出的轨迹跟踪系统硬件部分。通过串口通信的方式将主控与舵机控制模块连接到一起，从而操作由舵机控制模块及19个舵机组成的人形机器人运动系统。然后，使用硬件按键操作机器人的运行，并通过多个LED工作状态指示灯显示当前工作状态，便于操作人员调试与维护。

1.3 设计目标

本文所提出的轨迹跟踪方法主要用于实验室条件，具有引导线的情况下的人形机器人寻迹设计，能够在实验地图上按着既定的轨迹进行轨迹跟踪并最终到达指定的终点，完成实验任务。

2 轨迹跟踪方法设计

2.1 图像获取与处理

在实验室条件下，采用OV7725型CMOS类型数字图像传感器进行图像采集。该传感器用来输出2万像素的图像（160×120分辨率），输出图像采用RGB565数据格式，在获取图像的同时对图像进行畸变矫正与图像补偿。所采取的图像传感器体积小巧、工作电压低，以便于安装在人形机器人上，同时支持利用VGA格式输出图象数据。

图1为OV7725型图像传感器所采集的图像，由于图像平面上图像点在几何位置的误差，从而造成了图像与实际景物无法严格满足中心投影关系，使得图像产生畸变[7]。在收集到存在畸变的图象后，图像收集模块采取软件算法的方式对镜头畸变进行改正，图2为改正后的图像。

图3 经矫正后的图像Fig.3 Corrected image

图像采集系统采用LAB色彩空间模型，将采集到的图像中处于阀值范围内的像素全部变成黑色，其余像素变为白色。图4为经过二值化算法后所得的输出结果。

图4 二值化处理后的图像Fig.4 Image after binarization processing

2.2 画面划分

采取ROI（Region Of Interest）感兴趣区对图像区域进行划分，如图5所示。将图像横向划分为5个矩形区域，纵向划分为3个矩形区域，横向区域从上至下分别为A区、B区、C区、D区以及E区，纵向则忽略中间区域仅最左侧的F区与最右侧的G区。

图5 图像区域划分Fig.5 Image region division

2.3 图像处理

如表1所示情况，将已划分区域的图像按照ROI分别检测色块，检测色块的核心思想为：分别在各区域内寻找色块，输出色块中心点的坐标并在其上绘制十字形标记，若无色块则输出整个图像中心点的坐标以在运算过程中归为可忽略误差。

2.4 行动判断

按公式对ABCDE区域和FG区域进行处理。

以整个图片中心为二元坐标轴原点，由于图片为160×120大小，则A区域左上角坐标为（-80，60），E区域右下角坐标为（80，-60）。在ABCDE区域内，y轴坐标可忽略，故而命其内色块的x轴坐标分别为ax、bx、cx、dx、ex。将以上5个坐标分别除以80用以进行归一化处理，再按1：2：3：2：2的比例进行加权处理，则最终可得到基本x轴坐标如式（1）所示：

图6 根据式（2）所分配权值得到的二维图像Fig.6 2D Image obtained from the assigned weights according to equation(2)

将F、G区域内的坐标分别表示为fx、fy、gx、gy，首先依照F、G区域内色块的有无及基本坐标x判断当前路线类型，再根据fy、gy的相对位置依照表1所示的基本轨迹类型进行识别，从而依照轨迹类型及轨迹中心点坐标位置得出机器人运动指令。

2.5 结果作废

由于人形机器人在运动过程中视觉算法在不间断地工作，让机器人在一次运动过程中强行停止转为去做其他动作无疑是不现实的，故而视觉算法所得出的大部分运动指令为冗余数据。本文选择将这些冗余数据通过加权计算的方式，对当前运动指令的正确性做出判断。若判断为错误则直接将此次运动指令作废处理，此方法可极大提升资源利用率从而增加寻迹的准确性。

将上一小节所得到的运动指令与一个工程周期（此周期一般为机器人一次完整行动时间的1.5倍）内的历史运动指令进行比对，若运动指令相同则记为+1，若不同则记-1。将各个对比值依被对比的指令在工程周期内的位置按式（2）分配以相应的权值。

设当前工程周期内累计有α个运动指令，工程周期长度为t，各个历史运动指令与当前指令所对比的值分别为α1、α2、α3……，故而获取到如式（3）所示的作废指令。

若y＜0，则当前指令作废，不进入历史指令池；若y≥0，则将当前指令记入历史指令池中。当机器人一次行动结束后统计一个工程周期内出现次数最多的运动指令作为下一次行动指令。

3 结论

本文提出并设计了一种双足仿人型机器人的轨迹跟踪方法，此方法是基于STM32H750VBT6芯片与openmv软件框架的使用。在本文提出的轨迹跟踪算法中，先对图像传感器采集到的图像进行划分处理，再对于每一块的图像分别进行运算，最后进行数据汇总并对数据有效性进行判断。此方法所使用的运算资源相比较对图像整体进行运算而言，此方法不仅可以节约运算资源，而且能够有效地提高算法调试人员的调试灵活度。在调试的过程中能够得以根据运算过程中反馈的各个分区色块坐标来对不同类型的轨迹进行分类。同时，此方法提出了将机器人处于运动过程中所得出的数据结果作为历史数据池，将冗余数据进行多次运用，能够较为有效地解决机器人运动过程中的运算资源利用率问题。图像处理结果由分区计算算法给出，运动指令则由当前姿态数据结合历史数据池内数据加权运算后得出。将不同方法集成在轨迹跟踪算法中，以实现人形机器人在低速环境下的自主导航。