运用非线性规划对小型机械臂逆运动学的优化求解

吕北轩,熊 峰

(上海大学 机电工程与自动化学院 上海市智能制造及机器人重点实验室，上海 200072)

运用非线性规划对小型机械臂逆运动学的优化求解

吕北轩,熊 峰

(上海大学 机电工程与自动化学院 上海市智能制造及机器人重点实验室，上海 200072)

应用Denavit-Hartenberg 法建立起简化的小型机械臂的正运动学方程，并以机械臂由起始点到目标点各关节角度变化最小为优化目标，运用非线性规划针对运动方程的求解非进行优化，相应地运用Matlab构建出在非线性约束下的多变量最优化问题的fmincon函数。运用多组数据，以 Robotics Toolbox 作为对照对象，对机械臂正逆运动学进行仿真，从而验证所构建机械臂运动方程的正确性及非线性规划的合理性，为简单小型机械臂的末端定位问题提供了一种高效准确的逆向求解方法。

非线性规划；小型机械臂；逆运动学

0 引言

机械臂，作为由末端执行、杆件及关节组成的机电一体化的运动机构，已在工业生产、教学应用等诸多方面得到了广泛的应用[1]。特别是近年来出现了很多小型低成本的简单机械臂的产品，对于这类产品一般来说对精度、速度、末端运动轨迹的要求并不很高，且基本可以完成工作空间内的任务[2]。因此在很多情况下只需根据末段位置，确定各个关节角度即可。

已知末端位姿或位置求出满足要求的各关节角，即机械臂运动的逆向求解，一直为国内外机器人学的研究热点之一，采用方法主要分为几何图解与数值解析两种方法，前者求解较为直观，可以减少分离变量法时复杂庞大的计算量，对一些特殊结构的机械臂有着良好的效果，但不具备普适性[3-5]；后者通过求解非线性运动方程或用等价的最小化问题近似迭代求解[6]。近年来，学界多采用蒙特卡洛进行统计求解，但为了得到精确解，蒙特卡洛法往往需要大量的随机样本，使得计算效率较低[7-8]。

非线性规划，是以非线性约束条件或目标函数的数学规划，是运筹学的一个重要分支[9]，其用以求解一个n元实函数在一组等式或不等式的约束条件下的极值问题，且目标函数和约束条件至少有一个是未知量的非线性函数[10-11]。机械臂的运动方程是典型的非线性运动方程，因此对机械臂运动的逆向求解问题，可以将其转换有约束的非线性规划问题。通过建立机械臂运动方程，进而对其逆向求解问题进行了研究，提出了一种可以提高求解效率、简化求解方法，并具备一定的简便性与可操作性的非线性规划求解机械臂逆运动学的方法。

1 机械臂运动方程的构建

1.1 机械臂机构简述

以如图1所示的5自由度的小型机械臂作为研究对象，此类机器臂可以看作由一组连杆通过关节铰接而成的运动链，从而实现机械臂末端在三维空间的灵活运动。本机械臂由4个单元、5个自由度(分别由5个不同的旋转关节实现)，其中基座腰部由两个旋转关节组成，肩部、肘部、及腕部的单元各由一个旋转关节组成。为了便于分析和计算，对机器人结构进行简化，由于末端夹持器为对称结构，因此第5自由度a5对末端夹持器的最终位置可认为无影响[12]。而所有关节全部是旋转关节，a2,a3,a4在一个平面中，简化后的机械臂的等效简化模型如图2所示。

图1 某型小型机械臂实物图

图2 机械臂等效简化机构图

1.2 运动方程的构建

机械臂的运动方程是典型的非线性运动方程，因此对机械臂运动的逆向求解问题，应先建立运动方程，再将其化为非线性规划的约束和目标。机械臂的末端夹持器在笛卡尔空间中的位置是由a1到a4各个关节角度确定的，对于给定的一组角度即可确定一个唯一的位置[x，y，z]。首先先建立各关节角度与位置的方程。设关节角度A=[a1,a2,a3,a4],各连杆长度R=[R1,R2,R3]，第i个关节末端坐标为(xi,yi,zi)，因此x3，y3，z3表示为末端夹持器的最终位置。利用Denavit-Hartenberg表示法变换后得到各连杆末端的坐标方程如下：

2 非线性规划约束方程及目标函数建立

因所建立的机械臂运动方程的变量数和约束数目相对较少，故在非线性约束下的多变量最优化问题的求解，可以用Matlab函数fmincon进行求解[13]。

2.1 约束方程与目标函数的建立

所研究的问题可用下面的标准型表示：

minF(X)

s.t.Ax<=bAeqX=beqG(X)<=0Ceq(X)=0VLB<=X<=VUB

其中,X是n维变元向量，A、b是线性不等式约束，Aeq、beq是线性等式约束，G(X)和Ceq(X)均是非线性函数组成的向量，VLB、VUB是下上界。因此，利用运动方程可以建立A={a1,a2,a3,a4}的非线性等式约束Ceq(A)如下：

其中,x0,y0,z0表示末端位置的空间坐标,利用每个关节的回转角度建立上下界，具体设为VLB=[0,0,0,0],VUB={180,180,180,180}。

2.2 目标函数的建立

所设定的非线性规划求解目标为由起始点到目标点各关节角度变化最小(起始点的关节角度可以将起始点当目标点由任意4个合理的关节角度用本方法求得)。设起始点位置在笛卡尔空间中坐标为[x0,y0,z0]，关节角度为[a0(1),a0(2),a0(3),a0(4)]，目标点位置在笛卡尔空间中坐标为坐标为 [x,y,z]，关节角度为 [a(1),a(2),a(3),a(4)]。在分别给出关节长度R1、R2、R3及起始点各关节角度和目标点坐标后，运行Matlab即可得到需要的结果。

3 仿真验证

在Matlab软件环境下，利用Robotics Toolbox工具箱中的link()和SerialLink()语句， 用相同的参数来建立机械臂的虚拟模型，进行对比仿真实验。实验分为两部分: ①正运动学方程验证；②逆运动学求解验证，用以验证运动方程的正确性。

3.1 正运动学方程验证

首先验证正向运动学方程F1,F2,F3的正确性。随机产生多组关节角度，先利用Robotics Toolbox的过fkine()函数计算末端位置坐标，再用F1,F2,F3运动方程计算，对比计算结果。以所有关节转角为0°时作为初始位姿，a1,a2,a3,a4=[60,100,160,140]作为机械臂的末端位姿为例，Robotics Toolbox相关代码及结果如下：

得到[X,Y,Z]=[ -19.6211, 11.3282, 4.5551]，其机械臂位姿如图3所示。

图3 在Robotics Toolbox的末端位置

后通过前面建立的F1,F2,F3运动方程利用同组数据计算后的结果进行对比，计算得到的末端位置如图4所示。

ans=-19.6211 11.3282 4.5551

两组结果一致，说明建立的运动方程是正确的。

图4 运动方程计算得到的末端位置

3.2 逆运动学求解验证

而后进行求解运动学方程的验证，即解机械臂的逆运动学问题。同样随机产生多组坐标，分别利用Robotics Toolbox的fkine() 函数和所研究的非线性规划方法，以移动角的总和作为目标函数进行求解，相关数据与结果如表1所示：

表1 相关实验仿真数据

对数据与结果进行比对后分析，可知：

(1)通过对比Robotics Toolbox求解结果可知，使用非线性规划求解结果与其全部一致，从而验证了非线性规划在机械臂正、逆运动学方面应用的有效性。

(2)在求解时间上，非线性规划的求解速度明显快于Robotics Toolbox，从而说明了其应用的便捷性。

(3)在求解目标函数方面，非线性规划明显小于Robotics Toolbox的逆向求解结果，从而达到了优化的目的。

(4)对比Robotics Toolbox求解结果，非线性规划求解没有出现无解的情况，且所有解完全在约束范围内。

表2 两种方法的结果对比

4 结论

通过多组随机数据，以Matlab Robotics Toolbox 作为仿真实验的对照，验证了所构建的机械臂运动方程的正确性及使用非线性规划进行对机械臂正、逆运动学进行的合理性。

利用非线性规划对小型机械臂运动的逆向求解方法比较简单且计算速度快、效率高，精度能达到相应的要求，且可通过对目标函数及约束方程的调整得到比较理想符合要求的结果，如可以要求某个关节的角度变化最小等。可以使用其计算机他语言(如C语言)调用Matlab，在小型机械臂的控制程序中较方便的利用非线性规划来逆向求解以达到对机械臂运动优化和控制，为简单小型机械臂的末端定位逆向求解提供了一种简便高效的方法。

[1] 王勇,李秀娟.某四自由度机械臂建模与MATLAB仿真研究[J].电子测量技术,2015 (2):32-33.

[2] 魏德川.高精度五自由度机械臂的轨迹控制[D].哈尔滨：哈尔滨工业大学,2008.

[3] 韩兴国，殷鸣，刘晓刚,等.6R机器人逆运动学求解与运动轨迹仿真[J].四川大学学报工程科学版, 2015, 47(6):185-187.

[4] 刘亚军，黄田.6R操作臂逆运动学分析与轨迹规划[J].机械工程学报, 2012, 48(3):9-12.

[5] Guan Y, Yokoi K, Zhang X.Numerical methods for reachable space generation of humanoid robots[J].Yhe International Journal of Robotics Reserach，2008,27(8)：935-950.

[6] Dinesh Manocha，John F．Canny．Efficient inverse kinematics for general 6r manipulators [J]．IEEE Transactions on Robotics and Automation，1994，10(5)：648-657．

[7] 张明，何庆中，王志鹏，等.关节型码垛机器人的工作空间分析[J].组合机床与自动化加工技术，2013 (7) :68-74．[8] 杜健超，孙卫红，马冠宇.基于拉丁超立方体抽样的六自由度机械臂工作空间分析[J].组合机床与自动化加工技术，2016 (7) :30-31．

[9] 薛亚宏.一类基于MATLAB程序的线性规划及数学模型的求解[J].南京工业职业技术学院学报, 2011,11(4):16-18.

[10]顾基发，汪寿阳，黄思明.一个新的线性规划多项式算法——Karmarkar 方法[J].数学的实践与认识,1985(2):75-78.

[11]王春香,冯慧忠.MATLAB软件在机械优化设计中的应用[J].机械设计, 2004(7):52-53.

[12]Jorge Angeles. Fundamentals of robotic mechanical systems [M].New York: Springer, 1997.

[13]江伟伟.多自由度机器人的运动学仿真的研究[J].机械制造与自动化, 2014(6):166-167.

OptimalSolutionforInverseKinematicsofaSmallSimplifiedMechanicalArmwiththeApplicationofNonlinearProgramming

LV Bei-xuan, XIONG Feng

(School of Mechatronic Engineering and Automation,Shanghai Key Laboratory of Intelligent Manufacturing and Robotics,Shanghai University,Shanghai 200072,China)

The kinematics equations of a small simplified mechanical arm were established in Denavit-Hartenberg method, and optimized the goal of the minimum overall changes of each joint angle from the starting point to the target point, thus using linear programming for solving the non-optimized motion equation to construct multi-variable-optimization problem under nonlinear constraints with the function fmincon in Matlab. By using multiple sets of data, compared with Robotics Toolbox, the forward kinematics and inverse kinematics of the manipulator were simulated, which verifies the correctness of the kinematic equation of the manipulator and the rationality of the nonlinear programming. It provides an efficient and accurate method for solving the problem of the terminal location of small simplified mechanical arms.

nonlinear programming；small simplified mechanical arms；inverse kinematics

TH166；TG659

A

1001-2265(2017)12-0070-03

10.13462/j.cnki.mmtamt.2017.12.017

2017-02-15；

2017-03-21

吕北轩(1990—)，男，内蒙古人，上海大学硕士研究生，研究方向为机械设计理论及先进制造技术，(E-mail)lbx_workmail@163.com；通讯作者：熊峰(1974—)，男，重庆人，上海大学副研究员，博士后，研究方向为先进制造技术和网格制造，(E-mail)shurobots@126.com。

(编辑李秀敏)