✨ 本团队擅长数据搜集与处理、建模仿真、程序设计、仿真代码、EI、SCI写作与指导，毕业论文、期刊论文经验交流。
✅ 专业定制毕设、代码
✅如需沟通交流，查看文章底部二维码
（1）各向异性代价地图与RRT*-Connect的集群路径规划器：
考虑到四足机器人在不同地形上的通过能力差异（如斜坡、碎石、泥地），提出了一种各向异性代价地图，其中每个栅格的代价值不仅与地形类型有关，还与机器人的运动方向有关。例如，侧向通过斜坡的代价比正向爬坡高一倍。在此代价地图基础上，扩展RRT*-Connect算法为集群版本，同时规划多台四足机器人的路径。算法将每台机器人的位置与姿态角作为联合控制输入，在扩展节点时采用带优先级的并行探索策略，保证机器人间不发生碰撞且路径总和代价值最小。同时，在代价函数中引入了步态周期性因子，使得路径倾向于选择与机器人当前步态相位匹配的地形过渡点。在多个复杂地形场景（含45度碎石坡、0.3米高障碍）的对比测试中，该算法相比标准RRT-Connect搜索成功率达到94.3%，路径总长度平均缩短18.6%，且搜索时间减少27%。
（2）基于凸优化的分层编队控制与速度约束修正：
将集群编队控制问题解耦为两层：上层负责生成参考队形轨迹，下层负责单个机器人跟踪。上层采用凸优化方法求解一个带线性矩阵不等式约束的二次规划问题，目标是最小化各机器人实际位置与期望队形位置的偏差，并满足最大前后移动速度（1.2m/s）和侧向速度（0.6m/s）的不等式约束。该凸优化问题的决策变量还包括队形缩放因子，以自适应通过狭窄通道。下层则采用模型预测控制跟踪上层给出的参考速度，并考虑四足机器人步态周期内的质心高度变化。实验验证，在四台宇树A1机器人组成的菱形编队中，编队保持的横向误差均方根小于4.2厘米，在有动态障碍物干扰时，编队重构可在0.6秒内完成。
（3）增量式步态相位同步与改进目标约束的凸优化目标跟踪：
针对传统步态相位模型在控制指令突变时会出现相位跳变的问题，提出了增量式步态相位计算方法，即相位增量与期望速度的积分挂钩：Δφ = k_v * v_des * dt。这样即使期望速度突变，相位也连续变化，避免了机器人因相位跳变而摔倒。同时改进了凸优化目标约束函数，将目标点的航向角也纳入优化变量，使得机器人在跟踪全局路径时能够提前转向，减少不必要的绕行。步态同步控制器采用模型预测控制框架，预测时域为0.5秒，控制周期20ms，目标函数包含相位同步误差项。实物实验中，四台机器人在复杂场地（含减速带和窄门）进行协同巡逻，编队内部最大步态相位差不超过8度，成功完成直线编队变换为菱形编队的动态切换，且全程无碰撞。
import numpy as np import cvxpy as cp from scipy.spatial import KDTree # 各向异性代价地图（简化） class AnisotropicCostMap: def __init__(self, width, height, resolution): self.w, self.h, self.res = width, height, resolution self.terrain = np.zeros((height, width)) # 0平路 1碎石 2斜坡 3泥地 self.slope_angle = np.zeros((height, width)) def cost(self, x, y, direction_angle): ""方向角为机器人朝向，返回通过该栅格的代价"" terrain_type = self.terrain[int(y/self.res), int(x/self.res)] base_cost = {0:1.0, 1:2.5, 2:1.8, 3:4.0}[terrain_type] # 方向性修正：侧向通过斜坡增加代价 if terrain_type == 2: slope_dir = self.slope_angle[int(y/self.res), int(x/self.res)] direction_penalty = 1 + abs(np.sin(direction_angle - slope_dir)) * 1.5 return base_cost * direction_penalty return base_cost # RRT*-Connect集群路径规划（关键扩展函数） def cluster_rrt_star_connect(start_poses, goal_poses, cost_map, max_iter=500): ""多机器人并行RRT*，返回各机器人路径"" trees = [{'nodes': np.array([start]), 'parent': [-1]} for start in start_poses] for _ in range(max_iter): for i in range(len(trees)): # 随机采样，偏向各自目标 if np.random.rand() < 0.1: sample = goal_poses[i] else: sample = np.random.rand(2)*cost_map.h # 寻找最近节点 tree = trees[i] nearest_idx = np.argmin(np.linalg.norm(tree['nodes']-sample, axis=1)) # 扩展新节点（实现steer函数） new_node = steer(tree['nodes'][nearest_idx], sample, step=0.5) if not collision(new_node, cost_map): # 寻找最优父节点 best_parent = nearest_idx # 重新布线 trees[i]['nodes'] = np.vstack([trees[i]['nodes'], new_node]) trees[i]['parent'].append(best_parent) # 连接到其他树？简化，省略连接逻辑 # 检查是否所有机器人都到达各自目标区域 return [reconstruct_path(t) for t in trees] # 凸优化编队控制器 def formation_control_convex(current_positions, desired_relative_positions, v_max_fwd=1.2, v_max_side=0.6): n = len(current_positions) p_cur = np.array(current_positions).reshape(-1) # 决策变量：各机器人速度指令vx,vy v = cp.Variable(2*n) # 期望速度：维持队形所需的修正量（P控制） p_des = np.array(desired_relative_positions).reshape(-1) + np.mean(current_positions, axis=0).repeat(2) error = p_cur - p_des objective = cp.Minimize(cp.sum_squares(v + 0.5*error)) # 速度限制 constraints = [] for i in range(n): constraints.append(cp.abs(v[2*i]) <= v_max_fwd) constraints.append(cp.abs(v[2*i+1]) <= v_max_side) prob = cp.Problem(objective, constraints) prob.solve(solver=cp.OSQP) return v.value.reshape(n,2) # 增量式步态相位同步 class IncrementalGaitSync: def __init__(self, num_robots, gait_cycle_time=0.8): self.n = num_robots self.phase = np.zeros(num_robots) # 每个机器人的步态相位 [0,1) self.T_cycle = gait_cycle_time def update(self, desired_velocities, dt): # 相位增量与期望速度幅值成正比 speed_norms = np.linalg.norm(desired_velocities, axis=1) self.phase += speed_norms / self.T_cycle * dt self.phase = np.mod(self.phase, 1.0) # 同步控制：调整相位使群体趋于一致 mean_phase = np.mean(self.phase) self.phase = 0.9*self.phase + 0.1*mean_phase # 简单耦合 return self.phase if __name__ == '__main__': cmap = AnisotropicCostMap(100, 100, 0.1) start_pos = np.array([[10,10], [12,10], [10,12], [12,12]]) goal_pos = np.array([[90,90], [92,90], [90,92], [92,92]]) paths = cluster_rrt_star_connect(start_pos, goal_pos, cmap) # 编队控制演示 cur = start_pos + np.random.randn(4,2)*0.2 rel = np.array([[-0.5,0], [0.5,0], [0,0.5], [0,-0.5]]) vel_cmd = formation_control_convex(cur, rel) sync = IncrementalGaitSync(4) phases = sync.update(vel_cmd, 0.05) print(f'速度指令: {vel_cmd}\n步态相位: {phases}') ",如有问题，可以直接沟通
👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇
