李 杨，魏兆勇，高国华，赵韦东，陈 平

(北京工业大学材料与制造学部，北京 100124)

近来，全球的灾难频繁发生，人们的生命受到了非常大的危害.而灾害现场的复杂性和危险性给救援人员的工作带来了巨大的困难.随着科学和社会的快速发展，国内外的科学家都开始研究救援机器人.IRobot公司生产的Pack Bot机器人采用履带轮行走，可适应复杂的地形，采用连杆机械臂，可寻找幸存者[1]，负载可达到13.6 kg.八达重工公司设计出一种采用多自由度的双臂机械臂，具有剪切等多种功能，可提起8 t的重物[2].寇彦芸[3]设计了七自由度冗余机械臂，并采用液压驱动，考虑到救援的任务形式，采用双臂作业.

由于救援机械臂的自由度多，运动比较复杂，运动学模型的建立是机械臂轨迹规划的基础，决定了机械手的精度[4]，故需要对救援机械臂进行运动学分析，不同的机械臂构型的逆运动学的解法也不相同.目前比较常用的运动学逆解的方法有解析法、迭代法、几何法等传统方法和遗传算法、神经算法等智能算法.张震等[5]提出了一种基于PSO-RBF神经网络的机械臂逆运动学算法，能够很快得到精度高的关节角度，为机械臂的控制提供了基础.刘世平等[6]提出了建立BP神经网络模型来实现七自由度机械臂逆运动学的求解，并且进行了轨迹跟踪试验证明了模型的正确性.蒋灿灿等[7]利用旋量分析方法进行了运动学分析,采用三次多项式轨迹规划验证了方法的准确性.传统的方法求解比较复杂而且不能求出精确的唯一逆解，而智能算法往往不能保证求解的精度.

本文设计了一种高负载自重比的救援机械臂，通过底下移动履带车的行走实现对救援物资的搬运工作.对救援机械臂正逆运动学方程进行推导，利用机器人工具箱仿真验证运动学模型的正确性，运用BP神经网络和解析法相结合的方式来求取逆解.通过BP神经网络预测到的运动学结果来筛选解析法求出来的4组逆解，进而可以得出唯一的精确解，为进一步的轨迹规划研究奠定了基础.

1 救援机械臂的总体方案

运用三维画图软件SolidWorks进行机械臂模型的建立，五自由度机械臂可以使末端执行器实现空间任意的位姿变换，机械臂的运动包括腰关节的旋转、臂1关节及臂2关节的俯仰、臂3关节的旋转和俯仰5个自由度.救援机械臂的整体模型如图1所示.机械臂具有轻载和重载2种工作模式，通过结构的锁定和驱动的转换，实现轻载和重载模式的切换[8].

图1 救援机械臂的整体模型

2 救援机械臂的运动学分析

2.1 救援机械臂的正运动学分析

2.1.1 坐标系与参数的确立

Denavit-Hartenberg(D-H)参数法是Denavit和Hartenberg提出的，可以描述关节的变换，适合于连杆和关节的建模，通用性比较强[9].本文在每个关节处建立坐标系如图2所示.

图2 救援机械臂坐标系

通过D-H法则建立机械臂的连杆坐标系来对救援机械臂连杆关系及参数进行描述.从坐标系xn-yn-zn到坐标xn-1-yn-1-zn-1的变换矩阵为[10]

An+1=Rot(z,θn+1)×Trans(0,0,dn+1)×

Trans(an+1,0,0)×Rot(x,αn+1)=

(1)

2.1.2 机械臂D-H参数模型

在D-H关节坐标系基础上，建立D-H参数表，如表1所示.

表1 救援机械臂D-H参数表

2.1.3 救援机械臂正运动学建模

机械臂正运动学是指已知各关节的角度，求末端执行器相对于基坐标系的位姿.由式(1)可得机械臂末端坐标系相对于基坐标系的变换矩阵[11].正运动学公式为

(2)

nx=s1s5-s234c1c5

ny=-c1c5-s234c5s1

nz=c234c5

ox=c5s1+s234c1s5

oy=s234s1s5-c1c5

oz=-c234s5

ax=c234c1

ay=c234s1

az=s234

px=c1(a1+a3c23+a2c2+d5c234)

py=s1(a1+a3c23+a2c2+d5c234)

pz=d1+a3s23+a2s2+d5s234

式中：si表示sinθi；ci表示cosθi.

根据救援机械臂的工作范围的设计指标，本文中的参数设置为a1=-65 mm，d1=122 mm，a2=664 mm，a3=300 mm，d5=500 mm.将5个关节角的角度

带入得

2.2 救援机械臂的逆运动学分析

逆运动学求解是根据给定的机械臂末端的姿态和位置，求得所有关节的转动角度[12].求解过程如下：

1)求解θ1

(3)

化解，可以得到等式左边为

等式右边为

通过式(3)的(3，3)元素得

axs1-ayc1=0

可解得

θ1=atan2(ay,ax)，θ1=atan2(-ay,-ax)

2)求解θ5

通过式(3)的(3，1)和(3，2)元素得

nxs1-nyc1=s5

oxs1-oyc1=c5

(4)

可解得

θ5=atan2(nxs1-nyc1,oxs1-oyc1)

3)求解θ234

通过式(3)的(1，3)和(2，3)元素得

(5)

可解得

θ234=atan2(az,axc1+ays1)

4)求解θ3

根据式(3)的(1，4)和(2，4)元素有

(6)

重新排列式(6)，两边平方，然后相加得

(pxc1-a1+pys1-c234d5)2=(a3c23+a2c2)2

(pz-d1-d5s234)2=(a3s23+a2s2)2

所以

c3=

所以

θ3=atan2(s3,c3)

5)求解θ2

参考式(6)，得

pxc1-a1+pys1=a3(c2c3,s2s3)+a2c2+d5c234

pz-d1=a3(s2c3+c2s3)+a2s2+d5s234

所以

可解得

θ2=atan2(s2,c2)

6)求解θ4

θ4=θ234-θ2-θ3

2.3 运动学建模的仿真分析

2.3.1 正运动学模型的仿真分析

利用MATLAB机器人工具箱中的Link函数来建立机械臂的仿真对象[13]，得到仿真模型如图3所示.

图3 机械臂仿真模型

得到结果

可以看出,用MATLAB仿真的结果与正运动学方程推导的结果一致，故建立的模型是正确的.

2.3.2 逆运动学模型的验证

给定一个位姿x、y、z、R、P、Y，应用逆运动学理论可以得到位姿所对应的5个关节角.将得到的关节角度输入到救援机械臂的正运动学模型中,与给定的位姿进行比较，便可以验证逆运动学模型的正确性[14].给定位姿341.51、591.52、-188.40、-0.378、-4.981、171.318.通过计算，得到表2.

通过对比初始位姿x、y、z、R、P、Y和表2右侧4组逆解对应的位姿x′、y′、z′、R′、P′、Y′可以得出，机械臂逆运动学模型是准确的.

表2 逆运动学验证

2.4 基于BP神经网络的运动学逆解

BP神经网络是一种按误差逆传播算法训练的多层前馈网络.BP神经网络由信息的正向传播和误差的反向传播2个过程组成.BP神经网络系统拓扑结构包括输入层、隐含层和输出层.同一层神经不存在相互连接，隐含层可以有1层或者多层，BP算法流程图如图4所示[15].

图4 BP算法流程

2.4.1 BP网络模型的设计

1)输入输出层的设计

输入层节点数一般等于要训练的样本维度数目，输出层节点数根据输出结果的空间维数确定[16].

本文中取输入层节点数为10，分别为nx、ny、nz、ox、oy、ax、ay、px、py、pz，输出节点数为5，为5个关节角.

2)隐含层的设计

Robert Hecht-Nielsen证明了对于任何闭区间内的一个连续函数都可以用一个隐含层的BP神经网络来逼近.通过增加单层隐含层节点的数目，也可以保证网络模型的预测精度，并且这种方式更加方便[17]，故本文选择单隐含层.

隐含层节点数对神经网络的性能有一定影响，隐含层的节点数需要经验和多次试验来确定.根据经验可以参照

(7)

进行设计[18].式中：l为隐含层节点数；n为输入层节点数；m为输出层节点数；a为1～10之间的常数.

根据确定的输入层节点数为10，输出层节点数为5，由经验公式(7)可求出隐含层节点数目的范围是5～14.

3)传递、学习、及训练函数

传递函数必须是连续或可微的，常采用S型的对数或正切函数和线性函数[19].本文中隐含层传递函数使用tansig函数，输出层传递函数使用purelin函数.

学习函数有learngd函数和learngdm函数，本文建立的网络模型使用learngdm学习函数.

训练函数有trainbfg函数、traingdm函数、traingdx函数、Levenberg-Marquardt算法trainlm函数.本文使用trainlm函数进行训练,因为该算法对比一般的BP网络算法,收敛速度较快，预测精度较高.

4)学习速率的选取

学习速率可以决定循环训练中的权值变化量，学习速率太大，网络可能不稳定，学习速率太小导致网络训练的时间太长，可能收敛太慢，一般来说学习速率的选取范围在0.01～0.80[20].

2.4.2 BP网络运行

1)数据的处理

利用正运动学模型，在MATLAB中随机选取2 000组关节角度，得到运动学矩阵中的10个参数，其中1 600组数据作为训练数据，400组数据作为预测数据.

BP神经网络的输入和输出参数的大小差距较大，会导致网络模型最终计算结果的不准确，所以需要进行归一化处理.本文的网络模型使用mapminmax函数进行数据归一化[21]，将数据映射到[0,1]内，它的变换式为

(8)

2)BP模型的建立

由于前文确认BP网络中隐含层的节点数为5～14，学习效率为0.01～0.80，故需设计模型来确定最优的参数.

通过编写程序在MATLAB中运行，其中隐含层节点数的采样间隔是1，学习效率的采样间隔是0.01.得到结果可知，当隐含层节点数为7、学习速率为0.22时，训练的均方误差最小.

确定好BP神经网络的参数后进行训练，得到训练迭代过程如图5所示.

图5 训练迭代过程

对已知点的位姿，应用逆运动学的解析法求出4组逆解，用上面训练好的BP神经网络求出预测的关节角，以均方根误差为依据，筛选出唯一的逆解.

从400组预测数据中随机选取一组关节角(120°，30°，-45°，-60°，30°)，其逆解的解析解及预测值如表3所示，从表3可以看出，最终确定的关节角为(120°，30°，-45°，-60°，30°)，与给定的一致.

表3 末端点的逆解与预测值

之后，随机选取6个点进行验证，结果如表4所示，经过验证，采用BP神经网络得到的预测结果可以对用解析法得到的4组运动学逆解进行筛选，从而得到唯一的逆解[22].

表4 BP筛选运动学逆解结果

3 结论

1)本文设计了一种多自由度救援机械臂，用于救援物资的搬运工作，采用重载和轻载2种工作模式，并且建立了对应的三维模型.

2)本文建立了救援机械臂的正逆运动学模型，通过MATLAB中Robotics Toolbox工具箱对机械臂运动学进行仿真，验证了运动学模型的正确性，采用BP神经网络对机械臂逆运动学逆解进行预测，将预测结果作为依据，对用解析法得到的4组逆解进行筛选，从而得到唯一的精确逆解，为后续的轨迹规划的研究奠定了一定的基础.