diff --git a/nav_test_lidar_dynamic_v2.1.py b/nav_test_lidar_dynamic_v2.1.py index e0b00de..05bb3b9 100644 --- a/nav_test_lidar_dynamic_v2.1.py +++ b/nav_test_lidar_dynamic_v2.1.py @@ -16,28 +16,19 @@ from sensor_msgs.msg import Image, LaserScan from std_msgs.msg import String # ── Настраиваемые коэффициенты ────────────────────────────────────────────── -K_VISUAL = 0.55 # Мягче визуальная коррекция, чтобы снизить рыскание -K_VISUAL_DECAY = 0.85 # Быстрее забываем старую visual error, если маркеры пропали -VISUAL_FILTER_ALPHA = 0.65 # Сглаживание визуальной ошибки -VISUAL_ENABLE_THRESH = 0.015 -K_HEADING = 0.95 # Сильнее удерживаем целевой yaw на прямой -K_CROSS_P = 1.35 # Коррекция бокового ухода по одометрии -CROSS_TRACK_FILTER_ALPHA = 0.75 -MAX_ANGULAR = 0.28 # Чуть уменьшаем макс. угловую скорость на прямой -ANGULAR_RATE_LIMIT = 0.03 # Ограничение резкой смены angular.z за цикл +K_VISUAL = 0.8 # П-коэффициент визуального cross-track (агрессивная коррекция) +K_VISUAL_DECAY = 0.92 # Коэффициент затухания визуальной ошибки (память) +K_HEADING = 0.6 # П-регулятор удержания курса +K_CROSS_P = 2.0 # Пропорциональный коэффициент odom cross-track +MAX_ANGULAR = 0.35 # Максимальная скорость поворота (рад/с) K_TURN = 1.5 # П-регулятор поворота на месте -DRIVE_SPEED = 0.24 # Базовая скорость движения вперёд (м/с) -MIN_DRIVE_SPEED = 0.11 # Минимальная скорость на прямой при коррекции -RECOVERY_SPEED = 0.08 # Осторожная скорость при большом отклонении +DRIVE_SPEED = 0.3 # Скорость движения вперёд (м/с) CELL_SIZE_M = 0.65 # Предполагаемая длина одной клетки поля (м) -HEADING_SLOWDOWN_RAD = math.radians(10.0) -CROSS_TRACK_SLOWDOWN_M = 0.05 -VISUAL_SLOWDOWN_ERR = 0.18 # ── Параметры проверки препятствия лидаром ────────────────────────────────── LIDAR_FORWARD_HALF_ANGLE_DEG = 15.0 # Проверяем узкий сектор строго перед роботом -LIDAR_MIN_VALID_RANGE_M = 0.12 # Игнорируем слишком близкие шумные значения -LIDAR_BLOCK_DISTANCE_M = 0.5 # Если впереди объект ближе этого порога — следующая клетка занята +LIDAR_MIN_VALID_RANGE_M = 0.05 # Игнорируем слишком близкие шумные значения +LIDAR_BLOCK_DISTANCE_M = 0.85 # Если впереди объект ближе этого порога — следующая клетка занята LIDAR_CONFIRM_CYCLES = 1 # Сколько циклов подряд нужно подтвердить препятствие # ──────────────────────────────────────────────────────────────────────────── @@ -102,11 +93,6 @@ class RMC2Navigator(Node): self._last_front_range = None self._occupancy_check_after_turn_pending = False - # Стабилизация прямолинейного хода. - self._filtered_cross_track = 0.0 - self._last_angular_cmd = 0.0 - self._last_visual_source = 'none' - # Таймер главного цикла (20 Гц) self.timer = self.create_timer(0.05, self.control_loop) @@ -395,7 +381,7 @@ class RMC2Navigator(Node): self._line_start_x, self._line_start_y = self.get_marker_coords(self.current_marker_id) self._line_perp_yaw = self.target_yaw + math.pi / 2.0 self._cross_track_ref_marker = self.current_marker_id - self._reset_drive_stabilization() + self._visual_err_x_memory = 0.0 self._reset_lidar_block_detector() self._occupancy_check_after_turn_pending = True @@ -408,62 +394,15 @@ class RMC2Navigator(Node): def _cross_track_error(self, x, y): """ Рассчитывает signed cross-track error — расстояние от текущей позиции - до идеальной линии движения. + до идеальной линии движения (с указанием знака: + справа, − слева). + + Линия: проходит через (_line_start_x, _line_start_y) под углом target_yaw. """ dx = x - self._line_start_x dy = y - self._line_start_y cross_track = -dx * math.sin(self.target_yaw) + dy * math.cos(self.target_yaw) return cross_track - def _reset_drive_stabilization(self): - self._visual_err_x_memory = 0.0 - self._filtered_cross_track = 0.0 - self._last_angular_cmd = 0.0 - self._last_visual_source = 'none' - - def _limit_step(self, target, current, max_step): - if target > current + max_step: - return current + max_step - if target < current - max_step: - return current - max_step - return target - - def _select_visual_guidance_error(self, ids_list, corners, img_w, img_h): - if not ids_list: - return None, 'none' - - preferred_markers = [] - if self.waypoint_idx < len(self.waypoints): - preferred_markers.append((self.waypoints[self.waypoint_idx], 'next_wp')) - if self.current_marker_id is not None: - preferred_markers.append((self.current_marker_id, 'current_wp')) - if self.target_marker_id is not None: - preferred_markers.append((self.target_marker_id, 'target')) - - seen = set() - for marker_id, label in preferred_markers: - if marker_id in seen: - continue - seen.add(marker_id) - ex, ey = self._marker_offset(marker_id, ids_list, corners, img_w, img_h) - if ex is not None: - return ex, label - - best_err_x = None - best_score = float('inf') - for mid in ids_list: - ex, ey = self._marker_offset(mid, ids_list, corners, img_w, img_h) - if ex is None: - continue - score = abs(ex) + 0.7 * abs(ey) - if score < best_score: - best_score = score - best_err_x = ex - - if best_err_x is None: - return None, 'none' - return best_err_x, 'nearest_visible' - # ── ArUco-детектор (общий, переиспользуемый) ──────────────────────────── def _detect_markers(self): @@ -614,54 +553,32 @@ class RMC2Navigator(Node): self.cmd_pub.publish(twist) return - visual_err_x, visual_source = self._select_visual_guidance_error( - ids_list, corners, img_w, img_h - ) + visual_err_x = None + if ids_list: + best_dist = float('inf') + for mid in ids_list: + ex, ey = self._marker_offset(mid, ids_list, corners, img_w, img_h) + if ex is None: + continue + dist = abs(ex) + abs(ey) + if dist < best_dist: + best_dist = dist + visual_err_x = ex if visual_err_x is not None: - self._visual_err_x_memory = ( - VISUAL_FILTER_ALPHA * self._visual_err_x_memory - + (1.0 - VISUAL_FILTER_ALPHA) * visual_err_x - ) - self._last_visual_source = visual_source + self._visual_err_x_memory = visual_err_x else: self._visual_err_x_memory *= K_VISUAL_DECAY - self._last_visual_source = 'none' base_yaw_err = self.normalize_angle(self.target_yaw - self.current_yaw) - raw_cross_track = self._cross_track_error(self.current_x, self.current_y) - self._filtered_cross_track = ( - CROSS_TRACK_FILTER_ALPHA * self._filtered_cross_track - + (1.0 - CROSS_TRACK_FILTER_ALPHA) * raw_cross_track - ) - raw_angular = ( - K_HEADING * base_yaw_err - - K_CROSS_P * self._filtered_cross_track - ) - - use_visual = abs(self._visual_err_x_memory) > VISUAL_ENABLE_THRESH + use_visual = abs(self._visual_err_x_memory) > 0.01 + twist.linear.x = DRIVE_SPEED if use_visual: - raw_angular -= K_VISUAL * self._visual_err_x_memory - - raw_angular = max(-MAX_ANGULAR, min(MAX_ANGULAR, raw_angular)) - twist.angular.z = self._limit_step(raw_angular, self._last_angular_cmd, ANGULAR_RATE_LIMIT) - self._last_angular_cmd = twist.angular.z - - heading_ratio = min(1.0, abs(base_yaw_err) / HEADING_SLOWDOWN_RAD) - cross_ratio = min(1.0, abs(self._filtered_cross_track) / CROSS_TRACK_SLOWDOWN_M) - visual_ratio = min(1.0, abs(self._visual_err_x_memory) / VISUAL_SLOWDOWN_ERR) - slowdown = max(heading_ratio, cross_ratio, visual_ratio) - twist.linear.x = DRIVE_SPEED - (DRIVE_SPEED - MIN_DRIVE_SPEED) * slowdown - - if ( - abs(base_yaw_err) > math.radians(18.0) - or abs(self._filtered_cross_track) > 0.09 - or abs(self._visual_err_x_memory) > 0.24 - ): - twist.linear.x = min(twist.linear.x, RECOVERY_SPEED) - - twist.linear.x = max(RECOVERY_SPEED, min(DRIVE_SPEED, twist.linear.x)) + twist.angular.z = K_HEADING * base_yaw_err - K_VISUAL * self._visual_err_x_memory + else: + twist.angular.z = K_HEADING * base_yaw_err + twist.angular.z = max(-MAX_ANGULAR, min(MAX_ANGULAR, twist.angular.z)) if not hasattr(self, '_cross_log_counter'): self._cross_log_counter = 0 @@ -669,10 +586,9 @@ class RMC2Navigator(Node): if self._cross_log_counter % 10 == 0: lidar_msg = 'n/a' if self._last_front_range is None else f'{self._last_front_range:.2f} m' self.get_logger().info( - f'[STAB] vis={self._visual_err_x_memory:+.3f} ({self._last_visual_source}), ' - f'cross={self._filtered_cross_track:+.3f} m, ' + f'[VIS-MEM] err_x={self._visual_err_x_memory:+.3f}, ' f'yaw={math.degrees(base_yaw_err):+.1f}°, ' - f'v={twist.linear.x:.2f}, w={twist.angular.z:+.3f}, lidar={lidar_msg}' + f'tw.z={twist.angular.z:+.3f}, lidar={lidar_msg}' ) # ── Визуальное центрирование над финальным маркером ─────────────────