A major challenge for integrated sensing and communication (ISAC) systems is the degradation in localization accuracy caused by unfavorable propagation conditions, such as multipath effects and non-line-of-sight (NLoS) environments. These conditions can introduce outlier measurements that significantly affect localization performance. This paper explores ways to enhance target localization accuracy in multistatic ISAC systems under both line-of-sight (LoS) and NLoS conditions. We utilize the positioning reference signal (PRS) as the sensing signal. A new algorithm is proposed to improve localization accuracy by reducing the influence of outliers in range measurements and PRS range resolution. Finally, simulation results confirm the superiority of the proposed method compared to least squares (LS) method.

Integrated sensing and communication (ISAC) is anticipated to play a crucial role in sixth-generation (6G) mobile communication networks. A significant challenge in ISAC systems is the degradation of localization accuracy due to poor propagation conditions, such as multipath effects and non-line-of-sight (NLoS) scenarios. These conditions result in outlier measurements that can severely impact localization performance. This paper investigates the enhancement of target localization accuracy in multistatic ISAC systems under both line-of-sight (LoS) and NLoS conditions. We leverage positioning reference signal (PRS), which is currently employed in fifth-generation (5G) new radio (NR) for user equipment (UE) positioning, as the sensing signal. We introduce a novel algorithm to improve localization accuracy by mitigating the impact of outliers in range measurements, while also accounting for errors due to PRS range resolution. Eventually, through simulation results, we demonstrate the superiority of the proposed method over previous approaches. Indeed, we achieve up to 28 % and 20 % improvements in average localization error over least squares (LS) and iteratively reweighted least squares (IRLS) methods, respectively. Additionally, we observe up to 16 % and 13 % enhancements in the 90th percentile of localization error compared to LS and IRLS, respectively. Our simulation is based on 3rd Generation Partnership Project (3GPP) standards, ensuring the applicability of our results across diverse environments, including urban and indoor areas.

We introduce a simple yet effective approach for dense depth reconstruction that operates directly on raw disparity data, eliminating the need for additional disparity refinement stages. By leveraging disparity maps generated from conventional stereo methods, we train a U-Net-based model to directly map disparity to depth, bypassing complex feature engineering. Our method capitalizes on the robustness of traditional stereo matching techniques to varying scenes, focusing exclusively on dense depth reconstruction. This approach not only simplifies the training process but also significantly reduces the requirement for large-scale training datasets. Extensive evaluations demonstrate that our method surpasses classical stereo matching frameworks and state-of-the-art classical post-refinement techniques, achieving superior accuracy. Additionally, our approach offers competitive inference times, comparable to classical as well as end-to-end deep learning methods, making it highly suitable for real-time robotic applications.

Stopping the motion of industrial robots in response to warnings or unexpected sensor data is a special case of trajectory generation. In contrast to emergency stops, here the robot has to satisfy the limits of the acceleration and the jerk. In addition, during the deceleration the robot must follow the path accurately, i.e., the shape of the original path may not be left. This is usually done by scaling the desired velocity. However, for curved paths, e.g. those generated by blending of linear motion commands, by sensor corrections, or directly by splines, this method may leave the desired path. The problem is solved by interpolation using the arc length. In contrast to other methods, here the constraints are considered directly, resulting in a time-efficient computation. Finally, the proposed method prevents a rebound caused by the jerk limits when reaching zero velocity. Experiments are presented using a stiff KUKA robot whose path is exactly tracked during deceleration.

This work integrates visual and physical constraints to perform real-time depth-only tracking of articulated objects, with a focus on tracking a robot's manipulators and manipulation targets in realistic scenarios. As such, we extend DART, an existing visual articulated object tracker, to additionally avoid interpenetration of multiple interacting objects, and to make use of contact information collected via torque sensors or touch sensors. To achieve greater stability, the tracker uses a switching model to detect when an object is stationary relative to the table or relative to the palm and then uses information from multiple frames to converge to an accurate and stable estimate. Deviation from stable states is detected in order to remain robust to failed grasps and dropped objects. The tracker is integrated into a shared autonomy system in which it provides state estimates used by a grasp planner and the controller of two anthropomorphic hands. We demonstrate the advantages and performance of the tracking system in simulation and on a real robot. Qualitative results are also provided for a number of challenging manipulations that are made possible by the speed, accuracy, and stability of the tracking system.

Autonomous robots operating in semi- or unstructured environments, e.g. during search and rescue missions, require methods for online on-board creation of maps to support path planning and obstacle avoidance. Perception based on stereo cameras is well suited for mixed indoor/outdoor environments. The creation of full 3D maps in GPS-denied areas however is still a challenging task for current robot systems, in particular due to depth errors resulting from stereo reconstruction. State-of-the-art 6D SLAM approaches employ graph-based optimization on the relative transformations between keyframes or local submaps. To achieve loop closures, correct data association is crucial, in particular for sensor input received at different points in time. In order to approach this challenge, we propose a novel method for submap matching. It is based on robust keypoints, which we derive from local obstacle classification. By describing geometrical 3D features, we achieve invariance to changing viewpoints and varying light conditions. We performed experiments in indoor, outdoor and mixed environments. In all three scenarios we achieved a final 3D position error of less than 0.23% of the full trajectory. In addition, we compared our approach with a 3D RBPF SLAM from previous work, achieving an improvement of at least 27% in mean 2D localization accuracy in different scenarios.

Teams of mobile robots can be deployed in search and rescue missions to explore previously unknown environments. Methods for joint localization and mapping constitute the basis for (semi-)autonomous cooperative action, in particular when navigating in GPS-denied areas. As communication losses may occur, a decentralized solution is required. With these challenges in mind, we designed a submap-based SLAM system that relies on inertial measurements and stereo-vision to create multi-robot dense 3D maps. For online pose and map estimation, we integrate the results of keyframe-based local reference filters through incremental graph SLAM. To the best of our knowledge, we are the first to combine these two methods to benefit from their particular advantages for 6D multi-robot localization and mapping: Local reference filters on each robot provide real-time, long-term stable state estimates that are required for stabilization, control and fast obstacle avoidance, whereas online graph optimization provides global multi-robot pose and map estimates needed for cooperative planning. We propose a novel graph topology for a decoupled integration of local filter estimates from multiple robots into a SLAM graph according to the filters' uncertainty estimates and independence assumptions and evaluated its benefits on two different robots in indoor, outdoor and mixed scenarios. Further, we performed two extended experiments in a multi-robot setup to evaluate the full SLAM system, including visual robot detections and submap matches as inter-robot loop closure constraints.

The travel range of a mobile robot is directly linked to its energy consumption. Apart from the actuators, the computing resources are the main energy consumers. Thus, robots that can navigate with minimal computational resources would be able to travel longer distances and, hence, would be valuable tools in applications such as search and rescue or planetary exploration. Inspired from the navigational abilities of insects, we developed the Trail-Map as a data structure for biologically inspired homing that can easily be scaled in case of memory or computational shortage. The Trail-Map can be built in constant time and enables constant time homing vector calculation. In this paper, we evaluate the Trail-Map-based homing performance of a simulated mobile robot equipped with an omnidirectional camera under the presence of sensor noise, such as odometry errors and observation errors. Further, we will show that Trail-Map-based homing outperforms SLAM methods in terms of computational resources while achieving a comparable homing performance.

The paper considers an a priori given robot trajectory which has to be recomputed when online sensed information on the environment is available. Then the original trajectory is adapted in order to continue the so far commanded motion by the sensed geometric shape. The adapted trajectory has to comply with restrictions on velocity, acceleration and jerk. Furthermore it is desired to converge to the original trajectory. At least if the robot is in contact with the environment it is further essential that the geometrical path is not left when modifying the trajectory. This means that preferably only the temporal profile is changed by scaling or rescaling the velocity. In order to inhibit overshooting, future restrictions are predicted and backtracked in the case of a violation. All this computation is done within a single sampling step, i.e. within 4 ms for a standard KUKA industrial robot. This precludes accurate optimization algorithms. When applied without an a priori given trajectory the method results in an near time-optimal solution.

The creation of local and global maps is crucial for (semi-)autonomous operation of mobile robots in previously unknown environments, e.g. during search and rescue missions. We developed an on-board stereo-vision based mapping system, thereby introducing local obstacle maps that can directly be used for fast local obstacle avoidance and path planning. In addition, we designed them to constitute a suitable input to a widely-used simultaneous localization and mapping (SLAM) algorithm. We performed experiments in unknown indoor, unstructured outdoor as well as mixed environments and demonstrated the applicability of our method to camera setups with small as well as wide field of view. In all three scenarios, we achieved a final 2D position error of less than 0.08% of the full trajectory.

Position-based force control is presented, incorporating compliance in the robot joints and possibly in a force- / torque-sensor and/or the environment. First, the total compliance is identified. Then, in the control phase, the desired pose of the tool center point is computed from the force control error. Thus standard position control may be applied. This leads to an inherently stable control scheme, even with a low sampling rate of the sensor interface and unknown environmental compliance. The method is designed for applications of industrial robots, e.g. assembly tasks. Parallel control considers the existence of a reference trajectory which allows feedforward in force controlled directions. The paper further examines couplings between forces and torques, which are important for partially constrained configurations. A possible impact force is considered when colliding with an unexpected object.

Map based navigation is a crucial task for any mobile robot. On many platforms this problem is addressed by applying Simultaneous Localization and Mapping (SLAM) based on metric grid-maps. Such solutions work well on robots with adequate resources and limited workspaces. Platforms with limited payload which operate in unbounded workspaces, do often have insufficient resources to keep a metric world representation. Nevertheless, many applications demand that the robot can autonomously navigate between different operation areas. In this work the Landmark-Tree map (LT-map), a resource efficient topological map concept, is for the first time applied to a mobile robotic platform equipped with an omnidirectional camera. It enables the robot to efficiently adapt the acquired map online to the available memory. During map acquisition and navigation the motion is estimated by the Z∞-algorithm. Both methods are based on similar concepts, which results in a mutual benefit. An efficient navigation strategy based on the LT-map allows the robot to reliably follow previously recorded paths. The presented approach is evaluated on a mobile robot in indoor and outdoor scenarios. The experiments prove its feasibility and show that pruning the map just smooths the trajectories, which is the expected and desired behaviour.

Active scene exploration incorporates object recognition methods for analyzing a scene of partially known objects and exploration approaches for autonomous modeling of unknown parts. In this work, recognition, exploration, and planning methods are extended and combined in a single scene exploration system, enabling advanced techniques such as multi-view recognition from planned view positions and iterative recognition by integration of new objects from a scene. Here, a geometry based approach is used for recognition, i.e. matching objects from a database. Unknown objects are autonomously modeled and added to the recognition database. Next-Best-View planning is performed both for recognition and modeling. Moreover, 3D measurements are merged in a Probabilistic Voxel Space, which is utilized for planning collision free paths, minimal occlusion views, and verifying the poses of the recognized objects against all previous information. Experiments on an industrial robot with attached 3D sensors are shown for scenes with household and industrial objects.

Efficient visual pose estimation plays an important role for a variety of applications. To improve the quality, the measurements from different sensors can be fused. However, a reliable fusion requires the knowledge of the uncertainty of each estimate. In this work, we provide an error analysis for the Z∞algorithm. Furthermore, we extend the existing first-order error propagation for the 8-point algorithm to allow for feature normalization, as proposed by Hartley or Mühlich, and the rotation matrix based decomposition. Both methods are efficient visual odometry techniques which allow high frame-rates and, thus, dynamic motions in unbounded workspaces. Finally, we provide experiments which validate the accuracy of the error propagation and which enable a brief comparison, showing that the Z∞significantly outperforms the 8-point algorithm. We also discuss the influence of the number of features, the aperture angle, and the image resolution on the accuracy of the pose estimation.

This paper presents a method for finding the largest, connected, smooth surface in noisy depth images. The formulation of the fitting in a Sample Consensus approach allows the use of RANSAC (or any other similar estimator), and makes the method tolerant to low percentage of inliers in the input. Therefore it can be used to simultaneously segment and model the surface of interest. This is important in applications like analyzing physical properties of carbon-fiber-reinforced polymer (CFRP) structures using depth cameras. Employing bivariate polynomials for modeling turns out to be advantageous, allowing to capture the variations along the two principle directions on the surface. However, fitting them efficiently using RANSAC is not straightforward. We present the necessary pre- and post-processing, distance and normal direction checks, and degree optimization (lowering the order of the polynomial), and evaluate how these improve results. Finally, to improve the initial estimate provided by RANSAC and to stabilize the results, an Expectation Maximization (EM) strategy is employed to converge to the best solution. The method was tested on high-quality data and as well on real-world scenes captured by a RGB-D camera.

This work is focused on global registration of surface models such as homogeneous triangle meshes and point clouds. The investigated approach utilizes feature descriptors in order to assign correspondences between the data sets and to reduce complexity by considering only characteristic feature points. It is based on the decomposability of rigid motions into a rotation and a translation. The space of rotations is searched with a particle filter and scoring is performed by looking for clusters in the resulting sets of translations. We use features computed from homogeneous triangle meshes and point clouds that require low computation time. A major advantage of the approach proves to be the possible consideration of prior knowledge about the relative orientation. This is especially important when high noise levels produce deteriorated features that are hard to match correctly. Comparisons to existing algorithms show the method's competitiveness, and results in robotic applications with different sensor types are presented.

We introduce our new quadrotor platform for realizing autonomous navigation in unknown indoor/outdoor environments. Autonomous waypoint navigation, obstacle avoidance and flight control is implemented on-board. The system does not require a special environment, artificial markers or an external reference system. We developed a monolithic, mechanically damped perception unit which is equipped with a stereo camera pair, an Inertial Measurement Unit (IMU), two processor-and an FPGA board. Stereo images are processed on the FPGA by the Semi-Global Matching algorithm. Keyframe-based stereo odometry is fused with IMU data compensating for time delays that are induced by the vision pipeline. The system state estimate is used for control and on-board 3D mapping. An operator can set waypoints in the map, while the quadrotor autonomously plans its path avoiding obstacles. We show experiments with the quadrotor flying from inside a building to the outside and vice versa, traversing a window and a door respectively. A video of the experiments is part of this work. To the best of our knowledge, this is the first autonomously flying system with complete on-board processing that performs waypoint navigation with obstacle avoidance in geometrically unconstrained, complex indoor/outdoor environments.

A different way of force control is presented, that is especially advantageous for position controlled robots. Instead of usual force control laws we rely on the well tuned position control loop and just use the force sensor to measure the target pose or to predict the desired trajectory. In combination with a compliant sensor we introduce an inherently stable framework of force control which almost inhibits all control errors. After an unexpected impact the force error is reduced independently from the sensor's bandwidth or delays in signal processing. Thus the (inevitable) impact force is more significant than the measured force control errors. The special case of a sensor that is mounted far away from a vertex-face contact is discussed, too.

We present a next-best-scan (NBS) planning approach for autonomous 3D modeling. The system successively completes a 3D model from complex shaped objects by iteratively selecting a NBS based on previously acquired data. For this purpose, new range data is accumulated in-the-loop into a 3D surface (streaming reconstruction) and new continuous scan paths along the estimated surface trend are generated. Further, the space around the object is explored using a probabilistic exploration approach that considers sensor uncertainty. This allows for collision free path planning in order to completely scan unknown objects. For each scan path, the expected information gain is determined and the best path is selected as NBS. The presented NBS approach is tested with a laser striper system, attached to an industrial robot. The results are compared to state-of-the-art next-best-view methods. Our results show promising performance with respect to completeness, quality and scan time.

System state estimation is an essential part for robot navigation and control. A combination of Inertial Navigation Systems (INS) and further exteroceptive sensors such as cameras or laser scanners is widely used. On small robotic systems with limitations in payload, power consumption and computational resources the processing of exteroceptive sensor data often introduces time delays which have to be considered in the sensor data fusion process. These time delays are especially critical in the estimation of system velocity. In this paper we present a state estimation framework fusing an INS with time delayed, relative exteroceptive sensor measurements. We evaluate its performance for a highly dynamic flight system trajectory including a flip. The evolution of velocity and position errors for varying measurement frequencies from 15Hz to 1Hz and time delays up to 1s is shown in Monte Carlo simulations. The filter algorithm with key frame based odometry permits an optimal, local drift free navigation while still being computationally tractable on small onboard computers. Finally, we present the results of the algorithm applied to a real quadrotor by flying from inside a house out through the window.

The procedure of manually generating a 3D model of an object is very time consuming for a human operator. Next best-view (NBV) planning is an important aspect for automation of this procedure in a robotic environment. We propose a surface-based NBV approach, which creates a triangle surface from a real-time data stream and determines viewpoints similar to human intuition. Thereby, the boundaries in the surface are detected and a quadratic patch for each boundary is estimated. Then several viewpoint candidates are calculated, which look perpendicular to the surface and overlap with previous sensor data. A NBV is selected with the goal to fill areas which are occluded. This approach focuses on the completion of a 3D model of an unknown object. Thereby, the search space for the viewpoints is not restricted to a cylinder or sphere. Our NBV determination proves to be very fast, and is evaluated in an experiment on test objects, applying an industrial robot and a laser range scanner.

Inertia-visual sensor fusion has become popular due to the complementary characteristics of cameras and IMUs. Once the spatial and temporal alignment between the sensors is known, the fusion of measurements of these devices is straightforward. Determining the alignment, however, is a challenging problem. Especially the spatial translation estimation has turned out to be difficult, mainly due to limitations of camera dynamics and noisy accelerometer measurements. Up to now, filtering-based approaches for this calibration problem are largely prevalent. However, we are not convinced that calibration, as an offline step, is necessarily a filtering issue, and we explore the benefits of interpreting it as a batch-optimization problem. To this end, we show how to model the IMU-camera calibration problem in a nonlinear optimization framework by modeling the sensors' trajectory, and we present experiments comparing this approach to filtering and system identification techniques. The results are based both on simulated and real data, showing that our approach compares favorably to conventional methods.

Precise and robust perception of the environment is crucial for highly integrated and autonomous robot systems. In this paper the dedicated design of a triangulation based laser range scanner optimized for 3D-modelling and autonomous exploration in robotics is presented. The presented laser scanner design is based on an extremely small MEMS scan head permitting a compact, lightweight and highly integrated implementation allowing for hand-eye operation. Special capabilities like variable range and confidence rating of the measuring values increase robustness. The design considerations and a prototype are described and experimental results are presented.

Because bin-picking effectively mirrors great challenges in robotics, it has been a relevant robotic showpiece application for several decades. In this paper we describe the computer vision algorithms in combination with the sophisticated control schemes of the robot and demonstrate a reliable and robust solution to the chosen problem. This paper approaches the bin-picking issue by applying the latest state-of-the-art hardware components, namely an impedance controlled lightweight robot and a Time-of-Flight camera. Lightweight robots have gained new capabilities in both sensing and actuation without suffering a decrease in speed and payload. Time-of- Flight cameras are superior to common proximity sensors in the sense that they provide depth and intensity images in video frame rate independent of textures. The bin-picking solution presented in this paper aims at extending the classical bin-picking problem by incorporating an environment model and allowing for the physical human-robot interaction during the entire process. Existing imprecisions in Time-of-Flight camera measurements and environment uncertainties are compensated by the compliant behavior of the robot. The overall process is implemented in a generic state machine that also monitors the entire bin-picking process.

We present a generalization of the Koenderink-van Doorn (KvD) algorithm that allows robust monocular localization with large motion between the camera frames for a wide range of optical systems including omnidirectional systems and standard perspective cameras. The KvD algorithm estimates simultaneously ego-motion parameters, i.e. rotation, translation, and object distances in an iterative way. However due to the linearization of the rotational component of optic flow, the original algorithm fails for larger rotations. We present a generalization of the algorithm to arbitrary rotations that is especially suited for omnidirectional cameras where features can be tracked for long sequences. This reduces the need for vector summation of several individual motion estimates that leads to accumulation of odometry errors. The significant improvement in the performance of the proposed generalized algorithm compared to the original KvD implementation is validated using simulated data. The algorithm is also tested in a real-world experiment with ground-truth data obtained from an external tracking system. The experiment was carried out using a novel compact omnidirectional camera that is designed for small aerial vehicles. It consists of an off-the-shelf webcam that is combined with a reflective surface machined into acrylic glass.

Accurate online localization is crucial for mobile robotics. In this paper, we describe a real-time image-based localization technique, which is based on a single calibrated camera. This can be supported by a second camera to improve accuracy and to provide the correct translational scale. Our goal is a robust and unbiased pose estimation in highly dynamic scenes on resource-limited systems. The presented approach is characterized through significantly improved robustness of the pose estimation, a novel approach for stereo subpixel accurate landmark initialization, and the speed-up of conventional tracking routines to achieve online capability. Although the algorithm is designed for accurate, online short-range egomotion estimation in hand-held scanning devices, it can be used for any mobile robot application as shown in this paper. Various tests and experimental results with a mobile platform and a hand-held 3D modeler are presented and discussed.

In the context of 3-D scene modeling, this work aims at the accurate estimation of the pose of a close-range 3-D modeling device, in real-time and passively from its own images. This novel development makes it possible to abandon using inconvenient, expensive external positioning systems. The approach comprises an ego-motion algorithm tracking natural, distinctive features, concurrently with customary 3-D modeling of the scene. The use of stereo vision, an inertial measurement unit, and robust cost functions for pose estimation further increases performance. Demonstrations and abundant video material validate the approach.

This paper deals with the concept and implementation of a multi-purpose vision platform. In robotics, numerous applications require perception. A multi-purpose vision platform suited for object recognition, cultural heritage preservation and visual servoing at the same time is missing. In this work, we draw attention to the design principles for such a vision platform. We present its implementation, the 3D-modeller. In specifying and combining multiple sensors, laser-range scanner, laser-stripe profiler and stereo vision, we derive the required mechanical and electrical hardware design. The concepts for synchronization and communication round offs our approach. Precision and frame rate are presented. We illustrate the versatility of the 3D-modeller by addressing four applications: 3D-modeling, exploration, tracking and object recognition. Due to its low weight and generic mechanical interface, it can be mounted on industrial robots, humanoids, or free-handed as well. The 3D-modeller is flexibly applicable, not only in research but also in industry, especially in small batch assembly.

The development of applications for multi-sensor data fusion typically faces heterogeneous hardware components, a variety of sensing principles and limited computational resources. We present a concept for synchronization and communication which tackles these challenges in multi-sensor systems in a unified manner. Here, a combination of hardware synchronization and deterministic software signals is promoted for global synchronization. Patterns of event-driven communication ensure that sensor data processing and evaluation are not bound to runtime constraints induced by data acquisition anymore. The combination of unified range and pose data description, event-driven communication, and global synchronization allows to build 3D sensing applications for various tasks. The proposed concept is implemented and evaluated for a variety of applications based on the DLR multisensory 3D-modeller. Extendability to other range and pose sensors is straightforward.

This paper presents the DLR Laser Stripe Profiler as a component of the DLR multisensory Hand-Guided Device for 3D modeling. After modeling the reconstruction process, we propose a novel method for laser plane self-calibration based on the assessment of the deformations the miscalibration leads to. In addition, the requirement for absence of optical filtering implies the development of a robust stripe segmentation algorithm. Experiments demonstrate the validity and applicability of the approaches.

The concept of C-space entropy as a measure of knowledge of C-space for sensor-based path planning and exploration for general robot-sensor systems was introduced in Yu, Y. and Gupta, K. (2000). The robot plans the next sensing action to maximally reduce the expected C-space entropy, also called the maximal expected entropy reduction, or MER criterion. The expected C-space entropy computation, however, made an idealized assumption. The sensor was assumed to measure exact data, i.e., it was not subject to noise. In this paper we extend this approach by using a real noisy sensor model. Sensing actions can then be compared on the basis of their uncertainty models. This offers the ability for using more than one principle sensor (multisensory exploration), because sensor readings can be weighted by evaluating the expected measurement quality. Additionally, it makes robot motion planning viable for tasks such as object surface inspection, which require the robot to come very close to the obstacles to achieve high sensing accuracy.