版本：v0.7.0 beta

胶囊体避障

在机械臂、无人机等三维场景的轨迹规划中, 胶囊体 (capsule) 常用来包络连杆、机身或杆状障碍物. 胶囊体为到一条线段 (轴线) 的距离不超过半径 $r$ 的所有点组成的实体, 与使用多个包络球相比, 单个胶囊体即可紧凑地包络细长物体, 且距离计算简单.

支持以下两种胶囊体避障不等式约束:

point_to_capsule_inequality: 点对胶囊体的避障不等式约束
capsule_to_capsule_inequality: 胶囊体对胶囊体的避障不等式约束

两种约束均使用精确的有符号距离: 分离时为两者之间的最短距离, 重叠时为负的穿透深度. 胶囊体的端点和半径可以为常数、参数或优化变量的表达式, 因此可以处理静态障碍、动态障碍以及机械臂自碰撞等问题.

胶囊体定义

胶囊体通过capsule_3d接口定义:

capsule = capsule_3d(p1, p2, radius)

参数	说明
`p1`, `p2`	轴线的两个端点, 可以为长度为3的list/tuple或3维向量 (例如`Matrix`)
`radius`	胶囊体的半径, 为数值时必须非负

信息

p1, p2的各元素与radius可以为常数、关于参数或优化变量的表达式. 例如, 障碍胶囊体的几何量可以通过参数传入, 机械臂连杆胶囊体的端点可以为关节角的正运动学表达式.
p1与p2重合时, 胶囊体退化为球体; radius为0时, 胶囊体退化为线段.

点对胶囊体避障

点对胶囊体的避障功能可以实现点与胶囊体之间的避障约束, 以确保点不会进入胶囊体所占据的区域.

建模

下面为点对胶囊体的避障不等式约束的例子:

p2c_ineq = point_to_capsule_inequality(
    point, capsule,
    distance_to_avoid,
    use_hessian=False
)
prob.inequality(p2c_ineq)

其中, point_to_capsule_inequality接口的入参如下:

point: 需要避障的点的坐标 $(p_x, p_y, p_z)$ , 格式与capsule_3d的端点相同
capsule: 通过capsule_3d定义的胶囊体
distance_to_avoid: 避障距离, 可以为常数、关于参数或优化变量的表达式
use_hessian: bool类型, 是否提供距离的解析Hessian, 默认为False

该接口构建的不等式约束为:

d = \min_{t \in [0, 1]} \left\lVert p - p_1 - t\,(p_2 - p_1) \right\rVert - r \ge d_{safe}

其中, $p$ 为需要避障的点, $p_1$ , $p_2$ 与 $r$ 分别为胶囊体的轴线端点与半径, $d_{safe}$ 为避障距离. 点在胶囊体外部时, $d$ 为点到胶囊体表面的距离; 点在胶囊体内部时, $d$ 为负的穿透深度.

胶囊体对胶囊体避障

胶囊体对胶囊体的避障功能可以实现两个胶囊体之间的避障约束, 以确保两个胶囊体不会发生碰撞.

建模

下面为胶囊体对胶囊体的避障不等式约束的例子:

c2c_ineq = capsule_to_capsule_inequality(
    capsule_a, capsule_b,
    distance_to_avoid,
    use_hessian=False
)
prob.inequality(c2c_ineq)

其中, capsule_to_capsule_inequality接口的入参如下:

capsule1, capsule2: 通过capsule_3d定义的两个胶囊体 (即下图中的胶囊体a与胶囊体b), 两者的顺序可以互换
distance_to_avoid: 避障距离, 可以为常数、关于参数或优化变量的表达式
use_hessian: bool类型, 是否提供距离的解析Hessian, 默认为False

该接口构建的不等式约束为:

d = \min_{s, t \in [0, 1]} \left\lVert p_{a1} + s\,(p_{a2} - p_{a1}) - p_{b1} - t\,(p_{b2} - p_{b1}) \right\rVert - r_a - r_b \ge d_{safe}

即两条轴线之间的最短距离减去两个胶囊体的半径. 两个胶囊体分离时, $d$ 为两者表面之间的最短距离; 两个胶囊体重叠时, $d$ 为负的穿透深度.

注意

两条轴线平行时, 轴线之间的最近点不唯一, 距离在该处不可导. 当胶囊体的姿态为优化变量时, 少数情况下迭代会在平行姿态附近振荡直至达到最大迭代次数, 但迭代点仍满足避障约束, 可通过设置提前退出条件来避免长时间振荡.

建模示例

下面为利用胶囊体对胶囊体的避障功能实现的iiwa14机械臂带自碰撞约束的逆运动学问题设定:

使用4个胶囊体包络机械臂: 固定的基座胶囊体 (半径0.15 m), 以及上臂、前臂、腕部3个连杆胶囊体 (半径分别为0.164 m, 0.147 m, 0.099 m). 连杆胶囊体的端点为关节角的正运动学表达式, 通过Pinocchio的CasADi接口计算
对3组不相邻的胶囊体 (基座与前臂, 基座与腕部, 上臂与腕部) 施加胶囊体对胶囊体的避障约束, 避障距离为dsafe

# model, symbolic_data为Pinocchio的CasADi模型与数据, 已根据关节角变量完成正运动学计算
BASE_CAPSULE = ((0.0, 0.0, -0.125), (0.0, 0.0, 0.125), 0.15)
LINK_CAPSULES = (
    ("upper_arm", "iiwa_link_2", "iiwa_link_4", (0, .25, .5, .75, 1), .164),
    ("forearm", "iiwa_link_4", "iiwa_link_6", (0, .25, .5, .75, 1), .147),
    ("wrist", "iiwa_link_6", "iiwa_link_ee", (0, .5, 1), .099),
)
SELF_COLLISION_PAIRS = (("base", "forearm"), ("base", "wrist"), ("upper_arm", "wrist"))
capsules = {"base": capsule_3d(*BASE_CAPSULE)}
for name, frame_a, frame_b, _, radius in LINK_CAPSULES:
    a = symbolic_data.oMf[model.getFrameId(frame_a)].translation
    b = symbolic_data.oMf[model.getFrameId(frame_b)].translation
    capsules[name] = capsule_3d(a, b, radius)
for first, second in SELF_COLLISION_PAIRS:
    prob.inequality(capsule_to_capsule_inequality(capsules[first], capsules[second], dsafe))

胶囊体定义​

点对胶囊体避障​

建模​

胶囊体对胶囊体避障​

建模​

建模示例​

胶囊体定义

点对胶囊体避障

建模

胶囊体对胶囊体避障

建模

建模示例