CN101554726A - Flexible two-wheel self-balance robot system and motion control method thereof - Google Patents

Abstract

Translated fromChinese

一种柔性两轮自平衡机器人系统及其运动控制方法，包括主控制器(1)、运动控制器(2)、辅助控制器(3)、伺服驱动器(4)、传感器、输入输出设备、直流电机(5)和电源系统；躯干顶板(11)上设有摄像头(12)，躯干(6)和底盘(8)均装有倾角仪、陀螺仪，躯干(6)和底盘(8)之间以装有双臂扭簧(25)和圆柱弹簧(26)的柔性关节(7)连接；主控制器(1)与运动控制器(2)、辅助控制器(3)、输入输出设备连接。主控制器(1)接收的用户指令为即时指令或预存指令；通过主控制器(1)运行运动行为决策算法，运动控制器(2)运行运动平衡控制算法，伺服驱动器(4)对电机(5)的转矩控制，实现机器人平面行走和遥控。

A flexible two-wheel self-balancing robot system and its motion control method, including a main controller (1), a motion controller (2), an auxiliary controller (3), a servo driver (4), sensors, input and output devices, DC Motor (5) and power system; Trunk roof (11) is provided with camera (12), and trunk (6) and chassis (8) are all equipped with inclinometer, gyroscope, between trunk (6) and chassis (8) It is connected with a flexible joint (7) equipped with double-arm torsion springs (25) and cylindrical springs (26); the main controller (1) is connected with a motion controller (2), an auxiliary controller (3), and input and output devices. The user instructions received by the main controller (1) are immediate instructions or pre-stored instructions; the main controller (1) runs the motion behavior decision-making algorithm, the motion controller (2) runs the motion balance control algorithm, and the servo drive (4) controls the motor ( 5) Torque control to realize robot plane walking and remote control.

Description

Translated fromChinese

一种柔性两轮自平衡机器人系统及其运动控制方法A flexible two-wheel self-balancing robot system and its motion control method

技术领域technical field

本发明涉及一种两轮移动式机器人系统及其运动控制方法，为工科院校的控制理论课程和机器人设计课程提供一种实验研究平台。The invention relates to a two-wheel mobile robot system and a motion control method thereof, and provides an experimental research platform for control theory courses and robot design courses of engineering colleges.

背景技术Background technique

两轮自平衡机器人，又称移动式倒立摆，它的行走机构为轴心相对、平行连接于机体两侧的轮子。该种机器人兼有移动式机器人的离散时间动作决策问题和倒立摆的连续时间姿态控制问题。因而，它可成为机器人学和控制科学领域交叉的综合研究对象和理想示教设备。Two-wheeled self-balancing robot, also known as mobile inverted pendulum, its traveling mechanism is the wheels with opposite axes and parallel connection on both sides of the body. This kind of robot combines the discrete-time action decision-making problem of mobile robot and the continuous-time attitude control problem of inverted pendulum. Therefore, it can become a comprehensive research object and an ideal teaching device for the intersection of robotics and control science.

现有的两轮自平衡机器人的机身整体为刚性构造，例如专利ZL200510094939.X中所述机器人。从模仿人类身体来设计机器人的角度来说，刚性机体不能体现出人类躯体，尤其是腰部结构所具有的前后俯仰运动中的柔性(弹性)。然而，完全真实仿照人体，利用人工肌肉和人工关节软组织设计机器人难度大、成本高、控制复杂、不易维护，作为仅针对柔性机体机器人的平衡控制和运动控制问题的研究和示教设备并不实用。The fuselage of the existing two-wheeled self-balancing robot is a rigid structure as a whole, such as the robot described in patent ZL200510094939.X. From the perspective of designing a robot by imitating the human body, the rigid body cannot reflect the flexibility (elasticity) in the front and rear pitching motions of the human body, especially the waist structure. However, completely imitating the human body, using artificial muscles and artificial joints to design robots is difficult, costly, complicated to control, and difficult to maintain. It is not practical as a research and teaching device for balance control and motion control of flexible robots. .

针对上述问题，专利ZL200720103279.1设计了一种柔性两轮直立式机器人本体。该机器人引入一段圆柱弹簧连接实现柔性机身，这种设计虽然简单易行，但是存在一些不足。首先，单纯的圆柱弹簧连接方式使该机器人柔性段具有无限自由度，其中绝大部分不可控制，例如左右晃动自由度，这对于两轮直立式机器人姿态控制的研究并没有显著意义，反而带来很多不必要的麻烦。其次，在柔性关节高度不变的情况下，圆柱弹簧材料直径和圈数变化范围十分有限，直接制约柔性关节刚度值的取值范围，使机器人作为示教设备在操作性和参数调节范围上略显不足。此外，专利ZL200720103279.1仅涉及该类机器人的机械本体结构，并没有给出机器人适用的电气系统和姿态平衡控制方法。Aiming at the above problems, patent ZL200720103279.1 designed a flexible two-wheel upright robot body. The robot introduces a cylindrical spring connection to realize a flexible body. Although this design is simple and feasible, it has some shortcomings. First of all, the pure cylindrical spring connection method makes the flexible segment of the robot have infinite degrees of freedom, most of which are uncontrollable, such as the degree of freedom of shaking left and right, which has no significant significance for the research on the attitude control of two-wheeled upright robots. A lot of unnecessary trouble. Secondly, under the condition that the height of the flexible joint remains constant, the diameter and number of coils of the cylindrical spring material vary in a very limited range, which directly restricts the value range of the stiffness value of the flexible joint, making the robot as a teaching device less operable and parameter adjustment range. Insufficient. In addition, the patent ZL200720103279.1 only involves the mechanical body structure of this type of robot, and does not provide the applicable electrical system and posture balance control method for the robot.

发明内容Contents of the invention

本发明的目的在于提出一种机身具有柔性俯仰关节的两轮自平衡机器人系统及其运动控制方法。该机器人系统是一种开放式智能机器人研究开发平台，为控制科学和机器人学的研究和教学提供实验对象；运动控制方法实现该机器人的基本功能，为用户开发、使用该机器人提供参考。The object of the present invention is to propose a two-wheel self-balancing robot system with flexible pitch joints and a motion control method thereof. The robot system is an open intelligent robot research and development platform, which provides experimental objects for the research and teaching of control science and robotics; the motion control method realizes the basic functions of the robot, and provides a reference for users to develop and use the robot.

本发明是采用以下技术手段实现的：The present invention is realized by adopting the following technical means:

一种柔性两轮自平衡机器人系统，包括电气系统和机械本体；其中，所述电气系统包括主控制器1、运动控制器2、辅助控制器3、电机伺服驱动器4、传感器、输入输出设备、直流电机5和电源系统；所述机械本体包括躯干6、柔性关节7、底盘8、保护支架9和双轮10；A flexible two-wheel self-balancing robot system, including an electrical system and a mechanical body; wherein, the electrical system includes amain controller 1, amotion controller 2, anauxiliary controller 3, amotor servo driver 4, sensors, input and output devices, ADC motor 5 and a power supply system; the mechanical body includes atorso 6, aflexible joint 7, achassis 8, aprotective bracket 9 and twowheels 10;

躯干6为层式框架，顶板11上固定有至少一个摄像头12，顶板11和底板13之间由隔板14分隔成若干层，躯干安装倾角仪15、陀螺仪16和超声波传感器17；所述躯干底板13和底盘8之间以柔性关节7连接；底盘8为箱式结构，在其内部装载倾角仪18、陀螺仪19，直流电机5分别固定在底盘8两侧内壁上，电机5的轴一端与轮子10的轴联接，另一端接编码器20；底盘8底面安装有可拆卸保护支架9，支架9脚轮21；双轮10的轴心位于一条直线上；Thetorso 6 is a layered frame, and at least onecamera 12 is fixed on thetop plate 11, and thetop plate 11 and thebase plate 13 are separated into several layers by apartition 14, and theinclinometer 15, thegyroscope 16 and theultrasonic sensor 17 are installed on the torso; Thebase plate 13 and thechassis 8 are connected byflexible joints 7; thechassis 8 is a box structure, and aninclinometer 18 and agyroscope 19 are loaded inside it, and theDC motors 5 are respectively fixed on the inner walls of both sides of thechassis 8. It is connected to the shaft of thewheel 10, and the other end is connected to theencoder 20; the bottom surface of thechassis 8 is equipped with a detachableprotective bracket 9, and thebracket 9 casters 21; the axes of thedouble wheels 10 are located on a straight line;

主控制器1为嵌入式计算机系统；Themain controller 1 is an embedded computer system;

运动控制器2选用数字信号处理器系统；Themotion controller 2 selects a digital signal processor system;

辅助控制器3选用单片机、DSP嵌入式系统；Auxiliary controller 3 selects single-chip microcomputer, DSP embedded system;

传感器包括：检测机器人躯干6和底盘8俯仰倾角变化的倾角仪15、18，倾角速度变化的陀螺仪16、19，检测电机5转角变化的编码器20，检测图像信息的摄像头12，以及检测障碍物距离信息的超声波传感器17；The sensors include:inclinometers 15, 18 for detecting changes in pitch and inclination of therobot torso 6 andchassis 8,gyroscopes 16 and 19 for inclination velocity changes,encoders 20 for detecting changes in the rotation angle of themotor 5,cameras 12 for detecting image information, and detecting obstaclesUltrasonic sensor 17 for object distance information;

柔性关节7的顶端和底端为上、下支承圆盘22、23，剖面为“凸”字形，圆盘中心的通孔用于穿过电线；上、下支承圆盘以两个同心转动轴24铰接，两个转动轴24向外的延伸部分，各固定双臂扭簧25，扭簧25的上臂固定在上支承圆盘22上，扭簧的下臂固定在下支承圆盘23上；在上、下支撑圆盘22、23之间压紧固定一段圆柱弹簧26，其两端分别套在上、下支撑圆盘22、23的凸台外；The top and the bottom of theflexible joint 7 are upper andlower support discs 22, 23, the cross section is "convex" shape, and the through hole in the center of the disc is used to pass through the electric wire; 24 are hinged, and the outward extensions of two rotatingshafts 24 each fix the double-arm torsion spring 25, and the upper arm of thetorsion spring 25 is fixed on theupper support disc 22, and the lower arm of the torsion spring is fixed on thelower support disc 23; A section ofcylindrical spring 26 is pressed and fixed between the upper andlower support discs 22, 23, and its two ends are respectively sleeved outside the bosses of the upper andlower support discs 22, 23;

电气系统的主控制器1与运动控制器2、辅助控制器3、输入输出设备连接；运动控制器2与倾角仪15、18、陀螺仪16、19、编码器20、伺服驱动器4相连接；辅助控制器3与超声波传感器17和遥控接收器27连接；摄像头12与主控制器1；电源系统连接各电气设备供电。Themain controller 1 of the electrical system is connected with themotion controller 2, theauxiliary controller 3, and input and output devices; themotion controller 2 is connected with theinclinometers 15, 18,gyroscopes 16, 19,encoder 20, andservo driver 4; Theauxiliary controller 3 is connected with theultrasonic sensor 17 and theremote control receiver 27; thecamera 12 is connected with themain controller 1; the power supply system is connected with each electrical equipment for power supply.

前述的电源系统包括：充电电池28、转换电压的电源继电器板29、监测电池电压的电源监控板30。The foregoing power supply system includes: arechargeable battery 28 , apower relay board 29 for converting voltage, and apower monitoring board 30 for monitoring battery voltage.

前述的直流电机5选用直流力矩电机、带减速器的直流电机或直流无刷电机。Theaforementioned DC motor 5 is a DC torque motor, a DC motor with a reducer or a DC brushless motor.

前述的输入输出设备包括：键盘31、鼠标32、遥控器33、液晶显示屏34、显示器35、麦克风36、扬声器37。The aforementioned input and output devices include: akeyboard 31 , a mouse 32 , a remote controller 33 , aliquid crystal display 34 , a display 35 , amicrophone 36 , and aspeaker 37 .

一种柔性两轮自平衡机器人系统的运动控制方法，包括：主控制器1接收来自输入设备的用户操作指令；主控制器1监测各传感器反馈信息；主控制器1定时参照用户操作指令和传感器反馈信息，通过运动行为决策算法计算机器人的直行和偏航速度控制命令，下达给运动控制器2执行；在一个运动控制周期内，运动控制器2读取机器人躯干6和底盘8的倾角仪15、18、陀螺仪16、19的反馈信号，与零位值比较得出倾角和倾角速度的误差信号；运动控制器2读取电机编码器20的反馈信号，计算机器人直行和偏航速度，与主控制器给定的控制命令对比得出误差信号；根据误差信号，运动控制器2按预定的运动平衡控制算法计算电机的控制量，发送给伺服驱动器4执行；伺服驱动器4控制电机5运动，电机5带动双轮10使机器人维持机身平衡以及按指定方式运动，A motion control method for a flexible two-wheeled self-balancing robot system, comprising: amain controller 1 receiving user operation instructions from an input device;main controller 1 monitoring feedback information from various sensors;main controller 1 regularly referring to user operation instructions and sensors Feedback information, through the motion behavior decision-making algorithm to calculate the straight line and yaw speed control commands of the robot, and send them to themotion controller 2 for execution; within one motion control cycle, themotion controller 2 reads theinclinometer 15 of therobot torso 6 andchassis 8 , 18, the feedback signal ofgyroscope 16,19, draws the error signal of inclination angle and inclination angle velocity with zero value comparison;Motion controller 2 reads the feedback signal ofmotor encoder 20, calculates robot straight line and yaw velocity, and The control command given by the main controller is compared to obtain an error signal; according to the error signal, themotion controller 2 calculates the control amount of the motor according to a predetermined motion balance control algorithm, and sends it to theservo driver 4 for execution; theservo driver 4 controls the movement of themotor 5, Themotor 5 drives the twowheels 10 to make the robot maintain the balance of the body and move in a specified way,

主控制器1接收的用户指令，可以是即时指令，如遥控器键入，也可以是预存指令；The user instruction received by themain controller 1 can be an instant instruction, such as keyed in by a remote controller, or a pre-stored instruction;

主控制器1通过与运动控制器2通信间接获取倾角仪15、18、陀螺仪16、19和编码器20的反馈信息，通过与辅助控制器3通信间接获取超声波传感器17反馈信息和遥控器33输入的指令；Themain controller 1 indirectly obtains the feedback information of theinclinometers 15, 18,gyroscopes 16, 19 and theencoder 20 by communicating with themotion controller 2, and indirectly obtains the feedback information of theultrasonic sensor 17 and the remote controller 33 by communicating with theauxiliary controller 3 Entered instructions;

所述主控制器1的运动行为决策算法为：动作发生器参考用户命令或超声波测距信息计算出期望直行和偏航速度控制命令，决策器根据机器人姿态平衡的状况判断是否执行期望控制命令；若姿态平衡状况良好，则期望控制命令即为实际控制命令；若姿态平衡状况不佳，则实际控制命令为零，即先调整机器人平衡；The motion behavior decision-making algorithm of themain controller 1 is as follows: the action generator calculates the desired straight-ahead and yaw speed control commands with reference to user commands or ultrasonic ranging information, and the decision maker judges whether to execute the desired control commands according to the state of robot attitude balance; If the attitude balance is good, the expected control command is the actual control command; if the attitude balance is not good, the actual control command is zero, that is, adjust the balance of the robot first;

运动控制器2通过模数A/D转换多次采集倾角仪15、18和陀螺仪16、19的输出信号，并经过均值滤波等信号处理算法形成一个控制周期计算所用的反馈信号；Themotion controller 2 collects the output signals of theinclinometers 15, 18 and thegyroscopes 16, 19 multiple times through the analog-to-digital A/D conversion, and forms a feedback signal used for the calculation of the control period through signal processing algorithms such as mean value filtering;

所述运动控制器2的运动平衡控制算法为：直行速度控制器K₁根据姿态误差信号和直线运动速度误差计算出直线速度控制量u₁，偏航速度控制器K₂根据偏航速度误差计算出的偏航速度控制量u₂，[u₁ u₂]^T左乘耦合矩阵D计算出电机控制转矩[τ₁τ₂]^T；耦合矩阵D为二阶常数矩阵$\left[\begin{array}{cc}a& a\\ a& -a\end{array}\right].$The motion balance control algorithm of themotion controller 2 is as follows: the straight speed controller K₁ calculates the linear speed control value u₁ according to the attitude error signal and the linear motion speed error, and the yaw speed controller K₂ calculates according to the yaw speed error The obtained yaw speed control value u₂ , [u₁ u₂ ]^T is left multiplied by the coupling matrix D to calculate the motor control torque [τ₁ τ₂ ]^T ; the coupling matrix D is a second-order constant matrix$\left[\begin{array}{cc}a& a\\ a& -a\end{array}\right].$

伺服驱动器4通过电机电枢电流反馈闭环，实现电机5的转矩控制。Theservo drive 4 realizes the torque control of themotor 5 through the motor armature current feedback closed loop.

前述的预存指令包括；程序存储的动作顺序表、动作指令、任务式指令。The aforementioned pre-stored instructions include: program-stored action sequence tables, action instructions, and task-style instructions.

前述的动作发生器的设计方法包括查表、动态规划、专家系统等算法。The design method of the aforementioned action generator includes algorithms such as table lookup, dynamic programming, and expert system.

前述的决策器的设计方法包括查表、模糊逻辑、专家系统等算法。The design method of the aforementioned decision maker includes algorithms such as look-up tables, fuzzy logic, and expert systems.

前述的直线运动速度控制器和偏航速度控制器的设计方法包括：闭环极点配置、线性二次型最优控制、鲁棒控制、模糊控制、自适应控制等各种适合算法；The design methods of the aforementioned linear motion speed controller and yaw speed controller include: various suitable algorithms such as closed-loop pole configuration, linear quadratic optimal control, robust control, fuzzy control, and adaptive control;

本发明与现有技术相比，具有以下优点：Compared with the prior art, the present invention has the following advantages:

第一，本发明作为一种智能机器人，兼有移动式机器人的离散时间动作决策问题和倒立摆的连续时间姿态控制问题，因此可作为机器人学和控制科学领域交叉的综合研究对象，满足多学科研教的需要。First, as an intelligent robot, the present invention has both the discrete-time action decision-making problem of the mobile robot and the continuous-time attitude control problem of the inverted pendulum. Research and teaching needs.

第二，本发明在保留两轮自平衡机器人的原有优点基础上，具有新的应用价值，是一种与工程实际问题联系紧密，实用价值较高的研究、示教设备。在一些实际应用中，如火箭发射，存在重心在支点之上的控制问题，而这类对象并不是限定在直线或圆形轨道上运动的，因此轨道式倒立摆对这类问题的模拟具有局限性。两轮自平衡机器人(移动式倒立摆)则更接近实际控制问题。本发明具有双轮行走机构，延续了两轮自平衡机器人上述优势。此外，在实际应用中，某些仿人两轮机器人的腰部关节以及两轮电动车的驾驶员腰部都可看作柔性关节而抽象为本发明所述结构，因此本发明可作为研究这类特定系统控制问题的理想对象。Second, on the basis of retaining the original advantages of the two-wheeled self-balancing robot, the present invention has new application value, and is a research and teaching device that is closely related to engineering practical problems and has high practical value. In some practical applications, such as rocket launch, there is a problem of controlling the center of gravity above the fulcrum, and this type of object is not limited to move on a straight line or a circular orbit, so the orbital inverted pendulum has limitations for the simulation of this type of problem sex. A two-wheeled self-balancing robot (mobile inverted pendulum) is closer to the actual control problem. The present invention has a two-wheel walking mechanism, which continues the above-mentioned advantages of the two-wheel self-balancing robot. In addition, in practical applications, the waist joints of some humanoid two-wheeled robots and the driver's waist of two-wheeled electric vehicles can be regarded as flexible joints and abstracted as the structure described in the present invention. Ideal subject for system control problems.

第三，本发明改进了柔性关节的机械结构，使得它在应用中更加合理、实用。首先，引入单自由度旋转铰，将机器人柔性关节平衡的控制目标限定在俯仰自由度，从而保证系统的可控性。另外，在柔性关节高度不变的情况下，圆柱弹簧材料直径和圈数可调范围十分有限，制约了柔性关节刚度值的调节范围，而扭簧横卧的安装方式使得弹簧材料直径的选择余地大大增加，引入对称扭簧则能扩大柔性关节的刚度调节范围，使得机器人作为研究对象时具有更丰富的特性。Thirdly, the present invention improves the mechanical structure of the flexible joint, making it more reasonable and practical in application. First, a single-degree-of-freedom rotary hinge is introduced to limit the control target of the robot's flexible joint balance to the pitch degree of freedom, thereby ensuring the controllability of the system. In addition, when the height of the flexible joint remains constant, the adjustable range of the diameter and the number of turns of the cylindrical spring material is very limited, which restricts the adjustment range of the stiffness value of the flexible joint, and the horizontal installation method of the torsion spring makes the diameter of the spring material more flexible. The introduction of symmetrical torsion springs can expand the stiffness adjustment range of flexible joints, making the robot have more abundant characteristics when it is used as a research object.

第四，本发明中的柔性关节中弹簧的动态特性是非线性的，而弹簧材料的阻尼、疲劳以及关节的摩擦带来系统参数的不确定性，这使得系统的非线性和不确定性增加，更加适合非线性控制、鲁棒控制和智能控制的研究。Fourth, the dynamic characteristics of the spring in the flexible joint of the present invention are nonlinear, and the damping, fatigue and friction of the joint bring uncertainty to the system parameters, which increases the nonlinearity and uncertainty of the system, It is more suitable for the research of nonlinear control, robust control and intelligent control.

第五，本发明的电气系统采用层式结构，各级控制器各司其职，使得机器人的处理性能更加强劲，能够有效降低应对复杂控制算法时的计算负担，这十分符合该机器人作为多用途研究平台的定位。此外，分层结构使得硬件系统具有良好的扩展能力。Fifth, the electrical system of the present invention adopts a layered structure, and the controllers at all levels perform their duties, which makes the processing performance of the robot stronger and can effectively reduce the calculation burden when dealing with complex control algorithms, which is very suitable for the robot as a multi-purpose The positioning of the research platform. In addition, the layered structure makes the hardware system have good scalability.

第六，本发明的电气系统既有PC系统，也包括嵌入式系统，而且在其PC系统中能直接开发各嵌入式系统的应用软件。这样，用户在连接鼠标、键盘、显示器等I/O设备后，在机器人系统上直接开发多种硬件系统的程序，既方便了开发，又能够实践多种硬件体系的软件开发过程，使得机器人适用于多门课程的教学实践环节。Sixth, the electrical system of the present invention includes both PC system and embedded system, and the application software of each embedded system can be directly developed in the PC system. In this way, after connecting I/O devices such as mouse, keyboard, and monitor, users can directly develop programs for various hardware systems on the robot system. In the teaching practice link of many courses.

第七，本发明的运动控制方法能够实现柔性两轮自平衡机器人的姿态平衡控制和运动速度控制，使得用户能在此基础上进一步开发其他高级控制功能。Seventh, the motion control method of the present invention can realize the posture balance control and motion speed control of the flexible two-wheeled self-balancing robot, so that users can further develop other advanced control functions on this basis.

第八，本发明配备遥控系统和避障系统，使得机器人具有遥控操作和自主避障运行的双重模式。Eighth, the present invention is equipped with a remote control system and an obstacle avoidance system, so that the robot has dual modes of remote control operation and autonomous obstacle avoidance operation.

第九，本发明配备视觉传感系统和语音处理系统，使得机器人可扩展视觉识别、语音识别功能。Ninth, the present invention is equipped with a visual sensing system and a voice processing system, so that the robot can expand the functions of visual recognition and voice recognition.

附图说明Description of drawings

图1柔性两轮自平衡机器人的机械结构主视图；Figure 1 is the front view of the mechanical structure of the flexible two-wheeled self-balancing robot;

图2柔性两轮自平衡机器人的电子元件布局示意图；Fig. 2 Schematic diagram of the electronic component layout of the flexible two-wheeled self-balancing robot;

图3柔性两轮自平衡机器人的机械结构侧视图；The mechanical structure side view of Fig. 3 flexible two-wheeled self-balancing robot;

图4柔性两轮自平衡机器人底盘的俯视图；The top view of the flexible two-wheeled self-balancing robot chassis of Fig. 4;

图5柔性两轮自平衡机器人的柔性关节结构主视图；Figure 5 is the front view of the flexible joint structure of the flexible two-wheeled self-balancing robot;

图6柔性两轮自平衡机器人的柔性关节结构侧视图；Fig. 6 is a side view of the flexible joint structure of the flexible two-wheeled self-balancing robot;

图7柔性两轮自平衡机器人的柔性关节俯视图；Figure 7 is a top view of the flexible joint of the flexible two-wheeled self-balancing robot;

图8柔性两轮自平衡机器人的电气系统连接示意图；Figure 8 is a schematic diagram of the electrical system connection of the flexible two-wheeled self-balancing robot;

图9柔性两轮自平衡机器人的电气系统原理图；Figure 9 is a schematic diagram of the electrical system of the flexible two-wheeled self-balancing robot;

图10柔性两轮自平衡机器人的运动控制系统框图；Figure 10 is a block diagram of the motion control system of the flexible two-wheeled self-balancing robot;

图11主控制器监控软件串口接收消息处理函数流程图；Figure 11 is a flow chart of the main controller monitoring software serial port receiving message processing function;

图12主控制器监控软件定时器消息处理函数流程图；Figure 12 master controller monitoring software timer message processing function flow chart;

图13运动控制器主程序流程图；Figure 13 motion controller main program flow chart;

图14运动控制器定时中断控制程序流程图；Figure 14 motion controller timing interrupt control program flow chart;

图15运动控制器SCI接收中断程序流程图；Figure 15 motion controller SCI receiving interrupt program flow chart;

图16柔性两轮自平衡机器人的速度控制仿真曲线；Fig. 16 The speed control simulation curve of the flexible two-wheeled self-balancing robot;

图17柔性两轮自平衡机器人速度控制中姿态角变化仿真曲线；Fig. 17 The simulation curve of the attitude angle change in the speed control of the flexible two-wheeled self-balancing robot;

图18辅助控制器超声波测距控制函数流程图；Figure 18 Auxiliary controller ultrasonic ranging control function flow chart;

图19辅助控制器遥控指令检测控制函数流程图；Fig. 19 Auxiliary controller remote command detection control function flow chart;

图20辅助控制器主程序流程图。Figure 20 is the main program flow chart of the auxiliary controller.

具体实施方式Detailed ways

下面结合附图，介绍本实施例。Below in conjunction with accompanying drawing, introduce this embodiment.

一、柔性两轮自平衡机器人的硬件1. The hardware of the flexible two-wheeled self-balancing robot

1.电气系统选型1. Electrical system selection

主控制器1选用研祥嵌入式计算机(EPC)EC5-1717CLDNA。该系统是一款采用Intel915GM芯片组的高性能单板电脑，板载1.6GHz Pentium M处理器，256MB D DR II 533内存，集成显卡、声卡，提供各种接口。EPC的硬盘存储器38为东芝抗震硬盘MK4036GAC，存储容量40GB，工作中可抗最大2.0G振动及200G瞬时冲击。Themain controller 1 selects EVOC embedded computer (EPC) EC5-1717CLDNA. The system is a high-performance single-board computer with Intel915GM chipset, onboard 1.6GHz Pentium M processor, 256MB DDR II 533 memory, integrated graphics card, sound card, and various interfaces. Thehard disk memory 38 of EPC is Toshiba anti-vibration hard disk MK4036GAC, the storage capacity is 40GB, and it can resist the maximum 2.0G vibration and 200G instantaneous impact during work.

运动控制器2选用飓风公司MSK2812系统板。该系统的处理器采用TI公司TMS320F2812DSP，系统为5V直流供电。Themotion controller 2 uses the Hurricane MSK2812 system board. The processor of this system adopts TMS320F2812DSP of TI Company, and the system is powered by 5V DC.

MSK2812的仿真器39选用飓风公司的XDS510USB，USB2.0接口。Theemulator 39 of MSK2812 selects the XDS510USB of Hurricane Company, USB2.0 interface.

辅助控制器3选用凌阳科技公司的SPCE061A系统板，系统为5V直流供电，处理器为16位u′nSPTM微控制器，板载麦克风36，可实现语音识别功能。Auxiliary controller 3 selects SPCE061A system board of Sunplus Technology Co., Ltd., the system is powered by 5V DC, the processor is a 16-bit u'nSPTM microcontroller, and themicrophone 36 is mounted on the board, which can realize the voice recognition function.

伺服驱动器4选用Copley Motion公司的AJC55-18。AJC55-18可用于位置，转速和扭矩控制，外部控制器可以通过模拟方式(±10V，扭矩，速度，位置)，PWM(扭矩，速度)，以及极性PWM方式(扭矩，速度)对目标电机进行伺服控制。AJC55-18可通过串口与PC系统通信，设定工作模式和控制器参数。Servo driver 4 selects AJC55-18 from Copley Motion Company. AJC55-18 can be used for position, speed and torque control, the external controller can control the target motor through analog mode (±10V, torque, speed, position), PWM (torque, speed), and polar PWM mode (torque, speed) Perform servo control. AJC55-18 can communicate with the PC system through the serial port to set the working mode and controller parameters.

液晶显示屏17选用台湾友达光电公司的B084SN0，3.3v供电，分辨率800*600。接口包括LVDS端口，背光灯电源线。液晶显示屏的背光升压板(40)选用三菱公司的T511024.04，5V直流供电。The liquidcrystal display screen 17 is B084SN0 of Taiwan AU Optronics Co., Ltd., powered by 3.3v, and has a resolution of 800*600. The interface includes LVDS port, backlight power line. The backlight step-up board (40) of the liquid crystal display selects T511024.04 of Mitsubishi Corporation, 5V DC power supply.

扬声器37选用一对功率为2W的无源扬声器单元。Theloudspeaker 37 selects a pair of passive loudspeaker units with a power of 2W.

倾角仪15、18选用Crossbow公司的CXTA-01。它主要采用高稳定性的硅微机械电容倾角传感器，以模拟信号方式输出倾斜角度。CXTA-01的测量范围为±75°，分辨率为0.05°，带宽为125Hz，8-30V直流供电，输出电压0-5V，零位电压2.5V。Theinclinometers 15 and 18 are CXTA-01 from Crossbow Company. It mainly adopts high-stability silicon micro-mechanical capacitive inclination sensor, and outputs the inclination angle in the form of analog signal. The measurement range of CXTA-01 is ±75°, the resolution is 0.05°, the bandwidth is 125Hz, the power supply is 8-30V DC, the output voltage is 0-5V, and the zero voltage is 2.5V.

陀螺仪16、19选用Silicon Sensing Systems Japan公司的CRS03。CRS03角速度传感器基于MEMS技术制造，在剧烈冲击和震动条件下仍能保持卓越的性能，温漂小并具有良好的重复性，主要参数为：量程±100°/s，直流5V供电，零位电压2.5V，测量分辨率20mV/°/s，带宽为10Hz。Thegyroscopes 16 and 19 are CRS03 from Silicon Sensing Systems Japan. CRS03 angular velocity sensor is manufactured based on MEMS technology. It can still maintain excellent performance under severe shock and vibration conditions, with small temperature drift and good repeatability. The main parameters are: range ±100°/s,DC 5V power supply, zero voltage 2.5V, measurement resolution 20mV/°/s, bandwidth 10Hz.

选用四个Parallax公司的PING)))超声波传感器17组成的阵列作为避障传感器。PING)))为5V直流供电，具有一个超声波发射模块和一个接受模块，通过测量超声波脉冲遇到物体反射后的回波传播时间来测量距离。PING)))的测量范围2cm-3m，分辨率＜1cm。An array of four PING)))ultrasonic sensors 17 of Parallax Company is selected as the obstacle avoidance sensor. PING))) is powered by 5V DC, has an ultrasonic transmitter module and a receiver module, and measures the distance by measuring the echo propagation time after the ultrasonic pulse encounters the object reflection. PING))) has a measuring range of 2cm-3m and a resolution of <1cm.

机器人视觉传感器选用USB接口CMOS摄像头12，130万像素。The robot vision sensor uses a USBinterface CMOS camera 12, 1.3 million pixels.

遥控系统包括遥控器33和遥控接收器27，分别为3V和5V直流供电，遥控距离20米。遥控器33有12个按键，当有键按下时，遥控接收器27的4个IO口将输出不同的电平组合。The remote control system includes a remote control 33 and aremote control receiver 27, which are respectively powered by 3V and 5V DC, and the remote control distance is 20 meters. The remote controller 33 has 12 buttons, and when a button is pressed, the 4 IO ports of theremote receiver 27 will output different level combinations.

机器人的驱动电机5选用Maxon公司的直流电机RE40套件，电机为24V供电，额定功率150W，配备26∶1的行星齿轮减速器，最大输出转矩7.5Nm，电机后端配有增量式光电编码器20，精度为1000线。The drivingmotor 5 of the robot uses the DC motor RE40 kit of Maxon Company, the motor is powered by 24V, the rated power is 150W, it is equipped with a 26:1 planetary gear reducer, the maximum output torque is 7.5Nm, and the rear end of the motor is equipped with an incrementalphotoelectric encoder 20, the accuracy is 1000 lines.

机器人的电源为24V 9Ah镍氢充电电池28。29为电源继电器板，OP-DC01上的电压转换模块将24V直流输入转为5V直流输出，用于相关设备供电。定制直流24V输入的ATX电源适配器41，输出ATX/AT接口电源，为主控制器1、硬盘存储器38、背光升压板40供电。电源监控板30OP-DY01上的单片机在检测到电压低于22.5V时，控制蜂鸣器报警。The power supply of the robot is 24V 9Ah Ni-MHrechargeable battery 28. 29 is the power relay board, and the voltage conversion module on OP-DC01 converts 24V DC input to 5V DC output for power supply of related equipment. CustomizedATX power adapter 41 withDC 24V input, outputting ATX/AT interface power, supplying power tomain controller 1,hard disk storage 38, andbacklight booster board 40. When the single-chip microcomputer on the power monitoring board 30OP-DY01 detects that the voltage is lower than 22.5V, it will control the buzzer to alarm.

2.机械结构与电气元件布局2. Mechanical structure and electrical component layout

本实施例总重量20kg，高度700mm，宽度400mm，长度150mm，轮子直径250mm。机器人的机械结构和电器元件布局如下：The present embodiment has a total weight of 20kg, a height of 700mm, a width of 400mm, a length of 150mm, and a wheel diameter of 250mm. The mechanical structure and electrical component layout of the robot are as follows:

如图1、2、3所示，躯干部分6为铝合金框架，分为上、中、下三层。躯干6的上层为前后敞口的中空屉框，内部固定主控制器1，顶板11之上通过中空的圆柱形支架42固定摄像头12。躯干6的中层由平行底面的三块亚格力隔板14隔为四小层，其中，最上一层安装硬盘存储器38、辅助控制器3和遥控接收器27，第二层安装ATX电源适配器41，第三层安装电源继电器板29和电源监控板30，第四层安装背光升压板40。躯干的中层的四周装有亚格力面板，用以保护内部电子元件以及防止中间隔板14脱落，其中前面板43上固定着液晶显示屏34和扬声器37。躯干的下层为前后敞口的中空屉框，内部装载倾角仪15和陀螺仪16。底层框架前端固定一个面朝前的带状支架44，用于安装四个超声波传感器17。As shown in Figures 1, 2 and 3, thetorso part 6 is an aluminum alloy frame and is divided into upper, middle and lower layers. The upper layer of thetorso 6 is a hollow drawer frame with front and back openings, themain controller 1 is fixed inside, and thecamera 12 is fixed by a hollowcylindrical support 42 on thetop plate 11 . The middle layer of thetorso 6 is divided into four small layers by threeacrylic partitions 14 parallel to the bottom surface, wherein thehard disk storage 38, theauxiliary controller 3 and theremote control receiver 27 are installed on the top layer, and theATX power adapter 41 is installed on the second layer. Apower relay board 29 and apower monitoring board 30 are installed on the third layer, and abacklight booster board 40 is installed on the fourth layer. Acrylic panels are installed around the middle layer of the torso to protect the internal electronic components and prevent themiddle partition 14 from coming off, wherein thefront panel 43 is fixed with aliquid crystal display 34 and aloudspeaker 37. The lower layer of the torso is a hollow drawer frame with front and back openings, and aninclinometer 15 and agyroscope 16 are loaded inside. The front end of the bottom frame is fixed with a front-facing strip bracket 44 for installing fourultrasonic sensors 17 .

躯干底板13和底盘8顶端面之间以柔性俯仰关节7连接。柔性关节7的结构如图5、6、7所示：顶端和底端为上、下支承圆盘22、23，剖面为“凸”字形，圆盘中心开通孔用于电线穿过。在上支撑圆盘凸台面上，以沉头螺钉45固定上支座46。上支座46的方形底板中央开通孔，与支撑圆盘通孔孔径相同，装配时两孔同心。上支座46的底板两侧有竖起的圆顶角剑头形支座壁，其上开有同心的通孔。在下支撑圆盘23的相同位置以相同方式安装下支座47，下支座47与上支座46不同的是其两个侧壁均为双层，令上支座46侧壁可插入间隙紧密配合，另外，下支座壁47的通孔直径略小。上支座46两侧壁上的通孔中分别插入铜套48后与下支座47的侧壁通孔直径相同，上、下支座46、47装配时两侧壁通孔同心，在其中分别插入转动轴24，并以挡板49固定，使得上、下支承圆盘22、23能够以转动轴24为铰接点转动。两个转动轴24外侧各穿过一个双臂扭簧25，扭簧25的上臂固定在上支承圆盘22的卡槽中，扭簧的下臂末端的圆勾通过螺栓50固定在下支座47的侧壁上。在上、下支座46、47装配时，上、下支撑圆盘22、23之间压紧固定一段圆柱弹簧26，其两端分别套在上、下支撑圆盘22、23的凸台外。Thetrunk bottom plate 13 and the top surface of thechassis 8 are connected by a flexible pitch joint 7 . The structure of theflexible joint 7 is shown in Figures 5, 6, and 7: the top and bottom are upper andlower support discs 22, 23, and the section is "convex" in shape, and the disc center has a through hole for electric wires to pass through. On the upper support disk boss surface, fix theupper bearing 46 with the countersunkhead screw 45 . The central square bottom plate ofupper support 46 has a through hole, which is the same as the diameter of the through hole of the supporting disk, and the two holes are concentric during assembling. The base plate both sides ofupper bearing 46 has the dome angle sword head shape bearing wall that erects, has the through hole of concentricity on it. Install thelower support 47 in the same way at the same position of thelower support disc 23. The difference between thelower support 47 and theupper support 46 is that its two side walls are double-layered, so that theupper support 46 side walls can be inserted into the gap tightly. In addition, the diameter of the through hole of thelower seat wall 47 is slightly smaller. The through holes on the two side walls of theupper support 46 are respectively inserted into thecopper sleeves 48 and have the same diameter as the through holes on the side walls of thelower support 47. Insert the rotatingshaft 24 respectively, and fix with thebaffle plate 49, so that the upper andlower support discs 22, 23 can rotate with the rotatingshaft 24 as the hinge point. The outer sides of the tworotating shafts 24 respectively pass through a double-arm torsion spring 25, the upper arm of thetorsion spring 25 is fixed in the draw-in groove of theupper support disc 22, and the round hook at the end of the lower arm of the torsion spring is fixed on thelower support 47 by abolt 50. on the side wall. When the upper andlower bearings 46, 47 are assembled, a section ofcylindrical spring 26 is pressed and fixed between the upper andlower support discs 22, 23, and its two ends are respectively sleeved outside the bosses of the upper andlower support discs 22, 23. .

如图2、4所示，机器人的底盘部分8为铝合金制箱式结构。在底盘左右两侧的中心位置各装有一组轴系，主要包括转轴51、轴承52、轴承座53等。在底盘8内部中间位置，左右轮电机5前后错开固定在齿轮传动箱54外，电机的轴通过传动齿轮55与轴系的转轴51联接。使用螺栓56将轴系的转轴53与轴辖57连接，两者之间夹持固定轮子10。在底盘8内，运动控制器2和仿真器39安装在前侧，电机伺服控制器4安装在后侧，倾角仪18、陀螺仪19和电池28安装在底部。底盘8底面有两个可拆卸保护支架9，支架末端的脚轮21在机器人直立时悬空，倾倒时脚轮21触地起到支撑保护作用。As shown in Figures 2 and 4, thechassis part 8 of the robot is a box-type structure made of aluminum alloy. A group of shaft systems are respectively installed at the center positions on the left and right sides of the chassis, mainly including rotatingshaft 51, bearing 52, bearingseat 53 and the like. In the inner middle position of thechassis 8, the left andright wheel motors 5 are staggered and fixed outside thegear transmission box 54, and the shaft of the motor is connected with the rotatingshaft 51 of the shaft system through thetransmission gear 55.Bolts 56 are used to connect therotating shaft 53 of the shaft system to theshaft shaft 57, and thewheel 10 is clamped and fixed between the two. In thechassis 8, themotion controller 2 and theemulator 39 are installed on the front side, themotor servo controller 4 is installed on the rear side, and theinclinometer 18, thegyroscope 19 and thebattery 28 are installed on the bottom. There are two detachableprotective supports 9 on the bottom surface of thechassis 8, and the casters 21 at the end of the support are suspended in the air when the robot is upright, and the casters 21 touch the ground to play a supporting and protective effect when the robot is toppled over.

3.电气系统连接3. Electrical system connection

如图8所示，电气系统各部分的连接方法如下：As shown in Figure 8, the connection method of each part of the electrical system is as follows:

EC5-1717CLDNA与硬盘存储器38连接组成主控制器1；在EC5-1717CLDNA面板接口FP1的1、2脚之间串入触点开关58作为它的启动按钮。ATX电源适配器41连接EC5-1717CLDNA为其供电。EC5-1717CLDNA is connected with thehard disk storage 38 to form themain controller 1; acontact switch 58 is inserted in series betweenpins 1 and 2 of the panel interface FP1 of the EC5-1717CLDNA as its start button.ATX power adapter 41 is connected to EC5-1717CLDNA to provide power for it.

EC5-1717CLDNA的四个RS232串口COM1、COM2、COM3、COM4分别与MSK2812、SPCE061A、两个ACJ55-18的串口连接。此外，EC5-1717CLDNA通过USB接口连接DSP的仿真器39XDS510USB，仿真器39再与DSP的JTAG接口连接。The four RS232 serial ports COM1, COM2, COM3 and COM4 of EC5-1717CLDNA are respectively connected to the serial ports of MSK2812, SPCE061A and two ACJ55-18. In addition, EC5-1717CLDNA is connected to DSP emulator 39XDS510USB through USB interface, andemulator 39 is connected to DSP JTAG interface.

EC5-1717CLDNA的USB1接口与摄像头12连接，SPK接口与扬声器37连接，LVDS接口与液晶显示屏34连接。背光升压板40的一端连接液晶显示屏34的背光灯电源线，另一端的1、2、3、4线与ATX电源适配器41的直流5V输出连接，5、6线与ATX电源适配器41的GND相连。The USB1 interface of the EC5-1717CLDNA is connected with thecamera 12, the SPK interface is connected with thespeaker 37, and the LVDS interface is connected with theliquid crystal display 34. One end of thebacklight booster board 40 is connected to the backlight power line of theliquid crystal display 34, the 1, 2, 3, and 4 lines of the other end are connected with theDC 5V output of theATX power adapter 41, and the 5, 6 lines are connected with theATX power adapter 41. Connect to GND.

另外，在机器人停止运动时，EC5-1717CLDNA可以连接键盘31、鼠标32和显示器35，进行程序编写与调试。In addition, when the robot stops moving, the EC5-1717CLDNA can be connected to thekeyboard 31, the mouse 32 and the monitor 35 for programming and debugging.

MSK2812板由电源继电器板29的+5V输出供电，它的J7接口的38，37，36，35脚，即A/D转换输入通道，分别与两个倾角仪(CXTA01)15、18和两个陀螺仪(CRS03)16、19的模拟信号输出端连接；两个陀螺仪16、19分别由MSK2812的J7接口的1和7脚，即+5V输出供电；倾角仪15、18则由电源继电器板的24V输出直接供电。The MSK2812 board is powered by the +5V output of thepower relay board 29, and the 38, 37, 36, and 35 pins of its J7 interface, that is, the A/D conversion input channel, are respectively connected with two inclinometers (CXTA01) 15, 18 and two The gyroscopes (CRS03) 16 and 19 are connected to the analog signal output terminals; the twogyroscopes 16 and 19 are powered bypins 1 and 7 of the J7 interface of MSK2812, namely +5V output; theinclinometers 15 and 18 are powered by the power relay board The 24V output is directly powered.

MSK2812与两个伺服控制器(AJC55-18)4间的连接包括控制信号线和编码器反馈信号线。控制信号包括电机使能信号、电机转动方向信号和PWM转速控制量信号。其中，MSK2812的J5接口的3、7脚分别与控制左、右电机的AJC55-18的J5接口的3脚连接，作为AJC55-18的使能信号线；MSK2812的J5接口的5、1脚分别与控制左、右电机的AJC55-18的J5接口的6脚连接，作为电机5转动方向选择信号线；MSK2812的J7接口17、18脚为PWM输出，分别与控制左、右电机5的(AJC55-18)4的J5接口的20脚连接，作为转速控制量信号线。左、右电机编码器20的反馈信号经AJC55-18缓存后连接至MSK2812，具体接线为左、右电机AJC55-18的J5接口的10、11脚，分别接MSK2812的J7接口的27、28脚和J6接口的13、14脚。The connection between MSK2812 and two servo controllers (AJC55-18) 4 includes control signal lines and encoder feedback signal lines. The control signal includes a motor enable signal, a motor rotation direction signal and a PWM speed control signal. Among them, pins 3 and 7 of the J5 interface of MSK2812 are respectively connected topins 3 and 3 of the J5 interface of the AJC55-18 controlling the left and right motors, as the enabling signal line of the AJC55-18;pins 5 and 1 of the J5 interface of the MSK2812 are respectively Connect withpin 6 of the J5 interface of AJC55-18 controlling the left and right motors, and use it as a signal line for selecting the rotation direction of themotor 5; pins 17 and 18 of the J7 interface of MSK2812 are PWM outputs, which are respectively connected with the (AJC55 -18) The 20-pin connection of the J5 interface of 4 is used as the signal line of the speed control quantity. The feedback signals of the left andright motor encoders 20 are buffered by AJC55-18 and then connected to MSK2812. The specific wiring is the 10 and 11 pins of the J5 interface of the left and right motor AJC55-18, respectively connected to the 27 and 28 pins of the J7 interface of MSK2812 and pins 13 and 14 of the J6 interface.

两个AJC55-18的J3接口的3、4脚为电源输入端，分别接电源继电器板29的+24V和GND；J2接口的3、4脚为控制电压的输出端，分别与电机的+/-输入端连接，其中3脚与电机+输入端之间串接一个电机开关59；J4接口的4、6分别为+5V和GND，分别与编码器20排线的2、3线连接，J4接口的1、8、2、9、3、10脚为编码器A通道、B通道和零位信号的共模输入端，分别接编码器排线的5、6、7、8、9、10线。Pins 3 and 4 of the J3 interface of the two AJC55-18 are power input terminals, respectively connected to +24V and GND of thepower relay board 29;pins 3 and 4 of the J2 interface are output terminals of the control voltage, respectively connected to the +/ -Input terminal connection, in which amotor switch 59 is connected in series betweenpin 3 and the motor + input terminal; 4 and 6 of the J4 interface are respectively +5V and GND, respectively connected to the 2 and 3 wires of theencoder 20 cable, J4 The pins 1, 8, 2, 9, 3, and 10 of the interface are the common-mode input ends of the encoder A channel, B channel, and zero position signal, and are respectively connected to 5, 6, 7, 8, 9, and 10 of the encoder cable. Wire.

SPCE061A由电源继电器板29的+5V输出供电，它的J1接口的2～5针，分别连接超声波传感器17，1#～4#的Signal端；J2接口2～5针，分别连接遥控接收器27的输出D0～D3。SPCE061A is powered by the +5V output of thepower relay board 29, and the 2~5 pins of its J1 interface are respectively connected to the signal end of theultrasonic sensor 17 and 1#~4#; the 2~5 pins of the J2 interface are respectively connected to theremote control receiver 27 The output D0 ~ D3.

两组24V4.5Ah的镍氢电池组并联组成24V9Ah电池28。电池28经一个双刀双掷的船型开关60分别与电源继电器板29和充电器插口61连接。当船型开关60从“关”拨到“开”时，电池给开始供电，拨到“充电”时，外接直流充电器62为电池充电。Two sets of 24V4.5Ah Ni-MH battery packs are connected in parallel to form a24V9Ah battery 28 . Thebattery 28 is connected to thepower relay board 29 and thecharger socket 61 respectively via a double pole doublethrow rocker switch 60 . When theboat switch 60 is turned from "off" to "on", the battery starts to supply power, and when turned to "charging", theexternal DC charger 62 charges the battery.

电源继电器板29的+/-输入端连接电池组28的+/-端，提供24V和5V两种直流输出，分别连接各对应设备的供电端。The +/- input terminal of thepower relay board 29 is connected to the +/- terminal of thebattery pack 28 to provide two kinds of DC outputs of 24V and 5V, which are respectively connected to the power supply terminals of corresponding devices.

电源监控板30的Vd、GND和Vin分别接电源继电器板29的+5V、GND和+24V。Vd, GND and Vin of thepower monitoring board 30 are connected to +5V, GND and +24V of thepower relay board 29 respectively.

4.电气系统的工作原理4. How the electrical system works

本实施例机器人的主要功能是在保持机身俯仰姿态平衡的前提下，能够进行行走、转弯等运动，这种运动可以是机器人按照行为决策规则自主智能行为，也可以是按照用户即时下达的操作指令运动。由此，机器人电气系统的工作原理如图9所示：机器人的主控制器1经串口从运动控制器2和辅助控制器3读取各个传感器反馈信息和遥控操作指令并在液晶屏34上显示，然后按照运动行为决策算法计算出机器人的运动控制命令，并通过串口下达给运动控制器2；运动控制器2由模数转换模块得到倾角仪15、18、陀螺仪16、19的反馈信号，经过伺服驱动器4中继读取编码器20反馈信号，然后，综合接收到得控制命令和反馈信号，按预定的运动平衡控制算法计算出电机的转矩控制量，发送对应的PWM信号给伺服驱动器4执行；伺服驱动器4控制电机5运动，电机5带动轮子10使机器人维持机身平衡以及按主控制器1控制命令运动。The main function of the robot in this embodiment is to be able to perform movements such as walking and turning under the premise of maintaining the balance of the pitching attitude of the fuselage. command movement. Thus, the working principle of the robot electrical system is as shown in Figure 9: themain controller 1 of the robot reads each sensor feedback information and remote control operation instructions from themotion controller 2 and theauxiliary controller 3 through the serial port and displays them on theliquid crystal screen 34. , and then calculate the motion control command of the robot according to the motion behavior decision-making algorithm, and send it to themotion controller 2 through the serial port; The feedback signal of theencoder 20 is read through the relay of theservo driver 4, and then, the received control command and feedback signal are integrated, the torque control amount of the motor is calculated according to the predetermined motion balance control algorithm, and the corresponding PWM signal is sent to theservo driver 4 execution; theservo drive 4 controls the movement of themotor 5, and themotor 5 drives thewheels 10 to make the robot maintain the balance of the body and move according to the control command of themain controller 1.

二、机器人系统的运动控制方法Second, the motion control method of the robot system

本实施例给出柔性两轮自平衡机器人自主避障和遥控操作两种运动模式控制系统的软件实现，其他如机器人智能行为决策、语音操作等控制功能可以参考本实施例软件，添加相应模块进行设计。This embodiment provides the software implementation of the two motion control systems of the flexible two-wheeled self-balancing robot autonomous obstacle avoidance and remote control operation. Other control functions such as robot intelligent behavior decision-making and voice operation can refer to the software of this embodiment and add corresponding modules to implement design.

图10为控制系统的框图，功能为实现机器人的两种运动模式，一为自主避障运动模式，二为遥控操作运动模式。其中，运动模式通过遥控器按键选择；机器人的运动以动作集合描述，包含：前进、后退、左转弯、右转弯、静止和左、右原地旋转；通过控制机器人的直行速度V_x和偏航速度ω_α来完成动作，控制命令如下表所示：Figure 10 is a block diagram of the control system, which functions to realize two motion modes of the robot, one is the autonomous obstacle avoidance motion mode, and the other is the remote control operation motion mode. Among them, the movement mode is selected by the remote control button; the movement of the robot is described by a set of actions, including: forward, backward, left turn, right turn, stationary and left and right in situ rotation; by controlling the straight speed V_x and yaw of the robot Speed ω_α to complete the action, the control command is shown in the following table:

  前进 go ahead  后退back  左转弯turn left  右转弯turn right  原地左转turn left  原地右转turn right  静止static  V_x^cV_x^c  vv  -v-v  v/2v/2  v/2v/2  00  00  00  ω_α^cω_α^c  00  00  -2v/D-2v/D  2v/D2v/D  -2v/D-2v/D  2v/D2v/D  00

表中v的值取0.1m/s，D为两轮轮心距，取0.4m。The value of v in the table is 0.1m/s, and D is the wheel center distance between the two wheels, which is 0.4m.

整个控制系统由四部分软件实现，分别运行在主控制器1、运动控制器2、辅助控制器3和伺服驱动控制器4中。The entire control system is implemented by four parts of software, which run in themain controller 1,motion controller 2,auxiliary controller 3 andservo drive controller 4 respectively.

1.主控制器1中的监控程序1. The monitoring program in themain controller 1

主控制器1中的监控程序RobotControl.exe为Windows系统应用程序，采用Visual C++软件开发。该程序可实时监测遥控命令(遥控输入键值R^c)与机器人运行状态(直线速度V_x、旋转角速度ω_α、底盘和躯干的姿态角θ₁和θ₂、姿态角速度

和

超声波传感器测距信息D_S1-D_S4)，并实现机器人运动行为决策。程序的核心部分为串口通信和运动行为决策。The monitoring program RobotControl.exe in themain controller 1 is a Windows system application program, which is developed by Visual C++ software. The program can monitor the remote control command (remote control input key value R^c ) and the robot running state (linear velocity V_x , rotational angular velocity ω_α , attitude angles θ₁ and θ₂ of the chassis and torso, attitude angular velocity

and

Ultrasonic sensors measure distance information (D_S1 -D_S4 ), and realize robot movement behavior decision-making. The core part of the program is serial communication and motion behavior decision-making.

串口通信的内容为主控制器1向运动控制器2下达控制命令，以及运动控制器2和辅助控制器3向主控制器1上传机器人状态信息。通信方式为异步串行通信，通信协议为：串口传输波特率19200，每帧8位数据位，无奇偶校验，1个停止位。主控制器1下达给运动控制器2的控制命令数据包的格式为：<F0h，F0h，V，数据，W，数据，数据包长度值，FFh，FFh>。其中，数据包长度值为整型变量，字符“V”代表机器人直行速度控制命令V_x^c，字符“W”代表机器人偏航速度控制命令ω^c，“数据”为他们对应的浮点型变量值。The content of the serial port communication is that themain controller 1 issues control commands to themotion controller 2, and themotion controller 2 and theauxiliary controller 3 upload robot status information to themain controller 1. The communication method is asynchronous serial communication, and the communication protocol is: serial porttransmission baud rate 19200, 8 data bits per frame, no parity check, 1 stop bit. The format of the control command packet issued by themain controller 1 to themotion controller 2 is: <F0h, F0h, V, data, W, data, data packet length value, FFh, FFh>. Among them, the data packet length value is an integer variable, the character "V" represents the robot's straight forward speed control command V_x^c , the character "W" represents the robot's yaw speed control command ω^c , and "data" is their corresponding floating-point variables value.

主控制器1接收的状态信息数据包的格式为：<F0h，F0h，数据代码，数据，数据代码，数据，......，数据包长度值，FFh，FFh>。其中，数据包长度值为整型变量，数据代码与其对应的数据名称如下表所示：The format of the status information packet received by themaster controller 1 is: <F0h, F0h, data code, data, data code, data, ..., data packet length value, FFh, FFh>. Among them, the data packet length value is an integer variable, and the data code and its corresponding data name are shown in the following table:

表中除R^c以外其他数据类型均为浮点型，R^c为遥控指令代码，代码的值及其对应遥控器按键和所定义功能如下表所示：The other data types in the table except R^c are floating-point types, and R^c is the remote control command code. The value of the code and its corresponding remote control button and defined function are shown in the following table:

代码the code00h00h01h01h02h02h03h03h04h04h05h05h06h06h07h07h08h08h09h09h0Ah0Ah0Bh0Bh  按键button  1 1  2 2  33  44  55  66  77  8 8  9 9  1010  1111  1212功能Function预留reserve  自主避障模式Autonomous obstacle avoidance mode  遥控操作模式  Remote control mode前进go ahead后退step back左转弯turn left右转弯turn right原地左转turn left原地右转turn right静止still预留reserve预留reserve

本例采用开源的串口类CSerial实现串口操作，相关代码和说明可在http://codegure.earthweb.com/network/serialpot.shtml下载。In this example, the open source serial port class CSerial is used to realize the serial port operation. The relevant code and instructions can be downloaded at http://codegure.earthweb.com/network/serialpot.shtml.

图10中标出机器人运动行为决策控制的框图，其具体算法为：运动发生器查询当前运行模式参数值(缺省为遥控操作模式)，若为自主避障运行模式，则查询最新的超声波测距值，依照预置的行为规则表选择动作，若为遥控操作模式，则程序查询遥控输入指令直接从运动集合中选择相应的动作(缺省动作为静止)；随后将选定动作对应的期望直行和偏航速度控制命令V_x^d、ω_α^d输出到决策器；决策器读取最近10次接收的底盘和躯干的姿态角θ₁和θ₂、姿态角速度

和

计算它们的均方值Δ₁、Δ₂、Δ₃、Δ₄，若((Δ₁＞25)∧(Δ₂＞25))∨((Δ₃＞2500)∧(Δ₄＞2500))为真，则认为机器人姿态不稳定，决策器输出控制命令为$V_x^c={\mathrm{\&omega;}}_{\mathrm{\&alpha;}}^c=0,$即令机器人保持静止，若((Δ₁＞25)∧(Δ₂＞25))∨((Δ₃＞2500)∧(Δ₄＞2500))为假，决策器输出的控制命令为$V_x^c=V_x^d,$${\mathrm{\&omega;}}_{\mathrm{\&alpha;}}^c={\mathrm{\&omega;}}_{\mathrm{\&alpha;}}^d.$Figure 10 shows the block diagram of the decision-making control of the robot's motion behavior. The specific algorithm is: the motion generator queries the parameter value of the current operating mode (the default is the remote control operation mode), and if it is the autonomous obstacle avoidance operating mode, then query the latest ultrasonic distance measurement Value, select the action according to the preset behavior rule table, if it is the remote control operation mode, the program will query the remote control input command to directly select the corresponding action from the motion set (the default action is still); then the desired action corresponding to the selected action will go straight and yaw speed control commands V_x^d , ω_α^d are output to the decision maker; the decision maker reads the last 10 received attitude angles θ₁ and θ₂ of the chassis and torso, attitude angular velocity

and

Calculate their mean square values Δ₁ , Δ₂ , Δ₃ , Δ₄ , if ((Δ₁ ＞25)∧(Δ₂ ＞25))∨((Δ₃ ＞2500)∧(Δ₄ ＞2500)) is true, the attitude of the robot is considered to be unstable, and the control command output by the decision maker is$V_x^c={\mathrm{\&omega;}}_{\mathrm{\&alpha;}}^c=0,$ That is to say, the robot remains stationary, if ((Δ₁ ＞25)∧(Δ₂ ＞25))∨((Δ₃ ＞2500)∧(Δ₄ ＞2500)) is false, the control command output by the decision maker is$V_x^c=V_x^d,$${\mathrm{\&omega;}}_{\mathrm{\&alpha;}}^c={\mathrm{\&omega;}}_{\mathrm{\&alpha;}}^d.$

其中，机器人自主避障运行模式的行为规则与超声波传感器17反馈的障碍物距离信息直接相关，具体为下表所示：Among them, the behavior rules of the autonomous obstacle avoidance operation mode of the robot are directly related to the obstacle distance information fed back by theultrasonic sensor 17, specifically as shown in the following table:

编号serial numberS1S1S2S2S3S3S4S4动作action编号serial numberS1S1S2S2S3S3S4S4动作action  1 1  1 1  1 1  1 1  1 1  原地左转turn left  9 9  00  1 1  1 1  1 1  向左转弯turn left  2 2  1 1  1 1  1 1  00  向右转弯turn right  1010  00  1 1  1 1  00  向左转弯turn left  33  1 1  1 1  00  1 1  向右转弯turn right  1111  00  1 1  00  1 1  向左转弯turn left  44  1 1  1 1  00  00  向右转弯turn right  1212  00  1 1  00  00  向右转弯turn right  55  1 1  00  1 1  1 1  向左转弯turn left  1313  00  00  1 1  1 1  向左转弯turn left  66  1 1  00  1 1  00  向右转弯turn right  1414  00  00  1 1  00  向左转弯turn left  77  1 1  00  00  1 1  向左转弯turn left  1515  00  00  00  1 1  向左转弯turn left  8 8  1 1  00  00  00  向右转弯turn right  1616  00  00  00  00  前进 go ahead

表中S1-S4代表机器人身前从左至右的四个超声波传感器17，当S1所测距离值D_S1小于1米时，S1＝1，否则S1＝0，S2-S4同理赋值。In the table, S1-S4 represent the fourultrasonic sensors 17 from left to right in front of the robot. When the distance value D_S1 measured by S1 is less than 1 meter, S1=1, otherwise S1=0, and S2-S4 are assigned values in the same way.

监控程序的流程为：程序启动后，主线程初始化串口COM1、COM2(主要设定波特率19200、8位数据位、无奇偶校验、1位停止位、缓冲区512字节等)，创建并启动它们各自的串口监测线程，设置1000ms定时器Timer，最后在无限循环中处理消息队列中消息。当COM1或COM2收到一个字节时，串口监测线程将该字节以消息方式传递给主线程，主线程响应该消息后运行对应的消息处理函数OnCommunication1或OnCommunication2(程序流程如图11所示)，函数中将收到的字节读入缓冲区，判断此字节是否为数据包结尾，若否，退出消息处理函数，若是，则校验该数据包长度值是否正确，若不正确，则放弃该数据包，清空缓冲区，退出消息处理函数，若正确，则按串口通信协议解析该数据包，取出状态数据保存至存储区，更新视窗中对应的显示信息，清空缓冲区，退出消息处理函数。Timer计时满1000ms向主线程发送消息，主线程响应该消息后运行消息处理函数OnTimer(程序流程如图12所示)，函数中读取存储区用户指令和机器人的状态信息，根据运动行为决策算法计算出控制命令V_x^c、ω_α^c；如果控制命令与上一次定时程序计算结果不同，则通过串口COM1向运动控制器(2)发送新的控制命令后退出函数，若相同，则直接退出函数。The process of the monitoring program is: after the program starts, the main thread initializes the serial ports COM1 and COM2 (mainly setting the baud rate to 19200, 8 data bits, no parity, 1 stop bit, buffer 512 bytes, etc.), and creates And start their respective serial port monitoring threads, set the 1000ms timer Timer, and finally process the messages in the message queue in an infinite loop. When COM1 or COM2 receives a byte, the serial port monitoring thread sends the byte to the main thread in the form of a message, and the main thread runs the corresponding message processing function OnCommunication1 or OnCommunication2 after responding to the message (the program flow is shown in Figure 11) , the function reads the received byte into the buffer, and judges whether the byte is the end of the data packet. If not, exit the message processing function. If so, check whether the length value of the data packet is correct. If not, then Abandon the data packet, clear the buffer, and exit the message processing function. If it is correct, analyze the data packet according to the serial communication protocol, take out the state data and save it to the storage area, update the corresponding display information in the window, clear the buffer, and exit message processing function. The timer sends a message to the main thread when the timer reaches 1000ms, and the main thread runs the message processing function OnTimer after responding to the message (the program flow is shown in Figure 12). Calculate the control command V_x^c , ω_α^c ; if the control command is different from the calculation result of the last timing program, send a new control command to the motion controller (2) through the serial port COM1 and exit the function, if they are the same, exit the function directly function.

2.运动控制器2中的控制软件2. Control software inmotion controller 2

运动控制器2的程序采用TI公司的CCS软件开发，并固化在存储单元中。程序以SCI接收中断的方式接收主控制器1控制命令，以25ms定时中断的方式实现机器人姿态平衡和运动的实时控制，并在每次定时中断后向主控制器1发送状态数据。其中，定时中断的优先级高于SCI接收中断优先级。本实施例给出主程序和中断程序的算法流程，涉及到DSP的资源配置、使用，串口的接收、发送的操作等的具体实现，可直接参考TI公司为TMS320F2812DSP提供的例程。The program of themotion controller 2 is developed by the CCS software of TI Company and solidified in the storage unit. The program receives the control command of themain controller 1 in the form of SCI receiving interrupt, realizes the real-time control of the robot's posture balance and motion in the form of 25ms timing interruption, and sends status data to themain controller 1 after each timing interruption. Among them, the priority of timing interrupt is higher than that of SCI receiving interrupt. This embodiment provides the algorithm flow of the main program and the interrupt program, which involves the specific implementation of DSP resource configuration and use, serial port receiving and sending operations, etc., and can directly refer to the routines provided by TI for TMS320F2812DSP.

主程序流程如图13所示：首先进行必要的初始化，完成程序使用的变量的初始化和配置DSP各寄存器状态，主要配置控制中所需的IO端口和A/D通道、通用定时器T1-T4(设置并开启T1为中断周期计时器，T2、T4为编码器的计数器，T3为PWM信号输出的比较计时器)、SCI(设定波特率19200、8位数据位、无奇偶校验、1位停止位、使能收发FIFO缓冲等)。然后，开启PWM输出，向伺服驱动器发送使能信号；接着，使能SCI接收端口RX、T1定时中断、SCI接收中断；最后，进行无限循环等待中断到来，在每次循环中，判断定时中断程序运行标志变量Flag是否为1，若否则无操作，若是则将θ₁、

θ₂、

V_x、ω_α的数值按照串口通信协议经串口发送至主控制器1，随后将Flag置为0。The main program flow is shown in Figure 13: first, perform necessary initialization, complete the initialization of the variables used by the program and configure the status of each DSP register, mainly configure the IO ports and A/D channels required in the control, and general-purpose timers T1-T4 (Set and open T1 as the interrupt cycle timer, T2 and T4 as the encoder counter, T3 as the comparison timer for PWM signal output), SCI (set the baud rate to 19200, 8 data bits, no parity, 1 stop bit, enable transmit and receive FIFO buffer, etc.). Then, turn on the PWM output and send an enable signal to the servo driver; then, enable the SCI receiving port RX, T1 timing interrupt, and SCI receiving interrupt; finally, perform an infinite loop to wait for the interrupt to arrive, and judge the timing interrupt program in each cycle Whether the running flag variable Flag is 1, if not, no operation, if so, set θ₁ ,

θ₂ ,

The values of V_x and ω_α are sent to themain controller 1 through the serial port according to the serial port communication protocol, and then the Flag is set to 0.

当SCI的接收FIFO缓存收到一个字节时产生中断，CPU响应该中断后，保存当前程序现场，转入SCI接收中断程序。如图14，SCI接收中断程序的流程为：首先，关闭SCI接收中断；第二步，从接收FIFO缓存中读取一个字节；第三步，判断该字节是否为数据包末尾，若否，存储该字节，返回第二步读取下一个字节，若是，则判断该数据包长度是否正确，若不正确，则放弃该数据包，执行下一步，若正确，则解析该数据包数据，取出V_x^c和ω_α^c的数值并存储；第四步，清空接收FIFO缓存；最后，打开SCI中断，退出中断程序。When the receiving FIFO buffer of SCI receives a byte, an interrupt is generated. After the CPU responds to the interrupt, it saves the current program site and transfers to the SCI receiving interrupt program. As shown in Figure 14, the flow of the SCI receive interrupt program is: first, close the SCI receive interrupt; second, read a byte from the receive FIFO buffer; third, determine whether the byte is the end of the data packet, if not , store the byte, return to the second step to read the next byte, if yes, judge whether the length of the data packet is correct, if not, discard the data packet, go to the next step, if correct, parse the data packet Data, take out the values of V_x^c and ω_α^c and store them; the fourth step is to clear the receiving FIFO buffer; finally, open the SCI interrupt and exit the interrupt program.

每当T1计时满25ms时产生中断，DSP响应该中断，保存当前程序现场，转入定时中断程序。如图15所示，定时中断程序的流程为：Whenever T1 is over 25ms, an interrupt is generated, and DSP responds to the interrupt, saves the current program scene, and transfers to the regular interrupt program. As shown in Figure 15, the flow of the timer interrupt program is:

Step1：关闭定时器T1中断。Step1: Turn off timer T1 interrupt.

Step2：获取编码器信息。每个编码器输出信号为两组正交编码序列，DSP中的正交编码脉冲电路对这两组信号的上升沿和下降沿均进行计数，因此产生的时钟频率是每组输入序列的四倍，所以读取通用定时器T2、T4的计数器数值后，需除以四才能得到一个周期内编码器输出的脉冲数n_l，n_r。Step2: Obtain encoder information. Each encoder output signal is two sets of orthogonal encoding sequences, and the orthogonal encoding pulse circuit in DSP counts the rising and falling edges of these two sets of signals, so the clock frequency generated is four times that of each set of input sequences , so after reading the counter values of the general timers T2 and T4, it needs to be divided by four to get the number of pulses n_l and n_r output by the encoder in one cycle.

Step3：计算机器人的累积直线位移、累积偏航角度、直线速度和偏航角速度。由编码器信息可知一周期内每个轮子转过的角度

由于控制周期T很短(25ms)，可近似计算轮子转速

根据机器人运动学方程，其直线速度V_x＝(ω_l+ω_r)·R_w/2，转弯速度ω_α＝2(ω_l-ω_r)·R_w/D，其中

为左轮转速，

为右轮转速，D为两轮轮距，R_w为轮子半径。由于控制周期T很短，因此将V_x与T相乘累加到上个控制周期的累积直线位移得到当前累积直线位移，即x(t)＝x(t-T)+V_xT，用同样的方法计算累积偏航角度，即α(t)＝α(t-T)+ω_αT。Step3: Calculate the cumulative linear displacement, cumulative yaw angle, linear velocity and yaw angular velocity of the robot. From the encoder information, we can know the angle that each wheel turns in one cycle

Since the control cycle T is very short (25ms), the wheel speed can be approximated

According to the robot kinematics equation, its linear velocity V_x =(ω_l +ω_r )·R_w /2, turning speed ω_α =2(ω_l -ω_r )·R_w /D, where

is the rotation speed of the left wheel,

is the speed of the right wheel, D is the wheelbase of the two wheels, and_Rw is the radius of the wheel. Since the control cycle T is very short, multiply and accumulate V_x and T to the cumulative linear displacement of the last control cycle to obtain the current cumulative linear displacement, that is, x(t)=x(tT)+V_x T, and use the same method Calculate the cumulative yaw angle, that is, α(t)=α(tT)+ω_α T.

Step4：按照底盘的倾角仪18、陀螺仪19，躯干的倾角仪15、陀螺仪16的顺序，对它们输出的模拟电压信号分别进行A/D转换。为避免采样过程中偶然因素的影响，每个信号均连续采样10次，进行A/D转换，去掉其中最大值和最小值后求均值赋给对应的变量：

Step4: According to the order of theinclinometer 18 andgyroscope 19 of the chassis, theinclinometer 15 and thegyroscope 16 of the trunk, perform A/D conversion on the analog voltage signals output by them respectively. In order to avoid the influence of accidental factors in the sampling process, each signal is continuously sampled 10 times, A/D conversion is performed, and the maximum and minimum values are removed, and the mean value is assigned to the corresponding variable:

Step5：计算底盘和躯干的倾斜角度和倾斜角速度。通过公式：${\mathrm{\&theta;}}_i=(U_{{\mathrm{\&theta;}}_i}-{U0}_{{\mathrm{\&theta;}}_i})K_{\mathrm{\&theta;}},i=\mathrm{1,2},$${\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_i=\left(U_{{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_i}{-U0}_{{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_i}\right)K_{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}},i=\mathrm{1,2},$计算出对应机器人底盘和躯干的倾斜角度和倾斜角速度。其中，U0_x为设定的零位电压值，为2.5V；K_θ＝8，$K_{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}=9$为比例系数，是预先通过固定机器人双轮，摆动机身，将编码器测得的底盘转角、转速分别除以

和

获得。Step5: Calculate the tilt angle and tilt angular velocity of the chassis and torso. By formula:${\mathrm{\&theta;}}_i=(u_{{\mathrm{\&theta;}}_i}-{\mathrm{<figure-callout\; label="u"\; filenames="A20091008425900281.png,A20091008425900311.png"\; state="\{\{state\}\}">u</figure-callout>}0}_{{\mathrm{\&theta;}}_i})K_{\mathrm{\&theta;}},i=\mathrm{1,2},$${\stackrel{\&Center\; Dot;}{\mathrm{\&theta;}}}_i=\left(u_{{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_i}{-\mathrm{<figure-callout\; label="u"\; filenames="A20091008425900281.png,A20091008425900311.png"\; state="\{\{state\}\}">u</figure-callout>}0}_{{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_i}\right)K_{\stackrel{\&Center\; Dot;}{\mathrm{\&theta;}}},i=\mathrm{1,2},$ Calculate the tilt angle and tilt angular velocity corresponding to the robot chassis and torso. Among them, U0_x is the set zero voltage value, which is 2.5V; K_θ =8,$K_{\stackrel{\&Center\; Dot;}{\mathrm{\&theta;}}}=9$ is the proportional coefficient, which is to divide the chassis rotation angle and rotational speed measured by the encoder by

and

get.

Step6：以控制命令V_x^c、ω_α^c为参考输入，以检测到的x、V_x、α、ω_α、θ₁、

θ₂、

为反馈量，通过运动平衡控制算法计算出两个电机控制转矩对应PWM的占空比和转动方向。其中，转矩绝对值范围[0，5](Nm)，对应占空比为0～100％，大于等于零时为正转，小于零时为反转。Step6: Take the control command V_x^c , ω_α^c as the reference input, and take the detected x, V_x , α, ω_α , θ₁ ,

θ₂ ,

As the feedback quantity, the duty cycle and rotation direction of the PWM corresponding to the control torque of the two motors are calculated through the motion balance control algorithm. Wherein, the torque absolute value range is [0, 5] (Nm), and the corresponding duty ratio is 0 to 100%. When it is greater than or equal to zero, it is forward rotation, and when it is less than zero, it is reverse rotation.

Step7：刷新输出至伺服驱动器的PWM占空比和电机转动方向。Step7: Refresh the PWM duty cycle and motor rotation direction output to the servo drive.

Step8：设置计时器T1，重新开始25ms计时，设置T2、T4，重新开始计数。Step8: Set timer T1, restart 25ms timing, set T2, T4, restart counting.

Step9：开T1中断，退出中断程序。Step9: Turn on T1 interrupt and exit the interrupt program.

Step6中提到的运动平衡控制算法为：如图10所示，直行速度控制器K₁根据姿态零位误差和直行速度误差计算出直线速度控制量u₁，偏航速度控制器K₂根据偏航速度误差计算出的偏航速度控制量u₂，[u₁ u₂]^T左乘耦合矩阵D计算出电机控制转矩[τ₁ τ₂]^T；直行速度控制器和偏航速度控制器的设计选用线性二次型最优控制算法：The motion balance control algorithm mentioned inStep 6 is: as shown in Figure 10, the straight speed controller K₁ calculates the straight speedcontrol value u 1 according to the attitude zero position error and the straight speed error, and the yaw speed controller K₂ calculates the control value u₁ according to the yaw speed The yaw speed control variable u₂ calculated from the yaw speed error, [u₁ u₂ ]^T is multiplied by the coupling matrix D to the left to calculate the motor control torque [τ₁ τ₂ ]^T ; the straight speed controller and the yaw speed controller The design uses the linear quadratic optimal control algorithm:

首先，将机器人的线性状态方程分解为直行运动和偏航运动两个单输入子系统。根据机器人的机械系统的特点和参数，建立其数学模型，在直立平衡点线性化获得线性状态空间方程：First, the linear state equation of the robot is decomposed into two single-input subsystems, the straight motion and the yaw motion. According to the characteristics and parameters of the mechanical system of the robot, its mathematical model is established, and the linear state space equation is obtained by linearizing at the upright balance point:

$\stackrel{<span><span>\&CenterDot;</span>\&CenterDot;</span>}{\mathrm{<span><span>X</span>x</span>}}<span><span>=</span>=</span>\mathrm{<span><span>AX</span>AX</span>}<span><span>+</span>+</span>\mathrm{<span><span>Bu</span>Bu</span>}$

其中，$X={\left[\begin{array}{cccccccc}x& \mathrm{\&alpha;}& {\mathrm{\&theta;}}_{b_1}& {\mathrm{\&theta;}}_{b_2}& \stackrel{\&CenterDot;}{x}& \stackrel{\&CenterDot;}{\mathrm{\&alpha;}}& {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{b_1}& {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{b_2}\end{array}\right]}^T,$u＝[τ_l τ_r]^T，in,$x={\left[\begin{array}{cccccccc}x& \mathrm{\&alpha;}& {\mathrm{\&theta;}}_{{\mathrm{<figure-callout\; label="b"\; filenames="A20091008425900251.png,A20091008425900281.png"\; state="\{\{state\}\}">b</figure-callout>}}_1}& {\mathrm{\&theta;}}_{b_2}& \stackrel{\&Center\; Dot;}{x}& \stackrel{\&Center\; Dot;}{\mathrm{\&alpha;}}& {\stackrel{\&Center\; Dot;}{\mathrm{\&theta;}}}_{{\mathrm{<figure-callout\; label="b"\; filenames="A20091008425900251.png,A20091008425900281.png"\; state="\{\{state\}\}">b</figure-callout>}}_1}& {\stackrel{\&Center\; Dot;}{\mathrm{\&theta;}}}_{b_2}\end{array}\right]}^T,$ u=[τ_l τ_r ]^T ,

$\mathrm{<span><span>A</span>A</span>}<span><span>=</span>=</span>\left[\begin{array}{cccccccc}\mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>113.8</span>113.8</span>}& <span><span>-</span>-</span>\mathrm{<span><span>135.9</span>135.9</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1.4</span>1.4</span>}& <span><span>-</span>-</span>\mathrm{<span><span>1.4</span>1.4</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& <span><span>-</span>-</span>\mathrm{<span><span>123.58</span>123.58</span>}& \mathrm{<span><span>137.77</span>137.77</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& <span><span>-</span>-</span>\mathrm{<span><span>14.2</span>14.2</span>}& \mathrm{<span><span>14.2</span>14.2</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>100.83</span>100.83</span>}& <span><span>-</span>-</span>\mathrm{<span><span>102.07</span>102.07</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>10.9</span>10.9</span>}& \mathrm{<span><span>10.9</span>10.9</span>}\end{array}\right]<span><span>,</span>,</span>$$\mathrm{<span><span>B</span>B</span>}<span><span>=</span>=</span>\left[\begin{array}{cc}\mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>1.5726</span>1.5726</span>}& \mathrm{<span><span>1.5726</span>1.5726</span>}\\ \mathrm{<span><span>3.3755</span>3.3755</span>}& <span><span>-</span>-</span>\mathrm{<span><span>3.2755</span>3.2755</span>}\\ <span><span>-</span>-</span>\mathrm{<span><span>9.1867</span>9.1867</span>}& <span><span>-</span>-</span>\mathrm{<span><span>9.1867</span>9.1867</span>}\\ \mathrm{<span><span>2.4738</span>2.4738</span>}& \mathrm{<span><span>2.4738</span>2.4738</span>}\end{array}\right]<span><span>.</span>.</span>$

可得直行运动子系统的状态方程：The state equation of the straight-going motion subsystem can be obtained:

${\stackrel{<span><span>\&CenterDot;</span>\&CenterDot;</span>}{\mathrm{<span><span>X</span>x</span>}}}_{\mathrm{<span><span>1</span>1</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>A</span>A</span>}}_{\mathrm{<span><span>1</span>1</span>}}{\mathrm{<span><span>X</span>x</span>}}_{\mathrm{<span><span>1</span>1</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>B</span>B</span>}}_{\mathrm{<span><span>1</span>1</span>}}{\mathrm{<span><span>u</span><figure-callout\; label="u"\; filenames="A20091008425900251.png,A20091008425900281.png"\; state="\{\{state\}\}">u</figure-callout></span>}}_{\mathrm{<span><span>1</span>1</span>}}$

其中，$X_1={\left[\begin{array}{cccccc}x& {\mathrm{\&theta;}}_{b_1}& {\mathrm{\&theta;}}_{b_2}& \stackrel{\&CenterDot;}{x}& {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{b_1}& {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{b_2}\end{array}\right]}^T,$in,$x_1={\left[\begin{array}{cccccc}x& {\mathrm{\&theta;}}_{{\mathrm{<figure-callout\; label="b"\; filenames="A20091008425900251.png,A20091008425900281.png"\; state="\{\{state\}\}">b</figure-callout>}}_1}& {\mathrm{\&theta;}}_{b_2}& \stackrel{\&CenterDot;}{x}& {\stackrel{\&Center\; Dot;}{\mathrm{\&theta;}}}_{{\mathrm{<figure-callout\; label="b"\; filenames="A20091008425900251.png,A20091008425900281.png"\; state="\{\{state\}\}">b</figure-callout>}}_1}& {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{b_2}\end{array}\right]}^T,$

${\mathrm{<span><span>A</span>A</span>}}_{\mathrm{<span><span>1</span>1</span>}}<span><span>=</span>=</span>\left[\begin{array}{cccccc}\mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>113.8</span>113.8</span>}& <span><span>-</span>-</span>\mathrm{<span><span>135.9</span>135.9</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1.4</span>1.4</span>}& <span><span>-</span>-</span>\mathrm{<span><span>1.4</span>1.4</span>}\\ \mathrm{<span><span>0</span>0</span>}& <span><span>-</span>-</span>\mathrm{<span><span>1.2358</span>1.2358</span>}& \mathrm{<span><span>1.3777</span>1.3777</span>}& \mathrm{<span><span>0</span>0</span>}& <span><span>-</span>-</span>\mathrm{<span><span>14.2</span>14.2</span>}& \mathrm{<span><span>14.2</span>14.2</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1.0083</span>1.0083</span>}& <span><span>-</span>-</span>\mathrm{<span><span>1.0207</span>1.0207</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>10.9</span>10.9</span>}& \mathrm{<span><span>10.9</span>10.9</span>}\end{array}\right]<span><span>,</span>,</span>$${\mathrm{<span><span>B</span><figure-callout\; label="B"\; filenames="A20091008425900251.png,A20091008425900281.png"\; state="\{\{state\}\}">B</figure-callout></span>}}_{\mathrm{<span><span>1</span>1</span>}}<span><span>=</span>=</span>\left[\begin{array}{c}\mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>1.5726</span>1.5726</span>}\\ <span><span>-</span>-</span>\mathrm{<span><span>9.1867</span>9.1867</span>}\\ \mathrm{<span><span>2.4738</span>2.4738</span>}\end{array}\right]$

可得偏航运动子系统的状态方程：The state equation of the yaw motion subsystem can be obtained:

${\stackrel{<span><span>\&CenterDot;</span>\&Center\; Dot;</span>}{\mathrm{<span><span>X</span>x</span>}}}_{\mathrm{<span><span>2</span>2</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>A</span>A</span>}}_{\mathrm{<span><span>2</span>2</span>}}{\mathrm{<span><span>X</span>x</span>}}_{\mathrm{<span><span>2</span>2</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>B</span>B</span>}}_{\mathrm{<span><span>2</span>2</span>}}{\mathrm{<span><span>u</span><figure-callout\; label="u"\; filenames="A20091008425900262.png,A20091008425900281.png"\; state="\{\{state\}\}">u</figure-callout></span>}}_{\mathrm{<span><span>2</span>2</span>}}$

其中，$X_2={\left[\begin{array}{cc}\mathrm{\&alpha;}& \stackrel{\&CenterDot;}{\mathrm{\&alpha;}}\end{array}\right]}^T,$$A_2=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right],$$B_2=\left[\begin{array}{c}0\\ 3.3755\end{array}\right].$in,$x_2={\left[\begin{array}{cc}\mathrm{\&alpha;}& \stackrel{\&CenterDot;}{\mathrm{\&alpha;}}\end{array}\right]}^T,$$A_2=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right],$${\mathrm{<figure-callout\; label="B"\; filenames="A20091008425900262.png,A20091008425900281.png"\; state="\{\{state\}\}">B</figure-callout>}}_2=\left[\begin{array}{c}0\\ 3.3755\end{array}\right].$

u₁、u₂与u的关系为：$u=D\&CenterDot;\left[\begin{array}{c}{u}_1\\ u_2\end{array}\right]=\left[\begin{array}{cc}0.5& 0.5\\ 0.5& -0.5\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\end{array}\right].$The relationship between u₁ , u₂ and u is:$u=\mathrm{D.}\&Center\; Dot;\left[\begin{array}{c}{u}_1\\ {\mathrm{<figure-callout\; label="u"\; filenames="A20091008425900262.png,A20091008425900281.png"\; state="\{\{state\}\}">u</figure-callout>}}_2\end{array}\right]=\left[\begin{array}{cc}0.5& 0.5\\ 0.5& -0.5\end{array}\right]\left[\begin{array}{c}{u}_1\\ {\mathrm{<figure-callout\; label="u"\; filenames="A20091008425900262.png,A20091008425900281.png"\; state="\{\{state\}\}">u</figure-callout>}}_2\end{array}\right].$

其次，通过求得直行运动子系统与偏航运动子系统的能控性矩阵均满秩，证明两者完全能控，系统各状态值又均可测量获得，因此以V_x^c、ω_α^c分别为两个子系统参考输入，状态X₁、X₂为反馈量，构造如图10中标出的运动平衡控制系统，并采用线性二次型最优控制方法设计两个子系统的状态反馈控制器：Secondly, by obtaining the full rank of the controllability matrices of the straight motion subsystem and the yaw motion subsystem, it is proved that the two are completely controllable, and each state value of the system can be measured. Therefore, V_x^c , ω_α^c are the reference inputs of the two subsystems, and the states X₁ and X₂ are the feedback quantities, construct the motion balance control system marked in Figure 10, and use the linear quadratic optimal control method to design the state feedback controllers of the two subsystems:

定义两个子系统的性能指标：Define the performance metrics for the two subsystems:

其中Q₁＝diag[1 100 100 100 10 10]、Q₂＝diag[1 1]为状态变量的加权矩阵，R₁＝R₂＝1为控制量的加权系数。通过Matlab程序的函数K＝lqr(A，B，Q，R)可求得反馈控制律u₁^*＝-K₁X₁，u₂^*＝-K₂X₂，分别使得性能指标极小。Wherein Q₁ =diag[1 100 100 100 10 10], Q₂ =diag[1 1] are the weighting matrix of the state variable, and R₁ =R₂ =1 is the weighting coefficient of the control variable. Through the function K=lqr(A, B, Q, R) of the Matlab program, the feedback control law u₁^* =-K₁ X₁ , u₂^* =-K₂ X₂ can be obtained, respectively making the performance index extremely small.

K₁＝[-2.2361 -137.7072 35.3820 -23.769 -14.9711 -8.8326]K₁ =[-2.2361-137.7072 35.3820-23.769-14.9711-8.8326]

K₂＝[2.2361 22.3755]K₂ =[2.2361 22.3755]

机器人跟踪脉宽直线、旋转速度信号的曲线如图16所示。图17中曲线为调速过程中机器人底盘、躯干部分倾角的变化。The curves of the robot tracking the pulse width straight line and the rotation speed signal are shown in Figure 16. The curve in Fig. 17 is the variation of the inclination angle of the chassis and trunk of the robot during the speed regulation process.

3.辅助控制3中的软件3. Software inAuxiliary Control 3

辅助控制器3的软件在凌阳公司的μ’nSP^TM集成开发环境中编写，通过串口下载并固化到它的存储器中。软件的功能是控制超声波传感器17测距，检测遥控输入指令，并将结果发送给主控制器1。其工作流程如图20所示：程序开始时先进行必要的初始化，如完成程序中各个变量的初始化，设置串口、IO端口和定时器TimerA参数等；然后开始无限循环，每次循环中按照超声波传感器的编号顺序，循环调用GetDistance函数，控制超声波传感器测量与前方障碍物的距离并存储，待4个传感器均测量完毕，将所有测量结果按照通信协议通过串口发送给组织级EPC。在两个超声波传感器工作的间歇，调用GetRemoteKey函数读取遥控接收器的信息，根据返回结果更新遥控器指令代码，并按照通信协议经串口发至组织级EPC。The software of theauxiliary controller 3 is written in Sunplus'μ'nSP^TM integrated development environment, downloaded through the serial port and solidified into its memory. The function of the software is to control the distance measurement of theultrasonic sensor 17, detect the remote control input command, and send the result to themain controller 1. Its working process is shown in Figure 20: when the program starts, perform necessary initialization first, such as completing the initialization of each variable in the program, setting serial port, IO port and timer TimerA parameters, etc.; then start an infinite loop, and follow the ultrasonic wave in each loop The numbering sequence of the sensors calls the GetDistance function cyclically to control the ultrasonic sensor to measure the distance to the obstacle in front and store it. After the measurement of the four sensors is completed, all the measurement results are sent to the organization-level EPC through the serial port according to the communication protocol. During the working interval of the two ultrasonic sensors, call the GetRemoteKey function to read the information of the remote control receiver, update the command code of the remote control according to the returned result, and send it to the organization-level EPC through the serial port according to the communication protocol.

如图18所示，GetRemoteKey函数的流程为：设置IOA低八位为输入，将IOA低八位输入值存储至Key_temp变量，延时10ms，将IOA低八位输入值与Key_temp比较，若相同，则说明遥控按键确实触发，返回Key_temp值，若不同，则说明遥控按键没有触发，返回Null。As shown in Figure 18, the process of the GetRemoteKey function is: set the lower eight bits of IOA as input, store the input value of the lower eight bits of IOA in the Key_temp variable, delay 10ms, compare the input value of the lower eight bits of IOA with Key_temp, if they are the same, It means that the remote control key is indeed triggered, and the value of Key_temp is returned. If it is different, it means that the remote control key is not triggered, and Null is returned.

如图19所示，GetDistance函数的流程为：设置IOA高八位为输出，向第i号超声波传感器对应的IO引脚IOA.i发送一个5us脉冲；设置IOA为高八位为输入，等待IOA.i有上升沿到来，启动计时器TimerA；当IOA.i下降沿到来，停止计时，读取计时数据赋给变量SonarTime，按照公式Distance＝(SonarTime*0.01050917)/2计算出测距结果并返回。As shown in Figure 19, the process of the GetDistance function is: set the high eight bits of IOA as output, send a 5us pulse to the IO pin IOA.i corresponding to the i-th ultrasonic sensor; set IOA as high eight bits as input, and wait for IOA When .i has a rising edge, start the timer TimerA; when the falling edge of IOA.i arrives, stop timing, read the timing data and assign it to the variable SonarTime, calculate the distance measurement result according to the formula Distance=(SonarTime*0.01050917)/2 and return .

4.伺服驱动器4的软件4. Software ofservo drive 4

在AJC55-18配套PC端软件COPLEY MOTION2中，通过输入直流电机(5)参数能够自动计算伺服程序的比例积分(PI)调节参数，简单设置控制系统结构后即可自动生成伺服控制程序，并通过串口下载固化至AJC55-18的存储单元中。该伺服程序控制周期1ms，以DSP输出的PWM信号和转向信号分别为电机参考转矩和参考转向，取电枢电流为负反馈，实现电机转矩PI伺服控制。In the AJC55-18 supporting PC software COPLEY MOTION2, the proportional integral (PI) adjustment parameters of the servo program can be automatically calculated by inputting the parameters of the DC motor (5), and the servo control program can be automatically generated after simply setting the control system structure, and passed The serial port download is solidified into the storage unit of AJC55-18. The servo program has a control cycle of 1 ms. The PWM signal and the steering signal output by the DSP are the reference torque and the reference steering of the motor respectively, and the armature current is used as negative feedback to realize the PI servo control of the motor torque.

三、机器人系统的使用3. The use of robotic systems

使用本实施例的机器人时，可按如下步骤操作：When using the robot of this embodiment, it can be operated as follows:

1.确认机器人的电气系统连接正确，为机器人连接鼠标32。1. Confirm that the electrical system of the robot is connected correctly, and connect the mouse 32 to the robot.

2.将主电源的船型开关60拨至“开”。2. Turn therocker switch 60 of the main power supply to "ON".

3.按下触点开关58启动主控制器1EC5-1717CLDNA。3. Press thecontact switch 58 to start the main controller 1EC5-1717CLDNA.

4.运行RobotControl.exe。4. Run RobotControl.exe.

5.断开鼠标32，将机器人扶稳至直立位置。5. Disconnect the mouse 32 and stabilize the robot to an upright position.

6.打开电机开关59，机器人开始平衡控制。6. Turn on themotor switch 59, and the robot starts balance control.

7.待机器人在直立位置上稳定后，不再扶持。7. After the robot is stable in the upright position, no longer support it.

8.通过遥控器33按钮选择机器人运行模式或动作指令操作机器人运动。8. Use the button 33 on the remote control to select the robot operation mode or the action command to operate the robot movement.

9.停机时，通过遥控器33令机器人静止，扶稳机器人后依次关闭电机开关59、RobotControl.exe、EC5-1717CLDNA、主电源船型开关60。9. When shutting down, use the remote controller 33 to make the robot still, and then turn off themotor switch 59, RobotControl.exe, EC5-1717CLDNA, and the mainpower ship switch 60 in turn after stabilizing the robot.

Claims (9)

Translated fromChinese

1、一种柔性两轮自平衡机器人系统，包括电气系统和机械本体；其中，所述电气系统包括主控制器(1)、运动控制器(2)、辅助控制器(3)、电机伺服驱动器(4)、传感器、输入输出设备、直流电机(5)和电源系统；所述机械本体包括躯干(6)、柔性关节(7)、底盘(8)、保护支架(9)和双轮(10)；其特征在于：1. A flexible two-wheel self-balancing robot system, comprising an electrical system and a mechanical body; wherein the electrical system comprises a main controller (1), a motion controller (2), an auxiliary controller (3), and a motor servo driver (4), sensors, input and output devices, DC motors (5) and power supply systems; the mechanical body includes trunks (6), flexible joints (7), chassis (8), protective brackets (9) and double wheels (10 ); characterized in that:所述躯干(6)为层式框架，顶板(11)上固定有至少一个摄像头(12)，顶板(11)和底板(13)之间由隔板(14)分隔成若干层，躯干安装倾角仪(15)、陀螺仪(16)和超声波传感器(17)；The torso (6) is a layered frame, at least one camera (12) is fixed on the top plate (11), and the top plate (11) and the bottom plate (13) are separated into several layers by a partition (14). instrument (15), gyroscope (16) and ultrasonic sensor (17);所述躯干底板(13)和底盘(8)之间以柔性关节(7)连接；The trunk bottom plate (13) is connected with the chassis (8) by a flexible joint (7);所述底盘(8)为箱式结构，在其内部装载倾角仪(18)、陀螺仪(19)，两个直流电机(5)分别固定在底盘(8)左右两侧内壁上，电机(5)的轴一端与双轮(10)的轴联接，另一端接编码器(20)；底盘(8)底面安装有可拆卸保护支架(9)，支架(9)末端有悬空脚轮(21)；双轮(10)的轴心位于一条直线上；The chassis (8) is a box-type structure, and an inclinometer (18) and a gyroscope (19) are loaded inside it, and two DC motors (5) are respectively fixed on the inner walls of the left and right sides of the chassis (8), and the motors (5 One end of the shaft of ) is connected with the shaft of the double wheel (10), and the other end is connected with the encoder (20); the bottom surface of the chassis (8) is equipped with a detachable protective bracket (9), and there are suspended casters (21) at the end of the bracket (9); The axes of the double wheels (10) are located on a straight line;所述柔性关节(7)的顶端和底端为上、下支承圆盘(22)、(23)，剖面为“凸”字形，圆盘中心的通孔用于穿过电线；上、下支承圆盘以两个同心转动轴(24)铰接，两个转动轴(24)向外的延伸部分，各固定一个双臂扭簧(25)，扭簧(25)的上臂固定在上支承圆盘(22)上，扭簧(25)的下臂固定在下支承圆盘(23)上；在上、下支撑圆盘(22)、(23)之间压紧固定一段圆柱弹簧(26)，其两端分别套在上、下支撑圆盘(22)、(23)的凸台外；The top and bottom ends of the flexible joint (7) are upper and lower support disks (22), (23), the section is "convex" shape, and the through hole in the center of the disk is used to pass through the electric wire; the upper and lower support The disc is hinged with two concentric rotating shafts (24), and the outward extensions of the two rotating shafts (24) are respectively fixed with a double-arm torsion spring (25), and the upper arm of the torsion spring (25) is fixed on the upper supporting disc. (22), the lower arm of the torsion spring (25) is fixed on the lower support disc (23); between the upper and lower support discs (22), (23), a section of cylindrical spring (26) is fixed by compression, and its The two ends are respectively placed outside the bosses of the upper and lower support discs (22), (23);所述主控制器(1)为嵌入式计算机系统；The main controller (1) is an embedded computer system;所述运动控制器(2)选用数字信号处理器系统；The motion controller (2) selects a digital signal processor system;所述辅助控制器(3)选用单片机或数字信号处理器系统；Described auxiliary controller (3) selects single-chip microcomputer or digital signal processor system for use;所述传感器包括：检测机器人躯干(6)和底盘(8)俯仰倾角变化的倾角仪(15)、(18)，倾角速度变化的陀螺仪(16)、(19)，检测电机(5)转角变化的编码器(20)，检测图像信息的摄像头(12)，以及检测障碍物距离信息的超声波传感器(17)；Described sensor comprises: the inclinometer (15), (18) that detects robot torso (6) and chassis (8) pitch angle change, the gyroscope (16), (19) that inclination rate changes, detects motor (5) rotation angle A variable encoder (20), a camera (12) for detecting image information, and an ultrasonic sensor (17) for detecting obstacle distance information;所述电气系统的连接关系为：主控制器(1)与运动控制器(2)、辅助控制器(3)、输入输出设备连接；运动控制器(2)与倾角仪(15)、(18)、陀螺仪(16)、(19)、编码器(20)、伺服驱动器(4)相连接；辅助控制器(3)与超声波传感器(17)和遥控接收器(27)连接；摄像头(12)与主控制器(1)；伺服驱动器(4)与直流电机(5)连接；电源系统连接各电气设备供电。The connection relation of described electrical system is: main controller (1) is connected with motion controller (2), auxiliary controller (3), input and output equipment; Motion controller (2) and inclinometer (15), (18 ), gyroscope (16), (19), encoder (20), servo driver (4) are connected; auxiliary controller (3) is connected with ultrasonic sensor (17) and remote control receiver (27); camera (12 ) is connected with the main controller (1); the servo driver (4) is connected with the DC motor (5); the power supply system is connected with each electrical equipment for power supply.2、根据权利要求1所述的一种柔性两轮自平衡机器人系统，其特征在于：所述电源系统包括：充电电池(28)、转换电压的电源继电器板(29)、监测电池电压的电源监控板(30)。2. A flexible two-wheeled self-balancing robot system according to claim 1, characterized in that: the power supply system includes: a rechargeable battery (28), a power relay board (29) for converting voltage, and a power supply for monitoring battery voltage Monitoring board (30).3、根据权利要求1所述的一种柔性两轮自平衡机器人系统，其特征在于：所述直流电机(5)选用直流力矩电机、带减速器的直流电机或直流无刷电机。3. A flexible two-wheel self-balancing robot system according to claim 1, characterized in that: the DC motor (5) is a DC torque motor, a DC motor with a reducer or a DC brushless motor.4、根据权利要求1所述的一种柔性两轮自平衡机器人系统，其特征在于：所述的输入输出设备包括：键盘(31)、鼠标(32)、遥控器(33)、液晶显示屏(34)、显示器(35)、麦克风(36)、扬声器(37)。4. A flexible two-wheeled self-balancing robot system according to claim 1, characterized in that: said input and output devices include: keyboard (31), mouse (32), remote controller (33), liquid crystal display (34), display (35), microphone (36), loudspeaker (37).5、一种柔性两轮自平衡机器人系统的运动控制方法，包括：主控制器(1)接收来自输入设备的用户操作指令；主控制器(1)监测各传感器反馈信息；主控制器(1)定时参照用户操作指令和传感器反馈信息，通过运动行为决策算法计算机器人的直行和偏航速度控制命令，下达给运动控制器(2)执行；在一个运动控制周期内，运动控制器(2)读取机器人躯干(6)和底盘(8)的倾角仪(15)、(19)、陀螺仪(16)、(20)的反馈信号，与零位值比较得出倾角和倾角速度的误差信号；运动控制器(2)读取电机编码器(20)的反馈信号，计算机器人直行和偏航速度，与主控制器给定的控制命令对比得出误差信号；根据误差信号，运动控制器(2)按预定的运动平衡控制算法计算电机的控制量，发送给伺服驱动器(4)执行；伺服驱动器(4)控制电机(5)运动，电机(5)带动轮子(10)使机器人维持机身平衡以及按指定方式运动，其特征在于：5. A motion control method for a flexible two-wheeled self-balancing robot system, comprising: a main controller (1) receiving user operation instructions from an input device; a main controller (1) monitoring each sensor feedback information; a main controller (1) ) regularly refers to user operation instructions and sensor feedback information, calculates the straight line and yaw speed control commands of the robot through the motion behavior decision-making algorithm, and sends them to the motion controller (2) for execution; within a motion control cycle, the motion controller (2) Read the feedback signals of the inclinometers (15), (19), gyroscopes (16), (20) of the robot trunk (6) and chassis (8), and compare the error signals of the inclination angle and the inclination velocity with the zero value The motion controller (2) reads the feedback signal of the motor encoder (20), calculates the straight line and yaw speed of the robot, and compares the given control command with the main controller to obtain the error signal; according to the error signal, the motion controller ( 2) Calculate the control amount of the motor according to the predetermined motion balance control algorithm, and send it to the servo driver (4) for execution; the servo driver (4) controls the movement of the motor (5), and the motor (5) drives the wheels (10) to make the robot maintain the body Balance and movement in a prescribed manner, characterized by:所述主控制器(1)接收的用户指令，可以是即时指令，也可以是预存指令；The user instruction received by the main controller (1) may be an immediate instruction or a pre-stored instruction;主控制器(1)通过与运动控制器(2)通信间接获取倾角仪(15)、(19)、陀螺仪(16)、(20)和编码器(20)的反馈信息，通过与辅助控制器(3)通信间接获取超声波传感器(17)反馈信息和遥控器(33)输入的指令；The main controller (1) indirectly obtains the feedback information of the inclinometers (15), (19), gyroscopes (16), (20) and encoders (20) by communicating with the motion controller (2), and communicates with the auxiliary control Obtain the feedback information of the ultrasonic sensor (17) and the instruction input by the remote controller (33) indirectly through communication with the device (3);所述主控制器(1)的运动行为决策算法为：动作发生器参考用户命令或超声波测距信息计算出期望直行和偏航速度控制命令，决策器根据机器人姿态平衡的状况判断是否执行期望控制命令；若姿态平衡状况良好，则期望控制命令即为实际控制命令；若姿态平衡状况不佳，则实际控制命令为零，即先调整机器人平衡；The motion behavior decision-making algorithm of the main controller (1) is as follows: the action generator calculates the desired straight-ahead and yaw speed control commands with reference to user commands or ultrasonic ranging information, and the decision maker judges whether to execute the desired control according to the state of robot attitude balance command; if the attitude balance is good, the expected control command is the actual control command; if the attitude balance is not good, the actual control command is zero, that is, adjust the balance of the robot first;运动控制器(2)通过模数(A/D)转换多次采集倾角仪(15)、(18)和陀螺仪(16)、(19)的输出信号，并经过均值滤波等信号处理算法形成一个控制周期计算所用的反馈信号；The motion controller (2) collects the output signals of the inclinometers (15), (18) and gyroscopes (16) and (19) multiple times through analog-to-digital (A/D) conversion, and forms them through signal processing algorithms such as mean value filtering. a feedback signal for the calculation of the control period;所述运动控制器(2)的运动平衡控制算法为：直行速度控制器K₁根据姿态误差信号和直线运动速度误差计算出直线速度控制量u₁；偏航速度控制器K₂根据偏航速度误差计算出的偏航速度控制量u₂；[u₁ u₂]^T左乘耦合矩阵D计算出电机控制转矩[τ₁ τ₂]^T；耦合矩阵D为二阶常数矩阵$\left[\begin{array}{cc}a& a\\ a& -a\end{array}\right];$_The motion balance control algorithm of the motion controller (2) is: the straight speed controller K₁ calculates the linear speed control value u₁ according to the attitude error signal and the linear motion speed error; The yaw speed control value u₂ calculated by the error; [u₁ u₂ ]^T multiplied by the coupling matrix D to the left to calculate the motor control torque [τ₁ τ₂ ]^T ; the coupling matrix D is a second-order constant matrix$\left[\begin{array}{cc}a& a\\ a& -a\end{array}\right];$伺服驱动器(4)通过电机电枢电流反馈闭环，实现电机(5)的转矩控制。The servo driver (4) realizes the torque control of the motor (5) through the motor armature current feedback closed loop.6、根据权利要求5所述的柔性两轮自平衡机器人系统的运动控制方法，其特征在于：所述的预存指令包括：程序存储的动作顺序表、动作指令、任务式指令。6. The motion control method of the flexible two-wheeled self-balancing robot system according to claim 5, wherein the pre-stored instructions include: a program-stored action sequence table, action instructions, and task instructions.7、根据权利要求5所述的柔性两轮自平衡机器人系统的运动控制方法，其特征在于：所述动作发生器的设计方法包括查表、动态规划、专家系统算法。7. The motion control method of the flexible two-wheeled self-balancing robot system according to claim 5, wherein the design method of the motion generator includes table lookup, dynamic programming, and expert system algorithms.8、根据权利要求5所述的柔性两轮自平衡机器人系统的运动控制方法，其特征在于：所述决策器的设计方法包括查表、模糊逻辑、专家系统算法。8. The motion control method of the flexible two-wheeled self-balancing robot system according to claim 5, characterized in that: the design method of the decision maker includes table lookup, fuzzy logic, and expert system algorithms.9、根据权利要求5所述的柔性两轮自平衡机器人系统的运动控制方法，其特征在于：所述直线运动速度控制器和偏航速度控制器的设计方法包括闭环极点配置、线性二次型最优控制、鲁棒控制、模糊控制、自适应控制算法。9. The motion control method of the flexible two-wheeled self-balancing robot system according to claim 5, characterized in that: the design method of the linear motion speed controller and the yaw speed controller includes closed-loop pole configuration, linear quadratic type Optimal control, robust control, fuzzy control, adaptive control algorithm.

Images