码农三叔

还需注意的是，在每次模拟中，主动驾驶车辆与前车之间的初始距离保持不变（30米）。航向角的变化图和模拟快照（图（b）和图（c））表明该机动是自然完成的（没有不稳定的运动）。通过这些分析，可以评估控制策略在高速度超车场景中的有效性，突出主动车辆保持安全轨迹和速度的能力。通过图6-1能够直观地反映主动车辆在超车开始时的动态表现，包括其轨迹、方向和速度，有助于评估控制策略的有效性和车辆的安全性。在此过程中，为了展示鲁棒性，主动车辆的初始速度在20、25和30米/秒（45至67英里每小时）之间变化。

（1）下面代码的功能是设置一个环境，以便在 Google Colab 中安装所需的库和工具，并导入必要的模块，用于生成直道路段的超车轨迹。（7）下面代码的功能是利用优化模型解决一个控制问题，生成自车在给定状态下的轨迹，比较开环和闭环的轨迹，并将生成的轨迹进行可视化。（4）下面代码的功能是绘制自车在轨迹生成模型中运行的结果，包括自车的X坐标、Y坐标、速度和角度，并展示自车轨迹与障碍物的相对位置。（6）下面代码的功能是创建一个超车轨迹的动画，显示自车和障碍物的运动，并绘制不同车道的线路。

通过 Matplotlib 的 FuncAnimation 函数，代码创建了一个动态更新的图形，实时显示车辆的位置、速度和角度，同时绘制车道线和障碍物。（4）下面代码用于创建一个三维动画，展示自主车辆（ego vehicle）沿着预定轨迹行驶的过程，同时显示车辆的运动状态以及道路的不同车道。它定义了车道的形状、初始状态、障碍物的运动状态，并建立了一个优化模型，以最小化跟踪误差、控制输入变化和碰撞风险。然后，代码模拟了多个时间步长的状态变化，更新闭环状态并进行绘图，最后展示了闭环和开放式轨迹的对比。

近年来，人工智能、网络基础设施和传感器技术的进步，使得自动驾驶车辆的自主能力不断提升。同时，自动化和控制领域也在快速发展，以满足自动驾驶车辆操作的需求。汽车行业正致力于推进自动驾驶车辆实现端到端的自主能力。这一努力涉及到解决自动化过程中出现的挑战性极端情况，例如避开意外障碍物或停车。“超车”操作就是这样一种极端情况，它在自动化和自动驾驶车辆领域中提出了独特的挑战。与其他基本动作（如转弯或并道）不同，超车操作需要在顺序和及时性方面结合多种子操作（如变道、保持车道等）。

本项目实现了一个名为 RoboPID 的 PID 控制器，专为移动机器人设计，其主要功能包括：（1）动态控制：能够调节机器人的线性和角速度，以确保机器人在运动过程中保持稳定和精确的导航。（2）PID 控制算法：提供比例（P）、积分（I）和微分（D）控制，用户可以根据需求调整增益参数，以优化系统响应。（3）两种工作模式积分模式：在该模式下，PID 控制器内部处理时间步长的积分计算，适用于简单的实时控制场景。迭代模式：允许用户在外部算法中手动计算时间步长的积分，适合更复杂的控制需求。

否则，计算目标点的速度并根据逆运动学计算每个轮子的速度，发布适当的速度指令。和前面的文件controller_1.py相比，文件controller_2.py升级了运动控制逻辑：增加了路径点的定义、笔状态的处理以及更复杂的控制逻辑，适用于需要沿特定路径移动并控制多个机器人和笔状态的应用场景。（3）函数 rot_pts 的功能是将给定的坐标点 (x, y) 依据指定的角度 angle 进行旋转，该函数将角度从度数转换为弧度，并利用旋转矩阵计算旋转后的坐标点，最后返回旋转后的 x 和 y 坐标。

在主循环中，首先计算机器人 1 和机器人 3 之间的距离，并根据距离判断是否需要发布避免碰撞的速度指令。在控制过程中接收来自机器人位置的消息，根据计算的逆运动学速度控制机器人的运动，同时避免与其他机器人发生碰撞，并通过发布速度和笔的状态来控制机器人的行为。通过订阅机器人和笔的位置及状态话题，Controller能够接收机器人的位置信息和笔的状态，并发布相应的速度指令以控制机器人移动。在本项目的“task6_ws”目录中包含了实现一个综合的机器人控制系统的程序文件，负责机器人的运动控制、状态管理和位置估计。

在本项目中，文件feedback.py是一个 ROS2 节点，用于处理和分析相机图像中的 ArUco 标记，通过ArUco计算位置信息并发布到 ROS 2 主题。文件feedback.py从“/image_rect_color”话题接收图像，通过 ArUco 检测器识别图像中的标记，计算每个标记的位置和朝向，然后将这些数据发布到不同的话题上。代码还包括对图像进行透视变换和显示图像的功能，并维护了过去检测到的标记数据，以便在缺失数据时进行插补。

全向驱动机器人因其独特的设计和高效的移动能力，近年来在多个领域获得了广泛应用。与传统的轮式机器人相比，全向驱动机器人可以在保持固定朝向的情况下进行灵活的多方向移动，这一特性使其在狭小空间和动态环境中的表现尤为突出。这种自由移动的能力使其特别适用于自动化仓储系统，在这些系统中，机器人能够高效地在复杂的货架布局中进行物品的搬运和排列。全向驱动机器人的优势还体现在其对复杂环境的适应能力，特别是在自动驾驶和智能交通系统中，它们能够更好地处理复杂的导航任务和实时决策。

卫星的速度在地球固定坐标系 (ECEF) 中表示，但由于地球的自转，这会给卫星带来不符合物理的速度偏差，不适用于多普勒频移的计算。

随着卫星系统在全球通信、气象监测、军事和科学研究等领域的广泛应用，对精确速度估计的需求愈加迫切。在现代卫星导航和跟踪系统中，准确估计卫星的速度是确保导航精度和系统可靠性的关键。传统的速度估计方法在精度和实时性方面常常面临挑战，因此，需要采用更先进的技术来提升性能。本项目围绕基于多普勒频移和加权最小二乘法（WLS）的卫星速度估计展开。多普勒频移技术利用卫星信号的频率变化来推算其运动速度，而加权最小二乘法则通过最优化算法处理和融合不同来源的数据，从而提高估计的准确性。

类Tracker负责管理多个目标的跟踪过程，通过以下几个主要步骤来进行目标跟踪：预测每个目标的状态分布，更新目标的状态，进行目标与检测的匹配，管理和维护活动的跟踪目标，并根据检测结果初始化新的跟踪目标。（1）下面的代码实现了一个视频中目标跟踪的功能，首先加载视频文件，对每一帧图像进行目标检测和跟踪，使用 YOLOv3 模型进行目标检测，并通过深度学习特征编码器对目标进行特征提取。随后，卡尔曼滤波器用于预测目标的运动轨迹，并通过多目标跟踪算法管理目标的状态和匹配，实现对多个目标的持续跟踪和数据更新。

最近邻距离度量是一种用于目标跟踪和识别的算法，它通过计算特征向量之间的距离来确定两个样本的相似性。通过增量更新目标的特征库，最近邻距离度量能够动态适应目标的变化，确保在复杂环境中准确跟踪和匹配目标身份。下面的代码实现了非极大值抑制（Non-Maximum Suppression, NMS）算法，用于在目标检测任务中抑制重叠度超过设定阈值的边界框，保留检测置信度较高的框，以减少冗余的检测结果。下面的代码实现了一个最近邻距离度量功能，用于在目标跟踪场景中根据目标的特征计算与已有样本的最小距离。

（1）函数min_cost_matching的功能是通过线性分配算法解决跟踪和检测之间的匹配问题，此函数用于计算给定轨迹和检测之间的成本矩阵，并根据设定的阈值过滤掉高于最大距离的匹配。（3）函数gate_cost_matrix的功能是根据卡尔曼滤波器的状态分布来修正成本矩阵，通过对每个轨迹和检测对计算门控距离修正不可能的匹配项，将其成本设置为非常大的值。在本项目中，通过计算轨迹与检测之间的关联成本，应用线性分配算法来确定最佳匹配，同时考虑到卡尔曼滤波器预测的状态和门控距离，以提高匹配的准确性。

最后，从图像中的边界框提取补丁，利用预训练模型对这些补丁进行特征编码，并保存生成的特征向量。下面的代码实现了一个用于目标检测和特征提取的系统，其中类Detection用于表示图像中的一个检测结果，包括边界框坐标、置信度分数、类别名称和特征向量；下面代码的功能是从图像中的边界框提取图像补丁，并使用预训练模型对这些图像补丁进行特征编码，最后将生成的特征向量保存到指定目录中。具体包括加载保存的模型权重、设置YOLOv3模型的锚点（anchors）和输入图像的尺寸，以及设定检测对象的概率阈值。

随着计算机视觉和人工智能技术的飞速发展，实时目标跟踪已经成为许多应用领域中的重要技术。目标跟踪技术广泛应用于安全监控、智能交通、无人驾驶、运动分析等领域。卡尔曼滤波器作为一种经典的动态系统估计方法，因其在处理噪声和不确定性方面的优势，被广泛用于实时跟踪系统中。本项目旨在开发一个基于卡尔曼滤波器的实时跟踪系统，该系统能够高效地从视频中识别和追踪多个目标，为用户提供准确的目标位置和运动轨迹信息。这一技术不仅提高了目标跟踪的精度和稳定性，也为进一步的智能分析和决策提供了坚实的基础。

在下面的代码中，通过背景减除技术处理视频，以检测和高亮显示移动的物体，并在 Jupyter Notebook 环境中使用 Matplotlib 显示处理后的帧。如果检测到汽车并且有之前帧的位置数据则计算汽车的速度，若计算出的速度超过设定的速度限制则发出超速罚单，并记录在文本文件 Speeding_Ticket.txt 中。请看下面的的代码，使用预训练的目标检测模型（SSDLite320_MobileNet_V3_Large）从检测视频中的汽车，并根据检测到的车辆位置估算其速度。该模型用于检测视频中的汽车。

随着城市化进程的加快和机动车数量的增加，交通管理成为了现代城市面临的重要挑战之一。超速驾驶不仅增加了交通事故的风险，还对道路安全构成了严重威胁。传统的交通监控系统通常依赖于人工监控和固定的测速设备，这些方法不仅成本高昂，而且效率有限。近年来，计算机视觉和深度学习技术的发展为交通监控提供了新的解决方案。通过实时分析视频数据，这些技术可以自动检测和跟踪车辆，并准确估算其速度，从而提高了交通管理的智能化水平。本项目旨在构建一个智能车速检测系统，利用计算机视觉和深度学习技术对视频中的车辆进行检测和速度估算。

大会围绕世界机器人研究和应用重点领域以及智能社会创新发展，开展高水平的学术交流和最新成果展示，搭建国际协同创新平台，组织中国专家和国际同行研讨机器人发展创新趋势，明确机器人产业发展导向，探寻机器人革命对未来社会发展的深刻影响，为中国制定机器人产业发展战略、推动制造业转型升级提供决策参考，提升中国机器人产业的国际影响力。自2015年以来，世界机器人大会持续举办。十年来，我国机器人产业实现了“从小到大”“从大到强”的历史性跨越，大跨步迈入高质量发展阶段，人工智能与机器人深度融合，人形机器人创新成效显著。

与传统PID相比，很明显，在面对质量不确定性时，所提出的控制器的性能更好。此外，如果我们增加更多的质量，传统的PID方法将不会表现良好，因为其增益是为具有不同质量的系统设计的。当一个像四轴飞行器这样的欠驱动机器人能够在不同方向上跟踪期望的位置时，我们可以得出结论说系统的姿态控制已经成功实现，因为姿态控制是系统的内环，对于位置控制来说是首要的。上面的方程被称为风阵扰动的“整形滤波器”，其中qw是一个零均值的独立常数，τs=0.3 是风的相关时间，Bw是湍流输入单位矩阵，ρ=0.5 是标量加权因子。

在这个框架中，四轴飞行器的模型是完全未知的，只是连续使用系统的输入和输出。首先，选择在恒定高度上的方形路径作为跟踪的场景，如图8-17所示，在任务开始时，选择增益的方差（σ）较大，代理尝试选择不同的动作以改进识别。图8-18的可视化结果表明，该角度趋于零，这说明我们所提出的方法可以有效地控制四轴飞行器在一个简单的跟踪场景中的姿态，表现出了控制器的良好性能。当每个代理的奖励（如图8-19所示）和损失函数（如图8-11所示）随时间增加以达到最优点时，强调了网络权重已经被优化，从而形成了训练良好的网络。

具体来说，第一个子图显示了Z代理的损失函数（L_z），第二个子图显示了Phi代理的损失函数（L_\phi），第三个子图显示了Theta代理的损失函数（L_\theta），最后一个子图显示了Psi代理的损失函数（L_\psi）。（28）通过四个子图展示了在任务过程中每个代理的损失函数随时间的变化情况，每个子图表示一个代理的损失函数，横坐标是时间（秒），纵坐标是相应的损失值。（26）下面代码绘制了系统受到的高斯马尔可夫扰动的可视化图，展示了三个不同维度（d1、d2、d3）的扰动随时间的变化趋势。

执行后绘制了俯仰角（Pitch Angle）的可视化图，其中包括了俯仰角的实际值（用绿色虚线表示）、期望值（用灰色区域表示不确定性范围）、模型估计值（用灰色实线表示），如8-12所示。其中图中的第二个子图展示了PID控制器的增益随时间的变化情况，包括比例增益（Kp，用蓝色实线表示）、积分增益（Ki，用红色实线表示）、微分增益（Kd，用绿色实线表示）。（23）下面代码绘制了关于滚转角（phi）的可视化图，其中包括滚转角的实际值、期望值以及模型估计值，同时展示了控制器的比例、积分和微分增益随时间的变化。

（6）下面代码用于初始化控制输入，包括施加在四轴飞行器各轴上的控制输入（u1、u2、u3、u4）、期望的俯仰角和横滚角（tta_d、phi_d），以及用于积分项的变量（ei_psi、ei_phi、ei_tta、ei_z、ei_x、ei_y），这些变量将在控制器的运行过程中不断更新。（15）下面代码分别用于更新TTA轴和PSI轴上的飞行器，分别实现了对TTA轴和PSI轴的PID控制器的状态输入、目标输入的构建和网络输出的获取功能，同时记录了网络输出到tta_m和psi_m中的过程，并提取了动态增益参数。

训练过程中，考虑了高斯-马尔可夫扰动和计算奖励函数，同时绘制了各个代理的位置、角度、PID参数、奖励以及损失函数随时间的变化图，以全面评估系统性能。在深度学习中，训练模型通常包括神经网络的权重和偏置等参数。（2）初始化强化学习过程中的数值函数、奖励以及时间差分误差的相关变量，同时定义了一个在三维空间中的恒定高度上的方形轨迹作为期望轨迹，并设置了各个子系统的状态范围。综上所述，训练模型在这个项目中扮演着关键的角色，通过模型训练可以实现对PID参数的自主调节，提高无人机控制系统的性能、稳定性和智能化水平。

在优化过程中，Actor的目标是最小化输出估计误差，而Critic旨在最小化时间差分误差，通过计算两者的损失函数，以及额外的熵项，形成总体损失函数，最终通过ADAM优化器调整网络参数，提高模型性能。这个样本是Actor网络的最终输出，我们期望它能跟踪系统的实际输出，如图8-6所示，Actor网络的最终输出是从这个分布中随机抽取的。在上图8-8所示的总网络结构中，左侧网络为四轴飞行器的每个状态调整PID增益并计算相应的控制输入，右侧网络是Actor-Critic网络，用于估计每个状态。

文件utils/SMC_Controller.py定义了滑模控制器函数SMC，实现了四轴飞行器的滑模控制策略，使其能够在飞行过程中跟踪预定的轨迹。SMC控制器分别对高度、x轴和y轴方向进行控制，以及对滚转、俯仰和偏航进行控制。控制器使用了动态参数和期望轨迹的定义，对输入进行了分段控制，包括高度控制、x轴控制和y轴控制，以及滚转、俯仰和偏航的控制。此外，通过全局变量定义了四轴飞行器的一些动力学参数，这些函数和参数用于模拟四轴飞行器的动态行为以及在控制器训练中引入噪声和扰动。

文件utils/QuadParams.py定义了一些四轴飞行器的动力学参数，这些参数包括质量（m）、重力加速度（g）、臂长（l）、惯性矩（Jx、Jy、Jz）、推力和扭矩的常数（b、d）以及一些计算过程中用到的系数（a1、a2、a3、b1、b2、b3）。这些参数在整个代码中都被使用，以描述四轴飞行器的物理性质。m = 0.65;g = 9.81;l = 0.23;d = 7.5e-5;b1 = l/Jx;b2 = l/Jy;b3 = 1/Jz;

因此，根据下面的公式（2）所示，如图8-3所示，每个控制输入（u1,u2,u3,u4）是相应静态和动态增益输入的总和。四轴飞行器（如图8-1所示）是一个欠驱动系统，具有四个控制输入（u1、u2、u3、u4）和六个自由度（DOFs），包括位置（x、y、z）和姿态（φ、θ、ψ）。此外，还有一些用于初始化网络权重的函数。实际上，假设我们不知道系统的参数，并使用相应的控制输入和最近的状态来估计状态。在这组方程中，x、y、z是与参考坐标xI、yI、zI相关的重心位置，φ、θ、ψ是围绕xB、yB、zB轴的旋转角度。

四轴飞行器因其低成本、简单机械结构、垂直起降能力和高机动性等特点，被广泛应用于农业、搜索与救援、检测和监视等工业过程领域。由于执行器之间的耦合和外部干扰的不确定性，它们具有高度非线性的动态系统，这也是四轴飞行器轨迹跟踪和机动中姿态控制的角色至关重要的原因。即使在应用中执行了多种控制算法，但是在很多时候也无法满足现实要求，即使通过混合方法对通过干扰具有更好的适应性和鲁棒性。虽然PID控制器算法本身的简单性和易于实现而在实际工业系统中被广泛使用，但是，该控制器的精度高度依赖于其增益和系统模型。

上面的这些可视化图用于展示飞行器在仿真环境中的状态和控制信息。这有助于开发者和研究人员更好地理解飞行器的行为，评估控制算法的性能，并进行系统调试和优化。图中包括位置控制和速度控制的对比，以及姿态的实际值和期望值的对比，帮助分析控制算法的性能。图7-9 滚转、俯仰和偏航方向上的姿态的实际值与期望值的对比。图7-6 x 轴上的位置和速度的实际值与期望值的对比。图7-7 y 轴上的位置和速度的实际值与期望值的对比。图7-8 z 轴上的位置和速度的实际值与期望值的对比。

文件main.py实现了四轴飞行器的仿真控制系统的主程序，通过初始化飞行器模型、传感器和控制器生成期望轨迹，然后通过外层循环控制位置和内层循环控制姿态，最终通过Matplotlib库绘制轨迹与实际状态的对比图，以评估控制算法性能。（9）下面这段代码使用时间步长 step 在仿真时间范围内生成一个时间数组 t1，然后将之前保存的位置、速度、姿态、角速度、期望姿态等状态列表转换为NumPy数组，并且重新整形成相应的矩阵，以便进行后续的分析和可视化操作。（3）获取样条插值系数矩阵，具体实现代码如下所示。

（4）函数pos_control_PD(self, pos_atual, pos_des, vel_atual, vel_des, accel_des, psi)：这是实现位置控制的PD方法，用于计算期望位置的误差，处理法向量，并通过PID控制获取期望推力和姿态。（3）函数att_control_PD(self, ang_atual, ang_vel_atual, ang_des)：这是实现姿态控制的PD方法，用于计算期望角度的误差，并通过PID控制获取期望力矩。

其中函数__init__(self, env, velocity_plot=False, depth_plot=False)用于初始化方法接受仿真环境对象以及可选的参数，例如是否绘制速度信息（velocity_plot）和是否绘制深度图（depth_plot），然后根据输入参数设置绘图的相关属性。（9）reset(self, det_state=None)：用于初始化环境的状态，可以选择指定明确的初始状态（det_state），否则将生成一个随机状态。（21）gps(self)：用于模拟获取GPS的读数。

（2）下面这段代码定义了强化学习中的奖励参数，包括成功达到目标的奖励值、失败时的奖励值以及控制奖励的惩罚值等。根据飞行器的状态、控制输入和一些物理参数，计算飞行器的位置、速度、姿态和角速度的变化。其中包括风阻和陀螺效应的估算，以及对外部控制输入的处理，最终返回状态的变化。通过解线性方程组，将输入的推力和力矩映射到四个电机的角速度，并进行一些限幅处理，最终返回电机的角速度、新的力和新的力矩。（1）下面这部分代码定义了仿真中的边界框大小和飞行器的物理参数，包括质量、重力、空气密度、阻力系数、电机推力和力矩等。

本项目是一个全面的四轴飞行器仿真与控制平台，旨在为研究人员、学生和开发者提供一个实验环境，用于模拟和分析四轴飞行器的运动行为以及强化学习控制算法的性能。1.主要特点物理建模与仿真：采用先进的数学建模技术，精确模拟四轴飞行器的运动学和动力学，提供真实的飞行环境。传感器模拟：集成多种传感器模拟器，包括加速度计、陀螺仪、磁力计和GPS等，为控制算法提供丰富的感知信息。强大的运动控制：支持强大的运动控制功能的集成，用户可以设计、实施和评估各种强运动控制算法，从而优化飞行器的性能。

在文件test_basic.py中创建了一个 scheduler 对象 runObj，指定了输入类型为文本文件，源路径为 'r_inputs/randomStep.csv'，并且使用了一个具体的电路类型 PID_static_FOPDT，其中设置了该电路的各项参数。该测试使用了来自 r_inputs/randomStep.csv 文件的输入，并配置了具有特定参数的 RTCFL_RNN 电路，包括Plant模型（plant）和控制器（controller）的 RNN 模型参数、学习率、训练序列长度等。

文件scheduler.py是整个系统的调度器类，作用是使整个控制系统能够按照用户的设定正常运行，同时提供方便的接口来控制系统的行为和生成输出图。具体来说，类scheduler是整个应用程序的中心，负责设置所有的演员、执行主程序循环，并根据使用的电路在演员之间传递信息。（6）_parseInput 方法用于解析文本文件，用于简单地解析一个用作电路中r[t]输入的文本文件，并输出一个包含这些信息的浮点数列表，同时忽略'#'注释。它接受类型、源和电路等参数，根据类型选择不同的输入方式，并创建电路对象。

文件circuits.py实现了基于递归时间相关反馈线性化（RTCFL）的循环神经网络（RNN）控制电路，该电路包含的神经网络模型用于模拟黑盒Plant模型（plant_bb）和控制器（controller_nn），通过监督学习的方式迭代地优化Plant模型和控制器的性能。在本项目中，根据内部标志（_circuitModeFlag）的状态，将电路分为三个阶段：Step 1训练plant_nn，Step 2训练controller_nn，和最终的RTCFL优化模式。

（3）RNN是一个基于循环神经网络（RNN）的控制器，通过构造函数接收RNN堆叠数量、学习率和设备类型。这些控制器类均继承自controllerBase，并包含执行、训练和获取辅助信息的方法，提供了灵活性和可扩展性。（2）DNN是一个基于深度神经网络（DNN）的控制器，通过构造函数接收网络参数，包括输入维度、隐藏层结构、激活函数、学习率和设备类型。（1）PID_static是一个标准的静态PID控制器，通过构造函数接收PID参数，并实现了控制器的执行方法、参数调整方法以及获取辅助信息的方法。