一种基于切线递推的机器人平滑避障运动规划方法与流程

本发明涉及一种机器人路径规划方法，尤其涉及一种基于切线递推的机器人平滑避障运动规划方法。

背景技术：

超冗余机器人能够有效弥补传统机器人自由度少、避障能力不足、灵活性差等缺点，是近年来国内外的研究热点之一，其应用范围已不再局限于传统的工业生产线。机器人在星际航空、水下探测、疾病检查、管道维护、抗险救灾等变化环境中的应用越来越广泛。随着机器人的广泛应用，变化的环境也对机器人的避障能力提出了要求。随着冗余度的增加，超冗余机器人结构越发复杂，运动学求解的计算量不断增大，求解也愈加困难，其运动平稳也难以保证。为了降低机器人运行过程的振动和冲击，延长机器人的使用寿命，应对不断变化的环境，超冗余机器人的平滑避障运动轨迹规划是有必要的。

机器人的避障运动规划是指在有障碍物的复杂环境中规划机器人的无碰撞路径。常用的无碰撞运动规划方法有：自由空间法、单元分解法、路径地图法、人工势场法以及基于采样的规划方法诸如概率地图和快速扩展随机树等。超冗余机器人的避障路径规划方法常用人工势场法和基于采样的规划法。在实际中通常会在满足约束的条件下进行路径规划，再利用各种优化方法研究时间最短、驱动力矩最小的规划方法，或者综合最优为指标进行优化。以上方法多在任务空间规划路径，得到运动平滑的轨迹计算较复杂。

技术实现要素：

本发明实施例所要解决的技术问题在于，提供一种基于切线递推的机器人平滑避障运动规划方法，使得超冗余机器人避障计算量小、轨迹平滑且各驱动关节速度、加速度和跃度连续无突变。

为了解决上述技术问题，本发明实施例提供了一种基于切线递推的机器人平滑避障运动规划方法，包括以下步骤：

s1：任务空间建立球型障碍物，根据起始点、目标点及障碍物信息确定无碰撞轨迹中的关键路径点；

s2：使用模态脊线法逆解求得所述关键路径点对应的一系列关节空间位姿；

s3：对机器人的每个关节空间状态进行插值，得到平滑避障运动路径。

进一步地，所述关键路径点的确定方式为切线递推的方式。

更进一步地，所述模态脊线法逆解通过以下表达式进行：

其中，s∈[0,1]，l表示曲线弧长，κ(δ)表示曲线在点δ处的切向量。

更进一步地，所述切向量κ(s)表示为：

κ(s)＝[cosα(s)sinβ(s),sinα(s)sinβ(s),cosβ(s)]^t

其中，α(s)表示κ(s)在xkokyk平面的投影与y轴正方向的夹角，β(s)表示κ(s)与xkokyk平面的夹角，函数α(s)和β(s)表达式为：

其中，ai是模态协同参数，fi(s)是模态子函数，n1是与α(s)相关的模态函数的个数，n2是模态函数的总个数；gi(s)是脊线的方向角辅助控制函数，biα和biβ则是方向角辅助控制参数。

更进一步地，所述模态函数至少为3个，所述脊线在任意点的方向角为：

脊线在s处的位置和方向角为：

v(s)＝[p(s)^tα(s)β(s)]^t，c＝[a1a2a3a4b2αb2β]^t，c表示脊线在s处的位置和方向的控制参数；

c想以下方法求得：

vd为给定末端位置和方向角的脊线，其中，λ是控制迭代函数收敛性的常数，jc的伪逆矩阵

更进一步地，还包括根据所述控制参数c，构造满足期望的空间脊线方程，利用二分法搜寻机器人的每一个并联机构的动平台中心位置在脊线上的拟合点。

更进一步地，步骤s3具体包括：

利用b样条函数反算插值的方法得到超冗余机器人第k个并联机构模块动平台的运动轨迹s关于时间t(0≤t≤1)的函数表达式：

sk,i(t)＝f0(t)vk,i-1+f1(t)vk,i+f2(t)vk,i+1+f3(t)vk,i+2

+f4(t)vk,i+3+f5(t)vk,i+4+f6(t)vk,i+5+f7(t)vk,i+6

其中，sk,i＝[sk,i,xsk,i,ysk,i,z]，f(t)为七次b样条函数的基函数，vk,i+j＝[vk,i+j,xvk,i+j,yvk,i+j,z](j＝-1,0,…,6；i＝1,2,…,m-1)为该段七次b样条曲线的控制点。

实施本发明实施例，具有如下有益效果：具有计算量小、避障轨迹平滑无冲击且各驱动关节速度、加速度和跃度连续无突变等优点，较适合于超冗余机器人。

附图说明

图1为避障算法流程图；

图2为递推切线示意图；

图3为超冗余仿生象鼻机器人的避障运动规划图；

图4为超冗余仿生象鼻机器人各驱动关节位置曲线；

图5为超冗余仿生象鼻机器人各驱动关节速度曲线；

图6为超冗余仿生象鼻机器人各驱动关节加速度曲线；

图7为超冗余仿生象鼻机器人各驱动关节跃度曲线。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述。

本发明实施例的一种基于切线递推的机器人平滑避障运动规划方法，其思想是在安全距离一定的条件下用几何方法得到无撞路径点，计算量相对较小，再用脊线模态法得到逆解之后通过七次b样条插值得到平滑避障运动路径，各驱动关节速度、加速度和跃度连续无突变，运动特性好，理论上实现无冲击。

为使本发明的目的、技术方案及优点更加清楚明白，下面结合案例及附图对本发明作进一步详细描述。

本案例中机器人有六个单元体，它是由6个具有三转动一移动自由度的3ups－ps并联模块串联而成的超冗余机器人，障碍物为半径已知的球。本案例的任务为机器人从起始点p0避开障碍物or运动到目标点pf，机器人位于起始点p0和目标点pf时速度、加速度和跃度均为0。具体实现步骤如下：

1、在任务空间中建立球型障碍物，根据起始点p0、目标点pf及障碍物信息or等确定是否避障，当需要避障时求得无撞轨迹中的一系列关键路径点p0，p1，…，pn+1，pm，pf等，避障算法流程图如附图1所示。

当需要避障时，采用切线递推的方式求解关键路径点。过始末位置点做圆or切线，连接圆心与两切点，交圆or+l于点p1、pm，依次过p0、p1…找出一系列圆or的切线与圆or+l的交点p2，p3，…，pn+2，直到最后取p0，p1，…，pn+1，pm，pf为无碰路径上的路径点，如附图2所示。

2、运用脊线模态法求得超冗余机器人的位置逆解，得到与无碰路径点对应的关节空间位姿。

脊线模态法基于微分几何的思想，用参数化的方法表达曲线上的任意点，并用参数化生成的空间脊线来描述超冗余机器人的整体几何构型。空间任意曲线的积分表达式为：

其中，s∈[0,1]，l表示曲线弧长，κ(δ)表示曲线在点δ处的切向量。曲线切向量κ(s)可以表示为：

κ(s)＝[cosα(s)sinβ(s),sinα(s)sinβ(s),cosβ(s)]^t

其中，α(s)表示κ(s)在xkokyk平面的投影与y轴正方向的夹角，β(s)表示κ(s)与xkokyk平面的夹角。利用模态函数的线性组合来表示函数α(s)和β(s)：

其中，ai是模态协同参数，fi(s)是模态子函数，n1是与α(s)相关的模态函数的个数，n2是模态函数的总个数；gi(s)是脊线的方向角辅助控制函数，biα和biβ则是方向角辅助控制参数。由于空间脊线的末端位置具有三维约束，因此，至少需要3个线性无关的模态函数来限制脊线的总体形状。根据选用的模态函数，脊线在任意点的方向角为：

经过参数化处理后，脊线的逆运动学转化为求解ai、biα和biβ的问题。用v(s)表示脊线在s处的位置和方向角，用c表示脊线在s处的位置和方向的控制参数，则v(s)＝[p(s)^tα(s)β(s)]^t，c＝[a1a2a3a4b2αb2β]^t。对于给定末端位置和方向角的脊线vd，其模态控制参数c可由如下迭代方程求得：

其中，λ是控制迭代函数收敛性的常数，jc的伪逆矩阵

迭代出模态控制参数后，构造满足期望的空间脊线方程，利用二分法搜寻机器人的每一个并联机构的动平台中心位置在脊线上的拟合点。对于第k个并联机构的第i条支链，需满足约束条件：

lb,ki≤qki≤lr,ki+lb,ki,(i＝0,1,2,3；k＝1,2,...n)

其中，lb,ki和lr,ki分别表示支链套筒和推杆的长度。用向量lc＝[q10q20…qn0]^t表示中间运动支链杆长，则

其中，h＝[h1h2…hn]^t，hk(k＝1,2,…n,hk∈[0,1])是控制中间运动支链杆长的参数。当利用拟合后的并联机构的位姿进行位置逆解运算得到其他各支链的长度，且不满足约束条件时，可通过调整参数h进行重新搜索。由于基座并联机构模块是垂直安装在固定架上的，其动平台位置的拟合点无法落在脊线上。设p0＝[000]^t，则p1＝[00l1]^t。第k个并联机构动平台坐标系相对于基坐标系的姿态变换矩阵可用欧拉角φi和表示：

设第k个并联机构的中间支链向量的单位矢量wk＝[wkxwkywkz]^t，由wk和旋转矩阵的第三列相等，即

综上，第k个并联机构中动平台相对于静平台的位姿变换矩阵可描述为：

根据变换矩阵求得每个位姿下各并联机构动平台的中心位置，即一系列关节空间位姿。

3、对一系列关节空间位姿进行七次b样条插值，得到机器人平滑的避障运动路径。

利用b样条函数反算插值的方法得到超冗余机器人第k个并联机构模块动平台的运动轨迹s关于时间t(0≤t≤1)的函数表达式。

假设超冗余机器人第k个并联机构动平台运动轨迹的函数表达式为：

sk,i(t)＝f0(t)vk,i-1+f1(t)vk,i+f2(t)vk,i+1+f3(t)vk,i+2

+f4(t)vk,i+3+f5(t)vk,i+4+f6(t)vk,i+5+f7(t)vk,i+6

其中，sk,i＝[sk,i,xsk,i,ysk,i,z]，f(t)为七次b样条函数的基函数，vk,i+j＝[vk,i+j,xvk,i+j,yvk,i+j,z](j＝-1,0,…,6；i＝1,2,…,m-1)为该段七次b样条曲线的控制点。

由于关节运动具有连续性，因此相邻两段b样条曲线在连接处相等，即

sk,i(1)＝sk,i+1(0)

由机器人动平台运动轨迹的导数连续性可知，在两段轨迹的连接处七次b样条函数需同时满足一至六阶导数连续，即：

根据b样条基函数的性质∑fi(t)＝1，得：

f0(t)+f1(t)+f2(t)+f3(t)+f4(t)+f5(t)+f6(t)+f7(t)＝1

假设b样条基函数fi(t)的形式为：

fi(t)＝ait⁷+bit⁶+cit⁵+dit⁴+eit³+git²+hit+qi

根据上述约束可求得七次b样条函数的基函数，则机器人第k个并联机构动平台的运动轨迹为：

对时间t求导可得各运动支链的速度、加速度和跃度。

假设机器人各并联机构动平台的运动轨迹由m-1段七次b样条曲线组成，第k个并联机构动平台的中心位置为pk,i(i＝1,2,3,…,m)，由轨迹连续性可得：

以起始点和终止点的速度、加速度、跃度等于零为约束条件，即整理得

mv＝p

其中，向量矩阵v为七次b样条曲线的一系列控制点，

v＝[vk,0vk,1…vk,m+5]^t，向量矩阵p为考虑边界约束，在无碰路径点处超冗余机器人第k个并联机构动平台的中心位置，

m为系数矩阵，

解该线性方程组可确定唯一解vk,0，vk,1，…，vk,m+4，vk,m+5，即可得到所求七次b样条曲线函数表达式，最终得到由m-1个七次b样条函数分段表示的机器人平滑避障运动轨迹。避障运动规划图如附图3，各关节位置、速度、加速度、跃度曲线如附图4－7。

以上所揭露的仅为本发明一种较佳实施例而已，当然不能以此来限定本发明之权利范围，因此依本发明权利要求所作的等同变化，仍属本发明所涵盖的范围。