The approach of the door-opening method is shown in Figure 4, which can also be applied to pull doors with some revisions on the trajectory planning. The task can be decomposed into five subtasks: locating the door, rotating to align, locating the handle, opening the door and walking through. Here we use Q to denote the end-effecter of the tool, and Q is fixed to the tool. The positions of Q at different transients along the trajectory are marked by other letters, and these marks are fixed to the ground.
4.1 Locating the Door
The first subtask is locating the door (O → A→B → C), in which the robot identifies the orientation matrix of the DCS in the GCS(\({}_{\text{D}}^{\text{G}} {\mathbf{R}}\)) by touching three non-collinear points on the door, as shown in Figure 5.
Three non-collinear points define a plane. According to this basic principle, the robot firstly moves its body forward (–Z
R) until Q touches the first point A on the door (O → A). Then, the robot moves its body both backward and leftward to a different point (A → O’), and forward again to touch the second point B(O’ → B). Finally, the robot moves its body both backward and upward (B → O’’), and forward again to touch the third point C(O’’ → C). By making the backward and leftward distance during AO’ equal, the backward and upward distance during BO’’ equal, the maximum angle between the sagittal axis of the robot body and the normal line of the door plane is allowed to be \(45^\circ\).
4.1.1 Trajectory Generation
The 6D trajectory of the robot body is generated by a discrete force control model:
$$\left\{ {\begin{array}{*{20}l} {{\mathbf{M}}{\ddot{\mathbf{S}}}_{k} = {\mathbf{F}}_{k} - {\mathbf{C}}{\dot{\mathbf{S}}}_{{k - 1}} ,} \hfill \\ {\begin{array}{*{20}l} {{\dot{\mathbf{S}}}_{k} = {\dot{\mathbf{S}}}_{{k - 1}} + {\ddot{\mathbf{S}}}_{k} \Delta t,} \hfill \\ {{\mathbf{S}}_{k} = {\mathbf{S}}_{{k - 1}} + {\dot{\mathbf{S}}}_{k} \Delta t,} \hfill \\ \end{array} } \hfill \\ \end{array} } \right.$$
(7)
where S
k
—6D coordinates of the robot body at time k,
$${\mathbf{S}}_{k} = \left( {x_{k} ,y_{k} ,z_{k} ,\alpha_{k} ,\beta_{k} ,\gamma_{k} } \right)^{\text{T}} ,$$
F
k
—6D force at time k,
$${\mathbf{F}}_{k} = \left( {F_{xk} ,F_{yk} ,F_{zk} ,M_{xk} ,M_{yk} ,M_{zk} } \right)^{\text{T}} ,$$
M—Mass matrix, C—Damp matrix.
The robot determines M and C according to the required accelerations and velocities, and generates different trajectories by applying different F
k
. During locating the door, the F
k
is
$${\mathbf{F}}_{k} = \left\{ {\begin{array}{*{20}l} {\left( {\begin{array}{*{20}c} {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{O} \left( {0,0, - 1} \right)^{{\text{T}}} } \\ {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{O} \left( {0,0,0} \right)^{{\text{T}}} } \\ \end{array} } \right),\;{\text{if }}Q \in OA \cup O^{\prime}B \cup O^{\prime\prime}C,} \hfill \\ {\left( {\begin{array}{*{20}c} {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{O} \left( { - 1,0,1} \right)^{{\text{T}}} } \\ {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{O} \left( {0,0,0} \right)^{{\text{T}}} } \\ \end{array} } \right),\;{\text{if }}Q \in AO^{\prime},} \hfill \\ {\left( {\begin{array}{*{20}c} {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{O} \left( {0,1,1} \right)^{{\text{T}}} } \\ {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{O} \left( {0,0,0} \right)^{{\text{T}}} } \\ \end{array} } \right),\;{\text{if }}Q \in BO^{\prime\prime},} \hfill \\ \end{array} } \right.$$
(8)
where \({}_{\text{R}}^{\text{G}} {\mathbf{R}}_{O}\)—\({}_{\text{R}}^{\text{G}} {\mathbf{R}}\) at point O, derived by Eq. (5), \(Q \in OA \cup O^{\prime}B\)—Q is on line segment OA or O’B.
4.1.2 Orientation Matrix Calculation
By applying Eq. (5), G
O
R at A, B, C can be derived and denoted as G
O
RA
(x
A
, y
A
, z
A
)T, G
O
RB
(x
B
, y
B
, z
B
)T and G
O
RC
(x
C
, y
C
, z
C
)T. Let n(x
n
, y
n
, z
n
)T denote the normal vector of the door plane. Then n can be calculated by
$$\left( {\begin{array}{*{20}c} {\begin{array}{*{20}c} {x_{A} - x_{B} } \\ {y_{A} - y_{B} } \\ {z_{A} - z_{B} } \\ \end{array} } & {\begin{array}{*{20}c} {x_{C} - x_{B} } \\ {y_{C} - y_{B} } \\ {z_{C} - z_{B} } \\ \end{array} } \\ \end{array} } \right)^{{\text{T}}} \left( {\begin{array}{*{20}c} {{}^{{\text{G}}}x_{n} } \\ {{}^{{\text{G}}}y_{n} } \\ {{}^{{\text{G}}}z_{n} } \\ \end{array} } \right) = 0.$$
(9)
Transferring the vector n from the GCS to the RCS, we can get
$$\left( {\begin{array}{*{20}c} {{}^{{\text{R}}}x_{n} } & {{}^{{\text{R}}}y_{n} } & {{}^{{\text{R}}}z_{n} } \\ \end{array} } \right)^{{\text{T}}} = {}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{O}^{{ - 1}} {}^{{\text{G}}}{\mathbf{n}}.$$
(10)
Then projecting n into the X
R
O
R
Z
R plane, we can get
$${}^{{\text{R}}}{\mathbf{n}}_{{\text{p}}} = (\begin{array}{*{20}c} {{}^{{\text{R}}}x_{n} } & 0 & {{}^{{\text{R}}}z_{n} } \\ \end{array} )^{{\text{T}}} .$$
(11)
Here the Tait-Bryan angle, which includes roll, pitch and yaw, is used to express the orientation of the DCS in the GCS. The yaw angle is
$$Y_{{\text{a}}} = \theta = \arctan \left( {\frac{{{}^{{\text{R}}}x_{n} }}{{{}^{{\text{R}}}z_{n} }}} \right).$$
(12)
Taking into account that there may be stairs or slopes along the direction of Z
D in front of the door, making the door plane not normal to the ground plane, the pitch angle exists between n and n
p:
$$P_{{\text{i}}} = \alpha = - \arcsin \left( {\frac{{{}^{{\text{R}}}y_{n} }}{{\left\| {\mathbf{n}} \right\|_{2} }}} \right).$$
(13)
Considering there is nearly no doors with stairs or slopes along the direction of X
D, we can reasonably assume that the roll angle \(R_{\text{o}} = 0.\)
So, the orientation matrix can be calculated from the Tait-Bryan angle by
$$\left\{ {\begin{array}{*{20}l} {{}_{{\text{D}}}^{{\text{G}}} {\mathbf{R}} = {}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{O} {}_{{\text{R}}}^{{\text{D}}} {\mathbf{R}}_{{YX^{\prime}Z^{\prime\prime}}}^{{ - 1}} ,} \hfill \\ {{}_{{\text{R}}}^{{\text{D}}} {\mathbf{R}}_{{YX^{\prime}Z^{\prime\prime}}} \left( {Y_{{\text{a}}} ,P_{{\text{i}}} ,R_{{\text{o}}} } \right) = \left( {\begin{array}{*{20}c} {\cos \theta } & {\sin \theta \sin \alpha } & {\sin \theta \cos \alpha } \\ 0 & {\cos \alpha } & { - \sin \alpha } \\ { - \sin \theta } & {\cos \theta \sin \alpha } & {\cos \theta \cos \alpha } \\ \end{array} } \right).} \hfill \\ \end{array} } \right.$$
(14)
4.2 Rotating to Align
The second subtask is rotating around X
R and Y
R to align with the door plane (C → D). As shown in Figure 6, the robot moves its body, both transferring and rotating, from C to D, and at the same time moves the feet to follow the body (C
L → D
L). The point O
R at D superposes O
R at O, which determines the 6D trajectory as
$${\mathbf{CD}} = \left( {\begin{array}{*{20}c} {{}^{{\text{G}}}{\mathbf{O}}_{{{\text{R}}O}} - {}^{{\text{G}}}{\mathbf{O}}_{{{\text{R}}C}} } \\ {\left( {\begin{array}{*{20}c} {R_{{\text{o}}} } & {P_{{\text{i}}} } & {Y_{{\text{a}}} } \\ \end{array} } \right)^{{\text{T}}} } \\ \end{array} } \right).$$
(15)
After the alignment, the horizontal rod of the tool will always keep normal to the door plane when the body translates, which guarantees the tool not to collide with the door plane during the process of locating the handle.
4.3 Locating the Handle
The third subtask is locating the handle (D → E→F → G), in which the robot identifies translational parameter G
O
D by three touches in three orthogonal directions.
In order to touch the handle, the robot has to decide the height and the moving direction of the tool first. A statistical analysis [15] of the most frequent size of handles shows that the height of the handle is in a range from 99 cm to 103 cm. Based on this acknowledgement, the robot keeps the vertical rod of the tool in this range. Then the robot chooses right as its target direction to touch the handle. If the robot confirms that the handle is not in the current direction, it changes its direction to left and performs the process of locating the handle again. Here we present the process of the localization and confirmation of the handle in the direction of right, and it is similar for left.
As shown in Figure 6, in case that the robot is far from the handle, the robot needs to move rightward cyclically. In every cycle except the final one, the robot successively moves the body forward (–Z
R) until touching the door plane, backward for a short constant distance to avoid rubbing with the door plane, rightward for a constant distance decided by the workspace of the tool, and moves the legs to follow the body, thus finishing the process of D → E. The purpose of moving forward to touch and backward for a constant distance at the beginning of every cycle is to initialize the distance of the current cycle and eliminate the error accumulated in last one. When the robot starts too far from the handle, a very little angular error will cause a large translational error along Z
D, even though the robot has already rotated to align. The large error makes it highly possible for the tool to fail to enter the space between the door and the handle considering its narrowness, thus failing to open the door. Because the distance of every rightward cycle is limited to an acceptable constant value which will never be too far, the translational error along Z
D can be limited well. Furthermore, by applying multiple three-points contacts of locating the door and reducing the distance of every rightward cycle, the detection accuracy can be guaranteed even if there are embossments or grooves on the door.
In the final cycle (E → F→G in Figure 7), after touching something at F, the robot moves its body leftward for a constant distance shorter than the handle, and downward to touch the handle. If the tool touches nothing until it gets lower than the minimum height, the robot treats it as a confirmation that the handle is not in this direction. If the tool touches something, the robot treats it as a signal of successfully locating the handle and goes on to next subtask.
The trajectory is generated by Eq. (7) in every cycle. The F
k
of every cycle during D → E is similar to the F
k
of E → F in the final cycle, and in the final cycle the F
k
is
$${\mathbf{F}}_{k} = \left\{ {\begin{array}{*{20}l} {\begin{array}{*{20}l} {\left( {\begin{array}{*{20}c} {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{E} \left( {\begin{array}{*{20}c} 0 & 0 & { - 1} \\ \end{array} } \right)^{{\text{T}}} } \\ {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{E} \left( {\begin{array}{*{20}c} 0 & 0 & 0 \\ \end{array} } \right)^{{\text{T}}} } \\ \end{array} } \right),{\text{if }}Q \in EE^{\prime},} \hfill \\ {\left( {\begin{array}{*{20}c} {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{E} \left( {\begin{array}{*{20}c} 0 & 0 & 1 \\ \end{array} } \right)^{{\text{T}}} } \\ {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{E} \left( {\begin{array}{*{20}c} 0 & 0 & 0 \\ \end{array} } \right)^{{\text{T}}} } \\ \end{array} } \right),{\text{if }}Q \in E^{\prime}F^{\prime},} \hfill \\ \end{array} } \hfill \\ {\begin{array}{*{20}l} {\left( {\begin{array}{*{20}c} {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{E} \left( {\begin{array}{*{20}c} 1 & 0 & 0 \\ \end{array} } \right)^{{\text{T}}} } \\ {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{E} \left( {\begin{array}{*{20}c} 0 & 0 & 0 \\ \end{array} } \right)^{{\text{T}}} } \\ \end{array} } \right),{\text{if }}Q \in E^{\prime}F^{\prime},} \hfill \\ {\left( {\begin{array}{*{20}c} {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{E} \left( {\begin{array}{*{20}c} { - 1} & 0 & 0 \\ \end{array} } \right)^{{\text{T}}} } \\ {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{E} \left( {\begin{array}{*{20}c} 0 & 0 & 0 \\ \end{array} } \right)^{{\text{T}}} } \\ \end{array} } \right),{\text{if }}Q \in FG^{\prime},} \hfill \\ {\left( {\begin{array}{*{20}c} {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{E} \left( {\begin{array}{*{20}c} 0 & { - 1} & 0 \\ \end{array} } \right)^{{\text{T}}} } \\ {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{E} \left( {\begin{array}{*{20}c} 0 & 0 & 0 \\ \end{array} } \right)^{{\text{T}}} } \\ \end{array} } \right),{\text{if }}Q \in FG^{\prime}.} \hfill \\ \end{array} } \hfill \\ \end{array} } \right.$$
(16)
The location of O
D on the handle can be expressed by
$$\left\{ {\begin{array}{*{20}l} {\left( {\begin{array}{*{20}c} {{}^{{\text{G}}}{\mathbf{O}}_{{\text{D}}} } \\ 1 \\ \end{array} } \right) = {}_{{\text{R}}}^{{\text{G}}} {\mathbf{T}}_{E} \left( {\begin{array}{*{20}c} {{}^{{\text{R}}}{\mathbf{O}}_{{\text{D}}} } \\ 1 \\ \end{array} } \right),} \hfill \\ {{}^{{\text{R}}}{\mathbf{O}}_{{\text{D}}} = {}^{{\text{R}}}{\mathbf{O}}_{{{\text{R}}E}} + \left( {\begin{array}{*{20}c} {\left\| {{}^{{\text{G}}}{\mathbf{O}}_{{{\text{R}}F}} - {}^{{\text{G}}}{\mathbf{O}}_{{{\text{R}}F'}} } \right\|_{2} } \\ { - \left\| {{}^{{\text{G}}}{\mathbf{O}}_{{{\text{R}}G}} - {}^{{\text{G}}}{\mathbf{O}}_{{{\text{R}}G'}} } \right\|_{2} } \\ { - \left\| {{}^{{\text{G}}}{\mathbf{O}}_{{{\text{R}}E}} - {}^{{\text{G}}}{\mathbf{O}}_{{{\text{R}}E'}} } \right\|_{2} } \\ \end{array} } \right).} \hfill \\ \end{array} } \right.$$
(17)
4.4 Opening the Door
The fourth subtask is opening the door (G → H→I), in which the robot firstly moves along a circular line in the door plane to turn the handle until it reaches the end (G → H in Figure 8), and then moves forward to try to push the door open (H → I in Figure 8). When moving forward, the robot keeps detecting the contact force. If it exceeds the maximum force the robot can apply, the robot treats it as a signal of door blocked and stops the task. The trajectories of turning and pushing are both generated by Eq. (7), and the F
k
is
$${\mathbf{F}}_{k} = \left\{ {\begin{array}{*{20}l} {\left( {\begin{array}{*{20}c} {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{G} \left( {\begin{array}{*{20}c} {\sin \left( {\frac{{2\overline{v} k}}{{\pi r}}} \right)} & { - \cos \left( {\frac{{2\overline{v} k}}{{\pi r}}} \right)} & {\text{0}} \\ \end{array} } \right)^{{\text{T}}} } \\ {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{G} \left( {\begin{array}{*{20}c} 0 & 0 & 0 \\ \end{array} } \right)^{{\text{T}}} } \\ \end{array} } \right),{\text{if }}Q \in {\mathop {\frown}\limits_{GH}},} \hfill \\ {\left( {\begin{array}{*{20}c} {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{G} \left( {\begin{array}{*{20}c} 0 & 0 & { - 1} \\ \end{array} } \right)^{{\text{T}}} } \\ {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{G} \left( {\begin{array}{*{20}c} 0 & 0 & 0 \\ \end{array} } \right)^{{\text{T}}} } \\ \end{array} } \right),{\text{if }}Q \in HI,} \hfill \\ \end{array} } \right.$$
(18)
where \(\bar{v}\)—Average linear speed planed along the arc, r—Radius of the arc, \(Q \in {\mathop {\frown}\limits_{GH}}\)—Q is on the arc \({\mathop {\frown}\limits_{GH}}\).
The simpler mechanism of the 0-DOF tool plays a significant role here, in releasing the inner force in the tool when turning the handle. The open-loop structure of the end-effecter cannot achieve a firm grasp of the handle like widely used closed-loop multi-DOF grippers, which takes a notably positive effect on the subtask but not negative, because the inner force is effectively released. The inner force occurs because the motion of the manipulator cannot follow the position of the handle exactly due to the positional error and the imprecise modeling of the door, while a firm grasp compels the manipulator to follow. And it is nearly impossible to completely solve this confliction only if a firm grasp is applied. However, the firm grasp is not essential for all cases. When applying the loose grasp, the contact point of the tool and the handle can slide along themselves, so that the tool does not have to follow the handle exactly, thus releasing the inner force. And because of the large areas the tool can move in (red areas in Figure 8), it will not be a trouble for the tool to keep contacting with the handle when moving.
4.5 Walking Through
The fifth subtask is walking through(I → J→K → L), in which the robot adjusts its body back to the sagittal plane, walks leftward into the door range, and then walks forward to get through the door (Figure 9). The robot keeps detecting the contact force during the whole process. If the contact force exceeds the maximum force the robot can apply, the robot treats it as a signal of door blocked and stops the task.
When adjusting, the tool translates parallel to the wall plane (I → J) to release the handle and prepare for the left walking. The point J is in the sagittal plane like the point E, but higher than E for h to avoid colliding with the handle, so the adjustment trajectory is
$${\mathbf{IJ}} = \left( {\begin{array}{*{20}c} {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{I} \left( {\begin{array}{*{20}c} {\begin{array}{*{20}c} {\left( {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{I} {}^{{\text{R}}}{\mathbf{X}}_{{\text{R}}} } \right)^{{\text{T}}} \left( {{}^{{\text{G}}}{\mathbf{E}} - {}^{{\text{G}}}{\mathbf{I}}} \right)} \\ {\left( {{}_{R}^{G} {\mathbf{R}}_{I} {}^{{\text{R}}}{\mathbf{Y}}_{{\text{R}}} } \right)^{{\text{T}}} \left( {{}^{{\text{G}}}{\mathbf{E}} - {}^{{\text{G}}}{\mathbf{I}}} \right) + h} \\ \end{array} } \\ 0 \\ \end{array} } \right)} \\ {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{I} \left( {\begin{array}{*{20}c} 0 & 0 & 0 \\ \end{array} } \right)^{{\text{T}}} } \\ \end{array} } \right),$$
(19)
where \({}^{\text{R}}{\mathbf{X}}_{\text{R}} ,{}^{\text{R}}{\mathbf{Y}}_{\text{R}}\)—Basis vector of X, Y-axis of the RCS,
$${}^{\text{R}}{\mathbf{X}}_{\text{R}} = \left( {1,0,0} \right)^{\text{T}} ,\;{}^{\text{R}}{\mathbf{Y}}_{\text{R}} = \left( {0,1,0} \right)^{\text{T}} ,$$
\({}^{\text{G}}{\mathbf{E}},{}^{\text{G}}{\mathbf{I}}\)—Coordinates of point E, I in the GCS.
During the leftward walking, the robot keeps the end-effecter Q touching the door plane to prevent the door from closing automatically because of the door closer. Considering the high load capacity, the robot can deal with doors with large rebounding forces. The trajectory is
$${\mathbf{JK}} = \left( {\begin{array}{*{20}c} {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{J} \left( {\begin{array}{*{20}c} {\left( {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{J} {}^{{\text{R}}}{\mathbf{X}}_{{\text{R}}} } \right)^{{\text{T}}} \left( {{}^{{\text{G}}}{\mathbf{O}}_{{\text{D}}} - {}^{{\text{G}}}{\mathbf{O}}_{{\text{R}}} } \right) - \frac{{w_{{\text{R}}} }}{2}} \\ {\begin{array}{*{20}c} 0 \\ 0 \\ \end{array} } \\ \end{array} } \right)} \\ {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{J} \left( {\begin{array}{*{20}c} 0 & 0 & 0 \\ \end{array} } \right)^{{\text{T}}} } \\ \end{array} } \right),$$
(20)
where \(w_{\text{R}}\)—Width of the robot.
During the process of walking forward, the robot uses its body to push the door open, making a good use of the high load capacity. The forward trajectory is
$${\mathbf{KL}} = \left( {\begin{array}{*{20}c} {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{K} \left( {\begin{array}{*{20}c} 0 \\ 0 \\ {\left( {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{K} {}^{{\text{R}}}{\mathbf{Z}}_{{\text{R}}} } \right)^{{\text{T}}} \left( {{}^{{\text{G}}}{\mathbf{O}}_{{\text{D}}} - {}^{{\text{G}}}{\mathbf{S}}_{{41}} } \right) - l_{{\text{R}}} } \\ \end{array} } \right)} \\ {{}_{{\text{R}}}^{{\text{G}}} {\mathbf{R}}_{K} \left( {\begin{array}{*{20}c} 0 & 0 & 0 \\ \end{array} } \right)^{{\text{T}}} } \\ \end{array} } \right),$$
(21)
where \({}^{\text{R}}{\mathbf{Z}}_{\text{R}}\)—Basis vector of Z-axis of the RCS,
$${}^{\text{R}}{\mathbf{Z}}_{\text{R}} = \left( {0,0,1} \right)^{\text{T}} ,$$
\({}^{\text{G}}{\mathbf{S}}_{41}\)—Derived by Eq. (3), l
R —Length of the robot.
