Model predictive disturbance rejection during cooperative mobile robot assembly tasks

TECHNICAL PAPERS

Model predictive disturbance rejection during cooperative mobile robot assembly tasks

V. A. Sujan^I; M. A. Meggiolaro^II

^ICummins Engine Company, Air Handling and Combustion Control Division, 500 Jackson Street, 47201 Columbus, IN. USA, [e-mail]

^IIPUC-Rio, Departamento de Engenharia Mecânica, Rua Marquês de São Vicente, 225 - Gávea, 22453-900 Rio de Janeiro, RJ. Brazil, [e-mail]

ABSTRACT

This paper addresses the problem of disturbance compensation for the successful assembly of structures by mobile field robots. A control architecture, consisting of a linear PID joint controller with model predictive feed-forward compensation for mobile base motions and interactive-force disturbance rejection is discussed. Object insertion is achieved by predicting environment-object contact states and motions are planned in the direction of least resistance. Issues addressed include dynamic modeling of multiple cooperative robots, control architecture design and stability analysis, and environment-object contact state prediction. Simulation results show the effectiveness of the control architecture.

Keywords: Cooperative robots, model predictive control, robot assembly

Introduction

Future mobile field robotic systems, such as planetary and terrestrial mission robots, will be required to perform complex tasks (Huntsberger, 1997; Shaffer and Stentz, 1992). Planetary robots will be used to collect rock samples, to build infrastructures, and explore complex terrains. Tasks for terrestrial field robots may include explosive ordinance removal, de-mining and handling hazardous waste, and environment restoration (Baumgartner et al., 1998; Huntsberger, 1997; Osborn, 1989; Shaffer and Stentz, 1992). Those will require the handling of relatively large objects, such as deploying of solar panels and sensor arrays, anchoring of deployed structures, movement of rocks, and clearing of terrain. An important goal of robotics research is to develop mobile robot teams that can work cooperatively in unstructured field environments, such as shown conceptually in Fig. 1 (Baumgartner et al., 1998; Huntsberger, 1997).

Each field robot may be equipped with a manipulator arm and sensors such as inclinometers, accelerometers, vision systems, and force/torque sensors. The control of such systems typically requires models of the environment and task. This paper addresses the problem of assembly (insertion) tasks performed by cooperative mobile robots in field environments.

Substantial previous research has been devoted to control and planning of cooperative robots and manipulators (Alur, 2000; Donald et al., 1997; Gerkey and Mataric, 2000; Khatib, 1995; Marapane et al., 1996; Mataric, 1998; Parker, 1995; Veloso and Stone, 1999). However, these results are largely inapplicable to mobile robots in unstructured field environments. The methods developed to date generally rely on assumptions that include: flat and hard terrain; accurate knowledge of the environment; little or no task uncertainty; and sufficient sensing capability. Additionally, researchers have developed several approaches to the single robot object insertion problem including motion in direction of least resistance, perturbation methods, petri-nets and event-based approaches, and remote compliance center modeling for contact state identification (Giraud and Sidobre, 1992; Hirai and Iwata, 1992; Kang et al., 1998; Kitagaki et al., 1993; Kittipongpattana and Laowattana, 1998; Lee and Asada, 1999; McCarragher and Asada, 1993; Shimokura and Muto, 1996; Xiao and Liu, 1998).

Little work has been done in addressing the problem of autonomous field robots cooperatively assembling structures. In unstructured field environments the robot(s) needs to construct environment and task models from available sensory information. A number of problems can make this difficult. These include the uncertainty of the task in the environment, location and orientation errors in the individual robots, sensing occlusions, and external disturbances. Previously reported work has addressed problems due to environment and task sensing uncertainty and limitations (Sujan, 2002a; Sujan, 2002b; Sujan and Dubowsky, 2002).

This paper addresses the problem of disturbance compensation for the successful assembly of structures by mobile field robots. Disturbances may arise due to inter-robot and robot-environment interaction forces. Such disturbances can significantly degrade the performance of a manipulator. They may cause the manipulator to leave its prescribed path, saturate its actuators, and induce high stresses (both internal and external e.g. at the endpoint). Additionally, a critical element in task execution may be the stresses exerted by the system(s) on the structure elements and the environment. Therefore, these forces will have to be monitored and kept below a damage threshold during the entire task.

The control system proposed for manipulator control is a linear PID joint controller with model predictive feed-forward compensation for mobile base motions and interactive-force disturbance rejection. Object insertion is achieved by predicting environment-object contact states (from force/torque sensor readings) and motions are planned in the direction of least resistance. To model an appropriate controller for such a system, a dynamic model of this environment must be developed. Such a model will demonstrate the relationship between manipulator joint angles/positions, vehicle base motions and external end-point interactive forces.

Simulation results show the effectiveness of the control architecture. Without dynamic disturbance compensation in the control loop, the system is seen to fail. However, with dynamic disturbance compensation in the control loop, the robots are now able to predict the errors that would be introduced into the system due to external interaction forces, and succeed in task execution.

Nomenclature

b_i = 3x1 unit vector along joint axis i, dimensionless

d = measured vehicle disturbance vector [x_V, y_V, q_V]^T

dx_i, dy_i, dz_i = distance between the vehicle center of gravity and tire i in the x, y or z direction, m

F = measured manipulator endpoint external force, N

F_k = externally applied force [F_xk, F_yk, F_zk]^T, N

F_V = vehicle inertial feed-forward term (analytical)

f_V = vehicle damping feed-forward term (analytical)

f_fr = measured contact friction force, N

f_n = measured normal contact force, N

g = gravitational acceleration vector [g_x, g_y, g_z]^T, m/s²

G = gravitational term, N or N·m

H = augmented inertia tensor of the manipulator-vehicle system

H_ij = element (i, j) of the manipulator-vehicle system inertia matrix H

H_M = augmented inertia tensor of the manipulator solely

h = augmented matrix of Coriolis and centrifugal coefficients of the manipulator-vehicle system

h_ijk= Christoffel's three-index coefficient

h_M = augmented matrix of Coriolis and centrifugal coefficients of the manipulator solely

I = moment of inertia, N·m²

J = manipulator Jacobian matrix

J^(j) = column j of the manipulator Jacobian matrix

K_D = derivative control gain

K_I = integral control gain

K_P = proportional control gain

K^x,y,z = linear stiffness in the x, y or z direction, kN/m

K^q = angular stiffness, kN·m

l = link length, m

m = link mass, kg

M_j = externally applied moment [M_xj, M_yj, M_zj]^T, N·m

p = position vector [x_p, y_p, 0]^T of point p in Fig. 4, m

Q_i= generalized force on joint i, N

q_i = generalized coordinate associated to joint i, m or deg

r_i = position vector [x_i, y_i, z_i]^T of particle i, m

r_i,cj = vector of centroid of link j from i^th frame, m

S_k = position vector [x_Sk, y_Sk, z_Sk]^T where external force F_k is applied, m

T = total torque [T_x, T_y, T_z]^T acting on point p in Fig. 4, N·m

V = Lyapunov function

v_c = linear velocity of link centroid, m/s

Greek Symbols

q = angular displacement, rad

Q = manipulator parameter vector [q₁, q₂, q₃]^T

t = joint torque vector, N·m

w_c = link angular velocity, rad/s

Subscripts

A relative to angular velocities

D relative to derivative control

d desired value

F relative to force control

I relative to integral control

i relative to manipulator joint i

L relative to linear velocities

P relative to proportional control

v relative to vehicle center of gravity

x relative to the x direction

y relative to the y direction

z relative to the z direction

zmp relative to the zero moment point

~ estimation error

Control Algorithm Development

A feedback controller is characterized by not reacting to a disturbance before a control error has already occurred. But in many cases it is possible to measure the value of a disturbance before it gives rise to a control error. In model predictive control, disturbance rejection is accomplished by estimating the equivalent disturbance of a system based on its dynamic model and the sensed disturbances. This is also known as feed-forward control, which takes control action in the manipulator to eliminate the impact of uncontrolled vehicle disturbances before any errors in the manipulator can be detected, see Fig. 2. Disturbance measurements are fed into a dynamic system model to account for the errors caused by them. The resulting dynamic disturbance commands are fed-forward and added to the basic controller commands to give the system control input. The basic joint-level controller considered here is a PID controller. To function effectively, such a system is dependent on an accurate dynamic model and low noise sensors. Degradation in the accuracy of the models and the disturbance measurements result in corresponding degradation of the controller.

Using a Lagrangian formulation, the dynamic models of the systems and task (represented in Fig. 1) are developed. These models account for robot base motion, compliance, and multi-robot interaction forces (see Fig. 3 for a planar representation). This method can be readily extended to model the closed chain dynamics of multiple cooperating robots. The primary steps involved in this process are now described.

Step 1: Reduction of Suspension Compliance System

A 6 DOF linear stiffness and damping system, located at the vehicle base center-of-gravity, represents its multi-element suspension system. For small base motions, this model of the suspension is sufficient to model the vehicle dynamics accurately. If the contributions to the suspension are known to occur only from the vehicle tires and a passive compliance element, then the combined 6 DOF stiffness at the center of gravity is given by:

where are unit normal direction vectors for the x, y, z axes respectively, as shown in Fig. 3(a). Similar expressions may be derived for the 6 DOF damping terms.

Step 2: Robot Model Lagrangian Dynamics

In general for a multi-DOF serial manipulator, the spatial equation of motion for the i^th link is given by:

where H_ij is the element (i, j) of the robot inertia matrix H:

For example, in the planar system shown in Fig. 3, the generalized variables q are given by:

Note that the suspension effects are embedded into the measured vehicle variables x_v, y_v, q_V. Considering small perturbations Dq_i about an equilibrium state and substituting into the non-linear dynamic equations of motion gives:

Alternatively, these non-linear equations of motion may be simplified using Computed Torque Techniques. To compensate for the gravitational, Coriolis and centrifugal effects, the control input can be easily calculated in real-time from the generalized forces Q_i given by:

where Q^c is the output of a simple PID control law. To compensate for the (uncontrolled) vehicle disturbances d = [x_V, y_v, q_V]^T, the associated vehicle inertial and damping feed-forward terms F_V and f_V must be computed. These terms are readily obtained from the last 3 rows of the manipulator-vehicle system augmented matrices H and h, computed from Eqs. (3-4) using the generalized variables q from Eq. (7):

where the empty-bracketed terms are expressions not necessary for the development below, and H_M and h_M are the augmented matrices of the manipulator without considering the vehicle-suspension generalized variables.

Converting Eq. (9) into state space form:

where, for the planar system shown in Fig. 3, the measured manipulator system state x, the computed torque/force input u, and the matrices A, B, C, D are given by:

Step 3: Stability of Controller for Position Control

Using this state space formulation of the dynamic system model, the effects of disturbances may be predicted and compensated for. However, it is important to confirm that such a controller (see Fig. 2) will remain stable. For the planar 3 DOF arm system show in Fig. 3, the manipulator dynamics are now given by:

where Q = [q₁, q₂, q₃]^T is the manipulator parameter vector. For a PD joint controller coupled with gravity compensation and dynamic disturbance rejection feed-forward terms, we have the control input torque:

Note that the only gains in the above equation are K_P and K_D, which must be calibrated; all other terms are deterministic equations derived from the manipulator and vehicle kinematics and sensor readings. The stability of this controller may be verified by considering the following Lyapunov function candidate:

Since K_p and H_M are symmetric and positive definite, V > 0 (except when Q = Q_d). Differentiating V with respect to time gives:

For the derivative of the Lyapunov function to be equal to zero, it is necessary that

= 0, in which case the acceleration of the system is

Therefore, the acceleration is always different than zero, except when = 0, implying in Q = Q_d. It can be concluded then that the Lyapunov stability criteria apply and the controller is stable.

Step 4: Dynamic Tip-Over Stability

Once a dynamic model of the robotic system(s) has been set up, the controller needs to maintain tip-over stability. This is achieved by limiting the motion of the dynamic zero-moment point (dynamic center of gravity) to lie within the vehicle footprint, see Fig. 4 (Takanishi et al., 1989).

Using d'Alambert's principle, the forces/torques on each individual mass particle are evaluated. The X and Y components of the zero moment point are given by (Fig. 4):

The controller is able to determine the admissible robot motion states, by confining the position of the system zero moment point to within the vehicle footprint.

Cooperative Task Execution—Robotic Assembly

Using the above dynamic models and a model predictive control architecture, multiple robots can cooperatively execute an assembly task-cooperative insertion. Here a planar model of this problem is developed. The insertion problem is addressed by identifying the contact point (based on measured forces/torques) and formulating a motion plan to move in the direction of least resistance. Figure 5 shows an example of the six possible environment interaction states of a rectangular object at a similarly shaped insertion site.

For each of these contact states, a relation between interaction forces, the contact point(s), and the measured forces/torques (F_x, F_y and M) can be developed (see Figs. 6 through 10).

The measured forces/torques are evaluated from force/torque sensor readings of all cooperating robots. Note that although multiple contact points cannot be uniquely identified, yet the motion plan is valid. Figure 11 shows the error in location of a single contact point as a function of sensor noise.

Figure 12 shows the combined control architecture for a hybrid master-slave cooperative manipulation of an object by two robots. For insertion to a target site, after a rough approximation of the target site location (through initial visual identification that may consequently get occluded), the controller uses an estimation of contact state to "feel" its way to the final target site. This is known as surrogate sensing. Once again the motion plan is in the direction of least resistance, with the assumption that the target is located in such a direction. To overcome the limitations due to this assumption, more sophisticated sensing algorithms may need to be employed, which have been previously described (Sujan, 2002a; Sujan, 2002b; Sujan and Dubowsky, 2002).

Simulation Results

Two types of simulation tests were performed to verify the validity of the control methodologies developed in this paper. In both instances a planar model for the robotic system was used, consisting of a 3 DOF manipulator mounted on a compliant vehicle (see Fig. 3). The vehicle mass is assumed to be 10 kg, with moment of inertia 1.0 kg·m², and the base stiffness and damping terms in all directions are 200 kN/m and 1.0 kN/(m/s), respectively. The manipulator parameters and control gains are shown in Table 1.

The first test involved position control of the manipulator arm on a single mobile robotic system. Artificial base oscillations with roll amplitude of 11° and pitch amplitude of 2° have been introduced considering the periods of 17, 10, 2, 1 and 0.01 seconds (frequencies of 0.06, 0.1, 0.5, 1 and 100 Hz). An end-point payload mass of 8 kg under a 9.81 m/s² gravity field is assumed. Figure 13 shows the data obtained for the manipulator position control under 0.5 and 0.06 Hz oscillations. Table 2 shows the frequency response of the system. Although all simulations include gravity compensation in the control loop, it is seen that when dynamic disturbance compensation is eliminated from the loop, the manipulator is unable to maintain a constant position due to the dynamic forces from the base oscillations. By introducing the dynamic disturbance compensation into the control loop, the robot is now able to predict (from the manipulator dynamic model) the errors that would be introduced into the system due to the base motions. This feed-forward term, in addition to the PID control terms, allows the manipulator to compensate for the dynamic disturbance.

The second test involves two identical mobile robots (Table 1) cooperating using a model predictive master-slave hybrid position-force control architecture with surrogate sensing. In this task, the robots must insert a segment into a truss stage, which is slightly slanted by an angle of 6 degrees (which results in an insertion angle of 84 degrees). The manipulator bases are 2.5 m apart (in the horizontal direction), and the segment mass and inertia are respectively 2 kg and 0.21 kg·m². Figure 14 shows 3D renderings of the simulation output at several stages during this insertion process.

Figure 15 shows simulation results of the endpoint positions and forces felt by the cooperating robots during the approach phase of the task. Although no external oscillation is forced on the base, there is significant base motion due to the interacting forces combined with the vehicle compliance. However, with predictive compensation, these effects can be negated. Note that the system is attempting to distribute the 20N (2 kg) truss segment load evenly, since the steady-state vertical (Y) force is 10N for both robots. This is a consequence of the hybrid position (master) / force (slave) controller, where the slave robot is asked to support half the load. Finally, Fig. 16 shows the endpoint position of the master arm during the truss stage insertion. Notice that the truss segment is successfully inserted into the slanted stage at a vertical (Y) displacement of 1.0 m, where the segment orientation is 84 degrees (see Fig. 16).

Conclusions

This paper has addressed the problem of disturbance compensation for the successful assembly of structures by mobile field robots. A control architecture, consisting of a linear PID joint controller with model predictive feed-forward compensation for mobile base motions and interactive-force disturbance rejection has been discussed. Object insertion was achieved by predicting environment-object contact states using onboard force/torque sensors and inter-robot communication. Motions are planned in the direction of least resistance. Issues presented include dynamic modeling of multiple cooperative robots, control architecture design and stability analysis, and environment-object contact state prediction. Simulation results show the effectiveness of the control architecture.

Paper accepted July, 2004. Technical Editor: José Roberto de França Arruda.

• Alur, R., 2000, "A Framework and Architecture for Multirobot Coordination", Proceedings of the ISER 2000, Waikiki, Hawaii, USA.
• Baumgartner, E.T., Schenker, P.S., Leger, C. and Huntsberger, T.L., 1998, "Sensor-Fused Navigation and Manipulation from a Planetary Rover", Proceedings of the SPIE Symposium on Sensor Fusion and Decentralized Control in Robotic Systems, Vol. 3523, Boston, MA, USA.
• Donald, B., Jennings, J. and Rus, D., 1997, "Information Invariants for Cooperating Autonomous Mobile Robots", International Journal of Robotics Research, Vol.16, No. 5, pp. 673-702.
• Gerkey, B. and Mataric, M.J., 2000, "Principled Communication for Dynamic Multi-Robot Task Allocation", Proceeding of the ISER 2000, Waikiki, Hawaii, USA.
• Giraud, A. and Sidobre, D., 1992, "A Heuristic Motion Planner using Contact for Assembly", Proceedings of the 1992 IEEE International Conference on Robotics and Automation, Vol.3, Nice, France, pp. 2165-2170.
• Hirai, S. and Iwata, K., 1992, "Recognition of Contact State Based on Geometric Model", Proceedings of the 1992 IEEE International Conference on Robotics and Automation, Vol.2, Nice, France, pp. 1507-1512.
• Huntsberger, T.L., 1997, "Autonomous Multi-Rover System for Complex Planetary Retrieval Operations", Proceedings of the SPIE Symposium on Sensor Fusion and Decentralized Control in Autonomous Robotic Agents, Pittsburgh, PA, USA, pp. 220-229.
• Kang, S., Kim, M., Lee, W.L. and Lee, K.-I., 1998, "A Target Approachable Force-Guided Control for Complex Assembly", Proceedings of the 1998 IEEE International Conference on Robotics and Automation, Leuven, Belgium.
• Khatib, O., 1995, "Force Strategies for Cooperative Tasks in Multiple Mobile Manipulation Systems", Proceedings of the ISER 1995, Munich, Germany.
• Kitagaki, K., Ogasawara, T. and Suehiro, T., 1993, "Methods to Detect Contact State by Force Sensing in an Edge Mating Task", Proceedings of the 1993 IEEE International Conference on Robotics and Automation, Vol.2, Atlanta, GA, USA, pp. 701-706.
• Kittipongpattana, T. and Laowattana, D., 1998, "Robotic Insertion of Dual Pegs based on Force Feedback Signals", IEEE Proceedings of the Asia Pacific Conference on Circuits and Systems, pp. 527-530.
• Lee, S. and Asada, H., 1999, "A Perturbation/Correlation Method for Force Guided Robot Assembly", IEEE Transactions on Robotics and Automation, Vol.15, No. 4, pp. 764-773.
• Marapane, S.B., Trivedi, M.M., Lassiter, N., Holder, M.B., 1996, "Motion Control of Cooperative Robotic Teams through Visual Observation and Fuzzy Logic Control", Proceedings of the 1996 IEEE International Conference on Robotics and Automation, Vol.2, Minneapolis, MN, USA, pp. 1738-1743.
• Mataric, M.J., 1998, "Coordination and Learning in Multi-Robot Systems", IEEE Intelligent Systems, March/April 1998, pp. 6-8.
• McCarragher, B.J. and Asada, H., 1993, "A Discrete Event Approach to the Control of Robotic Assembly Tasks", Proceedings of the 1993 IEEE International Conference on Robotics and Automation, Atlanta, GA, USA.
• Osborn, J.F., 1989, "Applications of Robotics in Hazardous Waste Management", Proceedings of the SME 1989 World Conference on Robotics Research, Gaithersburg, MD, USA.
• Parker, L.E., 1995, "The Effect of Action Recognition and Robot Awareness in Cooperative Robotic Teams", Proceedings of the 1995 IEEE International Conference on Robotics and Automation, Vol.1, Nagoya, Japan, pp. 212-219.
• Shaffer, G. and Stentz, A., 1992, "A Robotic System for Underground Coal Mining", Proceedings of the 1992 IEEE International Conference on Robotics and Automation, Vol.1, pp. 633-638.
• Shimokura, K. and Muto, S., 1996, "A High Speed Method of Detecting Contact-State Transitions and its Implementation in a Task-Coordinate Manipulation System", Proceedings of the 1996 IEEE International Conference on Robotics and Automation, Minneapolis, MN, USA.
• Sujan, V.A., 2002a, "Optimum Camera Placement by Robot Teams in Unstructured Field Environments", Proceedings of the IEEE International Conference on Image Processing (ICIP), Rochester, NY, USA.
• Sujan, V.A., 2002b, "Task Directed Imaging in Unstructured Environments by Cooperating Robots", submitted to the 2002 Third Indian Conference on Computer Vision, Graphics and Image Processing, Ahmedabad, India.
• Sujan, V.A. and Dubowsky, S., 2002, "Visually Built Task Models for Robot Teams in Unstructured Environments", Proceedings of the 2002 IEEE International Conference on Robotics and Automation, Washington, D.C., USA.
• Takanishi, A., Tochizawa, M., Karaki, H. and Kato, I., 1989, "Dynamic Biped Walking Stabilized With Optimal Trunk And Waist Motion", Proceedings of the 1989 IEEE/RSJ International Workshop on Intelligent Robots and Systems (IROS'89) - The Autonomous Mobile Robots and Its Applications, pp. 187 192.
• Veloso, M. and Stone, P., 1999, "Task Decomposition and Dynamic Role Assignment for Real-Time Strategic Teamwork", Intelligent Agents V Proceedings of the International Workshop on Agent Theories, Architectures and Languages, Springer-Verlag, Heidelberg.
• Xiao, J. and Liu, L., 1998, "Contact States: Representation and Recognizability in the Presence of Uncertainties", Proceedings of the 1998 IEEE/RSJ International Conference on Intelligent Robots and Systems, Victoria, B.C., Canada.

Publication Dates

• Publication in this collection
  07 Oct 2004
• Date of issue
  Sept 2004

History

• Accepted
  July 2004
• Received
  July 2004