[0061] 条件4:其他情况跳转步骤七;
[0062] 步骤五:在降维稀疏的关节空间中使用单关节空间轨迹规划库SP_LIB库执行启发式搜索,获得当前库中最优运动基元作为规划结果。
[0063] 根据当前机械臂关节状态参数,采用简化机械臂运动学模型T1,在降维离散C-空间中使用启发式搜索算法,遍历步骤二中定义的单关节空间轨迹规划库SP_LIB库中的运动
基元进行启发搜索(只遍历21,22,23前三关节运动基元),即调用步骤三中定义的笛卡尔空
间启发函数hd(x)和笛卡尔空间距离检测函数DO(q),综合启发函数和碰撞函数结果对运动
基元库成员择优作为启发结果,更新机械臂状态,跳转步骤四。
[0064] 步骤六:在非降维稀疏关节空间中使用单关节空间轨迹规划库SP_LIB库或笛卡尔空间轨迹规划库CP_LIB库执行启发式搜索,获得当前库中最优运动基元作为规划结果。
[0065] 首先判断与障碍物距离是否小于紧急值KM。若不小于则进入非紧急状态,遍历步骤二中所有单关节空间轨迹规划库SP_LIB中的运动基元进行启发搜索,即调用步骤三中定
义的笛卡尔空间启发函数hd(x)和笛卡尔空间距离检测函数DO(q);若小于则进入紧急避障
状态,计算机械臂末端与障碍物笛卡尔空间距离梯度下降方向作为轨迹方向,利用笛卡尔
空间轨迹规划库CP_LIB中基元进行紧急避障轨迹规划,快速脱离障碍物区域。执行此步骤
后更新机械臂状态,跳转步骤四。
[0066] 步骤七:规划精确到达目标点轨迹,获得各个关节精确达到目标关节角的单元运动规划结果。
[0067] 机械臂位姿此时已经非常接近目标位置点,步骤2中的单关节空间轨迹规划库SP_LIB和笛卡尔空间轨迹规划库CP_LIB均不能保证精确达到指定C-空间状态SF,故采用自适
应逆运动学解(AP_LIB库)方法。
[0068] 这里的自适应库有两种。第一种是多关节同时进行轨迹规划MAP_LIB,实现协同运动;第二种是单关节独立进行规划SAP_LIB,根据具体需求进行选择。当机械臂在关节空间
的距离不大于阈值K2F时进行协同运动规划;当仅笛卡尔空间距离达到阈值K1F时进行单关
节独立规划。根据情况选取自适应逆运动学轨迹规划库中的运动基元进行启发搜索,调用
步骤三中定义的笛卡尔空间启发函数hd(x)和笛卡尔空间距离检测函数DO(q),综合启发函
数和碰撞函数结果对运动基元库成员择优作为启发结果,执行后更新机械臂状态,跳转步
骤四。
[0069] 步骤八:跳出计算过程,保存路径搜索结果,清理缓存和状态参数,等待指令执行下一次路径规划任务。
[0070] 实施例
[0071] 步骤1,建立机械臂完整运动学模型、机械臂简化运动学模型并构建机械臂工作环境,具体按如下步骤进行:
[0072] 步骤1-1根据厂家提供的D-H参数按式(1-2)建立机械臂关节坐标变换矩阵T0,将T0作为机械臂完整运动学模型,具体实施如下。其中,T0以28末端中心为原点建立工具坐标
系,以20中心为原点建立基座坐标,6个关节分别独立建立相应的关节坐标系wi,建立的机
械臂完整运动学模型的机械臂关节几何关系如附图7所示;式(1-2)中 为第i关节坐标
系wi到第i-1关节坐标系wi-1的变换矩阵,具体形式如式(1-1),其中αi为两相邻关节坐标系
zi-1轴到zi轴之间的夹角,ai为两个关节轴间距离,di为wi-1与wi坐标系沿坐标轴方向的偏移
距离,θi为第i关节旋转角度,
[0073]
[0074] 由上述方法可以获取机械臂末端到机械臂基座的变换矩阵T0如式(1-2),其中nx、ny、nz,ox、oy、oz,ax、ay、az以及Px、Py、Pz分别简记变换矩阵各项参数,需要注意的是最后一个
连杆坐标系原点应该是机械臂抓手末端中心点。
[0075]
[0076] 步骤1-2根据机械臂执行路径规划时的初始位置,按式(1-3)建立机械臂坐标变换矩阵T1,将T1作为机械臂简化运动学模型,具体实施如下。当指定机械臂起始位姿后,T1仍以
28末端中心为原点建立工具坐标系,以20中心为原点建立基座坐标,但将24、25、26、27、28
视为23的机械结构延伸,除了随动于23外没有相对运动,此时相当于只需建立21、22及23这
三个独立关节的关节坐标系,即附图7中w0、w1、w2、w3和w6。此时变换矩阵计算方法如式(1-
3),其中n1x、n1y、n1z,o1x、o1y、o1z,a1x、a1y、a1z以及P1x、P1y、P1z分别简记T1变换矩阵各项参数
[0077]
[0078] 步骤1-3杆塔元器件为标准件,故根据标准几何特征参数将各元器件利用简单几何体进行包覆替代,简化碰撞检测计算,具体实施如下。实施中将绝缘子用圆柱体包覆简
化;桁架用长方体包覆简化;电线用圆筒包覆替代;其他元器件运用类似方法,使用圆柱体、
球体或者长方体进行包覆简化,不再赘述。以附图3作为杆塔元器件包覆简化的说明实例,
如附图中31为耐张绝缘子及其包覆,32为横担及其包覆简化,33为杆塔及其包覆简化。
[0079] 步骤1-4建立作业环境的场景模型库,加速机器人三维工作环境实时建模速度,具体实施如下。根据作业杆塔的标准安装参数,建立描述作业环境的场景模板库。包括:接搭
线模板环境、绝缘子模板环境、隔离刀闸模板环境、
变压器模板环境。以杆塔连接线场景作
为场景模型库封装设计说明实例,如附图4所示,其中41为耐张线夹,42合成绝缘子,43并沟
线夹,44导线,45为杆塔,46为悬垂绝缘串,47为紧固圈。
[0080] 步骤2分类设计运动基元库,为各类库设计相应运动基元作为库成员,具体包括如下步骤:
[0081] 步骤2-1设计第一类基元库:单关节空间轨迹规划库(SP_LIB),对单关节独立运动过程按式(2-1)所示加速度变化规则进行轨迹规划,将规划结果作为库中成员,轨迹规划结
果曲线规律如附图5所示,具体实施如下。各关节运动基元根据设备给定的参考运动学参数
进行设计,此实施例加速度轨迹方程如式(2-1)所示,加速度-时间曲线由正弦曲线段与坐
标轴段组成,加速度、加加
速度曲线具有连续性,保证了轨迹的平稳性;速度-时间曲线光滑
连续,速度分为加速度段、匀速段和减速段,这样的运动规律可降低功耗,减小抖动和冲击,
提高运动平稳性。式(2-1)中accmax为加速度峰值、T为加速度曲线正弦段三角函数周期、T0
是加速度为负值的起点时刻、t为规划过程中的时间变量,进而可以推导得到关节匀速值
基元运动周期 其中accmax、T以及T0均可以根据机械臂运动学参数范围和
实际需求进行调整。
[0082]
[0083] 每个关节均需建立独立的轨迹规划过程,同时每个关节运动有顺
时针和逆时针运动两种情况。故6关节机械臂SP_LIB库需要12个库成员。每一种特定情况只需按照式(2-1)
调整三个参数即可完成规划过程。
[0084] 步骤2-2设计第二类基元库:自适应逆运动学轨迹规划库(AP_LIB),利用式(2-3)获取机械臂目标关节角和起始关节角,随后运用式(2-2)方程进行单关节轨迹规划,将规划
结果作为库中成员,具体实施过程如下。C空间中两点之间的轨迹规划采用三阶四次多项式
进行拟合,方程形式如式(2-2)所示。其中θi(t)(i=1…n)为第i关角度,c1、c2、c3及c4分别为
可求解系数,t为时间变量。
[0085] θi(t)=c1t3+c2t2+c3t+c4 (2-2)
[0086] 建立轨迹起始和末端的速度和加速度均为零的约束,起始和终止角度均已知或可由式(2-3)求解,故式中c1、c2、c3及c4均可确定。这里将规划结果分为两种情况:第一,6个关
节同时进行轨迹规划,轨迹运动在时间上同步;第二,6个关节独立进行规划(两种情况共有
7个库成员)。关节独立进行轨迹规划时,对各关节依次按式(2-2)进行独立轨迹规划即可;
多关节协同规划时,按式(2-2)进行轨迹计算中,各个关节计算过程中的时间t的步长保持
一致,则可得到多关节协同运动结果。
[0087] 当给定的目标点为笛卡尔空间下位姿时,根据T0、T1变换矩阵可以得到机械臂的逆运动学解,若是机械臂简化模型,则θ4、θ5、θ6无需求解。fk(x,y,z,α,β,γ),k∈[1,6]为第k关节的逆运动学解方程,其中x、y、z为机械臂末端笛卡尔空间中位置坐标,α、β、γ为笛卡尔
空间中姿态坐标。若机械臂结构不满足封闭逆运动学解条件,则进行数值
迭代求解。求解的
结果记为T-1(x,y,z,α,β,γ),逆解形式如式(2-3)所示。
[0088]
[0089] 已知当前
关节角度,利用式(2-3)获取期望关节角度后联立式(2-2)即可实现AP_LIB库下的轨迹规划。
[0090] 步骤2-3设计第三类基元库:笛卡尔空间轨迹规划库(CP_LIB),按式(2-4)、(2-5)确定笛卡尔空间中路径基元方向,按式(2-6)获取笛卡尔空间中路径点序列,对两相邻路径
点调用步骤2-2中关节空间轨迹规划过程可完成运动基元设计,将规划结果作为库的成员,
具体实施如下。在笛卡尔空间中规划机械臂末端路径轨迹时,路径的运动基元为笛卡尔空
间中直线段。首先计算机械臂末端与障碍物笛卡尔空间距离梯度方向作为直线段轨迹方
向,计算按式(2-4)、(2-5)进行,其中与障碍物距离计算方程DOc(x,y,z)定义如式(2-4),基
元直线段长度dl根据场景环境具体设置,直线插值按式(2-6)进行。为避开奇异点,根据机
械臂结构参数,预先求解出机械臂在笛卡尔空间中奇异点位置集S={s1,s2…sn},建立笛卡
尔空间中机械臂末端与奇异点距离检测函数Ds(x,y,z)如式(2-5)。当Ds(x,y,z)计算的距离
大于阈值DFs时,正常执行基元规划过程;当计算距离小于阈值时,以奇异点为圆心,机械臂
当前位置为半径随机做圆与预期直线段的交于两点,圆形中的直线段以随机圆产生的圆弧
代替,其它部分保持不变。这样的轨迹修正函数可以实现避免奇异点的目的。
[0091]
[0092]
[0093]
[0094]
[0095] 上述式(2-4)、(2-7)中(x,y,z)分别笛卡尔空间中机械臂末端位置坐标,(x0,y0,z0)为障碍物位置坐标,(x1,y1,z1)为奇异点位置坐标;式(2-5)中(dx,dy,dz)为距离增加的
单位梯度方向;式(2-6)中N为设置的路径点序列数,dl为基元路径直线长度,x(n)、y(n)及z
(n)分别为第n个序列点在笛卡尔空间中的x、y、z坐标。
[0096] 步骤3初始化机械臂工作环境参数;分类建立机械臂与目标点、障碍物的距离计算函数,设置危险区域阈值和到达目标点精度阈值;创建启发函数和启发规则,实现启发搜
索。具体按如下步骤进行:
[0097] 步骤3-1完成机械臂工作环境的搭建和环境参数初始化,具体实施如下。首先初始化世界坐标系w0(默认与机械臂基座中心重合,如附图7所示);然后根据三维点云功能模块
和图像处理模块的计算结果,选择步骤1-4中建立的相匹配的机器人作业场景模型进行匹
配操作,匹配成功后利用双目相机重复采样模型与环境匹配特征点的位置坐标,利用最小
二乘法拟合变换矩阵,获取各障碍物基准点相对w0位姿坐标Ow,若匹配失败则利用点云相机
和双目相机数据进行实时三维场景重建。
[0098] 步骤3-2建立机械臂与目标点、障碍物的距离计算函数,设置危险区域阈值和到达目标点精度触发阈值,具体实施如下。建立机械臂末端与障碍物的笛卡尔空间距离检测函
数DO(q)、机械臂末端与目标点笛卡尔空间距离检测函数D1F(q)和关节空间距离检测函数
D2F(q)(注:目标点约束包括位置和姿态,故需关节空间和笛卡尔空间两种距离检测函数,
只有当两者都归零或在一定精度范围内时才可认为到达目标点)。设置末端区域触发阈值
K1F(笛卡尔空间距离)、K2F(关节空间距离)和障碍物区域触发阈值KO(笛卡尔空间距离),设
置紧急避障区域阈值KM(笛卡尔空间距离),设置到达目标点精度范围阈值,AF1为笛卡尔空
间阈值,AF2为关节空间阈值。
[0099] 上述步骤3-2中Do(q)的计算具体按如下步骤进行:
[0100] 步骤S210根据步骤3-1建立的环境参数获取障碍物最新位姿[bx,by,bz,bα,bβ,bγ],其中bx、by、bz、bα、bβ及bγ分别为障碍物在笛卡尔空间中的(x,y,z,α、β、γ)位姿坐标,获取机械臂当前关节空间状态参数q=[θ1,θ2,θ3,θ4,θ5,θ6],其中θ1、θ2、θ3、θ4、θ5、θ6分别为第1到6关节的关节角。
[0101] 步骤S211利用步骤1-1建立的机械臂运动学模型,按式(3-1)计算获取当前机械臂末端在笛卡尔空间中位姿坐标[cx,cy,cz,cα,cβ,cγ]T,其中cx、cy、cz、cα、cβ及cγ分别为机械臂在笛卡尔空间中的位置坐标x、y、z和姿态角α、β、γ;
[0102]
[0103] 步骤S212计算机械臂末端到障碍物的笛卡尔空间距离为
[0104]
[0105] 上述步骤3-2中D1F(q)的计算具体按如下步骤进行:
[0106] 步骤S220根据步骤3-1建立的环境参数获取当前目标点在笛卡尔空间下最新位姿参数[ox,oy,oz,oα,oβ,oγ]T,其中ox、oy、oz、oα、oβ、oγ分别目标点为在笛卡尔空间中的位置坐标x、y、z和姿态角α、β、γ;获取机械臂当前关节空间状态参数q=[θ1,θ2,θ3,θ4,θ5,θ6];
[0107] 步骤S221利用步骤1-1建立的机械臂运动学方程,根据式(3-1)获取当前机械臂末端在笛卡尔空间中位姿[cx,cy,cz,cα,cβ,cγ]T;
[0108] 步骤S222计算机械臂末端到目标点的笛卡尔空间距离为
[0109]
[0110] 上述步骤3-2中D2F(q)的计算具体按如下步骤进行:
[0111] 步骤S230根据步骤3-1建立的环境参数获取当前目标点最新位姿[ox,oy,oz,oα,oβ,oγ]T;获取机械臂当前关节空间状态参数q=[θ1,θ2,θ3,θ4,θ5,θ6];
[0112] 步骤S231根据式(2-3)逆运动学解方程T-1(x,y,z,α,β,γ)求取目标点在机械臂关节空间中坐标为[θ1′,θ2′,θ3′,θ4′,θ5′,θ6′],其中θ1′、θ2′、θ3′、θ4′、θ5′、θ6′分别对应机械臂第1到6关节的关节角度;
[0113] 步骤S232计算关节空间距离可得:
[0114]
[0115] 步骤3-3创建自定义启发函数,设计启发搜索计算过程,实现对基元库中成员的启发计算,获取指定运动基元库中基元启发性能的降序排列,具体实施如下。更新机械臂当前
关节空间状态S0=[θ10,θ20,θ30,θ40,θ50,θ60],根据式(2-3)获取机械臂期望关节位置SF=
[θ1F,θ2F,θ3F,θ4F,θ5F,θ6F](注:若机械臂给定的目标末端位置是笛卡尔空间中的位姿点,则机械臂逆解存在多解的情况,这里对最优解的筛选按不碰撞、关节空间距离最小两个指标
进行)。构造并初始化笛卡尔空间中的启发函数hd(x)和关节空间中的启发函数hc(x)实现启
发计算。本发明设计的启发过程是基于运动基元库的,在路径启发搜素时遍历当前运动库
中有限的运动基元即可完成启发计算。
[0116] 上述步骤3-3中启发函数构造具体按如下步骤:
[0117] 步骤3-3-1构造笛卡尔空间中的启发函数hd(x)和启发计算规则,获取指定运动基元库中基元启发性能的降序排列。
[0118] 确定当前选中运动基元库,定义以笛卡儿空间距离为
基础的启发函数hc(x)=f(x0x,ε),此处x0为机械臂当前关节空间状态,x为运动基元,ε∈[0,1]为比例系数,这里的启
发函数计算采用遍历当前库中运动基元的方式实现。根据库中
选定的运动基元获取关节空
间新状态x0′,调用式(3-2)DO(q)函数计算新状态下末端点与障碍物的笛卡儿空间距离do
(k)(K为运动基元的编号),按距离由大到小给选定库中运动基元赋值do(k)∈{1,2,…,n}
(即max(do(k))=1,min(do(k))=n),其中n为运动基元总数;调用D1F(q)函数,计算新状态
下末端点与期望位置的笛卡儿空间距离da(k),按距离由小到大给运动基元赋值da(k)∈
{1,2,…,n}(即max(do(k))=n,min(do(k))=1)。启发性能指标为P=ε*da(k)+(1-ε)*do
(k),基元性能按P值升序排列,相同P值取da(k)小者性能更佳。
[0119] 其中计算新状态下do(k)与da(k)包括如下步骤:
[0120] 步骤S310根据定hd(x)=f(x0,x,ε),首先更新获取机械臂当前关节空间状态x0=s0=[θ10,θ20,θ30,θ40,θ50,θ60],计算加入运动基元后机械臂笛卡尔空间坐标,包括如下两个判断计算步骤:
[0121] 步骤S310-1若当前选定库为步骤2中AP_LAB库或SP_LIB库,则基元形式为:
[0122] x=[Δθ1,Δθ2,Δθ3,Δθ4,Δθ5,Δθ6]
[0123] 新状态x0′为:
[0124] x0′=[θ1+Δθ1,θ2+Δθ2,θ3+Δθ3,θ4+Δθ4,θ5+Δθ5,θ6+Δθ6]
[0125] 新状态下,机械臂末端笛卡尔空间位姿坐标c′为:
[0126]
[0127] 步骤S310-2当前选定库为步骤2中CP_LIB库,则基元形式为:
[0128] x=[Δcx,Δcy,Δcz,Δcα,Δcβ,Δcγ]T
[0129] 根据x0=s0,获取当前机械臂笛卡尔空间位姿坐标为:
[0130]
[0131] 计算加入运动基元后的新坐标为:
[0132] c′=[cx+Δcx,cy+Δcy,cz+Δcz,cα+Δcα,cβ+Δcβ,cγ+Δcγ]T (3-7)
[0133] 步骤S311计算机械臂末端与障碍物距离:
[0134] 获取当前障碍物最新位姿[bx,by,bz,bα,bβ,bγ]T,调用式(3-1)距离计算函数如下
[0135]
[0136] 步骤S312计算机械臂末端与目标点距离:
[0137] 获取当前目标点最新位姿[ox,oy,oz,oα,oβ,oγ]T,调用式(3-1)距离计算函数如下
[0138]
[0139] 步骤S313返回针对当前库中第k个运动基元的计算结果do(k)=dco和da(k)=dcs。
[0140] 步骤3-3-2构造关节空间中的启发函数hc(x)和启发计算规则,获取指定运动基元库中基元启发性能的降序排列。
[0141] 确定当前选中运动基元库,定义以关节空间距离为基础的启发函数hc(x)=f(x0x,ε),此处x0为机械臂当前关节空间状态,x为运动基元,ε∈[0,1]为比例系数。这里的启发函
数计算采用遍历当前库中运动基元的方式实现,根据库中选定的运动基元获取关节空间新
状态x0′,判断各关节角度当前值与期望值的差值ea(k)(k表示库中第k个基元),按照|ea
(k)|从小到大给运动基元赋值ea(k)∈{1,2,…,n}(n为关节数),即max(|ea(k)|)=n,min
(|ea(k)|)=1;;调用D2(q)函数,计算新状态与障碍物笛卡尔空间距离,按距离由大到小给
运动基元赋值do(k)∈{1,2,…,n}。启发性能指标为P=ε*ea(k)+(1-ε)*do(k),基元性能按
P值升序排列,相同P值取ea(k)小者性能更佳。
[0142] 其中,计算新状态下ea(k)及do(k)包括如下步骤:
[0143] 步骤S320根据定hd(x)=f(x0,x,ε),首先更新获取机械臂当前关节空间状态x0=s0=[θ10,θ20,θ30,θ40,θ50,θ60],获取目标点关节空间状态SF=[θ1F,θ2F,θ3F,θ4F,θ5F,θ6F],计算加入运动基元后机械臂关节空间坐标,具体包括如下两个判断执行步骤:
[0144] 步骤S320-1若运动基元为AP_LAB或SP_LIB,则基元形式为
[0145] x=[Δθ1,Δθ2,Δθ3,Δθ4,Δθ5,Δθ6]
[0146] 新关节空间状态为:
[0147] x0′=[θ1+Δθ1,θ2+Δθ2,θ3+Δθ3,θ4+Δθ4,θ5+Δθ5,θ6+Δθ6]
[0148] =[θc1,θc2,θc3,θc4,θc5,θc6]
[0149] 新笛卡尔空间位姿坐标为:
[0150]
[0151] 步骤S320-2若运动基元为CP_LIB,则基元形式为x=[Δcx,Δcy,Δcz,Δcα,Δcβ,Δcγ]T,根据x0获取当前机械臂坐标笛卡尔空间位姿坐标为:
[0152]
[0153] 计算加入运动基元后的笛卡尔下新坐标为:
[0154] c′=[cx+Δcx,cy+Δcy,cz+Δcz,cα+Δcα,cβ+Δcβ,cγ+Δcγ]T
[0155] =[c′x,c′y,c′z,c′α,c′β,c′γ]T
[0156] 此时,
[0157] x0′=T-1(c′)=[θc1,θc2,θc3,θc4,θc5,θc6] (3-11)
[0158] 步骤S321计算机械臂末端与障碍物距离:
[0159] 获取当前障碍物最新位姿[bx,by,bz,bα,bβ,bγ]T,调用距离计算函数如下
[0160]
[0161] 步骤S322计算机械臂新关节状态下各关节与目标关节状态差值:
[0162] 目标点关节空间状态为SF=[θ1F,θ2F,θ3F,θ4F,θ5F,θ6F],根据下式计算关节空间距离为:
[0163]
[0164] 步骤S321返回针对当前库中第k个运动基元的计算结果do(k)=dco和ea(k)=dea。
[0165] 步骤4计算机械臂当前位姿与期望位姿及障碍物的笛卡尔空间距离,根据设定的判断条件选择执行下一步,具体按如下步骤进行:
[0166] 步骤4-1获取机械臂当前关节空间状态x0,按式(3-6)利用步骤1-1建立的机械臂运动学模型获取当前机械臂在笛卡尔空间下位姿c,调用式(3-1)、式(3-3)及式(3-4)计算
DO(q)、D1F(q)和D2F(q)的值,将函数计算结果与区域触发阈值KO、K1F及K2F进行比较。
[0167] 步骤4-2对获得计算结果进行判断,根据判断条件选择执行以下步骤:
[0168] 条件1:若:D1F(q)>K1F或D2F(q)>K2F,且DO(q)>KO;顺序执行步骤5;
[0169] 条件2:若:D1F(q)>K1F或D2F(q)>K2F,且DO(q)
[0171] 条件4:其他情况跳转步骤7;
[0172] 步骤5在降维稀疏的C空间中使用SP_LIB库执行启发式搜索,获得当前库中最优运动基元作为规划结果。具体包括如下步骤:
[0173] 步骤5-1采用机械臂简化运动学模型T1,调用步骤3-3-1中定义的hd(x)启发函数遍历步骤2-1中定义的SP_LIB库中的运动基元进行启发搜索(只需遍历机械臂前三个关节的
运动基元,即如图2中21、22、23这三个关节),获得基元启发性能降序排列。调用步骤3-2中
的碰撞检测函数DO(q)按顺序对启发性能序列进行碰撞检测计算,第一个不碰撞基元作为
启发搜索结果,也即当前单元运动规划结果;
[0174] 步骤5-2更新当前机械臂关节空间状态;
[0175] 步骤5-3跳转步骤4;
[0176] 步骤6在非降维稀疏C空间中使用SP_LIB库或CP_LIB库执行启发式搜索,获得当前库中最优运动基元作为规划结果。具体包括如下步骤:
[0177] 步骤6-1判断与障碍物距离是否触发阈值KM,若DO(q)≥KM,则进入非紧急状态,顺序执行步骤6-2;反之跳转执行步骤6-3;
[0178] 步骤6-2采用完整机械臂运动学模型T0,调用步骤3-3-1中定义的hd(x)启发函数,遍历步骤2-1中定义的单关节空间轨迹规划库(SP_LIB)中的12种库成员中进行启发搜索,
获得基元启发性能降序排列。调用步骤3-2中的碰撞检测函数DO(q)按顺序对启发性能序列
进行碰撞检测计算,第一个不碰撞基元作为启发搜索结果,也即当前单元运动规划结果。跳
转步骤6-4;
[0179] 步骤6-3进入紧急避障状态,指定基元直线段长度dl,调用执行步骤2-3中定义的笛卡尔空间轨迹规划库(记为CP_LIB),获得紧急避障路径规划结果作为当前单元运动规划
结果;
[0180] 步骤6-4更新当前机械C-空间状态;
[0181] 步骤6-5跳转步骤4;
[0182] 步骤7规划精确到达目标点轨迹,获得各个关节精确达到目标关节角的单元运动规划结果。具体按如下步骤进行:
[0183] 步骤7-1检测是否出发阈值K1F,若DO(q)≤K1F,则顺序执行;反之跳转步骤7-3;
[0184] 步骤7-2利用步骤2-2中的自适应逆运动学轨迹规划库(记为AP_LIB),在关节空间完成轨迹规划。首先调用步骤3-3-2定义的hc(x)启发函数,遍历AP_LIB库中单关节独立运
动基元,获得基元启发性能降序排列。然后调用步骤3-2中的碰撞检测函数DO(q)按顺序对
启发性能序列进行碰撞检测计算,第一个不碰撞基元作为启发搜索结果,也即当前单元运
动规划结果,跳转步骤7-4;
[0185] 步骤7-3调用步骤2-2中的自适应逆运动学轨迹规划库(记为AP_LIB),在关节空间完成轨迹规划,运动基元为6个关节协同运动轨迹规划基元;
[0186] 步骤7-4更新当前C-空间状态;
[0187] 步骤7-5跳转步骤4;
[0188] 步骤8跳出计算过程,保存路径搜索结果,清理缓存和状态参数,等待指令执行下一次路径规划任务。