机械外文翻译外文文献英文文献机械臂动力学与控制的研究

外文出处：Ellekilde, L. -., & Christensen, H. I. (2009). Control of mobile manipulator using the dynamical systems approach. Robotics and Automation, Icra 09, IEEE International Conference on (pp.1370 - 1376). IEEE.
机械臂动力学与控制的研究
拉斯彼得Ellekilde
摘要
操作器和移动平台的组合提供了一种可用于广泛应用程序高效灵活的操作系统,特别是在服务性机器人领域。

在机械臂众多挑战中其中之一是确保机器人在潜在的动态环境中安全工作控制系统的设计。

在本文中,我们将介绍移动机械臂用动力学系统方法被控制的使用方法。

该方法是一种二级方法, 是使用竞争动力学对于统筹协调优化移动平台以及较低层次的融合避障和目标捕获行为的方法。

I介绍
在过去的几十年里大多数机器人的研究主要关注在移动平台或操作系统，并且在这两个领域取得了许多可喜的成绩。

今天的新挑战之一是将这两个领域组合在一起形成具有高效移动和有能力操作环境的系统。

特别是服务性机器人将会在这一方面系统需求的增加。

大多数西方国家的人口统计数量显示需要照顾的老人在不断增加,尽管将有很少的工作实际的支持他们。

这就需要增强服务业的自动化程度，因此机器人能够在室内动态环境中安全的工作是最基本的。

图、1 一台由赛格威RMP200和轻重量型库卡机器人组成的平台这项工作平台用于如图1所示,是由一个Segway与一家机器人制造商制造的RMP200轻机器人。

其有一个相对较小的轨迹和高机动性能的平台使它适应在室内环境移动。

库卡工业机器人具有较长的长臂和高有效载荷比自身的重量,从而使其适合移动操作。

当控制移动机械臂系统时,有一个选择是是否考虑一个或两个系统的实体。

在参考文献[1]和[2]中是根据雅可比理论将机械手末端和移动平台结合在一起形成一个单一的控制系统。

另一方面,这项研究发表在[3]和[4],认为它们在设计时是独立的实体,但不包括两者之间的限制条件,如延伸能力和稳定性。

这种控制系统的提出是基于动态系统方法[5],[6]。

它分为两个层次，其中我们在较低的水平，并考虑到移动平台作为两个独立的实体，然后再以安全的方式结合在上层操纵者。

在本文中主要的研究目的是展现动力系统方法可以应用于移动机械臂和使用各级协调行为的控制。

本文剩下的安排如下。

第二部分介绍系统的总体结构设计,其次是机械手末端移动平台的控制在第三第四部分讲述。

在第五部分我们在结束本文之前将显示一些实验。

然而,
首先与动力学系统有关工作总结与方法将在在部分I-A提供。

A.相关工作
动力学系统接近[5]， [6]为控制机器人提供一套动作的框架，例如障碍退避和目标捕捉。

每个动作通过一套一个非线性动力学系统的attractors和repellors来完成。

这些通过向量场的简单的加法被结合在一起来完成系统的整体动作。

动力系统的方法涉及到更广泛的应用势场法[7]，但具有一定的优势。

这里势场法的行为是由后场梯度形成的结果，行为变量，如航向和速度，可直接运用动力系统控制的方法。

成本相对较低的计算与方法有关，使得它在动态环境中在线控制适宜，允许它即使在相当低的水平有限的计算能力平台[8]实施。

传感器的鲁棒性在人声嘈杂中显示[9]和[10]其中一个是由红外传感器和麦克风的结合，当避障和目标获取时使用。

尽管能解决各种各样的任务，但它仅是一个局部的方法，为了其他的任务和使命级计划(即参见[11])其他的方法应该被采用。

当多行为被结合时，在[5]和 [6]的缺点是由潜在的假的因子引起的。

为了克服这个问题[12]介绍了一种基于竞争动态的行为比重。

每个行为的影响是控制使用一个相关的竞争优势，再加上定义的行为之间有竞争力的相互作用，控制重物。

如果所有的行为之间的竞争性相互作用是必需的，这种方法可以推广到任意数n，行为，除了这样一个最坏
情况的复杂度()2nϑ。

在现实世界中使用这种方法的竞争态势室内实验中可以找到[13]，[14]。

[13]是只在有标题方向的车辆上使用，而在[14]中航向和速度均得到控制。

[15]提供了一个为速度性能简短的策略讨论。

在[16]中提到动力系统的方法不仅被用于平面移动机器人，同时也可以作为控制机械手工具。

另外运用产生极限环Hopf振荡器动力系统的更复杂的动力系统也可被使用。

[17]展现出不同形状的极限环是如何产生的，其可运用于避障轨迹的生成。

[18]中介绍到使用Hopf振荡器产生一个定时的轨迹，实现了机械手可以接住从桌子上面滚下来的球。

动力系统的方法不仅可以用于控制的工具，也可以控制7自由度机械手多余的动作这一点在[19]中得到论证。

II.总体结构
我们整个系统的整体架构如图2所示。

在赛格威平台中为了控制移动平台，两个低级别的性能被使用：一个用于目标捕获和另一个是避障。

运用竞争动态的动作被混合在一
起是为了做出移动平台希望得到的指定的移动动作。

同样，在竞争态势的基础上目标捕获和机械手避障行为的融合给机器人收缩下达指令。

当目标不在范围内，应收回机械手到一个安全的位置，这是机械手缩回行为的目的。

最后融合是以一个安全的方式把所有的控制结合在一起，这样一来目标捕获和收回行为不互相干扰，另外移动平台在不开始朝着新的目标之前，移动机械手已被收回。

图.2. 控制系统的体系结构
用w
mobile
、
manip acquisition
w 和manip retract w 分别代表机械手移动、机械手捕获和机械手收缩行为的影响，控制信号mobile u 和manip
q
通过（1）（2）移动平台和机械手。

()left right
u mobile mobile
u u w = （1）
manip
manip manip
manip
manip
acquisition acquisition retract retract
q
q q w w =+ (2)
其中（left u right u ）是指控制输入信号以控制在第三节中描述的平台的左，右侧车轮；
manip
acquisition
q
和
manip retract
q
是在第四节描述的机械手关节速度。

A.竞争动态
这种竞争态势采用的方法是以[12]为基础的，除了附加参数b T 用于控制在[14]中的转换率。

动力系统采用（3）因此给予：
''3'2
(),b b b b b b b b b
T w a w w rb bw w noise ≠=--+∑ （3）
其中b a 是b 和r 'b 竞争优势产生的参数，b 是'b 和b 相互竞争作用的参数。

1）移动：在移动平台远离目标时它的竞争优势应该被加强；当目标被捕获时移动平台的竞争优势应该被降低。

这是通过（4）实现的。

tanh(())mobile mobile mobile
a tar threshold a k d d =-
（4） 其中，mobile a k 决定如何迅速的改变这种优势，tar d 是指到目标的距离和mobile
threshold d 是指移动平
台移动目标所需的最小距离。

移动的行为，没有能力进行互动，并抑制其他行为，因此它的竞争性相互作用被设置为0。

2）机械手捕获目标：当移动平台接近他的目标时，机械手捕获目标的动作应该别加强。

这样的竞争优势将被定义为：
tanh(())manip manip manip
acuisition a tar threshold a k d d =--
（5） 激活距离manip threshold d 必须大于mobile
threshold d 来确保其行为被激活。

此动作没有和其他的动作有
直接联系，因此它的相互作用参数设置为0。

3）机械手收缩：收回动作应该被激活当对面目标被捕获之后，因此
manip manip
retract acqisition
a a =- tanh(())manip mobile
a tar threshold k d d =- （6）
其中cur q 和hom e q ，是机械手当前和原始配置参数，q ε是指目标hom e q 最近的距离和retract
r k 指
定如何使相互作用迅速变化的参数。

III. 移动平台的控制
该移动平台的控制，结构与参考文献[14]中表述的非常相似，但也有一些不同。

刚开始时目标捕获和避障指令被使用。

紧接着除走廊和墙壁避障不包括在内，但将沿直线扩展。

第二个领域，不同的是这项工作的障碍是如何找出障碍密度的计算方法。

具体的论述在III-D 部分。

为了使控制系统能够根据具体的环境进行导航。

我们所使用的方法是基于参考文献[20]中论述的方法，它运用里程计和激光测距相结合对所在环境中地图的主导线匹配测量。

该平台控制编码的使用方向：φ；速度：V ，它在一个控制输入系统的结果数
{},mobile f Φ
ν= 的值是由两部分组成，mobile tar
f
和
mobile obs
f
，这里合并为
mobile
mobile mobile mobile
mobile
tar
obs
tar
obs
f
f
f
w w =+ （8）
其中mobile
tar w 和mobile
obs w 是被Eq 限制的。

(3)中的竞争优势和相互作用在III-C 中有详细的
描述。

A.动态目标:
捕获目标动作的基本动力是：
,,()sin()mobile mobile tar tar tar
f φφ
φλψφ=- (13)
,,max ()(min(,))mobile v mobile v mobile tar tar tar tar
v k d v v f
λ=- (14)
(a)移向目标（t=0s ） (b)图像伺服 (t= 28s)
(c) 移动到目标位置 (t = 40s) (d) 完成动作 (t = 72s)
图.3移动机器人实验。

假定环境和目标重物的角度是不变的。

在实验中使用的平台如图1所示，是由一个赛格威RMP200和轻重量型库卡机器人与崇德PG70平行爪装备组成。

该平台具有一个SICK LMS291定位和避障装有Unibrain
Fire-iFireWire摄像头的激光扫描仪，用于机械手瞄准并抓起目标。

不幸的是我们没有足够的时间来连接夹持器和控制目标。

因此，它仅仅是定位和准备抓。

但实际上从未关闭的抓手。

由于控制框架我们使用了Microsoft Robotics Studio1.5，这提供了一个从传感器的各种输入，到驱动器输出，并确保不同的控制算法同时运作的方法。

该赛格威运动和大多数机械手运动是基于特定的笛卡尔坐标定位目标的。

但是，一旦目标在toolmounted相机视线范围内，机械手依靠视觉输入指导切换。

第五部分A将会详细阐述视觉伺服系统方法，紧接着在第五部分B中会提供测试结果。

图.4.检测使用微软机器人SimpleVision方面的服务特征. 黑白边边框表示特征识别。

A.伺服系统
对于最终机械手的定位是使用视觉伺服系统方法获得标准图像进行定位的。

特征检测是根据Microsoft Robotics Studio的SimpleVision服务而测定的，获得能够识别颜色的斑点。

在这些试验中获得结果我们用绿色标记标出，如图4所示。

我们希望该机械手的方向是固定的，因此仅仅需要3个自由度（自由度）的位置应该被相关的视觉输入的影响。

这些自由度两个是由BLOB的定位控制，其中一个应在图像中心位置。

最后的自由度是由BLOB的大小决定的。

B.测试结果
如图3所示，移动机械手的任务是移动一个瓶子从图像的桌子上移动到右边相对的较远的箱子里。

机器人移动、机械手收缩和目标行为有关的数据关系可以在图5中看到。

图.5 机械手运行时各项的比例系数表
首先移动机械手收缩和移动指令被激活引起移动平台移向目标，同时手臂保持原始的配置装态。

经过约7秒之内达到目标并获得目标信号，因此机械手收缩动作被取消，机械手捕获动作被激活。

不久后，Segway动作也被取消，让机械手拿起无干扰的目标。

然而机械手运动会导致赛格威漂移，因此要过一会知道经过20s之后移动平台重新被激活，在这里移动平台又达到了预期目标的相对位置。

视觉伺服指挥机械手到如图3（b）所示的状态。

经过约30秒钟，瓶子应该被抓手拾起的和新的目标是给予，造成机械手收缩动作被重新激活而机械手捕获动作被取消。

同时移动平台移动动作也被激活，但当机械臂被收回时移动平台的移动动作会迅速被取消。

完成之后控制移动平台移动到所需位置放置，进而机械手被激活把目标放到箱子里。

VI.结论
本文已经介绍了如何使动态系统的方法应用于移动操作。

此文的主要结论包括两个层次，其中竞争态势是用于移动平台的整体协调和机械手运动以及避障和目标获取等动作。

该方法首先已被证实在模拟环境中，其次也通过实际工作的验证。

实验用的系统是Microsoft Robotics Studio1.5（MSRS）。

该系统最初是模拟和参数的调整，采用模拟器进行。

基于模拟器的物理参数理想的转向。

整个MSRS是一个执行工作有益环境的平台。

虽然控制是以20Hz被执行的，但由于Windows XP的非实性，动作间会有异常值出现。

——本文出自2009年IEEE国际机器人和自动化会议论文集
参考文献
[1] H. Seraji, A Unified Approach to Motion Control of Mobile Manipulators, The International Journal of Robotics Research, Vol. 17, No. 2, 1998, pp. 107-118.
[2] E. Papadopoulos, J. Poulakakis, Planning and Model-Based Control for Mobile Manipulators, Proceedings of the IROS’00 , 2000, pp. 1810-1815.
[3] Q. Huang, K. Tanie, S. Sugano, Coordinated Motion Planning for a Mobile Manipulator Considering Stability and Manipulation, Thee International Journal of Robotics Research, Vol. 19, No. 8, 2000, pp. 732-742.
[4] D.H. Shin, B.S. Hamenr, S. Singh, M. Hwangbo, Motion Planning for a Mobile Manipulator with Imprecise Loc omotion, Proceddings of the IROS’03, 2003, 847-853.
[5] G. Sch¨oner, M. Dose, A dynamical systems approach to task-level system integration used to plan and control autonomous vehicle motion, Robotics and Autonomous Systems, Vol. 10, 1992, pp. 253-267.
[6] G. Sch¨oner, M. Dose, C. Engels, Dynamics of behavior: theory and applications for autonomous robot architecture. Robotics and Autonomous Systems, Vol. 16, 1995, pp. 213-245.
[7] O. Khatib, Real-Time Obstacle Avoidance for Manipulators and Mobile Robots. The International Journal of Robotics Research, Vol. 5, No. 1, 1986, pp. 90-98.
[8] E. Bicho, G. Sch¨oner, The dynamic approach to autonomous robotics demonstrated on a low-level vehicle platform. Robotics and Autonomous Systems, Vol. 21, 1997, pp. 23-35. [9] E. Bicho, P. Mallet, G. Sch¨oner, Using Attractor Dynamics to Control Autonomous Vehicle Motion. Proceedings of the IECON’98, Vol. 2, 1998, pp. 1176-1181.
[10] E. Bicho, P. Mallet, G. Sch¨oner, Target Representation on an Autonomous Vehicle with Low-Level Sensors The International Journal of Robotics Research, Vol. 19, No. 5, 2000, pp. 424-447.
[11] H. Choset, K.M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L.E. Kavraki, S. Thrun, Principles of Robot Motion . The MIT Press, 2005.
[12] E.W. Large, H.I. Christensen, R. Bajcsy, Scaling the Dynamic Approach to Path Planning and Control: Competition among Behavioral Constraints. The International Journal of Robotics Research, Vol. 18, No. 1, pp. 37-58.
[13] P. Althaus, H.I. Christensen, F. Hoffmann, Using the Dynamical System Approach to Navigate in Realistic Real-World Environments. Proceedings of IROS’01 , Vol. 2, 2001, pp. 1023-1029.
[14] P. Althaus, Indoor Navigation for Mobile Robots: Control and Representations, Ph.d. Dissertation, Royal Institute of Technology (KTH), Stockholm, Sweden, 2003.
[15] S. Goldenstein, E. Large, D. Metaxas, Non-linear dynamical system approach to behavior modeling, The Visual Computer, Vol. 15, 1999, pp. 349-364.
[16] I. Iossifidic, G. Sch¨oner, Autonomous reaching and obstacle avoidance with the anthropomorphic arm of a robotics assistant using the attractor dynamics approach, Proceedings of ICRA’04, 2004, pp. 4295-4300.
[17] L.-P. Ellekilde, J.W. Perram, Tool Center Trajectory Planning for Industrial Robot Manipulators Using Dynamical Systems, The International Journal of Robotics Research, Vol. 24, No. 5, 2005, pp. 385-396.
[18] C. Santos, M. Ferreira, Ball Catching by a Puma Arm: a Nonlinear Dynamical Systems Approach, Proceedings of IROS’06, 2006, pp.916-921
[19] I. Iossifidic, G. Sch¨oner, Dynamical Systems Approach for the Autonomous Avoidance of Obstacles and Joint-limits for an Redundant Robot Arm. Proceedings of the IROS’06, 2006, pp. 580-585.
[20] P. Jensfelt, H.I. Christensen, Pose tracking using laser scanning and minimalistic environment models, IEEE Transactions on Robotisc and Automation, Vol. 17, No. 2, 2001, pp. 138-147.
[21] J. Forsberg, P. A° hman, . Wernersson, The Hough transform inside the feedback loop of a mobile robot, Proceedings of ICRA, Vol 1, 1993, pp. 791-798.
[22] K.O. Arras, R.Y. Siegwart, Feature Extraction and scene interpredation for map-based nagivation and map building, Proceedings of SPIE, Mobile Robotics XII, Vol. 3210, 1997, pp. 42-53.
[23] L.-P. Ellekilde, P. Favrholt, M. Paulin, H.G. Petersen, Robust control for high-speed visual servoing applications, International Journal of Advanced Robotic Systems , Vol. 4, No. 3, 2007, pp. 272-292.
Control of Mobile Manipulator using the Dynamical Systems Approach
Lars-Peter Ellekilde
Abstract—The combination of a mobile platform and a manipulator, known as a mobile manipulator, provides a highly flexible system, which can be used in a wide range of applications, especially within the field of service robotics. One of the challenges with mobile manipulators is the construction of control systems, enabling the robot to operate safely in potentially dynamic environments. In this paper we will present work in which a mobile manipulator is controlled using the dynamical systems approach. The method presented is a two level approach in which competitive dynamics are used both for the overall coordination of the mobile platform and the manipulator as well as the lower level fusion of obstacle avoidance and target acquisition behaviors.
I. INTRODUCTION
The majority of robotic research has in the last decades focused on either mobile platforms or manipulators, and there have been many impressive results within both areas. Today one of the new challenges is to combine the two areas, into systems, which are both highly mobile and have the ability to manipulate the environment. Especially within service robotics there will be an increased need for such systems. The demography of most western countries causes the number of old people in need of care to increase, while there will be less working to actually support them. This requires an increased automation of the service sector, for which robots able to operate safely in indoor and dynamic environments are essential.
Fig. 1. Platform consisting of a Segway RMP200 and a Kuka Light Weight Robot.
The platform used in this work is shown in Figure 1, and consist of a Segway RMP200 with a Kuka Light Weight Robot. The result is a platform that has a relative small footprint and is highly maneuverable, making it well suited for moving around in an indoor environment. The Kuka Light Weight Robot has a fairly long reach and high payload compared to its own weight, making it ideal for mobile manipulation.
When controlling a mobile manipulator, there is a choice of whether to consider the system as one or two entities. In [1] and [2] they derive Jacobians for both the mobile platform and the manipulator and combine them into a single control system. The research reported in [3] and [4], on the other hand, considers them as separate entities when planning, but do include constraints, such as reachability and stability, between the two.
The control system we propose is based on the dynamical systems approach [5], [6]. It is divided into two levels, where we at the lower level consider the mobile platform and the manipulator as two separate entities, which are then combined in a safe manner at the upper level. The main reesarch objective in this paper is to demonstrate how the dynamical systems approach can be applied to a mobile manipulator and used to coordinate behaviours at various levels of control.
The remaining of this paper is organized as follows. The overall architecture is described in Section II, followed by the control of the mobile platform and the manipulator in Sections III and IV. In Section V we will show some experiments before concluding the paper in Section VI. However, first a summary of work related to the dynamical systems approach will be provided in Section I-A.
A. Related Work
The dynamical systems approach [5], [6] provides a framework for controlling a robot through a set of behaviors, such as obstacle avoidance and target acquisition. Each behavioris generated through a set of attractors and repellors of a nonlinear dynamical system. These are combined through simple addition of the vector fields to provide the overall behavior of the system. The dynamical systems approach relates to the more widely used potential field method [7], but has certain advantages. Where the behavior in the potential field method is the result of following gradients of the field, the behavior variables, such as heading direction and velocity, can be controlled directly using the dynamical systems approach.
The relative low computational cost associated with the approach, makes it suitable for online control in dynamic environments, and allows it to be implemented even on fairly
low-level platforms with limited computational capabilities [8]. The robustness to noisy sensors is shown in [9] and [10] where a combination of infrared sensors and microphones is used for obstacle avoidance and target acquisition. Despite being able to solve various tasks it is only a local method, for task and mission-level planning other methods (see e.g. [11]) should be applied.
A drawback of the approach in [5], [6] is the potential creation of spurious attractors when multiple behaviors are combined. To overcome this problem [12] introduces a weighting of the behaviors based on competitive dynamics. The influence of each behavior is controlled using an associated competitive advantage, which together with competitive interactions defined between the behaviors, controls the weights. This approach generalizes to an arbitrary number, n, behaviors, but with a O(n2) worst-case complexity, if competitive interactions between all behaviors are needed.
Real-world indoor experiments using this competitive dynamics approach can be found in [13], [14]. In [13] only the heading direction of the vehicle is used, whereas in [14] both heading direction and velocity are controlled. [15] provides a brief discussion of strategies for the velocity behavior.
The dynamical systems approach has not only been used for planar mobile robots, but also for controlling the tool motion of a manipulator [16]. More complex dynamical systems using the Hopf Oscillator for generating limit cycles can also be used. [17] shows how limit cycles with different shapes can be constructed and used for both obstacle avoidance and trajectory generation. [18] uses the Hopf Oscillator to generate a timed trajectory, enabling a manipulator to catch a ball rolling down a table. The dynamical systems approach can not only be used for controlling the tool, but also to control the redundancy of a 7 degrees of freedom manipulator as demonstrated in [19].
II. OVERALL ARCHITECTURE
The overall architecture of our system is illustrated in Figure 2. To control the mobile platform, in this case a Segway, two low level behaviors are use: One for target acquisition and one for obstacle avoidance. Using competitive dynamics these are fused together to provide the Mobile behavior, which specifies the desired motion of the mobile platform. Similarly we have target acquisition and obstacle avoidance behaviors for the manipulator fused together based on competitive dynamics, to give the Manipulator Acquisition behavior. When the target is not within reach, the manipulator should retract to a safe configuration, which is the purpose of the
Manipulator Retract behavior. The last fusion combines the controls in a safe manner, such that the target acquisition and retract behaviors do not disturb one another and the mobile platform does not start moving towards a new target before the manipulator has been retracted.
Fig. 2. Overall architecture of the control system Using weights w mobile ，manip acquisition w and manip retract w to represent the influence of the Mobile,
Manipulator Acquisition and Manipulator Retract behaviors, the control signals mobile u and manip q
for the mobile platform and the manipulator are given by ()left
right
u mobile mobile u u w = （1） manip manip manip manip manip acquisition acquisition retract retract q
q q w w =+ (2) Where （left u right u ） are control inputs to the left and right wheels of the platform as
described in Section III, manip
acquisition q
and manip retract q are the manipulator joint velocities as described
in Section IV.
A. Competitive Dynamics The competitive dynamics approach used is based on [12], but with the additional parameter b T used to control the transition rate as in [14]. The dynamical system used is thus given by
''3'2(),b b b b b b b b b
T w a w w rb bw w noise ≠=--+∑ （3）
In which b a is the competitive advantage of behavior b and r 'b ,b is the competitive interaction of behavior 'b upon b.
1) Mobile: The competitive advantages of the mobile platform should strengthen the behavior when far away from the target and reduce it when the target is reached. This is achieved through
t a n h (()m o b i l e
m o b i l e
m o b i l e a
t a r
t h r e s h o l d a k d d =- （4）
In which mobile
a k determines how rapidly the advantage should change, tar d is the distance
to the target and mobile threshold d specifies a minimum distance to the target required before the
mobile platform should move.
The mobile behavior has no ability to interact and suppress other behaviors, thus its competitive interactions are set to 0.
2) Manipulator Acquisition: This behavior should be strengthened when the mobile platform gets close to its target. The competitive advantage will thus be defined as
tanh(())manip manip manip
acuisition a tar threshold a k d d =--
（5） The activation distance manip threshold d must be greater than mobile
threshold d to make sure the behavior
is activated. This behavior has no direct interaction with the others, thus its interactions are set to 0.
3) Manipulator Retract: The retract behavior should be activated opposite the goal behavior, hence
manip manip
retract acqisition
a a =- tanh(())manip mobile
a tar threshold k d d =- （6）
Except for a very small transition time this prevents the manipulators acquisition and retract behaviors from being active at the same time, thus we can set
,0retract acquisition
r
=. For the
interaction between the retract and the mobile behaviors we wish retract to deactivate mobile when the manipulator is far away from its home configuration. The interaction is therefore defined as
In which cur q and hom e q are the manipulators current and home configurations, ∈q specifies a proximity distance around hom e q and retract r
k
specifies how quickly the
interaction changes.
III. CONTROL OF THE MOBILE PLATFORM
The control of the mobile platform is constructed very similar to what is presented in [14], but with a few differences. First of all only the target acquisition and obstacle avoidance behaviors are used. The corridor following and wall avoidance are not included, but would be straight forward extensions. The second area in which this work differs is in how the density of obstacles is calculated. Details of this will be explained in section III-D.
For the control to actually be able to navigate through the environment, it is necessary with a method for localization. The approach we have used is based on the method described in [20], which combines odometry and laser range measurements matched against a map of dominating lines in the environment.
The control of the platform is encoded using the orientation, φ and the velocity, ν, which
results in a system with control inputs {}
,mobile f v φ=
; The values of mobile
f
are made up of
two parts,
mobile tar
f
and
mobile obs
f
, which are combined as
mobile
mobile mobile mobile
mobile
tar
obs
tar
obs
f
f
f
w w =+ （8）
Where the weights
mobile tar
w
and mobile
obs w are controlled using Eq. (3) with the competitive
advantage and interactions described in section III-C.
As control input we need expressions for the left and right wheels of the mobile platform, denoted
left
u
and right u , respectively. To obtain these v
is integrated to get v, which together with the desired rotational velocity φ , the wheel diameter wheel
d
and the distance
between the wheels
wheelbase
d
can be used to calculate the control inputs as
(,)2
right right wheel v d u u φπ=
+ （10）
Where ∆ is the needed difference in wheel speed given by
w h e e l b a s e w h e e l
d d φ
π∆= （12）
A. Target Dynamics
The basic dynamics of this target behavior is
,,()sin()mobile mobile tar tar tar f φφ
φλψφ=- (13)
,,max ()(min(,))mobile v
mobile v mobile tar tar tar tar
v k d v v f
λ=- (14)
In which ,mobile tar φ
λ and ,mobile v tar λ are the strengths of the attractors and tar ψ is the direction to the target. The constant mobile tar k gives the relation between the distance to the target and the
desired velocity. Finally max v is the maximal velocity allowed for the mobile platform 。

B. Obstacle Dynamics Given a distance ,obs i
d and a direction i ψ to th
e i ’th obstacle, the dynamics o
f the obstacle
avoidance are
Where max,,min max(,)
i obs obs i v k d v =
The dynamics of φconsists of 3 elements: (i) The relative direction to the obstacle
()i φψ-，(ii) a scale ,mobile
obs obs i
c d e -in which
mobile
obs
c -determines the decay depending of the distance,
,obs i
d ，
and (iii) a scale, 2
()2i i e φψσ--, based on the direction to the obstacle and with
,1arcsin(
)1s
i obs i
D d σ+=+ensuring the generation of an attractor between two obstacles if the robot
can pass through while ensuring the safety distance Ds. See [14] for more details.
For
,,mobile v obs i
f
the expression adjusts the velocity towards ,obs obs i k d , but ensures that a
minimum velocity of min v is kept. To obtain the value of
mobile obs
f
we sum over all obstacles
,,,,,,()()mobile mobile mobile obs i obs
mobile v
mobile v obs
i
obs
obs i
f f f
f
f
φφ==∑
（17）
C. Competitive Dynamics
The weights for the competitive dynamics are controlled by equation (3) as explained above. Below are the competitive advantages and interactions for the two behaviors.
1) Target: The competitive advantage is set to tar 0.5mobile
α=whenever a target is present, otherwise tar 0.5mobile α=-.
The target behavior has the ability to interact with and suppress the obstacle avoidance behavior, when the ratio between the distance to the target and the closest object is sufficient to ensure the movement towards the target will be collision free. This is modeled as
In which ,min obs d is the distance to the closest obstacle,
mobile
gain r is a gain constant giving how
quickly the behavior should interact and
lim 1mobile
it
r ≥expresses the ratio between the distances to obstacle and target for which we will start to suppress obstacle avoidance. 2) Obstacle: The competitive advantage of the obstacle behavior is given by
In which mobile ρis the obstacle density as defined in Section III-D. The interaction is defined as
The first part, 01
(1tanh())2mobile mobile
ρρρσ-+suppresses the target behavior when the obstacle
density exceeds the threshold 0mobile ρ. The last part,,1mobile
tar obs γ-,makes sure this only happens when the obstacle avoidance is not being suppressed due to ,mobile tar obs
γ.
D. Calculation of Obstacle Density
Given a set of distances, ,obs i
d
,
between the mobile platform and obstacles the density, ρ,
is calculated as
This density function differs from [14] in which ,obs i
d i e
ρ-=∑is used. The main problem
with this formulation is that we cannot distinguish between many objects relative far away and a single object closed by. For example having 5 objects 2 meters away will give the same density as a single object 40 centimeters away. With the exponential function a single object in the scene can never cause ρto exceed 1. The threshold for switching to the obstacle avoidance behavior will thus have to be less than 1, but given a scene with multiple obstacles the threshold of 1 will often be too low.
Furthermore it is found that using ,1
obs i d instead of ,obs i e -made tuning the parameters easier as we could now think of the density as the inverse of the distance. It also caused the density to grow very rapidly when getting close to an obstacle, thereby quickly forcing the behaviors to change.
IV. CONTROL OF MANIPULATOR
We will start by dividing the problem into two parts:
1) Determining the motion of the tool from the current position to the target while avoiding obstacles.
2) Inverse kinematics calculating joint velocities needed for the tool motion.
The second part is a well understood problem, which in this work is solved using the inverse kinematics strategy presented in [23]. This method incorporates both kinematics and dynamics limitations of the robot, such as joint position, velocity and acceleration limits. Furthermore
this approach, based on quadratic optimization, has shown to be very robust with respect to singularities.
The motion of the tool is controlled using the manipulators Target and Obstacle behaviors,
to which the weights manip tar ωand manip
obs ωare associated. As input the inverse kinematics needs a 6D
velocity screw
{},,,,,x y z x
y z νωωω= thus the behaviors must find a
change {},,,,,manip
x y z f
x y z ωωω= which can be integrated to give a desired tool velocity, des ν,as
.,(())manip manip manip manip manip
des cur tar tar obs obs dir obs dist dt f f f ννωω=+++ （22）
Where manip tar f ，.manip obs dir f and ,manip
obs dist f are the contributions from the target and obstacle avoidance
behaviors. A. Target Behavior The
inputs
to
the
target
behavior
are
the
current
and
desired
tool
transformations cur T and des T From these we can compute a desired 6D velocity-screw tar ν.To avoid
requiring
unrealistic
fast
motions
tar
νis scaled such that
{}max ,,tar x y z v ≤ and {}
max ,,x
y
z
tar
w w w ω≤where max v and max ωhere represent the maximal
allowed linear and rotational velocities of the tool. Calculating
()manip manip tar tar cur tar f λνν=-- （23）
We obtain a desired change to the current velocity. B. Obstacle-Behavior
As input the obstacle avoidance behavior takes the current Cartesian velocity,{},,v x y z = and a set of closest obstacles as vectors,3i n R ∈, giving direction and distance between tool and obstacle I . We now wish to compute a change to the Cartesian velocity based on the direction
and distance to obstacles, denoted .manip obs dir f and ,manip obs dist f respectively.
1) Dynamics for Direction: From the current velocity of the tool, v, and the vector i n we compute the angle i θ, between the two as。