Powered wheelchairs operation requires docking as one of the daily activities, many times to very specific poses like driving back to face a chair or laterally to a bed so that, the user can transfer between the wheelchair and other surfaces. Another type of powered wheelchairs - standing mobility devices - has been proposed to enable standing mobility for lower-limb impaired users. i.e., the user locomotes in a standing posture rather than seated. One such example is the Qolo device developed by our group [Eguchi2018, Paez2018]. As with powered wheelchairs, controlling the docking manoeuvre to a conventional chair or a bed is usually difficult for individuals with lower-limb disabilities because twisting their upper body while in the device is rather limited or some users might have limited muscle control. Therefore, we believe it is possible to automate this process for assisting end-users that might find difficulties in this task.
For wheelchair docking control, a camera rigidly fixed on the wheelchair body is the most common and viable choice as source of visual feedback for generating the virtual landmark. Nonetheless, the main challenge comes from combining a standard powered wheelchair nonholonomic constraints and the FOV constraint of the camera into a single efficient solution for embedded implementation.
Improving autonomous docking for wheelchair users have been tackled in some previous works, as the proposed Automated Transport and Retrieval System (ATRS) for a smart wheelchair docking to a lift platform of a car based on LiDAR information [Gao2008]. [Ren2012] proposed a bed docking control method for the wheelchairs consisting of path planning and line trajectory tracking. The work in [Jain2014c] showed a novel method for autonomous detection of safe docking locations using 3D point cloud data without any visual marking or environment customization requirements. While in [wei2012] the focus was on a 3D semantic map based-shared control for a smart wheelchair, achieving an object-related navigation task such as door passage or furniture docking. Nonetheless, these approaches have not handled the challenge from the FOV constraint even they all use visual sensors.
I-a Problem Definition
The problem of keeping a real object inside camera’s horizontal field of view (HFOV) and docking to a virtual landmark inferred from the same object is proposed here as described in Fig. 2, as a novel description that tends to simplify the controller for embedded applications.
First, we set two polar coordinate systems on the object ( and the virtual landmark individually, where one pole lies on the center of the object, another pole lies on the virtual landmark, one polar axis coincidents with the centerline of the object, another polar axis coincidents with the posture of the virtual landmark. Then the relative pose between the virtual landmark and the object was expressed by , where indicates the distance between the objective and the virtual landmark, represents the angle between virtual landmark posture and the line connecting the Objective and the center of the robot, indicates the angle between object centerline and the line connecting Objective and the center of the robot. On the other hand, the camera’s location effect for constraining the control of the robots was defined by . For simplicity and in order to reduce the effect of the FOV constraint, we set the centerlines of the camera aligned with the robot, therefore, the pose is set to . Herewith, the angle should be a constant for the chosen camera and equals to half of the camera’s FOV.
Our solution is to consider the object for creating a virtual target (landmark) as a reference frame, then the problem could be rephrased as a wheelchair with a camera fixed at docks to pose while keeping within the FOV defined by along the trajectory, where is the bearing angle between the centerline of the camera and the connecting line of camera and Objective. To the authors best knowledge there has not been such a complete framework that generalizes both the virtual landmark as and camera parameter as , which is quite challenging for the controller design. Most of the literature consider either one or all of as zero which simplifies the problem, but in practices it is not always true, e.g., could be nonzero due to possible robot physical limitation, could be nonzero when the size of the wheelchair and object are relatively big, could be nonzero due to the user’s requirement.
I-B Related Work
Considering similar kinematics of underactuated vehicles keeping an Objective inside an HFOV along the docking trajectory, several works have addressed parts of this problem. In [sans2016vision] the authors proposed to use a logarithmic spiral as a planned path which could keep the Objective with a constant bearing angle along the trajectory, however, they used an additional straight line as a second stage to enter the docking station, which resulted in the transition between logarithmic spiral and straight-line not smooth. In a subsequent work [Sans-Muntadas2017c], they extended the docking path planning by combining a Fermat and logarithmic spiral’s properties together keeping the Objective within the HFOV and ensuring a smooth transition until the goal. However, in this case, the distance to the goal is expected to be zero, thus the problem solution is not directly extendable to our described situation in Fig. 2.
The work in [DilanAmarasinghe2005c, Murrieri2004] proposed hybrid (or switching) controllers, which provided an efficient method to overcome the nonholonomic constraints, however, they are designed for a mobile robot without a rider and the results presented do not meet velocity smoothness expected by an end-user. Other possible approach to solve the docking manoeuvre was developed in [Maniatopoulos2013d]
through a dual-model predictive controller (MPC) scheme to maintain the visibility of the Objective and a dipolar vector field to guarantee the convergence. Similarly,[ke2016visual] also proposed to use MPC to deal with the multi-physical constraints including FOV in a mobile robot visual servoing problem. Nonetheless, this becomes very computationally expensive for embedded implementation. The study in [Widyotriatmo2015b] used a barrier function as a Lyapunov function candidate to give the HFOV constraint, the designed control law showed asymptotic stability around the origin, however, they also considered the as zero. A similar problem was targeted in [bhattacharya2007optimal] and [sans2019path] with proposed optimal paths to deal with the FOV constraint, however, the target task was limited to point navigation instead of pose to pose docking as we present in this work.
In contrast to previous works, we propose a novel autonomous docking support system consisting of a virtual landmark estimation algorithm and a nonlinear feedback controller. Although most of the literature above considered and as zero, we extend the problem with a direct solution. The virtual landmark estimation algorithm is based on point cloud information from a low-cost RGB-D camera and relies on the geometry feature of the object without specific environmental markers. The nonlinear feedback controller is designed under consideration of the camera’s limited HFOV, thus, we find a feasible space where the stability of the controller is proved theoretically and validated with experiments of a robot successfully docking into a chair with the state variables converging and the chair kept inside HFOV at all times.
The paper continues as follows: Section II presents the virtual landmark estimation algorithm and the design of a nonlinear feedback controller. Section III presents the system overview of the control architecture. Sections IV presents experimental setup and results in terms of convergency of the state variables and no violation of the HFOV constraint. Section V concludes this work and addresses future work.
An autonomous docking support system is proposed to help the user approach a chair, bed, or other resting surfaces naturally and safely. The autonomous docking system consists of two main parts, the first part is the virtual landmark estimation, the second part is the nonlinear feedback controller. With a user riding on the device, we assume that at the moment of switching from user control to autonomous docking, the user aligned the camera HFOV, a detailed description of the feasible solution space is given in the next section.
Ii-a Virtual Landmark Estimation
We designed a virtual landmark in order to execute the docking without altering the environment which would limit the applicability of the system. The sensor should be able to recognize and estimate the pose of the docking object effectively. In a wheelchair, the required sensing distance should be from 4m to 0.2m, which caters to the user’s requirement. We selected an RGB-D camera for three reasons:
The user should be able to freely choose the desired docking place in the environment (chairs or benches), thus it is impossible to install sensors at the docking place, e.g., IR used for vacuum robots, or QR code as a landmark.
In order to recognize the pose of the chair precisely, 3D information is important, although LiDAR are very precise, but they are very expensive.
Different resting furniture (chairs, benches and beds) share similar geometry features, which could be described as one surface parallel to the floor and another surface vertical to the floor. In order to make a simple algorithm and reduce the computation, we only use the depth information from the camera (currently focused on chairs).
With this in mind, we have chosen for our robot an Intel RealSense D435 camera (Intel, Santa Clara, CA, USA) as source, whose HFOV is 85 degrees.
For designing the camera pose w.r.t robot frame ( plane shown in Fig. 1, plane shown in Fig. 2), we consider two main factors: 1. Avoiding obstruction by the robot or the human body. 2. Keep the object inside the camera’s FOV. The camera pose w.r.t plane of frame was depicted in Fig. 2, which sets , considering the limited HFOV a smaller is preferred. In the case of plane, the pose of the camera is , is better to be set as a bigger value, the pitch angle is selected as the camera should be able to keep the front edge of the object inside the vertical field of view (VFOV) when the wheelchair arrives at the virtual landmark. In this work, , and are tuned by experimentally.
For the point cloud processing, first, we cut the point cloud using a pass-through filter, resulting in a cube container with the most probable volume of the chair. Then, we use a VoxelGrid filter to downsample the point cloud, these two steps help reduce the computation. After that, we use the Euclidean Cluster Extraction method to get the clusters of the point cloud, with a selection of the minimum and maximum size, we further reduce the number of unneeded clusters. Finally, as assumed that the chair is in the FOV, we select the closest cluster as the chair.
The virtual landmark was then derived from the centroid of the chair bottom and back. To extract the bottom and back of the chair, we simply divide the chair with a height threshold, which is related to the height of the chair bottom, most chairs will fall within a similar threshold. For the implementation of the algorithm, we rely on the PCL open library [Rusu_ICRA2011_PCL]. Because the docking control occurs on 2D, we project the centroid of the chair’s bottom and back to the horizontal plane [GeometryinCV]. With the coordinate frame fixed on the camera we derive the transformation to the horizontal plane as:
then the projection point on the horizontal plane is defined as:
where is the camera given original point. With (2) we projected and for the chair bottom and back centroids individually.
Subsequently, the fixed transformation between camera frame and wheelchair frame allows to easily obtain these two points w.r.t wheelchair frame as and .
Following the diagram of the configuration in Fig. 1, the virtual landmark was generated by:
where indicates the virtual landmark for , and represents the general case for any given . and are renamed as the Objective and virtual landmark in Section II-B for easier representation.
Ii-B Nonlinear Feedback Controller
With a closed-loop on the virtual landmark from the on-board sensor available in real-time, we propose a feedback controller without a prior definition of the path, as done in [bhattacharya2007optimal, sans2019path].
Kinematic Formulation for Docking
The motion of the wheelchair can be represented by the so-called unicycle model with linear and angular velocities at the center of the powered wheels. , and denote wheelchair’s position and orientation in the global reference system. The kinematics is defined by:
Fig. 3 depicts the wheelchair docking schematic detailing the coordinate system fixed at (the wheels center), indicates the location of the camera. and define the distance of CD and OA separately with (). denotes the angle from CD to posture of virtual landmark, denotes the length of OC, denotes the angle from OC to OA, describes the angle AD-OA, denotes the angle from the goal pose of the virtual landmark to line OC defined as follows:
Inversely, point D could be expressed by as follows:
The bearing angle could be then expressed as a function of , where are constants:
Control Law Design
We propose a time-invariant control law that guarantees convergence and stability within the local conditions of the system. The kinematic equations of the nonholonomic unicycle robot from (5) can be expressed using the state variables as follows:
This formulation allows to control directly the relevant variables for constraining the FOV of the robot while docking.
Subsequently, we choose a Lyapunov function candidate as :
We design the controller by , similar to the derivation in [Widyotriatmo2015b]. Where is a first gain, and the linear velocity is proportional to the distance between the robot and virtual landmark, and inversely proportional to the angle , then the Lyapunov function becomes:
Finally, we designed , first without the constraint of , we can have , in which case:
With , and , we can guarantee to be globally negative definite and global asymptotically stable. However, for the generalized case including , the solution is not straight forward. Using the formulation of in (LABEL:equ:alphaStar), we adopted a constraint component into (13) to regulate , resulting in:
where we cannot guarantee that is globally negative definite. However, we pursued a feasible space where can be locally negative definite, thus making the controller stable within this region of initial states. We achieved this solution by solving through a numerical computation.
Designing the control gains by Lyapunov indirect method
Before finding the feasible space to guarantee the stability, we can optimize the gains based on Lyapunov first method to guarantee the system asymptotic stability. With equations of and put into (9) and the Jacobian linearization of with respect to at the origin yields:
Where , and are assumed to be equivalent to and
individually when they are close to zero. The eigenvalues of the Jacobian matrix above are:
where . In order to keep all the eigenvalues of the Jacobian matrix negative, , , are constrained as follows:
With this, we can guarantee the system asymptotic stability. In order to keep the wheelchair under a low speed, here we choose , as 0.15, 0.6 separately, depends on the value of , and .
Computation of feasible set
With the designed control law and optimized , we conduct a simulation in a given space of initial states:
The range of the space is based on the possible user requirement. We simulated for two examples: case 1: backward docking to a chair, case 2: wheelchair docking to a car laterally. The configuration of and differs. For readability of the figure, we only show part of the simulated results in Fig. 4, we select the feasible set which meets requirements below: 1. The wheelchair converges to the virtual landmark. 2. Without violating HFOV constraint. For the second requirement, because the object is not a single point, in order to estimate the virtual landmark correctly, we need to keep the whole object including the edges inside HFOV. Those which meet the requirement are denoted by black arrows, those which do not meet the requirement are denoted by red arrows. The simulation results in Fig. 5 show the trajectories of the wheelchair, and Fig. 6 shows the relationship between and highlighting the convergence to the desired pose from the found feasible subset of initial states.
Then we can fit the boundary of the feasible set with inequalities below:
which is our ultimate feasible space , where , , , are positive parameters that scale the shape of the space, which is also related to , the example sets and trajectories of the ultimate feasible space are shown in Fig. 7.
We feed the fitted feasible space into in (14), validating that it is negatively definite in the feasible space. We also conduct the simulation with this fitted feasible space, all the trajectories could converge and is also within the . Thus we can say that the convergence and no violation of field of view are both guaranteed in this regulated control law.
Iii System Overview
The proposed system was implemented on the standing mobility vehicle Qolo [Paez2018] through a Jetson Xavier (Nvidia Corporation) selected as a single-board computer. A custom-developed circuit was designed to communicate and drive the in-wheel wheelchair controller (YAMAHA Motor Corp., Iwata, Japan). Communicating with the mainboard through Ethernet protocol and to the wheels driver by CAN-bus communication, as built in a previous work in [Leme2019].
The overall control architecture is presented in Fig. 8. For safety, the autonomous docking control was designed to be supervised by the user. This initial interface was proposed in [chen2020control] and the actuation method was explored in [chen2020holistic]. Since our proposed docking procedure does not rely on manual control, we conducted a preliminary study without a user on-board. The overall system was implemented within the Robot Operation System (ROS).
Iii-a Extended Kalman Filter
The well-known Extended Kalman Filter (EKF) is used to attenuate the random noises in the pose estimation process. Here we fuse the centroid calculated by the camera D435 and the visual odometry information obtained from a camera T265 (Intel, Santa Clara, CA, USA). The state variables used for filtering were chosen the Objective and virtual landmark w.r.t the wheelchair frame (Fig.3).
Iv Experiment Evaluation
We conducted real-time experiments to verify the effectiveness of the proposed autonomous docking support system. The camera was fixed on the robot Qolo with equal to . The goal object was a chair sized . The initial conditions were set to and for cases 1 and case 2 respectively. The experiment snapshot is shown in Fig. 10.
Iv-a Velocity Space Constraint
In order to confirm the feasibility of the proposed control law, first, we investigated the feasible velocity space of our standing mobility device Qolo, in order to understand the dead-zone of the robot, i.e., the velocity space limited by the internal wheel velocity controller.
Here we assume that the left and right wheel velocities have the same independent limits: , where and denote the minimum and maximum velocity, then we can relate the robot’s control through the kinematic model of a differential-drive robot [siegwart2011]:
where denotes half of the distance between two wheels. As shown in Fig. 9, the Simulated Velocity Space (SVS) of Qolo under the proposed control law is denoted in green lines. In here, the dead-zone means the robot could not completely converge to the virtual landmark, therefore, for the assessment we evaluated the landmark goal within this area.
Iv-B Experimental Setup
An experiment was carried out in an indoor environment to verify the feasibility of the proposed autonomous docking system. Different initial states were tested to verify the real feasible space. As when Qolo gets closer to the chair, the oscillation of the virtual landmark will cause a bigger effect on the angular velocity, thus, we used a two-phase estimation strategy here, when Qolo was far away from the virtual landmark, we use the Extended Kalman Filter proposed in section III-A, when Qolo gets over a threshold to the virtual landmark, we deactivated the virtual landmark estimation from depth camera side, and only use the tracking camera for odometry.
As there exists a dead-zone of the wheelchair velocity, so the robot could not converge perfectly, what’s more, we have not considered the dynamics of the robot, the effect from the casters, etc, so here we define a safety region of , only when the robot enters this region, we consider it as a successful docking. Practicality speaking, this small deviation should be acceptable for the user, who could adjust any final alignment.
With this in mind, we tested the system with different initial states, as shown in Fig. 13.
Iv-C Virtual Landmark Estimation
The virtual landmark estimation algorithm was validated by locating the robot at different relative poses to the chair (snapshots are shown in Fig. 11). Qualitatively, the chair is clearly recognized from the environment, bottom and back are separated well. From left to right, the robot gets closer to the chair, as we can see, the accuracy of the estimated virtual landmark decreases with the relative distance.
Iv-D Convergence Confirmation
In order to confirm the convergence of and that never violates , one example for each case is shown in Fig. 12, the trajectories of the robot are shown in Fig. 13. The plotted data were generated from the camera and control command of the system itself. For case 1, , and could converge to around 0.12m, -0.5 degrees and 4.5 degrees correspondingly. decreased from 22.2 degrees to 4.6 degrees remaining within . For case 2, , and could converge to around 0.11m, 1.8 degrees and 3.3 degrees correspondingly. decreased from 17.3 degrees to -5.5 degrees remaining within .
Overall results depicted in Fig. 14 describe the final states of the robot. For all cases , and entered the safety zone region defined above, the maximum along the trajectory is for case 1, and for case 2, therefore we can confirm that always remained within the .
In this work, an autonomous docking support approach for assistive mobility devices was proposed. The problem of keeping a real object inside the FOV of a camera and docking to a virtual landmark that is inferred from the object itself was defined by a novel description with generalized virtual landmark and camera parameters. A simple but effective virtual landmark estimation algorithm is designed and verified by placing the robot in a number of different initial postures. We also proposed a nonlinear feedback controller considering the limited HFOV of the camera, the convergence of the system could be guaranteed under selected and given constrained feasible space.
The proposed method is general, computationally low-cost and fully applicable to any mobility device such as wheelchair and standing mobility device, and equally to any differential-driven mobile robot. An experiment indoor environment showed the robot docking to a chair successfully from multiple initial states. There are also a few limitations with the proposed control law regarding the feasible space, but it could be assisted by the user’s action, which would be considered as sharing control, in this case, an indicator of the feasible space might be needed for the user.
Another limitation of the system is the usage of the tracking camera, with an accuracy depending on the texture of the surrounding environment, but it could be easily replaced or fused with other sensor sources such as wheel encoders. Future work will explore the proposed approach with user riding for evaluating the effectiveness of assistance during docking and the perception of the user to verify what needs could be mets.