6.4 综合实战：腿式机器人的IMU关节融合与状态估计
本实例是一个用于基于本体感知状态估计的开源项目，通过融合机器人身上的IMU（惯性测量单元）数据和关节编码器（Joint Encoders）数据来估计腿式机器人主体的运动状态（如姿态和位置轨迹）。在本实例中提供了一个Python实现，可以对从宇树科技Unitree Go2机器人采集的ROS2 rosbag数据进行解码和状态估计，不依赖完整的ROS2环境，适合作为研究和实验用途的基础代码框架。
实例6-4 ：腿式机器人的IMU 关节融合与状态估计（源码路径：codes\6\leg-odometry ）
6.4.1 配置文件
文件config/go2.yaml是本项目的核心配置文件，用于定义ROS话题、IMU采样率、数据处理时间、初始状态标准差、IMU噪声模型、机器人几何参数和输出目录。这些参数用于EKF状态估计和IMU前向传播模块，指导滤波器初始化、传感器数据融合及运动状态计算，从而实现腿式机器人精确的姿态与位置估计。
python
topic: "/lowstate" # ROS 话题名称
imudatarate: 200 # IMU 数据采样率 (Hz)
starttime: 337679 # rosbag2 数据开始时间
endtime: 337857 # rosbag2 数据结束时间
initAlignmentTime: 10 # 初始对齐所需的时间 (秒)
# 初始状态标准差
initposstd: [0.05, 0.05, 0.05] # 初始位置标准差 (m)
initvelstd: [0.05, 0.05, 0.05] # 初始速度标准差 (m/s)
initattstd: [1.0, 1.0, 0.5] # 初始姿态标准差 (度)
# IMU 噪声参数
imunoise:
arw: 0.1 # 陀螺角随机游走（角速度噪声）deg/√h
vrw: 6 # 加速度随机游走 (m/s^1.5)
gbstd: 0.1 # 陀螺零偏标准差
abstd: 2000 # 加速度偏置标准差
gsstd: 500 # 陀螺测量噪声标准差
asstd: 500 # 加速度测量噪声标准差
corrtime: 0.5 # 噪声相关时间常数 (秒)
# 坐标变换
rotmat: [-1,0,0,
0,-1,0,
0,0,1] # 旋转矩阵：IMU 坐标系到机器人基座坐标系
base_in_bodyimu: [0.0, 0.0, 0.1] # 机器人基座在 IMU 坐标系下的位置偏移 (m)
# 机器人几何参数
robotpara:
ox: 0.1934 # 前腿横向偏移
oy: 0.0465 # 前腿纵向偏移
ot: 0.0955 # 前腿高度偏移
lc: 0.213 # 腿长参数 lc
lt: 0.213 # 腿长参数 lt
# 输出文件夹
output:
dir: "./output" # 输出结果目录6.4.2 工具文件
"utils"目录提供了Unitree Go2机器人低层状态数据处理与辅助工具，包括数据结构定义、IMU与机器人传感器状态的表示、姿态与旋转矩阵转换、配置参数加载、文件读写、ROS消息到NumPy数组的转换、生成数据流，以及机器人轨迹可视化功能，为 EKF 状态估计和导航分析模块提供完整的基础工具支持。
（1）文件src/types.py定义了实例中使用的核心数据结构和常量，包括IMU数据、机器人姿态（Attitude）、位置-速度-姿态状态（PVA）、机器人传感器数据（关节角度、速度、足端力）、IMU误差与噪声模型、导航状态（NavState）以及机器人几何参数（RobotPara）。这些类用于统一管理传感器输入、滤波器状态、噪声参数和机器人几何参数，为EKF状态估计和运动融合模块提供数据接口。
python
import numpy as np
from scipy.spatial.transform import Rotation as R
# 数据类型与常量定义
DT = np.float64 # 使用的浮点类型
D2R = np.pi / 180.0 # 角度转弧度
R2D = 180.0 / np.pi # 弧度转角度
IMU_RATE = 200 # IMU 数据采样率 (Hz)
window_length = 11 # 滤波窗口长度
halfwindow_length = window_length // 2
NormG = 9.782940329221166 # 重力加速度标准值 (m/s^2)
# IMU 数据结构
class IMU:
def __init__(self):
self.timestamp = 0.0
self.dt = 0.0
self.angular_velocity = np.zeros(3, dtype=DT) # 三维角速度 (Eigen::Vector3d)
self.acceleration = np.zeros(3, dtype=DT) # 三维加速度 (Eigen::Vector3d)
# 姿态数据结构
class Attitude:
def __init__(self):
self.qbn = R.from_quat([0.0, 0.0, 0.0, 1.0]) # 四元数 (单位四元数)
self.cbn = np.eye(3, dtype=DT) # 方向余弦矩阵 (单位矩阵)
self.euler = np.zeros(3, dtype=DT) # 欧拉角 (roll, pitch, yaw)
# 位置-速度-姿态结构
class PVA:
def __init__(self):
self.pos = np.zeros(3, dtype=DT) # 位置 (m)
self.vel = np.zeros(3, dtype=DT) # 速度 (m/s)
self.att = Attitude() # 姿态
# 机器人传感器数据
class RobotSensor:
def __init__(self):
self.timestamp = 0.0
self.joint_angular_position = np.zeros(12, dtype=DT) # 关节角度 (12 关节)
self.joint_angular_velocity = np.zeros(12, dtype=DT) # 关节角速度 (12 关节)
self.footforce = np.zeros(4, dtype=DT) # 足端力 (4 只足)
# IMU 误差结构
class ImuError:
def __init__(self):
self.gyrbias = np.zeros(3, dtype=DT) # 陀螺零偏 (rad/s)
self.accbias = np.zeros(3, dtype=DT) # 加速度零偏 (m/s^2)
self.gyrscale = np.zeros(3, dtype=DT) # 陀螺比例误差
self.accscale = np.zeros(3, dtype=DT) # 加速度比例误差
# 导航状态
class NavState:
def __init__(self):
self.pos = np.zeros(3, dtype=DT) # 位置
self.vel = np.zeros(3, dtype=DT) # 速度
self.euler = np.zeros(3, dtype=DT) # 欧拉角
self.imuerror = ImuError() # IMU 误差
# IMU 噪声参数
class ImuNoise:
def __init__(self):
self.gyr_arw = np.zeros(3, dtype=DT) # 陀螺角随机游走 (deg/√h)
self.acc_vrw = np.zeros(3, dtype=DT) # 加速度随机游走
self.gyrbias_std = np.zeros(3, dtype=DT) # 陀螺零偏标准差
self.accbias_std = np.zeros(3, dtype=DT) # 加速度零偏标准差
self.gyrscale_std = np.zeros(3, dtype=DT) # 陀螺比例误差标准差
self.accscale_std = np.zeros(3, dtype=DT) # 加速度比例误差标准差
self.corr_time = 0.0 # 噪声相关时间常数
# 机器人几何参数
class RobotPara:
def __init__(self):
self.ox = 0.0 # 前腿横向偏移
self.oy = 0.0 # 前腿纵向偏移
self.ot = 0.0 # 前腿高度偏移
self.lc = 0.0 # 腿长参数 lc
self.lt = 0.0 # 腿长参数 lt
# 全局参数集合
class Paras:
def __init__(self):
# 初始状态及标准差
self.initstate = NavState()
self.initstate_std = NavState()
# IMU 噪声参数
self.imunoise = ImuNoise()
self.starttime = 0.0 # 数据开始时间
self.initAlignmentTime = 0 # 初始对齐时间
self.robotbody_rotmat = np.eye(3, dtype=DT) # 机器人机身旋转矩阵
self.base_in_bodyimu = np.zeros(3, dtype=DT) # 机器人基座在 IMU 坐标下的位置偏移
self.robotpara = RobotPara() # 机器人几何参数（2）文件src/rot.py提供了欧拉角与方向余弦矩阵（CBN，Coordinate Body to Navigation）之间的转换函数，包括：
- euler2cbn：将欧拉角（roll, pitch, yaw）转换为方向余弦矩阵，用于将机体坐标系向量旋转到导航坐标系。
- matrix2euler：将方向余弦矩阵转换回欧拉角，用于从旋转矩阵恢复roll、pitch、yaw值。
python
import numpy as np
def euler2cbn(euler_rpy: np.ndarray) -> np.ndarray:
"""
将欧拉角 (roll, pitch, yaw) 转换为方向余弦矩阵 C_bn
C_bn: body -> navigation
"""
roll, pitch, yaw = float(euler_rpy[0]), float(euler_rpy[1]), float(euler_rpy[2])
cr, sr = np.cos(roll), np.sin(roll)
cp, sp = np.cos(pitch), np.sin(pitch)
cy, sy = np.cos(yaw), np.sin(yaw)
# 绕 X 轴旋转矩阵
Rx = np.array([[1, 0, 0],
[0, cr, -sr],
[0, sr, cr]], dtype=np.float64)
# 绕 Y 轴旋转矩阵
Ry = np.array([[ cp, 0, sp],
[ 0, 1, 0],
[-sp, 0, cp]], dtype=np.float64)
# 绕 Z 轴旋转矩阵
Rz = np.array([[cy, -sy, 0],
[sy, cy, 0],
[ 0, 0, 1]], dtype=np.float64)
# 按 Z-Y-X 顺序旋转，返回 body->navigation 的方向余弦矩阵
return Rz @ Ry @ Rx
def matrix2euler(cbn: np.ndarray) -> np.ndarray:
"""
将方向余弦矩阵 C_bn 转换为欧拉角 (roll, pitch, yaw)
输出角度单位为弧度，范围 roll,pitch,yaw ∈ (-pi, pi]
"""
roll = np.arctan2(cbn[2,1], cbn[2,2])
pitch = -np.arcsin(cbn[2,0])
yaw = np.arctan2(cbn[1,0], cbn[0,0]) # (-pi, pi]
return np.array([roll, pitch, yaw], dtype=np.float64)（3）文件src/dataloader.py定义了DataLoader 类，用于从ROS2 rosbag文件中读取Unitree Go2机器人低层状态消息，并将其转换为NumPy数组。主要功能包括：
- 注册自定义ROS消息类型（从unitree_go/msg文件夹读取.msg文件）。
- ROS时间戳转换为GPS时间（用于统一时间基准）。
- 将lowstate 消息转换为NumPy数组，包括IMU数据、关节角度和速度、足端力。
- 按顺序读取rosbag数据，生成完整的低层状态数组，供EKF和状态估计模块使用。
python
class DataLoader:
def __init__(self, config: Config, data_path: Path):
self.config = config
self.data_path = data_path
self.add_types = {}
# 注册自定义 ROS 消息类型
go2_msg_folder = "./unitree_go/msg"
for root, dirs, files in os.walk(go2_msg_folder):
for file in files:
if file.endswith(".msg"):
msgpath = Path(root) / file
msgdef = msgpath.read_text(encoding="utf-8")
# 从 msg 文件解析类型并保存
self.add_types.update(get_types_from_msg(msgdef, self._guess_msgtype(msgpath)))
register_types(self.add_types)
def _guess_msgtype(self, path: Path) -> str:
"""根据路径猜测 ROS 消息类型名称"""
name = path.relative_to(path.parents[2]).with_suffix("")
if "msg" not in name.parts:
name = name.parent / "msg" / name.name
return str(PurePosixPath(name))
def _rostime2gpst(self, cur_ts) -> float:
"""将 ROS 时间戳 (ns) 转换为 GPS 周内秒 (s)"""
sec = cur_ts // 10**9
nanosec = cur_ts % 10**9
gps_epoch = datetime(1980, 1, 6) # GPS 起始时间
leap_seconds = 18 # UTC 与 GPS 的闰秒差
ros_datetime = datetime.fromtimestamp(sec + nanosec*1e-9)
# 计算从 GPS epoch 到当前时间的总秒数
delta = ros_datetime - gps_epoch
total_seconds = delta.total_seconds()
# 考虑本地时区偏移
offset_seconds = (datetime.fromtimestamp(sec) -
datetime.utcfromtimestamp(sec)).total_seconds()
# 计算 GPS 周和周内秒
gps_week = int(total_seconds // (7 * 24 * 3600))
sow = total_seconds % (7 * 24 * 3600) - offset_seconds + leap_seconds
return sow
def _rosMsg2np(self, lowstate_msg, lowstate_dim, cur_ts):
"""将 ROS lowstate 消息转换为 NumPy 数组"""
robot_sensor_np = np.zeros((1, lowstate_dim), dtype=np.float64)
# ROS 时间戳转 GPS 时间
ts = self._rostime2gpst(cur_ts)
robot_sensor_np[0, 0] = ts
# IMU 数据
robot_sensor_np[0, 1:4] = np.array(lowstate_msg.imu_state.gyroscope)
robot_sensor_np[0, 4:7] = -np.array(lowstate_msg.imu_state.accelerometer) # 加速度取负
# 电机位置 q
robot_sensor_np[0, 7] = lowstate_msg.motor_state[3].q
robot_sensor_np[0, 8] = lowstate_msg.motor_state[4].q
robot_sensor_np[0, 9] = lowstate_msg.motor_state[5].q
robot_sensor_np[0, 10] = lowstate_msg.motor_state[0].q
robot_sensor_np[0, 11] = lowstate_msg.motor_state[1].q
robot_sensor_np[0, 12] = lowstate_msg.motor_state[2].q
robot_sensor_np[0, 13] = lowstate_msg.motor_state[9].q
robot_sensor_np[0, 14] = lowstate_msg.motor_state[10].q
robot_sensor_np[0, 15] = lowstate_msg.motor_state[11].q
robot_sensor_np[0, 16] = lowstate_msg.motor_state[6].q
robot_sensor_np[0, 17] = lowstate_msg.motor_state[7].q
robot_sensor_np[0, 18] = lowstate_msg.motor_state[8].q
# 电机速度 dq
robot_sensor_np[0, 19:31] = [
lowstate_msg.motor_state[3].dq, lowstate_msg.motor_state[4].dq, lowstate_msg.motor_state[5].dq,
lowstate_msg.motor_state[0].dq, lowstate_msg.motor_state[1].dq, lowstate_msg.motor_state[2].dq,
lowstate_msg.motor_state[9].dq, lowstate_msg.motor_state[10].dq, lowstate_msg.motor_state[11].dq,
lowstate_msg.motor_state[6].dq, lowstate_msg.motor_state[7].dq, lowstate_msg.motor_state[8].dq
]
# 足端力
robot_sensor_np[0, 31] = lowstate_msg.foot_force[1]
robot_sensor_np[0, 32] = lowstate_msg.foot_force[0]
robot_sensor_np[0, 33] = lowstate_msg.foot_force[3]
robot_sensor_np[0, 34] = lowstate_msg.foot_force[2]
return robot_sensor_np
def load_data(self):
"""加载 rosbag 数据，并转换为 NumPy 数组"""
bag = AnyReader([self.data_path])
bag.open()
datatopic = self.config.topic
msg_counts = bag.topics[datatopic].msgcount
data_connections = [x for x in bag.connections if x.topic == datatopic]
lowstate_msgs = bag.messages(connections=data_connections)
lowstate_dim = 35 # 1 timestamp + 6 IMU + 24 motor state + 4 foot force
lowstate_np = np.zeros((msg_counts, lowstate_dim))
for i in tqdm(range(msg_counts), total=msg_counts, desc="Loading bag", unit="msg"):
try:
lowstate_connection, cur_ts, lowstate_data = next(lowstate_msgs) # 时间戳 ns
# 反序列化 ROS 消息
try:
lowstate_msg = deserialize_cdr(lowstate_data, lowstate_connection.msgtype)
except Exception as e:
print(f"Error deserialize_cdr {i}: {e}")
# 转为 NumPy 数组
lowstate_np[i] = self._rosMsg2np(lowstate_msg, lowstate_dim, cur_ts)
except Exception as e:
print(f"Error processing message {i}: {e}")
continue
return lowstate_np（4）文件src/loadanddump.py提供了文件读写与参数加载工具函数，包括：
- FileSaver类：将浮点数据按列写入文本文件，用于记录导航状态、IMU误差和标准差。
- loadConfig函数：从配置字典加载机器人初始状态、IMU噪声、几何参数和旋转矩阵，并填充到 Paras 对象中。
- writeNavResult函数：将当前导航状态（位置、速度、姿态）和IMU误差写入文件。
- writeSTD函数：将状态协方差的标准差写入文件，按不同量纲（位置、角度、偏置等）进行单位转换。
这些工具在EKF状态估计与数据保存模块中被频繁调用。
python
class FileSaver:
"""用于将浮点数据按列写入文本文件"""
def __init__(self, path: str, columns: int):
self.f = open(path, "w", encoding="utf-8")
self.columns = int(columns)
def dump(self, data: List[float]):
"""写入一行数据，列数必须匹配"""
if len(data) != self.columns:
raise ValueError(f"FileSaver columns mismatch: expect {self.columns}, got {len(data)}")
line = "".join(f"{float(x):<15.9f} " for x in data)
self.f.write(line.rstrip() + "\n")
def close(self):
"""关闭文件"""
try:
self.f.close()
except Exception:
pass
def loadConfig(config: dict, paras: Paras) -> bool:
"""从配置字典加载参数到 Paras 对象"""
try:
initposstd_vec = list(config["initposstd"])
initvelstd_vec = list(config["initvelstd"])
initattstd_vec = list(config["initattstd"])
except Exception:
print("加载初始状态标准差失败，请检查 position/velocity/attitude 的配置!", file=sys.stderr)
return False
# 设置初始状态标准差
for i in range(3):
paras.initstate_std.pos[i] = float(initposstd_vec[i])
paras.initstate_std.vel[i] = float(initvelstd_vec[i])
paras.initstate_std.euler[i] = float(initattstd_vec[i]) * D2R
# IMU 噪声参数
try:
arw = float(config["imunoise"]["arw"])
vrw = float(config["imunoise"]["vrw"])
gbstd = float(config["imunoise"]["gbstd"])
abstd = float(config["imunoise"]["abstd"])
gsstd = float(config["imunoise"]["gsstd"])
asstd = float(config["imunoise"]["asstd"])
paras.imunoise.corr_time = float(config["imunoise"]["corrtime"])
except Exception:
print("加载 IMU 噪声失败，请检查配置!", file=sys.stderr)
return False
for i in range(3):
paras.imunoise.gyr_arw[i] = arw * (D2R / 60.0)
paras.imunoise.acc_vrw[i] = vrw / 60.0
paras.imunoise.gyrbias_std[i] = gbstd * (D2R / 3600.0)
paras.imunoise.accbias_std[i] = abstd * 1e-5
paras.imunoise.gyrscale_std[i] = gsstd * 1e-6
paras.imunoise.accscale_std[i] = asstd * 1e-6
paras.initstate_std.imuerror.gyrbias[i] = gbstd * (D2R / 3600.0)
paras.initstate_std.imuerror.accbias[i] = abstd * 1e-5
paras.initstate_std.imuerror.gyrscale[i] = gsstd * 1e-6
paras.initstate_std.imuerror.accscale[i] = asstd * 1e-6
paras.imunoise.corr_time *= 3600.0 # 转换为秒
paras.starttime = float(config["starttime"])
paras.initAlignmentTime = int(config["initAlignmentTime"])
# IMU 安装偏移
base_in_bodyimu_vec = list(config["base_in_bodyimu"])
paras.base_in_bodyimu = np.array(base_in_bodyimu_vec, dtype=np.float64)
# 机器人几何参数
paras.robotpara.ox = float(config["robotpara"]["ox"])
paras.robotpara.oy = float(config["robotpara"]["oy"])
paras.robotpara.ot = float(config["robotpara"]["ot"])
paras.robotpara.lc = float(config["robotpara"]["lc"])
paras.robotpara.lt = float(config["robotpara"]["lt"])
# 机器人机身旋转矩阵
robot_b_rotmat_vec = np.array(config["rotmat"], dtype=np.float64)
if robot_b_rotmat_vec.size != 9:
raise ValueError("rotmat 必须包含 9 个元素")
paras.robotbody_rotmat = robot_b_rotmat_vec.reshape(3, 3, order="C")
return True
def writeNavResult(time: float, navstate: NavState, navfile: FileSaver, imuerrfile: FileSaver):
"""写入导航状态和 IMU 误差"""
# 导航状态
result = []
result.append(time)
result.append(navstate.pos[0])
result.append(navstate.pos[1])
result.append(navstate.pos[2])
result.append(navstate.vel[0])
result.append(navstate.vel[1])
result.append(navstate.vel[2])
result.append(navstate.euler[0] * R2D)
result.append(navstate.euler[1] * R2D)
result.append(navstate.euler[2] * R2D)
navfile.dump(result)
# IMU 误差
imuerr = navstate.imuerror
result = []
result.append(time)
result.append(imuerr.gyrbias[0] * R2D * 3600.0)
result.append(imuerr.gyrbias[1] * R2D * 3600.0)
result.append(imuerr.gyrbias[2] * R2D * 3600.0)
result.append(imuerr.accbias[0] * 1e5)
result.append(imuerr.accbias[1] * 1e5)
result.append(imuerr.accbias[2] * 1e5)
result.append(imuerr.gyrscale[0] * 1e6)
result.append(imuerr.gyrscale[1] * 1e6)
result.append(imuerr.gyrscale[2] * 1e6)
result.append(imuerr.accscale[0] * 1e6)
result.append(imuerr.accscale[1] * 1e6)
result.append(imuerr.accscale[2] * 1e6)
imuerrfile.dump(result)
def writeSTD(time: float, cov: np.ndarray, stdfile: FileSaver):
"""将状态协方差的标准差写入文件，并进行单位转换"""
result = []
result.append(time)
# 位置和速度标准差
for i in range(0, 6):
result.append(np.sqrt(max(cov[i, i], 0.0)))
# 姿态欧拉角标准差 (弧度转度)
for i in range(6, 9):
result.append(np.sqrt(max(cov[i, i], 0.0)) * R2D)
# 陀螺零偏标准差 (弧度转度每小时)
for i in range(9, 12):
result.append(np.sqrt(max(cov[i, i], 0.0)) * R2D * 3600.0)
# 加速度偏置标准差
for i in range(12, 15):
result.append(np.sqrt(max(cov[i, i], 0.0)) * 1e5)
# 比例误差标准差
for i in range(15, 21):
result.append(np.sqrt(max(cov[i, i], 0.0)) * 1e6)
stdfile.dump(result)（5）文件src/streams.py定义了从NumPy低层状态数组生成数据流的函数make_streams_from_lowstate，用于实现EKF或其他状态估计模块。
- imu_gen：生成IMU数据流（角速度、加速度、时间戳）。
- sensor_gen：生成机器人传感器数据流（关节角度、关节速度、足端力、时间戳）。
通过生成器方式返回两个可迭代对象，方便在状态估计循环中按时间顺序处理数据。
python
import numpy as np
from .types import IMU, RobotSensor
def make_streams_from_lowstate(lowstate_np: np.ndarray, imu_rate_hz: float):
"""
从 lowstate NumPy 数组生成 IMU 和 RobotSensor 数据流
参数:
lowstate_np: NumPy 数组，低层状态数据 (shape: [msg_count, 35])
imu_rate_hz: IMU 采样率 (Hz)
返回:
imu_gen(): IMU 数据生成器
sensor_gen(): RobotSensor 数据生成器
"""
assert lowstate_np.shape[1] >= 35
dt_default = 1.0 / float(imu_rate_hz) # 默认时间间隔
def imu_gen():
"""生成 IMU 数据流"""
for row in lowstate_np:
imu = IMU()
ts = float(row[0])
imu.timestamp = ts
imu.angular_velocity = row[1:4].astype(np.float64).copy() # 角速度
imu.acceleration = row[4:7].astype(np.float64).copy() # 加速度
imu.dt = dt_default
yield imu
def sensor_gen():
"""生成 RobotSensor 数据流"""
for row in lowstate_np:
rs = RobotSensor()
rs.timestamp = float(row[0])
rs.joint_angular_position = row[7:19].astype(np.float64).copy() # 电机位置 q
rs.joint_angular_velocity = row[19:31].astype(np.float64).copy() # 电机速度 dq
rs.footforce = row[31:35].astype(np.float64).copy() # 足端力
yield rs
return imu_gen(), sensor_gen()（6）文件src/vis.py提供了机器人轨迹可视化工具函数plot_traj，用于从保存的导航结果文件（文本文件）中读取位置数据，并在XY平面上绘制机器人的二维轨迹。它可以帮助快速直观地查看机器人运动路径，用于调试或结果分析。
python
def plot_traj(traj_path: Path):
"""
绘制机器人 XY 平面轨迹 (俯视图)
参数:
traj_path: 导航轨迹文件路径 (文本文件，列为 t px py pz roll pitch yaw)
"""
if not traj_path.exists():
raise FileNotFoundError(traj_path)
# 加载数据，跳过第一行表头
data = np.loadtxt(traj_path, skiprows=1)
x, y = data[:, 1], data[:, 2] # 提取 X 和 Y 坐标
fig, ax = plt.subplots(figsize=(6, 6))
ax.plot(x, y, lw=1.0)
ax.set_xlabel('X [m]')
ax.set_ylabel('Y [m]')
ax.set_title('XY Trajectory_python (Top View)')
ax.grid(True)
ax.set_aspect('equal', adjustable='box') # 保持 XY 比例一致
# 自动计算坐标轴范围并加上边距
dx, dy = (x.max()-x.min()), (y.max()-y.min())
pad = 0.05 * max(dx, dy) if max(dx, dy) > 0 else 1.0
ax.set_xlim(x.min()-pad, x.max()+pad)
ax.set_ylim(y.min()-pad, y.max()+pad)
plt.tight_layout()
plt.show()