CN106313047A - 一种基于Bezier样条的机器人实时拐角过渡方法 - Google Patents

Abstract

本发明属于工业机器人轨迹规划领域，并公开了一种基于Bezier样条的机器人实时拐角过渡方法，包括以下步骤：记录关节空间起始点,过渡点的坐标，笛卡尔空间目标点，过渡点的坐标，笛卡尔空间的过渡半径R及最大误差约束；计算笛卡尔空间过渡曲线起点和终点的坐标和关节空间过渡曲线起点的坐标；计算笛卡尔空间过渡曲线起点和终点速度方向单位矢量；计算笛卡尔卡尔空间过渡曲线中间控制点的坐标，并求解Bezier样条曲线的构造函数；最后进行速度规划和插补。通过本发明，同时实现了在点到点运动指令和线性运动运动指令之间过渡时过渡半径和最大过渡误差约束，高效率，高质量的完成点到点指令到线性运动指令之间的拐角过渡。

Description

一种基于Bezier样条的机器人实时拐角过渡方法

技术领域

本发明属于工业机器人轨迹规划领域，更具体地，涉及一种基于Bezier样条的机器人实时拐角过渡方法。

背景技术

工业机器人的轨迹规划方式根据是否通过指定路径点主要分为两种方式。通过指定路径点的轨迹规划由操作人员指定一系列路径点，机器人按给定的运动指令通过每一个路径点，并且在每一个路径点附近区域完成减速，停止再加速的过程，该方法可以实现精确到达指令要求的位置，但是在经过路径点时会有停顿，因此当路径点较多时会耗费较多时间，降低了整体效率。不通过指定路径点的轨迹规划即拐角过渡，由操作人员指定过渡路径点以及过渡半径，机器人按照过渡半径计算过渡段，其在过渡区域内采用曲线过渡，位置连续，且过渡曲线不经过过渡路径点，因此不需要降速为0，从而整体提高了作业效率。

根据规划过渡曲线的空间不同，拐角过渡方法又可以分为关节空间下的拐角过渡和笛卡尔空间的拐角过渡。如果拐角前后均为PTP(点到点)运动指令，则表示机器人在关节空间进行轨迹规划，此时的拐角过渡可以根据关节空间下的过渡半径约束来进行关节空间过渡曲线的规划。如果拐角前后均为线性运动指令，则表示机器人在笛卡尔空间进行轨迹规划，此时的拐角过渡可以根据笛卡尔空间下的过渡半径约束进行笛卡尔空间过渡曲线的规划。如果拐角前后分别为PTP(点到点)和线性运动指令，则规划过渡曲线的空间与前后运动指令所对应的轨迹规划空间不相同。因此存在着无法满足不同空间的过渡半径约束的问题。对于相同运动指令之间的过渡曲线的最大误差可以通过控制过渡曲线的参数实现，但是不同运动指令的过渡曲线的最大误差约束却不能用相同的方法进行计算求解。目前只能根据生成的过渡曲线计算距离过渡路径点的最大误差，而不能采用最大误差计算过渡曲线参数从而控制过渡曲线的生成。因为这些问题，目前已有的工业机器人PTP(点到点)和线性运动指令之间的拐角过渡不能实现过渡半径和最大误差的同时约束，增大了拐角过渡的误差。

发明内容

针对现有技术的以上缺陷和改进需求，本发明提供了一种基于Bezier样条曲线的机器人实时拐角过渡方法，其目的在于提供了一种能在PTP(点到点)运动指令和线性运动指令之间进行拐角过渡轨迹规划方法。由此解决了现有工业机器人在PTP(点到点)运动指令和线性运动运动指令之间过渡时无法满足过渡半径和最大过渡误差同时约束的技术问题。

为实现上述目的，按照本发明，提供了一种基于Bezier样条的机器人实时拐角过渡方法，其特征在于，该方法包括下列步骤：

(a)记录关节空间起始点的坐标q_start及其运动速度大小关节空间过渡点的坐标q_via，笛卡尔空间目标点的坐标P_end，笛卡尔空间的过渡半径R以及最大误差约束ε。笛卡尔空间过渡点的坐标为P_via，将所述P_via用所述q_via表示；

(b)通过笛卡尔空间过渡曲线起点的坐标满足的所述过渡半径R的约束条件，计算出所述笛卡尔空间过渡曲线起点的坐标P₀，并通过弦截法计算出关节空间过渡曲线起点的坐标q₀；

(c)通过所述P_via、所述P_end和所述笛卡尔空间的过渡半径R的约束条件，计算出笛卡尔空间过渡终点的坐标P₁；

(d)关节空间匀速运动，所述关节空间起始点的运动速度的速度方向等于所述关节空间过渡曲线起点的速度方向，采用雅克比矩阵转换将所述关节空间过渡曲线段起点的速度大小映射到所述笛卡尔空间过渡曲线起点速度，计算该速度方向的单位矢量V₀，利用所述笛卡尔空间目标点的坐标P_end和所述笛卡尔空间过渡点的坐标P_via，计算所述笛卡尔空间过渡曲线终点速度方向的单位矢量V₁；

(e)预设笛卡尔空间过渡曲线中间控制点的坐标P₂和P₃与所述V₀和所述V₁的关系式(一)，结合所述最大误差约束的条件ε，求解出所述P₂和所述P₃，同时计算出三次Bezier样条曲线的构造函数；

(f)采用S型加减速对前后段以及过渡曲线进行速度规划以及插补。

作为进一步优选地，在步骤(a)中，所述用q_via表示P_via优选采用运动学正解方程，其关系式(二)如下，其中，q_via＝[θ₁ θ₂ θ₃ θ₄ θ₅ θ₆]，θ₁，θ₂，θ₃，θ₄，θ₅和θ₆分别代表旋转关节1～6的关节变量，c₁＝cosθ₁、s₁＝sinθ₁、c₂₃＝cos(θ₂+θ₃)、s₂₃＝sin(θ₂+θ₃)，其余依此类推，a₂是连杆2的连杆长度参数，a₃是连杆3的连杆长度参数，d₃是连杆1和连杆2之间的连杆偏置参数，d₄是连杆3和连杆4之间的连杆偏置参数，

$P_{via}=\left[\begin{array}{c}{c}_1(a_2{c}_2+a_3{c}_{23}-d_4{s}_{23})-d_2{s}_1\\ s_1(a_2{c}_2+a_3{c}_{23}-d_4{s}_{23})-d_2{c}_1\\ -a_3{s}_{23}-a_2{s}_2-d_4{c}_{23}\end{array}\right].$

作为进一步优选地，在步骤(b)中，所述过渡半径的约束条件优选采用以下关系式(三)：

|P₀-P_via|-R＝0。

作为进一步优选地，在步骤(c)中，所述过渡终点的笛卡尔坐标P₁优选采用以下关系式(四)：

$P_1=P_{via}-\frac{P_{via}-P_{end}}{\left|P_{via}-P_{end}\right|}R.$

作为进一步优选地，在步骤(d)中，所述雅克比矩阵优选采用以下表达式(五)，

J(q)＝[J₁(q) J₂(q) J₃(q) J₄(q) J₅(q) J₆(q)]

$J_1\left(q\right)=\left[\begin{array}{c}-s_1(a_1+a_2{c}_2+a_3{c}_{23}+d_4{s}_{23})\\ c_1(a_1+a_2{c}_2+a_3{c}_{23}+d_4{s}_{23})\\ 0\\ 0\\ 0\\ 1\end{array}\right]$

$J_2\left(q\right)=\left[\begin{array}{c}{c}_1(d_4{c}_{23}-a_3{s}_{23}-a_2{s}_2)\\ s_1(d_4{c}_{23}-a_3{s}_{23}-a_2{s}_2))\\ d_4{s}_{23}+a_3{c}_{23}+a_2{c}_2\\ s_1\\ -c_1\\ 0\end{array}\right]$

$J_3\left(q\right)=\left[\begin{array}{c}-c_1(a_3{s}_{23}-d_4{c}_{23})\\ -s_1(a_3{s}_{23}-d_4{c}_{23})\\ a_3{c}_{23}+d_4{s}_{23}\\ s_1\\ -c_1\\ 0\end{array}\right]$

$J_4\left(q\right)=\left[\begin{array}{c}0\\ 0\\ 0\\ c_1{s}_{23}\\ s_1{s}_{23}\\ -c_{23}\end{array}\right]$

$J_5\left(q\right)=\left[\begin{array}{c}0\\ 0\\ 0\\ s_1{c}_4-c_1{s}_4{c}_{23}\\ -c_1{c}_4-s_1{s}_4{c}_{23}\\ -s_4{s}_{23}\end{array}\right]$

$J_6\left(q\right)=\left[\begin{array}{c}0\\ 0\\ 0\\ c_1{c}_5{s}_{23}+s_5(s_1{s}_4+c_1{c}_4{c}_{23})\\ c_1{s}_5{s}_{23}+s_5(-c_1{s}_4+s_1{c}_4{c}_{23})\\ -c_5{c}_{23}+c_4{s}_5{s}_{23}\end{array}\right];$

所述V₀优选采用以下表达式(六)：

$V_0=\frac{J\left(q_0\right)\stackrel{.}{q_0}}{\left|J\left(q_0\right)\stackrel{.}{q_0}\right|};$

所述V₁优选采用以下表达式(七)：

$V_1=\frac{P_{via}-P_{end}}{\left|P_{via}-P_{end}\right|}.$

作为进一步优选地，在步骤(e)中，所述关系式(一)优选采用以下表达式：

$\begin{array}{c}{P}_2-P_0=\frac{\mathrm{\λ}}{3}{V}_0\\ P_1-P_3=\frac{\mathrm{\λ}}{3}{V}_1\end{array};$

所述三次Bezier样条曲线的构造函数优选采用以下表达式(八)，其中t是0～1之间的任意数：

S(t)＝(1-t)³P₀+3t(1-t)²P₂+3t²(1-t)P₃+t³P₁；

所述最大误差约束的条件优选采用以下表达式(九)，并由所述表达式(九)获得一元二次方程，其中S(t₀)中t₀取0～1之间的任意数：

|S(t₀)-P_via|＝ε；

通过所述一元二次方程计算出λ优选采用以下表达式(十)，且λ取最小正数值

$\mathrm{\λ}=\frac{2(M_1\·M_2)\±\sqrt{\mathrm{\Δ}}}{2M_1};$

其中

Δ＝(2(M₁·M₂))²-4|M₁|²||M₂|²-ε²|

$\left\{\begin{array}{cc}{M}_1=\frac{V_0}{\left|V_0\right|}{t}_0{(1-t_0)}^2-\frac{V_1}{\left|V_1\right|}{t_0}^2(1-t_0)& M_1\∈R^3\\ M_2=(P_0{\left(1-t_0\right)}^3+P_1{t_0}^3+3P_0{t}_0{\left(1-t_0\right)}^2+3P_1{t_0}^2(1-t_0\left)\right)-P_{via}& M_2\∈R^3\end{array}\right.$

总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

1、本发明通过采用拐角过渡的方法，在指定的过渡区域内采用曲线过渡实现位置连续，在笛卡尔空间和关节空间，过渡曲线起点的速度都不为0，避免了经过指定路径点时的停顿，解决了当路径点较多时会耗费时间长的问题，整体提高了作业效率；

2、本发明通过采用笛卡尔空间过渡曲线起点满足过渡半径的约束条件，算出了笛卡尔空间过渡曲线起点的坐标P₀，并通过弦截法算出关节空间过渡曲线起点的坐标q₀，实现了不同空间的过渡半径约束条件；

3、本发明通过采用雅克比矩阵转换将所述关节空间过渡曲线段起点的速度大小映射到所述笛卡尔空间过渡曲线起点速度方向的单位矢量V₀，然后利用最大误差约束的条件求解Bezier样条曲线的构造函数，实现了不同运动指令(笛卡尔空间和关节空间指令)之间的最大误差约束的条件，同时实现了过渡半径和最大误差的同时约束，减少了拐角过渡的误差；

4、本发明通过采用了运动学正解方程、弦截法、雅克比矩阵和一元二次方程求解等方法实现了关节空间到笛卡尔空间的拐角过渡，整体方案简单，高质量地完成了整个拐角过渡过程，并且整个过程便于操作和控制。

附图说明

图1是机器人点到点运动指令和线性运动指令之间拐角过渡方法的流程图；

图2是按照本发明的优选实施例所构建的机器人结构图；

图3是按照本发明的优选实施例所构建的进行速度规划和插补后的拐角过渡效果图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

图1是机器人点到点运动指令和线性运动指令之间拐角过渡方法的流程图，如图1所示，该拐角过渡方法具体包括以下步骤：

S1：记录PTP(点到点)运动指令起始点的关节坐标q_start，线性运动指令目标点的笛卡尔空间坐标值P_end，给定笛卡尔空间的过渡半径R以及最大误差约束ε。路径过渡点的关节空间坐标为q_via，笛卡尔空间坐标P_via。由运动学正解方程可以将q_via表示为P_via，图2是按照本发明的优选实施例所构建的机器人结构图，本实例中采用基于图2所示的PUMA机器人结构，相应的运动学正解方程如下：

$P_{via}=\left[\begin{array}{c}{c}_1(a_2{c}_2+a_3{c}_{23}-d_4{s}_{23})-d_2{s}_1\\ s_1(a_2{c}_2+a_3{c}_{23}-d_4{s}_{23})-d_2{c}_1\\ -a_3{s}_{23}-a_2{s}_2-d_4{c}_{23}\end{array}\right]$

式中，c₁＝cosθ₁、s₁＝sinθ₁、c₂₃＝cos(θ₂+θ₃)、s₂₃＝sin(θ₂+θ₃)，其余依此类推。q_via＝[θ₁ θ₂ θ₃ θ₄ θ₅ θ₆]。a₂、a₃、d₂、d₄分别为机器人连杆参数。

S2：计算PTP(点到点)运动指令下的过渡曲线段起点P₀。由于约束在笛卡尔空间进行描述，过渡曲线段起点的笛卡尔空间坐标需满足如下约束条件：

|P₀-P_via|-R＝0

由于PTP(点到点)运动指令下机器人在关节空间进行轨迹规划，所以需要利用机器人运动学正解将关节空间的坐标值映射到笛卡尔空间。PTP(点到点)运动指令下机器人运动轨迹在映射到笛卡尔空间后为不规则曲线，因此不能找到距离约束的解析解。采用弦截法可以计算出任意精度的数值解。从而得出过渡曲线段起点的关节空间坐标和笛卡尔空间坐标。

S3：在线性运动指令下的过渡终点P₁可以根据给定的路径过渡点P_via和线性运动指令的目标点P_end以及过渡半径R求出：

$P_1=P_{via}-\frac{P_{via}-P_{end}}{\left|P_{via}-P_{end}\right|}R$

S4：V₀、V₁分别为过渡曲线段起点和过渡曲线段终点的笛卡尔空间速度方向的单位矢量，在计算V₀时需要将关节空间运动到该点时的关节速度映射到笛卡尔空间，采用雅克比矩阵进行转换：

$V_0=\frac{J\left(q_0\right)\stackrel{.}{q_0}}{\left|J\left(q_0\right)\stackrel{.}{q_0}\right|}$

$V_1=\frac{P_{via}-P_{end}}{\left|P_{via}-P_{end}\right|}$

其中q₀，J(q₀)分别为过渡起点的关节坐标，关节速度和该工业机器人的雅克比矩阵转换式。对于实例中所用机器人，其雅克比矩阵为：

J(q)＝[J₁(q) J₂(q) J₃(q) J₄(q) J₅(q) J₆(q)]

$J_1\left(q\right)=\left[\begin{array}{c}-s_1(a_1+a_2{c}_2+a_3{c}_{23}+d_4{s}_{23})\\ c_1(a_1+a_2{c}_2+a_3{c}_{23}+d_4{s}_{23})\\ 0\\ 0\\ 0\\ 1\end{array}\right]$

$J_2\left(q\right)=\left[\begin{array}{c}{c}_1(d_4{c}_{23}-a_3{s}_{23}-a_2{s}_2)\\ s_1(d_4{c}_{23}-a_3{s}_{23}-a_2{s}_2))\\ d_4{s}_{23}+a_3{c}_{23}+a_2{c}_2\\ s_1\\ -c_1\\ 0\end{array}\right]$

$J_3\left(q\right)=\left[\begin{array}{c}-c_1(a_3{s}_{23}-d_4{c}_{23})\\ -s_1(a_3{s}_{23}-d_4{c}_{23})\\ a_3{c}_{23}+d_4{s}_{23}\\ s_1\\ -c_1\\ 0\end{array}\right]$

$J_4\left(q\right)=\left[\begin{array}{c}0\\ 0\\ 0\\ c_1{s}_{23}\\ s_1{s}_{23}\\ -c_{23}\end{array}\right]$

$J_5\left(q\right)=\left[\begin{array}{c}0\\ 0\\ 0\\ s_1{c}_4-c_1{s}_4{c}_{23}\\ -c_1{c}_4-s_1{s}_4{c}_{23}\\ -s_4{s}_{23}\end{array}\right]$

$J_6\left(q\right)=\left[\begin{array}{c}0\\ 0\\ 0\\ c_1{c}_5{s}_{23}+s_5(s_1{s}_4+c_1{c}_4{c}_{23})\\ c_1{s}_5{s}_{23}+s_5(-c_1{s}_4+s_1{c}_4{c}_{23})\\ -c_5{c}_{23}+c_4{s}_5{s}_{23}\end{array}\right]$

S5：计算Bezier过渡曲线参数方程。P₂、P₃为样条曲线的中间控制点坐标。规定过渡曲线起点与终点的速度方向分别与按照原始指令进行运动规划时经过该点时的速度方向V₀、V₁相同。假设存在如下关系：

$P_2-P_0=\frac{\mathrm{\λ}}{3}{V}_0$

$P_1-P_3=\frac{\mathrm{\λ}}{3}{V}_1$

λ为比例系数。

三次Bezier样条曲线的构造函数为

S(t)＝(1-t)³P₀+3t(1-t)²P₂+3t²(1-t)P₃+t³P₁ t∈[0,1]

其中，t为样条曲线的控制参数。P₀、P₁分别为样条曲线的首末控制点坐标。

根据约束条件，设置当t为定值t₀，且t₀＝0.5时，过渡曲线段与路径过渡点之间的距离为ε，则有：

|S(t₀)-P_via|＝ε

由此可列出一元二次方程求解λ。先对方程判别式进行分析，防止出现无解情况。

Δ＝(2(M₁·M₂))²-4|M₁|²||M₂|²-ε²|

其中，M₁,M₂均为三维实数向量，

$\left\{\begin{array}{cc}{M}_1=\frac{V_0}{\left|V_0\right|}{t}_0{(1-t_0)}^2-\frac{V_1}{\left|V_1\right|}{t_0}^2(1-t_0)& M_1\∈R^3\\ M_2=(P_0{\left(1-t_0\right)}^3+P_1{t_0}^3+3P_0{t}_0{\left(1-t_0\right)}^2+3P_1{t_0}^2(1-t_0\left)\right)-P_{via}& M_2\∈R^3\end{array}\right.$

当Δ<0说明当t＝t₀时无法找到满足误差公式的解。因此需要替换不同的参数值直到Δ≥0，此时通过下式计算λ：

$\mathrm{\&lambda;}=\frac{2(M_1\&CenterDot;M_2)\&PlusMinus;\sqrt{\mathrm{\&Delta;}}}{2M_1}$

由于一元二次方程存在两个解，需取较小的正数值作为最终解，保证速度不反向以及过渡曲线位于原轨迹路径的内侧。

根据上述公式可以求解出P₂、P₃，从而表达出Bezier过渡曲线的参数方程。

S6：采用S型加减速对前后段以及过渡曲线进行速度规划以及插补。

本发明的一个优选实施例如下：

设PTP(点到点)运动指令起始点的关节空间坐标为q_start＝[8.04 82.3 -16.7 496.6 8.04]，单位为度。线性运动指令目标点的位置坐标为P_end＝[920 130 300]，单位为毫米。过渡路径点的关节空间坐标为q_via＝[7.98 82.1 -17 4 94.8 7.26]，经过运动学正解计算得出其笛卡尔空间位置坐标为P_via＝[790 0 300]。过渡半径为150mm，最大误差为60mm。

根据以上约束，计算得出PTP(点到点)运动指令下的过渡曲线段起点的笛卡尔空间坐标为P₀＝[939.3 17.1 298.9]，线性运动指令下的过渡曲线段终点的笛卡尔坐标为P₁＝[896.3 106.3 300]。比例系数λ＝451。P₂＝[814.2 8.1 216]，P₃＝[803 98 300]。

获得过渡段参数曲线后，进行速度规划和插补，图3是按照本发明的优选实施例所构建的进行速度规划和插补后的拐角过渡效果图，如图3所示的原始轨迹和插补后的过渡曲线，可以看出用Bezier过渡曲线代替原始的轨迹可以实现圆滑的拐角过渡。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种基于Bezier样条的机器人实时拐角过渡方法，其特征在于，该方法包括下列步骤：

(a)记录关节空间起始点的坐标q_start及其运动速度大小关节空间过渡点的坐标q_via，笛卡尔空间目标点的坐标P_end，笛卡尔空间的过渡半径R以及最大误差约束ε，笛卡尔空间过渡点的坐标为P_via，通过所述q_via和运动学正解方程计算出所述P_via；

(b)通过笛卡尔空间过渡曲线起点的坐标满足的所述过渡半径R的约束条件，并通过机器人运动学正解和弦截法计算出关节空间过渡曲线起点的坐标q₀和笛卡尔空间过渡曲线起点的坐标P₀；

(c)通过所述P_via、所述P_end和所述笛卡尔空间的过渡半径R的约束条件，计算出笛卡尔空间过渡终点的坐标P₁；

(d)所述关节空间起始点的运动速度的速度方向等于所述关节空间过渡曲线起点的速度方向，采用雅克比矩阵转换将所述关节空间过渡曲线段起点的速度大小映射到所述笛卡尔空间过渡曲线起点速度，计算该速度方向的单位矢量V₀，利用所述笛卡尔空间目标点的坐标P_end和所述笛卡尔空间过渡点的坐标P_via，计算所述笛卡尔空间过渡曲线终点速度方向的单位矢量V₁；

(e)预设笛卡尔空间过渡曲线中间控制点的坐标P₂和P₃与所述V₀和所述V₁的关系式(一)，结合所述最大误差约束的条件ε，求解出所述P₂和所述P₃，同时计算出三次Bezier样条曲线的构造函数；

(f)采用S型加减速对前后段以及过渡曲线进行速度规划以及插补。

2.如权利要求1所述的方法，其特征在于，在步骤(a)中，所述用q_via表示P_via优选采用运动学正解方程，其关系式(二)如下，其中，q_via＝[θ₁ θ₂ θ₃ θ₄ θ₅ θ₆]，θ₁，θ₂，θ₃，θ₄，θ₅和θ₆分别代表旋转关节1～6的关节变量，c₁＝cosθ₁、s₁＝sinθ₁、c₂₃＝cos(θ₂+θ₃)、s₂₃＝sin(θ₂+θ₃)，其余依此类推，a₂是连杆2的连杆长度参数，a₃是连杆3的连杆长度参数，d₃是连杆1和连杆2之间的连杆偏置参数，d₄是连杆3和连杆4之间的连杆偏置参数，

$P_{via}=\left[\begin{array}{c}{c}_1(a_2{c}_2+a_3{c}_{23}-d_4{s}_{23})-d_2{s}_1\\ s_1(a_2{c}_2+a_3{c}_{23}-d_4{s}_{23})-d_2{c}_1\\ -a_3{s}_{23}-a_2{s}_2-d_4{c}_{23}\end{array}\right].$

3.如权利要求1或2所述的方法，其特征在于，在步骤(b)中，所述过渡半径的约束条件优选采用以下关系式(三)：

|P₀-P_via|-R＝0。

4.如权利要求1-3任一项所述的方法，其特征在于，在步骤(c)中，所述过渡终点的笛卡尔坐标P₁优选采用以下关系式(四)：

$P_1=P_{via}-\frac{P_{via}-P_{end}}{|P_{via}-P_{end}|}R.$

5.如权利要求1-4任一项所述的方法，其特征在于，在步骤(d)中，所述雅克比矩阵优选采用以下表达式(五)，

J(q)＝[J₁(q) J₂(q) J₃(q) J₄(q) J₅(q) J₆(q)]

$J_1\left(q\right)=\left[\begin{array}{c}-s_1(a_1+a_2{c}_2+a_3{c}_{23}+d_4{s}_{23})\\ c_1(a_1+a_2{c}_2+a_3{c}_{23}+d_4{s}_{23})\\ 0\\ 0\\ 0\\ 1\end{array}\right]$

$J_2\left(q\right)=\left[\begin{array}{c}{c}_1(d_4{c}_{23}-a_3{s}_{23}-a_2{s}_2)\\ s_1(d_4{c}_{23}-a_3{s}_{23}-a_2{s}_2))\\ d_4{s}_{23}+a_3{c}_{23}+a_2{c}_2\\ s_1\\ -c_1\\ 0\end{array}\right]$

$J_3\left(q\right)=\left[\begin{array}{c}-c_1(a_3{s}_{23}-d_4{c}_{23})\\ -s_1(a_3{s}_{23}-d_4{c}_{23})\\ a_3{c}_{23}+d_4{s}_{23}\\ s_1\\ -c_1\\ 0\end{array}\right]$

$J_4\left(q\right)=\left[\begin{array}{c}0\\ 0\\ 0\\ c_1{s}_{23}\\ s_1{s}_{23}\\ -c_{23}\end{array}\right]$

$J_5\left(q\right)=\left[\begin{array}{c}0\\ 0\\ 0\\ s_1{c}_4-c_1{s}_4{c}_{23}\\ -c_1{c}_4-s_1{s}_4{c}_{23}\\ -s_4{s}_{23}\end{array}\right]$

$J_6\left(q\right)=\left[\begin{array}{c}0\\ 0\\ 0\\ c_1{c}_5{s}_{23}+s_5(s_1{s}_4+c_1{c}_4{c}_{23})\\ c_1{s}_5{s}_{23}+s_5(-c_1{s}_4+s_1{c}_4{c}_{23})\\ -c_5{c}_{23}+c_4{s}_5{s}_{23}\end{array}\right];$

所述V₀优选采用以下表达式(六)：

$V_0=\frac{J\left(q_0\right)\stackrel{\&CenterDot;}{q_0}}{\left|J\left(q_0\right)\stackrel{\&CenterDot;}{q_0}\right|};$

所述V₁优选采用以下表达式(七)：

$V_1=\frac{P_{via}-P_{end}}{|P_{via}-P_{end}|}.$

6.如权利要求1-5任一项所述的方法，其特征在于，在步骤(e)中，所述关系式(一)优选采用以下表达式：

$\begin{array}{c}{P}_2-P_0=\frac{\mathrm{\&lambda;}}{3}{V}_0\\ P_1-P_3=\frac{\mathrm{\&lambda;}}{3}{V}_1\end{array};$

所述三次Bezier样条曲线的构造函数优选采用以下表达式(八)，其中t是0～1之间的任意数：

S(t)＝(1-t)³P₀+3t(1-t)²P₂+3t²(1-t)P₃+t³P₁；

所述最大误差约束的条件优选采用以下表达式(九)，并由所述表达式(九)获得一元二次方程，其中S(t₀)中t₀取0～1之间的任意数：

|S(t₀)-P_via|＝ε；

通过所述一元二次方程计算出λ优选采用以下表达式(十)，且λ取最小正数值

$\mathrm{\&lambda;}=\frac{2(M_1\&CenterDot;M_2)\&PlusMinus;\sqrt{\mathrm{\&Delta;}}}{2M_1};$

其中

△＝(2(M₁·M₂))²-4|M₁|²||M₂|²-ε²|

$\left\{\begin{array}{cc}{M}_1=\frac{V_0}{\left|V_0\right|}{t}_0{(1-t_0)}^2-\frac{V_1}{\left|V_1\right|}{t_0}^2(1-t_0)& M_1\&Element;R^3\\ M_2=(P_0{\left(1-t_0\right)}^3+P_1{t_0}^3+3P_0{t}_0{\left(1-t_0\right)}^2+3P_1{t_0}^2(1-t_0\left)\right)-P_{via}& M_2\&Element;R^3\end{array}\right.$