CN111324146A - 一种水下巡检机器人的轨迹跟踪控制方法 - Google Patents

Abstract

本发明公开了一种水下巡检机器人的轨迹跟踪控制方法。包括如下步骤：（1）、预设水下巡检机器人轨迹跟踪的参考轨迹，参考轨迹用导航路径规划算法得到从起始点到目标点的最优路径，将其作为机器人轨迹跟踪的预设参考轨迹；（2）、结合参考轨迹与运动学模型设计轨迹跟踪的运动学控制器；（3）、对水下巡检机器人作受力分析，建立动力学模型，并设计轨迹跟踪的动力学控制器；（4）、将运动学控制器的控制值作为动力学控制器的输入，得到满足轨迹跟踪所需的推力及转矩，实现水下巡检机器人平滑稳定的轨迹跟踪控制。本发明通过设计动态的目标函数，不仅解决了速度跳变问题，还减小了实际轨迹与参考轨迹之间的滞后，在目标函数中加入的最短跟踪步长优化函数项，减小了实际轨迹的路径长度，进而降低了轨迹跟踪能耗；此外还考虑实际水下环境对轨迹跟踪的影响，最终实现了平滑稳定的轨迹跟踪控制。

Description

一种水下巡检机器人的轨迹跟踪控制方法

技术领域

本发明属于机器人轨迹跟踪控制技术领域，具体涉及一种水下巡检机器人的轨迹跟踪控制方法。

背景技术

自主水下机器人是海洋世界探索、海底资源开发及完成其他各项水下作业的重要工具。水下巡检机器人作为机器人的一个重要分支，在海底资源勘测、水下目标探测、水下管道检查、大坝裂缝检测等任务中发挥着重要的作用。三维水下环境中的轨迹跟踪控制问题是水下巡检机器人研究领域的热点及难点问题。水流等干扰因素的存在，对机器人的精确建模增加了难度，同时高度非线性和交叉耦合的系统动力学和不可预测的复杂水下环境，给控制器的设计带来相当大的困扰和不确定性。近年来，国内外研究机构在该课题上投入了大量精力，取得的科研成果也为其实际应用提供了重要的理论依据，进一步促进了水下机器人的发展与进步。但是，传统轨迹跟踪控制算法已不能满足当今水下机器人应用拓展的需要，例如反演控制、滑模控制和神经网络控制等，依然存在速度跳变、模型参数不确定以及控制律复杂度高等问题，并且没有考虑真实轨迹与期望轨迹间的滞后和轨迹跟踪能耗等问题。因此，实现水下机器人平滑稳定的轨迹跟踪控制具有重要的研究价值。

发明内容

本发明的目的是：提供一种基于改进模型预测控制的水下巡检机器人轨迹跟踪控制方法。既能解决速度跳变问题，且能减小实际轨迹与参考轨迹之间的滞后和降低轨迹跟踪能耗；实现了水下巡检机器人平滑稳定的轨迹跟踪控制。

为了达到上述目的，本发明的技术方案是：

一种水下巡检机器人的轨迹跟踪控制方法，包括如下步骤：

(1)、预设水下巡检机器人轨迹跟踪的参考轨迹，参考轨迹用导航路径规划算法得到从起始点到目标点的最优路径，将其作为机器人轨迹跟踪的预设参考轨迹；

(2)、结合参考轨迹与运动学模型设计轨迹跟踪的运动学控制器；

(3)、对水下巡检机器人作受力分析，建立动力学模型，设计轨迹跟踪的动力学控制器；

(4)、将运动学控制器的控制值作为动力学控制器的输入，得到满足轨迹跟踪所需的推力及转矩，实现水下巡检机器人平滑稳定的轨迹跟踪控制。

上述步骤(2)中结合参考轨迹与运动学模型设计轨迹跟踪的运动学控制器步骤如下：

(2a)、结合参考轨迹与运动学方程建立轨迹跟踪的离散状态空间模型，具体算法如下：

将水下巡检机器人轨迹跟踪控制系统看作是带有输入量u＝[u v w q r]^T和状态量ε＝[x y z θ ψ]^T的控制系统，其运动学方程表示为

对于给定的参考轨迹，轨迹上每个点都满足运动学方程，其形式表示为

其中ε_d＝[x_d y_d z_d θ_d ψ_d]^T表示水下巡检机器人的期望位姿，u_d＝[u_d v_d w_d q_d r_d]^T表示水下巡检机器人的期望速度；将式

的右端项在点(ε_d,u_d)处进行泰勒级数展开，忽略高阶项只保留一次项，得到：

式中J_f(ε)和J_f(u)分别表示f相对于ε和u的雅可比矩阵；

然后将式(1)减去一般形式的运动学方程，并将其近似离散化得到离散的状态空间模型为：

式中k表示离散时域，I表示单位矩阵，T采样时间间隔，并且

A(t)＝J_f(ε)和B(t)＝J_f(u)；

假定

则离散状态空间模型进一步表示为：

式中

和

其中m＝n，m和n分别表示输入维度与状态维度；

假定系统的预测时域和控制时域分别为N_p和N_c，系统在未来时刻的预测输出表示如下：

Y(k)＝ψ_kξ(k|k)+Θ_kΔU(k) (5)

式中

且

在预测时域内的系统状态量和输出量通过系统的当前状态量ξ(k|k)和控制时域内的控制增量ΔU(k)计算得到；

(2b)、设定合适的优化目标函数并对其求解得到系统的控制增量，具体算法如下：

设计目标函数：

式中Q和R分别表示系统状态量和控制量的权重矩阵，S表示J₃项的权重矩阵，Δx(k+i)＝u(k)·T、Δy(k+i)＝v(k)·T和Δz(k+i)＝w(k)·T，T表示采样时间间隔；

设定函数项||ε(k+i|k)-ε_d(k+i|k)||²和||Δx(k+i)||²+||Δy(k+i)||²+||Δz(k+i)||²的权重矩阵Q与S都是固定的单位矩阵；而函数项||Δu(k+i|k)||²的权重矩阵R随轨迹跟踪误差在线调整；

跟随轨迹跟踪误差在线调整的权重矩阵R表示如为：

R＝λI (7)

式中I表示m×m的单位矩阵，λ是跟随当前轨迹跟踪误差而在线动态调整的系数，表示为：

式中e_min和e_max分别表示参考跟踪精度要求而确定的两个轨迹跟踪误差边界，e表示当前轨迹跟踪的误差，e_x、e_y和e_z表示当前跟踪误差分别在x轴、y轴和z轴上的分量；

(2c)、在实际的水下巡检机器人轨迹跟踪控制系统中，需要满足系统状态量和控制量的一些约束条件：

控制量约束：u_min(k+i)≤u(k+i)≤u_max(k+i),i＝0,1,…,N_c-1

控制增量约束：Δu_min(k+i)≤Δu(k+i)≤Δu_max(k+i),i＝0,1,…,N_c-1；

(2d)、在每个控制周期内求解优化目标函数，得到控制时域内的一系列控制输入增量为

根据模型预测控制的基本原理，将该控制序列中第一个元素作为实际的控制输入增量作用于水下巡检机器人轨迹跟踪控制系统，即

该系统执行上述控制量直至下一时刻；在新的时刻，该系统根据状态信息重新预测下一段时域的输出，通过优化求解得到一个新的控制增量序列。

上述步骤(3)中对水下巡检机器人作受力分析，建立动力学模型，设计轨迹跟踪的动力学控制器步骤如下：

(3a)、考虑重力、浮力、水动力和推力对水下巡检机器人系统控制的影响，应用力、力矩平衡和牛顿第二运动定律得到水下巡检机器人轨迹跟踪的动力学方程表示如下：

式中

F_T＝(T_x T_y T_z M_T N_T)^T，

m_o表示水下巡检机器人质量，

和

分别表示x轴、y轴和z轴方向上的附加质量，

和

分别表示沿y轴和z轴旋转方向的附加质量，I_y和I_z是分别对应于y轴和z轴的转动惯量；F_T表示由水下巡检机器人推进器产生的推力和力矩向量；假定水下巡检机器人重心与载体坐标系原点重合，(x_b y_b z_b)表示浮力中心在载体坐标系中的坐标；ρ为水的密度，C_d为阻力系数，S_x、S_y和S_z分别表示水下巡检机器人垂直于x轴、y轴和z轴的投影区域面积，u_If＝(u_If v_If w_If)^T表示水流相对于水下巡检机器人的速度，u_Bf＝(u_Bf v_Bf w_Bf)^T表示水流在载体坐标系下的速度，K_y和K_z表示阻力矩系数；

(3b)、根据Udwadia-Kalaba理论，通过将非完整约束和完整约束分别对时间t做一次和两次微分获得一组约束表达式，其矩阵形式表示如下：

式中

表示一个m×n的约束矩阵，表示微分后得到的m维向量；

约束系统的显示运动方程表示如下：

式中

是一个n×1的约束矩阵，它是由完整约束和非完整约束产生的，即附加在无约束系统上的约束力，理想的约束力写成如下形式：

式中上标“+”表示穆尔-彭罗斯广义逆；

(3c)、求解约束系统的显示运动方程，具体算法如下：

在惯性坐标下由运动学控制器给出的期望速度

满足运动学方程，表示如下：

针对轨迹跟踪控制问题，Udwadia-Kalaba理论将期望轨迹视为系统约束，因此将式(15)对时间t做一次求导，得到如下的系统约束：

通过比较分析式(12)与(16)，得到：

通过比较分析式(10)与(13)，得到M(q,t)＝M(h,t)、

和

其中q＝h、

和

分别表示位移、速度和加速度；其中

和

将式(17)代入式(14)，得到满足轨迹跟踪的理想约束力。

本发明的有益效果如下：

(1)为减小实际轨迹与期望轨迹间的滞后，发明了随跟踪误差在线调整的控制增量权重

在设计合适的优化目标函数时，为确保机器人始终沿着期望轨迹航行，需要尽可能地减小实际轨迹与期望轨迹上对应点的横向偏移最小；同时考虑到机器人的加速与转向性能受到推进器性能和环境的影响，需要尽可能减小控制输入增量。在传统模型预测控制算法中，上述优化目标函数项的权重矩阵Q和R在整个控制过程中是固定不变的，本发明设计的目标函数主要考虑以下几个关键点：在整个控制过程中尽可能减少实际轨迹与期望轨迹之间的偏差，其次，控制增量在系统过渡阶段应该比它在稳定阶段更值得关注，并且控制增量在过渡阶段得到的关注度与它在稳定阶段得到的关注度应该是不同的。因此，本发明设计了随轨迹跟踪误差在线调整的控制增量权重，可以减少实际轨迹与期望轨迹之间的滞后。

(2)为降低轨迹跟踪能耗，在设计的优化目标函数中加入了最短步长优化函数项

在设计合适的优化目标函数时，在每两个连续采样点间，如果机器人行进的距离是单个预测区间内可行进的最短距离，就可以有效地减少实际轨迹的路径长度，进而降低轨迹跟踪能耗。在传统轨迹跟踪控制算法中，尚未考虑其轨迹跟踪能耗问题。本发明在设计目标函数时，考虑了轨迹跟踪能耗问题，在目标函数中加入了最短步长优化函数项。此外，在降低轨迹跟踪误差、限制推进器动作和降低轨迹跟踪能耗之间建立了折中的方案，在确保轨迹跟踪误差渐进稳定的同时，降低速度跳变值，减小实际轨迹与期望轨迹间的滞后，并降低了轨迹跟踪能耗。

(3)考虑实际的水下环境对机器人轨迹跟踪控制的影响

考虑重力、浮力、水动力和推力对机器人系统控制的影响，应用力、力矩平衡和牛顿第二运动定律得到机器人轨迹跟踪的动力学方程，并基于Udwadia-Kalaba理论设计了机器人轨迹跟踪的动力学控制器。

附图说明

图1水下巡检机器人的轨迹跟踪控制方法流程。

具体实施方式

围绕发明内容，本发明的具体实施方式(如图1所示)如下：

(1)、预设水下机器人轨迹跟踪的参考轨迹

参考轨迹用导航路径规划算法得到从起始点到目标点的最优路径，将其作为机器人轨迹跟踪的预设参考轨迹；

(2)、结合参考轨迹与运动学模型设计轨迹跟踪的运动学控制器

(2a)、结合参考轨迹与运动学方程建立轨迹跟踪的离散状态空间模型，具体算法如下：

将水下巡检机器人轨迹跟踪控制系统看作是带有输入量u＝[u v w q r]^T和状态量ε＝[x y z θ ψ]^T的控制系统，其运动学方程表示为

对于给定的参考轨迹，轨迹上每个点都满足该运动学方程，其形式表示为

其中ε_d＝[x_d y_d z_d θ_d ψ_d]^T表示水下巡检机器人的期望位姿，u_d＝[u_d v_d w_d q_d r_d]^T表示水下巡检机器人的期望速度；将式

的右端项在点(ε_d,u_d)处进行泰勒级数展开，忽略高阶项只保留一次项，可以得到：

式中J_f(ε)和J_f(u)分别表示f相对于ε和u的雅可比矩阵；

然后将式(1)减去一般形式的运动学方程，并将其近似离散化得到离散的状态空间模型为：

式中k表示离散时域，I表示单位矩阵，T采样时间间隔，并且

A(t)＝J_f(ε)和B(t)＝J_f(u)；

假定

则离散状态空间模型可以进一步表示为：

式中

和

其中m＝n，m和n分别表示输入维度与状态维度。

假定系统的预测时域和控制时域分别为N_p和N_c，那么系统在未来时刻的预测输出可以表示如下：

Y(k)＝ψ_kξ(k|k)+Θ_kΔU(k) (5)

式中

且

在预测时域内的系统状态量和输出量通过系统的当前状态量ξ(k|k)和控制时域内的控制增量ΔU(k)计算得到；

(2b)、设定合适的优化目标函数并对其求解得到系统的控制增量，具体算法如下：

设计目标函数：

式中Q和R分别表示系统状态量和控制量的权重矩阵，S表示J₃项的权重矩阵，Δx(k+i)＝u(k)·T、Δy(k+i)＝v(k)·T和Δz(k+i)＝w(k)·T，T表示采样时间间隔；

设定函数项||ε(k+i|k)-ε_d(k+i|k)||²和||Δx(k+i)||²+||Δy(k+i)||²+||Δz(k+i)||²的权重矩阵Q与S都是固定的单位矩阵；而函数项||Δu(k+i|k)||²的权重矩阵R随轨迹跟踪误差在线调整；

跟随轨迹跟踪误差在线调整的权重矩阵R表示如为：

R＝λI (7)

式中I表示m×m的单位矩阵，λ是跟随当前轨迹跟踪误差而在线动态调整的系数，表示为：

式中e_min和e_max分别表示参考跟踪精度要求而确定的两个轨迹跟踪误差边界，e表示当前轨迹跟踪的误差，e_x、e_y和e_z表示当前跟踪误差分别在x轴、y轴和z轴上的分量；

(2c)、在实际的水下巡检机器人轨迹跟踪控制系统中，需要满足系统状态量和控制量的一些约束条件：

控制量约束：u_min(k+i)≤u(k+i)≤u_max(k+i),i＝0,1,…,N_c-1；

控制增量约束：Δu_min(k+i)≤Δu(k+i)≤Δu_max(k+i),i＝0,1,…,N_c-1；

(2d)、在每个控制周期内求解优化目标函数，得到控制时域内的一系列控制输入增量为

根据模型预测控制的基本原理，将该控制序列中第一个元素作为实际的控制输入增量作用于水下巡检机器人轨迹跟踪控制系统，即

该系统执行上述控制量直至下一时刻；在新的时刻，该系统根据状态信息重新预测下一段时域的输出，通过优化求解得到一个新的控制增量序列。

(3)、对水下巡检机器人作受力分析，建立动力学模型，设计轨迹跟踪的动力学控制器

(3a)、考虑重力、浮力、水动力和推力对水下巡检机器人系统控制的影响，应用力、力矩平衡和牛顿第二运动定律得到水下巡检机器人轨迹跟踪的动力学方程表示如下：

式中

F_T＝(T_x T_y T_z M_T N_T)^T，

m_o表示水下巡检机器人质量，

和

分别表示x轴、y轴和z轴方向上的附加质量，

和

分别表示沿y轴和z轴旋转方向的附加质量，I_y和I_z是分别对应于y轴和z轴的转动惯量；F_T表示由水下巡检机器人推进器产生的推力和力矩向量；假定水下巡检机器人重心与载体坐标系原点重合，(x_b y_b z_b)表示浮力中心在载体坐标系中的坐标；ρ为水的密度，C_d为阻力系数，S_x、S_y和S_z分别表示水下巡检机器人垂直于x轴、y轴和z轴的投影区域面积，u_If＝(u_If v_If w_If)^T表示水流相对于水下巡检机器人的速度，u_Bf＝(u_Bf v_Bf w_Bf)^T表示水流在载体坐标系下的速度，K_y和K_z表示阻力矩系数；

(3b)、根据Udwadia-Kalaba理论，通过将非完整约束和完整约束分别对时间t做一次和两次微分获得一组约束表达式，其矩阵形式表示如下：

式中

表示一个m×n的约束矩阵，

表示微分后得到的m维向量；

约束系统的显示运动方程可以表示如下：

式中

是一个n×1的约束矩阵，它是由完整约束和非完整约束产生的，即附加在无约束系统上的约束力，理想的约束力可以写成如下形式：

式中上标“+”表示穆尔-彭罗斯广义逆；

(3c)、求解约束系统的显示运动方程，具体算法如下：

在惯性坐标下由运动学控制器给出的期望速度

满足运动学方程，表示如下：

针对轨迹跟踪控制问题，Udwadia-Kalaba理论将期望轨迹视为系统约束，因此将式(15)对时间t做一次求导，得到如下的系统约束：

通过比较分析式(12)与(16)，可以得到：

通过比较分析式(10)与(13)，得到M(q,t)＝M(h,t)、

和

其中q＝h、

和

分别表示位移、速度和加速度；其中

和

将式(17)代入式(14)，得到满足轨迹跟踪的理想约束力。

(4)、将运动学控制器的控制值作为动力学控制器的输入，得到满足轨迹跟踪所需的推力及转矩，实现水下巡检机器人平滑稳定的轨迹跟踪控制。

Claims (3)

1.一种水下巡检机器人的轨迹跟踪控制方法，其特征在于：包括如下步骤：

(1)、预设水下巡检机器人轨迹跟踪的参考轨迹，参考轨迹用导航路径规划算法得到从起始点到目标点的最优路径，将其作为机器人轨迹跟踪的预设参考轨迹；

(2)、结合参考轨迹与运动学模型设计轨迹跟踪的运动学控制器；

(3)、对水下巡检机器人作受力分析，建立动力学模型，设计轨迹跟踪的动力学控制器；

(4)、将运动学控制器的控制值作为动力学控制器的输入，得到满足轨迹跟踪所需的推力及转矩，实现水下巡检机器人平滑稳定的轨迹跟踪控制。

2.根据权利要求1所述的一种水下巡检机器人的轨迹跟踪控制方法，其特征在于：所述步骤(2)中结合参考轨迹与运动学模型设计轨迹跟踪的运动学控制器步骤如下：

(2a)、结合参考轨迹与运动学方程建立轨迹跟踪的离散状态空间模型，具体算法如下：

将水下巡检机器人轨迹跟踪控制系统看作是带有输入量u＝[u v w q r]^T和状态量ε＝[x y z θ ψ]^T的控制系统，其运动学方程表示为

对于给定的参考轨迹，轨迹上每个点都满足运动学方程，其形式表示为

其中ε_d＝[x_d y_d z_d θ_d ψ_d]^T表示水下巡检机器人的期望位姿，u_d＝[u_d v_d w_d q_d r_d]^T表示水下巡检机器人的期望速度；将式

的右端项在点(ε_d,u_d)处进行泰勒级数展开，忽略高阶项只保留一次项，得到：

式中J_f(ε)和J_f(u)分别表示f相对于ε和u的雅可比矩阵；

然后将式(1)减去一般形式的运动学方程，并将其近似离散化得到离散的状态空间模型为：

式中k表示离散时域，I表示单位矩阵，T采样时间间隔，并且

A(t)＝J_f(ε)和B(t)＝J_f(u)；

假定

则离散状态空间模型进一步表示为：

式中

和

其中m＝n，m和n分别表示输入维度与状态维度；

假定系统的预测时域和控制时域分别为N_p和N_c，系统在未来时刻的预测输出表示如下：

Y(k)＝ψ_kξ(k|k)+Θ_kΔU(k) (5)

式中

且

在预测时域内的系统状态量和输出量通过系统的当前状态量ξ(k|k)和控制时域内的控制增量ΔU(k)计算得到；

(2b)、设定合适的优化目标函数并对其求解得到系统的控制增量，具体算法如下：

设计目标函数：

式中Q和R分别表示系统状态量和控制量的权重矩阵，S表示J₃项的权重矩阵，Δx(k+i)＝u(k)·T、Δy(k+i)＝v(k)·T和Δz(k+i)＝w(k)·T，T表示采样时间间隔；设定函数项||ε(k+i|k)-ε_d(k+i|k)||²和||Δx(k+i)||²+||Δy(k+i)||²+||Δz(k+i)||²的权重矩阵Q与S都是固定的单位矩阵；而函数项||Δu(k+i|k)||²的权重矩阵R随轨迹跟踪误差在线调整；

跟随轨迹跟踪误差在线调整的权重矩阵R表示如为：

R＝λI (7)

式中I表示m×m的单位矩阵，λ是跟随当前轨迹跟踪误差而在线动态调整的系数，表示为：

式中e_min和e_max分别表示参考跟踪精度要求而确定的两个轨迹跟踪误差边界，e表示当前轨迹跟踪的误差，e_x、e_y和e_z表示当前跟踪误差分别在x轴、y轴和z轴上的分量；

(2c)、在实际的水下巡检机器人轨迹跟踪控制系统中，需要满足系统状态量和控制量的一些约束条件：

控制量约束：u_min(k+i)≤u(k+i)≤u_max(k+i),i＝0,1,…,N_c-1；

控制增量约束：Δu_min(k+i)≤Δu(k+i)≤Δu_max(k+i),i＝0,1,…,N_c-1；

(2d)、在每个控制周期内求解优化目标函数，得到控制时域内的一系列控制输入增量为

根据模型预测控制的基本原理，将该控制序列中第一个元素作为实际的控制输入增量作用于水下巡检机器人轨迹跟踪控制系统，即

该系统执行上述控制量直至下一时刻；在新的时刻，该系统根据状态信息重新预测下一段时域的输出，通过优化求解得到一个新的控制增量序列。

3.根据权利要求1所述的一种水下巡检机器人的轨迹跟踪控制方法，其特征在于：所述步骤(3)中对水下巡检机器人作受力分析，建立动力学模型，设计轨迹跟踪的动力学控制器步骤如下：

(3a)、考虑重力、浮力、水动力和推力对水下巡检机器人系统控制的影响，应用力、力矩平衡和牛顿第二运动定律得到水下巡检机器人轨迹跟踪的动力学方程表示如下：

式中

F_T＝(T_x T_y T_z M_T N_T)^T，

m_o表示水下巡检机器人质量，

和

分别表示x轴、y轴和z轴方向上的附加质量，

和

分别表示沿y轴和z轴旋转方向的附加质量，I_y和I_z是分别对应于y轴和z轴的转动惯量；F_T表示由水下巡检机器人推进器产生的推力和力矩向量；假定水下巡检机器人重心与载体坐标系原点重合，(x_b y_b z_b)表示浮力中心在载体坐标系中的坐标；ρ为水的密度，C_d为阻力系数，S_x、S_y和S_z分别表示水下巡检机器人垂直于x轴、y轴和z轴的投影区域面积，u_If＝(u_If v_If w_If)^T表示水流相对于水下巡检机器人的速度，u_Bf＝(u_Bf v_Bf w_Bf)^T表示水流在载体坐标系下的速度，K_y和K_z表示阻力矩系数；

(3b)、根据Udwadia-Kalaba理论，通过将非完整约束和完整约束分别对时间t做一次和两次微分获得一组约束表达式，其矩阵形式表示如下：

式中

表示一个m×n的约束矩阵，

表示微分后得到的m维向量；约束系统的显示运动方程表示如下：

式中

是一个n×1的约束矩阵，它是由完整约束和非完整约束产生的，即附加在无约束系统上的约束力，理想的约束力写成如下形式：

式中上标“+”表示穆尔-彭罗斯广义逆；

(3c)、求解约束系统的显示运动方程，具体算法如下：

在惯性坐标下由运动学控制器给出的期望速度

满足运动学方程，表示如下：

针对轨迹跟踪控制问题，Udwadia-Kalaba理论将期望轨迹视为系统约束，因此将式(15)对时间t做一次求导，得到如下的系统约束：

通过比较分析式(12)与(16)，得到：

通过比较分析式(10)与(13)，得到M(q,t)＝M(h,t)、

和

其中q＝h、

和

分别表示位移、速度和加速度；

其中

和

将式(17)代入式(14)，得到满足轨迹跟踪的理想约束力。

Images