As deployment of automated vehicles increases, so does the rate at which they are exposed to critical traffic situations. Such situations, e.g. a late detected pedestrian in the vehicle path, require operation at the handling limits in order to maximize the capacity to avoid an accident. Also, the physical limitations of the vehicle typically vary in time due to local road and weather conditions. In this paper, we tackle the problem of trajectory planning and control at the limits of handling under time varying constraints, by adapting to local traction limitations. The proposed method is based on Real Time Iteration Sequential Quadratic Programming (RTI-SQP) augmented with state space sampling, which we call Sampling Augmented Adaptive RTI-SQP (SAA-SQP). Through extensive numerical simulations we demonstrate that our method increases the vehicle's capacity to avoid late detected obstacles compared to the traditional planning tracking approaches, as a direct consequence of safe operating constraint adaptation in real time.
An intuitive way to reduce suboptimality of the trajectories in the set is to solve the two-point boundary value problem offline, using a dynamic model. This method has been demonstrated successfully in several previous works @cite_3 @cite_16 . However, this approach prohibits online model adaptation, since the trajectories in the pre-computed library assumes a static vehicle model.
@cite_16 @cite_3
1925320057 2735010494 2032541004 2116364955
Common techniques in structure from motion do not explicitly handle foreground occlusions and disocclusions, leading to several trajectories of a single 3D point. Hence, different discontinued trajectories induce a set of (more inaccurate) 3D points instead of a single 3D point, so that it is highly desirable to enforce long continuous trajectories which automatically bridge occlusions after a re-identification step. The solution proposed in this paper is to connect features in the current image to trajectories which discontinued earlier during the tracking. This is done using a correspondence analysis which is designed for wide baselines and an outlier elimination strategy using the epipolar geometry. The reference to the 3D object points can be used as a new constraint in the bundle adjustment. The feature localization is done using the SIFT detector extended by a Gaussian approximation of the gradient image signal. This technique provides the robustness of SIFT coupled with increased localization accuracy. Our results show that the reconstruction can be drastically improved and the drift is reduced, especially in sequences with occlusions resulting from foreground objects. In scenarios with large occlusions, the new approach leads to reliable and accurate results while a standard reference method fails. We propose a real-time depth edge based RGB-D SLAM system for dynamic environment. Our visual odometry method is based on frame-to-keyframe registration, where only depth edge points are used. To reduce the influence of dynamic objects, we propose a static weighting method for edge points in the keyframe. Static weight indicates the likelihood of one point being part of the static environment. This static weight is added into the Intensity Assisted Iterative Closest Point (IAICP) method to perform the registration task. Furthermore, our method is integrated into a SLAM (Simultaneous Localization and Mapping) system, where an efficient loop closure detection strategy is used. Both our visual odometry method and SLAM system are evaluated with challenging dynamic sequences from the TUM RGB-D dataset. Compared to state-of-the-art methods for dynamic environment, our method reduces the tracking error significantly. This paper presents a new perspective for 3D scene understanding by reasoning object stability and safety using intuitive mechanics. Our approach utilizes a simple observation that, by human design, objects in static scenes should be stable in the gravity field and be safe with respect to various physical disturbances such as human activities. This assumption is applicable to all scene categories and poses useful constraints for the plausible interpretations (parses) in scene understanding. Given a 3D point cloud captured for a static scene by depth cameras, our method consists of three steps: (i) recovering solid 3D volumetric primitives from voxels; (ii) reasoning stability by grouping the unstable primitives to physically stable objects by optimizing the stability and the scene prior; and (iii) reasoning safety by evaluating the physical risks for objects under physical disturbances, such as human activity, wind or earthquakes. We adopt a novel intuitive physics model and represent the energy landscape of each primitive and object in the scene by a disconnectivity graph (DG). We construct a contact graph with nodes being 3D volumetric primitives and edges representing the supporting relations. Then we adopt a Swendson---Wang Cuts algorithm to partition the contact graph into groups, each of which is a stable object. In order to detect unsafe objects in a static scene, our method further infers hidden and situated causes (disturbances) in the scene, and then introduces intuitive physical mechanics to predict possible effects (e.g., falls) as consequences of the disturbances. In experiments, we demonstrate that the algorithm achieves a substantially better performance for (i) object segmentation, (ii) 3D volumetric recovery, and (iii) scene understanding with respect to other state-of-the-art methods. We also compare the safety prediction from the intuitive mechanics model with human judgement. We describe and implement an algorithm for computing the set of reachable states of a continuous dynamic game. The algorithm is based on a proof that the reachable set is the zero sublevel set of the viscosity solution of a particular time-dependent Hamilton-Jacobi-Isaacs partial differential equation. While alternative techniques for computing the reachable set have been proposed, the differential game formulation allows treatment of nonlinear systems with inputs and uncertain parameters. Because the time-dependent equation's solution is continuous and defined throughout the state space, methods from the level set literature can be used to generate more accurate approximations than are possible for formulations with potentially discontinuous solutions. A numerical implementation of our formulation is described and has been released on the web. Its correctness is verified through a two vehicle, three dimensional collision avoidance example for which an analytic solution is available.