第六章STM32平臺機器人實戰6.1

STM32基礎簡介6.2

STM32開發基礎知識6.3單個關節的倒立擺控制器實戰6.4多個關節的機器人實戰

6.1

STM32基礎簡介6.1.1

STM32概述意法半導體(ST)集團于1988年6月成立，是由意大利的SGS微電子公司和法國Thomson半導體公司合并而成的。1998年5月，SGS-THOMSONMicroelectronics將公司名稱改為意法半導體有限公司，是世界最大的半導體公司之一。STM32產品廣泛應用于工業控制、消費電子、物聯網、通訊設備、醫療服務、安防監控等應用領域，其優異的性能進一步推動了生活和產業智能化的發展。

STM32是ARM?Cortex?內核單片機和微處理器市場及技術方面的領先者，目前STM32提供17大產品線(F0、G0、F1、F2、F3、G4、F4、F7、H7、MP1、L0、L1、L4、L4+、L5、WB、WL)，超過1000個型號。STM32F1屬于Cortex-M系列中的Cortex-M3內核，采用ARMv7-M架構。

STM32的產品定位及命名規則分別如圖6.1和圖6.2所示。圖6.1

STM32的產品定位圖6.2

STM32產品型號的命名規則

6.1.2

STM32資源與應用

1)?CPU芯片資源

關于CPU芯片內部資源，我們以STM32F103系列器件為例介紹。STM32F103Rx、STM32F103Vx和STM32F103Zx器件功能和配置如表6.1所示。

2)?STM32開發板資源

ALIENTEK戰艦STM32開發板(廣州市星翼電子科技有限公司(正點原子))的資源圖如圖6.3所示。圖6.3戰艦STM32開發板資源圖

ALIENTEK戰艦STM32開發板板載資源有：

?CPU：STM32F103ZET6，LQFP144；FLASH：512?KB；SRAM：64?KB。

外擴SRAM：IS62WV51216，1?MB。

外擴SPIFLASH：W25Q64，8?MB。

?1個電源指示燈(藍色)。

?2個狀態指示燈(DS0：紅色，DS1：綠色)。

1個紅外接收頭，并配備一款小巧的紅外遙控器；1個EEPROM芯片(24C02)，其容量為256B。

?1個重力加速度傳感器芯片(ADXL345)。

?1個高性能音頻編解碼芯片(VS1053)。

?1個FM立體聲收發芯片(RDA5820)。

1個2.4?GB無線模塊接口(NRF24L01)。

?1路CAN接口，采用TJA1050芯片。

?1路485接口，采用SP3485芯片。

?1路RS232接口，采用SP3232芯片。

1個PS/2接口，可外接鼠標、鍵盤。

?1個游戲手柄接口，可以直接插FC(紅白機)游戲手柄。

1路數字溫濕度傳感器接口，支持DS18B20/DHT11等。

?1個標準的2.4/2.8/3.5英寸(1英寸=2.54厘米)LCD接口，支持觸摸屏。

?1個攝像頭模塊接口。

2個OLED模塊接口。

1個USB串口，可用于程序下載和代碼調試(USMART調試)。

1個USBSLAVE接口，用于USB通信。

1個有源蜂鳴器。

1個FM收發天線接口，并配天線。

?1個RS232/RS485選擇接口。

1個CAN/USB選擇接口。

1個串口選擇接口。

1個SD卡接口(在板子背面，支持SPI/SDIO)。

?1個SD卡/網絡模塊選擇接口。

?1個標準的JTAG/SWD調試下載口。

?1個VS1053的IIS輸出接口。

?1個MIC/LINEIN選擇接口。

?1個錄音頭(MIC/咪頭)。

?1路立體聲音頻輸出接口。

?1路立體聲錄音輸入接口。

?1組多功能端口(DAC/ADC/PWMDAC/AUDIOIN/TPAD)。

?1組5V電源供應/接入口。

?1組3.3V電源供應/接入口。

?1個參考電壓設置接口。

?1個直流電源輸入接口(輸入電壓范圍為6～16?V)。

?1個啟動模式選擇配置接口。

?1個RTC后備電池座，并帶電池。

?1個復位按鈕，可用于復位MCU和LCD。

?4個功能按鈕，其中WK_UP兼具喚醒功能。

?1個電容觸摸按鍵。

?1個電源開關，控制整個板的電源。

獨創的一鍵下載功能以及除晶振占用的IO口外，其余所有IO口全部引出。

6.2

STM32開發基礎知識

6.2.1

C語言基礎

1.?位操作對于C語言位操作，相信學過C語言的人都不陌生了，簡而言之，位操作就是對基本類型變量可以在位級別進行操作。這節的內容很多朋友都應該很熟練了，這里我們點到為止，不深入探討。下面我們先講解幾種位操作符，然后講解位操作使用技巧。C語言支持如表6.2中所示的6種位操作。

1)?設置某幾個位的值

不改變其他位的值的狀況下，對某幾個位進行設置。這個場景在單片機開發中經常使用，其方法就是先對需要設置的位用&操作符進行清零操作，然后用?|?操作符進行設置。比如我們要改變GPIOA的狀態，可以先對寄存器的值進行&清零操作：

GPIOA->CRL&=0XFFFFFF0F;//將第4～7位清0

然后再與需要設置的值進行|或運算：

GPIOA->CRL?|?=0X00000040; //設置相應位的值，不改變其他位的值

2)?移位操作提高代碼的可讀性

移位操作在單片機開發中也非常重要，下面我們看看固件庫的GPIO初始化的函數里的一行代碼：

GPIOx->BSRR=(((uint32_t)0x01)<<pinpos);

這個操作就是將BSRR寄存器的第pinpos位設置為1，為什么要通過左移設置而不是直接設置一個固定的值呢？其實，這是為了提高代碼的可讀性及可重用性。這行代碼可以讓你很直觀明了地知道，是將第pinpos位設置為1。如果你寫成

GPIOx->BSRR=0x0030;

這樣的代碼可讀性差且不便于重用。類似這樣的代碼很多：

GPIOA->ODR|=1<<5; //PA.5輸出高電平，不改變其他位

這樣我們一目了然，5告訴我們是第5位也就是第6個端口，1告訴我們是將第6個端口設置為1了。

3)?～取反操作使用技巧

SR寄存器的每一位都代表一個狀態，如某個時刻我們希望去設置某一位的值為0，同時其他位都保留為1，簡單的做法是直接給寄存器設置一個值：

TIMx->SR=0xFFF7；

這樣的做法設置第3位為0，但是這樣的做法同樣不好理解，并且可讀性很差。看看下面庫函數代碼中是怎樣使用的：

TIMx->SR=(uint16_t)～TIM_FLAG;

2.?define宏定義

define是C語言中的預處理命令，它用于宏定義，可以提高源代碼的可讀性，為編程提供方便。define常見的格式：

#define標識符

字符串

“標識符”為所定義的宏名。“字符串”可以是常數、表達式、格式串等。例如：

#defineSYSCLK_FREQ_72MHz72000000

定義標識符SYSCLK_FREQ_72MHz的值為72000000。

3.?ifdef條件編譯

單片機程序開發過程中，經常會遇到一種情況，當滿足某條件時對一組語句進行編譯，而當該條件不滿足時則編譯另一組語句。條件編譯命令最常見的形式為：

#ifdef標識符

程序段1

#else

程序段2

#endif

上述命令的作用是：當標識符已經被定義過(一般是用#define命令定義)，則對程序段1進行編譯，否則編譯程序段2。

其中#else部分也可以沒有，即：

#ifdef

程序段1

#endif

這種條件編譯語句在MDK開發環境中比較常用，在stm32f10x.h這個頭文件中經常會看到這樣的語句：

#ifdefSTM32F10X_HD

程序段/*大容量芯片需要的一些變量定義*/

#end

而STM32F10X_HD則是我們通過#define來定義的。

4.?extern變量申明

C語言中extern可以置于變量或者函數前，以表示變量或者函數的定義在別的文件中，提示編譯器遇到此變量和函數時在其他模塊中尋找其定義。這里需要注意，對于extern，變量申明可以多次，但定義只有一次。在我們的代碼中你會看到這樣的語句：

externu16USART_RX_STA;

這個語句是申明USART_RX_STA變量在其他文件中已經定義了，在這里要使用到。所以，你肯定可以找到在某個地方有變量定義的語句，即：

u16USART_RX_STA;

下面通過一個例子說明一下變量申明的使用方法。

在Main.c中定義全局變量id，id的初始化都是在Main.c里進行的，如

Main.c文件

u8id;//全局變量id的初始化只允許在Main.C里面定義一次

main()

{

id=1;

printf("d%",id);//id=1

test();

printf("d%",id);//id=2

}

但是我們希望在test.c的changeId(void)函數中使用變量id，這個時候我們就需要在test.c里申明變量id是外部定義的了，因為如果不申明，變量id的作用域是到不了test.c文件中的。看下面test.c中的代碼：

externu8id;//申明變量id是在外部定義的，申明可以在很多個文件中進行

voidtest(void)

{

id=2;

}

在test.c中申明變量id在外部定義，然后在test.c中就可以使用變量id了。對于extern申明函數在外部定義的應用，這里不多講解。

5.?typedef類型別名

typedef用于為現有類型創建一個新的名字，或稱為類型別名，用來簡化變量的定義。typedef在MDK開發環境中用得最多的就是定義結構體的類型別名和枚舉類型了。例如下列語句：

struct_GPIO

{

__IOuint32_tCRL;

__IOuint32_tCRH;

…

};

定義了一個結構體GPIO，這樣我們定義變量的方式為：

struct_GPIOGPIOA;//定義結構體變量GPIOA

但是這樣很繁瑣，MDK中有很多這樣的結構體變量需要定義。這里我們可以為結構體定義一個別名GPIO_TypeDef，這樣我們就可以在其他地方通過別名GPIO_TypeDef來定義結構體變量了。方法如下：

typedefstruct

{

__IOuint32_tCRL;

__IOuint32_tCRH;

…

}GPIO_TypeDef;

Typedef為結構體定義一個別名GPIO_TypeDef，這樣我們就可以通過GPIO_TypeDef來定義結構體變量：

GPIO_TypeDef_GPIOA,_GPIOB;

這里的GPIO_TypeDef與struct_GPIO的作用相同。

6.結構體

MDK開發環境中太多地方會使用結構體以及結構體指針，這容易導致初學者學習STM32的積極性降低。其實，結構體并不是那么復雜，合理利用結構體能使我們便利高效地處理復合類型數據。

聲明結構體類型：

Struct結構體名{

成員列表;

}變量名列表；

在結構體申明的時候可以定義變量，也可以申明之后再定義變量，方法是：

Struct結構體名字結構體變量列表;

例如：structU_TYPEusart1,usart2;

結構體成員變量的引用方法是：

結構體變量名字.成員名

比如要引用usart1的成員BaudRate，方法是：usart1.BaudRate;結構體指針變量定義也是一樣的，跟其他變量沒有什么區別。

例如：structU_TYPE*usart3；//定義結構體指針變量usart1;

結構體指針成員變量引用方法是通過“->”符號來實現，比如要訪問usart3結構體指針指向的結構體的成員變量BaudRate，方法是：

Usart3->BaudRate;

在單片機程序開發過程中，經常會遇到要初始化一個外設比如串口，它的初始化狀態是由幾個屬性來決定的，比如串口號、波特率、極性以及模式等。對于這種情況，在沒有

學習結構體的時候，我們采用的一般方法是：

voidUSART_Init(u8usartx,u32u32BaudRate,u8parity,u8mode);

這種方式有效的同時在一定場合是可取的。但是若希望往這個函數里再傳入一個參數，那么勢必我們需要修改這個函數的定義，重新加入字長這個入口參數。于是我們的定義被修改為：

voidUSART_Init(u8usartx,u32BaudRate,u8parity,u8mode,u8wordlength);

于是，我們在初始化串口的時候入口參數就可以是USART_InitTypeDef類型的變量或者指針變量了，MDK中是這樣做的：

voidUSART_Init(USART_TypeDef*USARTx,USART_InitTypeDef*USART_InitStruct);

6.2.2

STM32系統架構

STM32的系統架構比51單片機強大很多。為了讓讀者在學習STM32之前對系統架構有一個初步的了解，本部分內容對STM32系統架構僅作基本介紹，下文所述的STM32系統架構主要針對的是STM32F103等非互聯型芯片。STM32的系統架構如圖6.4所示。圖6.4

STM32系統架構

STM32主系統主要由四個驅動單元和四個被動單元構成。

(1)?四個驅動單元是：

內核DCode總線;

系統總線;

通用DMA1;

通用DMA2。

(2)?四個被動單元是：

?AHB到APB的橋：連接所有的APB設備；

內部FlASH閃存；

內部SRAM；

?FSMC。

圖6.4中幾個總線的相關知識介紹如下：

①ICode總線：該總線將M3內核指令總線和閃存指令接口相連，指令的預取在該總線上面完成。

②DCode總線：該總線將M3內核的DCode總線與閃存存儲器的數據接口相連接，常量加載和調試訪問在該總線上面完成。

③

系統總線：該總線連接M3內核的系統總線到總線矩陣，總線矩陣協調內核和DMA間相互訪問。

④DMA總線：該總線將DMA的AHB主控接口與總線矩陣相連，總線矩陣協調CPU的DCode和DMA到SRAM、閃存和外設的相互訪問。

⑤

總線矩陣：總線矩陣協調內核系統總線和DMA主控總線之間的訪問仲裁，仲裁利用輪換算法。

⑥AHB/APB橋：這兩個橋在AHB和2個APB總線間提供同步連接，APB1操作速度限于36?MHz，APB2操作速度為全速。

6.2.3系統時鐘

眾所周知，時鐘系統是CPU的脈搏，就像人的心跳一樣。所以時鐘系統的重要性就不言而喻了。STM32的時鐘系統比較復雜，不像簡單的51單片機一個系統時鐘就可以解決一切時鐘問題。于是有人要問，采用一個系統時鐘不是很簡單嗎？為什么STM32要有多個時鐘源呢？

因為首先本身STM32非常復雜，外設非常多，但是并不是所有外設都需要系統時鐘這么高的頻率，比如看門狗以及RTC只需要幾十千赫茲的時鐘即可。同一個電路，時鐘越快功耗越大，同時抗電磁干擾能力也會越弱，所以對于較為復雜的MCU，一般都是采取多時鐘源的方法來解決這些問題。

下面我們來看看STM32的時鐘系統圖，如圖6.5所示。

在STM32中，有五個時鐘源，名稱分別為HSI、HSE、LSI、LSE、PLL。根據時鐘源頻率可以將其分為高速時鐘源和低速時鐘源，在這5個時鐘源中，HIS、HSE和PLL是高速時鐘源，LSI和LSE是低速時鐘源。根據時鐘源的來源可將其分為外部時鐘源和內部時鐘源，外部時鐘源就是以從外部通過接晶振的方式獲取時鐘信號的時鐘源，其中HSE和LSE是外部時鐘源，其他的是內部時鐘源。圖6.5時鐘系統圖

具體而言，按圖中圓圈標示的順序，分別為：

①HSI是高速內部時鐘，為RC振蕩器，頻率為8?MHz。

②HSE是高速外部時鐘，可接石英/陶瓷諧振器，或者接外部時鐘源，頻率范圍為4～16?MHz。本書介紹的開發板接的是8?MHz的晶振。

③LSI是低速內部時鐘，為RC振蕩器，頻率為40?kHz。獨立看門狗的時鐘源只能是LSI，同時LSI還可以作為RTC的時鐘源。

④LSE是低速外部時鐘，接頻率為32.768?kHz的晶振，其主要用于RTC時鐘源。

⑤PLL為鎖相環倍頻輸出，其時鐘輸入源可選擇HSI/2、HSE或者HSE/2，倍頻可為其的2～16倍，但是其輸出頻率最大不得超過72?MHz。

E處是指其他所有外設了。從時鐘圖上可以看出，其他所有外設的時鐘最終來源都是SYSCLK。SYSCLK通過AHB分頻器分頻后送給各模塊使用。這些模塊包括：

①AHB總線、內核、內存和DMA使用的HCLK時鐘。

②

通過8分頻后送給Cortex的系統定時器時鐘，也就是Systick。

③

直接送給Cortex的空閑運行時鐘FCLK。

④

送給APB1分頻器。APB1分頻器輸出一路供APB1外設使用(PCLK1，最大頻率為36?MHz)，另一路送給定時器(Timer)2、3、4倍頻器使用。

⑤

送給APB2分頻器。APB2分頻器分頻輸出一路供APB2外設使用(PCLK2，最大頻率為72?MHz)，另一路送給定時器(Timer)1倍頻器使用。

6.2.4

STM32NVIC中斷優先級管理

CM3內核支持256個中斷，其中包含了16個內核中斷和240個外部中斷，并且具有256級的可編程中斷設置。但STM32并沒有使用CM3內核的全部東西，而是只用了它的一部分。STM32有84個中斷，包括16個內核中斷和68個可屏蔽中斷，具有16級可編程的中斷優先級。常用的就是這68個可屏蔽中斷，但是STM32的68個可屏蔽中斷，真正在STM32F103系列上只有60個，在107系列上才有68個。因為我們開發板選擇的芯片是STM32F103系列的，所以我們就只針對STM32F103系列這60個可屏蔽中斷進行介紹。

具體的分配關系如表6.3所示。

結合實例說明一下：假定設置中斷優先級組為2，然后設置中斷3(RTC中斷)的搶占優先級為2，響應優先級為1；中斷6(外部中斷0)的搶占優先級為3，響應優先級為0；中斷7(外部中斷1)的搶占優先級為2，響應優先級為0。那么這3個中斷的優先級順序為：中斷7>中斷3>中斷6。上面例子中的中斷3和中斷7都可以打斷中斷6的中斷。而中斷7和中斷3卻不可以相互打斷。

通過以上介紹，我們熟悉了STM32中斷設置的大致過程。接下來我們介紹如何使用“庫函數”進行以上中斷分組設置以及中斷優先級管理，使得以后的中斷設置可以簡單化。NVIC中斷

管理函數主要在misc.c文件里。

首先要講解的是中斷優先級分組函數

NVIC_PriorityGroupConfig，其函數申明如下：

voidNVIC_PriorityGroupConfig(uint32_tNVIC_PriorityGroup);

這個函數的作用是對中斷的優先級進行分組，該函數在系統中只能被調用一次，一旦分組確定就最好不要更改。該函數的實現為：

voidNVIC_PriorityGroupConfig(uint32_tNVIC_PriorityGroup)

{

assert_param(IS_NVIC_PRIORITY_GROUP(NVIC_PriorityGroup));

SCB->AIRCR=AIRCR_VECTKEY_MASK|NVIC_PriorityGroup;

}

從該函數體可以看出，這個函數的唯一目的就是通過設置SCB->AIRCR寄存器來設置中斷優先級分組，這在前講解面寄存器的過程中已經講到。而其入口參數通過雙擊選中函數體里面的“IS_NVIC_PRIORITY_GROUP”然后右鍵單擊“Gotodefitionof…”可以查看到，如下所示：

#defineIS_NVIC_PRIORITY_GROUP(GROUP)

(((GROUP)==NVIC_PriorityGroup_0)||

((GROUP)==NVIC_PriorityGroup_1)||\

((GROUP)==NVIC_PriorityGroup_2)||\

((GROUP)==NVIC_PriorityGroup_3)||\

((GROUP)==NVIC_PriorityGroup_4))

這也是我們之前講解的分組范圍為0～4。比如我們設置整個系統的中斷優先級分組值為2，那么方法是：

NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);

這樣就確定了一共為“2位搶占優先級，2位響應優先級”。設置好系統中斷分組后，對于每個中斷我們又怎么確定它的搶占優先級和響應優先級呢？下面我們講解一個重要的函數——中斷初始化函數NVIC_Init，其函數申明為：

voidNVIC_Init(NVIC_InitTypeDef*NVIC_InitStruct)

比如我們要使能串口1的中斷，同時設置搶占優先級為1，子優先級為2，其初始化方法如下：

這里我們講解了中斷的分組的概念以及設定優先級值的方法，至于每種優先級還有一些關于清除中斷、查看中斷狀態等方法，后面講解每個中斷的時候會詳細講解。最后我們總結一下中斷優先級設置的步驟：

(1)系統運行開始的時候設置中斷分組。確定組號，也就是確定搶占優先級和子優先級的分配位數。調用函數為NVIC_PriorityGroupConfig();

(2)設置所用到的中斷的中斷優先級別。對每個中斷調用函數為NVIC_Init()。

6.3單個關節的倒立擺控制器實戰

6.3.1倒立擺系統倒立擺系統被認為是研究關節機器人控制理論中的典型實驗設備，也是抽象關節機器人的典型物理模型，它本身是一個自然不穩定體，在控制過程中能夠有效地反映控制中的許多關鍵問題。一級倒立擺硬件框圖如圖6.6所示，包括倒立擺本體、電控箱及由STM32控制器和計算機組成的控制功能模塊等三大部分。圖6.6固高倒立擺系統硬件框圖

為了實現倒立擺擺桿的穩定控制，在STM32平臺上位機開發環境中進行倒立擺控制程序的開發，如圖6.7所示為倒立擺系統控制程序流程圖。如圖6.7(a)所示，系統上電后，首先對STM32平臺微控制器的串口、中斷、定時器等進行初始化，同時對STM32平臺控制的倒立擺系統的編碼器、電機及顯示器進行初始化，然后進入定時器中斷，如圖6.7(b)所示為定時器中斷的程序流程框圖，可在中斷中實現相應倒立擺的各控制參數的檢測控制功能。圖6.7倒立擺擺桿的穩定控制流程圖

由圖6.7(b)所示定時器中斷的程序流程圖設計出的具體操作命令的執行過程是：首先，STM32平臺控制器讀取AD轉換模塊采集到的擺桿傾角信息，并通過兩路編碼器讀取滑塊的相對位置信息；然后，STM32平臺控制器根據所獲得的實時信息，計算保持擺桿平衡所需的滑塊位移，并將其轉換為相應的電機的控制信號，從而控制滑塊向相應方向進行移動，達到保持擺桿平衡的目的。采用上述方法實現一級倒立擺平穩地控制在擺體初始位置附近，如圖6.8所示為倒立擺實物控制圖。圖6.8倒立擺實物控制圖

6.3.2四種智能機器人控制器的倒立擺實戰

1.?模糊控制器的倒立擺實戰

基于MATLAB/Simulink軟件環境中的模糊控制器編輯調試的一般操作步驟如下：

(1)?在Simulink環境的命令行窗口輸入“fuzzy”，按回車鍵，或輸入fuzzysltank，這樣調出對應軟件中的工程范例，進入如圖6.9所示的模糊系統仿真控制工具箱進行編輯設計，從而分析系統的輸出及控制性能參數。

(2)?如圖6.9中數字“1”表示菜單，允許你使用5個基本GUI工具中的任何一個保存、打開或編輯模糊系統；數字“2”表示系統名顯示在這里，可以使用saveas……菜單項改變它；數字“3”表示這些下拉式菜單用于選擇模糊推理函數，例如選擇反模糊化(Defuzzification)，一般采用面積中心(重心)法(centroid)；數字“4”表示此狀態行描述了最近的當前操作；數字“5”表示雙擊輸入變量圖標打開隸屬度函數編輯器；數字“6”表示雙擊系統方框圖標打開規則編輯器；數字“7”表示雙擊輸出變量圖標打開隸屬度函數編輯器；數字“8”表示此編輯框域，用于命名并編輯輸入和輸出變量的名字。圖6.9模糊控制器的調用編輯

(3)?在Matlab主窗口命令行中輸入命令fuzzy，打開編輯好的fis文件，順序單擊菜單file-export-toworkspace，將模糊控制器程序文件存入工作空間中，進行下一步連接操作。這里強調一下，只有嵌入成功才能實現模糊控制，檢查方法就是如圖6.10所示，通過選擇“FuzzyLogicController”模塊，進入“模型編輯器”；再用鼠標右鍵單擊它，會彈出一個菜單；單擊彈出菜單中的“LookUnderMask”，就顯現出它的內部結構，如上寫著“sffis”，表明未嵌入FIS結構文件；如這時“sffis”變成“fis”，表明已嵌入FIS結構文件。

(4)?將模糊邏輯添加到Simulink中，打開Simulink，新建model(.mdl文件)，如圖6.10所示為搭建Simulink程序；這時在命令窗口中敲入命令代碼：fuzzy=readfis('fuzzycontrol.fis'?)，進入圖6.10所示的一階T-S型模糊控制器程序編輯界面。設置模糊控制器的輸入激勵信號，采用連接多路復用的mux模塊，如圖6.10中的復用模塊里面集成了多路輸入激勵信號連接零階保持器(Zero-OrderHold模塊)。

(5)?在做仿真的時候，仿真結束后，彈出一個對話框，若顯示的是“SolverStepsizeisbecominglessthanspecifedminimumstepsize.”，則說明是步長的問題，不要選變步長選項，應采用定步長方式。你可以在SimulationParameters設置參數窗口中將BooleanLogicSignalsOption設置為off，讓Matlab在仿真運行中自動轉換變量類型就可以了。

如圖6.10所示為模糊控制器程序編輯運行界面，來自Matlab自帶的一階T-S型模糊控制器slcp.mdl。其中圖6.10(a)為模糊控制器程序編輯運行界面，圖6.10(b)為一階T-S型模糊控制器仿真運行效果圖。圖6.10一階T-S型模糊控制器的調用編輯圖6.10一階T-S型模糊控制器的調用編輯

2.?BP神經網絡控制器的倒立擺實戰

1)?BP神經網絡概述

BP(BackPropagation)神經網絡是一種利用誤差反向傳播訓練算法的前饋型網絡，BP學習算法實質是求取網絡總誤差函數的最小值問題。這種算法采用非線性規劃中的最速下降方法，按誤差函數的負梯度方向修改權系數，這是梯度下降法在多層前饋網絡中的應用。具體學習算法包括兩大過程，其一是輸入信號的正向傳播過程，其二是輸出誤差信號的反向傳播過程。

BP神經網絡具有以下三方面的主要優點：

第一，只要有足夠多的隱含層，BP神經網絡即可逼近任意的非線性映射關系。

第二，BP學習算法是一種全局逼近方法，因而它具有較好的泛化能力。

第三，BP神經網絡具有一定的容錯能力，因為BP神經網絡輸入輸出間的關聯信息分布存儲于連接權中，由于連接權的個數眾多，個別神經元的損壞對輸入輸出關系只有較小影響。

2)基于BP神經網絡的控制器設計

神經網絡控制器可通過采集和分析實物系統其他控制器的數據訓練得到，也可利用仿真實驗中的控制器得到，但要注意模型參數是否匹配。這里神經網絡訓練數據來源于Matlab自帶的一階T-S型模糊控制slcp.mdl模塊，如圖6.11所示。在上面的控制系統中提取擺角、角速度、位移、速度的初始條件分別為0.5rad、1rad/s、0和0，在此條件下采集的輸入輸出對作為樣本。

使用Matlab實現神經網絡的步驟是首先根據應用創建一個神經網絡；其次進行訓練樣本的準備與處理，然后設定神經網絡的訓練參數，利用給定樣本對創建的神經網絡進行訓練；最后輸入測試數據，測試訓練好的神經網絡的性能。如圖6.11所示是BP神經網絡訓練數據的提取過程的示意圖，利用“SignalToWorkspace”模塊獲取一階T-S型模糊控制仿真過程的控制器輸入輸出數據對，并將其保存到工作區中，可以直接將其用到神經網絡的訓練中。圖6.11神經網絡訓練數據提取過程示意圖

3)?BP神經網絡控制的實現

用語句gensim(net,-1)可以在simulink里生成控制器并使用其進行控制，其中-1的意思是系統是實時的，生成神經網絡控制器結構。如圖6.12所示為BP神經網絡控制器輸入輸出結構示意。圖6.12BP神經網絡控制器輸入輸出結構

如圖6.13所示為直線一級機械臂神經網絡控制器，使用訓練后的BP神經網絡控制器代替原模糊控制器便可進行仿真試驗。單擊模型窗口上的“run”，運行以上的仿真實驗，通過“scope”模塊觀察機械臂的運動狀態曲線。隱含層函數嵌在神經網絡控制器里。圖6.13直線一級機械臂神經網絡控制器

3.?LQR控制器的倒立擺實戰

LQR(LinearQuadraticRegulator)即線性二次型調節器，其研究對象是現代控制理論中狀態空間形式的線性系統，而且目標函數為對象狀態和控制輸入的二次型函數。在Matlab/Simulink軟件環境中“LQRController”是一個封裝好的模塊，在Simulink中建立直線一級機械臂的模型。如圖6.14所示為一級機械臂LQR控制器。圖6.14一級機械臂LQR控制器

LQR理論是現代控制理論中發展最早也是最為成熟的一種狀態空間設計理論。采用LQR最優設計可以得到狀態線性反饋的最優控制量。對于一級機械臂的線性定常系統而言，任何平衡狀態通過線性變換可轉化為零狀態。為了方便起見，通常將系統的零狀態取為平衡狀態。如式(6.1)所示為以狀態方程組給出的一級機械臂線性反饋系統迭代控制策略結構。

(6.1)

式(6.1)中，A為線性時變系統矩陣，B為增益矩陣，C為反映系統狀態對輸出影響的系數矩陣，D為反映系統輸入對輸出直接作用的系數矩陣。

通過最佳控制量的矩陣K，使性能指標J的值極小。LQR最優設計是設計出的狀態反饋控制器K要使二次型目標函數J取最小值，而K由權重矩陣Q和R唯一決定。性能指標J的給定形式如式(6.2)所示。

(6.2)

式(6.2)中，Q為半正定的加權矩陣，是用來平衡狀態變量的權重；R為對稱正定矩陣，是用來平衡輸入變量的權重。在這里平衡狀態方程的形式為，根據該方程可以獲得P的值，然后用公式找到最優反饋矩陣K，調節Q和R可以找到不同的K。不一樣的反饋矩陣的控制效果也不一樣。通過不斷地調節找出最合適的K值。令Q=1求得K=[-1-1.785525.4224.6849]。

“LQRcontroller”是一個封裝好的模塊，在Matlab軟件Simulink環境的嵌入式系統模塊Invertedpendulumtoolbox內部，含有一個SinglependulumLQRcontroldemo的模型，單擊可以打開這個單級機械臂LQR控制模型，雙擊LQR控制器可以調節參數。同時配合打開硬件控制箱，慢慢地將機械臂擺桿推到平衡位置附近(動作一定要慢)。當擺桿很接近平衡位置的機器人小車時就會開始突然移動并且保持擺桿的平衡。雙擊startrealpendulumcontrol進入如圖6.15所示調節狀態，可以調節反饋矩陣K，也可以通過利用示波器(scope)觀察機械臂擺桿的角速度、擺桿角度以及擺桿速度的變化。圖6.15

LQR控制器內部參數調節結構

說明：軟件中實時調節求解K值的操作方法是：選擇“Lookundermask”，在其上點擊鼠標右鍵，打開如圖6.15所示為LQR控制器內部參數調節結構。點擊MatrixGainK可以調節K的值，先輸入理論計算最優值K=[-1-1.7855?25.422?4.6849]；后增加Q值使得控制倒立擺的穩定時間和上升時間變短，以及擺桿的角度變化減小。經過多次嘗試，調節參數K=[-32.7928?-23.8255?81.6182?14.7098]，將這個新參數K值輸入運行系統。通過不斷地調整反饋矩陣K，即可得到最理想的反饋矩陣K=[-16?-6?50?5]。

4.?RBF神經網絡控制器實戰

這里提出機器人關節控制策略，采用RBF神經網絡逼近不確定項的自適應控制策略，以動力學為基礎，在計算了力矩的基礎上，設計符合相應條件的魯棒控制器，將所建立的二關節機械手模型用RBF(徑向基神經網絡)對其中的不確定項f進行不斷逼近，使跟蹤誤差(理想位置與實際位置的差值)逐步縮小。

1)?RBF網絡特點及結構

RBF網絡的特點如下：

(1)?RBF網絡的作用函數為高斯函數，是局部的，BP網絡的作用函數為S函數，是全局的；

(2)?如何確定RBF網絡的隱層節點中心及基寬度參數是一個困難的問題；

(3)?已證明RBF網絡具有唯一最佳逼近的特性，且無局部極小。

RBF網絡是一種三層前向網絡，由于輸入到輸出映射是非線性的，而隱含層空間到輸出空間的映射是線性的，從而可以大大加快學習速度并避免局部極小問題。

采用RBF網絡可以逼近一對象的結構。如圖6.16所示為RBF網絡逼近對象結構示意圖。圖6.16

RBF網絡逼近結構

2)?RBF神經網絡函數newrbe及其參數介紹

應用newrbe()函數可以快速設計一個徑向基函數網絡，且使得設計誤差為0，調用方式如式(6.3)所示。

式(6.3)中P為輸入向量，T為期望輸出向量(目標值)，SPREAD為徑向基層的散布常數，缺省值為1。輸出為一個徑向基網絡，其權值和閾值完全滿足輸入和期望值關系要求。

3)?建立RBF神經網絡控制模型

利用Matlab自帶的一階T-S型模糊控制slcp.mdl模塊加載設計RBF神經網絡控制器，將如圖6.11所示的擺桿控制系統中提取擺角、角速度、位移、速度初始條件參數分別設置為0.5rad、1rad/s、0和0，利用SignalToWorkspace模塊獲取一階T-S型模糊控制在此條件下擺桿響應的輸入輸出數據對，并保存到工作區中，它們可以直接用到建立RBF神經網絡控制模型訓練中，工作原理如前面圖6.13所示的BP神經網絡控制器的數據提取過程，詳細操作如下面步驟所示。

首先將提取出的訓練數據變為標準的訓練數據形式。標準的訓練數據分為輸入和目標輸出兩部分。輸入部分是一個形式為輸入個數×訓練數據個數的矩陣，這里輸入個數為4。目標輸出為一個輸出個數×訓練數據個數的矩陣，這里輸出個數為1。而經signaltoworkspace模塊提取出的數據為一個訓練數據個數×輸入(或輸出)個數的矩陣，因此分別將u、v轉置后就得到標準訓練數據集u'、v'。使用下面語句應用newrbe()函數設計一個RBF神經網絡，如式(6.4)所示。

對網絡進行檢測，對于輸入向量input應用函數sim()進行仿真，觀察RBF對樣本向量的逼近效果，如式(6.5)所示。

y=sim(net，input)(6.5)

調用函數gensim()生成上述網絡的Simulink模型。設定st=-1，生成一個連續采樣的網絡模塊。用下面語句可以在Simulink里生成控制器并使用其進行控制，如式(6.6)所示。

gensim(net，-1)(6.6)

其中-1的意思是系統是實時的。最后生成RBF神經網絡控制器。如圖6.17所示為RBF神經網絡控制器結構原理圖。圖6.17

RBF神經網絡控制器結構原理圖6.17

RBF神經網絡控制器結構原理

4)?RBF神經網絡模型的穩定控制

使用這個RBF神經網絡控制器代替原模糊控制器，運行程序，比較結果。如圖6.18所示為RBF神經網絡控制器結構框圖。圖6.18RBF神經網絡控制器結構框圖

為了更直觀地研究RBF網絡逼近的效果，首先采用訓練RBF神經網絡控制策略，利用上節所求解的逆運動方程和MatlabRobotics工具箱的fkine語句，可以獲得末端機械手位姿所對應的齊次變換矩陣，用ikine函數求解對應的關節轉角。在實際仿真中，使用newrb函數時，函數格式為net=newrb(P,T,GOAL,SPREAD,MN,DF)，選擇徑向基神經元層散布常數spread(=1.25)，P是輸入向量，用Matlab自帶的模糊控制器仿真來獲取。T是目標向量，GOAL是均方誤差，當網絡誤差目標(goal)為0.01時，能夠得到很好的仿真結果。

源程序語句如式(6.7)所示。

RBF仿真模型基本上跟BP網絡模型相似，但是仿真效果不太理想。因為RBF網絡比較適用于分類問題而不是控制問題。與BP神經網絡的函數比較，RBF的訓練函數相對簡單一點，參數也不是很多，且大部分參數是系統默認的。

6.4多個關節的機器人實戰

6.4.1多個關節機器人仿真關節機器人，也稱關節手臂機器人或關節機械手臂，是當今工業領域中最常見的工業機器人的形態之一，適用于諸多工業領域的機械自動化作業。比如，自動裝配、噴漆、搬運、焊接等工作。這里介紹STM32平臺多個關節機器人的仿真分析，其具體內容包括關節機器人的數學表示、關節機械臂軟件建模、關節機械臂的仿真控制等。

1.?多個關節機器人的數學表示

機器人運動學作為理論力學的分支學科，運用幾何學方法來研究物體的運動，從幾何角度描述和研究物體的位置隨時間的變化規律。它主要研究物體的位置、速度和加速度之間的關系。

1)?正運動學

已知桿件的長度等幾何參數和關節角度矢量，求機械臂末端執行器的位置和姿態，稱為正運動學。兩連桿平面旋轉關節機械臂如圖6.19所示，圖中兩連桿平面旋轉關節變量主要通過連桿長度L1和L2及關節角θ1和θ2等參數來定義。表示關節位置的變量θ1和θ2稱為關節變量。對于旋轉關節，關節變量通常用關節角度θ表示；而對于移動關節，關節變量通常用關節移動距離d來表示。圖6.19兩連桿平面旋轉關節變量表示示意圖

定義矢量R和θ為表示機械臂末端的位置和關節變量，如式(6.8)所示。

其中x和y是機械臂末端在xOy坐標系中的坐標，θ1和θ2為兩個桿件的轉動角度。

根據幾何學知識不難得出如式(6.9)所示關節變量間關系式：

式(6.9)可以采用矢量函數的形式表示，如式(6.10)所示：

2)逆運動學

已知桿件的長度等幾何參數和機械臂末端執行器的位置姿態，研究達到給定的位置姿態所需的機械臂關節角度矢量的，稱為逆運動學。

對于兩連桿平面旋轉關節機械臂，假定末端執行器的位置B(x，y)已知。將其進行簡化，如圖6.20所示。圖6.20兩連桿平面旋轉關節機械臂簡圖

根據幾何關系可知，在△AOB中，利用余弦定理可求得α，如式(6.11)所示。

從而可得：θ2?=π－α。

3)機器人動力學

作為物體的力和物體運動之間關系的力學分支，機器人動力學是研究如何根據物體質量、物體運動狀態以及物體當前受力情況，根據物體運動與驅動力之間關系的規律，實現在其關節軸上施以驅動力來實現相應運動效果。與機器人運動學問題類似，機器人動力學問題也分為正動力學問題和逆動力學問題兩類：已知作用在機器人上的驅動力隨時間變化的規律，求機器人的位置、速度和加速度軌跡等運動規律，稱為機器人正動力學問題；已知機器人隨時間的運動規律，求期望的驅動力隨時間變化的規律，稱為機器人逆動力學問題。正動力學問題和逆動力學問題的研究對于機器人的仿真與控制都是非常重要的。

2.?關節機械臂的建模

利用RoboticsToolbox創建機械臂模型需調用程序6.1關節機械臂建模調用語句命令集。首先由公式R=f?(θ)可知，由機械臂關節變量θ求機械臂末端位置R，建立二關節機械臂模型；其次，要構建各個關節位置數學表示，通過設定D-H參數、關節類型(移動關節和旋轉關節)、機械臂長度和質量、關節質量、驅動舵機的轉動慣量等參數，構建二關節機械臂的操作對象。最后將各個關節以合適的連接方式加以組合，即構建機械臂對象的核心在于順利構建各個關節并實現關節之間的正確連接。

如程序6.1所示為MatlabR2014a軟件的機器人工具箱RoboticsToolboxV9.10中關節機械臂建模調用語句命令集的說明和特定函數的調用格式。

3.關節機械臂的仿真控制

1)?關節機械臂仿真環境

關節機械臂模型構建較多，這里采用Matlab軟件平臺進行講解，介紹機器人、自動控制系統領域的機械臂建模步驟，給出設置一般模型的過程。

一般線性系統的傳遞函數模型表示如式(6.18)所示。

將系統的分子和分母多項式的系數按降冪的方式以向量的形式輸入給兩個變量num和den，就可以將傳遞函數模型輸入到Matlab環境中，采用Matlab軟件控制系統工具箱，定義tf()函數，由傳遞函數分子分母給出的變量構造出單個傳遞函數對象。調用該函數的格式如式(6.19)所示

針對關節機械臂建模設計，可以選擇Matlab軟件機器人工具箱，其中RoboticsToolBox在建模、軌跡規劃、控制及可視化等方面都非常實用，能提供一些如運動學、動力學和生成機器人軌跡等功能，通過機器人工具箱的仿真分析可為機器人的設計和控制提供有效數據參考，以便使用者根據實際的機器人參數進行對應更改。下面介紹在MatlabR2014a中安裝機器人工具箱的具體方法：

(1)?下載V9.10版的機器人工具箱壓縮文件robot-9.10.zip，包含對工具箱中各函數使用進行介紹的文檔說明；

(2)?把robot-9.10.zip壓縮文件解壓，得到rvctools文件夾，將該文件夾復制到Matlab安裝目錄下的toolbox子目錄下；

(3)?在MatlabR2014a主界面下點擊“設置路徑”；

(4)?點擊“添加并包含子文件夾”；

(5)?找到toolbox子目錄下的rvctools子文件夾并選擇它；

(6)?在MatlabR2014a主界面的命令窗口鍵入“startup_rvc”安裝工具箱；

(7)?在命令窗口鍵入“ver”命令，則Matlab的所有工具箱都會被列出，可查看到RoboticsToolboxV9.10.0已在所列出的工具箱中。

至此，機器人工具箱安裝完成。

如圖6.21所示為機器人關節臂建模流程圖。

從圖6.21所示的建模流程圖中可以看出，程序開始調用MatlabR2016a中的RoboticsToolbox工具箱中的Link函數，從而建立機器人關節機械臂的控制對象，設置位姿，最后輸出三維圖像。圖6.21建模流程圖

2)?關節機械臂控制程序

程序6.2為關節機械臂的雙連桿組合控制程序代碼。兩個關節機械臂模型構建使用了最新版本的Seriallink函數對機械臂bot進行了定義，用bot.plot命令顯示出機械臂的三維圖形，用bot.teach命令調出機械臂的控制窗口，顯示出關節變量滑動條，如圖6.22所示。通過滑動窗口中的滑塊，實現對機械臂的運動控制。圖6.22兩自由度及多自由度關節建模示意圖

4.?關節模型仿真控制實現

機器人的連桿可看成是剛性的，可從剛體入手來描述它的位姿。剛體參考點的位置和剛體的姿態統稱為剛體的位姿，其描述方法較多，有多元的齊次矩陣變換法、矢量法、旋量法和四元數法等。Matlab軟件RoboticsToolBox內含多達數百個關于機械臂的函數，可以充分滿足用戶對機械臂的計算要求，且工具箱提供Simulink模塊，實現用戶在Simulink環境下對工具箱的使用。

其中含有Puma560和Stanford機器人的仿真實例，Puma560機器人具有六自由度，在軟件命令行窗口輸入mdl_puma560，在文件mdl_puma560.m中可查看到Puma560機械臂的建立過程，且在文件中定義了相應四個典型的位姿，其參數分別設置如下：

通過使用Plot函數可繪制機械臂的相應位姿圖，如圖6.23所示為Puma560的機械臂仿真界面。圖6.23

Puma560的機械臂仿真界面

圖6.24為Matlab機械臂四個關節位置角度仿真程序運行結果圖，其中，當改變機械臂關節的任意控制角度后，在Matlab上可以根據所創建的可跟隨移動機械臂模型進行一些控制性能分析。如圖6.24(a)所示為Matlab機械臂仿真模型程序運行生成的控制參數矩陣數據，圖6.24(b)所示為四個不同轉動關節角度位置值隨著控制時間的變化曲線，它們對應生成可調控的機械臂末端軌跡。圖6.24

Matlab機械臂角速度仿真程序運行結果圖

調用機器人聯動關節模型，運行程序6.3的關節機械臂的雙連桿組合控制程序代碼，先用toolbox中的Forwarddynamic進行編譯，再用Animation進行編譯，實現機械臂聯動模型仿真控制。

最后，采用總體逼近控制器實現圖6.25的關節機械臂模型末端軌跡跟蹤控制的仿真，控制系統仿真曲線如圖6.25所示。圖6.25關節首端與末端位置跟蹤曲線

6.4.2多自由度關節機器人的書寫實戰

一般認為由單個或多個關節機械臂組合控制實現的多關節控制機器人，其機械臂的舵機組合控制策略設計方案流程圖如圖6.26所示，三種關節機器人末端控制數據方式具體內容是：

方案一，采用有線數據線連接控制器與末端書寫執行器，控制器內部發送輸出字符書寫指令集，從而實現漢字的書寫；

方案二，采用無線通信方式實現機器人控制器與末端書寫執行器的命令傳達，數據手套實時將數據命令輸入到控制器內部，從而通過末端執行器實時實現漢字的書寫；

方案三，可以采用有線或無線通信方式實現機器人上位機與末端書寫執行器的命令傳達，而上位機的字體庫可以編輯大量漢字數據，從而通過末端執行器實時實現漢字的書寫。

圖6.26多關節舵機自主組合控制策略設計方案流程圖

1.?六自由度

這里給出的多關節機械臂末端控制實戰實例中，由六自由度舵機組合控制書寫機器人書寫模擬了復雜人類書寫漢字的控制行為，即末端筆尖運行位置軌跡問題；在完成調試信號調理、采樣速率、微定位平臺響應速度等方面參數指標后，多舵機組合控制策略的書寫機器人設計在建模仿真的基礎上進行實物關節舵機的組合，其中書寫機器人設計的六個自由度軟筆與地面接觸產生摩擦力，使舵機轉向，這相當于給筆尖一個向心力，在方向量不變時，筆尖就能夠克服重力，考慮舵機控制脈寬不變，書寫機器人能夠實現利用摩擦力進行筆尖勻速往復運動。

筆尖末端位置的運動學模型分析如圖6.27所示，模型運動學反解算的總體過程為：已知末端軟筆的目標位姿—→求出對應的六根繩子的長度—→與初始長度進行比較，求出各繩的伸長或縮短量—→解算各個伺服舵機應轉過的角度，便可以通過上位機發送相應的指令，使舵機轉動，從而控制軟筆運動。圖6.27六自由度的書法機器人運動模型

由P1、P2的坐標以及Q1～Q6的坐標即可計算得到6根繩子的長度，如式(6.21)所示。

已知坐標系中設置了軟筆控制點P1、P2和常量P3

的相對位置值，六自由度關節的固定支撐點為Q1～Q6，通過P1、P2坐標值和

Q1～Q6坐標值能計算出6根繩子的長度，將各繩的長度設為L1=L2=L3=LU，L4=L5=L6=LD，將其代入如式(6.22)所示的等式進行計算，可得

式(6.23)中，R為繞線輪的半徑，θ0是舵機的標定位置初始角度(即150°)，則各伺服舵機相對于初始位置的轉角即可計算得出。實際制作的軟筆書法機器人樣機上面三根繩與下面三根繩伸長時舵機轉動的方向是相反的，樣機結構參數是：上三繩初始繩長LU=209.045?mm，下三繩初始繩長LD?=?186.671?mm，繞線輪半徑R?=?37.5?mm。兩控制點間距離：=?|P1P2|=180mm，6個支撐點坐標(單位為mm)為：Q1(7.5,105.5,-174.7)；Q2(-155.08,105.5,80.87)；Q3(147.58,105.5,93.68)；Q4(-7.5,105.5,-174.74)；Q5(-147.58,105.5,93.68)；Q6(155.08,105.5,80.87)。

最后設計出的六自由度書寫機器人樣機的實物如圖6.28所示。

圖6.28六自由度書法機器人實物效果圖

2.二自由度

這里介紹的二自由度多舵機組合控制策略的書寫機器人設計是在建模仿真分析的基礎上，進行實物關節舵機組合控制。如圖6.29所示為書寫機器人的執行機構，主要由控制器、無線模塊和伺服舵機構成，其中軸臂舵機組合的二自由度硬筆書寫機器人實物包括只有抬起和放下兩個自由度控制動作的書寫末端、控制器和多個伺服舵機串聯，如圖6.29所示。圖6.29二自由度書寫機器人實物效果圖

6.4.3多關節機器人的仿生實戰

一般認為，作為自動化機器，能夠具備與人或生物相似的智能能力，如感知能力、規劃能力、動作能力和協同能力等的，可以稱之為仿生機器人。本節介紹仿生機器人實戰技術，包括四足機器人的腿部結構及步態設計分析、仿生動作編寫設置，最后完成四足機器人跟蹤光源行走起來的設計，以及四足仿生貓形機器人的連貫動作的設計；還有仿生蛇形機器人爬行設計。

1.四足機器人的構型

本小節介紹的四足機器人的構型設計是采用實驗室機器人套件中的STM32平臺控制器開展的實踐工作。首先進入STM32平臺控制器控制軟件工程設置界面，新建一個工程文件；接著在這個界面中的下方有一個構型選項框，其中有多個選項，已經預定義構型，如圖6.30所示。其中圖6.30(a)為“六足機器人”的構型，圖6.30(b)為“多足機器人”的構型，圖6.30(c)為“蛇形機器人”的構型，圖6.30(d)為“四足機器人”的構型。所有可以選擇的預定義構型都帶有三維虛擬場景，并已經對舵機進行了編號，這樣在使用時就會省去大量的準備工作。

這里我們直接用系統控制軟件提供的四足機器人構型進行編程，構型選擇“四足機器人”，如圖6.30(d)所示。最后點擊“下一步”進入舵機參數設置窗口，設置舵機數量以及舵機的控制構型。在舵機設置窗口中，通過修改“ID”欄內的舵機ID，就可以改變舵機ID號；參照窗口右邊圖示，設置ID與機器人關節之間的對應關系。需要注意的是，如果按照平臺控制軟件提供的預定義構型編程，搭建機器人時就必須嚴格按照圖6.30所示的不同構型的舵機設置界面設置的舵機ID和機器人關節對應關系搭建，否則機器人無法正常執行編寫的程序。圖6.30不同構型的舵機設置界面圖6.30不同構型的舵機設置界面圖6.30不同構型的舵機設置界面圖6.30不同構型的舵機設置界面

2.讓四足機器人跟蹤光源的步態設計

在前面的任務里，我們完成了四足機器人行走步態的設計，向機器人控制器燒寫如圖6.31所示的邏輯程序代碼，可以完成機器人頭部跟蹤光源的實驗。圖6.31程序邏輯設計流程圖

四足機器人步態設計的基本步驟如圖6.32所示。圖6.32(a)為初始步態起步動作，圖6.32(b)為模仿烏龜形動物向后劃水排水動作的步態一，圖6.32(c)為模仿烏龜形動物的右側劃水排水動作的步態二，圖6.32(d)為模仿烏龜形動物的右側前移劃水排水動作的步態三、圖6.32(e)所示為模仿烏龜形動物的右側后移劃水排水動作的步態四。圖6.32步態設計圖6.32步態設計圖6.32步態設計圖6.32步態設計圖6.32步態設計

將如圖6.33所示的四足機器人的步態控制程序進行編譯、下載程序文件到四足機器人的控制器里。然后將四足機器人放置到場地上，設置光源，查看機器人能不能發現并走近光源。然后移動光源，查看機器人能不能跟蹤光源。我們可以根據機器人的表現，修改光強傳感器的閾值，改進機器人的步態，實現機器人完美地走起來。圖6.33四足機器人的步態控制程序設計流程圖

3.?貓形機器人的連貫運動

對四足仿生貓形機器人的步態進行分析可以發現，主要是設計多個動作的連接而形成連貫的運動，其中包括貓形機器人的半蹲動作，這個動作是整個動作的起始動作，同時也是一個掌握平衡的動作。貓形機器人在進行所有動作前都會先還原為半蹲動作，達到平衡后再進行以后的動作，圖6.34所示是貓形機器人初始站立動作實物圖。圖6.34貓形機器人初始站立動作實物圖

貓形機器人共有24個舵機，ID是每個舵機的編號。初始站立結果是每個動作的基礎，整體速度都設為300。初始動作數據如表6.4所示。

貓天性好奇，遇到其他生物總會試探、捕捉。貓形機器人的前進捕捉動作就是根據此設計出來的。貓形機器人整體成匍匐姿勢，以左前爪的抬起、落下來模仿捕捉動作，如圖6.35所示。圖6.35前進捕捉動作實物圖

貓形機器人的前進捕捉動作中，整體舵機速度都設置為350；貓形機器人前進捕捉動作的24個舵機數據角度實驗參數設置如表6.5所示。

4.?蛇形機器人的連貫運動

在自然界中，蛇類有蜿蜒運動、伸縮運動、直線運動和側向運動等四種運動方式，其中蜿蜒運動是最常見也是效率最高的運動方式。本節主要介紹蛇形機器人運動的動作規劃和編程實現。仿生蛇形機器人的系統舵機個數為16個，如圖6.36所示，舵機的ID號為1～16，全部采用舵機模式；將各個舵機相連，模擬蛇形機器人的各個關節，達到仿生運動的效果。

1)?爬行運動

蛇形機器人的爬行動作屬于蛇類的蜿蜒運動，爬行運動調試完成的十四種動作如圖6.36所示。圖6.36(a)為“左側弓形”動作狀態1，圖6.36(b)為“頭部伸直”動作狀態2，圖6.36(c)為“頸部反向運動”動作狀態3，圖6.36(d)為“身體伸直”動作狀態4，圖6.36(e)為“腰部微微彎曲”動作狀態5，圖6.36(f)為“腰部伸直”動作狀態6，圖6.36(g)為“尾部彎曲”動作狀態7，圖6.36(h)為“右側弓形”動作狀態8，圖6.36(i)為“頭部彎曲”動作狀態9，圖6.36(j)為“頭部前伸”動作狀態10，圖6.36(k)為“腰部彎曲”動作狀態11，圖6.36(l)為“腰部伸直”動作狀態12，圖6.36(m)為“尾部伸直”動作狀態13，圖6.36(n)為“尾部彎曲”動作狀態14。圖6.36蛇形機器人爬行動作圖6.36蛇形機器人爬行動作圖6.36蛇形機器人爬行動作

蛇形機器人的爬行動作中，為保證機器人整體的協調性，16個舵機速度設定在180或200；16個舵機數據角度參數設置如表6.6所示。

2)?翻滾動作

如圖6.37所示為蛇形機器人翻滾動作的四個狀態。翻滾動作的實現要使蛇形機器人有一定的彎曲，然后再固定兩個點，調試其余的舵機。翻滾動作需要的速度設定為180～200，每個動作完成后延時2?s，使其可以360°地翻滾。圖6.37蛇形機器人翻滾動作

蛇形機器人翻滾動作的各個舵機速度及角度設置如表6.7所示。

3)?盤起、攻擊動作

蛇形機器人盤起、攻擊的動作是比較綜合的一個動作，因為不同的需要，每個動作中每個舵機的速度要求也不一樣，仿生蛇形機器人完成的三個動作情況如圖6.38所示。圖6.38盤起、攻擊動作

如表6.8所示為蛇形機器人做盤起、攻擊各個動作時每個舵機的速度和角度數據。蛇形機器人慢慢盤起的動作的速度設定為