Application of the Udwadia-Kalaba approach to tracking control of mobile robots
Udwadia-Kalaba 方法在移动机器人跟踪控制中的应用

Received: 26 January 2015 / Accepted: 11 August 2015
收稿日期： 2015-01-26 / 录用日期： 2015-08-11
© Springer Science+Business Media Dordrecht 2015
© Springer Science + Business Media 多德雷赫特 2015

K. Huang  黄国强

Chuzhou Automobile and Home Appliance Research Institute, Chuzhou 239000, Anhui, People’s Republic of China
滁州市汽车家电研究院， 中华人民共和国 安徽 滁州 239000
e-mail: hfhuang98@163.com
电子邮件： hfhuang98@163.com
ulations are performed to show the simplicity, efficacy and accuracy of this closed-form method.
进行 ulations 是为了显示这种封闭式方法的简单性、有效性和准确性。

Keywords Udwadia-Kalaba approach - Constraints . Mobile robot • Trajectory tracking
关键词 Udwadia-Kalaba 方法 - 约束 .移动机器人 • 轨迹跟踪

In recent years, the research of mobile robots has attracted a great deal of attention in international academia and industry since they play an important role in hazardous environments where would be unsafe for human. Mobile robots have broad practical application prospects in scientific research, national defense,
近年来，移动机器人的研究引起了国际学术界和工业界的广泛关注，因为它们在对人类不安全的危险环境中发挥着重要作用。移动机器人在科研、国防、

industry, logistics, search and rescue, space detection and other fields. Like many other mechanical systems, mobile robots are often required to follow desired trajectories (or certain constraints).
工业、物流、搜救、空间探测等领域。与许多其他机械系统一样，移动机器人通常需要遵循所需的轨迹（或某些约束）。

To address the tracking control problem of mobile robots, valuable classic control approaches have been presented, such as sliding mode control [1-3], adaptive control $[4,5]$$[4,5]$[4,5][4,5], robust control [6], adaptive sliding mode control [7], robust PID control [8] and adaptive robust control [9]. Alternatively, intelligent control has become an interesting topic for controlling complex systems by function approximation or obtaining rules from the experts knowledge using some powerful tools such as fuzzy logic, neural networks and intelligent algorithms. Hence, there are also some valuable research works in the tracking control of mobile robots, such as fuzzy control [10], adaptive fuzzy control [11], adaptive neural control [12], fuzzy neural control $[13,14]$$[13,14]$[13,14][13,14] and adaptive neural sliding mode control [15].
为了解决移动机器人的跟踪控制问题，已经提出了有价值的经典控制方法，例如滑模控制 [1-3]、自适应控制 $[4,5]$$[4,5]$[4,5][4,5] 、鲁棒控制 [6]、自适应滑模控制 [7]、鲁棒 PID 控制 [8] 和自适应鲁棒控制 [9]。或者，智能控制已成为一个有趣的话题，用于通过函数逼近来控制复杂系统，或者使用一些强大的工具（如模糊逻辑、神经网络和智能算法）从专家知识中获取规则。因此，在移动机器人的跟踪控制方面也有一些有价值的研究工作，如模糊控制 [10]、自适应模糊控制 [11]、自适应神经控制 [12]、模糊神经控制和 $[13,14]$$[13,14]$[13,14][13,14] 自适应神经滑模控制 [15]。

Unlike most control methods for the tracking control problem of mobile robots, this paper does not make any linearization or approximations and obtains exact, analytical solutions to the tracking control problem of mobile robots in which the desired trajectory can be any (suitably smooth) arbitrarily prescribed function of time. Our methodology is based on a novel, concise approach to explicitly formulate the equations of motion for constrained systems proposed by Udwadia and Kalaba [16-22].
与移动机器人跟踪控制问题的大多数控制方法不同，本文不做任何线性化或近似，而是获得了移动机器人跟踪控制问题的精确解析解，其中所需的轨迹可以是任何（适当平滑的）任意规定的时间函数。我们的方法基于一种新颖、简洁的方法，明确地表述了 Udwadia 和 Kalaba [16-22] 提出的约束系统的运动方程。

Udwadia and Kalaba $[16,17]$$[16,17]$[16,17][16,17] obtained a concise, explicit set of equations of motion for constrained discrete dynamical systems which lead to a simple and new fundamental view of Lagrangian mechanics. They derived the fundamental equation of motion that describes the dynamics of constrained systems from Gauss’s principle which seems somewhat less popular than the principles of Lagrange, Hamilton, Gibbs and Appell. The equations can deal with holonomic and also nonholonomic constraints. Udwadia and Kalaba $[18,19]$$[18,19]$[18,19][18,19] observed that all the above researches have used D’Alembert’s principle as their starting point which indicates that the forces of constraints are considered to be ideal and the total work done by the forces of constraints under virtual displacement is always zero. This assumption works well in many situations and is regarded as the core of classical analytical dynamics, but it is not applicable when the constraints are nonideal. Thus, Udwadia and Kalaba [20,21] general-
Udwadia 和 Kalaba $[16,17]$$[16,17]$[16,17][16,17] 为受约束的离散动力学系统获得了一组简洁、明确的运动方程，从而得出了拉格朗日力学的简单而新的基本观点。他们从高斯原理推导出描述约束系统动力学的基本运动方程，该原理似乎不如拉格朗日、汉密尔顿、吉布斯和阿佩尔原理流行。这些方程可以处理全息约束和非全息约束。Udwadia 和 Kalaba $[18,19]$$[18,19]$[18,19][18,19] 观察到，上述所有研究都以 D'Alembert 原理为起点，这表明约束力被认为是理想的，在虚拟位移下约束力所做的总功始终为零。此假设在许多情况下都适用，并被视为经典分析动力学的核心，但当约束不理想时，它不适用。因此，Udwadia 和 Kalaba [20,21] 将
ized their previous equations to constrained mechanical systems that may not satisfy D’Alembert’s principle. Systems with singular mass matrices are not common in classical dynamics when dealing with unconstrained motion. However, singular mass matrices can arise when one wants greater flexibility in modeling complex mechanical systems by using more than the minimum number of required generalized coordinates. Thus, Udwadia and Phohomsiri [22] developed general and explicit equations of motion to handle systems whether or not their mass matrices are singular. We call these discoveries Udwadia-Kalaba theory.
将他们之前的方程组化为可能不满足 D'Alembert 原理的约束机械系统。在处理无约束运动时，具有奇异质量矩阵的系统在经典动力学中并不常见。然而，当人们希望通过使用超过所需最小数量的广义坐标来建模复杂机械系统时，就会出现奇异质量矩阵。因此，Udwadia 和 Phohomsiri [22] 开发了一般和显式运动方程来处理系统，无论它们的质量矩阵是否为奇异。我们称这些发现为 Udwadia-Kalaba 理论。

According to Udwadia and Kalaba’s marvelous discoveries, many researchers have used this method to solve problems in the dynamics analysis of multibody systems. Schutte and Dooley [23] utilized the Udwadia-Kalaba approach to study the periodicity of tethered satellite motion. Pennestri et al. [24] creatively adopted the Udwadia-Kalaba formulation to study the dynamics analysis of flexible multibody systems and compared the numerical efficiency between the Udwadia-Kalaba formulation and the coordinate partitioning scheme usually adopted. Huang et al. [25] used the Udwadia-Kalaba formulation to obtain the closed-form equation of motion of a human body with joint friction. Zhao et al. [26] applied the UdwadiaKalaba approach to dynamical modeling of falling Uchain and fish robot problems and obtained explicit analytic equations of motion. Huang et al. [27] utilized the Udwadia-Kalaba approach for parallel manipulator dynamics analysis by segmenting a parallel manipulator system into several leg-subsystems and the platform subsystem, which are connected by kinematic constraints. Zhang et al. [28] applied the UdwadiaKalaba approach to deduce that the heavenly bodies, motion orbits are always conic based on the law of universal gravitation, and the angular momentum is conservative, and verify that the heavenly body’s motion complies with the inverse square law. Zhen et al. [29] adopted the Udwadia-Kalaba approach to study the falling cat’s movements and detailedly explained the surprise phenomenon that a cat when dropped at rest with its feet pointing up can always manage to right itself and land safely on its feet.
根据 Udwadia 和 Kalaba 的惊人发现，许多研究人员已经使用这种方法来解决多体系统动力学分析中的问题。Schutte 和 Dooley [23] 利用 Udwadia-Kalaba 方法研究了系留卫星运动的周期性。Pennestri 等 [24] 创造性地采用了 Udwadia-Kalaba 公式来研究柔性多体系统的动力学分析，并比较了 Udwadia-Kalaba 公式与通常采用的坐标划分方案之间的数值效率。Huang等[25]使用Udwadia-Kalaba公式获得了具有关节摩擦力的人体的闭式运动方程。Zhao et al. [26] 将 UdwadiaKalaba 方法应用于下落 Uchain 和鱼机器人问题的动力学建模，并获得了明确的运动解析方程。Huang等[27]利用Udwadia-Kalaba方法进行并行机械手动力学分析，将并联机械手系统分割成几个腿子系统和平台子系统，它们通过运动学约束连接。Zhang等[28]应用UdwadiaKalaba方法，根据万有引力定律推导出天体的运动轨道总是圆锥的，角动量是守恒的，并验证了天体的运动符合平方反比定律。Zhen等[29]采用Udwadia-Kalaba方法研究了下落猫的动作，并详细解释了猫在静止时双脚朝上时总是能够站直并安全地着地的意外现象。

Inspired by results related to the analytical dynamics with constrained motion, Udwadia creatively proposed a novel methodology for controlling general, nonlinear, structural and mechanical systems $[30,31]$$[30,31]$[30,31][30,31] and used this methodology to solve the control problem of rota-
受到与约束运动的分析动力学相关的结果的启发，Udwadia 创造性地提出了一种控制一般、非线性、结构和机械系统 $[30,31]$$[30,31]$[30,31][30,31] 的新方法，并使用该方法来解决旋转
tional rigid body [32] and satellite formation-keeping [33-35]. Using the fundamental equation, Udwadia et al. also successfully addressed the control of nonlinear multibody mechanical systems in the presence of system uncertainties [36-40]. This control methodology now is well known to many researchers in this field. Based on this methodology, Braun and Goldfarb [41] presented an approach for the closed-loop control of a fully actuated biped robot that leverages its natural dynamics when walking. Lam designed a space mission for a satellite to follow a circular orbit around an oblate rotating body whose gravitational field is nonuniform [42]. Following the framework established by Udwadia and Kalaba, Chen carried on the thorough research on the constraint-following control for mechanical systems with uncertainties and proposed several classes of adaptive robust controllers [43-46]. Schutte studied the control problem of nonlinear mechanical systems with holonomic and nonholonomic constraints on the basis of Udwadia and Kalaba equation and proposed two types of nonlinear state feedback controllers which are shown to provide exact tracking and stabilization to the constrained system under certain conditions [47].
刚性体 [32] 和卫星形成保持 [33-35]。利用这个基本方程，Udwadia et al. 还成功地解决了在存在系统不确定性的情况下非线性多体机械系统的控制 [36-40]。这种控制方法现在为该领域的许多研究人员所熟知。基于这种方法，Braun 和 Goldfarb [41] 提出了一种对完全驱动的双足机器人进行闭环控制的方法，该方法利用其行走时的自然动力学。Lam 为卫星设计了一个太空任务，该卫星围绕引力场不均匀的扁形旋转体遵循圆形轨道 [42]。遵循Udwadia和Kalaba建立的框架，Chen对具有不确定性的机械系统的约束跟随控制进行了深入研究，并提出了几类自适应鲁棒控制器[43-46]。Schutte 基于 Udwadia 和 Kalaba 方程研究了具有完整和非完整约束的非线性机械系统的控制问题，并提出了两种类型的非线性状态反馈控制器，这些控制器被证明在某些条件下为约束系统提供精确的跟踪和稳定 [47]。

In this paper, we establish the dynamic model of the mobile robot with two front driving wheels and two rear following wheels and apply the Udwadia-Kalaba approach to solve the trajectory tracking control problem. The control torques required to control the mobile robot so that it satisfies the given trajectory requirements are obtained explicitly and in closed form by solving Udwadia-Kalaba equation. Numerical simulations are performed to show the simplicity, efficacy and accuracy of this closed-form method.
在本文中，我们建立了具有两个前驱动轮和两个后后跟随轮的移动机器人的动力学模型，并应用 Udwadia-Kalaba 方法解决了轨迹跟踪控制问题。通过求解 Udwadia-Kalaba 方程，可以明确地以封闭的形式获得控制移动机器人以满足给定轨迹要求所需的控制扭矩。进行数值模拟以证明这种封闭式方法的简单性、有效性和准确性。

This section deals with the fundamental equation, called Udwadia-Kalaba equation, for constrained systems. Obtaining the explicit closed-form equations of motion can be concluded to a systematic general threestep procedure [17].
本节介绍约束系统的基本方程，称为 Udwadia-Kalaba 方程。获得明确的闭式运动方程可以归结为系统的一般三步程序 [17]。

First consider the unconstrained discrete mechanical system with the $n$$n$nn generalized coordinates $q:=$$q:=$q:=q:= ${[q_1,q_2,\dots ,q_n]}^T$${\left[q_1,q_2,\dots ,q_n\right]}^T$[q_(1),q_(2),dots,q_(n)]^(T)\left[q_{1}, q_{2}, \ldots, q_{n}\right]^{T} whose equation of motion can be expressed by using Newtonian or Lagrangian equation as
首先考虑具有 $n$$n$nn 广义坐标 $q:=$$q:=$q:=q:= ${[q_1,q_2,\dots ,q_n]}^T$${\left[q_1,q_2,\dots ,q_n\right]}^T$[q_(1),q_(2),dots,q_(n)]^(T)\left[q_{1}, q_{2}, \ldots, q_{n}\right]^{T} 的无约束离散机械系统，其运动方程可以用牛顿方程或拉格朗日方程表示为
$M(q,t)\ddot{q}=Q(q,\dot{q},t)q(0)=q_0,\dot{q}(0)={\dot{q}}_0$$M(q,t)\ddot{q}=Q(q,\dot{q},t)q(0)=q_0,\dot{q}(0)={\dot{q}}_0$M(q,t)q^(¨)=Q(q,q^(˙),t)quad q(0)=q_(0),q^(˙)(0)=q^(˙)_(0)M(q, t) \ddot{q}=Q(q, \dot{q}, t) \quad q(0)=q_{0}, \dot{q}(0)=\dot{q}_{0}
where $M(q,t)$$M(q,t)$M(q,t)M(q, t) is a positive definite inertia $n\times n$$n\times n$n xx nn \times n matrix, $\dot{q}$$\dot{q}$q^(˙)\dot{q} is the $n\times 1$$n\times 1$n xx1n \times 1 vector of velocity, $\ddot{q}$$\ddot{q}$q^(¨)\ddot{q} is the $n\times 1$$n\times 1$n xx1n \times 1 vector of acceleration, and $Q(q,\dot{q},t)$$Q(q,\dot{q},t)$Q(q,q^(˙),t)Q(q, \dot{q}, t) which denotes the known force imposed on the system whose constraints are released is also an $n\times 1$$n\times 1$n xx1n \times 1 vector. The imposed forces could include centrifugal force, gravitational force and control input. The initial conditions at time $t_0$$t_0$t_(0)t_{0} are defined by $q_0$$q_0$q_(0)q_{0} and ${\dot{q}}_0$${\dot{q}}_0$q^(˙)_(0)\dot{q}_{0}, respectively. The generalized acceleration at time $t$$t$tt of the unconstrained system, which we denote by $a(q,\dot{q},t)$$a(q,\dot{q},t)$a(q,q^(˙),t)a(q, \dot{q}, t), is thus given by
其中 $M(q,t)$$M(q,t)$M(q,t)M(q, t) 是一个正定惯性 $n\times n$$n\times n$n xx nn \times n 矩阵， $\dot{q}$$\dot{q}$q^(˙)\dot{q} 是速度 $n\times 1$$n\times 1$n xx1n \times 1 向量， $\ddot{q}$$\ddot{q}$q^(¨)\ddot{q} 是加速度 $n\times 1$$n\times 1$n xx1n \times 1 向量， $Q(q,\dot{q},t)$$Q(q,\dot{q},t)$Q(q,q^(˙),t)Q(q, \dot{q}, t) 表示施加在约束被释放的系统上的已知力也是一个 $n\times 1$$n\times 1$n xx1n \times 1 向量。施加的力可能包括离心力、重力和控制输入。time $t_0$$t_0$t_(0)t_{0} 的初始条件分别由 $q_0$$q_0$q_(0)q_{0} 和 ${\dot{q}}_0$${\dot{q}}_0$q^(˙)_(0)\dot{q}_{0} 定义。无约束系统在时间 $t$$t$tt 上的广义加速度，我们用 $a(q,\dot{q},t)$$a(q,\dot{q},t)$a(q,q^(˙),t)a(q, \dot{q}, t) 表示，由下式给出
$\ddot{q}=M^{-1}(q,t)Q(q,\dot{q},t)=a(q,\dot{q},t)$$\ddot{q}=M^{-1}(q,t)Q(q,\dot{q},t)=a(q,\dot{q},t)$q^(¨)=M^(-1)(q,t)Q(q,q^(˙),t)=a(q,q^(˙),t)\ddot{q}=M^{-1}(q, t) Q(q, \dot{q}, t)=a(q, \dot{q}, t)
Second, constraints presented in the system should be considered. We shall assume that the system is subjected to $h$$h$hh holonomic constraints of the form
其次，应考虑系统中存在的约束。我们将假设系统受到以下形式的 $h$$h$hh 整体约束
${\phi }_i(q,t)=0,i=1,2,\dots ,h$${\phi }_i(q,t)=0,i=1,2,\dots ,h$varphi_(i)(q,t)=0,quad i=1,2,dots,h\varphi_{i}(q, t)=0, \quad i=1,2, \ldots, h
and $m-h$$m-h$m-hm-h nonholonomic constraints of the form
和非 $m-h$$m-h$m-hm-h 完整约束形式的
${\phi }_i(q,\dot{q},t)=0,i=h+1,h+2,\dots ,m$${\phi }_i(q,\dot{q},t)=0,i=h+1,h+2,\dots ,m$varphi_(i)(q,q^(˙),t)=0,quad i=h+1,h+2,dots,m\varphi_{i}(q, \dot{q}, t)=0, \quad i=h+1, h+2, \ldots, m

In the description of any given set of constraints, the equations must be consistent; as such, we do not really care whether the constraints are linearly independent or not. Under the assumption of sufficient smoothness, we can obtain a set of constraint equations by differentiating nonholonomic constraints (4) once with respect to time $t$$t$tt and holonomic constraints (3) twice in the form of matrix equation:
在任何给定的约束集的描述中，方程必须是一致的;因此，我们并不真正关心 constraints 是否是线性独立的。在足够平滑的假设下，我们可以通过对非完整约束 （4） 对时间 $t$$t$tt 进行一次微分，将完整约束 （3） 以矩阵方程的形式微分两次来获得一组约束方程：
$A(q,\dot{q},t)\ddot{q}=b(q,\dot{q},t)$$A(q,\dot{q},t)\ddot{q}=b(q,\dot{q},t)$A(q,q^(˙),t)q^(¨)=b(q,q^(˙),t)A(q, \dot{q}, t) \ddot{q}=b(q, \dot{q}, t)
where $A(q,\dot{q},t)$$A(q,\dot{q},t)$A(q,q^(˙),t)A(q, \dot{q}, t) which denotes the constraint matrix is an $m\times n$$m\times n$m xx nm \times n matrix and $b(q,\dot{q},t)$$b(q,\dot{q},t)$b(q,q^(˙),t)b(q, \dot{q}, t) is an $m\times 1$$m\times 1$m xx1m \times 1 vector.
其中 $A(q,\dot{q},t)$$A(q,\dot{q},t)$A(q,q^(˙),t)A(q, \dot{q}, t) which 表示约束矩阵 是一个 $m\times n$$m\times n$m xx nm \times n 矩阵 并且 $b(q,\dot{q},t)$$b(q,\dot{q},t)$b(q,q^(˙),t)b(q, \dot{q}, t) 是一个 $m\times 1$$m\times 1$m xx1m \times 1 向量。

Remark The constraint used in Lagrangian’s equation of motion, Maggi equation, Boltzmann and Hamel equation as well as Gibbs and Appell equation is in either the zeroth- or first-order form. Second-order constraints are believed to be the most suitable form for further dynamic analysis and control design. The advantage of using the second-order constraint lies in the fact that it is linear in the acceleration. One may think that the differentiation of the constraint has resulted in the
备注 拉格朗日运动方程、Maggi 方程、玻尔兹曼和哈梅尔方程以及吉布斯和阿佩尔方程中使用的约束是零阶或一阶形式。二阶约束被认为是最适合进一步动力学分析和控制设计的形式。使用二阶约束的优点在于它在加速度上是线性的。有人可能会认为约束的微分导致了