多移动机器人协同运动控制系统及其控制方法

1.本发明涉及一种用于模块化多移动机器人的分布式控制系统及协同运动控制方法，属于多移动机器人控制技术领域。


背景技术：

2.模块化多移动机器人是指将单个移动机器人作为一个单元模块，并根据新的工作环境需求将相同构型或不同构型的单元模块按照一定排列方式进行组合以满足任务的需求。在模块化多移动机器人研究领域，移动机器人是否具备较强的运动能力以及较高效率与模块化协同运动能力以及响应速度快慢直接相关。其中，协同运动的高同步性与通信低时延又是保证问题的关键所在。在协同运动的过程中需要实现单元模块各自的姿态调整以及组合模块整体的状态调整等动作，因此在移动机器人协同运动研究领域，协同运动是一个具有挑战性的研究方向。
3.针对非结构化地形、松软地面等恶劣野外作业的需求，需要移动机器人具有很强的越障能力和强鲁棒性。单一的轮式移动机器人由于结构及能力的限制，很难满足以上要求，因此多个轮式移动机器人协同运动的概念就被提出。并且针对不同任务需求，可以将多移动机器人按照特定的方式进行组合，形成不同结构的组合体结构，可以适应不同的野外工作环境，增强了整体系统的适应能力。同时由于多移动机器人之间可以通过协同完成高难度动作，所以对于单个模块的性能要求可以降低，可以大大减少系统的成本。
4.虽然将单元模块通过对接形成组合模块运动时可以通过对接机构进行连接，但是对接机构的调整作用是有限的的，如果各单元模块姿态只通过对接机构进行姿态调整，那么随着系统的运行，最终组合模块的运动会趋于发散状态，因此设计一个高同步性的协同运动控制方法十分重要。通过构建合适的运动方法以及运动规划方式对研究强鲁棒性、高效地实现轮式移动器人意义重大：高效性要求各模块之间具备互相合作的运动方法，从何使得多个模块的运动方法具有高度一致性；将单个轮式移动机器人模块化后可以根据各自实现不同的子任务，选择具有不同构型和运动能力的单元模块，提高了整体系统的容错能力，同时降低成本；针对地形变化或者外力带来的扰动，设计的单元模块具有可靠的控制方法，并配有相应的传感器将状态信息进行反馈，实现运动的强鲁棒性。
5.对于目前的多轮式机器人协同运动控制的研究，还存在着以下问题：
6.1)目前多机器人协同研究大部分是针对静态任务，对于未知的复杂环境下存在静态任务及分配只是初步建立模型，并且大多数研究对动态任务进行分配及优化时并没有考虑最优收益，很少涉及到个体以及整体的最优能耗。
7.2)分布式控制系统由于全局缺乏全局时钟性，机器人在执行各自的任务时会出现不同步的问题，降低整体系统效率。


技术实现要素：

8.针对现有多轮式机器人控制技术存在的问题，本发明提供一种能耗低、效率高的
多移动机器人协同运动控制系统，同时提供该系统的控制方法。
9.本发明的多移动机器人协同运动控制系统，采用以下技术方案：
10.所述多移动机器人，包括多个相同构型或不同构型的单元模块，各个单元模块通过对接机构连接在一起，每个单元模块具备四个独立转向和独立运动的轮部执行器，根据不同需求任务构成三角型或方型组合体；
11.所述控制系统包括上位机、工控机、驱动器、惯性测量单元和编码器，每个单元模块具有对应的工控机、驱动器、惯性测量单元和编码器，上位机通过网络通讯协议与各个单元模块的工控机进行通讯，轮部执行器与驱动器连接后与工控机相连，编码器固定在执行器中的驱动电机处，惯性测量单元(imu)置于单元模块的质心部位。
12.编码器用于检测轮部执行器中驱动电机的转角，通过检测到的脉冲数得到当前轮部执行器的前进速度，工控机将当前的前进速度与预设的前进速度给定量做对比，得到驱动电机需要转动的脉冲速度，工控机将处理后的指令发送给驱动器，驱动器不断改变驱动电机的转动速度，使得每个单元模块与其它单元模块都处于相同的稳定运动状态，完成协同运动。
13.惯性测量单元将测得的单元模块位姿信息发送到工控机，并利用单元模块位姿信息与建立的模型一起构建加权范数，通过边界约束将加权范数转换为最优化凸二次规划问题，减小自身移动误差或者受到的外界干扰的影响，实现单元模块位姿的改变，配合其它单元模块更优的进行协同运动。
14.上述系统的控制方法，包括：建立动态任务模型、状态反馈以及协同运动的几何学方法；需要运动时，通过编码器及惯性测量单元将各个单元模块的当前状态及位姿信息进行反馈，并构建离散化后的组合体运动学模型及输入变量的加权范数，利用约束条件将加权范数转换为凸二次规划问题，通过求解当前输入状态的最优解，在实现局部最优解的基础上，将所需转角及速度输入到轮部执行器处实现协同运动；具体包括以下步骤：
15.(1)上位机对各个单元模块的工控机下达指令与时钟时间戳；
16.(2)工控机接收并处理指令，利用时间戳与自身时钟进行对比，减低通信延迟后向驱动器下达运动控制指令；单元模块根据不同的任务建立不同的运动模型(动态任务模型)，包括阿克曼几何转向模型以及预测模型；
17.(3)驱动器接收指令并控制轮部执行电机运动；
18.(4)编码器探测轮部执行器转角以及前进脉冲通过滤波器处理后反馈到驱动器；
19.(5)惯量测量单元与驱动器分别将测得的单元模块姿态信息与轮部执行器信息反馈到工控机；
20.(6)工控机根据建立的动态任务运动模型对反馈信息进行处理，更新当前状态，并重复步骤(3)-(6)，直到下一次上位机指令的到来。
21.所述阿克曼几何转向模型要解决的问题是转向与自身姿态调整；
22.由于单元模块采取四驱四转向且各执行器之间无连杆的模式，所以采取两种阿克曼几何转向方式：
①
将几何圆心o定于轮式移动机器人中心轴的延长线上，四个轮部执行器均执行转角操作以及前进的动作；
②
将几何圆心o定于轮式移动机器人后侧轮部执行器连线的延长线上，前侧轮部执行器执行转角操作以及前进的动作而后侧轮部执行器只执行前进的动作；不论采取哪种方式，轮部执行器求得转角的基本公式为：
[0023][0024]
其中l为轮部执行器中心到单元模块中心轴延长线的距离，记交点为c，w为几何圆心o到交点c的距离；根据组合体不同的运动选择，单元模块的运动几何学采用以下两种方式进行区分；
[0025]
选取方式
①
的运动模型，适用于单元模块在组合体进行转向时的运动，为了保证整体运动时各单元模块要保证自身的相对位置不会发生改变，各单元模块的几何转向的圆心o应该选取组合体中心轴延长线上，通过使用ij代表第i行第j列的轮部执行器的表示方法，对应的轮部执行器转角由以下公式计算得出：
[0026][0027][0028][0029]
其中r为转向几何中心延伸出来的半径，wr为单元模块的宽度，wb为两个单元模块横向之间的距离，lr为单元模块的长度，lb为两个单元模块纵向之间的距离；为了满足运动要求，除了求得轮部执行器转角之外，还需求得轮部执行器的前进速度；由于整体运动时各单元模块要保证自身位置不发生改变，因此各单元模块角速度是一致的，各单元模块的轮部执行器通过对转角角度求导获得角速度执行器的线速度映射为将最大线速度v
max
作为归一化单位，进行归一化处理获得各执行器的给定速度
[0030]
选取方式
②
的运动模型，适用于组合体进行低速直线运动时各单元模块的姿态调整；在组合体执行低速直线运动时，每个单元模块通过惯性测量单元(imu)反馈获得运动过程中的偏航角误差此时需要采用的几何转向进行自身姿态的独立调整，使得单元模块向偏航角反方向运动以弥补误差；通过采取方式
①
的命名方法，各轮部执行器转角表示为：
[0031][0032]
各轮部执行器的执行速度选取偶数行为最大速度，即v
max
＝v
ij
|
i＝2k
，使用最大速度对奇数行轮部执行器进行归一化，则执行转角动作的轮部执行器速度表达规则为：
[0033][0034]
所述预测模型，利用轮部执行器的四驱特性使用差速弥补单元模块之间的协调误差；首先对使用的符号进行说明：p＝[p
x p
y pz]
t
表示单元模块质心位置，其一次导数及二次导数分别代表质心速度与加速度；m为单元模块重量；g为重力加速度；i为机器人的转动惯量；ω＝[φ θ ψ]
t
为机器人姿态角，分别代表横滚角、俯仰角以及偏航角；ω＝[ω
x ω
y ωz]
t
表示单元模块自身角速度；r为机身到世界坐标系的旋转矩阵；fi为当前单元模块第i个轮部执行器；
[0035]
首先分析单元模块的动力特性，得到单元模块简化动力学模型为：
[0036][0037]
得到动力学模型之后，构建状态空间函数以对系统状态进行预测控制，选取状态变量控制变量输出变量为于是可以建立系统的状态空间表达式：
[0038][0039]
将常数项合并到系数矩阵当中并离散化，得：
[0040][0041]
根据离散化递推公式，构建单元模块n个时刻的状态
[0042]
xn＝a
qp
x[k]+b
qp
u，
[0043]
通过构建n时刻的状态xn与需要的状态x
des
、输入变量的最小加权范数，加上边界约束将最小加权范数转换为凸二次规划问题标准形式，即：
[0044][0045]
s.t.-μmg≤f
ix
≤μmg
[0046]-μmg≤f
iy
≤μmg
[0047]fz
≤mg，
[0048]
使用在线二次规划求解器qpoases求出所需的最优电机力f，再通过将最优电机力转换为轮部执行器最优输出速度。
[0049]
本发明增强了多单元模块协同运动的同步性和鲁棒性，可以实现多个移动单元模块之间的协同运动，且针对不同任务可以实现不同构型模块的组合，具有较高的容错性。在针对非结构化地面的地形运动时，在对接结构被动柔顺的基础上实现各模块自身调整的主动柔顺，有效减少了对系统的影响，有较强的外界环境运动适应能力。
附图说明
[0050]
图1是本发明中所采用的多移动机器人的组合构型示意图。
[0051]
图2是上位机实现对各个单元模块控制的流程图。
[0052]
图3是本发明控制方法的流程图。
[0053]
图4是单元模块阿克曼几何转向示意图。
[0054]
图5是单元模块姿态调整几何示意图。
具体实施方式
[0055]
本发明中所采用的多移动机器人组合构型如图1所示，各个相同构型或不同构型的单元模块通过对接机构连接在一起，每个单元模块具备四个独立转向和独立运动的轮部执行器，根据不同需求任务构成三角型或方型组合体。所用组合只用于解释本发明，并非用于限定本发明的范围。
[0056]
各单元模块之间通过对接机构进行组合，所述控制系统包括上位机、工控机、驱动器、惯性测量单元和编码器，每个单元模块具有对应的工控机、驱动器、惯性测量单元和编码器，上位机通过网络通讯协议与各个单元模块的工控机进行通讯，轮部执行器与驱动器
连接后与工控机相连，编码器固定在执行器中的驱动电机处，惯性测量单元置于单元模块的质心部位。编码器用于检测轮部执行器中驱动电机的转角，通过检测到的脉冲数得到当前轮部执行器的前进速度，工控机将当前的前进速度与预设的前进速度给定量做对比，得到驱动电机需要转动的脉冲速度，工控机将处理后的指令发送给驱动器，驱动器不断改变驱动电机的转动速度，使得每个单元模块与其它单元模块都处于相同的稳定运动状态，完成协同运动。惯性测量单元将测得的单元模块位姿信息发送到工控机，并利用单元模块位姿信息与建立的模型一起构建加权范数，通过边界约束将加权范数转换为最优化凸二次规划问题，减小自身移动误差或者受到的外界干扰的影响，实现单元模块位姿的改变，配合其它单元模块更优的进行协同运动。
[0057]
本发明的控制方法，包括：建立动态任务模型、状态反馈以及协同运动的几何学方法；需要运动时，通过编码器及imu将各个单元模块的当前状态及位姿信息进行反馈，并构建离散化后的组合体运动学模型及输入变量的加权范数，利用约束条件将加权范数转换为凸二次规划问题，通过求解当前系统输入状态的最优解，在实现局部最优解的基础上，在实现局部最优解的基础上，将所需转角及速度输入到轮部执行器处实现协同运动。
[0058]
上述控制方法具体包括以下步骤：
[0059]
(1)上位机对各个单元模块的工控机下达指令与时钟时间戳
[0060]
(2)工控机接收并处理指令，利用时间戳与自身时钟进行对比，减低通信延迟后向驱动器下达运动控制指令。
[0061]
(3)驱动器接收指令并控制轮部执行电机运动。
[0062]
(4)编码器探测轮部执行器转角以及前进脉冲通过滤波器处理后反馈到驱动器。
[0063]
(5)惯量测量单元与驱动器分别将测得的单元模块姿态信息与轮部执行器信息反馈到工控机。
[0064]
(6)工控机根据建立的动态任务运动模型对反馈信息进行处理，更新当前状态，并重复步骤(3)-(6)，直到下一次上位机指令的到来。
[0065]
上位机实现对各个单元模块控制流程如图2所示。图3给出了本发明控制方法的流程。
[0066]
在步骤(2)及步骤(6)中，单元模块需要根据不同的任务建立不同的运动模型，包括阿克曼几何转向模型以及预测模型。建立阿克曼几何转向的运动几何学如图4和图5所示，单元模块采取的阿克曼几何转向要解决的问题是转向与自身姿态调整，下面进行具体分析。
[0067]
由于单元模块采取四驱四转向且各执行器之间无连杆的模式，所以可以采取两种阿克曼几何转向方式：
①
将几何圆心o定于轮式移动机器人中心轴的延长线上，四个轮部执行器均执行转角操作以及前进的动作；
②
将几何圆心o定于轮式移动机器人后侧轮部执行器连线的延长线上，前侧轮部执行器执行转角操作以及前进的动作而后侧轮部执行器只执行前进的动作。不论采取哪种方式，轮部执行器求得转角的基本公式为：
[0068][0069]
其中l为轮部执行器中心到单元模块中心轴延长线的距离，记交点为c，w为几何圆心o到交点c的距离。
[0070]
根据组合体不同的运动选择，单元模块的运动几何学采用以下两种方式进行区分。
[0071]
选取方式
①
的运动模型一般适用于单元模块在组合体进行转向时的运动，此时为了保证整体运动时各单元模块要保证自身的相对位置不会发生改变，各单元模块的几何转向的圆心o应该选取组合体中心轴延长线上，通过使用ij代表第i行第j列的轮部执行器的表示方法，对应的轮部执行器转角可以由以下公式计算得出：
[0072][0073][0074][0075]
其中r为转向几何中心延伸出来的半径，wr为单元模块的宽度，wb为两个单元模块横向之间的距离，lr为单元模块的长度，lb为两个单元模块纵向之间的距离。为了满足运动要求，除了求得轮部执行器转角之外，还需求得轮部执行器的前进速度。由于整体运动时各单元模块要保证自身位置不发生改变，因此各单元模块角速度是一致的，各单元模块的轮部执行器可以通过对转角角度求导获得角速度执行器的线速度既可以映射为将最大线速度v
max
作为归一化单位，进行归一化处理获得各执行器的给定速度
[0076]
选取方式
②
的运动模型一般适用于组合体进行低速直线运动时各单元模块的姿态调整。在组合体执行低速直线运动时，每个单元模块可以通过imu反馈获得运动过程中的偏航角误差此时需要采用的几何转向进行自身姿态的独立调整，使得单元模块向偏航角反方向运动以弥补误差。通过采取方式
①
的命名方法，各轮部执行器转角可以表示为：
[0077][0078]
各轮部执行器的执行速度选取偶数行为最大速度，即v
max
＝v
ij
|
i＝2k
，使用最大速度
对奇数行轮部执行器进行归一化，则执行转角动作的轮部执行器速度表达规则为：
[0079][0080]
在组合体执行协同直线运动的高速阶段时，采取的几何转向方式由于自身频繁改变转角对协同产生影响，因此需要选取扰动更小的定转角运动方式，工控机此时需要建立预测模型，利用轮部执行器的四驱特性使用差速弥补单元模块之间的协调误差。首先对使用的符号进行说明：p＝[p
x p
y pz]
t
表示轮式机器人质心位置，其一次导数及二次导数分别代表质心速度与加速度；m为轮式机器人重量；g为重力加速度；i为机器人的转动惯量；ω＝[φ θ ψ]
t
为机器人姿态角，分别代表横滚角、俯仰角以及偏航角；ω＝[ω
x ω
y ωz]
t
表示轮式机器人自身角速度；r为机身到世界坐标系的旋转矩阵；fi为当前单元模块第i个轮部执行器。
[0081]
首先分析单元模块的动力特性，可得单元模块简化动力学模型为：
[0082][0083]
得到动力学模型之后，需要构建状态空间函数以对系统状态进行预测控制，选取状态变量控制变量输出变量为于是可以建立系统的状态空间表达式：
[0084][0085]
将常数项合并到系数矩阵当中并离散化可得：
[0086][0087]
根据离散化递推公式，可以构建单元模块n个时刻的状态
[0088]
xn＝a
qp
x[k]+b
qp
u，
[0089]
通过构建n时刻的状态xn与需要的状态x
des
、输入变量的最小加权范数，加上边界
约束可以将其转换为凸二次规划问题标准形式，即：
[0090][0091]
s.t.-μmg≤f
ix
≤μmg
[0092]-μmg≤f
iy
≤μmg
[0093]fz
≤mg，
[0094]
使用在线二次规划求解器qpoases可以求出所需的最优电机力，再通过可以将最优电机力转换为轮部执行器最优输出速度。