CN114089668B - 一种集成式移动机器人分布式控制方法 - Google Patents
一种集成式移动机器人分布式控制方法 Download PDFInfo
- Publication number
- CN114089668B CN114089668B CN202111382615.1A CN202111382615A CN114089668B CN 114089668 B CN114089668 B CN 114089668B CN 202111382615 A CN202111382615 A CN 202111382615A CN 114089668 B CN114089668 B CN 114089668B
- Authority
- CN
- China
- Prior art keywords
- wheel
- control driver
- control
- target
- driving motor
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 23
- 230000033001 locomotion Effects 0.000 claims abstract description 66
- 238000004891 communication Methods 0.000 claims abstract description 35
- 238000004364 calculation method Methods 0.000 claims abstract description 20
- 239000013598 vector Substances 0.000 claims description 92
- 230000008859 change Effects 0.000 claims description 5
- 230000003993 interaction Effects 0.000 claims description 4
- 230000003068 static effect Effects 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 4
- 238000001514 detection method Methods 0.000 claims description 3
- 230000008569 process Effects 0.000 claims description 2
- 238000013461 design Methods 0.000 abstract description 11
- 238000006243 chemical reaction Methods 0.000 description 5
- 230000005540 biological transmission Effects 0.000 description 2
- 238000000354 decomposition reaction Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/04—Programme control other than numerical control, i.e. in sequence controllers or logic controllers
- G05B19/042—Programme control other than numerical control, i.e. in sequence controllers or logic controllers using digital processors
- G05B19/0423—Input/output
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/20—Pc systems
- G05B2219/24—Pc safety
- G05B2219/24215—Scada supervisory control and data acquisition
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
Abstract
本发明公开了一种集成式移动机器人分布式控制方法，将传统运动控制器的集中解算分散至各电机的伺服驱动器中，形成分布式控制，大幅降低了运算量，提高了实时控制性能，且控制不受轮子数量限制，针对分布式控制设计，本发明在传统伺服驱动器硬件上，集成了单个车轮运动学解算和常规电机伺服控制功能，实现了传统运动控制器与伺服驱动器的一体化设计，形成了控制驱动器，与导航单元一起构成轮式移动机器人控制系统，将传统导航单元、运动控制器、伺服驱动器三层控制系统结构简化为导航单元、控制驱动器两层结构，降低了系统复杂度，提高了系统可靠性；导航单元与各控制驱动器之间采用EtherCAT与CAN双总线冗余通信，提高了通信可靠性。
Description
技术领域
本发明涉及移动机器人控制技术，特别是涉及一种集成式移动机器人分布式控制方法。
背景技术
目前现有的轮式移动机器人的控制实现方式主要是基于导航单元给出的车体运动位姿轨迹，通过以太网将轮式移动机器人运动位姿轨迹信息传递给运动控制器，运动控制器通过轮式移动机器人运动学模型，集中解算得到每个车轮的目标速度矢量曲线，进而得到该车轮所对应驱动电机目标角速度曲线和转向电机目标角度曲线，通过EtherCAT将驱动电机目标角速度曲线和转向电机目标角度曲线指令发送到电机伺服驱动器。由于传统方法需集中解算所有车轮的目标速度矢量曲线，因此对运动控制器CPU性能要求较高，并使得电控部分功耗较大，降低了移动机器人续航时间，且随着轮子数量增加，运动学模型更加复杂，集中解算量剧烈增加，将难以满足控制要求，且通信方式单一，一旦通讯线路状态故障，会导致移动机器人控制失效。
发明内容
针对现有技术中存在的缺陷，本发明的目的在于提供一种集成式移动机器人分布式控制方法，降低了系统复杂度和制造成本，增强了系统可靠性。
为了达到上述目的，本发明提供了一种集成式移动机器人分布式控制方法，硬件包括设置导航单元的四轮机器人，所述机器人的每一车轮分别设置两组集成式运动控制器与伺服驱动器；所述两组集成式运动控制器与伺服驱动器分别用于转向电机和驱动电机的控制；所述导航单元通过EtherCAT与CAN双总线分别与每一组集成式运动控制器与伺服驱动器冗余通信。所述机器人的行走过程为：
在1个通讯周期内，EtherCAT主站发送运动位姿信息给各个所述集成式运动控制器与伺服驱动器，数据帧到达后，每个所述集成式运动控制器与伺服驱动器首先依据运动位姿内容判断通讯线路状态是否正常，若线路通讯状态正常，则选用EtherCAT通讯线路进行通讯；若线路通讯状态故障，则选用备选线路CAN总线进行运动位姿信息的传输；
此后，每一所述集成式运动控制器与伺服驱动器根据接收到运动位姿信息针对所对应的单个车轮基于如下模型进行运动学解算，实时解算当前对应车轮的目标速度矢量，并分解得到相应驱动电机角速度给定值或转向电机角度给定值；
所述模型为：
设t为时间变量，表示随时间增加而变化；
直行模式下：移动机器人沿本体坐标系X轴方向直行，即绕质心旋转角速度矢量Y轴方向速度/>单个车轮的速度矢量均为：
横行模式下：移动机器人沿本体坐标系Y轴方向直行，即绕质心旋转角速度矢量X轴方向速度/>单个车轮的速度矢量均为：
斜行模式下：移动机器人可沿着任意航向角直行，即绕质心旋转角速度矢量每个车轮的速度矢量均为：
四轮转向模式下：移动机器人进行左转或者右转，即绕质心旋转角速度矢量
每个车轮的速度矢量如下，
左前车轮的速度矢量为：
左后车轮的速度矢量为：
右前车轮的速度矢量为：
右后车轮的速度矢量为：
其中L1为质心到前侧两个车轮中心连线的距离，L2为质心到后侧两个车轮中心连线的距离为；W1为质心到到左侧两个车轮中心连线的距离；W2为质心到右侧两个车轮中心连线的距离；
绕车内任意点零半径转向模式下：要求任意点需在左前车轮(L1，W1)，右前车轮(L1，-W2)，左后车轮(-L2,W2),右后车轮(-L2，-W2)四个点所围成的平面内；假设车体所绕任一点A(XA，YA)，每个车轮的速度矢量
左前车轮的速度矢量为:
右前车轮的速度矢量为:
左后车轮的速度矢量为:
右后车轮的速度矢量为:
在运动模式下，每个控制驱动器依据式(3)～式(13)中的单个公式，解算得到所对应的单个车轮目标速度矢量，进而分解得到相应驱动电机目标角速度或转向电机目标角度；
直行模式下：
左前轮1号控制驱动器对应转向电机的目标角度为0°
左前轮2号控制驱动器对应驱动电机的目标角速度为
右前轮3号控制驱动器对应转向电机的目标角度为0°
右前轮4号控制驱动器对应驱动电机的目标角速度为
左后轮5号控制驱动器对应转向电机的目标角度为0°
左后轮6号控制驱动器对应驱动电机的目标角速度为
右后轮7号控制驱动器对应转向电机的目标角度为0°
右后轮8号控制驱动器对应驱动电机的目标角速度为
横行模式下：
左前轮1号控制驱动器对应转向电机的目标角度为左前轮2号控制驱动器对应驱动电机的目标角速度为/>右前轮3号控制驱动器对应转向电机的目标角度为/>右前轮4号控制驱动器对应驱动电机的目标角速度为/>左后轮5号控制驱动器对应转向电机的目标角度为/>左后轮6号控制驱动器对应驱动电机的目标角速度为/>右后轮7号控制驱动器对应转向电机的目标角度为/>右后轮8号控制驱动器对应驱动电机的目标角速度为斜行模式下：
左前轮1号控制驱动器对应转向电机的目标角度为左前轮2号控制驱动器对应驱动电机的目标角速度为/>右前轮3号控制驱动器对应转向电机的目标角度为/>右前轮4号控制驱动器对应驱动电机的目标角速度为左后轮5号控制驱动器对应转向电机的目标角度为/>左后轮6号控制驱动器对应驱动电机的目标角速度为/>右后轮7号控制驱动器对应转向电机的目标角度为/>右后轮8号控制驱动器对应驱动电机的目标角速度为/>四轮转向模式下：
左前轮1号控制驱动器对应转向电机的目标角度为左前轮2号控制驱动器对应驱动电机的目标角速度为
右前轮3号控制驱动器对应转向电机的目标角度为右前轮4号控制驱动器对应驱动电机的目标角速度为
左后轮5号控制驱动器对应转向电机的目标角度为左后轮6号控制驱动器对应驱动电机的目标角速度为
右后轮7号控制驱动器对应转向电机的目标角度为右后轮8号控制驱动器对应驱动电机的目标角速度为
绕车内任意点零半径转向模式：
左前轮1号控制驱动器对应转向电机的目标角度为左前轮2号控制驱动器对应驱动电机的目标角速度为
右前轮3号控制驱动器对应转向电机的目标角度为右前轮4号控制驱动器对应驱动电机的目标角速度为
左后轮5号控制驱动器对应转向电机的目标角度为左后轮6号控制驱动器对应驱动电机的目标角速度为
右后轮7号控制驱动器对应转向电机的目标角度为
右后轮8号控制驱动器对应驱动电机的目标角速度为
其中上述公式t为时间变量，表示随时间增加而变化。
优选方式下，当一个车轮或两个车轮控制驱动器发生故障导致控制失效时，切除对应电机的供电，失效车轮将由主动轮变为从动轮。
此外，所述集成式运动控制器与伺服驱动器采用单片机和FPGA作为主控芯片，所述单片机与所述FPGA之间采用16位高速并行总线FSMC进行信息交互；
所述单片机采集当前电流和编码器反馈信息，根据目标角度或目标角速度信息，实施电流、速度、位置三闭环控制；所述集成式运动控制器与伺服驱动器均采用PID调节器，从而获得两相旋转坐标系下的电压控制量Ud和Uq，再通过PARK逆变换，得到两相静止坐标系下的电压控制量Uα和Uβ，通过高速并行总线传送给所述FPGA，由所述FPGA执行SVPWM调节，输出合成电压矢量，完成对相应驱动电机与转向电机的控制，同时，所述FPGA也完成过流和温度上限报警检测，在电流过大或温度过高时，通过逻辑电路将触发信号进行闭锁，对所述集成式运动控制器与伺服驱动器进行保护。
本发明的有益效果在于：
本发明在传统伺服驱动器硬件上，集成了单个车轮运动学解算和常规电机伺服控制软件功能，实现了传统运动控制器与伺服驱动器的一体化设计，形成了控制驱动器，将传统导航单元、运动控制器、伺服驱动器三层控制系统结构简化为导航单元、控制驱动器两层结构，降低了系统复杂度，提高了系统可靠性；并将传统运动控制器的集中解算分散至各电机的伺服驱动器中，形成分布式控制，每个伺服驱动器仅针对所对应的单个车轮进行运动学解算，且并不一次性计算整个目标速度矢量曲线，而仅实时解算当前驱动电机目标角速度或转向电机目标角度，大幅降低了运算量，提高了实时控制性能，且控制不受轮子数量限制，此外，由于采用分布式控制，当单个电机控制失效时，可切除该电机供电，从而将对应车轮由动力轮切换为随动轮，减小了其对整机控制的影响，增强了系统可靠性；每个控制驱动器采用常规32位微控制器与FPGA相结合的设计，能够根据电流和编码器反馈，采用电流、速度、位置三闭环控制方案，通过SVPWM调节，完成驱动电机角速度控制或转向电机角度控制；导航单元与各控制驱动器之间采用EtherCAT与CAN双总线冗余通信，提高了通信可靠性。
附图说明
图1是移动机器人整体结构框图；
图2是本发明实例中的程序运行主流程图；
图3是本发明实例中的控制驱动器驱动每个电机流程图；
图4是本发明实例中移动机器人平面示意图
具体实施方式
本发明一种集成式移动机器人分布式控制方法，具体包括以下步骤：
S1，将传统运动控制器与伺服驱动器进行一体化设计，形成控制驱动器，将传统三层控制系统结构简化为两层，降低了系统复杂度；
S2，基于控制驱动器，实施了轮式移动机器人分布式控制，提高了实时控制性能，增强了系统可靠性；
S3，采用EtherCAT与CAN双总线冗余通信，提高了通信可靠性；
优选的，步骤S1将传统运动控制器与伺服驱动器进行一体化设计，形成控制驱动器，将传统三层控制系统结构简化为两层，降低了系统复杂度具体指的是：取消了传统移动机器人运动控制器硬件，基于传统伺服驱动器硬件，集成了单个车轮运动学解算和常规电机伺服控制软件功能，实现了传统运动控制器与伺服驱动器的一体化设计，形成了控制驱动器，从而将移动机器人传统的导航单元、运动控制器、伺服驱动器三层控制系统结构简化为导航单元、控制驱动器两层结构。
优选的，步骤S2基于控制驱动器，实施了轮式移动机器人分布式控制，提高了实时控制性能，增强了系统可靠性具体指的是：在轮式移动机器人控制中，将传统由独立运动控制器集中完成的所有车轮目标速度矢量曲线解算，以及每个车轮所对应的驱动电机目标角速度曲线和转向电机目标角度曲线的分解运算，分散至每个控制驱动器中，形成分布式控制，控制驱动器采用常规32位微控制器与FPGA相结合的设计，每个控制驱动器仅针对所对应的单个车轮进行运动学解算，且并不一次性计算整个目标速度矢量曲线，而仅实时解算当前该车轮的目标速度矢量，并分解为驱动电机角速度给定值和转向电机角度给定值。进而根据角速度和角度给定值，以及实时采集的电机编码器反馈值，分别实施驱动电机速度控制和转向电机角度位置控制，具体控制方案则采用电流、速度、位置三闭环控制方式，通过SVPWM调节，完成相应电机控制，当单个电机控制失效时，可切除该电机供电，从而将对应车轮由动力轮切换为随动轮，减小其对整机控制的影响。
依据文献《移动机器人路径规划及跟踪算法研究》(张胜男.移动机器人路径规划及跟踪算法研究[D].黑龙江大学,2021,14-16)的方法，建立移动机器人环境坐标系(Xw(t),Yw(t),θw(t))、移动机器人车体坐标系(X(t),Y(t),θ(t))及其转换关系。给定移动机器人在环境坐标系中的运动位姿轨迹，可求得移动机器人在环境坐标系下的速度为移动机器人在车体坐标系下速度为/>其转换关系为：
根据刚体运动学欧拉定理，移动机器人每个车轮的速度矢量计算公式为：
Vn＝Vm+ω×Rn,n＝1,2,3,4 (2)
其中，Vm为移动机器人质心速度矢量，ω为移动机器人绕质心旋转的角速度矢量，R为车轮几何中心在移动机器人本体坐标系X-Y平面的投影点到质心的位置矢量。
具体，可参考轮式移动机器人共包括四个车轮，每个车轮包含一个驱动电机和一个转向电机，可根据工作环境和工作任务要求采取不同的运动模式，包括：直行模式、横行模式、斜行模式、四轮转向模式和绕车内任意点零半径转向模式。
直行模式下：移动机器人沿本体坐标系X轴方向直行，即绕质心旋转角速度矢量Y轴方向速度/>根据式(2)可得每个车轮的速度矢量均为：
横行模式下：移动机器人沿本体坐标系Y轴方向直行，即绕质心旋转角速度矢量X轴方向速度/>根据式(2)可得每个车轮的速度矢量均为：
斜行模式下：移动机器人可沿着任意航向角直行，即绕质心旋转角速度矢量根据式(2)可得每个车轮的速度矢量均为：
四轮转向模式下：移动机器人进行左转或者右转，即绕质心旋转角速度矢量
据式(2)可计算出每个车轮的速度矢量
左前车轮的速度矢量为：
左后车轮的速度矢量为：
右前车轮的速度矢量为：
右后车轮的速度矢量为：
其中L1为质心到前侧两个车轮中心连线的距离，L2为质心到后侧两个车轮中心连线的距离，W1为质心到左侧两个车轮中心连线的距离，W2为质心到右侧两个车轮中心连线的距离。
绕车内任意点零半径转向模式下：要求任意点需在左前车轮(L1，W1)，右前车轮(L1，-W2)，左后车轮(-L2,W2),右后车轮(-L2，-W2)四个点所围成的平面内。假设车体所绕任一点A(XA，YA)，据式(2)可计算出每个车轮的速度矢量
左前车轮的速度矢量为:
右前车轮的速度矢量为:
左后车轮的速度矢量为:
右后车轮的速度矢量为:
在不同运动模式下，由于每个控制驱动器作为分布式控制中的一个节点，仅对应单个车轮，因此仅需依据式(3)～式(13)中的单个公式，针对单个车轮目标速度矢量进行解算，而无需像传统运动控制器一样，对式(3)～式(13)进行全部集中解算；每个控制驱动器在解算得到单个车轮目标速度矢量后，将进一步分解得到该控制驱动器对应的驱动电机目标角速度或转向电机目标角度。
直行模式下：
左前轮1号控制驱动器对应转向电机的目标角度为0°
左前轮2号控制驱动器对应驱动电机的目标角速度为
右前轮3号控制驱动器对应转向电机的目标角度为0°
右前轮4号控制驱动器对应驱动电机的目标角速度为
左后轮5号控制驱动器对应转向电机的目标角度为0°
左后轮6号控制驱动器对应驱动电机的目标角速度为
右后轮7号控制驱动器对应转向电机的目标角度为0°
右后轮8号控制驱动器对应驱动电机的目标角速度为
横行模式下：
左前轮1号控制驱动器对应转向电机的目标角度为
左前轮2号控制驱动器对应驱动电机的目标角速度为
右前轮3号控制驱动器对应转向电机的目标角度为
右前轮4号控制驱动器对应驱动电机的目标角速度为
左后轮5号控制驱动器对应转向电机的目标角度为
左后轮6号控制驱动器对应驱动电机的目标角速度为
右后轮7号控制驱动器对应转向电机的目标角度为
右后轮8号控制驱动器对应驱动电机的目标角速度为斜行模式下：
左前轮1号控制驱动器对应转向电机的目标角度为
左前轮2号控制驱动器对应驱动电机的目标角速度为
右前轮3号控制驱动器对应转向电机的目标角度为
右前轮4号控制驱动器对应驱动电机的目标角速度为
左后轮5号控制驱动器对应转向电机的目标角度为
左后轮6号控制驱动器对应驱动电机的目标角速度为
右后轮7号控制驱动器对应转向电机的目标角度为
右后轮8号控制驱动器对应驱动电机的目标角速度为
四轮转向模式下：
左前轮1号控制驱动器对应转向电机的目标角度为
左前轮2号控制驱动器对应驱动电机的目标角速度为
右前轮3号控制驱动器对应转向电机的目标角度为
右前轮4号控制驱动器对应驱动电机的目标角速度为
左后轮5号控制驱动器对应转向电机的目标角度为
左后轮6号控制驱动器对应驱动电机的目标角速度为
右后轮7号控制驱动器对应转向电机的目标角度为
右后轮8号控制驱动器对应驱动电机的目标角速度为
绕车内任意点零半径转向模式：
左前轮1号控制驱动器对应转向电机的目标角度为
左前轮2号控制驱动器对应驱动电机的目标角速度为
右前轮3号控制驱动器对应转向电机的目标角度为
右前轮4号控制驱动器对应驱动电机的目标角速度为
左后轮5号控制驱动器对应转向电机的目标角度为
左后轮6号控制驱动器对应驱动电机的目标角速度为
右后轮7号控制驱动器对应转向电机的目标角度为
右后轮8号控制驱动器对应驱动电机的目标角速度为
中上述公式t为时间变量，表示随时间增加而变化。
32位微控制器实时采集当前电流和编码器反馈信息，根据目标角度或目标角速度给定，实施电流、速度、位置三闭环控制，从而获得两相旋转坐标系下的电压控制量Ud和Uq，再通过PARK逆变换，得到两相静止坐标系下的电压控制量Uα和Uβ，传送给FPGA，由FPGA执行SVPWM调节，输出合成电压矢量，完成对相应驱动电机与转向电机的控制，其中，微控制器与FPGA通过高速并行总线进行信息交互。
优选的，步骤S3导航单元与各控制驱动器之间采用EtherCAT网络与CAN总线双通信链路冗余传输，提高了数据传输的可靠性具体指的是
首先判断EtherCAT通讯状态是否正常，如果EtherCAT通讯状态正常，则选择通讯状态正常的EtherCAT线路将车体运动位姿信息实时发送到各个控制驱动器，如果EtherCAT通讯状态故障，则选择备选通讯线路CAN总线将车体运动位姿信息发送到各个控制驱动器。
一个具体实施例，建立移动机器人控制系统；
具体为：选择导航单元，本发明实例采用北斗星空的NPOS220S高性能组合导航系统；自主研发控制驱动器，本发明实例采用ST公司的STM32F407单片机和Xilinx的Spartan6系列的FPGA作为控制驱动器的主控芯片，其中单片机与FPGA之间采用16位高速并行总线FSMC进行信息交互。
NPOS220高性能组合导航系统计算出移动机器人运动位姿轨迹信息；
在1个通讯周期内，EtherCAT主站发送运动位姿信息给各个控制驱动器，数据帧到达控制驱动器后，每个控制驱动器首先依据运动位姿内容判断通讯线路状态是否正常，若线路通讯状态正常，则选用EtherCAT通讯线路进行通讯；若线路通讯状态故障，则选用备选线路CAN总线进行运动位姿信息的传输，控制驱动器接收到运动位姿信息后仅针对所对应的单个车轮进行运动学解算，且并不一次性计算整个目标速度矢量曲线，而仅实时解算当前该车轮的目标速度矢量，并分解得到相应驱动电机角速度给定值或转向电机角度给定值；
控制驱动器采用直流48V供电，经DC-DC变换，为控制驱动器的数字、模拟、驱动等部分提供隔离电源，并采用功率集成模块进行逆变，对永磁同步电机进行控制；
依据文献《移动机器人路径规划及跟踪算法研究》的方法，建立移动机器人环境坐标系(Xw(t),Yw(t),θw(t))、移动机器人车体坐标系(X(t),Y(t),θ(t))及其转换关系。给定移动机器人在环境坐标系中的运动位姿轨迹，可求得移动机器人在环境坐标系下的速度为移动机器人在车体坐标系下速度为/>其转换关系为：上文公式(1)。
根据刚体运动学欧拉定理，移动机器人每个车轮的速度矢量计算公式为：
Vn＝Vm+ω×Rn,n＝1,2,3,4 (2)
其中，Vm为移动机器人质心速度矢量，ω为移动机器人绕质心旋转的角速度矢量，R为车轮几何中心在移动机器人本体坐标系X-Y平面的投影点到质心的位置矢量。
直行模式下：每个车轮的速度矢量均为：
横行模式下：每个车轮的速度矢量均为：
斜行模式下：每个车轮的速度矢量均为：
四轮转向模式下：移动机器人进行左转或者右转，即绕质心旋转角速度矢量
据式(2)可计算出每个车轮的速度矢量
左前车轮的速度矢量为：
左后车轮的速度矢量为：
右前车轮的速度矢量为：
右后车轮的速度矢量为：
其中L1为质心到前侧两个车轮中心连线的距离为0.186m，L2为质心到后侧两个车轮中心连线的距离为0.2m，W1为质心到到左侧两个车轮中心连线的距离为0.2m，W2为质心到右侧两个车轮中心连线的距离为0.32m。
绕车内任意点零半径转向模式下：要求任意点需在左前车轮(L1，W1)，右前车轮(L1，-W2)，左后车轮(-L2,W2),右后车轮(-L2，-W2)四个点所围成的平面内。假设车体所绕任一点A(XA，YA)，据式(2)可计算出每个车轮的速度矢量
左前车轮的速度矢量为:
右前车轮的速度矢量为:
左后车轮的速度矢量为:
右后车轮的速度矢量为:
在不同运动模式下，每个控制驱动器依据式(3)～式(13)中的单个公式，解算得到所对应的单个车轮目标速度矢量，进而分解得到相应驱动电机目标角速度或转向电机目标角度。
直行模式下：
左前轮1号控制驱动器对应转向电机的目标角度为0°
左前轮2号控制驱动器对应驱动电机的目标角速度为
右前轮3号控制驱动器对应转向电机的目标角度为0°
右前轮4号控制驱动器对应驱动电机的目标角速度为
左后轮5号控制驱动器对应转向电机的目标角度为0°
左后轮6号控制驱动器对应驱动电机的目标角速度为
右后轮7号控制驱动器对应转向电机的目标角度为0°
右后轮8号控制驱动器对应驱动电机的目标角速度为
横行模式下：
左前轮1号控制驱动器对应转向电机的目标角度为
左前轮2号控制驱动器对应驱动电机的目标角速度为
右前轮3号控制驱动器对应转向电机的目标角度为
右前轮4号控制驱动器对应驱动电机的目标角速度为
左后轮5号控制驱动器对应转向电机的目标角度为
左后轮6号控制驱动器对应驱动电机的目标角速度为
右后轮7号控制驱动器对应转向电机的目标角度为
右后轮8号控制驱动器对应驱动电机的目标角速度为斜行模式下：
左前轮1号控制驱动器对应转向电机的目标角度为
左前轮2号控制驱动器对应驱动电机的目标角速度为
右前轮3号控制驱动器对应转向电机的目标角度为
右前轮4号控制驱动器对应驱动电机的目标角速度为
左后轮5号控制驱动器对应转向电机的目标角度为
左后轮6号控制驱动器对应驱动电机的目标角速度为/>
右后轮7号控制驱动器对应转向电机的目标角度为
右后轮8号控制驱动器对应驱动电机的目标角速度为
四轮转向模式下：
左前轮1号控制驱动器对应转向电机的目标角度为
左前轮2号控制驱动器对应驱动电机的目标角速度为
右前轮3号控制驱动器对应转向电机的目标角度为
右前轮4号控制驱动器对应驱动电机的目标角速度为
左后轮5号控制驱动器对应转向电机的目标角度为
左后轮6号控制驱动器对应驱动电机的目标角速度为
右后轮7号控制驱动器对应转向电机的目标角度为
右后轮8号控制驱动器对应驱动电机的目标角速度为
绕车内任意点零半径转向模式：
左前轮1号控制驱动器对应转向电机的目标角度为
左前轮2号控制驱动器对应驱动电机的目标角速度为
右前轮3号控制驱动器对应转向电机的目标角度为
/>
右前轮4号控制驱动器对应驱动电机的目标角速度为
左后轮5号控制驱动器对应转向电机的目标角度为
左后轮6号控制驱动器对应驱动电机的目标角速度为
右后轮7号控制驱动器对应转向电机的目标角度为
右后轮8号控制驱动器对应驱动电机的目标角速度为
其中上述公式t为时间变量，表示随时间增加而变化。
STM32F407单片机采集当前电流和编码器反馈信息，根据目标角度或目标角速度信息，实施电流、速度、位置三闭环控制，控制器均采用PID调节器，从而获得两相旋转坐标系下的电压控制量Ud和Uq，再通过PARK逆变换，得到两相静止坐标系下的电压控制量Uα和Uβ，通过高速并行总线传送给FPGA，由FPGA执行SVPWM调节，输出合成电压矢量，完成对相应驱动电机与转向电机的控制，同时，FPGA也完成过流和温度上限报警检测，在电流过大或温度过高时，通过逻辑电路将触发信号进行闭锁，对控制驱动器进行保护。
当移动机器人一个车轮或两个车轮控制驱动器发生故障导致控制失效时，可切除该电机供电，失效车轮将由主动轮变为从动轮，配合未失效车轮继续完成移动机器人运动，最大程度提高了移动机器人的容错能力。
本发明提供了一种集成式移动器人分布式控制方法，具体包括以下内容：传统轮式移动机器人控制，基于导航单元给出的车体运动位姿轨迹，采用独立运动控制器，通过轮式移动机器人运动学模型，集中解算得到每个车轮的目标速度矢量曲线，进而分解得到该车轮所对应驱动电机目标角速度曲线和转向电机目标角度曲线，然后由各电机的伺服驱动器实施具体电机控制；由于传统方法需集中解算所有车轮的目标速度矢量曲线，因此对运动控制器CPU性能要求较高，并使得电控部分功耗较大，降低了移动机器人续航时间，且随着轮子数量增加，运动学模型更加复杂，集中解算量剧烈增加，将难以满足控制要求；为此，本发明将传统运动控制器的集中解算分散至各电机的伺服驱动器中，形成分布式控制，每个伺服驱动器仅针对所对应的单个车轮进行运动学解算，且并不一次性计算整个目标速度矢量曲线，而仅实时解算当前驱动电机目标角速度或转向电机目标角度，大幅降低了运算量，提高了实时控制性能，且控制不受轮子数量限制，此外，由于采用分布式控制，当单个电机控制失效时，可切除该电机供电，从而将对应车轮由动力轮切换为随动轮，减小了其对整机控制的影响，增强了系统可靠性；针对分布式控制设计，本发明在传统伺服驱动器硬件上，集成了单个车轮运动学解算和常规电机伺服控制软件功能，实现了传统运动控制器与伺服驱动器的一体化设计，形成了控制驱动器，与导航单元一起构成轮式移动机器人控制系统，将传统导航单元、运动控制器、伺服驱动器三层控制系统结构简化为导航单元、控制驱动器两层结构，降低了系统复杂度，提高了系统可靠性；每个控制驱动器采用常规32位微控制器与FPGA相结合的设计，能够根据电流和编码器反馈，采用电流、速度、位置三闭环控制方案，通过SVPWM调节，完成驱动电机速度控制或转向电机位置控制；导航单元与各控制驱动器之间采用EtherCAT与CAN双总线冗余通信，提高了通信可靠性。
本发明将传统运动控制器与伺服驱动器进行一体化设计，形成控制驱动器，将传统三层控制系统结构简化为两层，降低了系统复杂度；基于控制驱动器，实施了轮式移动机器人分布式控制，提高了实时控制性能，增强了系统可靠性；采用EtherCAT与CAN双总线冗余通信，提高了通信可靠性。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。
Claims (3)
1.一种集成式移动机器人分布式控制方法，硬件包括设置导航单元的四轮机器人，所述机器人的每一车轮分别设置两组集成式运动控制器与伺服驱动器；所述两组集成式运动控制器与伺服驱动器分别用于转向电机和驱动电机的控制；所述导航单元通过EtherCAT与CAN双总线分别与每一组集成式运动控制器与伺服驱动器冗余通信；
其特征在于，所述机器人的行走过程为：
在1个通讯周期内，EtherCAT主站发送运动位姿信息给各个所述集成式运动控制器与伺服驱动器，数据帧到达后，每个所述集成式运动控制器与伺服驱动器首先依据运动位姿内容判断通讯线路状态是否正常，若线路通讯状态正常，则选用EtherCAT通讯线路进行通讯；若线路通讯状态故障，则选用备选线路CAN总线进行运动位姿信息的传输；
此后，每一所述集成式运动控制器与伺服驱动器根据接收到运动位姿信息针对所对应的单个车轮基于如下模型进行运动学解算，实时解算当前对应车轮的目标速度矢量，并分解得到相应驱动电机角速度给定值或转向电机角度给定值；
所述模型为：
设t为时间变量，表示随时间增加而变化；
直行模式下：移动机器人沿本体坐标系X轴方向直行，即绕质心旋转角速度矢量Y轴方向速度/>单个车轮的速度矢量均为：
横行模式下：移动机器人沿本体坐标系Y轴方向直行，即绕质心旋转角速度矢量X轴方向速度/>单个车轮的速度矢量均为：
斜行模式下：移动机器人可沿着任意航向角直行，即绕质心旋转角速度矢量每个车轮的速度矢量均为：
四轮转向模式下：移动机器人进行左转或者右转，即绕质心旋转角速度矢量
每个车轮的速度矢量如下，
左前车轮的速度矢量为：
左后车轮的速度矢量为：
右前车轮的速度矢量为：
右后车轮的速度矢量为：
其中L1为质心到前侧两个车轮中心连线的距离，L2为质心到后侧两个车轮中心连线的距离为；W1为质心到到左侧两个车轮中心连线的距离；W2为质心到右侧两个车轮中心连线的距离；
绕车内任意点零半径转向模式下：要求任意点需在左前车轮(L1，W1)，右前车轮(L1，-W2)，左后车轮(-L2,W2),右后车轮(-L2，-W2)四个点所围成的平面内；假设车体所绕任一点A(XA，YA)，每个车轮的速度矢量
左前车轮的速度矢量为:
右前车轮的速度矢量为:
左后车轮的速度矢量为:
右后车轮的速度矢量为:
在运动模式下，每个控制驱动器依据式(3)～式(13)中的单个公式，解算得到所对应的单个车轮目标速度矢量，进而分解得到相应驱动电机目标角速度或转向电机目标角度；
直行模式下：
左前轮1号控制驱动器对应转向电机的目标角度为0°
左前轮2号控制驱动器对应驱动电机的目标角速度为
右前轮3号控制驱动器对应转向电机的目标角度为0°
右前轮4号控制驱动器对应驱动电机的目标角速度为
左后轮5号控制驱动器对应转向电机的目标角度为0°
左后轮6号控制驱动器对应驱动电机的目标角速度为
右后轮7号控制驱动器对应转向电机的目标角度为0°
右后轮8号控制驱动器对应驱动电机的目标角速度为
横行模式下：
左前轮1号控制驱动器对应转向电机的目标角度为
左前轮2号控制驱动器对应驱动电机的目标角速度为
右前轮3号控制驱动器对应转向电机的目标角度为
右前轮4号控制驱动器对应驱动电机的目标角速度为
左后轮5号控制驱动器对应转向电机的目标角度为
左后轮6号控制驱动器对应驱动电机的目标角速度为
右后轮7号控制驱动器对应转向电机的目标角度为
右后轮8号控制驱动器对应驱动电机的目标角速度为
斜行模式下：
左前轮1号控制驱动器对应转向电机的目标角度为
左前轮2号控制驱动器对应驱动电机的目标角速度为
右前轮3号控制驱动器对应转向电机的目标角度为
右前轮4号控制驱动器对应驱动电机的目标角速度为
左后轮5号控制驱动器对应转向电机的目标角度为
左后轮6号控制驱动器对应驱动电机的目标角速度为
右后轮7号控制驱动器对应转向电机的目标角度为
右后轮8号控制驱动器对应驱动电机的目标角速度为
四轮转向模式下：
左前轮1号控制驱动器对应转向电机的目标角度为
左前轮2号控制驱动器对应驱动电机的目标角速度为
右前轮3号控制驱动器对应转向电机的目标角度为
右前轮4号控制驱动器对应驱动电机的目标角速度为
左后轮5号控制驱动器对应转向电机的目标角度为
左后轮6号控制驱动器对应驱动电机的目标角速度为
右后轮7号控制驱动器对应转向电机的目标角度为
右后轮8号控制驱动器对应驱动电机的目标角速度为
绕车内任意点零半径转向模式：
左前轮1号控制驱动器对应转向电机的目标角度为
左前轮2号控制驱动器对应驱动电机的目标角速度为
右前轮3号控制驱动器对应转向电机的目标角度为
右前轮4号控制驱动器对应驱动电机的目标角速度为
左后轮5号控制驱动器对应转向电机的目标角度为
左后轮6号控制驱动器对应驱动电机的目标角速度为
右后轮7号控制驱动器对应转向电机的目标角度为
右后轮8号控制驱动器对应驱动电机的目标角速度为
2.根据权利要求1所述集成式移动机器人分布式控制方法，其特征在于，
当一个车轮或两个车轮控制驱动器发生故障导致控制失效时，切除对应电机的供电，失效车轮将由主动轮变为从动轮。
3.根据权利要求1所述集成式移动机器人分布式控制方法，其特征在于，所述集成式运动控制器与伺服驱动器采用单片机和FPGA作为主控芯片，所述单片机与所述FPGA之间采用16位高速并行总线FSMC进行信息交互；所述单片机采集当前电流和编码器反馈信息，根据目标角度或目标角速度信息，实施电流、速度、位置三闭环控制；
所述集成式运动控制器与伺服驱动器均采用PID调节器，从而获得两相旋转坐标系下的电压控制量Ud和Uq，再通过PARK逆变换，得到两相静止坐标系下的电压控制量Uα和Uβ，通过高速并行总线传送给所述FPGA，由所述FPGA执行SVPWM调节，输出合成电压矢量，完成对相应驱动电机与转向电机的控制，同时，所述FPGA也完成过流和温度上限报警检测，在电流过大或温度过高时，通过逻辑电路将触发信号进行闭锁，对所述集成式运动控制器与伺服驱动器进行保护。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111382615.1A CN114089668B (zh) | 2021-11-22 | 2021-11-22 | 一种集成式移动机器人分布式控制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111382615.1A CN114089668B (zh) | 2021-11-22 | 2021-11-22 | 一种集成式移动机器人分布式控制方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114089668A CN114089668A (zh) | 2022-02-25 |
| CN114089668B true CN114089668B (zh) | 2023-11-07 |
Family
ID=80302345
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111382615.1A Active CN114089668B (zh) | 2021-11-22 | 2021-11-22 | 一种集成式移动机器人分布式控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114089668B (zh) |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106394715A (zh) * | 2016-12-02 | 2017-02-15 | 河海大学常州校区 | 一种全向移动轮式机器人平台及控制方法 |
| CN107380145A (zh) * | 2013-03-29 | 2017-11-24 | 日立汽车系统株式会社 | 车辆的运动控制装置 |
| CN108413976A (zh) * | 2018-01-23 | 2018-08-17 | 大连理工大学 | 一种面向多工况的爬壁机器人智能路径规划方法及系统 |
| CN108858165A (zh) * | 2018-07-10 | 2018-11-23 | 大连理工大学 | 新型可重构蛇形机器人及其控制方法 |
| CN109910617A (zh) * | 2019-03-27 | 2019-06-21 | 武汉理工大学 | 一种分布式轮毂电机驱动车辆失效故障的诊断方法 |
| CN110497373A (zh) * | 2019-08-07 | 2019-11-26 | 大连理工大学 | 一种移动作业机器人的三维激光雷达与机械臂间的联合标定方法 |
| CN110794834A (zh) * | 2019-10-25 | 2020-02-14 | 北京星航机电装备有限公司 | 一种基于后驱前转向的搬运汽车控制系统 |
| CN113147896A (zh) * | 2021-03-16 | 2021-07-23 | 江苏汇智高端工程机械创新中心有限公司 | 一种铰接转向分布式电驱动装载机转向控制系统及方法 |
| CN113661114A (zh) * | 2019-04-04 | 2021-11-16 | 蒂森克虏伯工业解决方案股份公司 | 用于经由多个单独速度可控的驱动单元来控制移动底盘组件、特别是设置有履带底盘的移动传送桥系统的移动的方法和装置 |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101554726B (zh) * | 2009-05-15 | 2011-01-19 | 北京工业大学 | 一种柔性两轮自平衡机器人系统及其运动控制方法 |
| US10860023B2 (en) * | 2018-06-25 | 2020-12-08 | Mitsubishi Electric Research Laboratories, Inc. | Systems and methods for safe decision making of autonomous vehicles |
| US11419686B2 (en) * | 2019-09-13 | 2022-08-23 | Verb Surgical Inc. | Trajectory representation in design and testing of a surgical robot |
-
2021
- 2021-11-22 CN CN202111382615.1A patent/CN114089668B/zh active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107380145A (zh) * | 2013-03-29 | 2017-11-24 | 日立汽车系统株式会社 | 车辆的运动控制装置 |
| CN106394715A (zh) * | 2016-12-02 | 2017-02-15 | 河海大学常州校区 | 一种全向移动轮式机器人平台及控制方法 |
| CN108413976A (zh) * | 2018-01-23 | 2018-08-17 | 大连理工大学 | 一种面向多工况的爬壁机器人智能路径规划方法及系统 |
| CN108858165A (zh) * | 2018-07-10 | 2018-11-23 | 大连理工大学 | 新型可重构蛇形机器人及其控制方法 |
| CN109910617A (zh) * | 2019-03-27 | 2019-06-21 | 武汉理工大学 | 一种分布式轮毂电机驱动车辆失效故障的诊断方法 |
| CN113661114A (zh) * | 2019-04-04 | 2021-11-16 | 蒂森克虏伯工业解决方案股份公司 | 用于经由多个单独速度可控的驱动单元来控制移动底盘组件、特别是设置有履带底盘的移动传送桥系统的移动的方法和装置 |
| CN110497373A (zh) * | 2019-08-07 | 2019-11-26 | 大连理工大学 | 一种移动作业机器人的三维激光雷达与机械臂间的联合标定方法 |
| CN110794834A (zh) * | 2019-10-25 | 2020-02-14 | 北京星航机电装备有限公司 | 一种基于后驱前转向的搬运汽车控制系统 |
| CN113147896A (zh) * | 2021-03-16 | 2021-07-23 | 江苏汇智高端工程机械创新中心有限公司 | 一种铰接转向分布式电驱动装载机转向控制系统及方法 |
Non-Patent Citations (3)
| Title |
|---|
| 基于模糊规划器的轮式移动机器人避障研究;丁吉;段锁林;任富强;姜峰;;常州大学学报(自然科学版)(04);全文 * |
| 李荣华 ; 褚金奎 ; 王洪青 ; 李庆瀛 ; .机器人分层分布式控制系统设计与实现.微计算机信息.2008,(第32期),全文. * |
| 罗翔,颜景平.一种新颖的冗余度机器人及其视觉伺服方案.东南大学学报(自然科学版).2001,(第05期),全文. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114089668A (zh) | 2022-02-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11079772B2 (en) | Vehicle control method and control system | |
| CN107167141A (zh) | 基于双一线激光雷达的机器人自主导航系统 | |
| Soetanto et al. | Adaptive, non-singular path-following control of dynamic wheeled robots | |
| CN109397244B (zh) | 一种一体化双7自由度机械臂全向移动机器人系统与控制方法 | |
| CN102039589B (zh) | 模块化的灾害救援机器人 | |
| CN110989526A (zh) | 一种双agv协同运载控制系统及方法 | |
| CN104950885A (zh) | 一种基于视觉和力觉反馈的uav群双边遥操作控制系统及其方法 | |
| CN106444754A (zh) | 一种轮式探测移动机器人控制系统及控制方法 | |
| CN107505941B (zh) | 四轮独立驱动独立转向电动车的集中-分布式控制系统 | |
| CN103223940A (zh) | 一种电动汽车协调控制系统 | |
| CN106527439A (zh) | 一种运动控制方法和装置 | |
| CN201525026U (zh) | 模块化的灾害救援机器人 | |
| CN114089668B (zh) | 一种集成式移动机器人分布式控制方法 | |
| CN111679676A (zh) | 一种agv运动轨迹控制方法 | |
| CN107045342B (zh) | 一种基于三轮全向移动的交互式自主导引系统及方法 | |
| CN102126220A (zh) | 一种基于现场总线仿人机器人六自由机械臂的控制系统 | |
| CN114872782B (zh) | 四轮全向底盘控制方法及系统 | |
| Lam et al. | Omni-directional steer-by-wire interface for four wheel independent steering vehicle | |
| CN114779766B (zh) | 一种自主避障陆空两栖装置及其控制方法 | |
| CN112462753B (zh) | 车-蛇复合式变结构移动机器人的运动学建模方法 | |
| Sen et al. | A ROS-based smooth motion planning scheme for a home service robot | |
| Wang et al. | A unified approach to inverse and direct kinematics for four kinds of wheeled mobile robots and its applications | |
| Wu et al. | Path tracking controller design of automatic guided vehicle based on four-wheeled omnidirectional motion model | |
| CN114237221A (zh) | 基于中心映射的低延迟组合体机器人运动控制系统及方法 | |
| CN114665751A (zh) | 一种基于EtherCAT通信的控制系统、方法及双轴驱动控制装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |