在求解加速度的基礎(chǔ)上可以得出末端運(yùn)行線速度，末端線速度分為加速段、勻速段和減速段，其具體表達(dá)式為：

在得到了線速度和角速度之后就可以通過雅克比矩陣得出各個(gè)關(guān)節(jié)的關(guān)節(jié)角速度，再乘以固定的時(shí)間即可得出在每一步的關(guān)節(jié)轉(zhuǎn)角。

2 Nios II軟核體系結(jié)構(gòu)

Nios II是Altera公司的第二代用戶可配置的通用32位RISC軟核處理器，是Altera公司特有的基于通用FPGA架構(gòu)的CPU軟核。它具備完整的32位指令集、32位數(shù)據(jù)通道和地址空間；帶有32個(gè)通用寄存器；支持32個(gè)外部中斷源；單指令的32位與32位乘法和除法結(jié)果是32位；對(duì)于結(jié)果為64位或128位的乘法，提供專用指令；大多數(shù)指令可以在一個(gè)時(shí)鐘周期內(nèi)完成；帶有單指令桶形移位寄存器；可以訪問各種片上外設(shè)，提供與片外存儲(chǔ)器和外設(shè)的接口；處理器性能超過200 DMIPS。

Nios II是一個(gè)可配置的軟核處理器，用戶可以根據(jù)性能和成本的要求來增加或刪減處理器的功能。Nios II處理器不像ARM那樣是由固定的芯片來實(shí)現(xiàn)，而是采用IP核的方式實(shí)現(xiàn)的，可以配置在滿足任何要求的AlteraFPGA器件中，因此，Nios II處理器給實(shí)際應(yīng)用帶來了很大的靈活性。只要芯片上有足夠的空間，就可以不斷進(jìn)行升級(jí)而不用修改電路結(jié)構(gòu)。另外，Nios II處理器作為一個(gè)標(biāo)準(zhǔn)的RISC處理器，具有執(zhí)行標(biāo)準(zhǔn)的C源代碼和程序的可移植性強(qiáng)等特點(diǎn)。

3 協(xié)處理器結(jié)構(gòu)

由于模塊化機(jī)器人的控制需要進(jìn)行大量復(fù)雜的三角函數(shù)運(yùn)算以及矩陣運(yùn)算，單一Nios II軟核處理器在完成這些運(yùn)算的同時(shí)并不能保證控制的快速性、實(shí)時(shí)性要求。因此本文設(shè)計(jì)一個(gè)專用的IP軟核作為協(xié)處理器，用來進(jìn)行各種機(jī)器人運(yùn)動(dòng)學(xué)的解算。