CN101590323B - 独轮机器人系统及其控制方法 - Google Patents

Abstract

本发明涉及一种独轮机器人系统及其控制方法，属于智能机器人范畴，涉及一种能够自主进行运动平衡控制进而自主骑行独轮车的静态不平衡机器人及其运动平衡控制方法。本发明的独轮机器人包括机械本体和控制系统；其特征在于：机械本体下部为一个含有可前后转动的独轮(14)的调试支架(13)；控制系统由状态感知传感器、运动控制器(10)和伺服驱动控制器，以及电源系统构成。其中，运动控制器(10)接收状态感知传感器的信号，按照控制程序对接收信号进行处理，进而发出控制指令，伺服驱动控制器接收控制指令，控制电机对机器人的姿态平衡作出调整。本发明提供了一种控制平台，除了机器人学，还涉及控制科学和智能控制领域，可满足多学科科研、教学的需要。

Description

独轮机器人系统及其控制方法

技术领域

本发明属于智能机器人范畴，涉及一种能够自主进行运动平衡控制进而自主骑行独轮车的静态不平衡机器人及其运动平衡控制方法。

背景技术

骑行独轮车是人类(或者其他经过专门训练的智能水平较高的动物)需要较为高级的运动平衡技能才能完成的活动。这种能力需要经过专门的学习、训练才能够具备。研究一种自主机器人，模仿人类这种高级的运动平衡技能完成骑行独轮车的任务，一直是机器人领域里的一项任务。

独轮机器人是模仿人骑独轮自行车的活动，建立的机器人系统，具有很高的仿生性，其出现是人工智能、智能控制、机械设计、机器人学等相关领域互相融合、快速发展的结果。与一般的静态平衡机器人不同，独轮机器人具有明显的动态平衡特点，具有广阔的应用前景：由于其本身特有的复杂平衡控制问题，在科研、展示和娱乐领域可以作为极具特色的平台和工具；利用其动态平衡特性，将它引入复杂地形环境，进行运输、营救和探测；利用其外形纤细的特性将它用作监控机器人，实现对狭窄地方的监控；在航天领域，基于单轮机器人的原理甚至可以开发一种运动自如的特殊月球车。

相比于一般的不需要运动平衡的静态平衡机器人(如4轮移动式机器人)，独轮机器人具有以下显著特点：1)机器人的运动过程基础是达到平衡状态，即机器人必须首先能够稳定直立，才能够进行其他运动，并且这种运动平衡过程是个动态过程；机器人在平衡点附近不停地变化进行调节以保持平衡。这虽然增加了机器人的控制难度，但也同时使其可以完成许多复杂的运动平衡任务；2)由于只有一个轮子，独轮机器人硬件上要更简捷，更轻，运动上也更灵活；3)若在机器人上身机构上加入机械臂，灵巧手，头部等后，机器人能够以一种特有的动态平衡的方式实现相应的任务，如在极窄的路径上骑行，骑过很窄的平衡木、原地转身，甚至可以完成走钢丝的高难度动作。

现有的独轮机器人多为简单的执行层控制结构，即机器人的本体上仅有驱动和控制的基本功能，没有完善的宏观规划和视觉功能等，或者是将需要处理的的信息传送到上位计算机进行处理的方式，而机器人本身没有处理这些信息的能力，例如日本村田制作所开发的骑独轮车的机器人“村田少女”，身高50cm，体重5kg，速度为5cm/s。在控制方面，左右方向(侧倾)通过竖直方向转动机器人体内配备的惯性轮来保持左右平衡。而前后方向(俯仰)上，则通过身体内的电机以链传动方式转动独轮车的车轮来保持平衡。在检测方面，为了控制前后方向和左右方向的平衡，在四个方向上分别配备了一个该公司的倾斜检测陀螺传感器。此外，还配备了用来检测障碍物的超声波传感器、用来收发控制指令等的蓝牙模块、陶瓷振荡器、检测温度的NTC热敏电阻等村田制作所产品。但是“她”没有能够执行任务的机械臂和灵巧手、仍旧是本体没有处理复杂信息的能力而是通过传送到上位机进行处理、传动方式为链式传动，较为复杂、没有可以灵活扩展的调试用脚轮组件、检测方面仅有四个单轴陀螺，会产生角度积分偏移问题，最终导致系统不稳定等。

发明内容

针对上述问题，本发明设计了一种能够骑行独轮车的机器人本体，并且提出了独轮机器人的运动平衡控制方法。利用该机器人系统不仅可以作为一种开放式智能机器人研究开发平台，为控制科学、机器人学、人工智能等领域的研究和教学提供实验对象，还可以成为娱乐、展示的极具特色的工具。

为了实现上述目的，本发明采取了如下技术方案：

独轮机器人系统，包括机械本体和控制系统；机械本体包括铝合金框架构成的刚性机器人躯干，模块化设计的二维云台构成的机器人头颈部，可拆卸的机械臂6，接于机械臂6端部的多指机械手7，由垂直方向上的下运动平衡组件和上平衡组件构成的总体平衡控制结构，其中，下运动平衡组件为独轮14及独轮电机12，上平衡组件为惯性飞轮17及飞轮电机18；控制系统由状态感知传感器、运动控制器10和两个伺服驱动控制器以及电源系统构成，其中，状态感知传感器包括姿态传感器和速度传感器，运动控制器10接收状态感知传感器的信号，在控制程序下将接收信号进行处理，从而发出控制指令，两个伺服驱动控制器接收控制指令，分别通过独轮电机12和飞轮电机18控制独轮14和惯性飞轮17转动，对机器人的姿态平衡作出调整；其特征在于：所述惯性飞轮17是水平放置的，可左右转动；在独轮14四周套有一个可上下调节的调试支架13，在调试支架13下面，前后左右各设置有一个可拆卸的脚轮15。

系统中的惯性飞轮17为双对可拆卸结构，由两个飞轮并列组成，每个飞轮由两个半圆结合而成。

系统中的姿态传感器为三轴惯性导航系统和两轴倾角仪；所述速度传感器为安装在独轮电机12和飞轮电机18中的编码器，且该编码器带有可进行温度补偿调整的温度传感器。

本发明中机器人的机械本体包括铝合金框架构成的刚性机器人躯干；模块化设计的二维云台构成的机器人头颈部；可拆卸的机械臂6；接于机械臂6端部的多指机械手7；由垂直安装的下运动平衡组件和上平衡组件构成的总体平衡控制结构，其中，下运动平衡组件为调试支架13及独轮电机12，上平衡组件为惯性飞轮17及飞轮电机18；控制系统的硬件支持：作为姿态传感器的倾角仪19和陀螺仪20，作为速度传感器的安装于驱动机构尾端的编码器，作为运动控制器10的MTS2812数字信号处理器板卡，两个作为伺服驱动控制器的伺服驱动器ACJ-55-18，作为电源系统的电源板卡5和充电电池模块9。

本机器人的感知传感器还有安装在机器人头部的由双摄像头构成的视觉传感器1、MIC构成的听觉传感器和位于双摄像头之间的红外线传感器2，以及安装在机器人腰部一周的声纳传感器11、遥控接收器；所述控制系统还有由单片机构成的接收声纳传感器11、遥控接收器信号的辅助控制器和由嵌入式计算机系统构成的接收视觉传感器1、红外线传感器2、听觉传感器信号的主控制器3，该主控制器3可以接收运动控制器10和辅助控制器的信息，进一步为运动控制器10提供控制指令，并能通过音箱16发声。

本机器人的脚轮主要用于机器人静态时的支撑和固定，当机器人运动时需要拆除脚轮或将脚轮调高。机器人运动的控制方法如下：

1)主控制器3按照设定的程序或接收来自遥控的用户操作指令，及监测各传感器反馈信息，通过运动行为决策算法计算机器人的直行和偏航速度的控制命令，下达给运动控制器10执行；其决策算法由以下步骤完成：动作发生器参考用户命令或超声波测距信息计算出期望直行和偏航速度控制命令，然后决策器根据机器人姿态平衡的状况判断是否执行期望控制命令，若姿态平衡状况良好，则期望控制命令即为实际控制命令，若姿态平衡状况不佳，则实际控制命令为零，即先调整机器人平衡；其中，动作发生器的设计可选：查表、动态规划或专家系统算法；决策器的设计可选：查表、模糊逻辑或专家系统算法；

2)在一个运动控制周期内，运动控制器10读取机器人底座上的倾角仪19、陀螺仪20的反馈信号，与零位值比较得出倾角和倾角速度的误差信号；其中的反馈信号由以下方法获得：运动控制器10多次采集倾角仪19和陀螺仪20的输出信号，并经过DSP信号处理算法形成一个控制周期计算所用的反馈信号；

3)运动控制器10读取电机编码器的反馈信号，计算机器人速度，与主控制器3给定的控制命令对比得出误差信号；

4)根据误差信号，运动控制器10按预定的运动平衡控制算法计算电机的控制量，发送给伺服驱动控制器执行；其运动平衡控制算法为：利用解耦控制的思想，将获得的俯仰倾角Angle_FB和俯仰倾角速度AngleAcc_FB信号分解到机器人的俯仰自由度方向，将获得的左右倾角Angle_LR和左右倾角速度AngleAcc_LR信号分解到机器人的左右自由度方向，然后利用既定的PID控制算法，或者模糊控制，或者LQR控制算法，或者极点配置算法，或者鲁棒控制算法，或者其他控制算法计算得到所需的控制量U_U、U_D，然后将控制量U_D和控制量U_U分别用以控制独轮电机M_D、惯性飞轮电机M_U；

5)伺服驱动控制器控制独轮电机12、飞轮电机18带动调试支架13中的独轮14、上运动平衡组件中的惯性飞轮17，使机器人维持机身平衡以及按指定方式运动；其中，通过电机电枢电流反馈闭环，实现电机的转矩控制，且伺服控制周期远小于机器人运动控制周期。

当调试支架13中拆除前后脚轮15，并使独轮14和左右脚轮15在同一水平线，机器人成为倒立摆，其控制方法服从倒立摆控制方法，也可以采用上述的控制方法。当调试支架13中拆除左右脚轮15，将前后脚轮换横装脚轮，并使独轮14和前后脚轮15在同一水平线，使机器人成为能够自主骑行自行车的机器人，其控制也可以采用上述的控制方法。

本发明中的机器人可以简化成如下最小结构：只保留机械本体中的机器人躯干、总体平衡控制结构，控制系统的硬件支持中的倾角仪19、陀螺仪20、编码器，MTS2812数字信号处理器板卡、两个伺服驱动器ACJ-55-18、电源板卡5和充电电池模块9。最小结构机器人的控制方法只针对运动控制器10和伺服驱动控制器，其控制程序分别运行在运动控制器10和独轮、飞轮伺服驱动控制器4、8中，用于机器人前后左右的平衡。运动控制器10和伺服驱动控制器的控制程序分别如下：

(1)运动控制器10中的程序：

程序开始时，首先进行必要的初始化，完成程序使用的变量的初始化和配置DSP各寄存器状态，主要配置控制中所需的I/O端口和A/D通道、通用定时器T1-T4，其中T1为中断周期计时器，T2、T4为编码器的计数器，T3为PWM信号输出的比较计时器；然后，开启PWM输出，向伺服驱动控制器发送使能信号；最后，进行无限循环等待中断到来，在每次循环中实施控制程序的检测和控制算法；

每当T1计时满25ms时产生中断，DSP响应该中断，保存当前程序现场，转入定时中断程序，其流程(控制程序)为：

1)关闭定时器T1中断，准备开始；

2)获取编码器信息。每个编码器输出信号为两组正交编码序列，DSP中的正交编码脉冲电路对这两组信号的上升沿和下降沿均进行计数，因此产生的时钟频率是每组输入序列的四倍，所以读取编码器的计数器T2、T4的数值后，除以四，得到一个周期内编码器输出的脉冲数n_u，n_d；

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

由于控制周期T很短(25ms)，可近似计算轮子转速其直线速度V_x＝ω·R/2，其中R为轮子半径，由于控制周期T很短，因此将V_x与T相乘累加到上个控制周期的累积直线位移得到当前累积直线位移，即x(t)＝x(t-T)+V_xT；

4)对倾角仪19输出的模拟电压信号进行A/D转换和信号获取：为避免采样过程中偶然因素的影响，每个信号均连续采样10次，进行A/D转换，去掉其中最大值和最小值后求均值赋给对应的变量：

通过公式：θ＝(U_θ-U₀)/S₂，其中θ是计算得到的某个自由度上的倾角，U_θ是计算得到的电压均值，U₀是零位电压，S₂是倾角仪19的灵敏度，计算出对应机器人的倾斜角度；

5)通过SPI端口直接获得前后自由度倾角和左右自由度倾角的倾角速度信号

6)以控制命令X^C为参考输入，以检测到的x、V_x、θ₁、

θ₂、为反馈量，通过运动平衡控制算法计算出两个电机控制转矩对应PWM的占空比，其中，转矩绝对值范围[0，6]N·m，对应占空比为0～100％，大于等于零时为正转，小于零时为反转；运动平衡控制其算法为：

i.根据机器人的机械系统的特点和参数，建立其数学模型，在直立平衡点线性化获得线性状态空间方程：

其中，

u＝[τ_d τ_u]^T，俯仰方向倾角θ₁、俯仰方向倾角速度

左右方向倾角θ₂、左右方向倾角速度

ω为轮子旋转的角度，

为轮子旋转的角速度，η为惯性飞轮17旋转的角度，

为惯性飞轮17角速度，

$A=\left[\begin{array}{cccccccc}0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 1\\ 24.9770& 0& 0& 0& 0& 0& 0& 0\\ 0& 60.9590& 0& 0& 0& 0& 0& 0\\ 0& -108.3715& 0& 0& 0& 0& 0& 0\\ -24.9770& 0& 0& 0& 0& 0& 0& 0\end{array}\right],$ $B=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ 0& 0\\ 0& -1.4159\\ -14.3988& 0\\ 40.4128& 0\\ 0& 134.7493\end{array}\right]$

ii.通过求得俯仰运动子系统与侧倾运动子系统的能控矩阵均满秩，证明两者完全能控，系统各状态值又均可测量获得，

iii.利用解耦控制的思想，将整个机器人系统的倾角控制任务分为俯仰自由度的倾角控制和左右自由度的倾角控制器，因此以

分别为两个子系统参考输入，即两个自由度的倾角为反馈量，构造运动平衡控制系统，并采用PID控制方法设计内环的两个倾角控制子系统的状态反馈控制器：PID控制参数采用临界震荡法获得，具体为：

内环俯仰自由度控制：

侧倾自由度控制：

$\left\{\begin{array}{c}{K}_p^d=20000\\ K_i^d=3\\ K_d^d=20000\end{array}\right.$

用同样的方法获得机器人的位移控制外闭环的PID控制参数：

外环X控制： $\left\{\begin{array}{c}{K}_p^x=0.02\\ K_i^x=0\\ K_d^x=0.148\end{array}\right.;$

7)刷新输出至伺服驱动的PWM占空比和电机转动方向；

8)设置计时器T1，重新开始25ms计时，设置T2、T4，重新开始计数；

9)开T1中断，退出中断控制程序；

(2)伺服驱动控制器的控制程序：

在伺服驱动器ACJ-55-18配套PC端的软件COPLEY MOTION2中，通过输入直流电机参数能够自动计算伺服程序的PI调节参数，简单设置控制系统结构后即可自动生成伺服控制程序，并通过串口下载固化至ACJ-55-18的存储单元中，该伺服程序控制周期1ms，以DSP输出的PWM信号和转向信号分别为电机参考转矩和参考转向，取电枢电流为负反馈，实现电机转矩PID伺服控制。

与现有的静态平衡机器人(如四轮移动式机器人)相比，本发明提供了一种控制平台，除了机器人学，还涉及控制科学和智能控制领，可满足多学科科研教学的需要，并且具有以下优点：

1)两个自由度的耦合使得系统的非线性和不确定性增加，更加适合非线性控制、鲁棒控制、智能控制和学习控制的研究。本发明的可调式调试支架不仅可以根据机器人调试的需要改变脚轮15的支撑高度，而且可以将其中一个自由度方向的脚轮15拆除，使机器人相当于三轮一线式移动倒立摆，或者两轮自平衡机器人。

2)控制系统可分为主控制器3、运动控制器10、伺服驱动控制器三层级别，符合生物分级控制结构，各级控制器各司其职，使得机器人的处理能力和智能水平有所提高，能够有效进行各种复杂控制算法、运动控制技能及其他智能行为的学习等的运算。

3)机器人结构采用模块化的设计思想，最小系统以为的组件都可以拆卸更换，这为系统的维护、升级以及用户根据需求进行柔性组装提供了极大的方便。如：可增设视觉传感系统和语音处理系统，可运行视觉识别、语音识别功能；可配备遥控系统和避障系统，使得机器人具有遥控操作和自主避障运行的双重模式。

附图说明

图1为独轮机器人的机械结构主视图；

图2为独轮机器人的机械结构侧视图；

图3为独轮机器人的电气系统原理图；

图4为独轮机器人的运动控制器主程序流程图；

图5为独轮机器人最小系统的电气系统连线图；

图6为独轮机器人的运动控制系统框图；

图7为独轮机器人最小系统结构主视图。

图中：1、视觉传感器，2、红外线传感器，3、主控制器，4、飞轮伺服驱动控制器，5、电源板卡，6、机械臂，7、机械手，8、独轮伺服驱动控制器，9、充电电池模块，10、运动控制器，11、声纳传感器，12、独轮电机，13、调试支架，14、独轮，15、脚轮，16、音箱，17、惯性飞轮，18、飞轮电机，19、倾角仪，20、陀螺仪。

具体实施方式

下面结合附图图1-图7对本发明进行详细说明：

本发明的自主骑行独轮车的机器人系统包括机械本体和电气系统(控制系统)，其中机械本体的总体结构为：

躯干：铝合金支架构成的刚性机器人本体设计。

头颈部：二维云台的模块化设计，机器人可上下俯仰、左右转动头部。

机械臂6：为了完成某些具体任务，使用能够进行复杂操作的、模块化的可拆卸机械臂。

机械手7：具有能够操作的、可更换的、模块化多指机械手来完成任务。

总体平衡控制结构：下运动平衡组件(下轴系组件)和上平衡组件(下轴系组件)垂直安装结构。

设计原则：模块化的总体设计思想，每个组件都是一个整体，可以方便地拆卸、更换，例如机器人的头部，可以安装带双目视觉、听觉以及音箱16的具有二维自由度的模块化头部，也可以安装具有单目视觉功能的模块化头部。二者的更换只需要拆下固定头部的四个螺钉，然后换上新的模块化头部即可。机械臂6、机械手7、声纳传感器11等均为模块化设计，替换、拆卸等都极为简单。不同的模块只要符合接口标准就可以完全通用。惯性飞轮17、独轮14、调试支架13等全部设计为可以拆卸、更换，或者变换高度；电机功率大小可更换，预留更换空间。

机械本体包括8个主要组件：

1.机器人底座组件：安装有姿态传感器(包括倾角仪19、陀螺仪20)。

2.腰部组件：安装有充电电池模块9，和声纳传感器11。

3.上身组件：安装有组织级主控制器3：EC3-1811工控机、控制级运动控制器10：MTS2812数字信号处理器板卡、控制级辅助控制器：MPCE061A开发板、飞轮和独轮伺服驱动控制器：两个伺服驱动器ACJ-55-18、电源板卡5、以及USB转串口通信模块Visual USB RS232模块。

4.下运动平衡组件(下轴系组件)：包括独轮14、独轮电机12、减速器、编码器(速度传感器)、以及滚珠轴承等。独轮14可前后向设置(独轮14的转轴呈左右向)，实现了机器人前进方向的骑行运动和平衡控制。

5.上平衡组件(上轴系组件)：包括惯性飞轮17、飞轮电机18、减速器、编码器(速度传感器)、以及滚珠轴承等。实现机器人左右方向的平衡控制。惯性飞轮17水平安装(其转动轴在竖直方向)。惯性飞轮为双对可拆卸结构，当一种惯性飞轮不满足控制要求或者需要更换其他飞轮进行相关实验时，可将其拆卸，然后安装上需要的飞轮，在这个过程中不需要将上轴系拆下。这样就有效增加了系统的灵活性。

6.头部组件：可安装视觉传感器1、发声装置(音箱16)、转动和俯仰自由度控制舵机、二维云台的机器人脖子等。

7.机械臂组件：设计有四自由度机械臂6。

8.灵巧手组件：在机械臂6端部设计有三指的九自由机械手7。

其中，1、2、3项构成了机器人躯干，1、2、3、4、5项构成了机器人的基本结构。

电气系统主要分为：感觉系统，控制系统，电源系统三个系统。

感觉系统是机器人的“眼睛”，“耳朵”等获取外界信息的组件，主要包括：视觉传感器1、红外线传感器2、声纳传感器11、姿态传感器、速度传感器、听觉传感器、遥控接收。

视觉传感器1用于检测机器人前方的视觉图像，采集图像信息传送至大脑(组织级)进行处理；

红外线传感器2——检测前方有无人类活动。本实施例中红外线传感器为红外微型人体感应模块KT-0003B。

声纳传感器11——安装在机器人腰部的超声波传感器11相当于蝙蝠的嘴和耳朵，当它们工作时，机器人能够利用该传感器测得周围物体的距离。从而可以组成声纳壁障系统，进而完成避障任务；

姿态传感器——倾角仪19和陀螺仪20，倾角仪19用于检测机器人相对于竖直位置的俯仰倾角、左右倾角。陀螺仪20用于检测机器人三轴倾角速度。二者为机器人提供了重要的自身状态信息。

速度传感器是安装于电机尾端的光电编码器或者磁编码器。速度传感器将速度信号转化为电信号传送到运动控制器10。本实施例中采用磁编码器作为速度传感器。

听觉传感器——接收声音信号，例如人的语音信号等。本实施例中听觉传感器采用MIC，集成于摄像头上。

遥控接收——接收遥控器临时发出的指令。

控制系统是机器人的神经中枢，它利用感觉系统提供的信号进行各种处理和映射，最终得到机器人的各种实时动作指令。它主要包括机器人的驱动级的肌肉(伺服驱动控制器及电机)，控制级的小脑控制中枢神经系统(运动控制器)和组织级的大脑神经中枢系统(主控制器)。其中，飞轮、独轮电机及其伺服驱动控制器相当于系统的肌肉；数字信号处理器(DSP)——平衡和动作控制中枢和辅助管理系统相当于系统的小脑控制中枢神经系统，即运动控制器10和辅助控制器；工业控制计算机(EC3-1811)作为宏观规划和管理中枢相当于机器人的大脑神经中枢，即主控制器3。

电源系统相当于机器人的消化系统，它为机器人提供了思考和动作的所有能源。电源系统主要由电源板卡5和充电电池模块9组成。

主控制器3为嵌入式计算机(EPC)系统，如EC3-1811工控机。

运动控制器10选用数字信号处理器(DSP)系统。

辅助控制器选用单片机(MCU)、DSP或其他嵌入式系统。

伺服驱动控制器选用伺服驱动器ACJ-55-18。

飞轮、独轮电机可选用直流力矩电机、带减速器的直流有刷电机或直流无刷电机。

电源系统：充电电池模块9选用锂电池LBS-100C、电源板卡5选用PW4512。

所述电气系统的连接关系为：主控制器3与运动控制器10、辅助控制器、输入输出设备连接；运动控制器10与倾角仪19、陀螺仪20、编码器、伺服驱动控制器相连接；辅助控制器与超声波传感器和遥控接收器(与辅助控制器一体安装)连接；视觉传感器1、红外传感器、听觉传感器(MIC)、音箱16(发声装置)与主控制器3的输入输出连接；充电电池模块9经电源板卡5给各电气设备供电。

如图4所示，机器人运动控制方法：主控制器3接收来自输入设备(遥控接收器)的用户操作指令；监测各传感器反馈信息；定时参照用户操作指令和传感器反馈信息，通过运动行为决策算法计算机器人的直行和偏航速度控制命令，下达给运动控制器10执行；在一个运动控制周期内，运动控制器10读取机器人底座上的倾角仪19、陀螺仪20的反馈信号，与零位值比较得出倾角和倾角速度的误差信号；运动控制器10读取电机上编码器的反馈信号，计算机器人直行和偏航速度，与主控制器3给定的控制命令对比得出误差信号；根据误差信号，运动控制器10按预定的运动平衡控制算法计算电机的控制量，发送给伺服驱动控制器执行；伺服驱动控制器控制飞轮、独轮电机运动，电机带动惯性飞轮18、独轮14运动，使机器人维持机身平衡以及按指定方式运动，其中：

主控制器3接收的用户指令，既可以是即时指令，如遥控器键入等，也可以是预存的指令，如程序存储的动作顺序表等；既可以是动作指令，如前进、转弯等，也可以是任务式指令，如自主避障运行等；

主控制器3通过与运动控制器10通信间接获取倾角仪19、陀螺仪20和编码器的反馈信息，通过与辅助控制器通信间接获取超声波传感器反馈信息和遥控输入的指令；

主控制器3的运动行为决策算法为：首先，动作发生器参考用户命令或超声波测距信息计算出期望直行和偏航速度控制命令，然后决策器根据机器人姿态平衡的状况判断是否执行期望控制命令，若姿态平衡状况良好，则期望控制命令即为实际控制命令，若姿态平衡状况不佳，则实际控制命令为零，即先调整机器人平衡；动作发生器的设计可选：查表、动态规划、专家系统等算法；决策器的设计可选：查表、模糊逻辑、专家系统等算法。

运动控制器10通过模数(A/D)转换和SPI端口多次采集倾角仪19和陀螺仪20的输出信号，并经过滤波等信号处理算法形成一个控制周期计算所用的反馈信号。

运动控制器10的运动平衡控制算法为：利用解耦控制的思想，将获得的倾角Angle_FB、Angle_LR信号和倾角速度AngleAcc_FB、AngleAcc_LR信号分解到机器人的俯仰自由度方向和左右自由度方向，然后利用既定的PID控制算法，或者模糊控制，或者LQR控制算法，或者极点配置算法，或者鲁棒控制算法，或者其他控制算法计算得到所需的控制量U_U、U_D，然后将控制量分别用以控制独轮电机14、飞轮电机18运动。

伺服驱动控制器通过电机电枢电流反馈闭环，实现电机的转矩控制，伺服控制周期远小于机器人运动控制周期。

以上控制方法适用于拆除所有脚轮，只有独轮的情况；也适用于拆除前后脚轮的情况。当拆除前后脚轮时，本系统为倒立摆，可以采用倒立摆的控制方法，也可以采用上述控制方法。

下面介绍本发明最小系统实施例。(最小系统的结构如图7所示)

一、机器人的硬件

1.电气系统选型

运动控制器10选用飓风数字系统(北京)有限公司MSK2812系统板。该系统的处理器采用TI公司TMS320F2812DSP，系统为5V直流供电。

MSK2812的仿真器选用飓风数字系统(北京)有限公司的XDS510USB，USB2.0接口。伺服驱动控制器选用Copley Motion公司的伺服驱动器ACJ-55-18。倾角仪19选用Crossbow公司的CXTA-02双轴倾角仪。陀螺仪20选用Analog Device公司的ADIS16355三轴惯导系统。

机器人的驱动电机(独轮电机12和飞轮电机18)选用Maxon公司的直流无刷电机套件EC32，24V供电，80W功率，190:1的行星齿轮减速器GP32C，电机配有增量式光电编码器，精度为500线。

充电电池模块9选用LBS-100C标准锂电池，标称电压：29.6V，工作范围：33.6V-24V，标称容量：150Wh，保护电路：内置过充、过放、过流及短路保护，集成电量监控。

电源板卡5选用华北工控的PW-4512电源模块，给控制器及其他电子设备供电，输入电压：16-40V DC，输出电压：ATX：+3.3V5A，+5V/+5VSB5A，+12V5A，-12V0.8A，

2.机械结构与电气元件布局

本实施例总重量8kg，高度520mm，宽度300mm，长度220mm，独轮14的直径210mm。机器人的机械结构和电器元件布局如下：

如图7所示，整个机器人为铝合金框架，分为底座，腰部，上身三层。各部分均为前后左右敞口的立柱支撑结构，便于各种电子器件的安装和上身电机组件的固定。各个部分的接口板均为具有中空和加固立环结构的铝合金方形板。上身上胸腔的外部前立面，利用亚克力板固定运动控制器10和电源板卡9，右立面固定飞轮伺服驱动控制器4。中间空出充足的空间，在前后立柱上加装电机固定板，用以在机器人的上身中心位置安装上惯性飞轮组件。下胸腔右立面利用亚格力隔板固定下轴系驱动器(调试支架13中的独轮14)。中间有类似人类的胸腔隔膜(中空和加固立环结构的铝合金方形加固板)。腰部与胸腔的加固板上固定充电电池模块9(LBS-100C标准锂电池)。底座的主固定板中心装倾角仪19和陀螺仪20。底座的竖装支架上固定有调试支架13，调试支架13下面的前后左右装有四个脚轮15。

3.电气系统连接

如图5所示，电气系统各部分的连接方法如下：

MSK2812板由PW-4512电源模块的+5V输出供电，它的J7接口的36，35脚，即A/D转换输入通道，38，39脚，即SPI引脚，分别与倾角仪CXTA02和陀螺仪ADIS16355的SPI信号输出端连接；陀螺仪ADIS16355由MSK2812的J7接口提供+5V输出供电；倾角仪CXTA02则由PW-4512电源模块的24V输出供电。

MSK2812与两个伺服驱动器ACJ-55-18间的连接包括控制信号线和编码器反馈信号线。控制信号包括电机使能信号、电机转动方向信号和PWM转速控制量信号。其中，MSK2812的J5接口的3、7脚分别与控制飞轮、独轮电机的伺服驱动器ACJ-55-18的J5接口的3脚连接，作为伺服驱动器ACJ-55-18的使能信号线；MSK2812的J5接口的5、1脚分别与控制飞轮、独轮电机ACJ-55-18的J5接口的6脚连接，作为电机转动方向选择信号线；MSK2812的J7接口17、18脚为PWM输出，分别与控制飞轮、独轮电机的伺服驱动器ACJ-55-18的J5接口的20脚连接，作为转速控制量信号线。飞轮、独轮电机编码器的反馈信号经伺服驱动器ACJ-55-18缓存后连接至MSK2812，具体接线为飞轮、独轮电机的伺服驱动器ACJ-55-18的J5接口的10、11脚，分别接MSK2812的J7接口的27、28脚和J6接口的13、14脚。

两个伺服驱动器ACJ-55-18的J3接口的3、4脚为电源输入端，分别接电源输出的+24V和GND；J2接口的3、4脚为控制电压的输出端，分别与电机的+/-输入端连接，其中3脚与电机+输入端之间串接一个电机开关；J4接口的4、6分别为+5V和GND，分别与编码器排线的2、3线连接，J4接口的1、8、2、9、3、10脚为编码器A通道、B通道和零位信号的共模输入端，分别接编码器排线的5、6、7、8、9、10线。

LBS-100C标准锂电池经一个双刀双掷的船型开关与PW-4512电源模块连接。PW-4512电源模块的+/-输入端连接LBS-100C标准锂电池的+/-端，提供±12V，±5V，+3.3V直流输出，分别连接各对应设备的供电端。

4.电气系统的工作原理

本实施例机器人的主要功能是在保持机身俯仰姿态平衡和左右姿态平衡的前提下，能够实现控制机器人实现前后向的运动。由此，机器人电气系统的工作原理如图3所示：机器人的运动控制器10得到倾角仪19、陀螺仪20的反馈信号，经过伺服驱动控制器中继读取编码器反馈信号，然后，综合接收到得控制命令和反馈信号，按预定的运动平衡控制算法计算出电机的转矩控制量，发送对应的PWM信号给伺服驱动控制器执行；独轮伺服驱动控制器8控制独轮电机12运动，独轮电机12带动独轮14保持机器人前后方向的平衡并且实现前后方向的运动。飞轮伺服驱动控制器4控制飞轮电机18旋转运动，飞轮电机18带动惯性飞轮17使机器人维持机身左右方向的平衡。

二、机器人系统的运动控制方法

本实施例给出独轮机器人的前进运动模式控制系统的软件实现，其他如机器人智能行为决策、语音操作等控制功能可以参考本实施例软件，添加相应硬件和软件模块进行设计。

整个控制系统由两部分软件实现，分别运行在运动控制器10和伺服驱动控制器中。

1.运动控制器中的控制软件(其原理如图6)

运动控制器10的程序采用TI公司的CCS软件开发，并固化在存储单元中。程序以25ms定时中断的方式实现机器人姿态平衡和运动的实时控制。本实施例给出主程序算法流程，涉及到DSP的资源配置、使用，可直接参考TI公司为TMS320F2812DSP提供的例程。

主程序开始时，首先进行必要的初始化，完成程序使用的变量的初始化和配置DSP各寄存器状态，主要配置控制中所需的IO端口和A/D通道、通用定时器T1-T4(设置并开启T1为中断周期计时器，T2、T4为编码器的计数器，T3为PWM信号输出的比较计时器)。然后，开启PWM输出，向伺服驱动控制器发送使能信号；最后，进行无限循环等待中断到来，在每次循环中实施主程序流程图中的检测和控制算法。

每当T1计时满25ms时产生中断，DSP响应该中断，保存当前程序现场，转入定时中断程序。如图11所示，定时中断程序(主控制程序)的流程为：

Step1：关闭定时器T1中断，准备开始。

Step2：获取编码器信息。每个编码器输出信号为两组正交编码序列，DSP中的正交编码脉冲电路对这两组信号的上升沿和下降沿均进行计数，因此产生的时钟频率是每组输入序列的四倍，所以读取通用定时器T2、T4的计数器数值后，需要除以四，才能得到一个周期内编码器输出的脉冲数n_u，n_d。

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

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

其直线速度V_x＝ω·R/2，其中R为轮子半径。由于控制周期T很短，因此将V_x与T相乘累加到上个控制周期的累积直线位移得到当前累积直线位移，即x(t)＝x(t-T)+V_xT。

Step4：对倾角仪19输出的模拟电压信号进行A/D转换和信号获取。为避免采样过程中偶然因素的影响，每个信号均连续采样10次，进行A/D转换，去掉其中最大值和最小值后求均值赋给对应的变量：

计算底盘和躯干的倾斜角度和倾斜角速度。通过公式：θ＝(U_θ-U₀)/S₂，其中θ是计算得到的某个自由度上的倾角，U_θ是计算得到的电压均值，U₀是零位电压，S₂是倾角仪19的灵敏度，计算出对应机器人的倾斜角度。

Step5：通过SPI端口直接获得前后自由度倾角和左右自由度倾角的倾角速度信号

Step6：以控制命令X^C为参考输入，以检测到的x、V_x、θ₁、

θ₂、

为反馈量，通过运动平衡控制算法计算出两个电机控制转矩对应PWM的占空比。其中，转矩绝对值范围[0，6](Nm)，对应占空比为0～100％，大于等于零时为正转，小于零时为反转。

Step7：刷新输出至伺服驱动控制器的PWM占空比和电机转动方向。

Step8：设置计时器T1，重新开始25ms计时，设置T2、T4，重新开始计数。

Step9：开T1中断，退出中断控制程序。

Step6中提到的运动平衡控制算法为：如图10所示，系统采用双闭环的控制结构，首先最外环是位置(位移)控制器K₁，K₁根据位置误差和位移控制算法，计算出直线位移控制量U_X，U_X作为内环的控制给定，传递给内环-倾角控制器K₂，K₂利用测得的现有俯仰方向倾角θ₁、俯仰方向倾角速度左右方向倾角θ₂、左右方向倾角速度通过耦合算法计算得到电机控制转矩[τ_u τ_d]^T；位移控制器的设计选用PID控制算法；倾角控制器也采用广泛应用的PID控制算法。整个控制系统构成双闭环控制结构。

具体控制算法实施过程为：首先，根据机器人的机械系统的特点和参数，建立其数学模型，在直立平衡点线性化获得线性状态空间方程：

$\stackrel{\·}{X}=\mathrm{AX}+\mathrm{Bu}$

其中，

u＝[τ_d τ_u]^T，俯仰方向倾角θ₁、俯仰方向倾角速度

左右方向倾角θ₂、左右方向倾角速度

ω为轮子旋转的角度，

为轮子旋转的角速度，η为惯性飞轮17旋转的角度，

为惯性飞轮17角速度。

$A=\left[\begin{array}{cccccccc}0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 1\\ 24.9770& 0& 0& 0& 0& 0& 0& 0\\ 0& 60.9590& 0& 0& 0& 0& 0& 0\\ 0& -108.3715& 0& 0& 0& 0& 0& 0\\ -24.9770& 0& 0& 0& 0& 0& 0& 0\end{array}\right],$ $B=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ 0& 0\\ 0& -1.4159\\ -14.3988& 0\\ 40.4128& 0\\ 0& 134.7493\end{array}\right]$

其次，通过求得俯仰运动子系统与侧倾运动子系统的能控矩阵均满秩，证明两者完全能控，系统各状态值又均可测量获得。

再利用解耦控制的思想：将整个机器人系统的倾角控制任务分为俯仰自由度的倾角控制和左右自由度的倾角控制器。因此以分别为两个子系统参考输入，状态θ_i、

(i＝u，d；)，即两个自由度的倾角，为反馈量，构造图15中运动平衡控制系统，并采用PID控制方法设计内环的两个倾角控制子系统的状态反馈控制器：

采用临界震荡法获得具体的PID控制参数为：

内环俯仰自由度控制：

侧倾自由度控制：

用同样的方法获得机器人的位移控制外闭环的PID控制参数：

外环X控制： $\left\{\begin{array}{c}{K}_p^x=0.02\\ K_i^x=0\\ K_d^x=0.148\end{array}\right.;$

从而构成整个控制系统，给定控制目标为机器人前行10米，即图16中的控制目标为10米(10米处的直线)，控制过程中倾角的变化如图15，实际位移见图16中的过(0，0)点的曲线。

2.伺服驱动控制器的软件

在伺服驱动器ACJ-55-18配套的PC端软件COPLEY MOTION2中，通过输入直流电机参数能够自动计算伺服程序的PI调节参数，简单设置控制系统结构后即可自动生成伺服控制程序，并通过串口下载固化至ACJ-55-18的存储单元中。该伺服程序控制周期1ms，以DSP输出的PWM信号和转向信号分别为电机参考转矩和参考转向，取电枢电流为负反馈，实现电机转矩PID伺服控制。

三、机器人系统的使用

使用本实施例的机器人时，可按如下步骤操作：

1.安装硬件

2.安装电气系统

3.确认硬件和电气系统连接正确。

4.将主电源的船型开关拨至“开”。

5.将机器人扶稳至直立位置。

6.打开电机开关，机器人开始平衡控制。

7.待机器人在直立位置上稳定后，不再扶持。

8.扶稳机器人后依次关闭电机开关、主电源开关。

Claims (6)

1.一种独轮机器人系统，包括机械本体和控制系统；机械本体包括铝合金框架构成的刚性机器人躯干，模块化设计的二维云台构成的机器人头颈部，可拆卸的机械臂(6)，接于机械臂(6)端部的多指机械手(7)，由垂直方向上的下运动平衡组件和上平衡组件构成的总体平衡控制结构，其中，下运动平衡组件为独轮(14)及独轮电机(12)，上平衡组件为惯性飞轮(17)及飞轮电机(18)；控制系统由状态感知传感器、运动控制器(10)和两个伺服驱动控制器以及电源系统构成，其中，状态感知传感器包括姿态传感器和速度传感器，运动控制器(10)接收状态感知传感器的信号，在控制程序下将接收信号进行处理，从而发出控制指令，两个伺服驱动控制器接收控制指令，分别通过独轮电机(12)和飞轮电机(18)控制独轮(14)和惯性飞轮(17)转动，对机器人的姿态平衡作出调整；其特征在于：所述惯性飞轮(17)的转轴是前后指向的，并且，为双对可拆卸结构，由两个飞轮并列组成，每个飞轮由两个半圆结合而成；在所述独轮(14)四周套有一个可上下调节的调试支架(13)，在调试支架(13)下面，前后左右各设置有一个可拆卸的脚轮(15)；所述姿态传感器为倾角仪(19)和陀螺仪(20)；所述速度传感器为安装在独轮电机(12)和飞轮电机(18)中的编码器，并且，该编码器中带有可进行温度补偿的温度传感器；所述运动控制器(10)为MTS2812数字信号处理器板卡；所述伺服驱动控制器为伺服驱动器ACJ-55-18；所述电源系统为电源板卡(5)和充电电池模块(9)。

2.如权利要求1所述的一种独轮机器人系统，其特征在于：所述状态感知传感器还有安装在机器人头部的由双摄像头构成的视觉传感器(1)、MIC构成的听觉传感器和位于双摄像头之间的红外线传感器(2)，以及安装在机器人腰部一周的声纳传感器(11)、遥控接收器；所述控制系统还有由单片机构成的接收声纳传感器(11)、遥控接收器信号的辅助控制器和由嵌入式计算机(EPC)系统构成的接收视觉传感器(1)、红外线传感器(2)、听觉传感器信号的主控制器(3)，该主控制器(3)可以接收运动控制器(10)和辅助控制器的信息，进一步为运动控制器(10)提供控制指令，并能通过音箱(16)发声。

3.如权利要求2所述的一种独轮机器人系统，其特征在于：拆除调试支架(13)的前后脚轮(15)，并使独轮(14)和左右脚轮(15)在同一水平线。

4.如权利要求2所述的一种独轮机器人系统，其特征在于：拆除调试支架(13)的左右脚轮(15)，并使独轮(14)和前后脚轮(15)在同一水平线。

5.如权利要求2或3或4所述的一种独轮机器人系统的控制方法，其控制方法如下：

1)主控制器(3)按照设定的程序或接收来自遥控的用户操作指令来工作，并且监测各传感器反馈信息，通过运动行为决策算法计算机器人的直行和偏航速度的控制命令，下达给运动控制器(10)执行；其决策算法由以下步骤完成：动作发生器参考用户命令或超声波测距信息计算出期望直行和偏航速度控制命令，然后决策器根据机器人姿态平衡的状况判断是否执行期望控制命令，若姿态平衡状况良好，则期望控制命令即为实际控制命令，若姿态平衡状况不佳，则实际控制命令为零，即先调整机器人平衡；其中，动作发生器的设计可选：查表、动态规划或专家系统算法；决策器的设计可选：查表、模糊逻辑或专家系统算法；

2)在一个运动控制周期内，运动控制器(10)读取机器人底座上的倾角仪(19)、陀螺仪(20)的反馈信号，与零位值比较得出倾角和倾角速度的误差信号；其中的反馈信号由以下方法获得：运动控制器(10)多次采集倾角仪(19)和陀螺仪(20)的输出信号，并经过DSP信号处理算法形成一个控制周期计算所用的反馈信号；

3)运动控制器(10)读取电机编码器的反馈信号，计算机器人速度，与主控制器(3)给定的控制命令对比得出误差信号；

4)根据误差信号，运动控制器(10)按预定的运动平衡控制算法计算电机的控制量，发送给伺服驱动控制器执行；其运动平衡控制算法为：利用解耦控制的思想，将获得的俯仰倾角Angle_FB和俯仰倾角速度AngleAcc_FB信号分解到机器人的俯仰自由度方向，将获得的左右倾角Angle_LR和左右倾角速度AngleAcc_LR信号分解到机器人的左右自由度方向，然后利用既定的PID控制算法，或者模糊控制，或者LQR控制算法，或者极点配置算法，或者鲁棒控制算法，计算得到控制独轮电机M_D的控制量U_D和控制惯性飞轮电机M_U的控制量U_U；

5)伺服驱动控制器控制独轮电机(12)、飞轮电机(18)带动调试支架(13)中的独轮(14)、上运动平衡组件中的惯性飞轮(17)，使机器人维持机身平衡以及按指定方式运动；其中，通过电机电枢电流反馈闭环，实现电机的转矩控制，且伺服控制周期远小于机器人运动控制周期。

6.如权利要求1所述的一种独轮机器人系统的控制方法，其特征在于：其控制程序分别运行在运动控制器(10)和伺服驱动控制器中，用于机器人前后左右的平衡，运动控制器(10)和伺服驱动控制器的控制程序分别如下：

(1)运动控制器(10)中的程序：

程序开始时，首先进行必要的初始化，完成程序使用的变量的初始化和配置DSP各寄存器状态，主要配置控制中所需的I/O端口和A/D通道、通用定时器T1-T4，其中T1为中断周期计时器，T2、T4为编码器的计数器，T3为PWM信号输出的比较计时器；然后，开启PWM输出，向伺服驱动控制器发送使能信号；最后，进行无限循环等待中断到来，在每次循环中实施控制程序的检测和控制算法；

每当T1计时满25ms时产生中断，DSP响应该中断，保存当前程序现场，转入定时中断程序，其流程为：

1)关闭定时器T1中断，准备开始；

2)获取编码器信息。每个编码器输出信号为两组正交编码序列，DSP中的正交编码脉冲电路对这两组信号的上升沿和下降沿均进行计数，因此产生的时钟频率是每组编码序列的四倍，所以读取编码器的计数器T2、T4的数值后，除以四，得到一个周期内编码器输出的脉冲数n_u，n_d；

3)计算机器人的累积直线位移、直线速度：由编码器信息可知一周期内每个轮子转过的角度由于控制周期T很短，可近似计算轮子转速

其直线速度V_x＝ω·R/2，其中R为轮子半径，由于控制周期T很短，因此将V_x与T相乘累加到上个控制周期的累积直线位移得到当前累积直线位移，即x(t)＝x(t-T)+V_xT；

4)对倾角仪(19)输出的模拟电压信号进行A/D转换和信号获取：为避免采样过程中偶然因素的影响，每个信号均连续采样10次，进行A/D转换，去掉其中最大值和最小值后求均值赋给对应的变量：

通过公式：θ＝(U_θ-U₀)/S₂，其中θ是计算得到的某个自由度上的倾角，U_θ是计算得到的电压均值，U₀是零位电压，S₂是倾角仪(19)的灵敏度，θ₁是俯仰方向倾角、

是俯仰方向倾角速度、θ₂是左右方向倾角、

是左右方向倾角速度，计算出对应机器人的倾斜角度；

5)通过SPI端口直接获得前后自由度倾角和左右自由度倾角的倾角速度信号

6)以控制命令X^C为参考输入，X^C为机器人接收到的控制命令或信息，以检测到的x、V_x、θ₁、

θ₂、

为反馈量，通过运动平衡控制算法计算出两个电机控制转矩对应PWM的占空比，其中，转矩绝对值范围[0，6]N·m，对应占空比为0～100％，大于等于零时为正转，小于零时为反转；运动平衡控制其算法为：

a)根据机器人的机械系统的特点和参数，建立其数学模型，在直立平衡点线性化获得线性状态空间方程：

其中，

u＝[τ_d τ_u]^T，θ₁为俯仰方向倾角、

为俯仰方向倾角速度、θ₂为左右方向倾角、

为左右方向倾角速度，ω为轮子旋转的角度，

为轮子旋转的角速度，η为惯性飞轮(17)旋转的角度，为惯性飞轮(17)角速度，

$A=\left[\begin{array}{cccccccc}0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 1\\ 24.9770& 0& 0& 0& 0& 0& 0& 0\\ 0& 60.9590& 0& 0& 0& 0& 0& 0\\ 0& -108.3715& 0& 0& 0& 0& 0& 0\\ -24.9770& 0& 0& 0& 0& 0& 0& 0\end{array}\right],$ $B=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ 0& 0\\ 0& -1.4159\\ -14.3988& 0\\ 40.4128& 0\\ 0& 134.7493\end{array}\right]$

b)通过求得俯仰运动子系统与侧倾运动子系统的能控矩阵均满秩，证明两者完全能控，系统各状态值又均可测量获得，

c)利用解耦控制的思想，将整个机器人系统的倾角控制任务分为俯仰自由度的倾角控制和左右自由度的倾角控制器，因此以分别为两个子系统参考输入，状态θ₁、

(i＝u，d；)，即两个自由度的倾角，为反馈量，构造运动平衡控制系统，并采用PID控制方法设计内环的两个倾角控制子系统的状态反馈控制器：PID控制参数采用临界震荡法获得，具体为：

内环俯仰自由度控制： $\left\{\begin{array}{c}{K}_p^u=6000\\ K_i^u=3\\ K_d^u=5200\end{array}\right.,$ 侧倾自由度控制：

$\left\{\begin{array}{c}{K}_p^d=20000\\ K_i^d=3\\ K_d^d=20000\end{array}\right.$

用同样的方法获得机器人的位移控制外闭环的PID控制参数：

外环X控制： $\left\{\begin{array}{c}{K}_p^x=0.02\\ K_i^x=0\\ K_d^x=0.148\end{array}\right.;$

7)刷新输出至伺服驱动的PWM占空比和电机转动方向；

8)设置计时器T1，重新开始25ms计时，设置T2、T4，重新开始计数；

9)开T1中断，退出中断控制程序；

(2)伺服驱动控制器的控制程序：

在伺服驱动器ACJ-55-18配套PC端的软件COPLEY MOTION2中，通过输入直流电机参数能够自动计算伺服程序的PI调节参数，简单设置控制系统结构后即可自动生成伺服控制程序，并通过串口下载固化至ACJ-55-18的存储单元中，该伺服程序控制周期1ms，以DSP输出的PWM信号和转向信号分别为电机参考转矩和参考转向，取电枢电流为负反馈，实现电机转矩PID伺服控制。

Images