ROS2海龟仿真：两只海龟跟踪实验（一只键盘控制，一只自动跟随）
控制理论与工程应用基础作业
1. 环境准备
确保ROS 2已安装，并可以正常运行turtlesim模拟环境。首先，测试turtlesim节点是否能够正常启动：
ros2 run turtlesim turtlesim_node
正常运行，关闭页面。
2. 创建功能包
进入~/colcon_ws/src文件目录，使用以下命令创建一个新的ROS 2功能包，命名为cpp_a_follow_turtle_pkg，并添加必要的依赖关系：
cd ~/colcon_ws/src
ros2 pkg create --build-type ament_cmake cpp_a_follow_turtle_pkg --dependencies rclcpp tf2 tf2_ros tf2_geometry_msgs std_msgs geometry_msgs turtlesim --license Apache-2.0
3. 编写代码
您需要编写多个文件以实现小海龟的跟随功能。
3.1 创建海龟生成器
在src/cpp_a_follow_turtle_pkg/src目录下创建一个名为new_turtle_spawner.cpp的文件，拷贝以下代码：
#include 3.2 发布海龟坐标信息
创建一个名为turtles_pose_pub.cpp的文件，并拷贝以下代码：
#include 3.3 创建跟随控制器
创建一个名为turtle2_controller.cpp的文件，并拷贝以下代码：
#include 4. 创建Launch文件
在cpp_a_follow_turtle_pkg文件夹下新建一个launch文件夹，在launch文件夹中创建一个名为follow_turtle.launch.py的文件，内容如下：
#!/usr/bin/env python3
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
turtle1 = Node(
package='turtlesim',
executable='turtlesim_node',
name='turtle1',
output='screen',
)
turtle2 = Node(
package='cpp_a_follow_turtle_pkg',
executable='turtle_spawner',
name='turtle2',
output='screen',
)
turtle_pub1 = Node(
package='cpp_a_follow_turtle_pkg',
executable='turtle_pub',
name='tf_caster1',
arguments=['turtle1'],
output='screen',
)
turtle_pub2 = Node(
package='cpp_a_follow_turtle_pkg',
executable='turtle_pub',
name='tf_caster2',
arguments=['turtle2'],
output='screen',
)
turtle2_controller = Node(
package='cpp_a_follow_turtle_pkg',
executable='turtle2_controller',
name='turtle2_controller',
output='screen',
)
return LaunchDescription([
turtle1,
turtle2,
turtle_pub1,
turtle_pub2,
turtle2_controller,
])
5. 修改CMakeLists.txt
在CMakeLists.txt中“ament_package()”之前添加以下内容：
add_executable(turtle_spawner src/new_turtle_spawner.cpp)
add_executable(turtle_pub src/turtles_pose_pub.cpp)
add_executable(turtle2_controller src/turtle2_controller.cpp)
ament_target_dependencies(turtle_spawner rclcpp turtlesim)
ament_target_dependencies(turtle_pub rclcpp turtlesim tf2_ros geometry_msgs)
ament_target_dependencies(turtle2_controller rclcpp tf2_ros geometry_msgs)
install(TARGETS turtle_spawner turtle_pub turtle2_controller
DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME}/)
6. 编译和运行
在工作空间的根目录下执行以下命令以编译您的功能包：
cd ~/colcon_ws
colcon build --packages-select cpp_a_follow_turtle_pkg
7. 启动节点
使用以下命令启动节点，启动后弹出的界面会出现两只小海龟，其中一只小海龟会向另一只移动：
source ~/colcon_ws/install/setup.bash
ros2 launch cpp_a_follow_turtle_pkg follow_turtle.launch.py
8. 启动键盘控制
新打开一个终端，运行以下命令以控制turtle1的运动：
ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/turtle1/cmd_vel
具体键盘按键使用方法如下：
8.1 移动的键位绑定
- i : 向前移动
- , : 向后移动
- j : 向左转
- l : 向右转
- k : 停止
8.2 更多控制
- u、o : 向前左转和向前右转
- m、. : 向后左转和向后右转
- t : 向上移动（+Z轴）
- b : 向下移动（-Z轴）
- Shift + 键（如 U、J、I 等）：用于侧移（全向移动）。
8.3 速度控制
- q/z : 增加/减少最大速度 10%
- w/x : 增加/减少线速度 10%
- e/c : 增加/减少角速度 10%
8.4 工作原理
teleop_twist_keyboard 节点会将速度命令（Twist 消息）发布到 /turtle1/cmd_vel 主题上。海龟（或机器人）订阅这个主题，并根据接收到的命令移动。每次按键会更新线性和角速度，从而控制机器人的前进和转向。
9. 效果展示
10. 添加PID控制
在上述代码基础上添加PID控制，使turtle2以更加平滑和稳定的方式跟随turtle1。PID控制器可以根据误差（期望值与实际值的偏差）动态调整linear.x和angular.z的值，以避免过大的加速或摆动。
10.1 PID控制的原理
PID（Proportional-Integral-Derivative）控制器的输出可以由以下公式表示：
u ( t ) = K p ⋅ e ( t ) + K i ∫ e ( t ) d t + K d d e ( t ) d t u(t) = K_p \cdot e(t) + K_i \int e(t) \, dt + K_d \frac{de(t)}{dt} u(t)=Kp⋅e(t)+Ki∫e(t)dt+Kddtde(t)
其中：
- u ( t ) u(t) u(t)：控制输出，用于调整
turtle2的速度和方向。 - e ( t ) e(t) e(t)：误差，即
目标位置 - 当前位置。 - K p K_p Kp：比例系数，控制器的主要增益，作用是减少误差。
- K i K_i Ki：积分系数，用于消除系统的偏差。
- K d K_d Kd：微分系数，抑制误差的快速变化，防止系统振荡。
在此基础上，线性速度和角速度可以分别用PID控制器计算，以实现对linear.x和angular.z的平滑控制。
在每次计算控制量时，执行以下公式：
- 线性速度（
linear.x）：
u l i n e a r = K p ⋅ e l i n e a r + K i ⋅ ∑ e l i n e a r + K d ⋅ ( e l i n e a r − e l i n e a r , p r e v ) u_{linear} = K_p \cdot e_{linear} + K_i \cdot \sum e_{linear} + K_d \cdot (e_{linear} - e_{linear,prev}) ulinear=Kp⋅elinear+Ki⋅∑elinear+Kd⋅(elinear−elinear,prev) - 角速度（
angular.z）：
u a n g u l a r = K p ⋅ e a n g u l a r + K i ⋅ ∑ e a n g u l a r + K d ⋅ ( e a n g u l a r − e a n g u l a r , p r e v ) u_{angular} = K_p \cdot e_{angular} + K_i \cdot \sum e_{angular} + K_d \cdot (e_{angular} - e_{angular,prev}) uangular=Kp⋅eangular+Ki⋅∑eangular+Kd⋅(eangular−eangular,prev)
这样可以动态调整turtle2的速度和角度，使其尽量避免过大的摆动，并更准确地跟随turtle1。
10.2 PID控制的程序
将turtle2_controller.cpp文件中的代码替换为以下代码，重新编译运行：
#include 11. 添加模糊控制
11.1模糊控制的原理
在模糊控制系统中，模糊规则表用于定义输入变量与输出变量之间的关系。对于你的海龟跟随代码，我们可以构建一个模糊控制表，显示不同输入距离误差对应的控制量。
11.1.1输入模糊化（距离误差分类）
输入距离误差（distance_error）的模糊分类如下：
- 距离分类
- VERY CLOSE: 距离 < 0.1
- NB (Negative Big): 0.1 <= 距离 < 0.5
- NS (Negative Small): 0.5 <= 距离 < 1.0
- ZO (Zero): 1.0 <= 距离 < 1.5
- PS (Positive Small): 1.5 <= 距离 < 2.0
- PB (Positive Big): 距离 >= 2.0
11.1.2控制信号对应表
根据不同的距离分类，输出控制信号的模糊规则如下：
| 距离分类 | 控制信号 |
|---|---|
| VERY CLOSE | STOP |
| NB (Negative Big) | NS |
| NS (Negative Small) | ZO |
| ZO (Zero) | PS |
| PS (Positive Small) | PB |
| PB (Positive Big) | PB |
11.1.3控制信号对应的速度值
为了进一步清晰地展示如何将控制信号转换为具体的速度，以下是控制信号和具体速度值的对应关系：
| 控制信号 | 对应速度值 (linear.x) |
|---|---|
| NB | -0.5 |
| NS | -0.2 |
| ZO | 0.1 |
| PS | 0.2 |
| PB | 0.4 |
| STOP | 0.0 |
11.1.4示例模糊规则集
结合输入输出，我们可以定义一些示例模糊规则：
- 如果 距离是 VERY CLOSE ,那么控制信号是 STOP;
- 如果 距离是 NB ,那么控制信号是 NS;
- 如果 距离是 NS ,那么控制信号是 ZO;
- 如果 距离是 ZO ,那么控制信号是 PS;
- 如果 距离是 PS ,那么控制信号是 PB;
- 如果 距离是 PB ,那么控制信号是 PB.
通过上述模糊控制表和规则，系统能够根据与目标海龟的距离误差动态调整控制信号，从而实现平滑的跟随行为。这种模糊控制方法使得系统能够处理不确定性和非线性的问题，而无需复杂的数学模型。
11.2模糊控制的程序
将turtle2_controller.cpp文件中的代码替换为以下代码，重新编译运行：
#include 参考网址：figoowen2003.github.io