移动机械手主动防碰撞控制系统研究——基于多传感信息融合

2019-05-27方华丽

农机化研究 2019年11期

方华丽，吴晴

(河南工业职业技术学院，河南南阳 473000)

0 引言

随着电子、计算机、网络、机械,以及智能控制技术的发展，针对农业机械人的应用研究已经成熟。但是,对于采摘机器人移动机械手的防碰撞研究，由于其作业周边环境复杂多变，该领域的障碍物识别和多传感信息的融合处理一直是研究的重难点。为此,通过研究移动机械手的避障策略和多传感信息融合算法，设计了移动机械手主动防碰撞控制系统，对于采摘机器人自动采摘作业具有较大的现实意义。

1 主动防碰撞控制系统总体流程

采摘机器人机械手在进行采摘作业中，会遇到树枝、叶子以及其他非果树上的障碍物等，为了防止机械手经常发生碰撞出现损坏，一般需要其对障碍物进行主动防碰撞。因此，移动机械手控制系统最重要的部分是对移动机械手进行主动防碰撞控制。移动机械手主动防碰撞控制系统总体流程如图1所示。

图1 移动机械手主动防碰撞控制系统总体流程图Fig.1 The overall flow chart of active collision avoidance control system for mobile manipulator

移动机械手主动防碰撞控制系统主要内容有：

1)机器人移动信息。获取系统得到的传感器组的信息，主要包括移动机器人的前行速度、前方障碍物、机械手和障碍物相对距离以及方位角等。

2)障碍物的判断与识别。通过CCD摄像机和机械手激光雷达获得前方目标信息，计算出机械手和目标物的相对距离和速度，判断是否为障碍物。

3)安全状态评估。根据机器人移动信息和障碍物的判断与识别两块的参数信息，建立机械手和障碍物的安全距离模型和安全距离，并根据机械手和目标物的相对距离和速度，判断是否需要进行主动防碰撞的控制。

2 移动机械手避障策略

2.1 移动机械手和障碍物之间碰撞条件

移动机械手是采摘机器人的核心部件，是保证在园林作业中准确采摘目标果实的综合性刚体。移动机械手包含多个可运动环节，整体运动复杂，因此需要提前进行避障分析。在移动机械手避障策略中，一般采用限制机械手与其周围障碍物之间的相对速度来避开障碍物。因此，假设障碍物是凸形或可以用包围的凸包络来表示。移动机械手和障碍物相对距离示意如图2所示。

图2 移动机械手和障碍物距离示意图Fig.2 The distance sketch diagram of mobile manipulator and obstacle

(1)

其中，ξ为调整收敛速度的正系数，该值越大，移动机械手和障碍物之间允许的相对速度越大。ds(≤di)为安全距离的正值。等式(1)称为d的速度坝，其表示当它小于di时，d不能减小得太快。

如果满足初始条件d(0)≥ds，则可以推导出

(2)

式(2)表明：如果机械手和障碍物之间的距离决不会小于ds，这意味着二者之间的碰撞永远不会发生。

2.2 移动机械手避障策略

从前一节可知，移动机械手和障碍物可以通过限制两个物体上最近两点之间的相对速度来避免碰撞，并假设每个障碍物都具有凸形或者可以用包围的凸包络来表示。移动机械手和障碍物之间的最近点如图3所示。

图3 移动机械手和障碍物之间的最近点Fig.3 The closest point between a mobile manipulator and an obstacle

图3中，i为移动机械手第i个连杆，考虑到采摘机器人的构造，已知连接的第i个连杆和第(i+1)个连杆的位置。由于移动机械手是凸形的，因此第i个关节上的任何点都可以表示为

pi,k=λpi+(1-λ)pi+1, 0≤λ≤1

(3)

其中，pi和pi+1分别是第i个关节和第(i+1)个关节的位置矢量。

假设po,i表示障碍物的位置，那么第i个关节上的任意点pi，k与第i个障碍物之间的距离可以表示为

‖pi,k-po,i‖=(pi-pi+1)2λ2+ 2(pi-pi+1)(pi+1-po,i)λ+(pi+1-po,i)2

(4)

为了避免移动机械手与障碍物发生碰撞，可以利用前面的方法计算关节与障碍物之间的最近距离；同时，通过分别限制移动机械手与障碍物之间的相对速度，控制移动机械手避免与障碍物发生碰撞。

3 多传感信息融合算法的设计

多传感信息融合算法较多，但不同的算法运行机制不同。根据移动机械手主动防碰撞控制系统的传感器网络硬件平台和安全状态评估流程，本文选择EKF(Extended Kalman Filter，扩展卡尔曼滤波器)算法。EKF算法一般用于处理较为复杂的状态转移与观测模型的系统，其转移模型为

(5)

其中，xk为时间节点k的状态；uk-1为时间节点k-1的系统输入；f(xk-uk-1)为时间节点k-1的状态转移矩阵；zk为时间节点k的观测函数；h(xk)为时间节点k的观测模型矩阵；vk为时间节点k的高斯噪声。EKF算法的流程如下：

4)跟新计算协方差误差矩阵Pk|k。

针对移动机械手主动防碰撞控制系统的控制特点，通过编码器推算的Odom数据和惯性测量数据对机械手运行状态进行建模，其运行状态量为

xk=[Xk,Yk,ψk,vk,ωk]T

(6)

假设当时间节点为k时速度量输入为[vk,ωk]T，编码器检测一次的时间差为△t，则

(7)

其中，vl、vr和v分别为左轮、右轮和采摘机器人移动速度；T和ω为采摘机器人旋转角速度；D和η分别为采摘机器人前后的轮间距离和转弯系数；μ为编码器和轮子之间的转换系数。

采摘机器人移动机械手在时间节点k的姿态为

xk+1=Xk+vkcos(ψk)Δt,Yk+vksin(ψk)Δt,ψk+ωkΔt,vk,ωk

(8)

那么，采摘机器人移动机械手控制系统的状态方程为

xk+1=φk+1|kxk+ΓkWk

(9)

即

xk+1=f(xk)+wk

(10)

因此，根据多传感信息的数据融合，可以得到采摘机器人移动机械手的姿态角，然后实现对其状态的控制。

4 移动机械手主动防碰撞控制系统

移动机械手主动防碰撞控制系统由嵌入式系统、多传感器网络和电机驱动构成。多传感器模块负责采集机械手周边环境信息，结合多传感信息融合算法，通过电机驱动电路对移动机械手运动电机进行控制，从而实现对障碍物的主动防碰撞功能。

4.1 硬件框架设计

采摘机器人的移动机械手在作业中，需要实时检测周边环境信息，主动对树枝和树叶等障碍物进行避障。因此，需要移动机械手配合车速传感器、机械手激光雷达和CCD摄像机进行操作和控制。该系统采用韩国三星公司的Exynos4412嵌入式芯片为核心处理器，系统硬件平台主要由嵌入式控制系统、多传感器信息采集子系统、电机驱动模块、采摘机器人主体、以太网口、串口、触摸屏和接近报警装置等组成。系统硬件框架如图4所示。

图4 移动机械手主动防碰撞控制系统硬件设计框架Fig.4 The hardware design framework of active collision avoidance control system for mobile manipulator

系统硬件各个子系统的功能描述如下：

1)嵌入式控制系统。通过获取多传感器模块的参数信息及系统设定的程序流程，实现对移动机械手的主动防碰撞控制。

2)多传感器信息采集子系统。通过不同位置、不同类型的车速传感器、机械手激光雷达以及CCD传感器，增加对环境探测的准确性；然后，利用多传感器融合算法，获得对周边环境的较为统一的感知信息，提高移动机械手主动防碰撞的决策能力。

3)伺服驱动模块。通过多传感器信息采集子系统获得的环境感知信息，由嵌入式系统控制电机驱动器电路，实现对移动机械手运动电机的控制。

4.2 系统软件设计

移动机械手主动防碰撞控制系统软件体系包括嵌入式Linux交叉编译环境的搭建、内核的裁剪、文件系统的移植及多传感器信息采集子系统驱动程序的编写等。其中，多传感器信息采集子系统驱动程序的编写采用模块化的思想设计，其基本流程如图5所示。

图5 多传感器信息采集子系统驱动程序的编写流程Fig.5 The compiling process of driver for multi-sensor information collection subsystem

5 实验结果分析

为了验证本文设计的移动机械手主动防碰撞控制系统的实时性、稳定性和可靠性，在完成关键技术研究及移动机械手主动防碰撞控制系统整机研制的基础上，利用MatLab搭建了移动机械手的仿真系统，并进行了实际的性能测试实验。移动机械手质量m=3kg，重力加速度为g=9.81m/s2。系统控制参数如下：控制输入、输出权重分别为ρ=[0.01 0.01 0.01]T和η=[1 1 1]T。静态障碍物的坐标为Ll=[1.4
1.4
1.4]T和Ul=[1.9 2.5 2.9]T。试验结果如图6所示。

图6 采摘机械手主动避障结果图Fig.6 The result diagram of the active obstacle avoidance of the picking manipulator

从图6可以看出：随着移动机械手移动速度的增加，轨迹控制误差也会增加。例如，当移动机械手移动速度v=0.5m/s时，其运动轨迹误差保持在8cm以下；而当速度v=2.0m/s时，误差接近23.5cm。其主要原因是，移动机械手较大的运动速度，则需要较高的机动性和控制速率，以保证其在较短时间内根据参考轨迹规划最优避障轨迹。

为了更好地分析系统，考虑到障碍物位于内部参考轨迹中，给出了如图7所示的移动机械手主动避障的2D图形。