智能控制——递阶控制
1 问题描述
考虑3台异构差动驱动机器人（R1、R2、R3），需在动态环境中协同完成编队跟踪任务。 系统需满足：
编队目标：形成并维持等边三角形队形（边长0.5米），同时跟踪参考轨迹。
动态约束：机器人运动受非完整约束（如差动驱动，速度限制），需避免相互碰撞 及环境障碍物。
总体目标：最小化编队跟踪误差（位置和队形误差），并保证系统稳定性。
请基于三级递阶控制架构设计控制系统，要求：
1、进行系统分解与层级设计
- 明确组织级、协调级、执行级的功能划分，画出该递阶控制系统的分级系统结构图。
- 定义各子系统的局部决策变量和目标函数。
2、分别采用以下两种协调方法进行协调策略设计
- 关联预测协调原则（直接干预法）
- 关联平衡协调原则（目标协调法）
2 系统分解与层级设计
对整个系统来说，控制的目标有整体跟踪参考轨迹、保持等边三角形队形、避障三个目标，第一级为组织级，其接收参考轨迹作为设定值。
组织级将任务分解为三个子任务后通过分派器分派给对应的协调器，分别对应三个目标，有全局轨迹协调器、避障协调器、队形协调器，协调器通过对各自任务的目标以及现有的控制器反馈回的值进行运算，得到控制指令下发给第三级执行级，即各种控制器。
而全局轨迹协调器和环境避障的协调器共同作用于其全局位置的控制器，负责控制机器人编队的全局位置；而机器人队形协调器和机器人之间的避障都作用于机器人相对位置的控制器。
整个系统可控的对象只有三个机器人的运动速度和方向以及其全局位置，而前者都由差速轮的电机决定，全局位置由另外的传感器决定（摄像头或者光电码盘）。两个被控对象之间也存在相互作用。故最终的分级系统结构图如下：
3 子系统的决策变量和目标函数
原题可分解为三个子问题：整体编队轨迹跟踪参考轨迹、机器人等边三角形编队、避障。对应可以分为三个子系统：总运动轨迹子系统，机器人队形保持子系统，安全保持子系统
3.1 总运动轨迹子系统
因为题目中并未提到三个机器人要以怎样的角度和顺序构成等边三角形，于是此处默认为参考轨迹指的是机器人三角形质心的轨迹。
决策变量：三个机器人组成的三角形质心的坐标 $$ p_c=[x_c,y_c] $$ ，质心的速度 $$ v_c=[|v_c|,\theta_c] $$
目标函数：最小化实际轨迹与参考轨迹之间的误差，其中既有此时刻绝对位置与参考轨迹上位置的误差，也有此时刻运动方向与参考轨迹上此点的切线方向的误差。
$$ J_1=||p_c-p_{ref}||^2+\lambda_1||\theta_c-\theta_{ref}||^2 $$假设初始状态都是0，速度与坐标有 $$ p_c(t)=\int^t_0v_c(t)dt $$ 的关系。
3.2 机器人队形保持子系统
决策变量：三个机器人各自的位置坐标 $$ p_1,p_2,p_3 $$ ，其中 $$ p_i=[x_i,y_i] $$ ；三个机器人各自的速度 $$ v_1,v_2,v_3 $$ ，其中 $$ v_i=[|v_i|,\theta_i] $$ 。
目标函数：使实际机器人之间的相对位姿尽可能接近等边三角形并且保持。接近三角形可用两两之间的相对距离为d=0.5m，保持三角形可用两两机器人之间在位置连线上的分速度之差为0 。
$$ J_2=\sum_{i,j}(||p_i-p_j||^2-d^2)^2+\lambda_2\sum_{i,j}E^2(v_i,v_j) $$其中 $$ E(v_i,v_j) $$ 为i，j两个机器人在其连线方向上的速度分量之差。假设初始状态都是0，速度与坐标有 $$ p_i(t)=\int^t_0v_i(t)dt $$ 的关系。
3.3 安全保持子系统
决策变量：三个机器人各自的位置坐标 $$ p_1,p_2,p_3 $$ ，其中 $$ p_i=[x_i,y_i] $$ ；三个机器人各自的速度 $$ v_1,v_2,v_3 $$ ，其中 $$ v_i=[|v_i|,\theta_i] $$ 。
对这个系统来说，用其监测整个系统的硬约束比建立一个目标函数的效果更好，如果建立目标函数，定义为三个机器人的位置坐标与已知障碍点的位置坐标之间的距离以及机器人之间的距离的加权和，并期望其越大越好，这可能会产生冲突，比如说已经有一个机器人与一个障碍物撞上了，但是整体上距离之和依然很大，没有起到保持安全的作用，或者决策函数用乘法，那么其距离关系更难以预测了，不如用硬约束来约束他们。
速度上限约束： $$ |v_i|\leq v_{max} $$
差动驱动约束： $$ \dot{x}_i = v_i\cos\theta_i,\quad \dot{y}_i = v_i\sin\theta_i $$
环境避障约束： $$ ||p_i-o_j||\geq d_{safe1} $$
机器人之间不碰撞约束： $$ ||p_i-p_j||\geq d_{safe2} $$
系统关联：机器人组成的三角形的质心的坐标与三个机器人坐标之间的关系为 $$ p_c=\frac{1}{3}(p_1+p_2+p_3) $$ ，机器人组成的三角形的质心的速度与三个机器人的速度之间的关系为 $$ v_c=\frac{1}{3}(v_1+v_2+v_3) $$ (矢量和)
总体目标：整体误差最小，即 J1 与 J2 的加权和最小
$$ min J=J_1+\lambda J_2=(||p_c-p_{ref}||^2+\lambda_1||\theta_c-\theta_{ref}||^2)+\lambda(\sum_{i,j}(||p_i-p_j||^2-d^2)^2+\lambda_2\sum_{i,j}E^2(v_i,v_j)) $$其中的 J 是每个时刻的误差值，对整体系统的期望是其在整个过程中的所有累积误差尽量小，尽量接近0，即：
$$ min J_{all}=\int^{t_0}_0J(t)dt $$协调参数：协调器需要处理关联变量 $$ p_c $$ 和 $$ p_i $$ ， $$ v_c $$ 和 $$ v_i $$
约束： $$ |v_i|\leq v_{max} $$ $$ \dot{x}_i = v_i\cos\theta_i,\quad \dot{y}_i = v_i\sin\theta_i $$ $$ ||p_i-o_j||\geq d_{safe1} $$ $$ ||p_i-p_j||\geq d_{safe2} $$
4 协调策略设计
我们计算总体目标误差时，是将三个机器人各自在整个过程中的速度函数
$$
v_i(t)
$$
代入
$$
J_{all}
$$
公式中运算的，即对每个时刻的误差结果积分后的总结果进行评估。具体优化
$$
v_i(t)
$$
时，即优化其在某些时刻的值时，会用此时刻的总体误差最小来作为目标进行优化。
4.1 关联预测协调原则（直接干预法）
- 协调级预测关联：预测三个机器人整个运动过程中的速度函数为 $$ v_i^* $$ ，其运动轨迹为 $$ p_i^*(t)=\int^t_0v_i^*(t)dt $$ ；构成的三角形质心的速度函数为 $$ v_c^*=\frac{1}{3}(v_1^*+v_2^*+v_3^*) $$ ，轨迹函数为 $$ p_c^*=\frac{1}{3}(p_1^*+p_2^*+p_3^*) $$
- 子系统决策：在约束 $$ v_i=v_i^* $$ 下，优化内部参数使得 $$ \int^{t_0}_0J_1dt $$ 最大，在约束 $$ v_c=v_c^* $$ 下，优化内部参数使得 $$ \int^{t_0}_0J_2dt $$ 最大
- 协调级修正关联：检查此时的总误差 $$ J_{all} $$ ，看是否达到预期，是否还能够下降；若还能下降，就微调 $$ v_i $$ ，注意要满足硬约束。
- 迭代：重复步骤1-3，直到总误差不能再降。
其中每一次迭代的解都是可行解，但是迭代过程太慢了，需要内部一次次根据局部误差最小进行优化。
4.2 关联平衡协调原则（目标协调法）
- 协调器引入干预变量：
$$
p_{\beta}
$$
- 总运动轨迹子系统新目标为 $$ J'_1=J_1+||p_c-p_{\beta}||^2 $$
- 机器人队形保持子系统新目标为 $$ J'_2=J_2+\sum_{i=1}^3(||p_i-p_{\beta}||^2) $$
- 子系统决策：忽略约束的情况下，分别独立求解 $$ min J'_1 $$ ， $$ minJ'_2 $$ ，得到最优 $$ p_c^*，p_i^* $$
- 协调级修正子目标(平衡关联)：检查关联是否平衡：求 $$ e=||p_c^*-\frac{1}{3}(p_1^*+p_2^*+p_3^*)||^2 $$ ，观察其是否为0，若不是0，调整 $$ p_{\beta} $$ 使得e尽量接近0
- 迭代：重复步骤1-3，直到e趋于0
子问题每次没有关联约束，都有解；但是迭代过程中的解都不满足关联约束说明其都不能作为真实解，只能离线规划，不能在线运行。