CN103170976A - 二自由度机器人运动控制方法 - Google Patents

Abstract

本发明公开了一种二自由度机器人运动控制方法，用于解决现有机器人控制方法控制精度差的技术问题。技术方案是首先首先对二自由度机器人的客观物理动力学特性进行建模，抽象出二自由度机器人一般形式的动力学特性方程。在此基础上，采用逐次逼近，最终收敛的方法完成转角控制，从根本上消除了θ→0时，二自由度机器人的震荡，为后续线运动控制奠定基础。然后引入实半径ρ₂和轨迹曲率半径ρ₁，变换二自由度机器人在二维极坐标下的运动学方程，令ρ₂正比于cotθ，再利用已有的转角控制，得到线运动控制方程，完成跑点控制。提高了二自由度机器人的控制精度，而且方程中的参数物理意义明确，不需人工调试即可确定。

Description

二自由度机器人运动控制方法

技术领域

本发明涉及一种机器人运动控制方法，特别是涉及一种二自由度机器人运动控制方法。

背景技术

文献“武卫霞，两轮自平衡机器人运动平衡控制方法的研究，北京工业大学硕士学位论文，2010.5.”公开了两轮自平衡机器人运动平衡控制的三种方法，LQR控制方法，模糊PID控制方法和反馈线性化控制方法。

二自由度机器人的控制包括转弯和跑点两个最基础的任务，在此基础上可组合出复杂的任务和动作。

文献公开的三种控制方法完全适用于二自由度机器人的控制。其中，LQR控制方法主要针对线性系统或非线性系统的局部控制问题；模糊PID控制方法有延迟时间和震荡超调量，抗干扰能力和实时性有限；反馈线性化控制方法较模糊PID控制方法性能有所改善，但由于未充分考虑控制对象的动力学性，在控制二自由度机器人时，二自由度机器人完成任务的精确度和快速性之间的折衷还有较大的优化余地。其次，三种控制方法都需要通过人工调试，手动优化参数，才能使用，工作较为繁复。再者，三种控制方法由于未能充分考虑控制对象的动力学特性，无法从根本上消除θ→0时，二自由度机器人的震荡，进而影响完成任务的时间和精度。

发明内容

为了克服现有机器人控制方法控制精度差的不足，本发明提供一种二自由度机器人运动控制方法。该方法首先对二自由度机器人的客观物理动力学特性进行建模，抽象出二自由度机器人一般形式的动力学特性方程。在此基础上，采用逐次逼近，最终收敛的方法完成转角控制，从根本上消除了θ→0时，二自由度机器人的震荡，为后续线运动控制奠定基础。然后引入实半径ρ₂和轨迹曲率半径ρ₁，变换二自由度机器人在二维极坐标下的运动学方程，令ρ₂正比于cotθ，再利用已有的转角控制，得到线运动控制方程，完成跑点控制。可以提高二自由度机器人的控制精度，而且方程中的参数物理意义明确，不需人工调试即可确定。

本发明解决其技术问题所采用的技术方案是：一种二自由度机器人运动控制方法，其特点是包括以下步骤：

（I）获取二自由度机器人运动控制系统的动力学特性方程：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\υ}}=f_{\mathrm{\υ}}(\mathrm{\υ},V)\\ \stackrel{\·}{\mathrm{\ω}}=f_{\mathrm{\ω}}(\mathrm{\ω},T)\\ g(V,T)\≤0\\ V_{\min }\≤V\≤V_{\max }\\ T_{\min }\≤T\≤T_{\max }\end{array}\right.,$

其中：

υ——二自由度机器人的线速度，ω——二自由度机器人角速度；

——二自由度机器人的线加速度，

——二自由度机器人角加速度；

V——二自由度机器人线速度的控制输入，T——二自由度机器人角速度的控制输入；

V_min——V的下限；V_max——V的上限；

T_min——T的下限；T_max——T的上限；

f_υ——函数映射关系；f_ω——函数映射关系；

当V_min≤V≤V_max时，

当T_min≤T≤T_max时，

V＝V_c,T＝T_c时，二自由度机器人处于无控制自然状态。

（II）以二自由度机器人中心为原点，二自由度机器人正方向为极轴建立极坐标系。

（III）计算目标点的极坐标θ和ρ。

（IV）转角控制。

步骤1：获取当前转角速度ω₀，计算完全靠惯性使二自由度机器人状态达到ω₀=0，所需转过的角度θ_{o_cri}：

则由得ω(t,T)＝F_ω(t,ω₀,T)，由于无控制自然状态下，运动物体最终一定会静止。所以，令T＝T_c，则有：

ω(∞,T_c)＝F_ω(∞,ω₀,T_c)＝0；

${\mathrm{\θ}}_{o\_\mathrm{cri}}=\mathrm{sign}\left(\mathrm{\θ}\right)\·{\∫}_0^{\∞}\mathrm{\ω}(\mathrm{\τ},T_c)\·\mathrm{d\τ}$ 是存在的。

其中，F_ω表示由微分方程

及初值条件ω(0,T)＝ω₀，所确定ω关于t和T的函数关系。

步骤2：确定控制器输出T_o。

若|θ|＞θ_{o_cri}，则使机器人以最大力度转角，即T＝max{T·sign(θ)|T_min≤T≤T_max}；

若|θ|≤θ_{o_cri}，则对任意T∈{T|f_ω(ω₀,T)·ω₀＜0,T_min≤T≤T_max}，令ω(t,)T＝_ω(Fω_,0)t＝,，解方程F_ω(t,ω₀,T)＝0，得：t_o(T)且t_o(T)＜+∞，此时机器人转过的有利角度为：

${\mathrm{\θ}}_o\left(T\right)=\mathrm{sign}\left(\mathrm{\θ}\right)\·{\∫}_0^{t_o\left(T\right)}\mathrm{\ω}(\mathrm{\τ},T)\·\mathrm{d\τ};$

求解T*使得θ_o(T^*)＝|θ|。

当T_min≤T^*≤T_max时，T_o＝T^*；否则，T_o＝min{f_ω(ω₀,T)·sign(ω₀)|T_min≤T≤T_max}。

（V）如果任务是原地转角，则线速度控制输出V_o＝V_c，跳转到步骤（VII）。否则转到步骤（VI）进行线运动控制。

（VI）线运动控制。

步骤1：变换运动方程。

目标点在极坐标系中的运动方程为：

令轨迹曲率半径实半径 ${\mathrm{\ρ}}_2=\frac{\stackrel{\·}{\mathrm{\ρ}}}{\stackrel{\·}{\mathrm{\θ}}},$ 则运动方程为： $\frac{1}{{\mathrm{\ρ}}_1\·\sin \mathrm{\θ}}=\frac{1}{\mathrm{\ρ}}+\frac{1}{{\mathrm{\ρ}}_2\·\tan \mathrm{\θ}}.$

步骤2：控制ρ₂，驱动ρ₁。令ρ₂＝a·cotθ，则上述步骤1得到的运动方程变为驱动方程： ${\mathrm{\ρ}}_1=\frac{1}{\sin \mathrm{\θ}\×(1/\mathrm{\ρ}+1/a)}.$

其中，a是模型参数，a＝V_max。

步骤3：将时间向前推进Δt，计算Δt时间后二自由度机器人的角速度和需要的速度：

${\mathrm{\ω}}_{\mathrm{\Δt}}\≈{\mathrm{\ω}}_0+\stackrel{\·}{\mathrm{\ω}}\·\mathrm{\Δt}={\mathrm{\ω}}_0+f_{\mathrm{\ω}}({\mathrm{\ω}}_0,T_o)\·\mathrm{\Δt};$

令 ${\mathrm{\ρ}}_1=\frac{{\mathrm{\υ}}_{\mathrm{\Δt}}}{{\mathrm{\ω}}_{\mathrm{\Δt}}},$ 代入驱动方程得： ${\mathrm{\υ}}_{\mathrm{\Δt}}=\frac{{\mathrm{\ω}}_{\mathrm{\Δt}}}{\sin \mathrm{\θ}\×(1/\mathrm{\ρ}+1/a)}.$

其中，Δt＝1f，f是控制器与机器人接收器的数据交换频率。

步骤4：模型修正：由于θ→0时，

会在1和-1上震荡，故模型修正为：

${\mathrm{\υ}}_{\mathrm{\Δt}}=\frac{(1-\mathrm{\α})\·{\mathrm{\ω}}_{\mathrm{\Δt}}+\mathrm{\α}\·\cos \mathrm{\θ}}{[(1-\mathrm{\α})\·{\mathrm{\ω}}_{\mathrm{\Δt}}+\mathrm{\α}\·\cos \mathrm{\θ}]\×(1/\mathrm{\ρ}+1/a)}.$

其中，ω'为环境对ω期望为0的随机扰动，模型参数α取为该扰动的幅值：α＝|ω'|。

步骤5：确定控制器输出V_o。计算Δt时间段内二自由度机器人需要的加速度：

$\stackrel{\‾}{a}\≈\frac{{\mathrm{\υ}}_{\mathrm{\Δt}}-{\mathrm{\υ}}_0}{\mathrm{\Δt}},$

求解V^*使得

若V^*≥V_max，则令V^*＝V_max，否则若V^*≤V_min，则令V^*＝V_min。

若g(V^*,T_o)≤0，则V_o＝V^*，否则由g(V_l,T_o)≤0解出V_l，令V_o＝V_l。

（VII）用获得的V_o,T_o驱动二自由度机器人。

本发明的有益效果是：由于该方法首先对二自由度机器人的客观物理动力学特性进行建模，抽象出二自由度机器人一般形式的动力学特性方程。在此基础上，采用逐次逼近，最终收敛的方法完成转角控制，从根本上消除了θ→0时，二自由度机器人的震荡，为后续线运动控制奠定基础。然后引入实半径ρ₂和轨迹曲率半径ρ₁，变换二自由度机器人在二维极坐标下的运动学方程，令ρ₂正比于cotθ，再利用已有的转角控制，得到线运动控制方程，完成跑点控制。提高了二自由度机器人的控制精度，而且方程中的参数物理意义明确，不需人工调试即可确定。

下面结合附图和实施例对本发明作详细说明。

附图说明

图1是本发明二自由度机器人运动控制方法所用极坐标系示意图。

图2是本发明二自由度机器人运动控制方法的流程图。

具体实施方式

参照图1～2。本发明二自由度机器人运动控制方法具体步骤如下：

一、本发明所应用的数学理论、控制原理及技术术语说明。

1、动力学特性方程。

动力学特性是指运动主体在具体环境中运动所遵循的动力学性质，其数学描述就是动力学特性方程。二自由度机器人的动力学特性方程一般可写成如下形式：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\υ}}=f_{\mathrm{\υ}}(\mathrm{\υ},V)\\ \stackrel{\·}{\mathrm{\ω}}=f_{\mathrm{\ω}}(\mathrm{\ω},T)\\ g(V,T)\≤0\\ V_{\min }\≤V\≤V_{\max }\\ T_{\min }\≤T\≤T_{\max }\end{array}\right.,$

其中：

υ——二自由度机器人的线速度，ω——二自由度机器人角速度；

——二自由度机器人的线加速度，

——二自由度机器人角加速度；

V——二自由度机器人线速度的控制输入，T——二自由度机器人角速度的控制输入；

V_min——V的下限；V_max——V的上限；

T_min——T的下限；T_max——T的上限；

f_υ——函数映射关系；f_ω——函数映射关系；

当V_min≤V≤V_max时，

当T_min≤T≤T_max时，

V＝V_c,T＝T_c时，二自由度机器人处于无控制自然状态。

2、运动方程。

参照图1，运动方程是刻划系统运动的物理参量所满足的方程或方程组。它们以这些参量对于时间的微分方程形式出现。(θ,ρ)为目标点在所建坐标系中的坐标，υ为机器人质心的线速度大小，ω为机器人质心作角速度大小。?由于在已建坐标系中机器人是静止的，故目标点始终以速度υ朝极轴的负方向运动，同时，绕原点以角速度-ω做圆周运动，可得如下运动方程:

3、实半径。

实半径是指在二维极坐标系中，在曲线ρ＝ρ(θ)某点，ρ对θ的导数。

二、本发明的具体实现。

本发明主要包括二自由度机器人运动到定点的控制方法和机器人原地转角具体步骤如下:

（I）经实验测量，数据处理，获取系统的动力学特性方程：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\υ}}=f_{\mathrm{\υ}}(\mathrm{\υ},V)\\ \stackrel{\·}{\mathrm{\ω}}=f_{\mathrm{\ω}}(\mathrm{\ω},T)\\ g(V,T)\≤0\\ V_{\min }\≤V\≤V_{\max }\\ T_{\min }\≤T\≤T_{\max }\end{array}\right.,$

其中：

υ——二自由度机器人的线速度，ω——二自由度机器人角速度；

——二自由度机器人的线加速度，

——二自由度机器人角加速度；

V——二自由度机器人线速度的控制输入，T——二自由度机器人角速度的控制输入；

V_min——V的下限；V_max——V的上限；

T_min——T的下限；T_max——T的上限；

f_υ——函数映射关系；f_ω——函数映射关系；

当V_min≤V≤V_max时，

当T_min≤T≤T_max时，

V＝V_c,T＝T_c时，二自由度机器人处于无控制自然状态。

（II）以机器人中心为原点，机器人正方向为极轴建立极坐标系。

（III）计算目标点的极坐标θ和ρ。

（IV）转角控制：

步骤1：获取当前转角速度ω₀，计算完全靠惯性使机器人状态达到到ω₀=0，所需转过的角度θ_{o_cri}：

则由

可得ω(t,T)＝F_ω(t,ω₀,T)，由物理常识可知：无控制自然状态下，运动物体最终一定会静止。所以，令T＝T_c，则有：

ω(∞,T_c)＝F_ω(∞,ω₀,T_c)＝0；

${\mathrm{\θ}}_{o\_\mathrm{cri}}=\mathrm{sign}\left(\mathrm{\θ}\right)\·{\∫}_0^{\∞}\mathrm{\ω}(\mathrm{\τ},T_c)\·\mathrm{d\τ}$ 是存在的。

其中，F_ω表示由微分方程

及初值条件ω(0,T)＝ω₀，所确定ω关于t和T的函数关系。

步骤2：确定控制器输出T_o。

若|θ|＞θ_{o_cri}，则可使机器人以最大力度转角，即T＝max{T·sign(θ)|T_min≤T≤T_max}；

若|θ|≤θ_{o_cri}，则对任意T∈{T|f_ω(ω₀,T)·ω₀＜0,T_min≤T≤T_max}，令ω(t,)T＝_ω(Fω_,0)t＝,，T解0方程F_ω(t,ω₀,T)＝0，得：t_o(T)且t_o(T)＜+∞，此时机器人转过的有利角度为：

${\mathrm{\θ}}_o\left(T\right)=\mathrm{sign}\left(\mathrm{\θ}\right)\·{\∫}_0^{t_o\left(T\right)}\mathrm{\ω}(\mathrm{\τ},T)\·\mathrm{d\τ};$

求解T^*使得θ_o(T^*)＝|θ|。

当T_min≤T^*≤T_max时T_o＝T^*，否则T_o＝min{f_ω(ω₀,T)·sign(ω₀)|T_min≤T≤T_max}。

（V）分析任务特点，如果任务是原地转角，则线速度控制输出V_o＝V_c，跳转到（VII）。否则转到（VI）进行线运动控制。

（VI）线运动控制

步骤1：变换运动方程。

目标点在极坐标系中的运动方程为：

令轨迹曲率半径

实半径 ${\mathrm{\ρ}}_2=\frac{\stackrel{\·}{\mathrm{\ρ}}}{\stackrel{\·}{\mathrm{\θ}}},$ 则运动方程可变为： $\frac{1}{{\mathrm{\ρ}}_1\·\sin \mathrm{\θ}}=\frac{1}{\mathrm{\ρ}}+\frac{1}{{\mathrm{\ρ}}_2\·\tan \mathrm{\θ}}.$

步骤2：控制ρ₂，驱动ρ₁。令ρ₂＝a·cotθ，则上述步骤1得到的运动方程变为： ${\mathrm{\ρ}}_1=\frac{1}{\sin \mathrm{\θ}\×(1/\mathrm{\ρ}+1/a)}$ （以下简称驱动方程）。

其中，a是模型参数，a＝V_max。

步骤3：将时间向前推进Δt，计算Δt时间后机器人的角速度和需要的速度：

${\mathrm{\ω}}_{\mathrm{\Δt}}\≈{\mathrm{\ω}}_0+\stackrel{\·}{\mathrm{\ω}}\·\mathrm{\Δt}={\mathrm{\ω}}_0+f_{\mathrm{\ω}}({\mathrm{\ω}}_0,T_o)\·\mathrm{\Δt};$

令 ${\mathrm{\ρ}}_1=\frac{{\mathrm{\υ}}_{\mathrm{\Δt}}}{{\mathrm{\ω}}_{\mathrm{\Δt}}},$ 代入驱动方程可得： ${\mathrm{\υ}}_{\mathrm{\Δt}}=\frac{{\mathrm{\ω}}_{\mathrm{\Δt}}}{\sin \mathrm{\θ}\×(1/\mathrm{\ρ}+1/a)}.$

其中，Δt＝1f，f是控制器与机器人接收器的数据交换频率。

步骤4：模型修正：由于θ→0时，

会在1和-1上震荡，故模型修正为：

${\mathrm{\υ}}_{\mathrm{\Δt}}=\frac{(1-\mathrm{\α})\·{\mathrm{\ω}}_{\mathrm{\Δt}}+\mathrm{\α}\·\cos \mathrm{\θ}}{[(1-\mathrm{\α})\·{\mathrm{\ω}}_{\mathrm{\Δt}}+\mathrm{\α}\·\cos \mathrm{\θ}]\×(1/\mathrm{\ρ}+1/a)}.$

其中，ω'为环境对ω期望为0的随机扰动，模型参数α取为该扰动的幅值：α＝|ω'|。

步骤5：确定控制器输出V_o。计算Δt时间段内机器人需要的加速度：

求解V^*使得 $f_{\mathrm{\υ}}({\mathrm{\υ}}_0,V^{*})=\stackrel{\‾}{a}.$

若V^*≥V_max，则令V^*＝V_max，否则若V^*≤V_min，则令V^*＝V_min。

若g(V^*,T_o)≤0，则V_o＝V^*，否则由g(V_l,T_o)≤0解出V_l，令V_o＝V_l。

（VII）用获得的V_o,T_o驱动机器人。

Claims (1)

1.一种二自由度机器人运动控制方法，其特征在于包括以下步骤：

（I）获取二自由度机器人运动控制系统的动力学特性方程：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\υ}}=f_{\mathrm{\υ}}(\mathrm{\υ},V)\\ \stackrel{\·}{\mathrm{\ω}}=f_{\mathrm{\ω}}(\mathrm{\ω},T)\\ g(V,T)\≤0\\ V_{\min }\≤V\≤V_{\max }\\ T_{\min }\≤T\≤T_{\max }\end{array}\right.,$

其中：

υ——二自由度机器人的线速度，ω——二自由度机器人角速度；

——二自由度机器人的线加速度，

——二自由度机器人角加速度；

V——二自由度机器人线速度的控制输入，T——二自由度机器人角速度的控制输入；

V_min——V的下限；V_max——V的上限；

T_min——T的下限；T_max——T的上限；

f_υ——函数映射关系；f_ω——函数映射关系；

当V_min≤V≤V_max时，当T_min≤T≤T_max时，

V＝V_c,T＝T_c时，二自由度机器人处于无控制自然状态；

（II）以二自由度机器人中心为原点，二自由度机器人正方向为极轴建立极坐标系；

（III）计算目标点的极坐标θ和ρ；

（IV）转角控制；

步骤1：获取当前转角速度ω₀，计算完全靠惯性使二自由度机器人状态达到ω₀=0，所需转过的角度θ_{o_cri}：

则由

得ω(t,T)＝F_ω(t,ω₀,T)，由于无控制自然状态下，运动物体最终一定会静止；所以，令T＝T_c，则有：

ω(∞,T_c)＝F_ω(∞,ω₀,T_c)＝0；

${\mathrm{\θ}}_{o\_\mathrm{cri}}=\mathrm{sign}\left(\mathrm{\θ}\right)\·{\∫}_0^{\∞}\mathrm{\ω}(\mathrm{\τ},T_c)\·\mathrm{d\τ}$ 是存在的；

其中，F_ω表示由微分方程

及初值条件ω(0,T)＝ω₀，所确定ω关于t和T的函数关系；

步骤2：确定控制器输出T_o；

若|θ|＞θ_{o_cri}，则使机器人以最大力度转角，即T＝max{T·sign(θ)|T_min≤T≤T_max}；

若|θ|≤θ_{o_cri}，则对任意T∈{T|f_ω(ω₀,T)·ω₀＜0,T_min≤T≤T_max}，令ω(t,)T＝_ω(Fω_,0)t＝,，T解0方程F_ω(t,ω₀,T)＝0，得：t_o(T)且t_o(T)＜+∞，此时机器人转过的有利角度为：

${\mathrm{\θ}}_o\left(T\right)=\mathrm{sign}\left(\mathrm{\θ}\right)\·{\∫}_0^{t_o\left(T\right)}\mathrm{\ω}(\mathrm{\τ},T)\·\mathrm{d\τ};$

求解T*使得θ_o(T*)＝|θ|；

当T_min≤T^*≤T_max时，T_o＝T^*；否则，T_o＝min{f_ω(ω₀,T)·sign(ω₀)|T_min≤T≤T_max}；

（V）如果任务是原地转角，则线速度控制输出V_o＝V_c，跳转到步骤（VII）；否则转到步骤（VI）进行线运动控制；

（VI）线运动控制；

步骤1：变换运动方程；

目标点在极坐标系中的运动方程为：

令轨迹曲率半径

实半径 ${\mathrm{\ρ}}_2=\frac{\stackrel{\·}{\mathrm{\ρ}}}{\stackrel{\·}{\mathrm{\θ}}},$ 则运动方程为： $\frac{1}{{\mathrm{\ρ}}_1\·\sin \mathrm{\θ}}=\frac{1}{\mathrm{\ρ}}+\frac{1}{{\mathrm{\ρ}}_2\·\tan \mathrm{\θ}};$

步骤2：控制ρ₂，驱动ρ₁；令ρ₂＝a·cotθ，则上述步骤1得到的运动方程变为驱动方程： ${\mathrm{\ρ}}_1=\frac{1}{\sin \mathrm{\θ}\×(1/\mathrm{\ρ}+1/a)};$

其中，a是模型参数，a＝V_max；

步骤3：将时间向前推进Δt，计算Δt时间后二自由度机器人的角速度和需要的速度：

${\mathrm{\ω}}_{\mathrm{\Δt}}\≈{\mathrm{\ω}}_0+\stackrel{\·}{\mathrm{\ω}}\·\mathrm{\Δt}={\mathrm{\ω}}_0+f_{\mathrm{\ω}}({\mathrm{\ω}}_0,T_o)\·\mathrm{\Δt};$

令 ${\mathrm{\ρ}}_1=\frac{{\mathrm{\υ}}_{\mathrm{\Δt}}}{{\mathrm{\ω}}_{\mathrm{\Δt}}},$ 代入驱动方程得： ${\mathrm{\υ}}_{\mathrm{\Δt}}=\frac{{\mathrm{\ω}}_{\mathrm{\Δt}}}{\sin \mathrm{\θ}\×(1/\mathrm{\ρ}+1/a)};$

其中，Δt＝1f，f是控制器与机器人接收器的数据交换频率；

步骤4：模型修正：由于θ→0时，

会在1和-1上震荡，故模型修正为：

${\mathrm{\υ}}_{\mathrm{\Δt}}=\frac{(1-\mathrm{\α})\·{\mathrm{\ω}}_{\mathrm{\Δt}}+\mathrm{\α}\·\cos \mathrm{\θ}}{[(1-\mathrm{\α})\·{\mathrm{\ω}}_{\mathrm{\Δt}}+\mathrm{\α}\·\cos \mathrm{\θ}]\×(1/\mathrm{\ρ}+1/a)};$

其中，ω'为环境对ω期望为0的随机扰动，模型参数α取为该扰动的幅值：α＝|ω'|；步骤5：确定控制器输出V_o；计算Δt时间段内二自由度机器人需要的加速度：

$\stackrel{\‾}{a}\≈\frac{{\mathrm{\υ}}_{\mathrm{\Δt}}-{\mathrm{\υ}}_0}{\mathrm{\Δt}},$

求解V^*使得

若V^*≥V_max，则令V^*＝V_max，否则若V^*≤V_min，则令V^*＝V_min；

若g(V^*,T_o)≤0，则V_o＝V^*，否则由g(V_l,T_o)≤0解出V_l，令V_o＝V_l；

（VII）用获得的V_o,T_o驱动二自由度机器人。

Images