ROS2(Cpp或Python)机器学习路径选择三维模拟平衡车及YOLOv8视觉消息

要点

机器人运动几何和运动学，Python短代码简述
ROS2创建包，使用C++和Python创建以下任务：创建发布者和订阅者并使用CMake编译，创建ROS2启动文件，创建自定义消息和Turtlebot3服务节点，使用ROS2组件编译视觉管道发布图像消息，创建并发处理和DDS功能
C++实现工业机器人二维和三维逆运动学求解器模板
6自由度ROS2协同机器人机器学习路径规划器，Gazebo 模拟：ROS2 平衡车，多机器人，自定义YOLOv8和ROS2视觉检测消息和发布者及订阅者
Python和ROS2：Raspberry Pi 4(树莓派) 控制无刷电机
3D图形游戏引擎模拟ROS2

机器人运动几何

几何的目的是描述空间、空间中的物体、它们的属性以及它们之间的关系。常见的概念是尺寸、距离、位置、角度和尺寸。常见的物体有点、线、平面、三角形、矩形、其他多边形、圆形、金字塔、长方体、其他多面体、圆柱体和球体。我们正在寻找的描述是可以在计算机程序中操作的正式描述。我们需要几何形状来对我们的机器人及其环境进行建模，从而定义我们希望（不）发生的事情。

例如，我们希望机器人 move() 和 navigate() 。我们必须能够以精确的数字方式表达这个目标。我们还必须对有关机器人和环境的几何信息进行编码，以规划和执行机器人的动作。机器人不应与自身或环境发生碰撞，但仍应以有目的的方式与其他物体进行物理交互。

物体由两个几何方面定义：形状，姿态。

形状的几何要素：

线的形状完全由其长度决定，即它只有一个参数。
圆的形状完全由其半径（或直径）决定，即它也只有一个参数。
三角形的形状可以通过多种方式确定，所有这些方式都唯一地定义了其形状。所有这些定义都包含三个参数。
矩形的形状完全由两个相邻边的长度决定，通常称为宽度和长度。
圆柱体的形状完全由圆和沿其高度的线（即两个参数）决定。
球体的形状，就像圆的形状一样，完全由其半径（或直径）决定。
长方体的形状完全由其三边的长度决定，换句话说，由底部矩形的宽度和长度以及沿其高度的线决定。

物体的姿态完全决定了它的行踪，即位置和方向。每当我们想要指定一个物体的姿态时，我们首先考虑的必须是我们想要定位该物体的空间的尺寸。维度可以定义为指定其中任何点所需的最小数量的值或坐标。因为机器人技术处理的是物体及其在不同空间中的运动，所以保持许多不同的概念会为你省去很多麻烦。

几何表示

在一维空间中，位置可以仅由单个数字表示。对这个数进行基本的数学运算，如加法、减法、乘法，都有相应的几何意义。两个数字相加对应于：

连接两个距离

将位置移动一定的距离。

理解一维中位置和距离/长度之间的这种模糊性至关重要，因为在更高维空间中也是如此，甚至更能解释。

a=3
b=2
c=a+b

对于一维几何来说，这可能意味着两种截然不同的事物。我们需要在我们的机器人软件中将这些不同的含义分开。以下两个代码语义片段执行相同的计算，但它们的语义很清晰。我们正在处理距离

a_to_b_distance = 3
b_to_c_distance = 2
a_to_c_distance = a_to_b_distance + b_to_c_distance

arm_segment1_length = 3
arm_segment2_length = 2
overall_arm_length = arm_segment1_length + arm_segment2_length

再次执行相同的计算，这无疑是关于计算新位置：

pick_up_position = 3
pick_up_to_drop_off_distance = 2
drop_off_position = pick_up_position + pick_up_to_drop_off_distance

人们容易混淆的原因不仅仅是代表许多不同概念的（原始）数据类型的问题。当涉及到空间表示时，混乱源于位置和距离之间的有效转换

长度+长度结果为长度（减法相同）
位置+长度产生位置（与减法相同）
位置 - 位置产生长度

添加两个位置没有意义。始终以保持位置与距离分开的方式命名变量。还有一种可以对距离/长度执行的基本运算：标量乘法。标量只是一个数字。术语标量乘法源自更高维度的运算，其中位置和变换不是单个数字，而是向量 - 正如我们将很快讨论的那样。一维中，标量乘法意味着将距离/长度乘以无单位数：

a_to_b_dist = 3
scaling_factor = 0.5
scaled_a_to_b_dist = scaling_factor * a_to_b_dist

重复一遍，此操作仅对距离/长度有意义，但对位置无效。

矢量

在 2D 和 3D 空间中，我们需要 2 个各自的 3 个数字来描述一个位置。使用正确的“数据结构”以及对其操作的适当定义使我们能够编写与一维情况非常相似的代码。为此，我们需要的数学工具是欧几里得向量或简称向量。就我们的目的而言，向量是一个有序的元组/数组，具有（至少）加法、减法、标量乘法和长度/幅度/范数作为定义的运算。

请注意，加法和减法仅针对相同大小的向量定义，即由相同数量的数字组成。在了解如何使用 numpy 在 Python 中对向量进行运算以及了解向量的数学符号之前，我们首先在基本 Python 中，实现三种向量运算：

import math
a_vec = [1, 2, 3]
b_vec = [4, 5, 6]
scalar = 2
def vector_addition(a, b): # a + bif len(a) != len(b):return Noneres = [0] * len(a)for i in range(len(a)):res[i] = a[i] + b[i]return res
def vector_scalar_multiplication(s, a): # s * ares = [0] * len(a)for i in range(len(a)):res[i] = s * a[i]return res
def vector_norm(a):sum = 0for val in a:sum += pow(val, 2)return math.sqrt(sum)
print(vector_addition(a_vec, b_vec)) # prints: [5, 7, 9]
print(vector_scalar_multiplication(scalar, a_vec)) # prints: [2, 4, 6]
print(vector_norm(a_vec)) # prints: 3.741...

虽然我们可以实现自己的向量类，包括运算符重载，但建议还是使用现有的库实现。 Python 中使用最广泛的向量和矩阵库是 numpy：

import numpy as npa_vec = np.array([1, 2, 3])
b_vec = np.array([4, 5, 6])print(a_vec + b_vec) # prints: [5, 7, 9]
print(scalar * a_vec) # prints: [2, 4, 6]
print(np.linalg.norm(a_vec)) # prints: 3.741...

让我们简单地看一下向量的数学符号。然后我们使用它们来处理 2D 和 3D 几何体。向量由粗体小写字母表示，各个数字由正常字体小写字母表示，并根据它们在向量中的位置进行排序。例如，3D 向量可写为 $\mathbf{a}=\left(\mathrm{a}_1, \mathrm{a}_2, \mathrm{a}_3\right )$ 或 $\mathbf{a}=\left[\begin{array}{l}a_1 \\ a_2 \\ a_3\end{array}\right]$

前者是行向量，后者是列向量。尽管在两种类型之间执行运算或涉及矩阵运算时，它们之间的差异可能很重要，但我们现在可以忽略这一点。使用这种表示法，向量加法可以写成
$\mathbf{c}=\left[\begin{array}{l} c_1 \\ c_2 \\ c_3 \end{array}\right]=\mathbf{a}+\mathbf{b}=\left[\begin{array}{l} a_1 \\ a_2 \\ a_3 \end{array}\right]+\left[\begin{array}{l} b_1 \\ b_2 \\ b_3 \end{array}\right]=\left[\begin{array}{l} a_1+b_1 \\ a_2+b_2 \\ a_3+b_3 \end{array}\right]$
标量乘法为
$s^* \mathbf{a}=\left[\begin{array}{c} s^* a_1 \\ s^* a_2 \\ s^* a_3 \end{array}\right]$
向量范数（欧几里得）为
$|\mathbf{v}|=\|\left[\begin{array}{l} v_1 \\ v_2 \\ v_3 \end{array}\right] \|=\sqrt{v_1^2+v_2^2+v_3^2}$
在简短介绍了向量、它们的符号和实现之后，我们就可以开始使用它们了。以下代码对应如图所示的几何运算

import numpy as npposition1 = np.array([1, 2])
translation1 = np.array([3, 1])
position2 = position1 + translation1position3 = np.array([6, 5])
translation2 = position3 - position1scalar1 = 0.5
translation3 = scalar1 * translation2

可以通过与值 -1 进行标量乘法或通过减法而不是加法来完成平移的反转或反转。在这方面，向量的行为与常规数字类似。
$\mathbf{a}+(-1) \cdot \mathbf{a}=\mathbf{a}-\mathbf{a}=\mathbf{0}$
与 1D 情况一样，两个位移/平移的加/减会产生位移/平移，位置的加/减和平移会产生一个位置，两个位置相减产生平移。

矩阵

简单看，矩阵只是一个二维数组，m×n 矩阵有 m 行和 n 列。在数学符号（粗体）中，大写字母通常用于矩阵。矩阵中的各个元素由一个小写字母和两个索引（行、列）表示。 3×4 矩阵可以写为
$\mathbf{A}=\left[\begin{array}{llll} a_{1,1} & a_{1,2} & a_{1,3} & a_{1,4} \\ a_{2,1} & a_{2,2} & a_{2,3} & a_{2,4} \\ a_{3,1} & a_{3,2} & a_{3,3} & a_{3,4} \end{array}\right]$
向量是矩阵的特例。它们是只有一行（1×n，行向量）或只有一列（m×1，列向量）的矩阵。由于索引之一的值始终为 1，因此索引的这一部分被删除，从而产生我们上面使用的向量表示法。与向量一样，加法和减法是按元素执行的。标量乘法的工作原理也与向量的解释相同。除了我们已经见过的这些运算之外，还有一种新运算：矩阵乘法。

兼容大小的矩阵可以相互相乘。这种乘法不是逐个元素进行的。相反，矩阵乘法定义如下。给定两个矩阵 $\mathbf{A}$ 和 $\mathbf{B}$ ，我们按元素计算结果矩阵 $\mathbf{C}=\mathbf{A B}$ 中的每个条目 $c_{i, j}$ A 的第 i 行与 B 的第 j 列相乘。表达为函数：

def matrix_multiplication(A, B):# The number of columns in matrix A# must be equal the number of rows in matrix Bif len(A[0]) != len(B):return None# C: Zero initialized matrix of size len(A) x len(B[0]),# i.e. A rows x B columnsC = ...for i in range(len(C)): # iterate rows in Cfor j in range(len(C[0])): # iterate columns in Cfor k in range(len(B)): # iterate rows in B (= columns in A)C[i][j] += A[i][k] * B[k][j]return C

旋转矩阵是沿着定义的旋转轴将向量旋转一定角度的矩阵。这里我们不会从三角学中导出旋转矩阵的条目。此时，您只需知道以下 2×2 旋转矩阵 $\mathbf{R}(\theta)$ 在与 (θ) 相乘时，可以正确地将 $\mathbf{R}(\theta)$ 2D 向量旋转 theta (θ) 度。
$\mathbf{R}(\theta)=\left[\begin{array}{cc} \cos (\theta) & -\sin (\theta) \\ \sin (\theta) & \cos (\theta) \end{array}\right]$
让我们使用新获得的知识和 Python 来验证上图c:

from math import pi, cos, sin, radians
points = [np.array([1.5, 0.5]), np.array([1.5, 1.7]), np.array([2.5, 1.5])]
theta = radians(30)
rotation_matrix = np.array([[cos(theta), -sin(theta)],[sin(theta), cos(theta)]])
for i in range(len(points)):points[i] = rotation_matrix @ points[i]
print(points)
# values rounded: [1.0, 1.2], [0.4, 2.2], [1.4, 2.5]

可以通过反转旋转矩阵来执行给定旋转的相反操作。给定旋转的相反旋转或逆旋转是旋转回到起始点的旋转。执行旋转然后执行相应的反向旋转与不执行任何旋转相同。用矩阵表示法表示，给定旋转矩阵 $\mathbf{R}$ 及其逆旋转矩阵 $\mathbf{R}^{-1}$ ，它们相乘的结果是单位矩阵 $\mathbf{I}$ ：
$\mathbf{R} \mathbf{R}^{-1}=\mathbf{I}$

机器人运动学

在机器人技术中，出现在两个主要环境中：

轨迹，即时间参数化路径
描述关节空间和 3D 空间之间关系的运动学方程

速度和加速度

给定起始姿势 $\mathbf{P}_1$ 、结束姿势 $\mathbf{P}_2$ 以及起始时间 $t_1$ 和结束时间 $t_2$ ，我们可以表达速度的计算公式中的 $\mathbf{V}$
$\mathbf{V}=\frac{\mathbf{P}_2-\mathbf{P}_1}{t_2-t_1}$
由于姿势由位置（以米为单位）和方向（以度或弧度为单位）组成，因此它们之间的移动速度不能用单个数字表示。相反，位置和方向是分开处理的。这也导致线速度 $\mathbf{v}$ （源自位置变化）和角速度 $\boldsymbol{\omega}$ (omega) （源自方向变化）的分离。