点到路径的平方距离

在轨迹规划中, 有时需要计算点到路径的距离以进行路径跟踪. 点到路径的平方距离扩展功能可以实现点到由多个线段组成的路径的最近平方距离计算.

在使用前需要先导入相关模块:

from optimake.extension import *

建模

下面为定义点到路径的平方距离的例子:

path = path_2d('road_middle', max_dim=10)
sqd_dist = squared_distance_to_path(path, px, py)

首先, 通过path_2d接口定义二维路径, 其入参如下:

• name: 路径名称
• max_dim: 组成该路径的最大点数, 必须为大于等于2的整数

信息

• 路径沿着第一段线段的反方向无限延伸
• 路径沿着最后一段线段的方向无限延伸
• 当上述的两条射线相交或者与路径的其他线段相交时, 则停止延伸

然后, 通过squared_distance_to_path接口定义点到路径的平方距离, 其入参如下:

• path: 通过path_2d接口定义的路径
• px, py: 需要计算平方距离的点的坐标, 关于优化变量$v$的表达式

通过该接口定义的sqd_dist可以作为目标函数或者约束进行建模.

例如, 下面为将点到路径的平方距离作为目标函数进行最小化的例子, 可以用于路径跟踪 (path following)问题:

prob.objective(sqd_dist + w_v * (v - v_ref) ** 2)

下面为将点到路径的平方距离作为不等式约束进行建模的例子, 表示点到路径的平方距离小于等于1.0:

ineq = general_inequality(expr=[sqd_dist], sign='<=', bound=[1.0])
prob.inequality(ineq)

求解

路径的数据都保存在prob结构体中. 在求解前, 需要设置路径数据. 下面为C/C++代码示例:

prob.path2d_road_middle.valid_dim = 4;
prob.path2d_road_middle.x[0] = 0.0; prob.path2d_road_middle.y[0] = 0.0;
prob.path2d_road_middle.x[1] = 10.0; prob.path2d_road_middle.y[1] = 0.0;
prob.path2d_road_middle.x[2] = 20.0; prob.path2d_road_middle.y[2] = 2.0;
prob.path2d_road_middle.x[3] = 30.0; prob.path2d_road_middle.y[3] = 2.0;

其中, 有以下说明:

• road_middle为路径的名称, 与建模时定义的名称一致
• valid_dim为路径的有效点数, 即通过多少个点定义该路径，必须小于等于建模时定义的max_dim且大于等于2
• x和y为路径上各个点的坐标数组

信息

• 当valid_dim小于2时, 求解器会返回0距离
• 当valid_dim大于max_dim时, 求解器会使用max_dim作为有效点数
• 计算量与valid_dim成正比, 因此建议合并相邻且共线的点以减少valid_dim