📖 第01课：SLAM概述

import numpy as np import matplotlib.pyplot as plt # ============================================================ # 2D EKF-SLAM Simulation # State: [x, y, θ, m_x1, m_y1, ..., m_xM, m_yM] # ============================================================ class EKFSlam2D: def __init__(self, n_landmarks=10): # State: [x, y, θ, lx1, ly1, ..., lxM, lyM] self.n_lm = n_landmarks self.dim = 3 + 2 * n_landmarks # State vector and covariance self.x = np.zeros(self.dim) self.P = np.zeros((self.dim, self.dim)) # Initial pose uncertainty self.P[0:3, 0:3] = np.diag([0.01, 0.01, 0.001]) # Landmark uncertainty: large initially self.P[3:, 3:] = np.eye(2 * n_landmarks) * 1000.0 # Motion noise self.R = np.diag([0.05, 0.05, 0.01]) # [Δx, Δy, Δθ] # Observation noise self.Q = np.diag([0.1, 0.05]) # [range, bearing] # Track which landmarks have been observed self.observed = [False] * n_landmarks def predict(self, u): """EKF predict step with motion model u=[Δx, Δy, Δθ] x_new = x + R(θ) * [Δx, Δy]^T θ_new = θ + Δθ """ dx, dy, dtheta = u theta = self.x[2] # Compute Jacobian of motion model w.r.t. pose # x' = x + cos(θ)*Δx - sin(θ)*Δy # y' = y + sin(θ)*Δx + cos(θ)*Δy # θ' = θ + Δθ Fx = np.zeros((3, self.dim)) Fx[0:3, 0:3] = np.eye(3) # Jacobian of motion w.r.t. [x, y, θ] Gx = np.array([ [1, 0, -np.sin(theta)*dx - np.cos(theta)*dy], [0, 1, np.cos(theta)*dx - np.sin(theta)*dy], [0, 0, 1] ]) # Jacobian of motion w.r.t. control noise V = np.array([ [np.cos(theta), -np.sin(theta), 0], [np.sin(theta), np.cos(theta), 0], [0, 0, 1] ]) # Update state mean self.x[0] += np.cos(theta) * dx - np.sin(theta) * dy self.x[1] += np.sin(theta) * dx + np.cos(theta) * dy self.x[2] += dtheta # Update covariance: P' = G*P*G^T + V*R*V^T G = np.eye(self.dim) G[0:3, 0:3] = Gx self.P = G @ self.P @ G.T + Fx.T @ V @ self.R @ V.T @ Fx def update(self, z_list): """EKF update with landmark observations. z_list: [(landmark_id, range, bearing), ...] """ for lm_id, r_obs, b_obs in z_list: lx_idx = 3 + 2 * lm_id ly_idx = 4 + 2 * lm_id # Predicted landmark position lx = self.x[lx_idx] ly = self.x[ly_idx] dx = lx - self.x[0] dy = ly - self.x[1] q = dx**2 + dy**2 sqrt_q = np.sqrt(q) # Predicted observation: [range, bearing] r_pred = sqrt_q b_pred = np.arctan2(dy, dx) - self.x[2] # Innovation innov = np.array([r_obs - r_pred, np.arctan2(np.sin(b_obs - b_pred), np.cos(b_obs - b_pred))]) # Jacobian H = dh/dstate H = np.zeros((2, self.dim)) # dh/d[x, y, θ] H[0, 0] = -dx / sqrt_q H[0, 1] = -dy / sqrt_q H[1, 0] = dy / q H[1, 1] = -dx / q H[1, 2] = -1 # dh/d[lx, ly] H[0, lx_idx] = dx / sqrt_q H[0, ly_idx] = dy / sqrt_q H[1, lx_idx] = -dy / q H[1, ly_idx] = dx / q # Kalman gain S = H @ self.P @ H.T + self.Q K = self.P @ H.T @ np.linalg.inv(S) # State update self.x += K @ innov # Covariance update: P' = (I - K*H)*P I_KH = np.eye(self.dim) - K @ H self.P = I_KH @ self.P self.observed[lm_id] = True # ============================================================ # Simulation # ============================================================ np.random.seed(42) # Generate landmarks in a room n_landmarks = 8 landmarks = np.array([ [5, 0], [5, 5], [0, 5], [-5, 5], [-5, 0], [-5, -5], [0, -5], [5, -5] ], dtype=float) # Generate robot trajectory (circular path) T = 100 dt = 0.1 true_traj = [] for t in range(T): theta = 2 * np.pi * t / T x = 3 * np.cos(theta) y = 3 * np.sin(theta) true_traj.append([x, y, theta + np.pi/2]) true_traj = np.array(true_traj) # Initialize EKF-SLAM slam = EKFSlam2D(n_landmarks) slam.x[0:3] = true_traj[0] # Run SLAM est_traj = [] for t in range(1, T): # True motion prev = true_traj[t-1] curr = true_traj[t] dx = (curr[0] - prev[0])*np.cos(prev[2]) + (curr[1] - prev[1])*np.sin(prev[2]) dy = -(curr[0] - prev[0])*np.sin(prev[2]) + (curr[1] - prev[1])*np.cos(prev[2]) dtheta = curr[2] - prev[2] u = np.array([dx, dy, dtheta]) # Add motion noise u_noisy = u + np.random.multivariate_normal(np.zeros(3), slam.R * 0.5) # Predict slam.predict(u_noisy) # Observe landmarks within range z_list = [] for j in range(n_landmarks): dx_lm = landmarks[j, 0] - curr[0] dy_lm = landmarks[j, 1] - curr[1] r = np.sqrt(dx_lm**2 + dy_lm**2) if r < 8.0: # max observation range b = np.arctan2(dy_lm, dx_lm) - curr[2] r_noisy = r + np.random.randn() * 0.1 b_noisy = b + np.random.randn() * 0.05 z_list.append((j, r_noisy, b_noisy)) # Update if z_list: slam.update(z_list) est_traj.append(slam.x[0:3].copy()) est_traj = np.array(est_traj) # Plot results fig, axes = plt.subplots(1, 2, figsize=(14, 6)) # Trajectory comparison axes[0].plot(true_traj[:,0], true_traj[:,1], 'b-', label='Ground Truth', linewidth=2) axes[0].plot(est_traj[:,0], est_traj[:,1], 'r--', label='EKF-SLAM Estimate', linewidth=2) axes[0].scatter(landmarks[:,0], landmarks[:,1], c='green', s=100, marker='*', label='True Landmarks', zorder=5) est_lm = slam.x[3:].reshape(-1, 2) axes[0].scatter(est_lm[:,0], est_lm[:,1], c='red', s=50, marker='o', label='Estimated Landmarks', alpha=0.7) axes[0].set_xlabel('X (m)') axes[0].set_ylabel('Y (m)') axes[0].set_title('EKF-SLAM: Trajectory & Landmarks') axes[0].legend() axes[0].grid(True, alpha=0.3) axes[0].set_aspect('equal') # Position error over time pos_error = np.sqrt((true_traj[1:,0] - est_traj[:,0])**2 + (true_traj[1:,1] - est_traj[:,1])**2) axes[1].plot(pos_error, 'b-', linewidth=1) axes[1].set_xlabel('Time Step') axes[1].set_ylabel('Position Error (m)') axes[1].set_title('EKF-SLAM Localization Error') axes[1].grid(True, alpha=0.3) plt.tight_layout() plt.savefig('ekf_slam_result.png', dpi=150) plt.show() # Print landmark estimation error for j in range(n_landmarks): err = np.linalg.norm(est_lm[j] - landmarks[j]) print(f"Landmark {j}: error = {err:.3f} m")

# slam_sim_node.py - ROS2 2D SLAM Simulation Node import rclpy from rclpy.node import Node from geometry_msgs.msg import PoseStamped, Twist from nav_msgs.msg import OccupancyGrid, Odometry from sensor_msgs.msg import LaserScan from tf2_ros import TransformBroadcaster from geometry_msgs.msg import TransformStamped import numpy as np class SlamSimNode(Node): def __init__(self): super().__init__('slam_sim_node') # Robot state self.robot_pose = np.array([0.0, 0.0, 0.0]) # x, y, theta self.dt = 0.1 # 10 Hz update rate # Subscribers self.cmd_vel_sub = self.create_subscription( Twist, '/cmd_vel', self.cmd_vel_callback, 10) # Publishers self.odom_pub = self.create_publisher(Odometry, '/odom', 10) self.scan_pub = self.create_publisher(LaserScan, '/scan', 10) self.map_pub = self.create_publisher(OccupancyGrid, '/map', 10) self.pose_pub = self.create_publisher(PoseStamped, '/slam_pose', 10) # TF broadcaster self.tf_broadcaster = TransformBroadcaster(self) # Timer for simulation loop self.timer = self.create_timer(self.dt, self.simulation_step) # Simple map (walls at boundaries) self.map_resolution = 0.05 # m/pixel self.map_width = 200 self.map_height = 200 self.grid = np.zeros((self.map_height, self.map_width), dtype=np.int8) # Add boundary walls self.grid[0, :] = 100 self.grid[-1, :] = 100 self.grid[:, 0] = 100 self.grid[:, -1] = 100 self.get_logger().info('SLAM simulation node started') def cmd_vel_callback(self, msg): # Apply velocity to update robot pose v = msg.linear.x omega = msg.angular.z theta = self.robot_pose[2] # Differential drive model self.robot_pose[0] += v * np.cos(theta) * self.dt self.robot_pose[1] += v * np.sin(theta) * self.dt self.robot_pose[2] += omega * self.dt def simulation_step(self): # Publish odometry self.publish_odometry() # Publish simulated laser scan self.publish_scan() # Publish map self.publish_map() # Broadcast TF self.broadcast_tf() def publish_odometry(self): odom = Odometry() odom.header.stamp = self.get_clock().now().to_msg() odom.header.frame_id = 'odom' odom.child_frame_id = 'base_link' odom.pose.pose.position.x = float(self.robot_pose[0]) odom.pose.pose.position.y = float(self.robot_pose[1]) # Orientation from yaw from tf_transformations import quaternion_from_euler q = quaternion_from_euler(0, 0, self.robot_pose[2]) odom.pose.pose.orientation.x = q[0] odom.pose.pose.orientation.y = q[1] odom.pose.pose.orientation.z = q[2] odom.pose.pose.orientation.w = q[3] self.odom_pub.publish(odom) def publish_scan(self): scan = LaserScan() scan.header.stamp = self.get_clock().now().to_msg() scan.header.frame_id = 'laser' scan.angle_min = -np.pi scan.angle_max = np.pi scan.angle_increment = np.pi / 180 # 1 degree resolution scan.range_min = 0.1 scan.range_max = 10.0 n_beams = 360 scan.ranges = [10.0] * n_beams # default: max range # Simulate wall detections (simplified) for i in range(n_beams): angle = self.robot_pose[2] + scan.angle_min + i * scan.angle_increment # Ray-cast to boundary walls at ±5m for wall_dist in [5.0]: dx = np.cos(angle) * wall_dist dy = np.sin(angle) * wall_dist wx = self.robot_pose[0] + dx wy = self.robot_pose[1] + dy if abs(wx) >= 5.0 or abs(wy) >= 5.0: r = min(5.0/max(abs(np.cos(angle)), 1e-6), 5.0/max(abs(np.sin(angle)), 1e-6)) scan.ranges[i] = min(r, 10.0) + np.random.randn()*0.02 self.scan_pub.publish(scan) def publish_map(self): map_msg = OccupancyGrid() map_msg.header.stamp = self.get_clock().now().to_msg() map_msg.header.frame_id = 'map' map_msg.info.resolution = self.map_resolution map_msg.info.width = self.map_width map_msg.info.height = self.map_height map_msg.info.origin.position.x = -5.0 map_msg.info.origin.position.y = -5.0 map_msg.info.origin.orientation.w = 1.0 map_msg.data = self.grid.flatten().tolist() self.map_pub.publish(map_msg) def broadcast_tf(self): t = TransformStamped() t.header.stamp = self.get_clock().now().to_msg() t.header.frame_id = 'odom' t.child_frame_id = 'base_link' t.transform.translation.x = float(self.robot_pose[0]) t.transform.translation.y = float(self.robot_pose[1]) from tf_transformations import quaternion_from_euler q = quaternion_from_euler(0, 0, self.robot_pose[2]) t.transform.rotation.x = q[0] t.transform.rotation.y = q[1] t.transform.rotation.z = q[2] t.transform.rotation.w = q[3] self.tf_broadcaster.sendTransform(t) def main(args=None): rclpy.init(args=args) node = SlamSimNode() rclpy.spin(node) node.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()

<!-- slam_sim.launch.py --> <launch> <node pkg="slam_course" exec="slam_sim_node" name="slam_sim" output="screen"> <param name="map_resolution" value="0.05"/> <param name="max_range" value="10.0"/> </node> <node pkg="rviz2" exec="rviz2" name="rviz2" args="-d $(find-pkg-share slam_course)/config/slam.rviz"/> </launch>

<!-- gazebo_slam.world - Simple room for SLAM testing --> <?xml version="1.0"?> <sdf version="1.6"> <world name="slam_test_room"> <include> <uri>model://ground_plane</uri> </include> <include> <uri>model://sun</uri> </include> <!-- Room walls (10m x 10m) --> <model name="room"> <static>true</static> <!-- North wall --> <link name="north_wall"> <pose>0 5 1 0 0 0</pose> <collision name="collision"> <geometry><box><size>10 0.2 2</size></box></geometry> </collision> <visual name="visual"> <geometry><box><size>10 0.2 2</size></box></geometry> </visual> </link> <!-- Add South, East, West walls similarly --> </model> <!-- Obstacles --> <model name="obstacle_1"> <static>true</static> <link name="link"> <pose>2 2 0.5 0 0 0</pose> <collision name="collision"> <geometry><cylinder><radius>0.3</radius><height>1</height></cylinder></geometry> </collision> <visual name="visual"> <geometry><cylinder><radius>0.3</radius><height>1</height></cylinder></geometry> </visual> </link> </model> </world> </sdf>