44、移动机器人的运动控制与定位技术

移动机器人的运动控制与定位技术

1. 轨迹跟踪模拟

在移动机器人的研究中，轨迹跟踪是一个重要的课题。通过输入/输出线性化的控制器对独轮车进行轨迹跟踪模拟，有两个不同参数的模拟情况。

1.1 模拟一（b = 0.75）

独轮车实际沿着“平滑”轨迹移动，因此在轨迹周围需要有一个无障碍物的通道，以考虑独轮车在方形顶点处扫过的区域。

1.2 模拟二（b = 0.2）

当减小参数b以实现独轮车代表点更精确的跟踪时，虽然独轮车相对于第一次模拟更紧密地跟踪方形轨迹，但在顶点处转向速度要高得多。这在速度输入存在饱和的情况下可能会成为问题，因为当b接近零时，相关矩阵T趋于奇异。

2. 调节问题

2.1 反馈控制律设计的挑战

设计一个反馈控制律，使独轮车达到期望的配置是一个具有挑战性的问题。之前描述的跟踪方案都不能直接用于此目的。基于近似线性化的控制器和非线性控制器都需要持续的状态轨迹；基于输入/输出线性化的方案虽然可以跟踪非持续轨迹，但会驱动点B到达目的地，而不是独轮车的代表点，并且无法控制目的地的最终方向。

由于系统的非完整性，独轮车不存在通用控制器，即不能渐近稳定任意状态轨迹（持续或非持续）的控制器。这与机械手的情况完全不同，机械手基于逆动力学的方案是通用控制器的一个例子。因此，非完整移动机器人的姿态调节问题必须使用专门设计的控制律来解决。

2.2 笛卡尔调节

2.2.1 问题描述

设计一个反馈控制器，使独轮车到达给定的笛卡尔位置，而不指定最终方向。这在实际应用中很有意义，例如移动机器人探索未知环境时，只要传感器各向同性分布，机器人在视点的方向就无关紧要。

2.2.2 控制律设计

假设期望的笛卡尔位置是原点，笛卡尔误差$e_p$为$[ -x -y ]^T$。控制律如下：
- $v = -k_1(x \cos \theta + y \sin \theta)$
- $\omega = k_2(\text{Atan2}(y, x) - \theta + \pi)$

其中$k_1 > 0$，$k_2 > 0$。驱动速度$v$与笛卡尔误差$e_p$在独轮车矢状轴上的投影成正比，转向速度$\omega$与独轮车方向和向量$e_p$方向的差值（指向误差）成正比。

2.2.3 稳定性分析

考虑“类Lyapunov”函数$V = \frac{1}{2}(x^2 + y^2)$，它在原点处仅为正半定。使用独轮车方程和控制输入，可得$\dot{V} = -k_1(x \cos \theta + y \sin \theta)^2$，在原点处为负半定。这表明$V$有下界且趋于极限值，位置误差$e_p$的范数有界。通过Barbalat引理可知，$\dot{V}$趋于零，从而笛卡尔误差趋于零。

2.3 姿态调节

2.3.1 问题转换

为了设计一个能调节独轮车整个配置向量（笛卡尔位置和车辆方向）的反馈控制器，将问题转换到极坐标下。假设期望配置是原点$q_d = [ 0 0 0 ]^T$。

2.3.2 极坐标定义

设$\rho$为独轮车代表点$(x, y)$与笛卡尔平面原点的距离，$\gamma$为向量$e_p$与车辆矢状轴的夹角，$\delta$为向量$e_p$与$x$轴的夹角。公式如下：
- $\rho = \sqrt{x^2 + y^2}$
- $\gamma = \text{Atan2}(y, x) - \theta + \pi$
- $\delta = \gamma + \theta$

2.3.3 运动学模型

在极坐标下，独轮车的运动学模型为：
- $\dot{\rho} = -v \cos \gamma$
- $\dot{\gamma} = \frac{\sin \gamma}{\rho} v - \omega$
- $\dot{\delta} = \frac{\sin \gamma}{\rho} v$

2.3.4 反馈控制设计

反馈控制定义为：
- $v = k_1\rho \cos \gamma$
- $\omega = k_2 \gamma + k_1 \frac{\sin \gamma \cos \gamma}{\gamma} (\gamma + k_3 \delta)$

其中$k_1 > 0$，$k_2 > 0$。在该控制律作用下，运动学模型渐近收敛到期望配置$[ \rho \gamma \delta ]^T = [ 0 0 0 ]^T$。

2.3.5 稳定性证明

考虑Lyapunov候选函数$V = \frac{1}{2} (\rho^2 + \gamma^2 + k_3 \delta^2)$，其时间导数$\dot{V} = -k_1 \cos 2\gamma \rho^2 -k_2 \gamma^2$为负半定。通过Barbalat引理可知，$\dot{V}$趋于零，$\rho$和$\gamma$也趋于零，进一步分析可知$\delta$也收敛到零。

2.4 模拟结果

通过模拟两个停车操作，展示了上述调节方案的特点。
- 笛卡尔调节器 ：使用$k_1 = 1$和$k_2 = 3$，独轮车最终方向随接近方向变化，最多反转一次运动后以向前运动到达目的地。
- 姿态调节器 ：使用$k_1 = 1$，$k_2 = 2.5$和$k_3 = 3$，轨迹与笛卡尔调节器的轨迹相似，但期望方向也被驱动到零，同样最多反转一次运动后以向前运动接近目的地。

3. 里程计定位

3.1 定位的必要性

任何反馈控制器的实现都需要在每个时间点获取机器人的配置。与机械手不同，移动机器人配备的增量编码器只能测量车轮的旋转，不能直接测量车辆相对于固定世界坐标系的位置和方向。因此，需要设计一个实时估计机器人配置的定位程序。

3.2 定位方法

假设独轮车在每个采样间隔内速度命令$v$和$\omega$恒定，在采样时间$t_k$的机器人配置$q(t_k) = q_k$已知，以及在区间$[t_k, t_{k + 1})$内应用的速度输入$v_k$和$\omega_k$已知，则可以通过运动学模型的前向积分来重建采样时间$t_{k + 1}$的配置变量$q_{k + 1}$。

3.2.1 欧拉方法

使用近似公式：
- $x_{k + 1} = x_k + v_k T_s \cos \theta_k$
- $y_{k + 1} = y_k + v_k T_s \sin \theta_k$
- $\theta_{k + 1} = \theta_k + \omega_k T_s$

其中$T_s = t_{k + 1} - t_k$是采样间隔的持续时间。该方法在计算$x_{k + 1}$和$y_{k + 1}$时引入误差，误差随着$T_s$的减小而减小，第三个公式是精确的。

3.2.2 二阶龙格 - 库塔方法

使用估计公式：
- $x_{k + 1} = x_k + v_k T_s \cos (\theta_k + \frac{\omega_k T_s}{2})$
- $y_{k + 1} = y_k + v_k T_s \sin (\theta_k + \frac{\omega_k T_s}{2})$
- $\theta_{k + 1} = \theta_k + \omega_k T_s$

该方法使用了采样间隔内独轮车方向的平均值。

3.2.3 精确重建方法

通过简单的几何论证或利用独轮车运动学模型可转换为链式形式的特性，得到精确公式：
- $x_{k + 1} = x_k + \frac{v_k}{\omega_k} (\sin \theta_{k + 1} - \sin \theta_k)$
- $y_{k + 1} = y_k - \frac{v_k}{\omega_k} (\cos \theta_{k + 1} - \cos \theta_k)$
- $\theta_{k + 1} = \theta_k + \omega_k T_s$

当$\omega_k = 0$时，前两个公式与欧拉和龙格 - 库塔方法的相应公式一致。

3.3 速度输入的重建

由于执行系统的非理想性，使用机器人的本体感受传感器（车轮执行器的编码器）来重建速度输入$v_k$和$\omega_k$更为方便。对于差动驱动独轮车，通过增量编码器测量的右轮和左轮的旋转$\Delta \varphi_R$和$\Delta \varphi_L$，可以得到：
- $\Delta s = \frac{r}{2} (\Delta \varphi_R + \Delta \varphi_L)$
- $\Delta \theta = \frac{r}{d} (\Delta \varphi_R - \Delta \varphi_L)$

其中$\Delta s$是机器人行驶的基本路段长度，$\Delta \theta$是路段上方向的总变化。

3.4 里程计定位的误差与改进

里程计定位（或航位推算）通过迭代使用上述公式，从初始配置估计开始提供估计，但精度不会优于初始配置$q_0$。实际上，里程计定位会受到误差的影响，误差会随着时间增长（漂移），在足够长的路径上会变得很显著。误差的原因包括车轮打滑、运动学参数校准不准确以及积分方法引入的数值误差等。

一种更鲁棒的解决方案是基于外部传感器的定位。例如，当机器人配备接近传感器（如激光测距仪）并知道工作空间的地图时，可以通过比较外部传感器的预期测量值与实际读数来校正航位推算提供的估计。这些技术使用贝叶斯估计理论的工具，如扩展卡尔曼滤波器或粒子滤波器，提供比纯里程计定位更高的精度，因此在长路径导航任务中至关重要。

3.5 定位方法对比

定位方法	公式	误差情况
欧拉方法	$x_{k + 1} = x_k + v_k T_s \cos \theta_k$ $y_{k + 1} = y_k + v_k T_s \sin \theta_k$ $\theta_{k + 1} = \theta_k + \omega_k T_s$	计算$x_{k + 1}$和$y_{k + 1}$有误差，随$T_s$减小而减小
二阶龙格 - 库塔方法	$x_{k + 1} = x_k + v_k T_s \cos (\theta_k + \frac{\omega_k T_s}{2})$ $y_{k + 1} = y_k + v_k T_s \sin (\theta_k + \frac{\omega_k T_s}{2})$ $\theta_{k + 1} = \theta_k + \omega_k T_s$	误差较小，使用方向平均值
精确重建方法	$x_{k + 1} = x_k + \frac{v_k}{\omega_k} (\sin \theta_{k + 1} - \sin \theta_k)$ $y_{k + 1} = y_k - \frac{v_k}{\omega_k} (\cos \theta_{k + 1} - \cos \theta_k)$ $\theta_{k + 1} = \theta_k + \omega_k T_s$	无积分误差，$\omega_k = 0$需特殊处理

3.6 里程计定位流程

graph LR
    A[已知q(tk)、vk、ωk] --> B[选择积分方法]
    B --> C{Euler方法?}
    C -- 是 --> D[使用Euler公式计算qk+1]
    C -- 否 --> E{二阶Runge–Kutta方法?}
    E -- 是 --> F[使用二阶Runge–Kutta公式计算qk+1]
    E -- 否 --> G[使用精确公式计算qk+1]
    D --> H[得到qk+1]
    F --> H
    G --> H

4. 问题与挑战

4.1 轨迹跟踪问题

在轨迹跟踪中，不同的控制器有不同的适用条件和局限性。例如，基于近似线性化和非线性的控制器需要持续的状态轨迹，而输入/输出线性化的控制器虽然能处理非持续轨迹，但存在驱动点和方向控制的问题。

4.2 调节问题

独轮车不存在通用控制器，这使得姿态调节问题需要专门设计控制律。而且，设计的控制律在原点可能是不连续的，这是由系统的非完整性决定的。

4.3 里程计定位问题

里程计定位存在误差积累的问题，误差来源包括车轮打滑、参数校准不准确和积分方法误差等。为了提高定位精度，需要采用基于外部传感器的定位方法。

4.4 未来研究方向

未来的研究可以集中在开发更通用的控制器，以解决非完整移动机器人的姿态调节问题。同时，可以探索更精确的定位方法，减少里程计定位的误差，提高机器人在复杂环境中的导航能力。此外，还可以研究如何将不同的控制和定位方法进行融合，以实现更高效、更智能的移动机器人系统。

5. 相关问题的详细探讨与解决方案

5.1 不同类型机器人的约束问题

5.1.1 连接 N 个拖车的后轮驱动三轮车

对于由后轮驱动三轮车连接 N 个拖车组成的移动机器人，需要找到一组广义坐标来描述其状态。每个拖车是一个带有固定轮轴的刚体，可视为一个位于轮轴中点的单轮，并通过旋转关节与前一个轮轴的中点相连。确定广义坐标有助于准确建模机器人的运动。

5.1.2 全向移动机器人

考虑一个具有三个麦克纳姆轮的全向移动机器人，轮子放置在等边三角形的顶点，且每个轮子的方向与对应角的角平分线垂直。该机械系统受到一组普法夫约束：
[
A_1^T(q)
\begin{bmatrix}
\dot{q}_1 \
\dot{q}_2 \
\dot{q}_3
\end{bmatrix}
+ A_2^T
\begin{bmatrix}
\dot{q}_4 \
\dot{q}_5 \
\dot{q}_6
\end{bmatrix}
= 0
]
其中
[
A_1(q) =
\begin{bmatrix}
\frac{\sqrt{3}}{2} \cos q_3 - \frac{1}{2} \sin q_3 & \sin q_3 \
-\frac{1}{2} \sin q_3 - \frac{\sqrt{3}}{2} \cos q_3 & \frac{1}{2} \cos q_3 + \frac{\sqrt{3}}{2} \sin q_3 \
-\cos q_3 & \frac{1}{2} \cos q_3 - \frac{\sqrt{3}}{2} \sin q_3 \
\ell & \ell \
\ell & \ell
\end{bmatrix}
]
和 (A_2 = r I_3)。可以证明这组约束是部分可积的，特别是车辆的方向是轮子旋转角度的线性函数。具体操作步骤为：将运动学约束并排相加，通过一系列数学推导得出结论。

5.2 约束的可积性问题

5.2.1 单个运动学约束的可积性

对于形式为 ( (11.7) ) 的单个运动学约束，其可积性条件 ( (11.9) ) 与对应运动学模型输入向量场相关的分布 ( \Delta = \text{span}{g_1, \ldots, g_{n - 1}} ) 的对合性一致。证明过程需要运用相关的数学理论和方法，对约束条件和分布的性质进行深入分析。

5.2.2 不依赖广义坐标的普法夫约束

使用可控性条件 ( (11.11) ) 可以证明，不依赖广义坐标的一组普法夫约束总是可积的。具体步骤为：根据可控性条件，对约束的性质进行分析和推导，得出可积性的结论。

5.3 运动与李括号的关系

对于独轮车的运动学模型 ( (11.13) )，考虑以下速度输入序列：
- (v(t) = 1)，(\omega(t) = 0)，(t \in [0, \varepsilon))
- (v(t) = 0)，(\omega(t) = 1)，(t \in [\varepsilon, 2\varepsilon))
- (v(t) = -1)，(\omega(t) = 0)，(t \in [2\varepsilon, 3\varepsilon))
- (v(t) = 0)，(\omega = -1)，(t \in [3\varepsilon, 4\varepsilon))

通过对模型方程的前向积分，可以证明当 (\varepsilon) 无穷小时，独轮车在配置空间中的位移与输入向量场的李括号对齐。具体操作是将速度输入代入运动学模型，进行积分运算，然后分析位移与李括号的关系。

5.4 差动驱动车辆的速度转换

对于差动驱动车辆，其角速度输入（右轮和左轮各一个）受到以下边界限制：
[
|\omega_R(t)| \leq \omega_{RL}
]
[
|\omega_L(t)| \leq \omega_{RL}
]
(\forall t)

这对应于 (\omega_R)，(\omega_L) 平面上的一个正方形可允许区域。可以推导出等效独轮车模型的驱动速度 (v) 和转向速度 (\omega) 的约束表达式。具体步骤为：根据差动驱动车辆的运动学关系，将角速度输入转换为驱动速度和转向速度，然后分析可允许区域的形状，最终证明在 (v)，(\omega) 平面上的可允许区域是一个菱形。

5.5 路径规划问题

5.5.1 不同形式的链式方程

对于 (2,4) 链式形式，需要计算矩阵 (D(\Delta)) 和向量 (d(z_i, z_f, \Delta)) 在 ( (11.50) ) 中的表达式。这需要对链式方程的性质和相关数学理论有深入的理解，通过具体的计算步骤得出结果。

5.5.2 路径规划方案的修改

修改基于参数化输入的路径规划方案，以获得 (s_f = 1)。具体操作是对原方案的参数和算法进行调整，使其满足 (s_f = 1) 的要求。

5.5.3 不同路径规划方案的比较

可以证明在 (2,3) 链式形式的情况下，基于不同次数多项式和参数化输入的路径规划方案给出的结果完全相同。证明过程需要对两种方案的数学模型和计算方法进行详细分析和比较。

5.5.4 基于立方笛卡尔多项式的路径规划

在计算机程序中实现基于立方笛卡尔多项式的独轮车路径规划方法。具体步骤如下：
1. 编写程序代码，实现立方笛卡尔多项式的计算和路径规划算法。
2. 使用该程序规划一条路径，使机器人从配置 (q_i = [ 0 0 0 ]^T [m,m,rad]) 到达配置 (q_f = [ 2 1 \pi/2 ]^T [m,m,rad])。
3. 确定路径上的定时律，以满足速度边界条件：
- (|v(t)| \leq 1 m/s)
- (|\omega(t)| \leq 1 rad/s)
- 具体方法是根据路径的长度和形状，结合速度边界条件，计算出合适的定时律。

5.6 轨迹跟踪问题的深入分析

5.6.1 (2,3) 链式形式的轨迹跟踪

对于 (2,3) 链式形式，需要制定轨迹跟踪问题，并推导相应的误差动态。具体步骤为：
1. 明确轨迹跟踪问题的目标和约束条件。
2. 根据链式形式的运动学模型，推导误差动态方程。
3. 基于参考轨迹的线性近似，设计反馈控制器。

5.6.2 后轮驱动自行车的输入/输出线性化

对于后轮驱动自行车的运动学模型 ( (11.19) )，类似于独轮车的情况，需要确定两个输出 (y_1)，(y_2)，以便进行静态输入/输出线性化。具体方法是考虑一个位于通过前轮中心且与前轮方向一致的直线上的点 (P)，通过对该点的运动分析，确定合适的输出变量。

5.7 控制器的实现与修改

5.7.1 笛卡尔调节器的实现与修改

在计算机程序中实现笛卡尔调节器 ( (11.78) )，( (11.79) )，并进行修改，使独轮车可以向前或向后运动到达原点。具体步骤为：
1. 编写程序代码，实现笛卡尔调节器的控制算法。
2. 修改 ( (11.79) ) 以迫使机器人根据最方便的选择将自身定向为向量 (e_p) 或向量 (-e_p)。

5.7.2 独轮车姿态调节器的实现

在计算机程序中实现基于极坐标的独轮车姿态调节器，通过二阶龙格 - 库塔里程计定位方法计算状态反馈。具体步骤为：
1. 编写程序代码，实现姿态调节器的控制算法。
2. 利用二阶龙格 - 库塔里程计定位方法，实时计算机器人的状态反馈。

6. 总结与展望

6.1 关键要点总结

• 在轨迹跟踪方面，不同的控制器有各自的适用范围和局限性，输入/输出线性化控制器在处理非持续轨迹时存在驱动点和方向控制问题。
• 调节问题上，独轮车不存在通用控制器，姿态调节需要专门设计控制律，且控制律可能在原点不连续。
• 里程计定位存在误差积累问题，需要采用基于外部传感器的定位方法来提高精度。

6.2 未来发展方向

未来的研究可以从以下几个方面展开：
- 开发更通用的控制器，解决非完整移动机器人的姿态调节问题，提高机器人的运动控制能力。
- 探索更精确的定位方法，减少里程计定位的误差，使机器人能够在复杂环境中更准确地导航。
- 研究不同控制和定位方法的融合，构建更高效、智能的移动机器人系统，以适应各种实际应用场景。

6.3 研究意义与价值

对移动机器人的运动控制、调节和定位问题的研究，有助于推动机器人技术的发展，提高机器人在工业、服务、探索等领域的应用能力。通过解决上述问题，可以使移动机器人更加灵活、准确地完成各种任务，为人类的生产和生活带来更多的便利和效益。

6.4 研究方法对比

研究问题	传统方法	改进方法	优势
轨迹跟踪	基于近似线性化和非线性控制器	输入/输出线性化控制器	可处理非持续轨迹
调节问题	无通用控制器	专门设计控制律	解决姿态调节问题
里程计定位	纯里程计定位	基于外部传感器的定位	提高定位精度

6.5 整体研究流程

graph LR
    A[问题提出] --> B[理论分析]
    B --> C[方法设计]
    C --> D[程序实现]
    D --> E[实验验证]
    E --> F[结果分析]
    F --> G[改进优化]
    G --> H[应用推广]

通过以上对移动机器人各个方面问题的详细研究和分析，我们可以更好地理解移动机器人的运动特性和控制原理，为进一步的研究和应用提供坚实的基础。