一种连续体机器人的轨迹跟随运动规划方法和系统与流程

1.本发明属于连续体机器人避障算法领域，更具体地，涉及一种连续体机器人的轨迹跟随运动规划方法和系统。


背景技术：

2.在现代工业工程领域，机器人已经在诸多工作场景中得到了广泛的应用，如：加工制造、生产制造、物流分拣、检测维修等方面。但是，在面对如飞机油箱内窥检查、发动机维修检查、单孔手术作业等结构复杂、空间狭小的作业环境时，这类工业工程机器人便由于自身结构尺寸过大、运动灵巧性不足等限制，无法再满足此类应用场景的需求。
3.相比传统的机器人，连续体机器人的运动并非是绕关节旋转或平移的形式，而是靠自身弯曲变形产生，这使得连续体机器人具有更高的灵巧性、安全性和更强的环境适应能力，且其绳驱的设计使得驱动器被布置于远端，大大缩小了连续体机器人的体积和重量，使得其具有更好的负载能力和可操控能力。以上特性使得连续体机器人在微创手术、微孔探测及复杂受限空间中的任务执行等场合显现出了巨大的优势并发挥着重要的作用。
4.虽然连续体机器人具有如上优点和应用价值，但是其运动学模型却与传统机器人有着很大的不同，其在结构复杂、空间狭小的狭窄环境等应用场景中存在着诸如运动学模型更复杂、控制精度不高等问题。


技术实现要素：

5.针对相关技术的缺陷，本发明的目的在于提供一种连续体机器人的轨迹跟随运动规划方法和系统，旨在解决连续体机器人在通过狭窄腔道进行轨迹跟随时控制精度不高的问题。
6.为实现上述目的，本发明的一个方面提供了一种连续体机器人的轨迹跟随运动规划方法，所述连续体机器人沿着由末端指向基座的方向包括第一关节、第二关节和第三关节；所述轨迹跟随运动规划方法包括以下步骤：
7.定义腔道口坐标系{o0}、末端坐标系{o1}和基座坐标系{o2}，移动机器人使所述末端坐标系{o1}与腔道口坐标系{o0}重合；
8.根据腔道内部环境，设定连续体机器人的目标状态的关节参数，得到目标轨迹；其中，所述关节参数包括弯曲关节段末端切线与基座坐标系{o2}z轴正方向夹角θ
i
，以及单关节中心轴线在基座坐标系{o2}oxy平面的投影到基座坐标系{o2}x轴正方向的转角
9.将目标轨迹划分为三条首尾相连的圆弧轨迹段，每条圆弧轨迹段分成多段步进运动进行跟随，包括：
10.所述连续体机器人沿所述腔道口坐标系{o0}的z轴正向移动；
11.根据阻尼牛顿法求解非线性方程组以计算第一关节、第二关节和第三关节的关节参数；
12.按照各自对应的关节参数，依次控制第三关节、第二关节和第一关节运动。
13.进一步地，根据阻尼牛顿法求解非线性方程组以计算第一关节、第二关节和第三关节的关节参数，包括以下步骤：
14.定义与第i关节对应的圆弧轨迹段为path
ia
，求取path
ia
的关键点b
ia
、o
ia
在所述腔道口坐标系{o0}下的坐标以及该圆弧轨迹段的半径r
ia
；其中，关键点o
ia
是path
ia
的圆心，关键点b
ia
与o
ia
连接组成的线段o
ia
b
ia
长度为l0且垂直于path
ia
所在的平面；
15.将path
ia
的关键点b
ia
、o
ia
坐标变换到所述末端坐标系{o1}下；
16.根据阻尼牛顿法求解非线性方程组函数g(y
m
)的自变量为第m次迭代得到的关节参数；构造迭代方程和约束条件如下：
17.迭代方程：y
m 1
＝y
m
‑
λg
′
(y
m 1
)
‑1g(y
m
)
18.约束条件：norm(g(y
m 1
)，2)＜norm(g(y
m
)，2)
19.式中λ∈(0，1)为步长因子，对于每一次迭代，从λ＝1开始计算，当不满足约束条件时，步长因子缩小至原来的9/10，直到约束条件成立；当迭代至norm(g(y
m
)，2)≤0.1时，迭代结束，输出关节参数y
m
。
20.进一步地，按照各自对应的关节参数，依次控制第三关节、第二关节和第一关节运动包括：
21.在进行第j段轨迹跟随时，控制第三关节运动至其关节参数等于且指定当j
‑
2n≤0时，再控制第二关节运动至其关节参数等于然后控制第一关节运动至连续体机器人的末端与目标轨迹重合；最后，记录此时的关节参数
22.进一步地，所述腔道口坐标系的原点位于腔道入口的中心处，其z轴方向为垂直腔道入口所在平面向里；所述末端坐标系与基座坐标系均为与连续体机器人随动的拉格朗日坐标系；
23.所述末端坐标系{o1}的原点位于机器人末端中心处，其z轴方向与机器人中心轴重合且从初始复位状态的基座指向末端；
24.所述基座坐标系{o2}的原点位于机器人基座盘中心处，其z轴方向与机器人中心轴重合且从初始复位状态的基座指向末端，其x轴与{o1}坐标系x轴平行且正方向一致。
25.本发明的另一方面提供了一种连续体机器人的轨迹跟随运动规划系统，所述连续体机器人沿着由末端指向基座的方向包括第一关节、第二关节和第三关节；所述轨迹跟随运动规划系统包括：
26.初始化单元，定义腔道口坐标系{o0}、末端坐标系{o1}和基座坐标系{o2}，移动机器人使所述末端坐标系{o1}与腔道口坐标系{o0}重合；
27.设定单元，根据腔道内部环境，设定连续体机器人的目标状态的关节参数，得到目标轨迹；其中，所述关节参数包括弯曲关节段末端切线与基座坐标系{o2}z轴正方向夹角θ
i
，以及单关节中心轴线在基座坐标系{o2}oxy平面的投影到基座坐标系{o2}x轴正方向的转角
28.跟随单元，将目标轨迹划分为三条首尾相连的圆弧轨迹段，每条圆弧轨迹段分成
多段步进运动进行跟随，包括：
29.所述连续体机器人沿所述腔道口坐标系{o0}的z轴正向移动；
30.根据阻尼牛顿法求解非线性方程组以计算第一关节、第二关节和第三关节的关节参数；
31.按照各自对应的关节参数，依次控制第三关节、第二关节和第一关节运动。
32.进一步地，所述跟随单元根据阻尼牛顿法求解非线性方程组以计算第一关节、第二关节和第三关节的关节参数，包括：
33.定义与第i关节对应的圆弧轨迹段为path
ia
，求取path
ia
的关键点b
ia
、o
ia
在所述腔道口坐标系{o0}下的坐标以及该圆弧轨迹段的半径r
ia
；其中，关键点o
ia
是path
ia
的圆心，关键点b
ia
与o
ia
连接组成的线段o
ia
b
ia
长度为l0且垂直于path
ia
所在的平面；
34.将path
ia
的关键点b
ia
、o
ia
坐标变换到所述末端坐标系{o1}下；
35.根据阻尼牛顿法求解非线性方程组m为迭代次数；构造迭代方程和约束条件如下：
36.迭代方程：y
m 1
＝y
m
‑
λg
′
(y
m 1
)
‑1g(y
m
)
37.约束条件：norm(g(y
m 1
)，2)＜norm(g(y
m
)，2)
38.式中λ∈(0，1)为步长因子，对于每一次迭代，从λ＝1开始计算，当不满足约束条件时，步长因子缩小至原来的9/10，直到约束条件成立；当迭代至norm(g(y
m
)，2)≤0.1时，迭代结束，输出关节参数y
m
。
39.进一步地，所述跟随单元按照各自对应的关节参数，依次控制第三关节、第二关节和第一关节运动包括：
40.在进行第j段轨迹跟随时，控制第三关节运动至其关节参数等于且指定当j
‑
2n≤0时，再控制第二关节运动至其关节参数等于然后控制第一关节运动至连续体机器人的末端与目标轨迹重合；最后，记录此时的关节参数
41.进一步地，所述腔道口坐标系的原点位于腔道入口的中心处，其z轴方向为垂直腔道入口所在平面向里；所述末端坐标系与基座坐标系均为与连续体机器人随动的拉格朗日坐标系；
42.所述末端坐标系{o1}的原点位于机器人末端中心处，其z轴方向与机器人中心轴重合且从初始复位状态的基座指向末端；
43.所述基座坐标系{o2}的原点位于机器人基座盘中心处，其z轴方向与机器人中心轴重合且从初始复位状态的基座指向末端，其x轴与{o1}坐标系x轴平行且正方向一致。
44.通过本发明所构思的以上技术方案，与现有技术相比，本发明基于连续体机器人的运动学建模、正逆运动学分析，通过控制外部直线驱动器驱动机器人整体在竖直方向上进行步进运动的同时，控制机器人本体不断跟随目标轨迹，实现了控制所述机器人无碰撞地通过狭窄腔道到达指定位置，轨迹跟随更精准、求解耗时短，适用于飞机油箱等入口狭窄内部复杂环境的内窥检查。
附图说明
45.图1是本发明实施例中连续体机器人采用轨迹跟随避障算法的流程图。
具体实施方式
46.为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
47.下面结合一个优选实施例，对上述实施例中涉及的内容进行说明。
48.s1：定义腔道口坐标系{o0}、连续体机器人末端坐标系{o1}和基座坐标系{o2}，定义坐标系的要求如下：
49.所述腔道口坐标系，其原点位于腔道入口的中心处，其z轴方向为垂直腔道入口所在平面向里；所述连续体机器人末端与基座坐标系，其为与连续体机器人随动的拉格朗日坐标系；所述连续体机器人由第一、二、三关节从上而下串联组成；所述连续体机器人末端坐标系{o1}，其原点位于机器人末端中心处，其z轴方向与机器人中心轴重合且沿竖直向上；所述连续体机器人基座坐标系{o2}，其原点位于机器人底部中心处，其z轴方向与机器人中心轴重合且沿竖直向上，其x轴与{o1}坐标系x轴平行且正方向一致。
50.在其中一个实施例中，所述连续体机器人为三关节连续体机器人。机器人采用线驱动，单关节在线驱动作用下发生弯曲变形，发生变形时其中心轴线可视作曲率恒定的圆弧，通过控制可以改变弯曲的方向和弯曲曲率的大小，故每个关节具有两个自由度，机器人一共具有六个自由度。
51.s2：保持连续体机器人处于初始复位状态，移动机器人使末端坐标系{o1}与腔道口坐标系{o0}重合；
52.所述初始复位状态，指连续体机器人三个关节的中心轴线均为直线且重合，即机器人的各关节参数均为0。
53.s3：根据腔道内部环境指定机器人完全进入腔道后的末态关节参数此时基座坐标系{o2}与腔道口坐标系{o0}重合，其中所述关节参数的定义如下：
54.每个单关节定义有关节参数其中θ
i
描述弯曲关节段中心轴线的圆心角大小，描述单关节中心轴线在基座坐标系{o2}oxy平面的投影到基座坐标系{o2}x轴正方向转角的大小，以顺时针为正向。
55.s4：将机器人完全进入腔道后三段关节中心轴线连成的连续曲线命名为目标轨迹a。轨迹a由三段圆弧首尾相连组成，每段圆弧的弧长均等于单关节长度l0＝100mm，取机器人第三关节段基座盘中心为起点，第一关节段末端盘中心为终点。
56.对轨迹a进行轨迹跟随，整个轨迹跟随过程共分为3n段，轨迹a的每段圆弧轨迹均分为n段进行跟随，其中n根据轨迹跟随精度需求给定。轨迹跟随过程如下：
57.在进行第j段轨迹跟随时，首先机器人整体在外部直线驱动器的驱动下沿z0轴正向移动300/(3n)mm的距离；控制第三关节段运动至其关节参数等于(指定当
j
‑
2n≤0时，)；再控制第二关节段运动至其关节参数等于然后控制第一关节段运动至末端与轨迹a重合；最后记录此时的关节参数经过3n段步进运动后，机器人指定目标任务。
58.在上述第j段步进运动中，要控制第一关节段运动至与轨迹a重合，为达到这一目标，只需计算θ
1，j
、这两个关节参数，其余四个关节参数从已记录的数据中读取即可。虽然仅需求解两个关节参数，但是其解析解依然很难获得，对此本发明采用阻尼牛顿法来求解。其中所述控制第一关节段运动至末端与轨迹a重合，其具体求解过程如下：
59.定义轨迹a上与第i关节段对应的圆弧轨迹段为path
ia
，整个轨迹跟随过程中的第j段将对第段对应的圆弧轨迹段进行跟随。求取path
ia
的关键点b
ia
、o
ia
在腔道口坐标系{o0}下的坐标，以及该圆弧轨迹段的半径r
ia
。其中关键点o
ia
是path
ia
的圆心，关键点b
ia
与o
ia
连接组成的线段o
ia
b
ia
长10mm且垂直于path
ia
所在的平面。
60.假设正在进行第j(j∈[1，3n]且j∈n
*
)段步进运动，则将path
ia
的关键点b
ia
、o
ia
坐标变换到当前的末端坐标系{o1}下。
[0061]
定义函数如下：
[0062][0063]
式中，函数g(y
m
)的自变量为第m次迭代得到的关节参数，表示path
ia
圆弧半径与第m次迭代后关节末端坐标系原点o1到点o
ia
的距离的差值，表示o1到path
ia
所在平面的距离。方程组g(y
n
)＝0的解即为第一关节运动至轨迹a重合对应的关节参数。根据阻尼牛顿法求解非线性方程组的解法，构造迭代方程和约束条件如下：
[0064]
迭代方程：y
m 1
＝y
m
‑
λg
′
(y
m 1
)
‑1g(y
m
)
[0065]
约束条件：norm(g(y
m 1
)，2)＜norm(g(y
m
)，2)
[0066]
式中λ∈(0，1)为步长因子，每一次迭代中，从λ＝1开始计算，当不满足约束条件时，步长因子缩小至原来的9/10，直到约束条件成立。当迭代至norm(g(y
m
)，2)≤0.1时，表明求解误差已经在可以接受的范围内，迭代结束，输出关节参数y
m
。
[0067]
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。