一种基于双闭环自适应PID的多机器人编队控制方法技术

本发明专利技术涉及一种基于双闭环自适应PID的多机器人编队控制方法，包括：建立领航

【技术实现步骤摘要】
一种基于双闭环自适应PID的多机器人编队控制方法


[0001]本专利技术属于机器人编队控制
，具体涉及一种基于双闭环自适应PID的多机器人编队控制方法。

技术介绍

[0002]石化场站是指以石油、天然气及其它产品为原料通过石化设备进行分离。液化等操作而存储输运的场所，石化生产主要由原料处理、化学反应及产品精制三大工艺过程构成，原料经预处理满足加工要求，再通过复合化学反应制得高质量产品。为满足生产介质及工艺过程的多样化需求，石油炼制及加氢裂化等生产环节通常要求设备多元化、功能多样化及输出最大化，故对石化设备性能与工况提出了严格要求。
[0003]石化设备通常面临不同压力及温度、多介质的工况条件。由于生产介质具有强腐蚀、易燃易爆及有毒有害等特性，当设备长期高负荷运行或超出阈值时，设备强度、塑性及韧性等力学性能与耐腐蚀、抗氧化等化学性能将逼近最大承载值，对设备造成不可逆变化，增加企业设备成本。
[0004]生产介质易燃﹑易爆及有毒等特性极易造成泄漏安全事故。石化场站因占地面积大，生产设备类型复杂及数量庞大，电气设备运行时极易产生电火花、撞击火花等可燃源，当介质泄漏达到一定浓度时，因可控性低一旦接触可燃源即会造成火灾爆炸事故，造成巨大损失。
[0005]在采用以多个机器人组成机器人编队进行泄漏安全事故处理时，为了能够更有效的执行任务，通常采用编队的形式进行；而且在执行任务中，根据环境变化需要改变编队的队形，在改变队形时还要考虑碰撞问题，因此，多机器人编队控制成为技术需求。

技术实现思路
<br/>[0006]鉴于上述的分析，本专利技术旨在公开了一种基于双闭环自适应PID的多机器人编队控制方法，用于解决多机器人的编队控制问题。
[0007]本专利技术公开了一种基于双闭环自适应PID的多机器人编队控制方法，包括：
[0008]建立领航
‑
跟随者的多机器人编队结构；在多机器人编队中指定其中一个机器人为领航机器人，其余机器人为跟随机器人；在编队结构中为每个跟随机器人设置一个对应的虚拟跟随机器人，所述虚拟跟随机器人所处位置为对应的跟随机器人在编队中保持队形所要达到的期望位置；
[0009]通过双闭环自适应PID调节的编队控制器进行编队控制；在控制中，使领航机器人对给定的编队运动参考轨迹的轨迹点位置进行实时跟踪；根据领航机器人的实时位置计算编队中的每个虚拟跟随机器人的实时位置；使跟随机器人对虚拟跟随机器人的实时位置进行跟踪；以实现多机器人编队的按队形移动或队形变换。
[0010]进一步地，所述双闭环自适应PID调节的编队控制器中采用基于运动学模型的PID调节和基于动力学模型的自适应PID调节的双闭环PID调节方式。
[0011]进一步地，在编队运动时，建立领航机器人的运动学模型和动力学模型，采用双闭环自适应PID调节的编队控制器，对领航机器人的位置和姿态进行控制，使领航机器人的位置收敛到给定的编队运动参考轨迹的轨迹点位置；
[0012]建立跟随机器人的运动学模型和动力学模型，采用双闭环自适应PID调节的编队控制器，对跟随机器人的位置和姿态进行控制，使跟随机器人的位置收敛到对应的虚拟跟随机器人的位置；
[0013]所述虚拟跟随机器人的位置为根据领航机器人的位置和姿态数据进行坐标转换获得。
[0014]进一步地，所述虚拟跟随机器人的位置获得方法，包括：
[0015]根据编队的队形形状，得到跟随机器人与领航机器人之间需要保持的期望距离和期望方位角
[0016]获得编队中所述领航机器人的位置数据(x
L
,y
L
)和方位角数据φ
L
；
[0017]根据公式得到所述虚拟跟随机器人的位置数据(x
V
,y
V
)和方位角数据φ
V
。
[0018]进一步地，所述领航机器人和跟随机器人均为球形机器人，则建立的机器人运动学模型为：
[0019][0020]其中，q＝[x,y,φ,θ,ψ]T
，(x,y)为球体与平面接触点的位置坐标；(φ,θ,ψ)为球体的三轴欧拉角，φ为球体绕其载体坐标系Z
b
轴旋转对应偏航角，θ为球体绕其载体坐标系Y
b
轴旋转对应横滚角，ψ为球体绕其载体坐标系X
b
轴旋转对应俯仰角，r为球体的半径。
[0021]进一步地，基于运动学模型的PID调节的控制律
[0022]其中，k
p
＞0、k
i
≥0、k
d
≥0为机器人位置误差的比例、积分和微分调节系数；
[0023]在进行领航机器人的基于运动学模型的PID调节时，e
x
＝x
d
‑
x
L
；e
y
＝y
d
‑
y
L
；(x
L
,y
L
)为领航机器人的位置数据，(x
d
,y
d
)为编队运动参考轨迹的轨迹点位置数据；
[0024]在进行跟随机器人的基于运动学模型的PID调节时，e
x
＝x
V
‑
x
F
；e
y
＝y
V
‑
y
F
；(x
V
,y
V
)为所述虚拟跟随机器人的位置数据，(x
F
,y
F
)为跟随机器人的位置数据。
[0025]进一步地，同时考虑机器人的位置控制和避障控制，得到具有避障效果的机器人运动学控制律u＝εu
p
+(1
‑
ε)u
a
；
[0026]其中，u
p
为基于运动学模型的PID调节的控制律；u
a
为进行避障控制的控制律；ε≥0
是权重因子。
[0027]进一步地，所述避障控制的控制律其中，表示机器人j对机器人i施加的斥力势场力，定义为：其中，k
a
为斥力系数，d
ij
为机器人i与机器人j之间的距离，d0为机器人避障响应距离，r为机器人半径，p
i
为机器人i的位置，p
j
为机器人j的位置，n为编队机器人的数量。
[0028]进一步地，将所述领航机器人和跟随机器人均为球形机器人，则建立的机器人动力学模型为：
[0029][0030]其中，为对称正定惯性矩阵，为向心力矩阵，为输入变换矩阵，为作用于球形机器人的控制力矩，τ
θ
为Y
b
轴的旋转控制力矩、τ
ψ
为X
b
轴的旋转控制力矩；为雅可比矩阵，为拉格朗日乘子；m为球壳的质量，I
s
为转动惯量。...

【技术保护点】

【技术特征摘要】
1.一种基于双闭环自适应PID的多机器人编队控制方法，其特征在于，包括以下步骤：建立领航
‑
跟随者的多机器人编队结构；在多机器人编队中指定其中一个机器人为领航机器人，其余机器人为跟随机器人；在编队结构中为每个跟随机器人设置一个对应的虚拟跟随机器人，所述虚拟跟随机器人所处位置为对应的跟随机器人在编队中保持队形所要达到的期望位置；通过双闭环自适应PID调节的编队控制器进行编队控制；在控制中，使领航机器人对给定的编队运动参考轨迹的轨迹点位置进行实时跟踪；根据领航机器人的实时位置计算编队中的每个虚拟跟随机器人的实时位置；使跟随机器人对虚拟跟随机器人的实时位置进行跟踪；以实现多机器人编队的按队形移动或队形变换。2.根据权利要求1所述的多机器人编队控制方法，其特征在于，所述双闭环自适应PID调节的编队控制器中采用基于运动学模型的PID调节和基于动力学模型的自适应PID调节的双闭环PID调节方式。3.根据权利要求2所述的多机器人编队控制方法，其特征在于，在编队运动时，建立领航机器人的运动学模型和动力学模型，采用双闭环自适应PID调节的编队控制器，对领航机器人的位置和姿态进行控制，使领航机器人的位置收敛到给定的编队运动参考轨迹的轨迹点位置；建立跟随机器人的运动学模型和动力学模型，采用双闭环自适应PID调节的编队控制器，对跟随机器人的位置和姿态进行控制，使跟随机器人的位置收敛到对应的虚拟跟随机器人的位置；所述虚拟跟随机器人的位置为根据领航机器人的位置和姿态数据进行坐标转换获得。4.根据权利要求3所述的多机器人编队控制方法，其特征在于，所述虚拟跟随机器人的位置获得方法，包括：根据编队的队形形状，得到跟随机器人与领航机器人之间需要保持的期望距离和期望方位角获得编队中所述领航机器人的位置数据(x
L
,y
L
)和方位角数据φ
L
；根据公式得到所述虚拟跟随机器人的位置数据(x
V
,y
V
)和方位角数据φ
V
。5.根据权利要求1
‑
4任一项所述的多机器人编队控制方法，其特征在于，所述领航机器人和跟随机器人均为球形机器人，则建立的机器人运动学模型为：其中，q＝[x,y,φ,θ,ψ]
T
，，(x,y)为球体与平面接触点的位置坐标；(φ,θ,ψ)为球体的三轴欧拉角，φ为球体绕其载体坐标系Z
b
轴旋转对应偏航角，θ为球体绕其载体坐标系Y
b
轴旋转对应横滚角，ψ为球体绕其载体坐标系X
b
轴旋转对应
俯仰角，r为球体的半径。6.根据权利要求5所述的多机器人编队控制方法，其特征在于，基于运动学模型的PID调节的控制律其中，k
p
＞0、k
i
≥0、k
d
≥0为机器人位置误差的比例、积分和微分调节系数；在进行领航机器人的基于运动学模型...

【专利技术属性】
技术研发人员：史聪灵，车洪磊，韩松，刘国林，胥旋，
申请(专利权)人：中国安全生产科学研究院，
类型：发明
国别省市：