基于滚动时域优化的移动舞台机器人位姿估计

何德峰，徐晨辉，朱 威，郑雅羽

(浙江工业大学 信息工程学院，浙江 杭州 310023)

近年来，在日常文化发展过程中，舞台演艺文化逐渐占据重要地位。一场文化演出的成功因素已经不再局限于剧本、演员、独白、服装与道具等传统元素[1]，现代演艺更多地将舞台设备融入到演出的各个环节，由此对舞台电子技术的发展提出了更高的要求，并催生了大量的相关研究[2-5]，其中移动舞台机器人是呈现高品质舞台演出艺术的重要道具之一[6-7]。为了配合剧情演绎，移动舞台机器人需要在保证设备和人员安全的情况下平稳且准确地到达指定位置，然后跟踪预先规划的轨迹路线进行无偏运动，以此实现演出场景的动态变化。在此过程中，移动舞台机器人通常采用各类传感器测量状态数据以实现反馈控制。然而复杂多变的舞台环境容易降低数据采集的准确性，进而影响舞台机器人的正常运行，甚至引发安全事故。因此，在演出过程中需要对舞台机器人的运行状态进行实时监控，以便及时发现并处理各类安全隐患。由于通信噪声会加剧测量数据的不确定性，监控端需对接收的数据进行处理，从而获得舞台机器人准确的状态信息，通常可采用状态估计的方式，即根据接收的系统输出数据和相关的参数信息估计出系统状态。常见的状态估计算法主要有Kalman滤波[8-13]、滚动时域估计[14-23]等，其中滚动时域估计由于其具有处理范数有界扰动的复杂非线性和受约束系统的能力而受到国内外学者的广泛关注。滚动时域估计是一种基于优化的状态估计算法[19]，通过最小化在固定大小的滑动窗口上定义的代价函数来获得最优状态估计。代价函数通常包括总结了过去数据的到达代价项，以及利用输出作为估计依据的输出误差项。滚动时域估计的概念源于所谓的全信息估计(FIE)，其目的是根据从初始时刻到当前时刻的所有过去测量值来生成状态估计，这种估计算法被称为计算难解[19]。为了避免由此带来的计算负担，将全信息用固定长度的时域窗口内的信息来近似，从而设计了滚动时域估计算法[14]。

目前，滚动时域估计在移动机器人领域已有大量的应用[17,21-23]，如文献[17]研究了针对移动机器人多速率采样问题的滚动时域估计算法，将未采样的传感器输出视为丢包，并用二进制切换序列进行建模。采用预测器对测量输出进行补偿，进而通过求解正则化最小二乘问题得到最优估计。文献[21]考虑多个移动机器人对一个动态目标进行轨迹跟踪的场景，针对目标具有的噪声和状态约束，提出了一种基于滚动时域估计的分布式估计算法，该算法中的协商一致性过程保证了所有移动机器人实现对运动目标的一致性估计。文献[22]研究了分布式多机器人的同步定位与地图构建(SLAM)问题，基于机器人与邻居机器人共享测量值和状态估计，提出了一种分布式滚动时域估计算法，并采用C/GMRES方法对算法进行优化，以提高计算效率。针对移动机器人中出现的状态估计和跟踪控制问题，文献[23]提出了一种基于优化的解决方案，其中考虑利用滚动时域估计算法求解估计问题，利用非线性模型预测控制求解跟踪控制问题，并将所提出的方案与其它两种常用的状态估计和控制技术进行了比较，验证了方案的良好性能。尽管如此，针对移动舞台机器人的运行状态估计结果鲜有报道。笔者虑及移动舞台机器人运行过程中的安全监控问题，提出一种基于滚动时域优化的移动舞台机器人位姿估计算法。首先，结合移动舞台机器人的运动学模型和预设的参考位姿，采用PD控制，建立移动舞台机器人闭环误差系统模型；其次，采用滚动时域估计算法对舞台机器人跟踪误差进行估计，进而结合已知的参考位姿计算出舞台机器人的估计位姿，以此获得移动舞台机器人的准确位姿；最后，结合移动舞台机器人进行曲线运动的场景来仿真验证该算法的有效性。

1 移动舞台机器人闭环跟踪系统模型

考虑如图1所示的移动舞台机器人，以全局坐标来描述移动舞台机器人的行进路线和位姿。其中，以XOY表示真实环境下的全局坐标系，机器人在该坐标系中的位姿完全由向量(x,y,θ)指定，其中(x,y)为机器人中心点的横纵坐标，θ为机器人的旋转角度。

图1 移动舞台机器人位姿示意图

对于图1所示的移动舞台机器人，建立其运动学模型[6]为

(1)

式中：v，ω分别为机器人的移动线速度和旋转角速度，同时也是机器人的控制输入。假设机器人在侧向上不打滑，即不能沿着驱动轮轴线的方向运动。考虑移动舞台机器人的轨迹跟踪控制问题，设计反馈控制器控制v和ω，使得机器人从任意初始位姿出发都能够准确跟踪已知的参考位姿，其表达式为

(2)

(3)

(4)

(5)

将式(3)代入式(5)，可得

(6)

式(6)等价于

(7)

进而可得

(8)

将式(8)代入式(4)，可得闭环误差系统模型为

(9)

假设KP和KD选择合理，使得PD控制器具有良好的控制性能，进而保证闭环误差系统式(9)能快速收敛到稳态点，即零点。

2 移动舞台机器人的位姿估计

在实际运行过程中，移动舞台机器人自身配备的传感器实时测量运行状态，并将数据发送给监控端。然而舞台上烟雾、焰火和灯光等设备的使用容易加剧移动舞台机器人实际运行环境的复杂性，进而干扰机器人的正常运行和传感器的数据采集，使得测量得到的数据往往不能准确反映系统的真实状态，因此监控端需通过状态估计器，利用接收的数据和已知的先验信息(如机器人模型参数、噪声统计特性等)实时估计机器人的状态。取采样周期为T，对模型式(9)进行离散化处理，可得闭环误差系统离散模型为

(10)

考虑采用高斯噪声来近似复杂环境对机器人的影响，基于模型式(10)可得干扰下的移动舞台机器人闭环误差系统离散模型为

(11)

(12)

(13)

式中：Q为已知的过程噪声协方差矩阵；R为已知的测量噪声协方差矩阵；E[·]为数学期望。

考虑到移动舞台机器人在每个时刻的参考位姿可根据模型(式2)单独计算，结合移动舞台机器人的闭环误差系统模型，机器人运行位姿的估计问题可转化为对系统(式11)状态的实时估计问题，即每个时刻先估计系统(式11)的状态，再利用估计的跟踪误差和已知的参考位姿估计出机器人在当前时刻的位姿。对系统模型(式11，12)进行简化，可得

Xk+1=AXk+εk

yk=CXk+ηk

(14)

针对系统式(14)，采用滚动时域估计算法对其进行估计，该算法在一个长度有限的时域窗口内，利用系统输出值和预先给定的状态先验值来估计系统状态，具体描述如下：

(15)

(16)

(17)

(18)

(19)

3 仿真与结果分析

为检验滚动时域估计算法对移动舞台机器人位姿的估计效果，给出一个移动舞台机器人进行简单曲线运动的仿真例子进行验证，同时以估计结果的均方误差为标准，对比算法在不同输出矩阵和噪声强度下的估计性能。仿真参数如表1所示，其中量测噪声和过程噪声由Matlab函数randn生成，I表示具有合适维度的单位矩阵。机器人的参考位姿设定为从坐标系的任意位置出发，以0.5 m/s的线速度和0.3 deg/s的角速度运行而生成的位姿。在表1所示参数的设定下，滚动时域估计算法对移动舞台机器人位姿的估计结果如图2～5所示。

表1 仿真参数表

图2 机器人轨迹估计

图3 机器人横向位置误差估计

图4 机器人纵向位置误差估计

图5 机器人旋转角误差估计

由图2可知：机器人的参考轨迹呈椭圆形，轨迹的中心点坐标在(1.2, 3.8)附近。机器人初始位置的坐标大致为(0, 1)，PD控制器能使机器人在初始位置偏离参考轨迹的情况下快速跟踪到预定的参考轨迹。由于噪声的影响，机器人的实际轨迹会围绕参考轨迹出现细微波动。同时，滚动时域估计算法能快速收敛，使估计的轨迹能准确跟踪真实的轨迹。

图3～5展示了估计算法对跟踪误差的估计情况，由图3～5可知：跟踪误差的收敛时间大致为5 s，之后在噪声的影响下，跟踪误差围绕零点出现波动；同时，估计算法能较好地估计出跟踪误差，估计误差的收敛时间大致为5 s。

表2为滚动时域估计算法在不同强度噪声下对移动舞台机器人跟踪误差的均方估计误差对比情况。由表2可知：随着测量噪声的协方差逐渐增大，机器人跟踪误差3个分量的均方估计误差也随之增大，原因是测量噪声强度的增大降低了系统测量输出的准确性，进而增加了估计的难度。总之，与噪声强度的增加幅度相比，均方估计误差的增加幅度明显更小，这表明滚动时域估计算法对噪声具有一定的抑制能力。

表2 不同噪声强度下的均方估计误差对比

表3为滚动时域估计算法在不同系统输出矩阵下对移动舞台机器人跟踪误差的均方估计误差对比情况。由表3可知：不同的系统输出矩阵会使估计算法产生不同的估计效果。就跟踪误差的单个分量ye而言，若系统输出数据中包含与ye相关的信息，则该信息与ye的相关度越高，估计算法对ye的均方估计误差越小，估计效果越好，该结果与估计算法的原理相符。对于算法而言，系统输出数据是算法获得系统状态信息的直接来源，因此输出数据中与状态分量相关的信息越准确，算法的估计效果就越好。

表3 不同输出矩阵下的均方估计误差对比

4 结 论

提出了一种基于滚动时域优化的移动舞台机器人位姿估计算法来处理移动舞台机器人的位姿监控问题。首先，基于移动舞台机器人的运动学模型和预设的参考位姿模型，提出了移动舞台机器人的开环误差动力学系统模型，考虑移动舞台机器人的轨迹跟踪控制问题，采用PD控制，建立了舞台机器人的闭环误差动力学系统模型。其次，用高斯噪声模拟复杂环境的影响，采用滚动时域估计算法对移动舞台机器人的跟踪误差进行估计，结合已知的参考位姿，反推出移动舞台机器人的估计位姿，以此获得移动舞台机器人在复杂环境下的准确位姿。最后，以移动舞台机器人进行简单的曲线运动为例进行仿真。结果表明：PD控制能使机器人准确跟踪预设的参考位姿，同时滚动时域估计算法能在数据受到噪声污染的情况下有效估计出机器人的跟踪误差，进而估计出机器人的真实位姿。