From e49fdb4c24a7d5cff6319a2f18d3ff4f8a94b30b Mon Sep 17 00:00:00 2001 From: 22268 <2226804607@qq.com> Date: Fri, 19 Dec 2025 16:02:35 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AF=B9=E6=A8=A1=E5=9E=8B=E7=8E=AF=E5=A2=83?= =?UTF-8?q?=E7=9A=84=E9=A2=84=E6=B5=8B=E5=92=8C=E9=AA=A8=E6=9E=B6=E7=BB=93?= =?UTF-8?q?=E6=9E=84=E5=B1=95=E7=8E=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/minicarsim/main.py | 151 +++++++++++++++++++++++++++-------------- 1 file changed, 101 insertions(+), 50 deletions(-) diff --git a/src/minicarsim/main.py b/src/minicarsim/main.py index 8b6f3adee6..7e29be23fe 100644 --- a/src/minicarsim/main.py +++ b/src/minicarsim/main.py @@ -68,13 +68,23 @@ def generate_realistic_lidar_data(self): try: # 获取车辆位置和朝向 vehicle_pos, vehicle_quat = self.get_world_pose("vehicle") + + # 获取LiDAR传感器的位置和朝向 + lidar_site_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SITE, "lidar_site") + if lidar_site_id >= 0: + lidar_pos = self.data.site_xpos[lidar_site_id].copy() + # 获取LiDAR的旋转矩阵 + lidar_mat = self.data.site_xmat[lidar_site_id].reshape(3, 3) + else: + # 如果找不到LiDAR站点,使用默认位置 + lidar_offset = np.array(LIDAR_PARAMS["pos"]) + lidar_pos = vehicle_pos + lidar_offset + lidar_mat = np.eye(3) # 单位矩阵表示无旋转 except ValueError: vehicle_pos = np.array([0, 0, 0.5]) - - # LiDAR相对位置 - lidar_offset = np.array(LIDAR_PARAMS["pos"]) - lidar_pos = vehicle_pos + lidar_offset - + lidar_pos = vehicle_pos + np.array(LIDAR_PARAMS["pos"]) + lidar_mat = np.eye(3) + # 生成角度范围 azimuth_angles = np.arange(0, 360, LIDAR_PARAMS["azimuth_res"]) # 方位角:0~360° elevation_angles = np.arange( @@ -102,6 +112,9 @@ def generate_realistic_lidar_data(self): # 归一化方向向量 dir_local = dir_local / np.linalg.norm(dir_local) + # 将方向向量从LiDAR坐标系转换到世界坐标系 + dir_world = lidar_mat @ dir_local + # 创建参数 geom_group = np.array([1, 1, 1, 1, 1, 1], dtype=np.uint8) geom_id = np.zeros(1, dtype=np.int32) @@ -110,7 +123,7 @@ def generate_realistic_lidar_data(self): distance = mujoco.mj_ray( self.model, self.data, lidar_pos, # 射线起点 - dir_local, # 射线方向 + dir_world, # 射线方向(世界坐标系) geom_group, # 几何体组 1, # flg_static: 检测静态几何体 -1, # bodyexclude: 不排除任何body @@ -120,7 +133,7 @@ def generate_realistic_lidar_data(self): # 记录点云数据 if distance >= 0 and distance <= LIDAR_PARAMS["range"]: # 如果检测到碰撞且在范围内 # 计算交点位置 - hit_pos = lidar_pos + dir_local * distance + hit_pos = lidar_pos + dir_world * distance point_cloud.append(hit_pos) # 转换为numpy数组 @@ -136,6 +149,12 @@ def detect_objects(self): """检测环境中的物体""" detected_objects = [] + # 获取车辆位置 + try: + vehicle_pos, _ = self.get_world_pose("vehicle") + except ValueError: + vehicle_pos = np.array([0, 0, 0.5]) + # 遍历所有物体 for i in range(self.model.nbody): body_name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_BODY, i) @@ -143,49 +162,66 @@ def detect_objects(self): # 获取物体位置 pos = self.data.xpos[i].copy() - # 简单的距离检测(10米内认为可检测到) - try: - vehicle_pos, _ = self.get_world_pose("vehicle") - distance = np.linalg.norm(pos - vehicle_pos) + # 计算与车辆的距离 + distance = np.linalg.norm(pos - vehicle_pos) + + # 只有在检测范围内才记录 + if distance <= 15.0: # 扩大检测范围 + # 获取物体类型(根据名称) + obj_type = "box" - if distance <= 10.0: - # 获取物体类型(根据名称) - obj_type = "box" - - # 获取物体的几何信息用于更好的可视化 - geom_id = self.model.body_geomadr[i] - if geom_id >= 0: - size = self.model.geom_size[geom_id][:3].copy() - else: - size = [0.5, 0.5, 0.5] # 默认大小 - - detected_objects.append({ - "id": i, - "name": body_name, - "type": obj_type, - "position": pos.tolist(), - "distance": distance, - "size": size.tolist() - }) - except ValueError: - # 如果找不到车辆,跳过距离检测 + # 获取物体的几何信息用于更好的可视化 geom_id = self.model.body_geomadr[i] if geom_id >= 0: size = self.model.geom_size[geom_id][:3].copy() else: size = [0.5, 0.5, 0.5] # 默认大小 - + detected_objects.append({ "id": i, "name": body_name, - "type": "box", + "type": obj_type, "position": pos.tolist(), - "distance": 0, + "distance": float(distance), "size": size.tolist() }) return detected_objects + def calculate_avoidance_control(self, lidar_data, detected_objects): + """基于传感器数据计算避障控制指令""" + # 初始化控制指令 + left_speed = 5.0 + right_speed = 5.0 + steering_angle = 0.0 + + if len(detected_objects) > 0: + # 找到最近的障碍物 + closest_obj = min(detected_objects, key=lambda x: x['distance']) + + if closest_obj['distance'] < 5.0: # 如果障碍物很近 + obj_pos = np.array(closest_obj['position']) + try: + vehicle_pos, _ = self.get_world_pose("vehicle") + # 计算障碍物相对于车辆的方向 + direction = obj_pos[:2] - vehicle_pos[:2] # 只考虑XY平面 + angle_to_obstacle = np.arctan2(direction[1], direction[0]) + + # 简单避障策略:向相反方向转弯 + if angle_to_obstacle > 0: # 障碍物在左侧 + steering_angle = -5.0 # 向右转 + else: # 障碍物在右侧 + steering_angle = 5.0 # 向左转 + + # 如果非常接近,减速 + if closest_obj['distance'] < 3.0: + left_speed = 2.0 + right_speed = 2.0 + except ValueError: + pass + + return left_speed, right_speed, steering_angle + def generate_annotations(self): """生成物体检测标注数据""" # 检测到的物体 @@ -306,26 +342,17 @@ def run_simulation(self): prev_detected_count = 0 for i in range(SIMULATION_FRAMES): - # 设置控制输入(使车辆向前移动并轻微转向) - if rear_left_idx >= 0: - self.data.ctrl[rear_left_idx] = 5.0 # 左后轮速度 - if rear_right_idx >= 0: - self.data.ctrl[rear_right_idx] = 5.0 # 右后轮速度 - # 设置前轮转向 - if front_left_steer_idx >= 0: - self.data.ctrl[front_left_steer_idx] = np.sin(i * 0.05) * 10 # 正弦变化转向 - if front_right_steer_idx >= 0: - self.data.ctrl[front_right_steer_idx] = np.sin(i * 0.05) * 10 # 正弦变化转向 - - # 执行仿真步长 - mujoco.mj_step(self.model, self.data) - # 每20帧生成和保存一次数据 if i % 20 == 0: # 生成传感器数据和标注 lidar_data = self.generate_realistic_lidar_data() annotations = self.generate_annotations() + # 基于传感器数据计算控制指令 + left_speed, right_speed, steering_angle = self.calculate_avoidance_control( + lidar_data, annotations["objects"] + ) + # 显示检测到的物体数量 detected_count = len(annotations["objects"]) if detected_count != prev_detected_count: @@ -344,9 +371,33 @@ def run_simulation(self): self.visualize_detection(lidar_data, annotations) print(f"已仿真 {i}/{SIMULATION_FRAMES} 帧") + else: + # 使用上一帧的控制指令 + try: + left_speed, right_speed, steering_angle + except NameError: + left_speed, right_speed, steering_angle = 5.0, 5.0, 0.0 + + # 设置控制输入 + if rear_left_idx >= 0: + self.data.ctrl[rear_left_idx] = left_speed # 左后轮速度 + if rear_right_idx >= 0: + self.data.ctrl[rear_right_idx] = right_speed # 右后轮速度 + # 设置前轮转向 + if front_left_steer_idx >= 0: + self.data.ctrl[front_left_steer_idx] = steering_angle + if front_right_steer_idx >= 0: + self.data.ctrl[front_right_steer_idx] = steering_angle + + # 执行仿真步长 + mujoco.mj_step(self.model, self.data) + + # 更新可视化 + if hasattr(self, 'viewer') and self.viewer is not None: + self.viewer.sync() # 控制仿真速度以便观察 - time.sleep(0.02) + time.sleep(0.01) print(f"仿真完成!数据已保存到:{self.output_dir}")