1. 引言
机器人技术在国外的自动化行业迅猛的增涨 [1] ,在研发中人们开始利用各类传感器让机器人具备环境感知能力,近年来机器人的研究趋势也逐渐成为热点并且更加紧密地与虚拟技术相结合,例如麦肯锡的具备视觉传感器系统视觉机器人控制系统与博尼的安装有压力传感器的“灵巧手”等。在国外 [2] ,日本致力于建立大规模VR知识库的研究,并应用于机器人之上。东京大学基于建立虚拟现实技术对物体三维结构的判定和三维图形的表示、静态图像的提取来辅助研究机器人。在国内,清华大学对虚拟现实和临场感方面进行了研究,在球面屏幕显示和图像随动、克服立体图闪烁的措施和深度感等方面都具有不少独特的方法。
本文研究的是基于虚实交互的机器人运动场景控制系统,通过虚与实的结合让机器人能应对各种复杂环境,在虚拟方面,利用Unity3D建立机器人模型和运动场景,且虚拟机器人能躲过障碍物走到终点,在实际方面,使用OPNEMV摄像头采集现实场景中障碍物的位置和大小信息,将该信息发送给上位机,并在虚拟场景中实现对应比例的实时重现。除此之外,通过蓝牙通信模块将虚拟机器人运动控制指令发送给下位机(机器人)单片机,让机器人完成相应的控制,最终完成虚实交互运动控制的任务。设计分为五个模块:现实障碍物图像数据采集模块(Python编写)、虚拟场景搭建模块(Unity 3D软件)、Socket服务器(C#编写)、蓝牙通信模块(C++编写)、机器人控制模块(C++编写)。
2. 机器人正运动学分析
本文采用双足六自由度机器人 [3] ,如下图1所示,机器人共有六个舵机作为关节。其中有2个270度的舵机作为胯部活动大腿部分,另外4个180度的舵机分别用于膝盖和脚踝关节。六个舵机相互协调工作模拟人体动作仿生行走。
![](//html.hanspub.org/file/8-2610168x9_hanspub.png)
Figure 1. The physical appearance of a bipedal six-degree-of-freedom robot
图1. 双足六自由度机器人实物外观图
对于机器人的正动力学问题是 “已知力求运动”,当给出了机器人各关节的驱动力、关节初始角度及初始角速度,求解各关节的角度、角速度和角加速度。本文通过采用拉格朗日方法求解正向动力学方程。拉格朗日方法具有完整约束的质点系动力学问题的普遍方法。它根据全部杆件的动能和势能求出拉格朗日函数,再带入拉格朗日方程中,从而导出机械系统的运动方程。此方法的主要特征是可以不考虑杆件相互的内部约束力 [4] 。具体步骤为:
设q表示步行系统的广义坐标,
表示步行系统的广义速度,拉格朗日函数
定义为系统的总动能K与总势能V之差,即
根据动力学普遍方程,系统的动力学方程可以用拉格朗日函数表示成
式中,
为作用在第i个关节上的非保守广义力,它可以分解成:
其中,
为作用在第i个关节上的驱动力矩;
为除关节驱动力矩之外的其他非保守力;
为外力
作用点的笛卡尔坐标对广义坐标的雅可比矩阵。
由于动力学方程的解析形式通常十分复杂,特别是是正向动力学拉格朗日方程中各矩阵的解析形式。为此采用MATLAB中的符号计算工具Symbolic来自动计算和生成拉格朗日方程中各矩阵项的解析表达式。
由本实验的双足六自由度的机器人,取
为步行系统的广义坐标,对于机器人身体上的每一个杆件的质心位置在绝对坐标系下的笛卡尔坐标为
,并将其表示成广义的q的函数:
根据函数求导规则,杆件i的质心线速度和绝对线速度可表示成
则杆件i的动能为
杆件i的势能为
系统总动能为
系统总势能为
拉格朗日函数为
将拉格朗日函数代入动力学普遍方程就可以推导出拉格朗日方程。通过MATLAB仿真可以得到本文机器人的正运动动力学模型的拉格朗日方程解析方程矩阵:
,
,
,
,
,
。
3. 虚拟场景构建与智能避障
采用基于Unity3D的物理引擎构建刚体组件 [5] ,碰撞系统等进而构建出虚拟场景,通过使用其中的navigation地图导航网格将机器人的行走表面勾选出来,即勾勒出静态避障的空间区域。在虚实交互区域,摄像头捕捉采集,当识别到物块时,会将识别到的物块像素发送到上位机,进而使上位机在虚拟场景中生成与场景成比例的虚拟物块,此时通过动态避障的脚本处理后机器人将会绕开物块进行避障。
3.1. 虚拟场景的构建
3.1.1. 刚体构建
为对象添加Rigidbody刚体组件 [6] ,可实现该对象在场景当中的物理交互。此时对象便可以接收外力与扭矩力,并且任何对象只有在添加Rigidbody组件之后才能受到重力的影响,让物体具有在现实世界的一般物体特性。Rigidbody钢体组件如下图2所示:
![](//html.hanspub.org/file/8-2610168x36_hanspub.png)
Figure 2. Rigidbody steel body assembly
图2. Rigidbody钢体组件
3.1.2. 碰撞体构建
碰撞体是物理组件中的一类,每个物理组件都有独立的碰撞体组件,它要与刚体一起添加到对象上才能触发碰撞。如果两个刚体相互撞在一起,只有两个对象是碰撞体时物理引擎才计算碰撞,在物理模拟中,没有碰撞体的刚体会彼此互相穿过。本文采用的是简单方便及耗内存少的Box Collider碰撞器,选用下图3所示的Is Trigger触发器选项,即可产生碰撞效果。Box Collider碰撞器如下图3所示:
![](//html.hanspub.org/file/8-2610168x37_hanspub.png)
Figure 3. Box Collider Collider
图3. Box Collider碰撞器
3.1.3. 虚拟场景构建
在Unity 3D软件中通过GameObject窗口生成不同形状的3D物体,再对每个物体进行材质渲染,经过搭建便能得下虚拟运动场景如下图4。
3.2. 虚拟场景动画添加
使用Nav Mesh Agent (避障寻址插件),让机器人在虚拟场景中进行位置移动,并设置好动画之间的转换时间、播放长度和动画播放速度,让机器人在运动控制场景按照一定规则动起来。虚拟场景中机器人模型如下图5所示:
![](//html.hanspub.org/file/8-2610168x39_hanspub.png)
Figure 5. Robot model built by virtual scene
图5. 虚拟场景搭建的机器人模型
3.3. 虚拟场景避障
在虚拟场景中,机器人需要从设定的起点走到终点,并在这个过程中完成智能避障。我们使用Unity 3D中的Navigation地图导航网格将整个场景中行走表面勾选出来,再将障碍物选中,设置机器人参数(长宽高、可攀爬最大斜坡角度、可跨越最高台阶及跳跃的最大距离),导航网格图如下图6所示。
在静态避障的基础下,机器人还需完成动态避障。现实场景中通过OpenMV摄像头识别障碍物的图像信息,并将物体的信息发送给虚拟场景,并在虚拟场景中实时的生成相应物块,机器人完成动态避障动作。动态避障流程如下图7。
3.4. 虚实地图比例设计
本设计中的虚拟场景和现实场景为两个不同的场景,要达到虚实同步动作的要求,需要一个同步的标准,即现实场景的地图与虚拟场景中的地图所对应的比例,若是没有相应的比例则无法确定现实场景中的机器人所在的位置在虚拟场景中对应的关系。由于现实机器人是由舵机控制前进,其速度很难定量控制,故我们不采用直接设定地图比例的方法,而是通过在现实中与虚实场景对应进行多次匀速测试得到。测试方法是多次让现实场景与虚拟场景同步运动,通过多次测量并剔除偏差过大的某次测量值之后
![](//html.hanspub.org/file/8-2610168x41_hanspub.png)
Figure 7. Dynamic obstacle avoidance flow chart
图7. 动态避障流程图
再求取平均值,最后求取两个地图的比值G。得到数据如下表1:
![](Images/Table_Tmp.jpg)
Table 1. Virtual and real map scale measurement data table
表1. 虚实地图比例测量数据表
得出虚拟地图与现实地图之间的比例值G为:
4. 虚实场景的交互
本文使用的是OpenMV摄像头,通过该摄像头来模拟真实情况下,机器人遇到突发状况后的运动特性,例如在机器人行走过程中,在它前方放置一块障碍物,摄像头识别到物体后能将物体的大小坐标信息传到Unity3D中,Unity3D根据物体信息重现障碍物。
该模块带有ARM Cortex M7处理器,400 MHz,1 MB RAM以及2 MB flash。摄像头焦距为2.8 mm。OpenMV摄像头如下图8所示:
在摄像头中,最重要的元件是感光元件,感光元件将光信息转换为电信号,再送到计算机中处理,OpenMV使用的是OV7725感光元件,它能处理640 × 480 8位灰度图或者640 × 480 16位RGB565彩色图像。对于一张图片,有许多参数如分辨率,像素,色彩模式等 [7] ,其中色彩模式主要分为RGB、CMYK、HSB、LAB,在本项目中对于色块的分辨运用的是LAB模式。通过调节LAB值来让摄像头找到我们所放置的障碍物。
4.1. 图像获取
我们使用OpenMV摄像头来识别红色障碍物体 [8] ,打开摄像头采集数据,截取其中一张图片进行阈值调节,我们需要的是红色,其他颜色需要排除在外,将红色区域调节成白色,其他区域调节成黑色,阈值设定如下图9所示:
调节完成后记录LAB值,把LAB值赋值到red_threshold数组中备用,通过
img=sensor.snapshot()
blobs=img.find_blobs{[red_threshold]}
这两行代码,摄像头就能准确获取红色的物块,随后通过
img.draw_rectangle(max_blob.rect())
将红色物块框出来,方便计算它的长度和宽度。
摄像头获取到红色物块的信息储存在blob中,我们需要的是边框的w,h以及中心坐标,分别对应的是
blob.wblob.hblob.cxblob.cy
最后将获取到的数据打包通过串口发送,再以比例G在Unity3D中重现障碍物。
4.2. 物体与摄像头距离测定
使用OpenMV摄像头进行物体距离测定主要有两种方法,其一是利用Apriltag对物体定位,Apriltag是一个视觉基准系统,可用于各种任务包括AR,机器人和相机校准,Apriltag可以直接在OpenMV IDE中生成,通过Apriltag检测程序可以计算到物体相对于摄像头的3D位置,方向和ID,Apriltag如图10所示。
Apriltag主要分为以下几种:Tag16H5,Tag25H7,Tag25H9,Tag36H10Tag36h11以及ARTOOL-KIT,每一种称为一个家族,每个家族有各自的成员数量,如Tag16H5中有0~29号共30个标记,Tag25H7中有0~241号共242个标记,不同家族之间除成员数量不同以外,每个家族的有效区域也有差别,如Tag16H5的有效区域是4 × 4个方块,Tag36H11的有效区域为6 × 6个方块,有效区域大,校验的信息多,所以Tag36H11的出错率要比Tag16H5小。
Apriltag具有3D定位功能,除了能测量到物体相对于摄像头的
坐标外,还能知道物体在每个轴上的旋转量。如图11所示,
为空间的三个位置量,
为三个旋转量。
但用这种方法需要在每个物体上贴Apriltag,使用起来比较麻烦,所以我们采用第二种方法来测量距离,其原理如下图12。
![](//html.hanspub.org/file/8-2610168x50_hanspub.png)
Figure 12. Camera measurement object distance schematic
图12. 摄像头测量物体距离原理图
为摄像头画面固定像素,
为物体在摄像头画面中所占像素,R为物体半径(以球为例),
为摄像头视角的一半。左边摄像头的几何关系为:
所以有:
由右边实物环境中几何关系可得:
综合上述式子可得:
所以
,即,
。
在知道这个常数之后,就可以测出物体与摄像头的距离,首先用一个参照物,放在离摄像头A cm处,打印出物体在摄像头中直径的像素,再将两个数据相乘,即可得到K值。
4.3. 动态时间归整算法
为了衡量虚拟主角运动轨迹和实体机器人轨迹的相似度,本文使用了动态时间归整算法,即(Dynamic Time Warping, DTW),该算法最初运用在语音识别领域,解决说话语速不一致的问题。DTW算法主要是基于动态规划的思想,通过动态规整来对齐时间轴,使每个数据点有最好的对齐方式,进而消除不齐造成的误差,获取时间序列间最好的匹配方式 [9] 。两条时间序列各个数据点对齐如下图13所示,在经过时间轴拉伸之后,得到如下图14所示,最终发现两条相似的时间序列。
![](//html.hanspub.org/file/8-2610168x60_hanspub.png)
Figure 13. Data point alignment of time series
图13. 时间序列的数据点对齐
![](//html.hanspub.org/file/8-2610168x61_hanspub.png)
Figure 14. Time series after dynamic rounding
图14. 经过动态归整后的时间序列
计算参考模板与测试模板之间的直接距离可以得出它们之间的相似度。两个模板之间的距离越短,模板之间的相似度就越高。在使用DTW算法进行相似判断前,需要先使这两组时间序列在时间轴上对齐。
首先,构造一个
的矩阵D,矩阵元素
表示
和
两点之间的距离(也是参考模板中的每条时间序列和测试模板每条时间序列之间相似度),其中
,根据DTW的边界条件,最短距离也就是
到
之间的最短路径如图15。再结合DTW算法的单调性和连续性要求,设定局部路径约束条件为:当从一个方格
、
或者
中到下一个方格
,横向或竖向的距离为
,斜着对角线过来的则是
。下面的数学(1)可以解释这种约束条件,其中
表示2个模板都从
)点到
逐次匹配时两个模板之间的距离,并且要在上一次匹配的基础上加
或者
,然后取最小值。
![](//html.hanspub.org/file/8-2610168x80_hanspub.png)
Figure 15. Finding the shortest path between two time series
图15. 找到两个时间序列间的最短路径
(1)
5. 通信桥梁——Socket服务器
5.1. Socket服务器与上位机通信
Socket服务器 [10] 作为联系上位机和下位机、虚拟场景和现实的关键通信媒介。Unity3D虚拟场景客户端与Socket服务器通过在Visual Studio上配置需要的引用文件和对应命名的空间。其具体步骤为首先应在VS中配置Socket服务器的I/O地址和端SS口号,用于客户端访问时访问的唯一标识。当客户端初始化Socket并和Socket服务器连接成功后,服务器就会监听配置地址端口上的服务信息,当配置地址有数据传输过来,便会立刻将数据保存下来。此时Socket服务器既可以向客户端发送数据,客户端接收到数据后又可以将数据缓存到对应的寄存器中以便下次使用。在本实验中,Unity3D Socket服务器中使用运动刚体函数Velocity,获取物体运动中的速度向量等信息从而读取虚拟界面机器人的运动状态。
5.2. Socket与下位机通信
串口是连接Socket服务器和蓝牙模块的通信媒介 [11] ,通过Socket服务器定义的端口号接入一个USB转TTL的连接器,并将两个转接器的TX/RX反接,即RX接TX,TX接RX。其中一个串口通过串口调试助手用于对服务器的数据传输实施实时监听,而另一个串口为Socket服务器的数据发送接收端,将客户端发出的运动角度信息发送出来,发送到蓝牙模块。
6. 实验
6.1. 摄像头识别精度
6.1.1. 不同标定下距离测量精度
根据实验,红色小球在20 cm时,在摄像头中的像素为70,那么K = 1400,下表2是小球在15~25 cm时,摄像头测出的摄像头到小球的距离数据。
![](Images/Table_Tmp.jpg)
Table 2. Camera measurement data of the ball under the 20 cm standard
表2. 小球在20 cm标准下的摄像头测量数据
小球在30 cm时,在摄像头中的像素为49,那么K = 1470,下表3是小球在25~35 cm时,摄像头测出的摄像头到小球的距离数据。
![](Images/Table_Tmp.jpg)
Table 3. Camera measurement data of the ball under the 30 cm standard
表3. 小球在30 cm标准下的摄像头测量数据
小球在不同位置时,对应的K值也有差异,下表4是小球在10个不同位置处对应的K值。
![](Images/Table_Tmp.jpg)
Table 4. K value of the ball at different distances
表4. 小球在不同距离下的K值
通过表4看出,小球在15、20 cm处的K值差别较大,所以通过在20 cm处的标定值来测量15 cm处的物体误差较大,同理20 cm与25 cm处的K值差别较大,用20 cm处的K值来测量25 cm处的物体误差较大。小球在30 cm,35 cm处的K值相同,那么理论上用30 cm处的K值来测量35 cm处的物体,测量结果应该很准确,通过表3看出确实如此。
小球距离摄像头太近,会造成像在摄像头中比较模糊,如图16为小球在距离摄像头15 cm处的成像,可以看到成像比较模糊,那么对于像素的计算就不准确,计算出的K值就有较大差别。
而在30 cm处定标,小球在摄像头中较为清晰(图17),对于物体像素的计算较为准确,测出的距离接近实际距离。
6.1.2. 不同物体大小精度
在摄像头测距的基础上,同样可以测得物体的大小,首先用一个已知直径A cm的小球,放在距离摄像头B cm处进行定标,该小球在图像中的像素数为α,那么可以得到另一个比例关系k2 = A/α,通过这一比例关系可以测得其他物体在同样距离处的大小。
根据实验,一个直径2 cm的小球分别在距离摄像头15 cm,20 cm,30 cm,40 cm处,在图像中的像素数分别为91,71,49,36.5,那么k2的值分别为0.0219,0.02817,0.04082,0.05479,以下是通过摄像头测得的不同边长的正方形。
由表5~8可以看出,在距离摄像头15 cm处测量物体长度的精度相对较差,在30 cm处测量物体的大小精度相对较高。
![](Images/Table_Tmp.jpg)
Table 5. Measurement data of square size at 15 cm
表5. 15 cm处正方形大小的测量数据
![](Images/Table_Tmp.jpg)
Table 6. Measurement data of square size at 20 cm
表6. 20 cm处正方形大小的测量数据
![](Images/Table_Tmp.jpg)
Table 7. Measurement data of square size at 30 cm
表7. 30 cm处正方形大小的测量数据
![](Images/Table_Tmp.jpg)
Table 8. Measurement data of square size at 40 cm
表8. 40 cm处正方形大小的测量数据
6.1.3. DTW算法轨迹相似度比较
用DTW算法将虚与实场景中对应的数据点进行时间序列相似度比较。虚拟场景中的角度信息是机器人正前方与右肩所在坐标系的角度,该角度信息在脚本中计算完成后通过InvokeRepeating (“yan”,7f,0.8f);将角度信息每隔0.8秒打印在Unity3D的控制台上。现实场景中机器人的角度信息是通过陀螺仪测量得到后发送给上位机,在VS中把蓝牙接收到的角度信息打印在VS的控制台中。将多次实验的数据进行数据处理,通过Matlab仿真中,得到的仿真图如图18和图19:
![](//html.hanspub.org/file/8-2610168x84_hanspub.png)
Figure 18. DTW cumulative distance matrix and optimal path (Similarity)
图18. DTW累积距离矩阵和最优路径(相似度)
![](//html.hanspub.org/file/8-2610168x85_hanspub.png)
Figure 19. Rounding time series signal diagram
图19. 归整时间序列信号图
7. 结论
1) 通过建立Unity 3D软件搭建起了机器人虚拟运动场景,并能够在虚拟场景中设置各类障碍物,可以采用Navgation导航网络模块计算出在避开障碍物的条件下,从起点到终点之间整个地图的最优路径,并利用NavMesh插件完成机器人的寻址避障运动控制。
2) 现实场景采用双足六自由度机器人,机器人将跟随虚拟场景中的机器人同步运动,并用OpenMV3摄像头识别现实场景中的障碍物图像信息,通过图像视觉处理后将障碍物的信息反馈于Socket服务器,虚拟场景从服务器中提取障碍物信息,并在虚拟场景中对应比例的重现,同时虚拟机器人进行躲避障碍物的运动控制,以此来达到虚实交互运动控制的目的。
3) 通过摄像头精度识别实验,从物体距摄像头的距离不同的测量数据,得出摄像头在距离为15 cm处精度相对较低,在距离为30 cm处精度相对较高。
4) 通过该摄像头来模拟真实情况下,将虚拟场景中机器人行走路径与现实场景中机器人行走路径按时间序列进行记录,通过DTW算法对两个路径进行相似图分析,通过MATLAB仿真得出参考模板与测试模板之间相似度较高,其相似度为0.918完成了虚实机器人与现实机器人同步运动控制,并说明了机器人遇到突发状况后的运动特性。