摆式球形机器人姿态扰动补偿控制方法及其系统

1.本发明涉及机器人姿态补偿技术，具体涉及一种摆式球形机器人姿态扰动补偿控制方法及其系统。


背景技术：

2.近年来，信息及制造技术加速发展，机器人技术开始应用于各行业中。机器人的概念范围也越来越广泛，出现了多种新形式的机器人。其中，异形结构的特种机器人逐渐成为研究热点。
3.在特殊环境下，传统机器人无法满足任务需求，多种适用于复杂环境的特种机器人应运而生。球形机器人因其独特的外形备受关注，其具有较高的运动灵活性与环境适应力，可用于环境侦察，信息收集等多个方面。但是，复杂环境下的摆式球形机器人控制难度增大，同时环境中存在各种扰动，工程常用的pid控制器无法有效抑制环境中的扰动，导致运动效果差及机器人姿态摇摆。
4.球形机器人作为欠驱动系统，机器人具有静态稳定性，但是在存在外部扰动时，机器人将出现非线性的振荡现象，对控制系统与测量传感器会造成较为严重的影响，同时会降低球形机器人的运动稳定性。


技术实现要素：

5.针对现有技术中的上述不足，本发明提供的摆式球形机器人姿态扰动补偿控制方法及其系统解决了外部扰动致使摆式球形机器人运动稳定性差的问题。
6.为了达到上述发明目的，本发明采用的技术方案为：
7.第一方面，提供一种摆式球形机器人姿态扰动补偿控制方法，其包括步骤：
8.s1、接收来自遥控端发送的/传感器传输的章动角θ和自转角
9.s2、将章动角θ和自转角与机器人的实时章动角y
θ
和实时自转角求差，得到误差量e
θ
和对误差量e
θ
和进行微分得到微分量和
10.s3、将误差量e
θ
和微分量及误差量和微分量分别输入一个模糊pid控制器，分别得到控制量u
θ
和
11.s4、将u
θ
和组合成传输给摆式球形机器人执行，采集摆式球形机器人的姿态角及角速度，并输出实时章动角y
θ
和实时自转角
12.s5、将姿态角度、角速度和控制量u
pid
发送给非线性扰动观测器，输出估计的扰动
13.s6、将扰动与控制量u
pid
相减得到补偿扰动后的控制量并发送给摆式球形机器人，重复步骤s2～步骤s6，直至完成姿态调整。
14.进一步地，所述摆式球形机器人的动力学方程以矩阵形式的表达式为：
[0015][0016]
其中，q＝[0,0,0,τ1,τ2]
t
为球形机器人的电机输入力矩；τ1,τ2分别为摆式球形机器人内部的驱动电机输出力矩；为q的二阶导数；为球形机器人动力学模型的速度耦合矢量。
[0017]
进一步地，所述q的二阶导数的表达式为：
[0018][0019]
其中，ω为环境扰动。
[0020]
进一步地，所述状态方程f(x)的表达式为：
[0021][0022]
其中，为球形机器人动力学模型的速度耦合矢量。
[0023]
上述技术方案的有益效果为：构建完成球形机器人的非线性控制模型，为后续扰动观测器的建立提供基础。
[0024]
进一步地，所述非线性扰动观测器的数学表达式为：
[0025][0026][0027]
其中，为状态方程；z为实时状态，其等于的积分；和均为扰动观测器的系数函数；ks为对称的系数矩阵；g2(x)＝g1(x)均为中间参数；m(q)为摆式球形机器人动力学模型的惯性矩阵；为扰动；为角速度，q为摆式球形机器人的广义坐标；θ,ψ,α,γ分别为球形机器人球壳的章动角、自转角、进动角、摆锤摆角和内部框架的旋转角；[.]
t
为转置；为q的一阶导数。
[0028]
上述技术方案的有益效果为：针对球形机器人的运动情况构建了基于球形机器人动力学方程的扰动观测器，可实时消除球形机器人运动过程中受到的外部扰动，提升球形机器人的姿态稳定性能，使得球形机器人易于控制。
[0029]
进一步地，采用模糊pid控制器得到控制量u
θ
/的方法包括：
[0030]
模糊控制器接收误差量e
θ
和微分量或者误差量和微分量并进行模糊化处理；
[0031]
结合模糊规则推理对模糊化处理后的数据去模糊化，输出pid控制器中kp、ki、kd三个参数的变化值；
[0032]
pid控制器根据模糊控制器的变化值进行误差控制，输出控制量u
θ
或者控制量
[0033]
上述技术方案的有益效果为：引入模糊控制器与pid控制器，有效提高球形机器人
在不同环境的适应能力，提高控制效果。
[0034]
第二方面，提供一种应用于摆式球形机器人姿态扰动补偿控制方法的姿态扰动控制系统，其包括非线性扰动观测器和至少一个pid模糊控制器；非线性扰动观测器的设计方法为：
[0035]
根据拉格朗日法，构建摆式球形机器人的动力学方程，以矩阵形式的表达式为：
[0036][0037]
其中，q＝[0,0,0,τ1,τ2]
t
为球形机器人的电机输入力矩；τ1,τ2分别为摆式球形机器人内部的驱动电机输出力矩；为q的二阶导数；为球形机器人动力学模型的速度耦合矢量；
[0038]
以摆式球形机器人动力学方程为基础，设计非线性扰动观测器：
[0039][0040][0041]
其中，为状态方程；z为实时状态，其等于的积分；和均为扰动观测器的系数函数；ks为对称的系数矩阵；g2(x)＝g1(x)均为中间参数；m(q)为摆式球形机器人动力学模型的惯性矩阵；为扰动；为角速度，q为摆式球形机器人的广义坐标；θ,ψ,α,γ分别为球形机器人球壳的章动角、自转角、进动角、摆锤摆角和内部框架的旋转角；[.]
t
为转置；为q的一阶导数；
[0042]
设计摆式球形机器人的模糊pid控制器，模糊pid控制器包括模糊控制器和pid控制器，
[0043]
模糊控制器，用于划定误差范围及误差微分量范围，采用高数隶属函数对误差及其微分量的范围进行模糊化处理，划定为'nb'、'nm'、'ns'、'zo'、'ps'、'pm'、'pb'七大类；
[0044]
采用三角形隶属函数构建模kp、ki和kd的糊规则，并依据模糊规则获取三个参数kp、ki和kd的隶属函数对应变量和隶属函数，采用重心法进行去模糊化，得到pid控制器中kp、ki、kd三个参数的变化值；
[0045]
pid控制器，用于根据模糊控制器输出的变化值进行误差控制，输出对摆式球形机器人运动进行控制的控制量。
[0046]
本发明的有益效果为：(1)本方案提供的扰动观测器系统用于控制过程中的扰动补偿，可有效减少在运动过程中系统及外部扰动对摆式球形机器人的影响，提高控制精度，提升运动控制效果。
[0047]
(2)本方案提供的运动与姿态控制系统有效结合了pid控制器的快速反应能力与扰动观测器的抗干扰能力。在复杂环境下，能高效进行运动姿态调节，运动控制，保证了运动控制的稳定性。
附图说明
[0048]
图1为摆式球形机器人姿态扰动补偿控制方法的流程图。
[0049]
图2为摆式球形机器人姿态扰动补偿控制的结构示意图。
[0050]
图3为本方案所应用的球形机器人坐标系示意图。
[0051]
图4为本方案提供的实施例中仿真结果示意图。
具体实施方式
[0052]
下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。
[0053]
第一方面，提供一种摆式球形机器人姿态扰动补偿控制方法，其包括步骤：
[0054]
在步骤s1中，接收来自遥控端发送的/传感器传输的章动角θ和自转角
[0055]
在步骤s2中，将章动角θ和自转角与机器人的实时章动角y
θ
和实时自转角求差，得到误差量e
θ
和对误差量e
θ
和进行微分得到微分量和
[0056]
在步骤s3中，将误差量e
θ
和微分量及误差量和微分量分别输入一个模糊pid控制器，分别得到控制量u
θ
和
[0057]
在步骤s4中，将u
θ
和组合成传输给摆式球形机器人执行，采集摆式球形机器人的姿态角及角速度，并输出实时章动角y
θ
和实时自转角
[0058]
在步骤s5中，将姿态角度、角速度和控制量u
pid
发送给非线性扰动观测器，输出估计的扰动
[0059]
实施时，本方案优选非线性扰动观测器的数学表达式为：
[0060][0061][0062]
其中，为状态方程；z为实时状态，其等于的积分；和均为扰动观测器的系数函数；ks为对称的系数矩阵；g2(x)＝g1(x)均为中间参数；m(q)为摆式球形机器人动力学模型的惯性矩阵；为扰动；为角速度，q为摆式球形机器人的广义坐标；θ,ψ,α,γ分别为球形机器人球壳的章动角、自转角、进动角、摆锤摆角和内部框架的旋转角；[.]
t
为转置；为q的一阶导数。
[0063]
在步骤s6中，将扰动与控制量u
pid
相减得到补偿扰动后的控制量并发送给摆式球形机器人，重复步骤s2～步骤s6，直至完成姿态调整。
[0064]
下面对本方案提供的非线性扰动观测器的详细设计过程进行说明：
[0065]
以球形机器人动力学模型为基础，设计扰动观测器的系数函数l(x)，针对动力学
模型加入已知环境扰动ω并进行矩阵运算，可以得到：
[0066][0067]
选取状态变量构建球形机器人的非线性状态空间方程，可知状态空间方程的数学表达式为：
[0068][0069]
其中，为状态方程，y＝h(x)为输出方程，进一步，将公式3表达式简化表达可知：
[0070][0071]
其中，
[0072]
由此，建立了球形机器人的非线性状态空间方程组，以非线性状态空间方程组为基础，构建估计的扰动与其微分量的关系，设计扰动的估计为：
[0073][0074]
其中，为扰动的观测值，设定ks为对称的系数矩阵，具体为ks＝diag(a,b,
…
)，a、b均为常数。考虑球形机器人运动中主要扰动为外部运动环境的干扰，扰动相对于系统变化缓慢，则定义辅助函数其中，p(x)为扰动观测器的系数函数，取值为对辅助函数求导可得：
[0075][0076]
结合辅助函数，进行简单移项，可构建出基于球形机器人非线性模型的扰动观测器：
[0077][0078]
其中扰动观测器的动态方程为其中扰动观测器的动态方程为
[0079]
实施时，本方案优选涉及到的摆式球形机器人的动力学方程以矩阵形式的表达式为：
[0080]
[0081]
其中，q＝[0,0,0,τ1,τ2]
t
为球形机器人的电机输入力矩；τ1,τ2分别为摆式球形机器人内部的驱动电机输出力矩；为q的二阶导数；为球形机器人动力学模型的速度耦合矢量。
[0082]
摆式球形机器人的动力学方程构建过程中选取摆式球形机器人的球壳的三个欧拉角与内部驱动摆锤的两个转动角为广义坐标，即球形机器人坐标系示意图可以参考图2。
[0083]
在本发明的一个实施例中，模糊pid控制器得到控制量u
θ
/的方法包括：
[0084]
模糊控制器接收误差量e
θ
和微分量或者误差量和微分量并进行模糊化处理；
[0085]
结合模糊规则推理对模糊化处理后的数据去模糊化，输出pid控制器中kp、ki、kd三个参数的变化值；
[0086]
pid控制器根据模糊控制器的变化值进行误差控制，输出控制量u
θ
或者控制量
[0087]
如图3所示，本方案还提供了一种应用于摆式球形机器人姿态扰动补偿控制方法的姿态扰动控制系统，其包括非线性扰动观测器和至少一个pid模糊控制器；如果需要实现多个姿态角度控制，则将多个pid模糊控制器进行并联，输出的多个控制量与非线性扰动观测器输出的多维观测误差进行减法运算，发送给摆式球形机器人以达到消除扰动的目的。
[0088]
其中，非线性扰动观测器的设计方法为：
[0089]
根据拉格朗日法，构建摆式球形机器人的动力学方程，以矩阵形式的表达式为：
[0090][0091]
其中，q＝[0,0,0,τ1,τ2]
t
为球形机器人的电机输入力矩；τ1,τ2分别为摆式球形机器人内部的驱动电机输出力矩；为q的二阶导数；为球形机器人动力学模型的速度耦合矢量；
[0092]
以摆式球形机器人动力学方程为基础，设计非线性扰动观测器：
[0093][0094][0095]
其中，为状态方程；z为实时状态，其等于的积分；和均为扰动观测器的系数函数；ks为对称的系数矩阵；g2(x)＝g1(x)均为中间参数；m(q)为摆式球形机器人动力学模型的惯性矩阵；为扰动；为角速度，q为摆式球形机器人的广义坐标；θ,ψ,α,γ分别为球形机器人球壳的章动角、自转角、进动角、摆锤摆角和内部框架的旋转角；[.]
t
为转置；为q的一阶导数；
[0096]
设计摆式球形机器人的模糊pid控制器，模糊pid控制器包括模糊控制器和pid控制器，
[0097]
模糊控制器，用于划定误差范围及误差微分量范围，采用高数隶属函数对误差及其微分量的范围进行模糊化处理，划定为'nb'、'nm'、'ns'、'zo'、'ps'、'pm'、'pb'七大类；
[0098]
采用三角形隶属函数构建模kp、ki和kd的糊规则，并依据模糊规则获取三个参数kp、ki和kd的隶属函数对应变量和隶属函数，采用重心法进行去模糊化，得到pid控制器中kp、ki、kd三个参数的变化值；
[0099]
pid控制器，用于根据模糊控制器输出的变化值进行误差控制，输出对摆式球形机器人运动进行控制的控制量。
[0100]
下面对模糊pid控制器设计过程进行详细说明：
[0101]
获取到误差量及其微分量后，根据实验经验划定误差范围，ek∈[-50,50],k＝θ、隶属函数选择为高斯型，其表达式为g(x；c,σ)，其中c代表为隶属函数的中心，σ表示为隶属函数的宽度。以高斯型隶属函数为基础，分为'nb'、'nm'、'ns'、'zo'、'ps'、'pm'、'pb'七部分，对误差进行模糊化，根据三个参数的变化量kp、ki、kd∈[-1,1]，采用三角形隶属函数设计三组模糊规则kp、ki和kd，如表1～3所示。
[0102]
表1模糊规则kp
[0103][0104]
表2模糊规则ki
[0105][0106]
表3模糊规则kd
[0107][0108]
根据三个规则获取到三个参数的隶属函数对应变量y
p
、yi、yd和隶属函数u
p
(y)、ui(y)、ud(y)，采用重心法进行去模糊化，对于摆式球形机器人，可用求得输出值，其中u(y)为参数的隶属函数，y为参数的隶属函数对应的变量，然后将输出值赋给pid控制器的三个参数，最终求得pid控制器的三个控制参数。
[0109]
下面结合仿真对本方案的姿态扰动控制方法的效果进行说明：
[0110]
以章动角θ的角速度控制，在球形机器人模型中加入一个随机扰动ω，具体为均值为0，方差为5的连续随机量。模拟球形机器人在强风扰动、凹凸路面扰动下的姿态控制情况。验证模糊pid与线性扰动观测器结合的控制效果(ndob)，分别使用了自抗扰控制(adrc)、模糊pid(fpid)作为对照组，具体仿真可以参考图4。
[0111]
在图4中，4条曲线为球形机器人θ的角速度控制，分别表示为理想控制曲线、在ndob、adrc、fpid控制下的速度控制曲线；通过4条曲线对比可知，模糊pid与非线性扰动观测器结合的控制系统具有较高的扰动抑制效果，且性能优于adrc控制器与模糊pid控制器，从而可知本方案具有球形机器人姿态扰动实时快速补偿等优势。

技术特征：
1.摆式球形机器人姿态扰动补偿控制方法，其特征在于，包括步骤：s1、接收来自遥控端发送的/传感器传输的章动角θ和自转角s2、将章动角θ和自转角与机器人的实时章动角y
θ
和实时自转角求差，得到误差量e
θ
和对误差量e
θ
和进行微分得到微分量和s3、将误差量e
θ
和微分量及误差量和微分量分别输入一个模糊pid控制器，分别得到控制量u
θ
和s4、将u
θ
和组合成传输给摆式球形机器人执行，采集摆式球形机器人的姿态角及角速度，并输出实时章动角y
θ
和实时自转角s5、将姿态角度、角速度和控制量u
pid
发送给非线性扰动观测器，输出估计的扰动s6、将扰动与控制量u
pid
相减得到补偿扰动后的控制量并发送给摆式球形机器人，重复步骤s2～步骤s6，直至完成姿态调整。2.根据权利要求1所述的摆式球形机器人姿态扰动补偿控制方法，其特征在于，所述非线性扰动观测器的数学表达式为：线性扰动观测器的数学表达式为：其中，为状态方程；z为实时状态，其等于的积分；和均为扰动观测器的系数函数；k
s
为对称的系数矩阵；g2(x)＝g1(x)均为中间参数；m(q)为摆式球形机器人动力学模型的惯性矩阵；为扰动；为角速度，q为摆式球形机器人的广义坐标；θ,ψ,α,γ分别为球形机器人球壳的章动角、自转角、进动角、摆锤摆角和内部框架的旋转角；[.]
t
为转置；为q的一阶导数；f(x)为状态方程。3.根据权利要求1所述的摆式球形机器人姿态扰动补偿控制方法，其特征在于，所述摆式球形机器人的动力学方程以矩阵形式的表达式为：其中，q＝[0,0,0,τ1,τ2]
t
为球形机器人的电机输入力矩；τ1,τ2分别为摆式球形机器人内部的驱动电机输出力矩；为q的二阶导数；为球形机器人动力学模型的速度耦合矢量；ω为环境扰动。4.根据权利要求1所述的摆式球形机器人姿态扰动补偿控制方法，其特征在于，所述q的二阶导数的表达式为：其中，ω为环境扰动。5.根据权利要求2所述的摆式球形机器人姿态扰动补偿控制方法，其特征在于，所述状
态方程f(x)的表达式为：其中，为球形机器人动力学模型的速度耦合矢量。6.根据权利要求1-5任一所述的摆式球形机器人姿态扰动补偿控制方法，其特征在于，采用模糊pid控制器得到控制量的方法包括：模糊控制器接收误差量e
θ
和微分量或者误差量和微分量并进行模糊化处理；结合模糊规则推理对模糊化处理后的数据去模糊化，输出pid控制器中kp、ki、kd三个参数的变化值；pid控制器根据模糊控制器的变化值进行误差控制，输出控制量u
θ
或者控制量7.一种应用于权利要求1-6任一所述摆式球形机器人姿态扰动补偿控制方法的姿态扰动控制系统，其特征在于，包括非线性扰动观测器和至少一个pid模糊控制器；非线性扰动观测器的设计方法为：根据拉格朗日法，构建摆式球形机器人的动力学方程，以矩阵形式的表达式为：其中，q＝[0,0,0,τ1,τ2]
t
为球形机器人的电机输入力矩；τ1,τ2分别为摆式球形机器人内部的驱动电机输出力矩；为q的二阶导数；为球形机器人动力学模型的速度耦合矢量；以摆式球形机器人动力学方程为基础，设计非线性扰动观测器：以摆式球形机器人动力学方程为基础，设计非线性扰动观测器：其中，为状态方程；z为实时状态，其等于的积分；和均为扰动观测器的系数函数；k
s
为对称的系数矩阵；g2(x)＝g1(x)均为中间参数；m(q)为摆式球形机器人动力学模型的惯性矩阵；为扰动；为角速度，q为摆式球形机器人的广义坐标；分别为球形机器人球壳的章动角、自转角、进动角、摆锤摆角和内部框架的旋转角；[.]
t
为转置；为q的一阶导数；设计摆式球形机器人的模糊pid控制器，模糊pid控制器包括模糊控制器和pid控制器，模糊控制器，用于划定误差范围及误差微分量范围，采用高数隶属函数对误差及其微分量的范围进行模糊化处理，划定为'nb'、'nm'、'ns'、'zo'、'ps'、'pm'、'pb'七大类；采用三角形隶属函数构建模kp、ki和kd的糊规则，并依据模糊规则获取三个参数kp、ki和kd的隶属函数对应变量和隶属函数，采用重心法进行去模糊化，得到pid控制器中kp、ki、kd三个参数的变化值；pid控制器，用于根据模糊控制器输出的变化值进行误差控制，输出对摆式球形机器人
运动进行控制的控制量。

技术总结
本发明公开了一种球形机器人姿态扰动补偿控制方法及其系统，控制方法包括S1章动角θ和自转角S2将章动角θ和自转角与机器人的实时章动角y


技术研发人员：霍建文 林锐 王启官 杨瑞林
受保护的技术使用者：西南科技大学
技术研发日：2023.07.12
技术公布日：2023/9/20