The subject matter of the present disclosure relates to methods and systems for robots and aerial vehicles capable of sensing a collision and recovering from the collision.
Robots and aerial vehicles experience collisions. Often the collisions damage the robots or aerial vehicles. There exists a need for collision-resilient robots and aerial vehicles that can recover from collisions and continue their tasks.
Consistent with the disclosed embodiments, an apparatus for use in an aerial vehicle is disclosed. The apparatus comprises an arm. The arm comprises a first substrate, a second substrate, and a flexible member having a longitudinal axis and a length, the flexible member to couple the first substrate to the second substrate. The apparatus comprises a sensing system mounted on the arm, the sensing system to detect changes in the length of the flexible member. In some embodiments, the flexible member comprises a prismatic joint. In some embodiments, the flexible member comprises a shock absorber. In some embodiments, the sensing system comprises a signal source mounted on the first substrate. In some embodiments, the signal source comprises a magnet. In some embodiments, the sensing system comprises a Hall effect sensor mounted on the flexible member.
Consistent with the disclosed embodiments, the apparatus comprises an aerial vehicle including an arm including a flexible member having a length, and a sensing system included in the aerial vehicle, the sensing system to detect a change in the length. In some embodiments, the sensing system includes a Hall effect sensor. In some embodiments, the arm includes a shock absorber adjusted such that the length of the arm remains substantially unchanged during free flight. In some embodiments, the shock absorber comprises a passive spring. In some embodiments, the apparatus further comprises a collision recovery control system communicatively coupled to the sensing system. In some embodiments, the collision recovery control system comprises a single stage collision recovery control system. In some embodiments, the aerial vehicle comprises a quadrotor. In some embodiments, the apparatus further comprises a protective structure mechanically coupled to the arm.
Consistent with the disclosed embodiments, a method comprises generating a collision signal for an aerial vehicle in response to a collision, and processing the collision signal to identify an axis of the aerial vehicle from which the collision signal originated. In some embodiments, generating the collision signal for the aerial vehicle in response to the collision comprises detecting compression of an arm of the aerial vehicle. In some embodiments, generating the collision signal for the aerial vehicle in response to the collision comprises processing a Hall effect sensor signal generated from a change in a length of a flexible member included in the aerial vehicle. In some embodiments, generating the collision signal for the aerial vehicle in response to the collision comprises detecting the collision without inertial measurement unit data. In some embodiments, the method comprises estimating deformation of the aerial vehicle resulting from the collision. In some embodiments, the method further comprises processing the collision signal to identify an intensity of the collision.
Consistent with the disclosed embodiments, the method comprises generating a collision signal from compression of a flexible member resulting from a mid-air collision of an aerial vehicle including the flexible member, and processing the collision signal and initiating execution of a recovery operation to sustain and resume flight of the aerial vehicle following the mid-air collision. In some embodiments, processing the collision signal and initiating execution of the recovery operation to sustain and resume flight of the aerial vehicle following the mid-air collision comprises processing the collision signal to identify an intensity of the mid-air collision. In some embodiments, the method further comprises maintaining a pre-collision thrust in the aerial vehicle during the collision and generating a new thrust to stabilize the aerial vehicle after the collision.
Consistent with the disclosed embodiments, an apparatus comprises an aerial vehicle including an arm including a flexible member having a length. The apparatus comprises a sensing system included in the aerial vehicle, the sensing system to detect a change in the length resulting from a mid-air collision and to generate a collision signal. The apparatus comprises a controller to receive the collision signal and execute a collision recovery operation for the aerial vehicle in which the aerial vehicle sustains and resumes flight following the mid-air collision. In some embodiments, the arm includes a prismatic joint.
Consistent with the disclosed embodiments, a method comprises receiving a collision signal after deformation of a collision-resilient robot from a collision. The method comprises recovering control of the collision-resilient robot after the collision. And the method comprises planning a post-collision trajectory for the collision-resilient robot using a global search-based planner. In some embodiments, recovering control of the collision-resilient robot after the collision comprises, for the collision-resilient robot having an orientation after the collision, maintaining the orientation throughout the collision recovery process. In some embodiments, planning the post-collision trajectory for the collision-resilient robot after the collision using the global search-based planner comprises using a post-collision state determined while recovering control of the collision-resilient robot after the collision as the initial state for post-collision trajectory generation. In some embodiments, planning the post-collision trajectory for the collision-resilient robot after the collision using the global search-based planner comprises, for the collision-resilient robot having a collision state, planning the post-collision trajectory when there is no direct line of sight between the collision state and a waypoint at an end of an immediately next trajectory segment following collision recovery.
Consistent with the disclosed embodiments, a collision resilient robot comprises a processor. The processor is configured to receive a collision signal after deformation of the collision-resilient robot from a collision. The processor is configured to recover control of the collision-resilient robot after the collision. And the processor is configured to plan a post-collision trajectory for the collision-resilient robot using a global search-based planner. In some embodiments, the collision signal is generated from a sensor embedded between a main chassis and a deflection surface of the collision resilient robot. In some embodiments, the global search-based planner uses information from the sensor to plan a post-collision trajectory for the collision-resilient robot. In some embodiments, the collision resilient robot of claim 30, wherein the collision-resilient robot has a field of view and the global search-based planner evaluates possible collisions within the field of view and outside the field of view to plan the post collision trajectory. In some embodiments, the global search-based planner adjusts one or more waypoints of a preplanned trajectory with information obtained from the collision to generate the post-collision trajectory plan. In some embodiments, the global search-based planner evaluates effects of possible collisions and determines when a preferred post-collision trajectory is to collide with a surface instead of avoiding the surface. In some embodiments, the collision resilient robot further comprises an arm to couple a main chassis of the collision resilient robot to a deflection surface.
It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the invention, as claimed.
The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and together with the description, serve to explain the principles of the invention.
The flexible member 108 couples the first substrate 104 to the second substrate 106. The flexible member 108 has a longitudinal axis 110 and a length 112. The flexible member 108 provides flexibility for the arm 102 along the longitudinal axis 110. The flexible member 108 returns to a resting length after the force causes compression of the flexible member 108 is removed. The flexible member 108 is not limited to a particular type of mechanical structure. In some embodiments, the flexible member 108 includes a prismatic joint. A prismatic joint is a coupling between two objects that allows one passive degree of freedom along a single axis. In operation, with the arm 102 including the flexible member 108 included in an aerial vehicle, the arm 102 allows compression of the flexible member 108 along the longitudinal axis 110 during a collision of the aerial vehicle. Compression of the flexible member 108 results in a change in the length 112 of the flexible member 108 and the distance between the first substrate 104 and the second substrate 106.
In some embodiments, the first substrate 104 and the second substrate 106 along with the flexible member 108 are a standalone unit that can be incorporated into the airframe of an aerial vehicle as shown in
The sensing system 114 is mounted on the arm 102. In some embodiments, the sensing system 114 includes a signal source mounted on the first substrate 104 and a signal detector mounted on the flexible member 108. In some embodiments, the signal source includes a magnet, and the signal detector includes a Hall-effect sensor. In operation, with the arm 102 included in an aerial vehicle, the sensing system 114 detects a change in the magnetic field at the Hall-effect sensor that results from a change in the length 112 of the flexible member 108 triggered by a collision of the aerial vehicle that results in compression of the flexible member 108.
The aerial vehicle 402 is not limited to a particular category of aerial vehicles. In some embodiments, the aerial vehicle 402 is an unmanned aerial vehicle such as those used for search and rescue, surveillance, and commercial applications. In some embodiments, the aerial vehicle 402 is a quadrotor—an unmanned aerial vehicle including four rotors. In some embodiments, the aerial vehicle 402 includes a protective structure 406 mechanically coupled to the arm 102. In operation, the protective structure 406 reduces the harmful effects of collisions on the aerial vehicle 402.
The sensing system 404 is not limited to a particular type of sensing system. In some embodiments, the signal source in the sensing system 404 is a magnet, such as the magnet 206 shown in
As shown in
Referring again to
In some embodiments, generating the collision signal for the aerial vehicle in response to the collision includes detecting compression of an arm of the aerial vehicle. In some embodiments, generating the collision signal for the aerial vehicle in response to the collision includes processing a Hall effect sensor signal generated from a change in a length of a flexible member included in the aerial vehicle. In some embodiments, generating the collision signal for the aerial vehicle in response to the collision includes detecting the collision without inertial measurement data.
In some embodiments, the method 500 further includes estimating deformation of the aerial vehicle resulting from the collision.
In some embodiments, the method 500 further includes processing the collision signal to identify an intensity of the collision. In some embodiments, the method further includes recovering from the collision. In some embodiments, recovering from the collision includes recovering from a wall collision. In some embodiments, recovering from the wall collision comprises minimizing the snap trajectory generation. In some embodiments, recovering from the wall collision comprises recovering from the wall collision where the aerial vehicle had a velocity of about 5.9 meters per second at a time slightly before the collision. In some embodiments, the method further includes maintaining a pre-collision thrust during the collision and generating a new thrust to stabilize the aerial vehicle after the collision.
In some embodiments, a method includes generating a collision signal from compression of a flexible member resulting from a mid-air collision of an aerial vehicle including the flexible member and processing the collision signal and initiating execution of a recovery operation to sustain and resume flight of the aerial vehicle following the mid-air collision. In some embodiments, processing the collision signal and initiating execution of the recovery operation to sustain and resume flight of the aerial vehicle following the mid-air collision includes processing the collision signal to identify an intensity of the mid-air collision. In some embodiments, the method further includes maintaining a pre-collision thrust in the aerial vehicle during the collision and generating a new thrust to stabilize the aerial vehicle after the collision.
The following components are included in the working prototype: Wi-Fi Module, Arduino Nano (for analog to digital conversion), Magnet, Hall Sensor, Prismatic Joint, Shock Absorber, Odroid XU4 (with ROS environment as the companion computer), Pixhawk 4 Mini (flight controller with the open-source PX4 autopilot), Power Distribution Board, Electronic Speed Controller, 5×5.3×3 Propeller, JB 2208 Motor—2400 k, Protective Cage (nylon).
The components of the quadrotor center on the quadrotor's compliant arm design and the integration of Hall sensors to estimate deformation resulting from a collision.
1) Compliant Arm Design: Unlike existing resilient flying robots whose physical configurations are drastically changed after collision, the configuration of quadrotor remains the same before and immediately after the collision. This key property makes the quadrotor an appropriate platform to conduct complex tasks that require model-based control. To meet this end, the design retains rigidity when in free flight while allowing for one passive degree of freedom in the direction along each arm.
The computer-aided design (CAD) assembly and main components of arm 102 are shown in
A close-up view of an arm 102 of the quadrotor before and during collision is shown in
2) Hall Sensor: The ability to detect and characterize collisions is critical to stabilize quadrotors and sustain flight. Existing collision-detection methods with inertial measurement units (IMUs) require detailed modeling of the environment and obstacles, which limits application in unknown environments. In this disclosure, the Hall-effect sensor 208 is adopted for collision detection.
Hall-effect sensors are commonly used to measure the magnitude of a magnetic field. The sensor's output voltage is directly proportional to the magnetic field strength through the sensor. In the disclosed prototype, A1302 ratiometric linear Hall-effect sensors are fixed on the prismatic joints, coming in contact with the magnet when the length of a shock absorber reduces to its minimum length (
Using one sensor per arm 102 allows determination of whether the quadrotor is in contact with an obstacle or not, and gives an approximation of the collision intensity. By utilizing four Hall-effect sensors, the quadrotor can approximate where the collision occurs in the body frame.
1) Notation: A world frame W is used with orthonormal basis
{xW, yW, zW} and a body frame B with orthonormal basis {xB, yB, zB}. The body frame is fixed to the quadrotor with the origin located at the center of mass (
2) Model: The quadrotor equations of motion are
{dot over (x)}=v (1)
m{dot over (v)}=mge
3
+fRe
3 (2)
{dot over (R)}=R{circumflex over (Ω)} (3)
j{dot over (Ω)}+Ω×JΩ=M (4)
where m is the mass and g is the gravitational acceleration constant. Unit vector e3 represents the direction of gravity in the world frame. Ω denotes the angular velocity in the body frame, and {circumflex over (Ω)} is the skew-symmetric matrix representation of Ω, f and M=[M1 M2 M3]T are the force and torque control inputs. The required thrust of each rotor fi can be determined by solving the system of equations
where l is the length of the arm and cf is the fixed constant for thrust generation.
When a collision happens, the arm length l will be reduced, therefore altering the required thrust (5). However, by taking advantage of Hall sensors, the quadrotor can wait until an elastic collision has been terminated, recovering arm length l and hence the system of equations to find the thrust for each motor. In the waiting time to recover the original arm length (less than one second), the fight control unit computes rotor thrusts according to the original arm length l and the motors continue rotating.
3) Controller: A geometric tracking controller is deployed for quadrotors. Control inputs f, M are chosen as
f=−(−kxex−kvev−mge3+m{umlaut over (x)}d)·Re3 (6)
M=−k
R
e
R
−k
Ω
e
Ω
+Ω×JΩ−J({circumflex over (Ω)}RTRdΩd−RTRd{dot over (Ω)}d) (7)
Tracking errors are given by
e
x
=x−x
d (8)
e
v
=v−v
d (9)
e
R=½(RdTR−RTRd)∨ (10)
e
Ω
=Ω−R
T
R
dΩd (11)
where the vee map ∨ is the inverse of a skew-symmetric mapping.
The software was developed in C++ using the ROS framework (
Three nodes are running within the Odroid. The first one is the MAVROS node to communicate with the PX4 firmware in Pixhawk via MavLink. The second node is the trajectory generation node. The trajectory generation node outputs the desired position, velocity, acceleration, yaw, and yaw rate to the geometric controller node. Finally, the controller node calculates and sends the desired thrust and attitude to the flight controller.
The Pixhawk 4 Mini runs in offboard mode. The inbuilt MAVROS function setpoint raw/attitude is called at 50 Hz to set motors commands according to the given thrust and attitude data. The Pixhawk runs an internal extended Kalman filter (EKF) to fuse IMU data and estimate the quadrotor's pose and orientation.
One Arduino Nano connects four Hall sensors and converts the analog signals to digital ones. The Arduino sends digital readings to the Odroid via UART at 200 Hz for collision detection and characterization.
The Hall-effect sensor outputs a voltage proportional to the magnetic field strength through it. The magnet is positioned at one side of the shock absorber and the Hall-effect sensor is positioned at the other end (
To estimate the intensity and location of the collision CB, the magnitude of CB needs to be calculated and orientation ΨB∉[−π, π] (positive orientation corresponds to counterclock direction). First, all vectors di are projected onto the basis xB and yB with di,x, di,y. For example, the projections of d1 can be written as
where θ=∠(d1, d2) represents the angle between the vectors d1 and d2. After adding up all projections, the collision can be characterized as follows
An effective collision detection requires both accuracy and an appropriate time of detection td to initiate recovery control. Unlike existing methods that often rely on certain thresholds to decide on collision occurrence and detection time, detection herein is based on an algorithm to measure the maximum collision intensity and time td.
The algorithm initiates with zero for the maximum collision intensity CB and false for the collision detection flag. While there is no collision detected, the loop reads data from the Hall-effect sensors at the current time j and calculates CB,j and ΨB;j as discussed previously. When a collision intensity greater than 0.1 is detected, the loop compares the current collision magnitude with the maximum one and saves the value if the current collision magnitude is greater. After a maximum collision magnitude is found, the algorithm starts counting for the loop and sets the collision flag true after N loops afterward, where N is selected so that td is close to the time when the lengths of the arms recover to satisfy the nominal length dynamic model.
Existing work in recovery control uses different stages to track, resulting in discontinuities in the overall trajectory. The disclosed recovery controller does not have multiple stages, leading to a smooth desired trajectory to track. This single-stage control design can improve the robustness of the recovery and be suitable to apply to collisions with a variety of objects such as walls and poles.
A minimum-snap trajectory generation is disclosed. Minimum-snap polynomial splines can be effective for quadrotor trajectory planning since the motor commands and attitude accelerations of the vehicle are proportional to the snap of the path. Minimizing the snap of a trajectory can maintain the quality of onboard sensor measurements while avoiding abrupt or excessive control inputs.
After a collision occurs, the quadrotor is made to return to a position xd that is in the opposite direction of the collision at a distance proportional to the collision intensity. The recovery position xd is constrained to be at the same height as the collision position. Thus, the desired position is found in the body frame xd,B as
where Rot(z. ΨB) is the rotation matrix along the z-axis by angle ΨB. Then, xd is calculated in the world frame by
x
d
=R
T
x
d,B (19)
For the flat output variables x, y, z and yaw angle, the yaw angle is fixed and only consider x=[xyz]T. A single trajectory segment between two points is composed of independent polynomials P(t) for x. A cost function penalizes the squares of fourth-order derivatives of P(t) as:
J=∫
0
T
c
4
P
(4)(t)2dt=pTQp (20)
In the expression, p is a vector of the N=10 coefficients of a single polynomial. Only the fourth-order derivative of P(t) is involved in the cost function in order to minimize snap. The construction of the Hessian matrix Q is known to those skilled in the art. In sum, the optimization is formulated as:
In this optimization, the constraint Ap=b imposes the endpoint constraints that x(0)=x0, {dot over (x)}(0)=v0, x(T)=xd where x0, v0 are the position and velocity of the quadrotor when collision happens and T is estimated based on the maximum acceleration and velocity. The velocity and acceleration at t=T are set to zero. Note the acceleration is not commanded at time t=0 since the feedback from the motion capture system has no acceleration data.
The collision-resilient performance of quadrotor was evaluated through five experiments: passive collision, wall collision, pole collision, colliding with unstructured surfaces, and free fall.
The quadrotor used in these experiments measured 380 mm from propeller tip-to-tip and 590 mm from protection cage tip-to-tip. A 3500 mAh 3-cell LiPo battery was used to power the quadrotor. The quadrotor weighed 1.419 kg with the battery and had a maximum thrust-to-weight ratio of 4.58. The average energy efficiency was 2.2 g/W. During the experiments, the actual thrust-to-weight ratio was limited to 2 by reducing the maximum pulse width modulation (PWM) values. A safety tether was connected the robot to prevent damages in case of losing control (
Existing work on collision detection and recovery assumes that the quadrotor operates within a static environment. In other words, the robot is the only moving object within the environment. However, as UAVs are tasked to explore more dynamic environments, the quadrotor can be hit by an obstacle when it is hovering. This type of collision as a passive collision.
Surviving passive collisions is challenging to the quadrotor because such type of collision is almost undetectable by using IMU data alone. There is no change in IMU readings before the collision happens, therefore all IMU based methods will fail in this case. Similarly, vision-based resilience is very likely to fail as well unless the colliding object is right inside the camera field of view and can be detected fast enough.
The quadrotor can handle passive collisions because the Hall sensors provide rapid collision detection while hovering. In the test, the quadrotor was hit along the direction of an arm while it was hovering. The recovery process is shown in
After the collision is detected and characterized, the quadrotor calculates a smooth stabilizing trajectory, the initial velocities of which match the velocities when recovery control begins. The geometric controller commands the quadrotor to track the desired trajectory. However, irregular changes in velocities were observed due to the body morphing (
In the wall test, a vertical wooden wall was covered by the EVA foam sheet in front of the quadrotor (
To clearly demonstrate the individual contribution of compliance and recovery control components, two additional wall tests were conducted with a collision speed of 2.3 m/s. In the first test, the arms remained compliant but the recovery controller was disabled. In the second test, the arms were fixed in place using custom jigs (these added 25 g of weight to the robot) while the recovery controller was disabled. Experimental trials showed that compliance alone can reduce collision impact, but cannot help sustain fight; the robot falls to the ground after getting stuck to the wall for several seconds because of lack of recovery control. In the second test, a worse impact was observed which makes the robot fall to the ground immediately. The two tests showed that compliance can reduce detrimental impacts from collisions while recovery control is essential to sustain fight afterward.
The quadrotor's capability of recovering from a collision with a cylindrical/pole-like object was demonstrated. The cylinder had a diameter of 300 mm.
Results demonstrate that the quadrotor can accurately detect pole collisions. The recovery process was very similar to the one with vertical walls, except the collision intensity (0.20) is much smaller, due to the shape of the obstacle. The recovery controller stabilized the robot with a hovering position close to the obstacle (
Unlike existing IMU-based methods, the quadrotor does not require prior knowledge of collision models to handle collisions. A collision test on an unstructured surface was performed to demonstrate this property. A metal folding step ladder was placed along a wall (
The quadrotor was commanded to hit the unstructured surface with a collision speed of 1.95 m/s. The process is illustrated in
To better demonstrate the benefit of adding compliance, a free fall test with ARQ (
In contrast to collision avoidance algorithms, we do not impose any obstacle-related constraints in trajectory generation, nor we run a geometric collision check once a trajectory is generated at the low level. Instead, we directly generate a trajectory based on given waypoints (The list of waypoints can be computed via any path planning method. It is independent from our proposed collision-inclusive planning algorithm). If a collision occurs, the robot receives a signal that a collision has occurred from any of the Hall effect sensors embedded between the main chassis and its deflection surfaces and activates a deformation recovery controller. The controller (Sec. IV) makes the robot detach from the collision surface by recovering from the deformation, and determines a post-collision state for the robot so as to facilitate post-impact trajectory replanning. The replanner (Sec. V) refines the initial trajectory since collisions change the second-order continuity of the trajectory followed before collision. To do so, the replanner uses the post-collision state determined by the recovery controller as the initial state for refined trajectory generation. The procedure repeats as new collisions may occur in the future, in a reactive and online manner. The DRR strategy (along with specific implementation components for experimental evaluation) is visualized in
Key notation is shown in Table. I.
b
wR
c
wR
The purpose of the deformation controller is to make the robot recover from a collision and reach a post-impact state that can facilitate recovery trajectory replanning (which is discussed in the next section).
Consider a holonomic mobile robot, modeled as a point mass m. The robot's main chassis is connected to its deflection surfaces via visco-elastic prismatic joints (
We use three coordinate systems. The world and body frames (note denotes the rotation matrix from body frame to world frame while bl denotes the deformation vector expressed in the body frame), and a (local) collision frame Fc. This frame is defined at the time instant a collision occurs, tc, remains fixed for the duration of the collision recovery process, T, and then it is deleted. Its origin coincides with the origin of the robot when a collision is detected. Basis vector {n, t, k} of Fc are defined normal, tangent and upwards with respect to the deformation vector l. Let θ be the angle of deformation vector l in Fc. The (frame-agnostic) robot collision dynamics is then given by
m{umlaut over (l)}+k
d
{dot over (l)}+k
e(l−l0)=main,
where ain is the robot's body acceleration input as provided by the robot's motors.
The deformation recovery controller's task is to steer the post-impact state of the robot to a desired one within a time period of [tc; tc+Tr]. The time horizon Tr is an important hyper-parameter tuned by the user. Typically, longer Tr means the robot will recover from collision with longer time and smoother motion pattern; we select Tr=0.5 s.
The deformation controller operates with respect to the local, collision frame Fc. Let the state variable be cs=[x y θ vx vy)]T. The control input is u=[ux uy uθ]T, where
with cω being the angular velocity of the robot in the collision frame. Note that position control terms include compensation for the force caused by the spring being pre-tensioned when the robot's arm is at its rest length. Then, the state space model of the robot recovering from collision can be expressed as
where f0=μkesign(vy)(cls−cl0)·cn.
Since the robot is holonomic, we can decouple orientation from position control. In our approach we seek to make the robot keep the same orientation it has at the instant it collides throughout the collision recovery process. We follow this approach because it can simplify the overall deformation recovery control problem without sacrificing optimality. The orientation and angular velocity errors during recovery time t∈[tc; tc+Tr] are eR (t)=½(RdT−RTRd)∨ and e{dot over (R)}(t)=ω−RTRdωd, respectively. The vee map v is the inverse of a skew-symmetric mapping. Indexd denotes desired quantities; these are Rd=R(tc) and ωd=[0 0 0]T. (All terms are with respect to collision frame Fc.) Then,
u
θ
=K
r
e
R,z(t)−Kωe{dot over (R)},z(t). (2)
Note that since this is a planar collision problem, the collision recovery orientation controller considers only the z-components of orientation and angular velocity errors.
Regarding collision recovery position control, note that the translation-only motion in (1) is affine. Thus, we can apply feedback linearization. The linearized system matrix F is
with state vector x=[x y θ vx vy]T. The control input matrix is G=I2×2 with control input vector v=[vx vy] given by
We formulate an optimal control problem with fixed time horizon T based on the linearized system {dot over (x)}=Fx+Gv. Using the change of variable τ=t−tc, we employ this change of variable for clarity. Problem (4) resets every time a new collision occurs; this gives rise to an LTI system, hence the change of variable can apply. We seek to solve
Matrices
and H=h I2×2 penalize the displacement during the recovery process and the control input, respectively. There is a trade-off between the displacement and the control input of the robot. Tuning parameters γ and h balance this trade-off to select the controller with minimal control energy and displacement.
Constraint (4c) dictates that the robot should be in contact with the collision surface until the colliding arm's spring has recovered its original, pre-tensioned length ls (i.e. the arm is no longer compressed) without compressing beyond its linear region le. Constraints (4d) and (4e) enforce initial and terminal position and velocity conditions, respectively. In detail, x0 is determined by the colliding arm's Hall effector sensor reading. Since the vector form of the sensor's reading (that is, bl−bls) is expressed in the body frame, we need transform it to the collision frame Fc as per
The velocity components at the collision instant v0,x and v0,y are expressed in frame Fc and are estimated at runtime. In the experiments conducted in this work, velocity measurements are provided via a motion capture camera system, but the method applies as long as velocity estimates are available, e.g., via optical flow. Post-impact, the arm needs to be uncompressed (hence XT is set to 0), but yT is treated as an unconstrained free variable. Post-impact terminal velocity components vT,x and vT,y are also expressed in Fc and can be set freely. Below, we discuss how to generate vT,x and vT,y based on the preplanned trajectory. We discretize the linearized system in (4b) with sampling frequency f=10 Hz using the Euler method, and solve the corresponding quadratic program with CVXOPT. The process is summarized in Alg. 1.
Computed control inputs (4) and (2) make the robot detach from the collision surface and help bring it to a temporary post-collision state which can be used as the initial condition for post-impact trajectory generation.
cvT,x ← 0
cvT,y ← vmaxnormalize(cvT)
We formulate the post-impact trajectory generation problem as a quadratic program with equality constraints. Let
where ic is the segment where the collision happens and NI is the number of trajectory segments. We seek to solve
Superscript q denotes the derivative order; for example, q={1, 2, 3, 4} correspond to min-velocity, min-acceleration, min-jerk and min-snap trajectories, respectively. Subscript β∈{x,y} indicates the x and y component of the trajectory, and Δti is the time duration for ith polynomial segment. Parameter ηi,β is the vector of coefficients of ith polynomial. wA0,i,β(α) maps the coefficients to αth order derivative of the start point in segment i, while wAΔt
Constraints (6b) and (6c) impose the initial values for the 0th and the 1st order derivatives to match the position and velocity values attained via the collision recovery controller, respectively. Constraint (6d) imposes that the αth order derivatives of the end position are fixed. Constraint (6e) imposes that the trajectory will pass through desired waypoints after Constraint (6f) is imposed to ensure αth continuity among polynomial segments.
We solve this quadratic programming (QP) problem given initial (post-collision) and end states, and intermediate waypoints. Then, we perform time scaling to reduce the maximum values for planned velocities, accelerations and higher-order derivatives as appropriate, and thus improve dynamic feasibility of the refined post-impact trajectory.
The solution of the QP problem serves as the initial value for GTO where we change the objective function to
min λsJs+λoJo+λd(Jv+Ja), (7)
where Jo is the cost to avoid collisions, and Jv and Ja are the penalties when candidate velocity and acceleration solutions exceed the dynamic feasibility limit, respectively. Weight parameters λs, λo and λd trade off between smoothness, trajectory clearance and dynamical feasibility, respectively.
We use an exponential cost function. At a position with distance d to the closest obstacle, the cost co(d) is written as
c
o(d)=αo exp(−d−do)/ro, (8)
where αo is the magnitude of the cost function, do is the threshold where the cost starts to rapidly rise, and ro controls the rate of the function's rise. Then, Jo can be computed as
Jv can be computed in a similar manner, whereby cv(v) is the cost function applied on the velocity and attains the same form as in (8). We can then obtain
The formulation of Ja is similar to (10). The cost function of the acceleration constraint ca(a) is also an exponential function similar to cd(d) and cv(v), since it is can penalize when close to or beyond acceleration bounds while staying flat when away from the bounds. We apply a Newton trust region method to optimize the objective.
In some cases, it may be necessary to adjust the waypoints given by a preplanned trajectory with the information obtained from the collision, and then solve the aforementioned problem in Sec. V-A with the adjusted waypoints. Such cases occur when there is no direct line of sight between the collision state and the waypoint at the end of the immediately next trajectory segment following collision recovery. By enabling such waypoint adjustment, the algorithm promotes exploration and in certain cases prevents the robot from being trapped in a local minima in which repeated collisions at the same (or very close-by) place could otherwise occur.
With reference to Alg. 2, we express in the local collision frame Fc the next waypoint wwaypoint_list[ic+1] (lines 2-4). In line 5, we adjust wwaypoint_list with the information we get from collision. Details of this process are shown in
If the waypoint at the end of ic segment lies on the opposite direction of cpr, we insert a new intermediate waypoint in the waypoint list. The waypoint is generated by displacing cpr by an ‘exploration distance’ ∈explore expressed in Fc as
wwaypoint_list
wpnext ← wwaypoint_list[ic + 1]
wwaypoint_list as FIG. 21
In this section, we propose the main algorithm to generate the waypoint list and trajectory segments that serve as the input to the DRR strategy.
Let x(t)∈⊂n×q be a dynamical system state, consisting of position and its (q−1) derivatives (velocity, acceleration, jerk, etc.) in 2D (n=2) or 3D (n=3). Let Xfree⊂X denote the free region of the state space that, in addition to capturing the obstacle-free positions Pfree, also specifies constraints Dfree on system dynamics, i.e. minimum and maximum velocity vmin, vmax, acceleration amin, amax, jerk jmin, jmax and higher order derivatives in each axis. Note that Pfree is bounded by the size of the environment. Thus, Xfree=Pfree×Dfree=Pfree×[vmin, vmax]×[amin, amax]× . . . . We denote the obstacle region as Pobs=P\Pfree and Xobs=Pobs×Dfree.
The differential flatness of mobile robot systems allows us to construct control inputs from 1D time-parameterized polynomial trajectories specified independently in each of the n position axes (n=2 or n=3). Thus, we consider polynomial state trajectories
x(t)=[pD(t)T,{dot over (p)}D(t)T, . . . ,pD(q−1)(t)T]T
where
and D=[d0, . . . , dq]⊂n×(q+1), and di=[di,xdi,y]T in (6). To simplify the notation, we denote the system's velocity by v(t)={dot over (p)}DT(t), acceleration by a(t)={umlaut over (p)}DT(t), jerk by j(t)=DT(t), etc., and drop the subscript D where convenient.
Polynomial trajectories can be generated via a linear time-invariant dynamical system pD(q)(t)=u(t), where the control input is u(t)∈=[−umax,umax]n⊂n. In state space form we obtain {dot over (x)}(t)=Afx(t)+Bfu(t), with
In collision-inclusive motion planning, we consider a smoothness cost
The trajectory is not qth order differentiable as it would be in a collision-avoidance problem formulation. The smoothness of the entire trajectory is the summation of the qth order differentiable segments of the entire trajectory. We consider two additional costs. First,
penalizes duration of the overall trajectory. Then,
evaluates the effect of a collision in changing the direction of motion of the robot. ΔEβ=(cvβ+−cvβ−)2, where cvβ+=cvβ(t+Tr) and cvβ−=cvβ(t). cvβ+ can be approximated via Alg. 4 if pgoal is known. (We discuss Alg. 4 in detail in Sec. VI-D.) We also define an indicator function ζ(t)={0,1} that signals if the robot is colliding at time instant t.
We can then define the optimization problem
Parameters ρi>0 and ρc>0 regulate the relative importance of trajectory smoothness, duration, and amount of collisions that switch the direction of motion. Conditions (13e) and (13f) determine how the value for ζ(t) is being set. In (13i), vc=[−vmax,c, vmax,c]; vmax,c indicates the maximum collision velocity which, if exceeded, will lead to the robot flipping over. Thus, we set the pre-collision velocity component along x axis of Fc as vx (t)∈Vc.
In this paper, we show that, similar to the collision avoidance motion planning problem, these safety constraints can be handled by converting the problem to a deterministic shortest path problem with a n×q dimensional state space X and a n dimensional control space u. Since the control space u is always n dimensional, a search based planning algorithm such as A* that discretizes u using motion primitives is efficient and resolution-complete, i.e. it can compute the optimal trajectory in the discretized space in finite-time, unlike sampling-based planners.
First, we construct motion primitives for the system (11) that will allow us to convert the motion planning problem from an optimal control problem to a graph-search problem. Instead of using the control set u, we consider a lattice discretization uM={u1, . . . , uM}, where each control uM∈n vector defines a motion of short duration for the system. One way to obtain the discretization uM is to choose a number of samples μ∈+ along each axis [−vmax,c, vmax,c], which defines a discretization step:
and result in M=(2r+1)n motion primitives. Given an initial state x0=[p0T, v0T, . . . ]T, we generate a motion primitive of duration τ>0 that applies a piece-wise constant control
where um∈M for t∈[0, τ]. With an initial condition x0,
is a piece-wise function. Equivalently, the resulting trajectory of the linear time invariant system (11) is
This discretization of the state space allows us to construct a graph representation of the reachable system states by starting at x0 and applying all primitives to obtain the M possible states after a duration of τ∈[0, τf]. (see Alg. 3). Applying all possible primitives to each of the M states again, will result in M2 possible states at time 2τ. Since the free space Xfree is bounded and discretized, the set of reachable states S is finite. This defines a graph (S, ε), where S is the discrete set of reachable system states and ε is the set of edges that connect states in the graph, each defined by a motion primitive e=(ũm, τ, c) with c an integer value (discussed in Sec. VI-D).
We use Alg. 3 to explore the free state space Xfree and build the connected graph: in line 4, the primitive is calculated using the fully defined state s and a control input um given the constant time upper-bound τf; lines 6-29 check whether the primitive intersects with the obstacles and then modify those primitives intersecting with the obstacles. This step will be further discussed Sec. VI-D. Line 14 shows that we consider Tr for the robot recovering from the collision using DRR in the cost function. In lines 7-17, we modify the end state of the primitive and add it to the set of successors of the current node; meanwhile, we estimate the edge cost related to from the corresponding modified primitive. Further modification of the cost function about estimating the cost related to Jc part will be discussed in Sec. VI-D. In lines 20-28, we evaluate the end state of a valid primitive not intersecting with the obstacles and we add it to the set of successors of the current node; meanwhile, we estimate the
edge cost from the corresponding primitive. After checking through all the primitives in the finite control input set, we add the nodes in successor set R(x) to the graph, and we continue expanding until we reach the goal region.
Given the set of motion primitives uM and the induced space discretization discussed in the previous section, we can re-formulate problem (13) as a graph-search problem. This can be done by introducing additional constraints for the control input u(t) in (13) to be piecewise-constant. We introduce an additional variable N∈+, so that
and ũk is computed by (14) with uk∈uM for k=0, . . . , N−1 and a constraint in (13h):
We define
to force the control trajectory to be a composition of the motion primitives in uM, leading to the following deterministic shortest path problem. Given an initial state x0∈Xfree a goal region Xgoal and a finite set of motion primitives UM with duration τ>0, choose a sequence of motion primitive u0:N−1 of length N such that:
The optimal cost of (17) is an upper bound to the optimal cost of (13) because (17) is a constrained version of (13). However, this reformulation to discrete control and state-spaces enables an efficient solution. Such problems can be solved via search-based or sampling-based motion planning algorithms. Since only the former can guarantee finite time (sub-)optimality, we use an A* method similar to and focus on the design of efficient, guaranteed collision checking and post-collision behavior classifying methods, and an accurate, consistent heuristic as follows.
For a computed edge e(t)=[p(t)T v(t)T a(t)T . . . ]T, in Alg. 3, we need to check if e(t)∈Xfree for all t∈[0, τf]. For e(t)∈free∧e(t+δt)∈obs with δt→0 for all t∈[0, τf], we need to modify the edge e(t) as in lines 6-16 in Alg. 3. We check collisions in the geometric space free⊂n separately from enforcing dynamic constraints free⊂n×(q−1). An edge e(t) is collision-free only if its geometric shape pe(t)∈Pfree for all t∈[0, τf].
In general, determining collision points for each motion primitive can be very challenging. In this work, we model P as an occupancy grid map Mo. Other representations such as polyhedral maps are also possible but are usually hard to obtain from a robot's FoV sensor data (from LIDAR) and hence not pursued herein. Let Pe={pe(ti)|ti∈[0,τf], i=1, . . . I} be a set of positions that the system traverses along the trajectory. For collision-free primitives we need pe(ti)∈Pfree for all i∈{0, . . . I}. The duration of the collision-free trajectory is τ=τf.
For the given polynomial pe(t), t∈[0, τf], the positions pe(ti) are sampled by defining
such that
where ∈map is the occupancy grid resolution, and parameter vmax is calculated as vmax=max{|vmin|, |vmax|}. The condition ensures that the maximum distance between two consecutive samples will not exceed the map resolution. Since it is an approximation, some cells that are traversed by pe(t) with a portion of the curve within the cell shorter than ∈map may be missed, but it guarantees the collision-free trajectory not hitting any obstacles.
In not collision-free e(t), the estimated collision time instance ti is when pe(ti)∈free∧pe(ti+δt)∈obs with δt≈ti+1−ti for all i∈{0, . . . I−1}. In this case, we set the duration τ of the collision-inclusive motion primitives in Alg. 3 as ti. Then, we modify the end state xe of this e(t) as xe={e−, xe+} with xe−=e(ti)=[pe(ti)T ve(ti)T ae(ti)T . . . ]T. We set the duration of this edge τ=ti and set ζ(τ)=1. xe+=FDRR(xe−) is the post-impact state recovered using the DRR strategy. We discuss how to set xe+ shortly.
Since v, a and other higher-order derivatives are polynomial functions, we can compute their extrema within the time period [0, τ] to check if the respective maximum bounds are violated. For n≤3, the order of these polynomials is less than 5, which means we can solve for the extrema in closed form. We prune those primitives which are not dynamically feasible (i.e. any bounds are exceeded). For the collision-inclusive primitives, we need to check the x component of the velocity ve− in Fc corresponding cve,x−∈. We prune those with cve,x−∉ to prevent the robot from flipping over after colliding.
To generate the frame Fc required for evaluating the collision-inclusive primitives, we need to get the geometric information of each obstacle that the robot collides on. Therefore, we need to generate a grid map Minf with the same resolution of the occupancy map to record the geometric information of the obstacles. With the information we get from a FoV-based sensor (e.g., LIDAR), we can create a list of obstacles in the field. Then, we create a grid map that can map the position grid ((xi, yi) for 2D map) to a list which stores the geometric features of the convex obstacles. In practice we use the open-source obstacle detector package to create two kinds of obstacle lists according to the sensor data: circle shape obstacles and line segment shape obstacles (which can be used to create convex polygon obstacles). The geometric information map Minf will help us generate Fc. Basis vectors of Fc are generated as discussed in Sec. IV-A whereas the origin of Fc is set to be the estimated position of collision pe− in xe−.
After generating Fc, we are able to generate xe+ based on the geometric information of the obstacle obsc which we predict the robot will collide on when arriving at xe− with the given motion primitive. Given the goal position pgoal, we are able to set wxe+ according to Alg. 4. This way, we can ensure the trajectory generated by the search based algorithm respect constraint (17g). In
We set a lower bound to Jc, J−c, to ensure that there is a cost if the robot tries to use collisions alone to steer. Tuning ρc can help regulate between collision avoidance and collision-inclusive planned trajectories.
We also create an infeasible, Pinf, area to link pruned collision-inclusive pe−∈inf and collision-free primitives pe∈inf. We apply Pinf to prevent the robot from getting into areas where the collisions are difficult to detect using this arm design (i.e. when collision surfaces reduce to almost a point, such as obstacle corners).
Devising an efficient graph search for solving (17) requires an approximation of the optimal cost function (a heuristic function) that is admissible, informative (i.e. provides a tight approximation of the optimal cost), and consistent (i.e. it can be inflated in order to obtain solutions with bounded suboptimality efficiently. We obtain a good heuristic function by solving a relaxed version of (13). The main idea is to replace constraints in (13) that are difficult to satisfy, namely, x(t)∈free and u(t)∈, with a constraint on the time T. In this section, we show that such a relaxation of problem (13) can be solved optimally and
wxe− = [wpe−T wυx−T wax−T . . . ]T;
wpgoal, obsc):
cυx+ 0
wpadd ← ∅
wpe+ wpc−
wxe+ ← [wpe+ T wυx+ T wax+ T . . .]T
efficiently. If padd≠∅, we add a constraint to ensure that the robot will pass the waypoint padd and avoid colliding with the same obstacle multiple times, that is
p(Tadd)=padd,∀Tadd∈[0,Tg]. (19)
1) Lower Bound of Time: Intuitively, the constraints on maximum velocity, acceleration, jerk, etc due to Xobs and u induce a lower bound T on the minimum achievable time in (13). For example, since the system's maximum velocity is bounded by vmax along each axis, if padd=∅, the minimum time for reaching the closest state xgoal in the goal region Xgoal is bounded below by
The system's maximum acceleration is bounded by amax, hence the state xgoal=[pgoalT vgoalT] cannot be reached faster than
The above is a minimum-time optimal control problem with input constraints, which can be solved in closed form along each individual axis to obtain the lower bound Ta=min{Ta,x, Ta,y, Ta,z}. This procedure can be continued for constraints higher-order derivatives, but in practice the computed times are less likely to provide better bounds than the previous ones while requiring higher computational effort. Hence, even though we can define a lower bound on the minimum achievable time via T=min{Tv Ta, . . . }, for simplicity we use the efficiently computed (but less tight) bound T=Tv. For those cases with padd≠∅, we generate T1 and T2 for path segments p0→padd and
2) Velocity Control Linear Quadratic Minimum Time Heuristic Function:
The lower bound T can be used to relax optimization problem (13) by replacing the state and input constraints. If padd=∅, we can deduce
The relaxed problem (21) is in fact the classical Linear Quadratic Minimum-Time Problem. The optimal cost generated from (21) according to some commentators is:
h(x0)=δTTWT−1δT+ρtTg. (22)
We define δT=xgoal−eA
Let us consider velocity control as an illustrative example of (22). Given Tg, x0=p0, xgoal=pgoal, we can rewrite the optimal cost of (21) shown in (22) as
Here C* represents the optimal cost which turns out to be a polynomial function of Tg. We are able to derive the optimal Tg* by minimizing C* in (23) with constraint Tg*≥T. The solution of this optimization problem is the positive real root of
if the positive real root root+≥T. Otherwise, Tg*=T. Furthermore, the optimal cost is C*(Tg*). For the case where padd≠∅, we modify (23) to
Similarly, we are able to derive the optimal T1* and T2* by minimizing C* in (24) with constraints T1*≥T1 and T2*≥T2.
We can get the solution of this optimization problem by solving the positive real root of
The optimal cost then is C*(T1*, T2*)
In the previous subsections from Sec. VI-A to Sec. VI-E, we introduced the overall structure of our proposed collision inclusive search-based motion planning algorithm that is based on A* graph search. From (17), we notice that we extend the feasible set of the optimization problem compared to the collision avoidance planning problem. Extending the feasible set forces our method to traverse more nodes on the graph compared to the collision avoidance method. Even though our method can generate a less conservative result with less cost compared to the collision avoidance method, the computational time of our proposed method is larger than the collision avoidance method.
To improve computational efficiency and reduce the computational time of our method, we can replace the postimpact motion primitive generation technique introduced in Sec. VI-D in A* graph search with a more efficient variant that is inspired by jump point search. Specifically, we notice that when the robot needs to add a new waypoint between the collision point pe− of the motion primitive and the goal pgoal (c=2), we can modify pe+=padd. Performing this modification will help us eliminate traversing multiple nodes with the same padd. This way, the number of nodes we are traversing can reduce, thus reducing computational time. Even though applying this technique can result in sacrificing the optimality of the solution, solving the planning problem with less computational time is more important in practice.
If colliding with a convex obstacle (as shown in
The cost will be updated with new τ←τ+τadd. When we go through edges with c=2, we split the trajectory of this given edge with two segments, given the start and the end waypoints as p0 and pe− for the first segment, pe− and pe+ for the second segment. The time duration of the first segment is τ−τadd and the time duration of the second segment is τadd. We set the c=0 and ζ=0 with respect to the waypoint pe+.
The resulting trajectory based on the previous sections gives not only the collision-free path, but also the time for reaching the respective waypoints. Thus, we are able to use it as a prior to generate a smoother trajectory in higher dimension for controlling the actual robot. The refined trajectory x*(t) is derived from solving a gradient-based trajectory generation problem similar to the one in Sec. VA with given initial and end states x0 and xgoal and intermediate waypoints pk, k∈{0, 1, . . . , N}. The time for each trajectory segment τk is also given from the prior trajectory. All pk are stored in waypoint list required for Alg. 2. The differential variable of the waypoint with ck≥1 is fixed variable instead of free variable in the collision-free method, which indicates
x
k
=x
k
−
,i∈{0,1, . . . ,Nc}ck≥1,k∈{0,1, . . . ,N}
We also delete obstacles that the robots planned to collide on (ck≥1) when computing the potential field for trajectory generation to simplify calculations. The trajectory after refinement is n-th order continuous. However, the differential of the actual output trajectory changes when a collision occurs, indicating the dynamics of the robot change rapidly during the collision-recovery process. Yet, the overall output trajectory will always be continuous. We can also simplify the waypoint list by deleting waypoints that are collinear with the previous and next waypoints. We reallocate the duration for the simplified segments as the sum of durations of segments before merging. It is important to note that even though the refinement step produces a smoother trajectory, the refined course might be dynamically infeasible; we need to perform time scaling to reduce the maximum dynamics of the refined trajectory. The refined trajectory might collide with the obstacle in the trajectory segment that is checked to be collision free according to Sec. VI-D. In such cases, our DRR strategy ensures robustness and safety.
In this section, we validate the effectiveness of our unified framework for collision-inclusive motion planning and control by presenting several benchmark testing results in simulation and via physical experimentation with our robot. 2) First, we test the deformation controller on the robot to test its performance and generate a post-collision model which is required in simulation. 2) Second, we test the local DRR trajectory generation component experimentally with our robot. 3) Then, we test our global planning method in a double corridor environment and compare it with state-of-the-art search-based collision avoidance and sampling-based collision-inclusive methods. 4) After that we test the overall planning strategy in unknown environment. 5) Lastly, we implement the overall method on our impact-resilient robot and test it in a real corridor environment.
Testing the deformation controller (Sec. VII-B) and DRR strategy (Sec. VII-C) experimentally takes place in a 2.0×2.5 m area with a rectangular pillar serving as a static polygon-shaped obstacle. For testing the overall method (Sec. VII-F) experimentally, we consider a 2.5×3.5 m area with a long rectangular pillar right in the middle to create a U-shaped single corridor environment.
We use the two active omni-directional impact-resilient wheeled robots we built in-house. The main chassis is connected to a deflection ‘ring’ via 4 or 8 arms that feature a visco-elastic prismatic joint each. Each arm has embedded Hall effect sensors to measure the length of the arm and detect collisions along each of their direction when the deformation exceeds a certain threshold. In physical experiments, odometry feedback is provided by a 12-camera VICON motion capture system. An onboard Intel NUC mini PC (2:3 GHz i7 CPU; 16 GB RAM) processes odometry data and sends control commands to the robot at 10 Hz. The robot is equipped with a single-beam LIDAR (RPLidar A2) with 8 m range to detect the obstacles in the environment.
The robot may flip when colliding with velocities over a bound. To identify a theoretical collision velocity bound to avoid flipping, we use an energy conservation argument. Assume the kinetic energy before collision transfers into elastic potential energy of the arm, and the gravitational potential energy of the robot with small flipping angle counters the negative work input from the controller, i.e.
E
k,t−(vmax)=Eep(le)−Eep(ls)+Egp(σmax)+main,max(ls−lc)
Then.
The robot's radius is 0.3 m. The difference between the initial and neutral position of each arm is ls=30.0 mm, the maximum load length is le=15.0 mm, and the neutral length lo=41.5 mm. The spring coefficient is k=2.31 N/mm. We set the largest flip angle σmax=3°. For the 4-arm robot, the maximum acceleration input is ain,max=5.0 m/s2, and its mass is 6.0 kg. Then, we compute an upper theoretical velocity bound of vmax≈0.7 m/s. The 8-arm robot features motors with higher torque and different gear ratio that increase ain;max and despite the mass increase to 8 kg, the same upper theoretical velocity bound remains valid.
Simulated comparison against other methods (Sec. VIID) takes place in a double-corridor environment, whereas simulated benchmark testing of our method when noise is added takes place in the same double-corridor environment but with added isolated obstacles added as well (Sec. VII-E).
We use a rigid cylinder body to emulate the robot. A numerical model is generated from the experiments for the deformation recovery controller to determine the output velocity after collision. The output velocity is generated by adding uniform random noise to the reference velocity cvT. Then, we use a ray-casting algorithm to emulate the LIDAR (we consider the range of the LIDAR can cover all visible operating space). We implement simulation benchmarks in a python environment. All simulations run on a workstation
with Intel Core Xeon-E2146G CPU.
To examine the deformation controller's effect in local trajectory generation, we command the robot to collide with an obstacle and then apply the proposed deformation recovery controller. We perform 10 trials of various input-output velocity combinations. Collision detection is very accurate; only 9 out of 249 were not detected.
Results suggest that the deformation controller generates a negative velocity to make the robot detach from the obstacle after collision. Actual output velocity
We test our DRR strategy with a trajectory generated based on using the online safe trajectory generation method with time scaling without collision checking. We compare the strategies in two cases: 1) when the previous path does not intersect with the collision surface; and 2) when the previous path intersects with the collision surface.
Case 1 tests the condition in
Even though we design a collision-free desired trajectory, the robot may still collide with the environment given for instance unmodeled dynamics such as drift. In case 1 there are 3 out of 10 trials that the robot in fact collides with the obstacle applying trajectory generation that aims to avoid collisions. Table II shows statistics on mean arrival times, path lengths and control energy.
end [s]
In case 1 for DRR, mean arrival times
To test our proposed framework in simulation, we first benchmark it in a double corridor environment to test our search-based collision-inclusive global planner. We compare our method for global planning against two methods: 1) a search-based collision-avoidance motion planning algorithm, and 2) an RRT*-based (sampling-based) collision inclusive planning algorithm. Note that no open-source python code is available for either, so we implemented both ourselves to the best possible extent for fair comparisons.
In all tests, the dynamic limits are set as amax=5.0 m/s2. The holonomic robot only translates but does not rotate during the process. Constant orientation is maintained via a separate stabilizing controller. We set the upper bound of the robot velocity vmax=2.0 m/s. The cost function in all methods considers ρt=1.0. The overall size of the map is 70×70 m. The position resolution of the grid map in the benchmark is 1.0 m, and the position resolution of the velocity map in the benchmark is 0.5 m/s. The time interval for each motion primitive is set as τ=5.0 s and the resolution of acceleration r=1.0 m/s2. We set λs=0.5, λo=1.0 and λv=0.1.
Results from testing the global planner are shown in
Table III contains more detailed results and also presents comparisons against the sampling-based (RRT*) method. First, both our collision-inclusive method and collision avoidance n generate kinodynamically-feasible trajectories. When the initial velocity is) v0=[00]T m/s and the parameter ρc=1.0, our method with jump points tends to generate a path with the shortest duration compared to both the collision inclusive planner without jump points and the collision avoidance planner. However, the control cost for doing so is slightly higher. The computational time of the collision inclusive planner with jump points is the second-lowest among all the methods. Comparing these results with those obtained by an RRT* method we find that—in spite that sampling-based methods can solve certain problems that are hard or impossible to solve using graph search—the search-based planning method is more suitable for collision inclusive planning in the configuration space 2. When there is non-zero initial velocity, v0[2 2]T>m/s, we find that our proposed collision-inclusive search-based method with jump points tends to generate paths with lowest control energy and time duration. The computational time of our proposed method is also the lowest among all search-based algorithms. Taken together, we deduce that the search-based collision-inclusive method with jump points can serve best as the global planner in our unified collision-inclusive motion planning and control framework.
We first test our proposed unified collision-inclusive motion planning and control strategy in a double corridor environment with online sensing, and compare its performance against that of a collision avoidance framework. In the collision avoidance framework, the global planner includes a search-based method; we also make the optimistic assumption of treating the unknown space as free. The local planner is the trajectory generation method based on gradients and time duration adjustment. We design a backup safety trajectory to make sure the robot will stop at the frontier. Then, we test both methods in a double corridor environment populated with circular isolated obstacles of increasing density. In both scenarios, each method is run for 10 times with the same initial configuration and parameter settings.
Note that we test with and without estimation noise in the global planner. Added position estimation noise is zero-mean truncated Gaussian with variance of 0.3 and bounds of ±0.9. Added velocity estimation noise is zero-mean truncated Gaussian with variance of 0.1 and bounds of ±0.3. Detailed comparative results are presented in
With reference to
Further, success rates of the proposed collision-inclusive method and collision avoidance are shown in
Finally, we validate our proposed framework experimentally, and also test is against the collision avoidance strategy in Sec. VII-E, in a single corridor environment (
By implementing our proposed collision-inclusive planning method, the robot can reach the goal area with higher success rates since unmodeled dynamics in real robot planning and control make the robot collide with the obstacle even if the reference trajectory generated from collision avoidance method is designed to be collision-free. Further, by utilizing collisions, the robot can reach the goal faster while requiring less control energy by trading off the average path length.
The application describes a unified collision-inclusive motion planning and control framework applied for navigation in unknown environment. A global search-based method is devised to generate a path which contains explicit information about collisions. The effect of the collisions is explored in the global planner. The local planner is enhanced by a lower-level deformation recovery control and trajectory replanning strategy, which enables the robot to detect and recover from collisions and move toward the goal. The deformation controller is designed based on the dynamics of the robot, which herein is a holonomic omni-directional wheeled robot. This part can be easily swapped out with another robot model if the robot dynamics change, while other parts of the overall method stay the same. The planning system was evaluated extensively through several benchmark comparisons in simulation as well as via physical experimental testing. The proposed collision-inclusive planning method is implemented in simulation first and then integrated with state estimation, mapping and control into our custom-made robot platform to check the feasibility of the method in physical world experiments. Results show that the proposed method is robust and capable to generate fast and safe trajectories compared to collision-avoidance methods.
In the preceding specification, various example embodiments have been described with reference to the accompanying drawings. It will, however, be evident that various modifications and changes can be made thereto, and additional embodiments may be implemented based on the principles of the present disclosure. The specification and drawings are accordingly to be regarded in an illustrative rather than restrictive sense.
For example, advantageous results still could be achieved if steps of the disclosed techniques were performed in a different order or if components in the disclosed systems were combined in a different manner or replaced or supplemented by other components. Other implementations are also within the scope of the following example claims.
This application claims the benefit of priority of U.S. Provisional Application Ser. No. 63/106,772 filed on Oct. 28, 2020, which application is incorporated by reference herein.
This invention was made with government support under 1910087 awarded by the National Science Foundation. The government has certain rights in the invention.
Filing Document | Filing Date | Country | Kind |
---|---|---|---|
PCT/US2021/057128 | 10/28/2021 | WO |
Number | Date | Country | |
---|---|---|---|
63106772 | Oct 2020 | US |