自主操作机器人末端执行器的行为动力学运动规划

王文运， 傅卫平， 魏明明， 王 雯， 杨世强， 韩改宁

(西安理工大学 机械与精密仪器工程学院，陕西 西安 710048)



自主操作机器人末端执行器的行为动力学运动规划

王文运， 傅卫平， 魏明明， 王 雯， 杨世强， 韩改宁

(西安理工大学 机械与精密仪器工程学院，陕西 西安 710048)

在动态或人机合作装配场合中，基于规则而不是生物行为启示的现有操作机器人的运动规划方法，难于以自然的方式对工作空间内物体的运动进行描述，而行为动力学方法则能弥补这一缺点。本文在工作空间内，建立不考虑关节约束时的操作机器人末端执行器的行为动力学运动规划模型。首先,定义了操作机器人末端执行器位置运动的基本行为，通过竞争动力学协调得到位置运动的整体行为模型。然后，利用行为动力学方法设计了末端执行器姿态运动的行为。仿真实验结果表明：利用行为动力学方法可以实现操作机器人的静态及动态场景下的自主运动规划。

自主操作机器人； 行为动力学； 运动规划； 位置姿态； 行为协调

自主操作机器人是指在无人员干预或人机共用工作空间的情况下自主完成作业任务的机器人。通常情况下，这类机器人的作业任务只对中间运行轨迹做安全性要求，譬如，自主装配机器人抓取工件或者到达预装位置的过程。如何按一定评价标准设计安全、平稳的机器人中间运行轨迹，构成了操作机器人的运动规划任务。

操作机器人运动规划方法主要有可视图法[1]、构型空间法[2]、快速随机树[3]、人工势场法[4]等，但这些方法存在建模困难或者适合静态场景[5-7]，不适用于与动态环境构成复杂动力系统的自主操作机器人的运动规划问题。 将环境与机器人统一考虑的行为动力学[8]运动规划方法是解决该问题的可能途径。

本文针对自主操作机器人对运动规划的智能、安全等要求，以及国外采用行为动力学进行运动规划时可能出现的突变问题，利用基于竞争动力学协调的行为动力学方法对其工作空间的轨迹进行规划。在不考虑关节避障约束的假设下，建立自主操作机器人末端执行器的位置运动和姿态运动的行为动力学模型，并利用竞争动力学对其行为权重进行动态协调，避免了运动规划时可能出现的突变。通过在静态和动态场景下的仿真实验，验证了该方法规划操作机器人末端执行器运动的可行性。

1 行为动力学运动规划模型

串联型操作机器人的驱动方式与移动机器人不同，除了末端执行器外，还有与之相联接的各关节连杆，它们随着末端执行器一起运动。一般情况下，自主操作机器人运动轨迹通过规划方法得到其工作空间的描述，再通过求逆解获得关节空间的描述。此外，在操作机器人实际工作空间中，可能存在任意复杂轮廓的静态和动态障碍物，此时，环境障碍物对关节连杆运动形成约束。由于考虑关节约束和复杂轮廓障碍物的操作机器人末端执行器运动规划问题涉及到更为复杂的理论和方法，需要专门的研究工作方能完成，因此，本文为了将行为动力学应用到操作机器人末端执行器的三维空间运动规划，暂时不考虑关节连杆的约束，即末端执行器独立运动，并对障碍物进行了简化处理，将其简化为障碍球模型(以障碍物最大轮廓包络面直径作为障碍球的直径)。由于操作机器人末端执行器位置和姿态运动不耦合，本节将分别建立其位置运动和姿态运动的行为动力学模型。

1.1 行为动力学方法简介

行为动力学方法本质上适合解决未知复杂环境中的机器人运动规划问题，其优势在本文给出的相关参考文献中已得到证明。该方法利用非线性动力学基本理论，将机器人与其所处动态环境看成一个复杂动力系统，将环境中的目标和障碍物作为动力系统的平衡点。如果动力系统的特征值均具有负实部，则平衡点是稳定的，在该平衡点构成了一个吸引子(作为目标)，吸引子附近形成了吸引域，在该区域中行为变量逐渐趋向于该平衡点；如果动力系统所有特征值的实部中至少有一个正数，则平衡点不稳定，在该平衡点形成了一个排斥子(作为障碍物)，排斥子附近形成了排斥域，该区域中行为状态变量逐渐远离该平衡点。

行为动力学方法的基本原理是，首先，通过一定的规则[22]设计描述机器人运动的行为变量，得到包含目标吸引子、障碍物排斥子以及机器人当前位置的描述；其次，设计机器人的奔向目标、避障等一系列的基本行为；最终，通过行为的竞争得到整体行为模型，由此产生机器人的规划路径。

假设机器人的行为变量为x(如航向角，速度等)，则其基本行为模型由非线性动力学方程(1)表示。

(1)

其中：t表示时间，env表示机器人所处位置环境，f是当前变量状态x和机器人所处环境env的函数，表示吸引力或排斥力，即描述奔向目标或避障行为。

奔向目标行为模型以吸引子为稳定平衡点，其稳定性由状态变量前的系数为负(负实部特征值)来构造；避障行为以排斥子为不稳定平衡点，其稳定性由状态变量前的系数为正(正实部特征值)来构造。关于行为动力学稳定性证明可参见文献[9-12]。

根据上述方法构造的基本行为，对其通过合适的权重进行线性叠加，得到自主机器人整体行为的动力学模型，即式(2)。

(2)

其中权值ωb∈[-1,1]表示由机器人和环境通过竞争得到基本行为活跃程度(见1.2.3节)。

1.2 自主操作机器人末端执行器位置运动行为动力学建模

选取末端执行器运动参考点作为操作机器人当前位置运动描述点(简称机器人当前点，不特别指出，均为位置运动描述)。选取操作对象的运动参考(如装配孔心)作为目标点集(静止目标为单点)，并将场景内与其无关的物体(其余的操作对象、非执行该任务机器人、操作者等)作为障碍球，对机器人从初始点到达目标点的路径进行规划。

根据行为变量设计准则[22]，如图1所示，在世界坐标系o-xyz内，选取机器人当前点速度v、速度v在局部坐标系x″o″y″平面的投影v″与x″轴的夹角φ以及速度v与局部坐标系x″o″y″平面的夹角θ作为行为变量，基于动力系统理论有相应的行为动力学模型即式(3)。

(3)

图1 机器人当前点位置运动行为变量Fig.1 The behavior variables of current point location of manipulator

1.2.1 奔向目标行为

当工作空间中没有障碍物时，机器人从当前点起始,沿直线趋向目标点的运动称为奔向目标行为。

图2 操作机器人末端执行器xoy平面投影Fig.2 The projection of xoy plane of end executor of manipulator

根据奔向目标行为描述，建立目标点为稳定平衡点的动力学模型如式(4)所示。行为变量v不能在奔向目标时无限制增大，因此，将奔向目标的速度平衡点设定为合适的有限值，当奔向目标行为权值激活后，速度v随着动力系统的渐近稳定逐渐趋向期望速度。

(4)

其中：φtar、θtar分别为目标点相对于机器人当前点的x″轴方向、x″o″y″平面的方位角；vmax为机器人运动最大线速度；φ、θ、v均为操作机器人行为变量；λtar、λv为吸引强度；dtar为目标点与机器人当前点之间的距离(见式(5))；c1为吸引强度衰减系数；c2为最小吸引强度；cv为速度变化影响系数。

(5)

趋向目标行为动力学模型的行为变量φ、θ、v与φtar、θtar、dtar、vmax有关，与其余环境信息无关。

1.2.2 避障行为

机器人当前点的避障行为是指其在奔向目标点过程中遇到障碍时，能够自主安全地绕过障碍的行为。本节设计了场景内为单个障碍物的情况下的基于不稳定平衡点的避障行为动力学模型，即式(6)。当机器人工作空间存在障碍物时，首先获得每个障碍物的避障行为模型，然后将其线性叠加，最终得到考虑所有障碍物影响的避障行为模型。

(6)

其中：φobs、θobs分别为障碍物简化球心相对于机器人当前点的x″轴方向、x″o″y″平面方位角；vmin为避障期望速度；λobs为排斥强度；λv为避障期望速度的吸引强度；dobs为机器人当前点与障碍球心之间的距离(见式(7))；c3、c4分别为排斥力的距离和速度衰减系数；R′为规划路径的曲率半径；σ为机器人避障排斥的角度范围，(见式(8))。

(7)

(8)

式中r是机器人当前点简化成球体后的半径，R是障碍物最大半径。

1.2.3 操作机器人当前点的整体行为模型

操作机器人当前点的整体行为是外部环境与操作机器人内部固有性质共同决定的行为。通过设计随整个系统内部相对位置及速度因素变化而动态变化的权重系数，得到线性叠加后的整体行为模型，即式(9)。

(9)

式中，ωtar为趋向目标行为权重，ωobs为避障行为权重，b为动力系统阻尼系数(防止运动轨迹发散)。

式(9)中，ωtar∈[0,1]和ωobs∈[0,1]作为操作机器人当前点的行为权重，反映了奔向目标行为与避障行为的相互抑制程度。基于非线性动力学理论，通过设计合适参数，得到奔向目标行为与避障行为的竞争动力学模型如式(10)[23]。

(10)

式中，α1、α2分别为奔向目标行为、避障行为的竞争优势；γ12为奔向目标与避障行为共存时避障行为对奔向目标行为的制约程度；γ21为奔向目标与避障行为共存时奔向目标行为对避障行为的制约程度。

α1、α2、γ12、γ21均为与环境相关参数， 考虑到取值范围为[0,1]以及速度和障碍物距离对其产生的影响，设计竞争参数式(11)。

(11)

式中，do为机器人与障碍物之间的距离；de为相互作用区中心与障碍物之间的距离；dc为相互作用区半径；co1～co4为竞争参数的衰减系数。

1.3 自主操作机器人姿态的行为动力学建模

将机器人当前姿态假想成具有方向的球体，如图3所示姿态方向线ori在局部坐标系x″o″y″平面的投影ori″与x″轴夹角φ以及姿态方向线ori与局部坐标系x″o″y″平面的夹角ψ作为姿态运动的行为变量；选取操作机器人工作空间中的被抓物体形心所在中心线姿态作为目标姿态吸引子，如图4所示目标中心线ori在局部坐标系x″o″y″平面投影ori″与x″轴夹角为φtar以及目标中心线ori与局部坐标系x″o″y″平面夹角ψtar，建立操作机器人末端执行器的姿态行为动力学模型，即式(12)。

(12)

其中,λori为姿态吸引强度。

图3 机器人当前点姿态行为变量Fig.3 The behavior variables of current posture of manipulator

(o-xyz为世界坐标系，x″y″z″为随操作对象平移的坐标系，其坐标原点为操作对象的形心)图4 目标姿态Fig.4 The posture of target

2 仿真实验

2.1 静态场景运动规划仿真实验

本次仿真实验所设计的静态场景是指操作机器人的工作空间内的目标的位置及姿态和障碍简化球的位置及尺寸不发生变化。实验的模型参数设置如下：

行为动力学模型初始参数设置如下：

从图5可以看出，操作机器人位置运动轨迹实现了三维避障，并且随着位置趋近目标点的同时，姿态也在趋近目标姿态。仿真实验中奔向目标和避障行为的权重变化如图6所示，在接近障碍物时避障行为权重急剧增加，奔向目标行为权重相应降低；当远离障碍物时，避障行为权重迅速下降，奔向目标行为权重相应上升，实现了基本行为通过竞争动力学得到激活或者抑制的目的，且不出现突变，但速度有变化。操作机器人位置运动当前点的线速度变化如图7所示，奔向目标时线速度增加到限定值，奔向目标行为和避障行为共存时速度降低到安全速度。

(红色箭头代表起点及其姿态，黑色箭头代表目标点及其姿态，蓝色箭头代表机器人姿态)图5 末端执行器位姿运动参考点三维轨迹Fig.5 The 3D posture movement trajectory of end executor point of manipulator

(蓝色图线代表奔向目标行为，红色图线代表避障行为)图6 奔向目标行为和避障行为权重变化图Fig.6 The weight variable figure of behavior towards the target and avoidance obstacle

图7 末端执行器运动参考点速度变化图Fig.7 The velocity variable figure of end executor of manipulator

从静态场景仿真实验可以看出，采用行为动力学方法可以实现静态场景下的自主操作机器人末端执行器的运动规划任务。采用酷睿i5处理器、主频为2.5GHz的笔记本电脑，在MATLAB平台R2010中运行仿真程序，平均每步规划计算时间136ms，可以满足实时性要求。

2.2 动态场景运动规划仿真实验

根据动态场景的要求，设计障碍球半径变化规律如下：ri=ai+bi*rand(1)，其中ri为第i个障碍球半径，ai为基本半径(范围为5～15)，bi为半径变化因子(范围为5～8)，rand(1)为0～1之间随机数。为了方便比较，障碍球半径变化仿真实验进行了两次，实验数据在同一幅图中显示(其中障碍球半径为基本尺寸)。从图8可以看出，由于障碍球尺寸的微小变化，两条规划路径差异比较大，但是末端执行器位置运动轨迹均实现了平滑避障。

图8 障碍球半径动态变化的末端执行器运动规划Fig.8 The motion planning of end executor in the case of obstacle sphere dynamic change

针对动态场景中可能会有移动障碍的情况，同时考虑障碍球半径随机变化，设计移动障碍球的运动规律：

式中t为迭代步。

从图9可以看出，当工作场景中存在移动障碍时，末端执行器能够平滑地避开移动障碍球。

图9 存在移动障碍的末端执行器运动规划Fig.9 The motion planning of end executor in the case of obstacle movement

从动态场景仿真实验可以看出，行为动力学方法在一定条件下适应场景动态变化的运动规划任务，而且规划路径比较灵活，可以实现自主机器人的行为演化。

3 结 语

运动规划是操作机器人成为自主装配机器人面临的关键问题。为了解决现有操作机器人运动规划方法不能很好适应自主操作机器人与环境的耦合信息难以融合问题，本文采用行为动力学方法对操作机器人末端执行器工作空间的位置和姿态的运动进行了规划。研究表明，依据所建立的行为动力学运动规划模型，可以在静态或动态的工作场景中产生光滑连续的末端执行器位姿运动轨迹，实现无关节约束时的操作机器人导航要求。由于行为动力学方法只需要底层工作空间中实体的距离信息，因此也适用于其它类型操作机器人的运动规划。

下一步研究将以行为动力学为基础，主要围绕以下内容展开：①通过基于机器视觉的末端执行器空间运动规划轨迹的检测，试验验证本文的理论模型；②考虑机器人末端执行器速度平滑的运动规划；③操作机器人关节连杆约束避障的运动规划；④复杂场景下的操作机器人运动规划；⑤模型参数的自适应动态调整，以提高操作机器人运动规划的自主性。

[1]RASHIDAT,ALIAA,FRASCAM,etal.Pathplanningwithobstacleavoidancebasedonvisibilitybinarytreealgorithm[J].RoboticsandAutonomousSystems, 2013, 61(12): 1440-1449.

[2]ROYD.Algorithmicpathplanningofstaticrobotsinthreedimensionsusingconfigurationspacemetrics[J].Robotica, 2011, 29(2): 295-315.

[3]周芳，朱齐丹，张新福，等. 基于雅可比转秩-RRT法的冗余机械手路径规划[J]. 控制工程，2009，16(6)：756-759.

ZHOUFang,ZHUQidan,ZHANGXinfu,etal.PathplanningapproachforredundantmanipulatorbasedonJacobiantranspose-RRTalgorithm[J].ControlEngineeringofChina, 2009, 16(6): 756-759.

[4]彭艳，国文青，刘梅，等. 基于切点优化人工势场法的三维避障规划[J]. 系统仿真学报，2014，26(8)：1758-1762.

PENGYan,GUOWenqing,LIUMei,etal.Obstacleavoidanceplanningbasedonartificialpotentialfieldoptimizedbypointoftangencyinthree-dimensionalspace[J].JournalofSystemSimulation, 2014, 26(08):1758-1762.

[8]IOSSIFIDISI.Utilizingartificialskinfordirectphysicalinteraction[C]//2013IEEEInternationalConferenceonRoboticsandBiomimetics(ROBIO), 2013: 810-815.

[12]LARGE E W, CHRISTENSEN H I, BAJCSY R. Scaling the dynamic approach to path planning and control: Competition among behavioral constraints[J]. The International Journal of Robotics Research, 1999, 18(1): 37-58.

[13]XU Heting, Yang S X. Real-time collision-free motion planning of nonholonomic robots using a neural dynamics based approach[C]//2002 Proceedings ICRA’02 IEEE International Conference on Robotics and Automation, 2002: 3087-3092.

[14]ALTHAUS P, CHRISTENSEN H I. Behavior coordination in structured environments[J]. Advanced Robotics, 2003, 17(7): 657-674.

[19]IOSSIFIDIS I, MALYSIAK D, REIMANN H. Model-free local navigation for humanoid robots[C]//2011 IEEE International Conference on Robotics and Biomimetics (ROBIO), 2011: 2204-2210.

[20]雷艳敏，朱齐丹，冯志彬. 基于速度障碍和行为动力学的动态运动规划[J]. 华中科技大学学报(自然科学版)，2011，39(4)：15-19.

LEI Yanmin, ZHU Qidan, FENG Zhibin. Dynamic path planning using velocity obstacles and behavior dynamicals[J]. Journal Huazhong Univsity of Science and Technology(Natural Science Edition), 2011, 39(4): 15-19.

[21]郝大鹏，傅卫平，王雯. 基于接触时间的移动机器人安全导航[J].计算机应用，2014，34(4)：1209-1212.

HAO Dapeng, FU Weiping, WANG Wen. Mobile robot safety navigation based on time to contact[J]. Journal of Computer Applications, 2014, 34(4): 1209-1212.

[23]LARGE E W, CHRISTENSEN H I, BAJCSY R. Dynamic robot planning: cooperation through competition[C]//1997 IEEE International Conference on Robotics and Automation, 1997: 2306-2311.

(责任编辑 王卫勋)

Behavior dynamics method for the motion planning of the end-effector of autonomous manipulator

WANG Wenyun, FU Weiping, WEI Mingming, WANG Wen, YANG Shiqiang, HAN Gaining

(School of Mechanical and Precision Instrument Engineering, Xi’an University of Technology, Xi’an 710048, China)

In the dynamic or human-robot cooperation assembly scene, it is difficult for the existing methods for path planning of manipulator based on rules rather than the enlightenment of biological behavior to describe the movement of objects in the space with the natural way, with behavior dynamics method making up for the disadvantages. In this paper, the behavior dynamics method is used in working space of the end-effector of manipulator for the motion planning. Firstly, we define the basic behavior for position movement of robot end-effector, with the position movement of the overall behavior obtained by a competitive dynamics model; secondly, we use the behavior dynamics method for designing orientation movement behavior. The results of the simulation experiment show that the use of the behavior dynamics method can realize the manipulator’s motion planning in the static and dynamical scene.

autonomous manipulator; behavior dynamical method; motion planning; position and orientation; behavior coordination

10.19322/j.cnki.issn.1006-4710.2016.04.016

2015-11-02

国家自然科学基金资助项目(51475365)；陕西省自然科学基础研究计划重点资助项目(2011JZ012)；陕西省科技统筹创新工程重点实验室资助项目(2014SZ10-P04)；陕西省教育厅重点实验室科研计划资助项目(13JS070)

王文运，男，硕士生，研究方向为智能机器人技术。E-mail: wyw11_11_11@foxmail.com

傅卫平，男，博士，教授，博导，研究方向为智能机器人、智能车辆及其控制理论与技术、机电系统动力学及其控制、现代物流系统工程与技术等。E-mail: weipingf@xaut.edu.cn

TP242.6

A

1006-4710(2016)04-0468-07