一种轮式移动舞台机器人平摆线跟踪控制方法与流程

本发明属于轮式移动舞台机器人控制领域，涉及一种轮式移动舞台机器人平摆线跟踪控制方法。

背景技术：

随着我国人民生活水平的提高和对文化建设的迫切需要，舞台演出艺术已成为我国日常文化生活的重要组成部分，其中轮式移动舞台机器人是呈现高品质舞台演出艺术的重要道具之一。通常在曲目演出过程中，舞台机器人需要根据演出曲目的变化快速跟踪指定的运动轨迹，与演艺人员及曲目在时间和空间位置实现完美配合。移动舞台机器人快速跟踪指定的运动轨迹，可视为移动机器人的轨迹跟踪控制问题。在移动舞台运动轨迹设计中，平摆线是一种非常重要且常见的舞台运动轨迹线。为实现移动舞台机器人的平摆线快速跟踪要求，需要设计一个反馈控制律使得移动舞台机器人渐近跟踪平摆线。由于轮式移动舞台机器人的位姿参数包括X轴和Y轴位置以及方向角三个变量，而控制变量包括车速和前轮偏角两个变量，因此是一个典型的欠驱动非线性控制系统，这对轮式移动舞台机器人的平摆线轨迹跟踪控制提出了挑战性。通过对现有轮式舞台机器人一般轨迹跟踪控制方法的文献的检索发现，轮式移动机器人轨迹跟踪控制方法主要有：非连续轨迹跟踪控制、混杂轨迹跟踪控制和滑模轨迹跟踪控制，但由于平摆线运行需要舞台机器人自身旋转和前进同步驱动，非连续轨迹跟踪控制和混杂轨迹跟踪控制不能处理轮式移动舞台机器人的平摆线轨迹跟踪，而滑模轨迹跟踪控制需要人为构造抽象的滑模移动曲面，在实际应用中会产生抖动现象，从而影响舞台机器人运动的平稳性，危及演艺人员的安全和动作柔美性，而且这些轨迹跟踪控制方法理解复杂。因为轮式移动舞台机器人需要根据曲目变化按各种平摆线运动到相关舞台位置，控制实时性要求高，而且轮式移动舞台机器人又是一个典型的欠驱动非线性控制系统。因此，尽管轮式移动机器人轨迹跟踪控制研究取得了一些成果，但近年来相关学者和演艺专家对于这个具有挑战性的重要难题仍然进行了大量细致地研究和探讨，以满足当前高品质舞台演出艺术对轮式移动舞台机器人的简便、灵活的轨迹跟踪控制的迫切要求。

技术实现要素：

为了克服已有轮式移动舞台机器人平摆线跟踪控制方法的设置参数多、理解抽象和实现困难的不足，本发明提供一种理解直观、设计简单、易于实现的轮式移动舞台机器人平摆线跟踪控制方法。

本发明解决其技术问题所采用的技术方案是：

一种轮式移动舞台机器人平摆线跟踪控制方法，所述控制方法包括如下步骤：

1)、输入需要跟踪的平摆线方程，参见式(1)：

其中，常量a为拱的宽度，单位米，常量h为拱的宽度，单位米，常量w为平摆线旋转角速度，单位弧度/秒，变量k表示采样时刻；X(k)和Y(k)分别表示在k时刻平摆线在直角坐标系中横向和纵向上的位置坐标；

2)、建立轮式移动舞台机器人移动过程的离散时间动力学模型，参见式(2)：

其中，正常数T_s表示采样周期，单位秒；x₁(k)和x₂(k)分别表示在k时刻轮式移动舞台机器人在舞台直角坐标系中横向和纵向的位置坐标；x₃(k)表示在k时刻轮式移动舞台机器人在直角坐标系中的方位角；u₁(k)和u₂(k)分别表示在k时刻轮式移动舞台机器人的线速度V和前轮偏角δ；l表示轮式移动舞台机器人的轴距；考虑模型式(2)，定义轮式移动舞台机器人的状态列向量x＝[x₁x₂x₃]^T和控制列向量u＝[u₁u₂]^T，其中，符号T表示向量的转置；

3)、将模型式(2)简记为式(3)：

x(k+1)＝f(x(k),u(k)) (3)

其中，f(x(k),u(k))＝[f₁(x(k),u(k))f₂(x(k),u(k))f₃(x(k),u(k))]^T，f₁(x(k),u(k))＝x₁(k)+T_su₁(k)cosx₃(k)，f₂(x(k),u(k))＝x₂(k)+T_su₁(k)sinx₃(k)，f₃(x(k),u(k))＝x₃(k)+T_su₁(k)tanu₂(k)，建立轮式移动舞台机器人的离散时间预测模型，参见式(4)：

x(k+j+1|k)＝f(x(k+j|k),u(k+j|k)),j＝0,1,...,M-1 (4)

其中，x(k+j|k)表示轮式移动舞台机器人控制系统在时刻k对未来时刻k+j状态的预测向量；u(k+j|k)表示轮式移动舞台机器人控制系统在时刻k对未来时刻k+j控制变量的预测向量；正整数M表示优化时间窗口；

4)、令U(k)＝[u^T(k|k)u^T(k+1|k)…u^T(k+M-1|k)]^T表示在k时刻由2M个元素组成的一个控制序列，定义平摆线跟踪误差函数J(x(k),U(k))，参见式(5)：

其中，常量q₁、q₂、q₃和q₄分别为横向位置偏差、纵向位置偏差、线速度和前轮偏角的加权系数；

5)、考虑模型式(2)，定义轮式移动舞台机器人的平摆线跟踪控制问题，参见式(6)：

其中，符号“s.t.”表示约束；x(k)是轮式移动舞台机器人在k时刻的状态测量值；

6)、利用序列二次规划算法求解式(6)，得到在k时刻的最优控制序列U^*(k)，进而定义轮式移动舞台机器人在k时刻的平摆线跟踪控制量u(k)，参见式(7)：

u(k)＝[I₂ 0₂ … 0₂]_2×2MU^*(k) (7)

其中，符号I₂表示2阶单位矩阵，符号0₂表示2阶零矩阵；

7)、将控制量u(k)作用于轮式移动舞台机器人，在下一个采样时刻k+1到达后，检测轮式移动舞台机器人的运动状态x(k+1)，然后令k＝k+1并返回步骤5)，周而复始，直到轮式移动舞台机器人走完整一条平摆线为止。

本发明的技术构思为：针对欠驱动非线性的轮式移动舞台机器人实现平摆线跟踪控制需要，先通过定义轮式移动舞台机器人的离散时间三阶动力学模型和平摆线跟踪误差函数，再定义在有限时间窗口内的轮式移动舞台机器人平摆线跟踪控制问题，利用序列二次规划算法求解每个采样时刻平摆线跟踪最优控制序列，并结合滚动优化原理计算在每个采样时刻的平摆线跟踪控制量，实现轮式移动舞台机器人的平摆线跟踪控制。本发明设计方法的优点是理解简单、通用性强以及在线计算简便。

本发明主要执行部分在轮式移动舞台机器人运动控制计算机上运行实施。本方法应用过程可以大致分为3个阶段：

1、参数设置：在参数导入界面中，输入平摆线参数a、h和w，输入采样周期T_s、优化时间窗口M和加权系数p₁、p₂、p₃和p₄，输入参数确认后，由控制计算机将设置数据送入计算机存储单元RAM中保存；

2、离线调试：点击组态界面中的“调试”按钮，控制系统进入控制器离线仿真调试阶段，调整组态界面中的加权系数q₁、q₂、q₃和q₄，观察轮式移动舞台机器人状态变量即位置和方向角的跟踪效果，由此确定一个能良好实现轮式移动机器人平摆线跟踪控制的加权系数；加权系数q₁、q₂、q₃和q₄的取值规则：q₁、q₂、q₃和q₄取正实数；加权系数的调整规则：增大q₁和q₂可以提高轮式移动舞台机器人运动过程的快速性，但轮式移动舞台机器人容易产生运动轨迹的震荡，反之，减小q₁和q₂可以提高轮式移动舞台机器人运动过程的平稳性，但会减慢机器人的跟踪速度；q₃和q₄的调整规则与q₁和q₂的调整规则相反，增加q₃和q₄可以提高轮式移动舞台机器人运动过程的平稳性，但会减慢机器人的跟踪速度，减小q₃和q₄可以提高轮式移动舞台机器人运动过程的快速性，但轮式移动舞台机器人容易产生运动轨迹的震荡，因此，实际调试加权系数时，应权衡轮式移动舞台机器人平摆线跟踪响应的快速性、平稳性、跟踪速度和控制量之间的综合性能；

3、在线运行：点击组态界面“运行”按钮，启动轮式移动舞台机器人运动控制计算机的CPU读取参数a、h、w、T_s、M和加权系数p₁、p₂、p₃和p₄，并执行“轮式移动舞台机器人平摆线跟踪控制程序”，通过在线测量轮式移动舞台机器人的位置和方位角，控制进入轮式移动舞台机器人的线速度和前轮偏角度，实现轮式移动舞台机器人对给定平摆线的跟踪控制，在下一个采样周期到达时，在线测量轮式移动舞台机器人的实际位置和方位角，之后重复整个执行过程，周而复始，直到轮式移动舞台机器人走完整一条平摆线为止。

全套轮式移动舞台机器人平摆线跟踪控制方法可以在轮式移动舞台机器人控制系统组态界面上完成，此过程可以参考本说明书下文中提供的实例应用。与传统轮式移动舞台机器人平摆线跟踪控制方法相比，本发明给出的轮式移动舞台机器人平摆线跟踪控制方法的最大特点是，根据轮式移动舞台机器人当前位置和方位角，实时优化平摆线跟踪误差函数，得到轮式移动舞台机器人的最佳线速度和前轮偏角度，提高了轮式移动舞台机器人平摆线跟踪控制的快速性、平稳性和在线实施简易性。下文具体实施方法以轮式移动舞台机器人一条给定的平摆线跟踪控制为例说明本发明的实际效果，但本发明的应用范围并不以本实施例中的轮式移动舞台机器人平摆线跟踪控制为限。

本发明的有益效果主要表现在：设计简单、容易理解、在线实施简单、通用性强，提高了轮式移动舞台机器人平摆线跟踪控制的快速性和平稳性，满足移动舞台机器人平摆线跟踪的快速控制实时性要求。

附图说明

图1为轮式移动舞台机器人的线速度实时曲线的示意图。

图2为轮式移动舞台机器人的前轮偏角实时曲线的示意图。

图3为轮式移动舞台机器人平摆线跟踪轨迹曲线的示意图，其中，实线为待跟踪的平摆线，虚线为轮式移动舞台机器人跟踪生成的平摆线。

具体实施方式

下面结合附图对本发明作进一步描述。

参照图1～图3，一种轮式移动舞台机器人平摆线跟踪控制方法，所述控制方法包括如下步骤：

1)、输入需要跟踪的平摆线方程，参见式(1)：

其中，常量a为拱的宽度，单位米，常量h为拱的宽度，单位米，常量w为平摆线旋转角速度，单位弧度/秒，变量k表示采样时刻；X(k)和Y(k)分别表示在k时刻平摆线在直角坐标系中横向和纵向上的位置坐标；

2)、建立轮式移动舞台机器人移动过程的离散时间动力学模型，参见式(2)：

其中，正常数T_s表示采样周期，单位秒；x₁(k)和x₂(k)分别表示在k时刻轮式移动舞台机器人在舞台直角坐标系中横向和纵向的位置坐标；x₃(k)表示在k时刻轮式移动舞台机器人在直角坐标系中的方位角；u₁(k)和u₂(k)分别表示在k时刻轮式移动舞台机器人的线速度V和前轮偏角δ；l表示轮式移动舞台机器人的轴距；考虑模型式(2)，定义轮式移动舞台机器人的状态列向量x＝[x₁ x₂ x₃]^T和控制列向量u＝[u₁ u₂]^T，其中，符号T表示向量的转置；

3)、将模型式(2)简记为式(3)：

x(k+1)＝f(x(k),u(k)) (3)

其中，f(x(k),u(k))＝[f₁(x(k),u(k)) f₂(x(k),u(k)) f₃(x(k),u(k))]^T，f₁(x(k),u(k))＝x₁(k)+T_su₁(k)cosx₃(k)，f₂(x(k),u(k))＝x₂(k)+T_su₁(k)sinx₃(k)，f₃(x(k),u(k))＝x₃(k)+T_su₁(k)tanu₂(k)，建立轮式移动舞台机器人的离散时间预测模型，参见式(4)：

x(k+j+1|k)＝f(x(k+j|k),u(k+j|k)),j＝0,1,...,M-1 (4)

其中，x(k+j|k)表示轮式移动舞台机器人控制系统在时刻k对未来时刻k+j状态的预测向量；u(k+j|k)表示轮式移动舞台机器人控制系统在时刻k对未来时刻k+j控制变量的预测向量；正整数M表示优化时间窗口；

4)、令U(k)＝[u^T(k|k) u^T(k+1|k) … u^T(k+M-1|k)]^T表示在k时刻由2M个元素组成的一个控制序列，定义平摆线跟踪误差函数J(x(k),U(k))，参见式(5)：

其中，常量q₁、q₂、q₃和q₄分别为横向位置偏差、纵向位置偏差、线速度和前轮偏角的加权系数；

5)、考虑模型式(2)，定义轮式移动舞台机器人的平摆线跟踪控制问题，参见式(6)：

其中，符号“s.t.”表示约束；x(k)是轮式移动舞台机器人在k时刻的状态测量值；

6)、利用序列二次规划算法求解式(6)，得到在k时刻的最优控制序列U^*(k)，进而定义轮式移动舞台机器人在k时刻的平摆线跟踪控制量u(k)，参见式(7)：

u(k)＝[I₂ 0₂ … 0₂]_2×2MU^*(k) (7)

其中，符号I₂表示2阶单位矩阵，符号0₂表示2阶零矩阵；

7)、将控制量u(k)作用于轮式移动舞台机器人，在下一个采样时刻k+1到达后，检测轮式移动舞台机器人的运动状态x(k+1)，然后令k＝k+1并返回步骤5)，周而复始，直到轮式移动舞台机器人走完整一条平摆线为止。

本实例为轮式移动舞台机器人跟踪一条给定平摆线控制过程，具体操作过程如下：

第一、在参数设置界面中，输入参数a＝1.0米、h＝1.0米、w＝0.5弧度/秒、T_s＝0.1秒、M＝10和加权系数p₁、p₂、p₃和p₄；

第二、在组态界面上点击“调试”按钮进入调试界面，启动轮式移动舞台机器人运动控制计算机的CPU调用事先编制好的“控制器计算程序”求解控制器，具体计算过程如下：根据给定的p₁、p₂、p₃和p₄，计算最优控制序列U^*(k)，定义轮式移动舞台机器人在k时刻线速度和前轮偏角的一个控制器(7)，根据p₁、p₂、p₃和p₄的取值和调整规则，比较轮式移动舞台机器人平摆线跟踪结果和控制量计算结果，调试得到p₁＝10、p₂＝10、p₃＝0.01和p₄＝0.01，将调试结果保存到计算机存储单元RAM中；

第三、点击组态界面“运行”按钮，启动轮式移动舞台机器人控制计算机的CPU读取参数a、h、w、T_s、M和加权系数p₁、p₂、p₃和p₄，并执行“轮式移动舞台机器人平摆线跟踪控制程序”，通过在线测量轮式移动舞台机器人的位置和方位角，控制进入轮式移动舞台机器人的线速度和前轮偏角，实现轮式移动舞台机器人对给定平摆线的跟踪控制，在下一个采样周期到达时，在线测量轮式移动舞台机器人的实际位置和方位角，之后重复整个执行过程，周而复始，实现轮式移动舞台机器人对给定平摆线的跟踪控制。

以上阐述的是本发明给出的一个轮式移动舞台机器人对一条给定平摆线跟踪控制实例所表现出的良好运动性能。需要指出，上述实例用来解释说明本发明，而不是对本发明进行限制，在本发明的精神和权利要求的保护范围内，对本发明做出的任何修改，都落入本发明的保护范围。