源码:/home/lby/project/c++/ompl-master/demos/RigidBodyPlanning.cpp
简介:使用RRTconnect 规划路径 start goal随机生成(包括位置向量、姿态四元数) 有完全规划和简单规划两个实现
SE3CompoundSpace0 的状态空间说明:
-
SE3CompoundSpace0 是一个复合状态空间,其维度描述了整体空间的复杂性和结构。在这个情况下,它包含两个子空间:
- RealVectorSpace1:一个3维的实数向量空间,代表位置信息。
- SO3Space2:一个4维的SO(3)空间,通常使用四元数表示姿态信息。
-
维度为6 的解释:
- 这里的维度6指的是整个复合空间 SE3CompoundSpace0 的维度,而不是简单地将位置向量的维度3和姿态四元数的维度4简单相加得到的7。
- SE3CompoundSpace0 的维度表示其内部复合的特性,包括两个子空间的维度。
-
位置向量和姿态四元数的分离:
- 在状态空间中,位置向量和姿态四元数是分开处理的,但它们都是 SE3CompoundSpace0 的一部分。
- 实际上,SE3CompoundSpace0 由一个3维的位置向量空间和一个4维的姿态空间组成,但它们合并在一起形成了一个维度为6的整体空间。
总结:
SE3CompoundSpace0 的维度为6表示整体状态空间的复合性,包含了位置和姿态两个独立的部分,它们在复合空间中是并列的,而不是简单的维度相加。这种表示方式使得在路径规划和状态空间描述中更容易管理和操作位置和姿态信息。
SE3 和SO(3) 空间的说明
-
SO(3) 空间:
- SO(3) 是三维空间中的特殊正交群,表示所有旋转矩阵的集合,这些矩阵具有单位行列式和正交特性。
- SO(3) 空间描述了物体在三维空间中的纯旋转部分,不包括平移。
-
SE3 空间:
- SE3 是三维空间中的特殊欧几里德群,包括所有包含平移和旋转的变换矩阵。
- SE3 空间描述了物体在三维空间中的位置和方向,即平移和旋转的组合。
-
SE3 包含 SO(3):
- 在 SE3 空间中,旋转部分通常表示为 SO(3) 空间的一部分。这意味着 SE3 的变换可以由一个三维的平移向量和一个旋转部分(通常用旋转矩阵或四元数表示)组成。
- 因此,SE3 可以看作是在 SO(3) 的基础上添加了平移的空间,它不仅可以描述物体的旋转状态,还可以描述物体在空间中的具体位置。
-
在机器人运动学中的应用:
- 在机器人运动学和路径规划中,通常使用 SE3 来描述机器人的末端执行器或工具的位姿(位置和姿态)。这样做既能描述位置的移动,也能描述姿态的变化,非常适合描述机器人在三维空间中的运动。
因此,SE3 包含 SO(3),但 SE3 的描述不仅仅局限于旋转,还包括了平移,这使得 SE3 在描述三维空间中的物体位姿时更加全面和准确。
state validity check resolution: 1%
- state validity check resolution (状态有效性检查分辨率):这是一个百分比,表示在路径规划中,状态的有效性检查的分辨率。
- 1% 意味着在路径上的每一个状态将以路径的1%为间隔进行检查。
- 例如,如果路径从点A到点B是100个单位长,则状态有效性检查将每隔1个单位进行一次。
- 这个参数确保路径上的每一段都经过充分的检查,以避免碰撞或进入无效区域。
valid segment count factor: 1
- valid segment count factor (有效段计数因子):这是一个因子,用于确定在路径规划过程中,每个路径段的检查次数。
- 1 意味着每个路径段将按照默认的段数进行检查。
- 这个参数通常用于调整路径段的分段数量,以确保路径的有效性和安全性。
state space
- state space (状态空间):这是路径规划中定义的状态空间,用于描述机器人或物体在空间中的可能位置和姿态。
- 在这个例子中,使用的是 SE3CompoundSpace,这是一个复合状态空间,包括实数向量空间和三维旋转空间(SO(3))。
- SE3CompoundSpace0: 表示一个六维的复合状态空间。
- Real vector state space (实数向量状态空间):包含三维空间中的位置坐标,其边界从 -1 到 1。
- SO(3) state space (SO(3) 状态空间):表示三维旋转,使用四元数进行表示。
具体解释:
-
有效性state validity check resolution:
- 这是一个百分比,表示路径段上进行状态有效性检查的分辨率。例如,1% 表示每1%路径长度进行一次检查。
-
valid segment count factor:
- 这是一个因子,用于调整路径段的检查细分数量。它可以看作是对默认分辨率的一个乘数因子。
- 如果
valid segment count factor
为1,则使用默认的检查分辨率。 - 如果
valid segment count factor
大于1,则检查的频率会更高(更多的检查点)。 - 如果
valid segment count factor
小于1,则检查的频率会降低(更少的检查点)。
举例说明:
假设路径段长度为1000个单位,state validity check resolution
为1%,valid segment count factor
为1。
- 根据
state validity check resolution
,路径段的1%为10个单位。 - 因此,在路径段上每10个单位插入一个检查点,总共会插入100个检查点。
有效性现在,假设 valid segment count factor
改为2。
- 这意味着检查频率加倍。
- 在路径段上每 10 / 2 = 5 个单位插入一个检查点。
- 总共会插入 1000 / 5 = 200 个检查点。
如果 valid segment count factor
改为0.5。
- 这意味着检查频率减半。
- 在路径段上每 10 / 0.5 = 20 个单位插入一个检查点。
- 总共会插入 1000 / 20 = 50 个检查点。
综上所述:
state validity check resolution
决定了路径段的基本检查分辨率。valid segment count factor
是对这个基本检查分辨率的一个调整因子,用于增加或减少路径段上的检查点数量。
通过这两个参数的配合,可以灵活地控制路径段检查的频率,以平衡计算资源和路径安全性。
说明:两个采样点AB,AB之间的路径通过上述步骤检测有效性。