规范导航器接口

技术文档（软件）
算法, 路径规划, 造轮子
Ydr

前言

通过前一段时间实现的动态窗口法导航，我对导航模块也有了更深一层的认识。为了使软硬件异构的机器人都能使用导航模块，我抽象出了导航模块，以及可导航运动平台的共性，其实现即规范导航器接口。本文主要介绍导航器的模型和概念，并辅以少量演示代码。

下文中“导航”与“局部路径规划”是等价的，可互换。

概念

以动态窗口法和定时弹带法为代表的一系列导航算法具有相同的运行过程，即预测→优化→控制。其中预测过程确定有效控制量的范围，并根据控制量预测机器人可能轨迹；优化过程从所有可能轨迹里找到最优的一条；控制过程执行最优轨迹对应的控制量。

局部路径规划中有2个重要的概念。

1. 轨迹 trajectory

轨迹正式局部路径规划算法应该给出的结果。那轨迹究竟是什么呢？请参阅 较为详细的阐述，但此处给出结论：所谓轨迹，即是时刻到机器人位姿的映射关系。

1
trajectory := time -> pose
1
2
3
4
5
6
7
8
9
10
11
12
/**
 * 轨迹
 * 时间到位姿的映射
 */
interface ITrajectory {
	...

	/** @return 某时刻的位姿 */
	operator fun get(time: Double): Pose?

	...
}

有了轨迹，就能对机器人的运动进行评判，从而选出最优的控制量。

2. 运动模型

从控制量获取轨迹的过程就是运动模型。用信号与系统的话说，控制是对定位平台的激励，而平台对控制的响应则是运动（轨迹），而激励到响应的传递函数就是运动模型。

1
model := current pose * command -> trajectory
1
2
3
4
5
6
7
8
9
10
11
12
13
14
/**
 * 可导航的
 * @param T 控制量类型
 */
interface INavigatable<T> {
	/**
	 * 轨迹预测
	 * 基于当前位姿、速度等状态量和控制量，计算未来一段时间的运动轨迹
	 * 各种运动学状态量表现为位姿的 n 阶导数
	 * @param current 当前位姿及位姿的 n 阶导数
	 * @param cmd     控制量
	 */
	fun predict(current: Map<Int, Pose>, cmd: T): ITrajectory
}

运动模型，尤其是有速度甚至加速度参与的精确的运动模型与机械结构的设计是密不可分的。所以运动模型的数学表达应该是由机械工程师提供的。近期我将另撰一文讨论机械控制和软件的界限问题。

轨迹的表示

轨迹通常来源于预测，而预测总是基于某种不变性的假设（如速度是常数或加速度是常数），因此轨迹具有时效性。定义域说明了轨迹有效的范围。

1
2
3
4
5
6
7
interface ITrajectory {
	/** 定义域 */
	val timeRange: DoubleRange

	/** @return 某时刻的位姿 */
	operator fun get(time: Double): Pose?
}

轨迹定义为时刻到机器人位姿的映射，但这种映射关系并不总是可以用函数表达的，它在不同的系统中可能是方程或其他不方便求解的特殊形式。对于这种情况，枚举通常是行之有效的表达方式。对连续轨迹进行采样，可以得到离散轨迹。采样点足够密集的离散轨迹也能正确描述一个运动。

1
2
3
4
5
6
7
8
/** 离散位姿点组成的轨迹 */
interface IDiscreteTrajectory : ITrajectory {
	/** 可获取精确位姿的周期 */
	val period: Double

	/** 丢弃时间信息，转为有序的位姿列表 */
	val path: List<Pose>
}

工程已经提供了对连续轨迹、等间隔采样离散轨迹和任意采样离散轨迹的实现，开发者可自行查阅。