【正文】 B. Obstacle DynamicsGiven a distance and a direction to the i’th obstacle, the dynamics of the obstacle avoidance are (15) (16)Where The dynamics of consists of 3 elements: (i) The relative direction to the obstacle ，(ii) a scale in which determines the decay depending of the distance, ，and (iii) a scale, , based on the direction to the obstacle and with 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.Forthe expression adjusts the velocity towards , but ensures that a minimum velocity ofis kept.To obtain the value of we sum over all obstacles （17）C. Competitive DynamicsThe weights for the petitive dynamics are controlled by equation (3) as explained above. Below are the petitive advantages and interactions for the two behaviors.1) Target: The petitive advantage is set towhenever a target is present, otherwise.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 （18）In whichis the distance to the closest obstacle, is a gain constant giving how quickly the behavior should interact andexpresses the ratio between the distances to obstacle and target for which we will start to suppress obstacle avoidance.2) Obstacle: The petitive advantage of the obstacle behavior is given by （19）In whichis the obstacle density as defined in Section IIID.The interaction is defined as （20）The first part, suppresses the target behavior when the obstacle density exceeds the threshold. The last part,makes sure this only happens when the obstacle avoidance is not being suppressed due to.D. Calculation of Obstacle DensityGiven a set of distances, , between the mobile platform and obstacles the density, , is calculated as （21）This density function differs from [14] in whichis 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 causeto 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 usinginstead of 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 MANIPULATORWe 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 weightsandare associated. As input the inverse kinematics needs a 6D velocity screwthus the behaviors must find a changewhich can be integrated to give a desired tool velocity, ,as （22）Where ，andare the contributions from the target and obstacle avoidance behaviors.A. Target BehaviorThe inputs to the target behavior are the current and desired tool transformationsandFrom these we can pute a desired 6D velocityscrew .To avoid requiring unrealistic fast motionsis scaled such that andwhereandhere represent the maximal allowed linear and rotational velocities of the tool.Calculating （23）We obtain a desired change to the current velocity.B. ObstacleBehaviorAs input the obstacle avoidance behavior takes the current Cartesian velocity, and a set of closest obstacles as vectors, giving direction and distance between tool and obstacle I . We now wish to pute a change to the Cartesian velocity based on the direction and distance to obstacles, denotedandrespectively.1) Dynamics for Direction: From the current velocity of the tool, v, and the vectorwe pute the angle, between the two as （24）The size of the change in direction of the tool is then calculated as （25）In which is the strength of the repellor, control the decay based the distance andcontrols the relation with the angle to the obstacle. is then used to calculate a desired change in the direction of the tool as （26）Summing up the contributions from all obstacles we can calculate the change in motion of the tool based on direction to obstacles as （27）2) Dynamics for Velocity: The dynamics of the velocity are controlled similar to Eq. (16). The contribution of obstacle I . is （28）WithSumming up over all obstacles the total contribution bees （29）C. Competitive Dynamics1) Target Behavior: As for the mobile platform the petitive advantage of the target behavior is set to when a target is present and － otherwise. The petitive interaction of the target upon the obstac