✅ 博主简介：擅长数据搜集与处理、建模仿真、程序设计、仿真代码、论文写作与指导，毕业论文、期刊论文经验交流。

 ✅ 具体问题可以私信或扫描文章底部二维码。

1）基于距离-角度观测信息的多机器人领航跟随协同控制算法设计

地面无人集群协同控制的核心在于实现多个机器人在动态环境中的高效协作与稳定编队。本文提出一种基于距离-角度观测信息的领航跟随协同控制策略，该策略以领航机器人为核心，其余跟随机器人通过感知与领航者之间的相对距离和方位角信息，自主调整自身运动状态，以维持预设队形。相较于传统的基于全局坐标系的位置同步方法，该算法仅依赖局部相对观测，显著降低了对高精度定位系统（如GPS或UWB）的依赖，提高了系统在无基础设施环境下的适应能力。在建模方面，首先构建了以领航机器人为参考点的极坐标系下的相对运动模型，将每个跟随机器人的状态表示为与领航者之间的距离和角度。在此基础上，进一步推导出协同误差模型，该误差模型不仅包含位置偏差，还融合了方向偏差，从而更全面地反映编队状态的偏离程度。为确保系统稳定性，采用Backstepping递推设计方法，将复杂的非线性系统分解为多个子系统，逐层设计虚拟控制律，并最终合成实际控制输入。每一步设计均结合Lyapunov稳定性理论构造能量函数，证明误差系统在控制作用下能够渐近收敛至零。该控制律充分考虑了地面移动机器人的非完整约束特性（即无法横向滑移），通过合理分配线速度与角速度指令，避免了因运动学限制导致的跟踪失效。在实际部署中，每个跟随机器人仅需配备低成本的激光雷达或视觉传感器，即可实时提取与领航者之间的距离和角度信息，无需复杂的通信拓扑或全局地图。实验验证表明，无论在静态直线队形、V型编队还是动态变换队形场景下，该算法均能实现快速队形生成与高精度队形保持，即使在领航者突然转向或加速的情况下，跟随机器人也能在短时间内恢复稳定队形，表现出良好的鲁棒性与收敛性。此外，该算法支持动态增减机器人数量，具备良好的可扩展性，适用于搜索救援、物资运输、区域巡逻等多样化任务场景。

（2）基于贝塞尔曲线平滑与MPC路径跟踪的领航机器人运动控制

在多机器人协同系统中，领航机器人的路径质量直接决定了整个集群的运动效率与安全性。本文针对传统路径规划算法（如A*）生成的路径存在折点、不连续、不可导等问题，提出一套融合动态启发式A搜索、贝塞尔曲线平滑与模型预测控制（MPC）跟踪的完整路径生成与执行方案。首先，对标准A算法的启发式函数进行改进，引入动态权重机制，根据当前搜索节点与目标点的距离自适应调整启发式项的比重，在保证路径最优性的同时显著提升搜索速度，尤其适用于大规模或动态障碍物环境。随后，将A*输出的离散路径点作为控制点，构造高阶贝塞尔曲线进行平滑处理。贝塞尔曲线具有良好的几何连续性与局部可控性，通过合理选择控制点数量与阶数，可在保留原始路径走向的同时消除尖锐拐角。为确保平滑路径满足机器人运动学约束，本文在曲线生成过程中引入多重约束条件：一方面，对路径的位置、一阶导数（速度）和二阶导数（加速度）施加等式约束，保证路径在连接处的C²连续性，避免机器人执行过程中出现速度或加速度突变；另一方面，结合环境地图信息，对曲线上的关键点施加不等式约束，确保平滑路径始终位于自由空间内，远离障碍物边界，从而提升路径的安全裕度。在路径跟踪环节，采用模型预测控制（MPC）实现高精度跟踪。首先，基于领航机器人的非完整运动学模型（如差速驱动模型），利用泰勒展开对其进行局部线性化处理，再通过前向欧拉法将连续模型离散化，得到适用于数字控制器的离散状态空间方程。MPC控制器在每个控制周期内，以当前状态为起点，在有限时域内滚动优化未来若干步的控制输入序列，目标函数综合考虑路径跟踪误差、控制输入变化率及系统能耗等因素，并通过二次规划（QP）求解器实时计算最优控制量。该方法不仅能够有效抑制外部扰动（如地面摩擦变化、负载波动）对跟踪精度的影响，还能在路径曲率突变区域提前调整速度，避免因动力学限制导致的超调或失稳。实车测试表明，经贝塞尔平滑后的路径显著降低了机器人的转向频率与加速度峰值，MPC跟踪器在室内外复杂地形下均能实现厘米级的位置跟踪精度，路径执行流畅性与安全性大幅提升，为后续多机器人协同提供了高质量的运动基准。

（3）融合协同队形变换与自主避障的多机器人避障策略

在动态复杂环境中，多机器人系统需具备高效、灵活的避障能力。本文提出一种分层融合的避障控制策略，将宏观的协同队形变换避障与微观的跟随机器人自主避障有机结合，形成互补机制。对于大尺度静态或缓慢移动障碍物（如建筑物、大型车辆），系统优先采用协同队形变换方式进行规避。为此，预先构建一个多机器人协同队形库，包含直线、楔形、环形、疏散等多种典型队形，每种队形对应不同的任务需求与环境适应性。在此基础上，设计一套可变队形评价准则，综合考虑队形紧凑度、通信连通性、视野覆盖范围及避障所需空间等因素，对候选队形进行量化评分。当领航机器人检测到前方存在不可穿越障碍物时，触发队形变换决策机制：首先评估当前队形是否具备绕行能力，若否，则根据障碍物几何特征与环境约束，从队形库中筛选最优替代队形，并通过分布式协商机制协调所有机器人同步执行队形切换。该过程通过预设的过渡轨迹实现平滑变形，避免机器人间发生碰撞。对于突发性、小尺度或高速移动障碍物（如行人、小型车辆），则启用跟随机器人的自主避障能力。本文对传统人工势场法进行三项关键改进：一是引入距离自适应引力系数，当机器人接近目标时逐步减小引力强度，防止因引力过大导致冲过目标点；二是设计动态斥力场，结合障碍物速度信息预测其未来位置，提前生成斥力，缓解局部最优问题；三是引入虚拟目标点机制，在检测到目标点被势场“屏蔽”时，临时设定一个靠近真实目标的可达点作为导航目标，待脱离局部极小区域后再切换回原目标，有效解决目标不可达问题。为实现两种避障模式的无缝切换，设计多级避障决策机制：系统实时监测环境障碍物的尺度、速度及分布密度，若障碍物占据空间超过预设阈值或队形整体无法安全通过，则激活协同队形变换；否则，由各跟随机器人独立执行改进人工势场法进行局部避障。该机制通过共享局部环境感知信息（如障碍物位置、速度）实现有限通信下的协同避障，既避免了全局重规划的计算开销，又保证了个体灵活性

import numpy as np
import matplotlib.pyplot as plt
from scipy.special import comb
from cvxopt import matrix, solvers

class BezierPath:
    def __init__(self, control_points, num_points=100):
        self.control_points = np.array(control_points)
        self.num_points = num_points
        self.path = self.generate_bezier()

    def generate_bezier(self):
        n = len(self.control_points) - 1
        t = np.linspace(0, 1, self.num_points)
        path = []
        for ti in t:
            point = np.zeros(2)
            for i in range(n + 1):
                point += comb(n, i) * (ti ** i) * ((1 - ti) ** (n - i)) * self.control_points[i]
            path.append(point)
        return np.array(path)

class MPCController:
    def __init__(self, dt=0.1, N=10, Q=np.diag([1.0, 1.0]), R=np.diag([0.1, 0.1])):
        self.dt = dt
        self.N = N
        self.Q = Q
        self.R = R

    def linearize_model(self, x, u):
        theta = x[2]
        v, omega = u
        A = np.array([[1, 0, -v * np.sin(theta) * self.dt],
                      [0, 1,  v * np.cos(theta) * self.dt],
                      [0, 0, 1]])
        B = np.array([[np.cos(theta) * self.dt, 0],
                      [np.sin(theta) * self.dt, 0],
                      [0, self.dt]])
        return A, B

    def solve_mpc(self, x0, ref_path):
        n = 3
        m = 2
        num_vars = self.N * (n + m)
        P = np.zeros((num_vars, num_vars))
        q = np.zeros(num_vars)
        for k in range(self.N):
            idx_x = k * n
            idx_u = self.N * n + k * m
            P[idx_x:idx_x+n, idx_x:idx_x+n] += self.Q
            P[idx_u:idx_u+m, idx_u:idx_u+m] += self.R
            ref = ref_path[k] if k < len(ref_path) else ref_path[-1]
            q[idx_x:idx_x+n] = -2 * self.Q @ ref
        P = matrix(P)
        q = matrix(q)
        G = []
        h = []
        A_eq = []
        b_eq = []
        x = x0.copy()
        for k in range(self.N):
            A_lin, B_lin = self.linearize_model(x, [0.5, 0.0])
            idx_x_k = k * n
            idx_x_k1 = (k + 1) * n
            idx_u_k = self.N * n + k * m
            A_block = np.zeros((n, num_vars))
            A_block[:, idx_x_k:idx_x_k+n] = -np.eye(n)
            A_block[:, idx_x_k1:idx_x_k1+n] = np.eye(n)
            A_block[:, idx_u_k:idx_u_k+m] = -B_lin
            A_eq.append(A_block)
            b_eq.append(A_lin @ x)
            x = A_lin @ x
            G_block_u = np.zeros((2*m, num_vars))
            G_block_u[:m, idx_u_k:idx_u_k+m] = np.eye(m)
            G_block_u[m:, idx_u_k:idx_u_k+m] = -np.eye(m)
            G.append(G_block_u)
            h.extend([1.0, 1.0, 1.0, 1.0])
        A_eq = np.vstack(A_eq)
        b_eq = np.hstack(b_eq)
        G = np.vstack(G)
        h = np.array(h)
        A_eq = matrix(A_eq)
        b_eq = matrix(b_eq)
        G = matrix(G)
        h = matrix(h)
        sol = solvers.qp(P, q, G, h, A_eq, b_eq)
        u_opt = np.array(sol['x'])[self.N*n:self.N*n+m]
        return u_opt[0], u_opt[1]

class LeaderFollowerSystem:
    def __init__(self, num_followers=3):
        self.num_followers = num_followers
        self.leader_state = np.array([0.0, 0.0, 0.0])
        self.follower_states = [np.array([ -2.0*i, 0.0, 0.0]) for i in range(1, num_followers+1)]
        self.desired_offsets = [np.array([ -2.0*i, 0.0]) for i in range(1, num_followers+1)]
        self.mpc = MPCController()

    def update_leader(self, v_cmd, omega_cmd, dt=0.1):
        x, y, theta = self.leader_state
        x += v_cmd * np.cos(theta) * dt
        y += v_cmd * np.sin(theta) * dt
        theta += omega_cmd * dt
        self.leader_state = np.array([x, y, theta])

    def follower_control(self, follower_idx, dt=0.1):
        x_l, y_l, theta_l = self.leader_state
        x_f, y_f, theta_f = self.follower_states[follower_idx]
        dx = x_l - x_f
        dy = y_l - y_f
        rho = np.sqrt(dx**2 + dy**2)
        alpha = np.arctan2(dy, dx) - theta_f
        beta = theta_l - theta_f - alpha
        k_rho, k_alpha, k_beta = 1.0, 3.0, -1.0
        v_f = k_rho * rho * np.cos(alpha)
        omega_f = k_alpha * alpha + k_beta * beta
        if abs(alpha) > np.pi / 2:
            v_f = -v_f
        x_f += v_f * np.cos(theta_f) * dt
        y_f += v_f * np.sin(theta_f) * dt
        theta_f += omega_f * dt
        self.follower_states[follower_idx] = np.array([x_f, y_f, theta_f])

    def simulate(self, leader_path, total_time=20.0, dt=0.1):
        steps = int(total_time / dt)
        leader_traj = []
        followers_traj = [[] for _ in range(self.num_followers)]
        ref_path = leader_path.path
        current_ref_idx = 0
        for step in range(steps):
            if current_ref_idx < len(ref_path) - 1:
                target = ref_path[current_ref_idx]
                dist_to_target = np.linalg.norm(self.leader_state[:2] - target)
                if dist_to_target < 0.2:
                    current_ref_idx += 1
                v_cmd, omega_cmd = self.mpc.solve_mpc(self.leader_state, ref_path[current_ref_idx:current_ref_idx+self.mpc.N])
            else:
                v_cmd, omega_cmd = 0.0, 0.0
            self.update_leader(v_cmd, omega_cmd, dt)
            leader_traj.append(self.leader_state.copy())
            for i in range(self.num_followers):
                self.follower_control(i, dt)
                followers_traj[i].append(self.follower_states[i].copy())
        return np.array(leader_traj), [np.array(traj) for traj in followers_traj]

def main():
    waypoints = [[0, 0], [2, 1], [4, -1], [6, 0], [8, 2]]
    bezier_path = BezierPath(waypoints, num_points=50)
    system = LeaderFollowerSystem(num_followers=3)
    leader_traj, followers_traj = system.simulate(bezier_path, total_time=30.0)
    plt.figure(figsize=(10, 6))
    plt.plot(bezier_path.path[:, 0], bezier_path.path[:, 1], 'k--', label='Reference Path')
    plt.plot(leader_traj[:, 0], leader_traj[:, 1], 'b-', label='Leader')
    colors = ['r', 'g', 'm']
    for i, traj in enumerate(followers_traj):
        plt.plot(traj[:, 0], traj[:, 1], colors[i], label=f'Follower {i+1}')
    plt.scatter(np.array(waypoints)[:, 0], np.array(waypoints)[:, 1], c='black', marker='x')
    plt.legend()
    plt.axis('equal')
    plt.grid(True)
    plt.title('Leader-Follower Formation with Bezier Path and MPC Tracking')
    plt.show()

if __name__ == "__main__":
    solvers.options['show_progress'] = False
    main()

如有问题，可以直接沟通

👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇