Abstract
As a substitute for humans, the mobile manipulator has become increasingly vital for on-site rescues at Nuclear Power Plants (NPPs) in recent years. The high energy efficiency of the mobile manipulator when executing specific rescue tasks is of great importance for the mobile manipulator. This paper focuses on the energy consumption of a robot executing the door-opening task, in a scenario mimicking an NPP rescue. We present an energy consumption optimization scheme to determine the optimal base position and joint motion of the manipulator. We developed a two-step procedure to solve the optimization problem, taking the quadric terms of the joint torques as the objective function. Firstly, the rotational motion of the door is parameterized by using piecewise fifth-order polynomials, and the parameters of the polynomials are optimized by minimizing the joint torques at the specified base position using the Quasi-Newton method. Second, the global optimal movement of the manipulator for executing the door-opening task is acquired by means of searching a grid for feasible base positions. Comprehensive door-opening experiments using a mobile manipulator platform were conducted. The effectiveness of the proposed method has been demonstrated by the results of physical experiments.
1. Introduction
Mobile manipulators, as a replacement for humans, play a key role when they perform rescue tasks in the extreme environment of a nuclear power plant (NPP), such as door-opening and turning a valve. The energy optimization of rescue tasks performed by a robotic arm, in particular that of the fundamental task of door-opening, is of great importance when the energy supply is restricted by the capacity of the battery with which the robot can be equipped, unlike in traditional methods where energy is supplied via a cable.
The Fukushima Daiichi NPP accident in Japan on March 11, 2011, was triggered by an earthquake of magnitude 9.0 and the resultant tsunami marked the beginning of the worst nuclear accident of the last two decades [1–3]. Nuclear power is an important resource and (NPPs) have recently undergone rapid development in China [4]. The complexity of the NPP’s structure, the radioactive working environment, and some special characteristics, such as high temperatures and high pressure, render nuclear disaster rescue very difficult. The dangers caused by such an accident mean that mobile robot plays an important role in the NPP rescue process. After the Fukushima Daiichi NPP accident, the Defense Advanced Research Projects Agency (DARPA) initiated a new challenge in 2012, the DARPA Robotics Challenge (DRC) [5], including eight rescue tasks to test the capabilities of teams, as well as of individuals. The tasks were ranked by DARPA in terms of difficulty as Valve (easiest), Terrain and Hose (easier), Door, Debris, Wall, and Ladder (harder), and Vehicle (hardest) [6].
In this paper, we focus on the energy consumption of a mobile manipulator performing the door-opening task. Nagatani and Yuto proposed a method that allows a mobile manipulator, named the “YAMABICO-10” robot, to open a door and pass through the doorway. In their approach, they applied to the mobile manipulator control system the concept of action primitives, which control the “YAMABICO-10” robot according to sequences of planned motion primitives. However, each action primitive was designed with an error adjustment mechanism to handle the accumulated positioning error of the mobile base [7–9]. Peterson et al. presented the design and implementation of a door-opening controller using a hybrid dynamic system model; when using this simple controller, the radius and center of rotation of the door are estimated online. The results of their experiments demonstrated that off-the-shelf algorithms for force/torque control are very effective for solving the task of grasping the handle and opening the door [10]. Chung et al. [11] proposed a control strategy for the door-opening procedure executed by a service robot, PSR1, which utilized a three-fingered robot hand for grasping the door handle. Two active-sensing strategies were proposed to estimate the kinematic parameters in a real environment. An integrated strategy of motion coordination was presented based on the components of three subsystems: a robotic hand, a robotic arm, and a mobile robot. The force and position control were successfully achieved by using the contact force of the three-fingered robotic hand during the door-opening procedures.
Ahmad et al. designed a modular and reconfigurable robot (MRR) mounted on a wheeled mobile platform [12, 13]. They proposed a new method that utilizes the multiple working modes of the MRR modules to prevent the occurrence of large internal forces that arise because of positioning errors or imprecise modeling of the robot or its environments. By selectively switching the joints of the MRR to work in passive mode during the door-opening operation, the controller design was significantly simplified. Zhang et al. [14] presented a multiple mode control system of a two-degree-of-freedom (DOFs) compact wrist that can work in active mode with position or torque control, or in passive mode with wrist-environment interactive force compensation. They verified in their door-opening experiments that the wrist could move freely without generating excessive internal force. Kobayashi et al. [15] designed a rescue robot series named UMRS. The robot had a special end-effector, which can grip and rotate cylindrical type and lever type door handles. The robot equipped with a door-opening system was capable of moving freely through rooms, even if there were doors between them. Klingbeil et al. [16] proposed a method that used vision to identify a small number of key positions, such as the axis of rotation of the door handle, and the end-point of the door handle, to allow a manipulator to open various types of doors without prior knowledge of their parameters. Karayiannidis et al. [17] developed an algorithm that can be implemented in a velocity-controlled manipulator, the end- effector which is equipped with force sensing capabilities. The method consists of a velocity controller, which uses force measurements and an estimation of the radial direction based on adaptive estimates of the position of the door hinge. The control action can be decomposed into an estimated radial and tangential direction following the concept of hybrid force/motion control. Endres et al. [18] presented an approach for learning a dynamic model of a door from sensor observations and utilizing it for effectively swinging the door open to the desired angle. The learned models enable the realization of dynamic door-opening strategies and reduce the complexity of the door-opening task.
As proposed in [19], a power efficiency estimation-based health monitoring and fault detection method has been developed for a modular and reconfigurable robot (MRR). The power efficiency of each of the robot’s joints is measured using sensors. Luo et al. [20] presented the Lagrange interpolation method to express each joint trajectory function to realize trajectory planning that achieves energy minimization of industrial robotic manipulators. Field and Stepanenko presented an iterative dynamic programming method that is modified to perform a series of dynamic programming, passing over a small reconfigurable grid that covers only a portion of the solution space at any one pass, to plan minimum energy consumption trajectories for robotic manipulators [21]. Liu et al. [22] proposed the fourth-order Runge-Kutta method, multiple shooting methods, and traversing method to solve optimal energy trajectory planning for palletizing robot.
The objective of this study was to develop an energy consumption optimization method for the door-opening procedure. The main contributions of this paper are summarized as follows. An energy consumption optimization scheme that finds the optimal base position and joint motion of the manipulator for optimizing energy consumption is presented. Since the end-effector trajectory is assigned according to the trajectory of the door handle, the problem of searching the optimal manipulator movement by using the parameters of the door’s fifth-order splines transforms to a simple parametric optimization problem at each base position. The global optimal energy-efficient movement of the robotic arm is finally obtained by exhaustively searching the entire base position grid.
The rest of the paper is organized as follows. In Section 2, we introduce the door-opening task and establish the energy consumption optimization objective function. Section 3 provides a description of the mobile modular robot and introduces the kinematic and the dynamics model of the manipulators. In the Section 4, we address the optimization method and describe numerical simulations. The results of our experiments are discussed in Section 5, and conclusions are presented in Section 6.
2. Formulation of Optimization Problem
2.1. Door-Opening Operation
In this section, we propose a door- opening method. The following assumptions were made: the door-opening and door handle rotating directions are known (the door is opened toward the left-side and the door handle is rotated toward the right-side); the door axis of rotation is perpendicular to the floor; the door moves in the horizontal plane; the mobile platform travels on the ground, which can always be adjusted for a structured laboratory environment; and the axis of rotation of the first manipulator module is perpendicular to the ground.
A brief explanation of the door-opening procedure is as follows.(a)The mobile manipulator moves such that it is positioned in front of the door and grasps the door handle;(b)The mobile base remains static. The manipulator realizes turning the door handle by tracking a planned trajectory;(c)The mobile base still remains static. The manipulator realizes pulling the door by tracking a planned trajectory in the horizontal plane.
A model of the door-opening procedure using the mobile manipulator is depicted in Figure 1. The figure shows the origin of the reference frame, , set at the intersection point of the door hinge, the horizontal plane that crosses the origin of the reference frame of the mobile manipulator, , and the reference frame, , located at the position of the manipulator end-effector, which is used to grasp the door handle.
In order to plan the path of the mobile manipulator, we first need to know the accurate value of the door handle radius and the door radius , the initial base position of the mobile manipulator , and the position of the end-effector, which holds the door handle firmly . The door motion is conformed to follow the door trajectory in the plane with the center of rotation at and a radius as shown in the Figure 1. The trajectory radius of pulling the door is derived as follows:
2.2. Path Planning of Door-Opening
In this section, we focus on pulling the door handle to open the door. During this procedure, the home position of the mobile base, the door radius, and the height of the door handle are measured. These measured parameters are then used for planning the path of the mobile manipulator that allows it to open the door to the desired angle.
The mobile manipulator achieves the action of pulling the door by tracking the circle arc represented by the door trajectory, as shown in Figure 2. The arc of the door trajectory can be acquired by using the interpolation points , , and . In Figure 2, we show the optimal base position during the door-opening procedure. The mobile manipulator approaches the door and stops at any position near the door. The end-effector is fixed at the same position as the door handle during the door-opening procedure, and each joint of the manipulator rotates in accordance with the motion planning. The door is pulled in the horizontal plane, and the initial and final angles of the door are consistent. The angles of Joints 1, 2, 3, 4, 5, and 6 are denoted as , , , , , and , respectively. The input torques of each joint of the manipulator are expressed as , , , , , and , respectively. The angles and the torques are represented in matrix forms as and . The angle of the door is expressed as , and the conditions on at the start time, , and the end time, , are written as
The positions of the end-effector at points , and are computed as follows:where denotes the rotation radius of the door, denotes the rotation angle of the door when reaching points , , and , and denotes the rotation angle of the door that allows the mobile manipulator to enter the doorway.
The orientations of the end-effector and the door are the same during the process of pulling the door. When the position of the end-effector remains constant with respect to the door handle, as shown in Figure 1, the variation in the rotation matrix during the process denoted by . denotes the initial rotation matrix of the end-effector after turning the door handle, which can be acquired by using forward kinematics [23]. Then, at time t, this matrix is multiplied by a rotation matrix parameterized by the rotation angle of the door :
2.3. Objective Formulation of Energy Consumption
The energetic cost is an important metric of the energy consumption of the manipulator. We experimentally evaluated the effect of different base positions of the robot on the energy consumption of the manipulator during the door-opening procedure. The input energy of the motor is as follows:where is the power supply voltage and is the instantaneous current of the DC motor. The total energy consumed by the robot’s actuators includes the generated mechanical power (), heat power (), and power losses (). This can be described as
The mechanical power generated by each actuator is related to the rotation angular velocity and torque. Therefore, the total instantaneous mechanical power during the movement of the manipulator can be stated aswhere is the total mechanical power of the actuators, is the torque of each actuators in N·m, is the rotation angular velocity of motors in rad/s, and is the number of manipulator joints.
The torque of each actuator is related to the torque constant , where, is the gear ratio of the joint, is the efficiency of the transmission mechanism, and is the instantaneous current; is provided by [24]
As shown in (7), the mechanical power consists of the torque of each actuators and the rotation angular velocity of the motors. However, the angular velocity range of the joint trajectory planning of the manipulator is relatively small. Therefore, the problem of energy optimization becomes the joint torque optimization problem. For the trajectory optimization procedure, the integral of squared joint torques as the cost function is known from studies in the literature [25]. We introduce the following objective function as a standard for optimization. Here, denotes the optimal parameter, which is chosen as the position of mobile platform base and the trajectory of the door angle. To solve the problem of energy consumption optimization during the door-opening procedure, we determine the parameters that minimize :
3. Description of the Mobile Manipulator
3.1. Kinematics of the Robot
The kinematics model configuration of the 6-DOF Schunk modular manipulator is shown in Figure 3.
The forward kinematics is developed by using the D-H method and represented by .where and are the rotation and translation matrix. Based on (11), the joint angles by inverse kinematics calculations can be derived aswhere
Because of the redundancy, there are eight groups of joint angles through the inverse kinematics for giving the position of the end-effector. An algorithm that can minimize the Euler distance in joint space from the initial state should be chosen from the eight solutions [23]. The Jacobin matrix of the 6-DOF manipulator is
Based on (14), the angular velocity and acceleration of the joint can be derived aswhere is the position and orientation vector of the end-effector of the manipulator.
3.2. Dynamics of the Robot
From the inverse kinematics in the previous subsection, the equations for the motion of the system can be written by using :where is the force and torque of the direction used for grasping the door handle. denotes the dynamics parameter matrix of the door in (16), including the symmetric positive definite manipulator inertia matrix , the vector of centripetal and Coriolis torques , and the vector of gravitational torques . The matrix can be obtained as
where is a function of the relationship between the angle of the door-opening and the joint angle of the manipulator..where is the coordinate value of the direction in the coordinate system of the door, and are the abbreviations for and , respectively, and , , , and are the lengths of the links in Figure 3.
We cannot determine in (16), which means that the system is indeterminate. can be measured by using the six-axis F/T sensor, and the forward six rows of (16) can be rewritten aswhere is the inertia matrix of the manipulator, is the vector of the centrifugal force and Coriolis force, and is the vector of the gravity.
4. Optimization Method
In this part, we focus on the optimal method for the door-opening procedure. Since is an infinite dimensional parameter, it is difficult to find the optimal solution that minimizes rigorously. Therefore, we approximate as the fifth-order spline functions of time and find the coefficients of splines that minimize the cost function. The position of the mobile manipulators base is discretized into a grid, and at each grid point, the quasi-optimal motion of the door is calculated by using the spline functions.
4.1. Optimal Motion of Door
We divide the time interval by and assume that the trajectory of in each time interval ( and for ) is expressed by a fifth-order polynomial function of time, , aswhere , , , , and are the coefficients of the polynomial. In order to make the input torque continuous, we choose the function such that , , , for , and (20) satisfies (2). The coefficients , , and are constant. Therefore, there are independent parameters, and they can be shown as
We assume that the door angle is a monotonically increasing function at with constraint . At each position of the mobile manipulator’s base, we search for the values of that minimize the by using the Quasi-Newton method. We can find a vector that conforms to . Eq. (16) is multiplied by the vector , and thus, we obtain
The manipulator joint torque satisfying (22) can obtain a specified motion of the door . When is minimized, can be shown aswhere is a scalar parameter. By using (23), we can calculate uniquely. We propose a method, as shown in Algorithm 1, to acquire the optimal motion of the door and the corresponding torque of each joint.
| ||||||||||||||||||||||
4.2. Optimal Position of the Mobile Manipulator’s Base
We determine the optimal position of the mobile manipulator’s base and the end-effector’s grasp by using the exhaustive method. The region of , defined by , is divided into a grid, where each rectangle is given by . By calculating the objective function at each grid point using the method in Section 4.1, with a set of , we can determine the optimal position of the mobile manipulator’s base. The scheme for optimizing the position is shown in Algorithm 2.
| ||||||||||||||||||||
4.3. Numerical Simulations
In this section, we describe the acquisition of the optimal solution by using numerical simulations. We used MATLAB to find the optimal values of under the constraint that . The open angle of the door, , was set to be at s. The time of the door pulling interval was divided into ten subintervals; that is, . The measured length of the door was 0.85 [m]. The mass and inertia of the door were set to 35.7 [kg] and 8.598 [kg·m2], respectively. The lengths of , , , and shown in Figure 3 were 0.3 [m], 0.3 [m], 0.305 [m], and 0.415 [m], respectively. The mass and inertia of the manipulator were as provided in [26]. In the simulation calculation, we chose the grid points of , that is, and as 0.02 [m], the search area of was , and the objective function defined by (9) was calculated at each point. Figure 4 shows the contour plot of the objective function . It shows that the position of the minimum is and the value of is [N2m2s]. Figure 5 shows the motion process of the door in the planning time.
5. Experimental Results
5.1. Robot System
The mobile robot system (see Figure 6) consists of a 6-DOF modular manipulator produced by the SCHUNK Company, a four-wheeled platform with two drive wheels, and a two free wheels and two-finger gripper mounted on the arm wrist module. The 6-DOF modular manipulator comprises three types of modules, and each joint module consists of a brushless DC motor, a harmonic drive, a braking system, and an encoder. The mobile robot system is equipped with various types of sensors, including two six-axis force sensors and a camera. The two six-axis force sensors are mounted on the manipulator wrist module and the base module, respectively, to measure the mechanics date of the door-opening procedure. A joystick with associated force feedback control from the base-mounted 6-axis force/torque sensor is used for teleoperation. A camera is mounted on top of the frame to support 3D displays for continuous teleportation and object recognition. An API T3 (Automated Precision Inc.) laser tracker system was used to measure the base position in the experiment.
5.2. Controller Description
The structure of the proposed controller scheme is illustrated in Figure 7. The control system is shown in terms of the composition of the PD-computed torque controller. The computed torque controller (CTC) uses a feedback linearization method. It is assumed that the desired motion trajectory for the manipulator is determined by a path planner [27]. The tracking error is defined as where is the error of the plant, is the desired input variable, which is the desired joint angular displacement in the control system, and is the actual joint angular displacement.
The control torque is described as
The dynamic model in (19) is equivalent to a decoupled linear time-invariant system:
Considering that the desired trajectory is determined, and are known. This is a nonlinear feedback control law that guarantees tracking of the desired door-opening trajectory. Selecting PD feedback for results in the PD-computed torque controller.where and are the controller gains and are positive definite diagonal matrices, , . The closed-loop system equation is
By substituting (27) into (25), we obtain the complete expression of the control law:
5.3. Experimental Setup
The experiments were conducted in a simulated NPPs internal environment and a real NPPs fire door with a door closer was used. The trajectory during door-opening was calculated using the method described in Section 2.2. The entire door-opening procedure was realized using C++ programming with a cycle of 20 ms. Four groups of experiments using different base positions were conducted. In the first group, the base position, denoted by G1, was ( m, m). This is the point of the minimum objective function according to the numerical simulations. In the second group, denoted by G2, was ( m, m). In the third group, the base position, denoted by G3, was ( m, m). In the fourth group, the base position, denoted by G4, was ( m, m). The points G1, G2, G3, and G4 have been pointed and marked in Figure 4. G1 is the point of the minimum objective function as shown in Figure 4.
Figure 8 shows sequential pictures of the door-opening experiments. The duration of the door-opening procedure is 20 s, which is the same as that the numerical simulation described in Section 4.3. The mobile robot system can measure the instantaneous current in the door-opening procedure. The torque of each joint was calculated using the values of the instantaneous current and (8). Therefore, the value of could be calculated in the various groups of experiments. The test data were acquired at a sampling frequency of 50 Hz. The experiments were conducted more than three times under the same conditions to ensure the repeatability and effectiveness of the experimental results. During the comparison of the energy consumption in the different groups of experiments, the end-effector grasped the door handle in the same positions according to the door-opening path planning provided in Section 2.
(a)
(b)
The performance parameters of the motors and actuating devices of joints are shown in Table 1.
5.4. Comparison of the Different Base Positions
The matrix when the robot base is positioned at G1, G2, G3, and G4 can be calculated by using (8) and the performance parameters of the joints, shown in Table 1. The values of during the door-opening procedure are shown in Figure 9. The sums of the objective function for the different base positions are shown in Table 2.
The curves shown in Figure 9 demonstrate that energy consumption is optimized at base position G1. Figure 9 shows that the torque is increased during the start and end of the door-opening procedure because of the resistance of the door handle and the door closer. It can be clearly seen in Figure 9 that, in comparison with other selected positions, over time G1 is better than the other selected base positions minimizing the objective function, not only in terms of the accumulated value of , shown in Table 2, but also in terms of sustaining the lowest value of . This verifies the effectiveness of the proposed algorithm. The experiment value of is larger than the simulation value at the same base position (G1), because the simulation only considered the mechanical power of (6); however, the experiment value included the generated mechanical power, heat power, and power losses due to factors such as friction.
Figures 10 and 11 show the optimal trajectories of the joint angle and the torque τ of each joint for G1. It can be seen in Figure 11 that the values of the joints’ torques are larger than those of the other torques. Figure 10 shows that in the motion during the door-opening procedure the angle degree of Joints 2 and 3 is larger than that of the other joints.
6. Conclusion
In this paper, we proposed a novel energy consumption optimization scheme for a mobile manipulator executing the door-opening task. Focusing on the power consumption of the manipulator during the entire task period, we chose the quadric terms of the joint torques as the objective function. Furthermore, in this study a two-step optimization procedure was developed to solve the corresponding joint trajectories of the manipulator. In the first step, the feasible base positions of the manipulator are decentralized into a grid in order to simplify the entire optimization process. A piecewise fifth-order polynomials over time is utilized to parameterize the rotational motion of the door. By applying the Quasi-Newton method, the local optimal trajectories of the manipulator are obtained for a given base position. In the second step, the optimal base position is attained via searching the decentralized grid of the feasible base positions. The numerical results when the proposed method was applied showed that energy consumption was optimized at base position G1. The experimental results for door-opening at the different base positions demonstrate the effectiveness of the method proposed in this study. The proposed method will be useful for the development of the NPP rescue robots.
Conflicts of Interest
The authors declare no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.
Acknowledgments
This work was supported by the National Key Basic Research Development Plan Project of China (973) (2013CB035502), Project supported by the Foundation for Innovative Research Groups of the National Natural Science Foundation of China (Grant no. 51521003), Research Project of State Key Laboratory of Mechanical System and Vibration (MSV201610), Harbin Talent Program for Distinguished Young Scholars (no. 2014RFYXJ001), Heilongjiang Province Higher Education Project of Basic Scientific Research (2017-KYYWF-0568), Harbin Applied Technology Project of Research and Development (2015RQQXJ081), and “111” Project (B07018).