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

技术特征：

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

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

X ( k ) = a w k - h s i n ( w k ) Y ( k ) = a - h c o s ( w k ) - - - ( 1 ) ]]>

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

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

x 1 ( k + 1 ) = x 1 ( k ) + T s u 1 ( k ) cos x 3 ( k ) x · 2 ( k + 1 ) = x 2 ( k ) + T s u 1 ( k ) sin x 3 ( k ) x · 3 ( k + 1 ) = x 3 ( k ) + T s u 1 ( k ) tan u 2 ( k ) / l - - - ( 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)：

J ( x ( k ) , U ( k ) ) = Σ i = 0 M - 1 [ x 1 ( k + i + 1 | k ) - X ( k + i + 1 ) ] T q 1 [ x 1 ( k + i + 1 | k ) - X ( k + i + 1 ) ] + Σ i = 0 M - 1 [ x 2 ( k + i + 1 | k ) - Y ( k + i + 1 ) ] T q 2 [ x 2 ( k + i + 1 | k ) - Y ( k + i + 1 ) ] + Σ i = 0 M - 1 [ u 1 ( k + i | k ) T q 3 u 1 ( k + i | k ) + u 2 ( k + i | k ) T q 4 u 2 ( k + i | k ) ] - - - ( 5 ) ]]>

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

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

min U ( k ) J ( x ( k ) , U ( k ) ) s . t . x ( k + i + 1 | k ) = f ( x ( k + i | k ) , u ( k + i | k ) ) , x ( k | k ) = x ( k ) , i = 0 , ... , M - 1 - - - ( 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)，周而复始，直到轮式移动舞台机器人走完整一条平摆线为止。