危险环境消防侦察机器人视轴规划算法研究

刘满禄，李铭浩，敖天翔，赵 皓

1.中国科学技术大学 信息科学技术学院，合肥 230026

2.西南科技大学 信息工程学院，四川 绵阳 621000

1 引言

社会经济的不断发展导致建筑物愈加密集，发生火灾的可能性和危害越来越高。不仅给人民群众的生命财产安全带来重大损失，更威胁消防人员的生命安全。例如天津港8.12特大爆炸事故以及近期发生的伦敦公寓楼大火等。在工业环境中，各种工业原料和产品的燃烧会使灾害现场充满有毒有害物质，同时伴随的爆炸会严重破坏现场结构，消防人员无法入内[1-2]。随着机器人技术的发展，消防侦察机器人开始替代消防人员进入高危灾害现场，进行现场侦察，并完成图像、数据等信息采集，有效地帮助消防人员进行抢险救灾。

国际上，美国、日本、英国、德国在消防机器人研究领域处于领先地位。如美国的Pandora、HAZBOTⅢ和Thermite消防机器人；日本的FLYGO机器人。我国已研制出JMX-LT50型消防机器人及上海强狮消防侦查机器人等[1-3]。目前，消防侦察机器人已经成为各国消防技术领域的研究热点，已有多种不同功能的消防机器人用于救灾现场。图1为自主研发的一台针对危险环境的消防侦察机器人，机器人为履带式车身，高清相机装载在一个3自由度机械臂末端，车身搭载线激光，温度传感器，气体传感器等各种传感器，用于在危险环境中采集信息。利用后端控制台可以控制车体与机械臂关节移动。为了实现机器人更加智能地采集图像信息，利用计算机辅助操作者采集图像信息，能够使操作端的操作更加便捷高效。

图1 危险环境消防侦查机器人实物图

灾害现场的非结构性容易对相机画面产生遮挡，针对此问题，Su Yong Kim等[3]提出一种通过集成系统来提升伪超声波移动机器人的定位精度，在超声波发射器和接收器之间存在障碍时，通过卡尔曼滤波器提出一种故障检测和计算方法并通过实验验证了系统的定位性能和故障定位算法的有效性；Gustavo Figueiredo等[4]分析了二维和三维的视轴遮挡问题，并认为贪婪算法不能很好地解决逼近问题，并提出一种采用局部搜索算法来达到1+1/K阶逼近的猜想；Lavi M.Zamstein等[5]对处于未知环境中的移动机器人使用Q学习算法来解决此问题。朱柏林[6]按照云台技术指标，对路面条件下视轴偏转角进行了分析；王宣[7]采用基于局部敏感直方图的目标跟踪算法，进行抗遮挡跟踪研究；本文提出了一种机器人在固定位置下，利用外部环境位置信息，自主寻找最优视角拍摄图像的方法。该方法利用蒙特卡洛采样思想，利用凸包检测的方法进行碰撞检测，在机械臂构型空间进行均匀采样。为保证视线区域没有遮挡，在观察区域与相机之间建立虚拟连杆，通过碰撞检测算法保证视角中心无碰撞[8]。在采样获得点云中寻找无碰撞的位姿，搜索最佳方向作为最佳拍摄位姿。

2 机械臂正运动学

为了求解机械臂运动后机械臂的位置情况，需要建立机械臂正运动学模型。本文使用指数积建模法建立机械臂正运动学方程，与传统D-H参数建模相比，指数积模型[9-10]更加直观、间接。指数积建模需要机械臂在初始位型时的关节旋量。

在机器人底端中心位置建立坐标系，按照图2、3的初始位型参数和每个关节的旋转方向，建立关节旋量如下：

其中，w的第n列代表第n个旋转轴ξn的方向。

γ的第n列代表第n个旋转轴ξn的某一个点的位置（单位：mm）。

Pend(0)为末端位置的初始位型（单位：mm）。设θ=(θ1θ2θ3)，有：

根据式（4）和（5）可知：

设Pend(θ)为移动之后的末端点位姿。

图2 机器人侧视图

图3 机器人俯视图

这种建模方法可以根据机械臂上各点在初始位型上的位姿信息以一种统一的形式求解关节角度变化后机械臂上各个点位置。

3 碰撞检测

3.1 利用Minkowski和进行问题转换

假设平面移动物体R是一个凸多边形，而且障碍物是凸的。如果移动物体的参考点在(x,y)，就将R的位置记作R(x,y)。对于移动物体R而言，每一个障碍物P对应的空间障碍区域都被定义为空间的一个点集，如果R对应的位置对于该点集中的某个点，R就会和P相交。因此如果将P的空间障碍区域记作CP，有：

为画出CP的形状，使R沿着P的边界滑行一圈，R的参考点经过的轨迹曲线，就为CP的边界。

由此，得到Minkowski和[11-12]。对于任意两个集合S1，S2，其Minkowski和为：

为了能够利用Minkowski和来叙述空间障碍区域还需要借助原点对称镜像概念，对于二维点p=(px,py)，令-p=(-px,-py)。同理对于任何集合S，定义-S={-p|p∈S}。可得：若R为移动物体，P为任意障碍物体，则P对应的障碍区域为P⊕(-R(x,y))，又P与R为凸包，则P的障碍区域为P对应的凸包边界点减去R在参考坐标点下表示的凸包边界点形成的点集得到的新凸包。

如此可把空间中两凸物体的碰撞问题转化参考点的区域判定问题，如果移动物体的参考点在世界坐标系下落在障碍物的碰撞区域即可认为物体发生了碰撞。

针对本文研究的消防侦察机器人，障碍物为外部物体和车身，中间连杆和末端相机及其连杆为移动物体。在移动物体的中心建立参考坐标系，得到移动物体边缘点在此坐标系下位置坐标。通过外部传感器获得外部障碍物的边缘点位置，用上述方法能建立出障碍物对于移动物体的障碍区域，然后根据参考点是否落在障碍区域内判断物体是否碰撞。

3.2 三维凸包构造

借助随机增量式算法构造P的凸包CH(P)，首先选出不共面的4个点，它们形成的凸包必是一个四面体[13-14]。用双向链接边表[13]来存储凸包的4个边界点、12条半边和4个面。其次将其余的各点排列成序并按照次序逐点处理，动态地维护凸包。不断循环将新的点Pr插入到凸包中，即将CH(Pr-1)转化为CH(Pr)，期间可能出现两种情况。

（1）若Pr落在CH(Pr-1)的内部或者边界上。此新插入点对凸包无影响，即CH(Pr)=CH(Pr-1)。

（2）若Pr落在CH(Pr-1)的外部。在Pr位置上，朝CH(Pr-1)可以看到CH(Pr-1)的一些平面，这些可见的平面组成一个联通区域，称为Pr在CH(Pr-1)上的可见区域，也称有向平面对Pr可见（由半边的包围方向使用右手定则确定方向），设Ef为某个面指向的半边，Pf为半边Ef的起始点，Pp为半边Ef的前向边的起始点，Pn为半边Ef的后向边的起始点。通过下式，若v＞0，则面对Pr可见。

如图4所示，可见区域由CH(Pr-1)的一些边和一条封闭折线围成，此折线为Pr在CH(Pr-1)的地平线。新插入一个点后，地平线恰好是可见和不可见部分之间的边界，连接Pr与地平线生成若干新的平面并替换原有可见平面，同时保留不可见平面。双向链接边表处理：将CH(Pr-1)中所有对Pr可见的平面的信息从双向链边表中删除，将Pr地平线连接，并将新平面的信息存储其中。值得注意的是，当Pr与CH(Pr-1)的某平面 f共面时，必须构成统一平面。

图4 可视区域与地平线

通过遍历CH(Pr-1)的各个面查找出对Pr可见的面，通过维护一些附加信息，建立一个冲突列表G来减少复杂度。具体来说，对于当前的凸包CH(Pr)的每个平面 f，都维护一个 f对其可见的点组成的集合同样，对于每个点Pt(t＞r)，也要维护一个集合Fconflict(f)，此集合由CH(Pr)中所有对Pt可见的平面组成。若有一点 p∈Pconflict(f)加入到凸包中，平面 f就必须被删除。同理，为将Pr插入CH(Pr-1)中，只需在G中找出对Pr可见的所有平面并删除，由Pr与地平线相连接而生成的平面代替，得到更新后的凸包。上述算法的伪代码如下：

输入：三维空间中n个点组成的集合P

输出：P的凸包CH(P)

取点 p1、p2、p3、p4，构成一个4面体 CH({p1,p2,p3,p4})，任取其余各点的一个随机排列：p5、p6、…、pn

初始化冲突图G，令G={(pt,f)|f为C H上的小平面，pt为对 f可见的点，t＞4}

For r←5 to n //即遍历CH外的各个点

Do（将 pr插入到CH 中）

If（CH中有小平面对pr可见）//即pr在CH外部

Then在CH中删除这些小平面，沿着 pr地平线各边组成有序表Lorder

For(∀l∈Lorder) //将各地平线与新插入点相连

Do（生成三角形小平面 f连接l与 pr）

If（和f相邻于l的平面f′与f共面）

//此时 f′与 f为同一平面

Then将 f与 f′合二为一

Else（确认 f引起了冲突）

在G中实例化一个对象 f

令原凸包上与l关联两平面为 f1和 f2

在图G中寻找所有与 f1和 f2有关联的点p，若 f对 p可见，添加( )p,f→G

在G中删除 pr节点，删除对 pr可见各平面以及相关联的所有弧线。

Return CH

3.3 碰撞检测

机器人在调节拍摄视角的时候需要驱动前置的机械臂各个关节，故可将机械臂的第二个连杆、摄像机及其连杆看作移动物体，在此两个移动物体的中心建立参考坐标系，分别提取两个移动物体的边缘特征点，在各自的坐标系下表示并存储。其次，将车身、底盘、履带、后箱与环境中的其他物体视为障碍物。在某一个固定机械臂位型下建立移动物体对于每一个障碍物的障碍区域，每一个障碍物对于一个移动物体都存在一个以三维凸包形式存在的障碍区域。对于每一个参考点和障碍区域，分别遍历障碍区域的所有面。如图5和6所示。

图5 机械臂第二连杆凸包与障碍物凸包

图6 对障碍物凸包点进行遍历

设Pr为参考点，由式（10）计算v，若存在v＞0，可判断移动物体和障碍物无碰撞，否则发生碰撞，遍历所有障碍物的障碍区域可得移动物体是否与周围环境有无碰撞发生，对每个移动物体做类似处理可判断整个机械臂是否与环境发生碰撞。此算法的伪代码为：

输入：每个关节对应的控制量Q1,Q2,Q3

输出：是否碰撞

已知参数：core1,core2,…,coren移动物体对应的参考坐标系

Set1,Set2,…,Setn移动物体对应边缘点集合

Obstacles1,Obstacles2,…,Obstaclesm为障碍物的边缘点集合

For i=1:n（n为移动物体数量）

利用正运动学计算更新移动物体的参考坐标系corei

更新相关物体边缘点集合信息

For j=1:m

根据障碍物 j的边缘点集与移动物体i的边缘点集建立障碍区域点集

建立障碍区域凸包

遍历凸包面查看参考坐标点是否落在障碍区域内

If（落在障碍区域内）

Return（发生碰撞）

Return（无碰撞）

4 蒙特卡洛方法寻找适合的视轴

机器人需要找到合适的位置摆放机械臂的末端摄像头以便观察固定的热点区域，特别是对于存在障碍物的情况下要保证视线和机械臂同时避开障碍物，同时保证热点区域处于摄像头的中心位置。本文采用蒙特卡洛随机采样的方法[15-16]，在机械臂的关节空间随机采样足够多的点，利用碰撞检测算法检测机械臂是否与外界发生碰撞。保存无碰撞的采样点，根据正运动学方程，建立末端位置矢量图。

依据算法得到的位置方向矢量图，计算摄像机朝向向量与末端到目标点朝向向量的相关度度。使用末端摄像机的朝向和相机与热点区域的方向向量的点乘作为计算相关度λ。

其中，end为末端摄像头的朝向，Pend为末端点位置，Phot为热点区域的位置。得到一个相关度大于阈值的备选集合。按照备选集，在热点区域与摄像机末端建立虚拟连杆[17-18]，利用碰撞检测算法检测碰撞，将视角的遮挡问题转化为三维物体的碰撞检测问题，保证视线不被遮挡。

选择满足条件的备选采样点，计算角度变化量，选择角度变化总和最小的采样点为最佳的位置。

5 仿真验证

在SolidWorks（版本号：2014）中建立机器人的CAD模型，导入至V-REP（版本号：3.3.2），将各个模块从新组装起来，同时添加转动关节，建立起机器人本体，添加两个方块作为障碍物。搭建实验仿真环境仿真环境。如图7所示。

图7 仿真环境图

利用均匀采样和碰撞检测算法可以在角空间形成栅格地图，在笛卡尔空间可以生成对应的末端点位置和方向矢量，可以用图8矢量图表示（构型空间随机采样10 000个点，第一个关节范围为−180°～+180°，第二个关节范围为−120°～+120°，第三个关节为−90°～+90°）。

图8 末端位置矢量图

依据算法得到的位置方向矢量图，计算摄像机朝向向量与末端到目标点朝向向量的相关度，得到一个相关度大于阈值的备选集合。设置朝向评价指标范围为0.85～1，在目标点与机械臂末端点间建立虚拟连杆，遍历所有备选采样点可以建立一个无碰撞，无遮挡，方向指标满足的采样集合，然后再在集合中计算根据各个关节角变化值之和选择最小的采样点以此确定最佳的相机摆放方法。

在仿真中以一个小球作为机器人重点观察的热点区域，左下角浮窗为相机拍摄到的画面，不断改变小球的位置（从左、中至右，如图9），利用上述算法可以实现机械臂对热点区域的定位拍摄。

如图10所示，对于同一个热点位置多次运行获得的拍摄画面差异度也极小。

仿真实验平均耗时为34 s，算法主要耗时在第一步建立位置矢量图，需要约30 s，如果外部环境不发生变化，机器人本体不发生移动，则不需要更新位置矢量图，如图11所示算法可以在5 s内运算出结果。适量减少采样点数可以减少算法时间。

图9 热点区域不同位置仿真图

图10 同一热点多次拍摄图

图11 多次实验耗时统计

实验证明，基于上述方法的机器人视角规划方法可以自主地寻找到优化的摄像头摆放位置，为操作人员提供决策支持。

6 结束语

本文使用旋量理论对3自由度机械臂建立正运动学模型，对机械臂末端相机的视轴进行避障规划并对重点区域进行视轴锁定。利用Minkowski和将空间中两凸物体的碰撞问题转换为转化参考点的区域判定问题，借助随机增量式算法构造三维凸包，并使用双向链接边表对数据进行存储，在相机和重点观察区域之间建立虚拟连杆，使用碰撞检测算法解决机械臂与周围环境碰撞以及相机视轴无遮挡问题，并给出相应伪代码。最后使用蒙特卡洛算法求取获得机械臂末端相机最佳位姿。实验证明，该方法能完成机械臂末端相机视轴无遮挡规划，仿真效果良好。