From ac781ae7be0a6bbf31bf5176010a2ae1e22c3910 Mon Sep 17 00:00:00 2001 From: Yang Zhou Date: Sat, 31 Aug 2024 00:16:47 -0400 Subject: [PATCH] improve obstacle avoidance planner --- README.md | 20 ++++++++++--------- config/quad.yaml | 17 ++++++++++++---- src/lib.rs | 51 +++++++++++++++++++++++++++++++++--------------- 3 files changed, 59 insertions(+), 29 deletions(-) diff --git a/README.md b/README.md index 7598d018..fef6a84b 100644 --- a/README.md +++ b/README.md @@ -94,19 +94,21 @@ The markdown lines are used for generating the documentation. ```markdown =============================================================================== Language Files Lines Code Comments Blanks - SVG 2 492 480 0 12 +=============================================================================== + SVG 1 1 1 0 0 TOML 1 31 30 0 1 - YAML 1 94 86 0 8 + YAML 1 103 95 0 8 ------------------------------------------------------------------------------- - Markdown 4 273 0 207 66 - |- Markdown 1 17 0 17 0 - (Total) 290 0 224 66 + Markdown 4 267 0 201 66 + |- BASH 1 6 6 0 0 + |- Markdown 1 16 0 16 0 + (Total) 289 6 217 66 ------------------------------------------------------------------------------- - Rust 3 1487 1438 13 36 - |- Markdown 1 300 0 300 0 - (Total) 1787 1438 313 36 + Rust 3 1503 1455 12 36 + |- Markdown 1 303 0 303 0 + (Total) 1806 1455 315 36 =============================================================================== - Total 11 2377 2034 220 123 + Total 10 1905 1581 213 111 =============================================================================== ``` diff --git a/config/quad.yaml b/config/quad.yaml index 992aea8f..c7577eab 100644 --- a/config/quad.yaml +++ b/config/quad.yaml @@ -67,17 +67,26 @@ planner_schedule: center: [0.5, 0.5, 1.0] radius: 0.5 angular_velocity: 1.0 - duration: 8.0 + duration: 5.0 ramp_time: 2.0 - - step: 37000 + - step: 32000 + planner_type: MinimumJerkLine + params: + end_position: [-2.5, 0.0, 1.0] + end_yaw: 0.0 + duration: 3.0 + - step: 35000 planner_type: ObstacleAvoidance params: - target_position: [1.5, 1.0, 0.5] + target_position: [2.5, 1.0, 0.5] duration: 10.0 end_yaw: 0.0 k_att: 0.03 - k_rep: 0.02 + k_rep: 0.01 + k_vortex: 0.005 d0: 0.5 + d_target: 0.5 + max_speed: 0.1 - step: 45000 planner_type: MinimumSnapWaypoint params: diff --git a/src/lib.rs b/src/lib.rs index fdf55c67..f704bf64 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -670,22 +670,31 @@ pub struct ObstacleAvoidancePlanner { k_att: f32, /// Repulsive force gain k_rep: f32, + /// Vortex force gain + k_vortex: f32, /// Influence distance of obstacles d0: f32, + /// Influence distance of target + d_target: f32, + /// Maximum speed of the quadrotor + max_speed: f32, } impl Planner for ObstacleAvoidancePlanner { fn plan( &self, current_position: Vector3, - _current_velocity: Vector3, + current_velocity: Vector3, time: f32, ) -> (Vector3, Vector3, f32) { let t = ((time - self.start_time) / self.duration).clamp(0.0, 1.0); - // Attractive force towards the goal - let f_att = self.k_att * (self.target_position - current_position); + let distance_to_target = (self.target_position - current_position).norm(); + let f_att = self.k_att + * self.smooth_attractive_force(distance_to_target) + * (self.target_position - current_position).normalize(); // Repulsive force from obstacles let mut f_rep = Vector3::zeros(); + let mut f_vortex = Vector3::zeros(); for obstacle in &self.obstacles { let diff = current_position - obstacle.position; let distance = diff.norm(); @@ -694,11 +703,12 @@ impl Planner for ObstacleAvoidancePlanner { * (1.0 / distance - 1.0 / self.d0) * (1.0 / distance.powi(2)) * diff.normalize(); + f_vortex += + self.k_vortex * current_velocity.cross(&diff).normalize() / distance.powi(2); } } - let f_total = f_att + f_rep; - let max_speed: f32 = 1.0; - let desired_velocity = f_total.normalize() * max_speed.min(f_total.norm()); + let f_total = f_att + f_rep + f_vortex; + let desired_velocity = f_total.normalize() * self.max_speed.min(f_total.norm()); let desired_position = current_position + desired_velocity * self.duration * (1.0 - t); let desired_yaw = self.start_yaw + (self.end_yaw - self.start_yaw) * t; (desired_position, desired_velocity, desired_yaw) @@ -710,6 +720,15 @@ impl Planner for ObstacleAvoidancePlanner { } } +impl ObstacleAvoidancePlanner { + fn smooth_attractive_force(&self, distance: f32) -> f32 { + if distance <= self.d_target { + distance + } else { + self.d_target + (distance - self.d_target).tanh() + } + } +} /// Waypoint planner that generates a minimum snap trajectory between waypoints /// # Arguments /// * `waypoints` - A list of waypoints to follow @@ -777,11 +796,9 @@ impl MinimumSnapWaypointPlanner { j as f32 * (j - 1) as f32 * (j - 2) as f32 * duration.powi(j as i32 - 3); } } - let coeffs = a.lu().solve(&b).ok_or_else(|| { - SimulationError::NalgebraError( - "Failed to solve for coefficients in MinimumSnapWaypointPlanner".to_string(), - ) - })?; + let coeffs = a.lu().solve(&b).ok_or(SimulationError::NalgebraError( + "Failed to solve for coefficients in MinimumSnapWaypointPlanner".to_string(), + ))?; self.coefficients.push( (0..8) .map(|j| Vector3::new(coeffs[(j, 0)], coeffs[(j, 1)], coeffs[(j, 2)])) @@ -979,7 +996,10 @@ fn create_planner( obstacles: obstacles.clone(), k_att: parse_f32(params, "k_att")?, k_rep: parse_f32(params, "k_rep")?, + k_vortex: parse_f32(params, "k_vortex")?, d0: parse_f32(params, "d0")?, + d_target: parse_f32(params, "d_target")?, + max_speed: parse_f32(params, "max_speed")?, })), "MinimumSnapWaypoint" => { let mut waypoints = vec![quad.position]; @@ -997,9 +1017,7 @@ fn create_planner( coords.get(2)?.as_f64()? as f32, )) }) - .ok_or_else(|| { - SimulationError::OtherError("Invalid waypoint".to_string()) - }) + .ok_or(SimulationError::OtherError("Invalid waypoint".to_string())) }) .collect::, _>>()?, ); @@ -1007,12 +1025,12 @@ fn create_planner( yaws.extend( params["yaws"] .as_sequence() - .ok_or_else(|| SimulationError::OtherError("Invalid yaws".to_string()))? + .ok_or(SimulationError::OtherError("Invalid yaws".to_string()))? .iter() .map(|y| { y.as_f64() .map(|v| v as f32) - .ok_or_else(|| SimulationError::OtherError("Invalid yaw".to_string())) + .ok_or(SimulationError::OtherError("Invalid yaw".to_string())) }) .collect::, _>>()?, ); @@ -1528,6 +1546,7 @@ pub fn log_depth_image( /// * `gray` - The gray value in the range [0, 255] /// # Returns /// * The RGB color value in the range [0, 255] +#[inline] pub fn color_map_fn(gray: f32) -> (u8, u8, u8) { let x = gray / 255.0; let r = (34.61