本發(fā)明屬于機(jī)器視覺(jué)與人工智能領(lǐng)域和肢體動(dòng)作識(shí)別領(lǐng)域，尤其涉及一種面向醫(yī)療機(jī)器人的肢體語(yǔ)言檢測(cè)與跟蹤系統(tǒng)設(shè)計(jì)方法。

背景技術(shù)：

隨著國(guó)家醫(yī)療衛(wèi)生事業(yè)的蓬勃發(fā)展和自動(dòng)化學(xué)科的不斷進(jìn)步，以自動(dòng)化理論為核心的醫(yī)療輔助器械層出不窮。近幾年來(lái)，一些醫(yī)療器具，如體溫計(jì)、血糖計(jì)、滅菌燈、手術(shù)刀、高電位治療儀等，都在不斷進(jìn)步，向著智能化、小型化和便攜化發(fā)展，為醫(yī)生和患者都提供了極大的便利。

在新興的醫(yī)療器械中，用在手術(shù)中進(jìn)行輔助的自動(dòng)化機(jī)械占很大比例。由于多數(shù)外科手術(shù)時(shí)間長(zhǎng)，強(qiáng)度大，精度要求高，對(duì)主刀醫(yī)生的技術(shù)和體力都有非常高的要求，而利用自動(dòng)控制理論制造的相關(guān)器械，在不影響精度的同時(shí)，可以長(zhǎng)時(shí)間工作，能夠比較完美的解決類(lèi)似問(wèn)題，減輕醫(yī)生的負(fù)擔(dān)，也為病患的康復(fù)添了一份保障。這是醫(yī)學(xué)進(jìn)步的一部分，同時(shí)也是自動(dòng)化學(xué)科實(shí)際應(yīng)用的經(jīng)典案例。

針對(duì)需要用到內(nèi)窺鏡的外科手術(shù)，傳統(tǒng)的方法常常需要用一只手來(lái)控制內(nèi)窺鏡的情況，這就限制了醫(yī)生另一只操縱手術(shù)刀的手的動(dòng)作幅度和精度，增加了醫(yī)生體力的消耗，在一定程度上增加了相關(guān)手術(shù)的風(fēng)險(xiǎn)，減少了患者被成功治療的機(jī)會(huì)。本發(fā)明基于人機(jī)交互技術(shù)和自動(dòng)控制理論，通過(guò)用傳感器采集醫(yī)生的腳部的位姿信息，并識(shí)別某些特定的姿態(tài)變化及其按一定順序的組合，完成對(duì)內(nèi)窺鏡的控制，在保證手術(shù)能夠順利完成的前提下，減輕了主刀醫(yī)師的負(fù)擔(dān)，實(shí)現(xiàn)了我們方便醫(yī)生，造?；颊叩哪繕?biāo)。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的在于提供一種面向醫(yī)療機(jī)器人的肢體語(yǔ)言檢測(cè)與跟蹤系統(tǒng)設(shè)計(jì)方法。

本發(fā)明的目的是這樣實(shí)現(xiàn)的：

(1)設(shè)計(jì)系統(tǒng)的整體結(jié)構(gòu)；

整體系統(tǒng)分為兩個(gè)大的功能模塊，分別是下位機(jī)模塊和上位機(jī)模塊；

下位機(jī)模塊由主控芯片、姿態(tài)傳感器和通信元件組成；在上電以后，主控芯片對(duì)各個(gè)部件進(jìn)行初始化，之后以固定的頻率從姿態(tài)傳感器中讀取數(shù)據(jù)，在經(jīng)過(guò)簡(jiǎn)單的平滑之后，通過(guò)通信模塊，以既定的通信協(xié)議，向上位機(jī)進(jìn)行發(fā)送；

上位機(jī)模塊由通信模塊，姿態(tài)解算模塊，曲線顯示模塊，3D模型創(chuàng)建模塊，骨骼創(chuàng)建模塊，骨骼變換模塊，3D顯示及更新模塊組成；啟動(dòng)時(shí)使用3D模型創(chuàng)建模塊對(duì)3D模型的頂點(diǎn)、貼圖、內(nèi)骨骼擬合信息進(jìn)行初始化，導(dǎo)入要用的渲染器，并創(chuàng)建好繪圖的環(huán)境；之后通過(guò)通信模塊與下位機(jī)進(jìn)行通信，每當(dāng)接收到下位機(jī)傳來(lái)的姿態(tài)信息之后，將其通過(guò)既定接口送給姿態(tài)解算模塊；姿態(tài)解算模塊通過(guò)相關(guān)原理，將下位機(jī)傳來(lái)的原始數(shù)據(jù)解算成所需的姿態(tài)信息，并進(jìn)一步將數(shù)據(jù)傳給曲線顯示模塊和骨骼變換模塊；曲線顯示模塊將數(shù)據(jù)用實(shí)時(shí)數(shù)據(jù)的形式顯示出來(lái)；骨骼變換模塊根據(jù)姿態(tài)信息，深度優(yōu)先遍歷骨骼樹(shù)，對(duì)骨骼所要做的姿態(tài)變換進(jìn)行計(jì)算，之后將骨骼位姿信息傳給3D更新模塊；3D更新模塊根據(jù)收到的骨骼位姿信息，最終將結(jié)果以3D模型的形式顯示在電腦的屏幕上；

(2)設(shè)計(jì)系統(tǒng)的下位機(jī)模塊；對(duì)姿態(tài)傳感器、主控芯片、通信器件進(jìn)行選型，并分別進(jìn)行下位機(jī)硬件設(shè)計(jì)和下位機(jī)軟件設(shè)計(jì)；

姿態(tài)傳感器采用六軸跟蹤傳感器MPU-6050，主控芯片采用STM32F103C8T6單片機(jī)，通信模塊使用無(wú)線串口進(jìn)行通信；

其中MPU-6050固定在醫(yī)生的腳上，采集姿態(tài)信息，單片機(jī)STM32F103C8T6負(fù)責(zé)MPU-6050的初始化工作，并從MPU-6050中讀取數(shù)據(jù)，通過(guò)無(wú)線串口發(fā)送給上位機(jī)；使用STM32F103C8T6的SWD接口下載器的VCC和GND接口完成供電，MPU-6050和無(wú)線串口連接在STM32F103C8T6上，引入TPS76833供電；

系統(tǒng)下位機(jī)的軟件設(shè)計(jì)包括STM32F103C8T6與MPU-6050的初始化，MPU-6050的數(shù)據(jù)的讀取，通過(guò)無(wú)線串口將數(shù)據(jù)發(fā)送至上位機(jī)；

STM32F103C8T6的初始化主要完成四個(gè)步驟：

(2.1)初始化系統(tǒng)的SysTick定時(shí)器，使之啟動(dòng)之后將每1m秒觸發(fā)一次中斷；

(2.2)串口的初始化：?jiǎn)?dòng)GIPOA和USART1的設(shè)備時(shí)鐘，將GPIOA9和GPIOA10設(shè)為復(fù)用推挽輸出模式、浮空輸入模式；配置USART1將其波特率設(shè)為9600，數(shù)據(jù)位為8位，1位停止位，無(wú)奇偶校驗(yàn)，無(wú)硬件控制，使用數(shù)據(jù)接收和數(shù)據(jù)發(fā)送；

(2.3)將C標(biāo)準(zhǔn)庫(kù)中的printf()的輸出重定向到USART1；

(2.4)I2C通信的初始化：?jiǎn)?dòng)GPIOB和I2C1的時(shí)鐘，GPIOB6和GPIOB7設(shè)為復(fù)用漏輸出模式I2C1的工作模式設(shè)為I2C，低電平數(shù)據(jù)變化時(shí)SCL線的Duty為0.33，開(kāi)啟消息應(yīng)答，7位地址，設(shè)置自身地址為0xB；

MPU-6050的初始化是對(duì)內(nèi)部的寄存器進(jìn)行設(shè)置，其中寄存器PWR_MGMT_1設(shè)置為0x00，寄存器SAMPLE RATE DIVIDER設(shè)為0x07，寄存器CONFIG為0x06，寄存器ACCELEROMETER CONFIGURATION設(shè)為0x01，寄存器GYROSCOPE CONFIGURATION設(shè)為0x18；讀取MPU-6050的WHO_AM_I寄存器，如果其值為0x68則正常啟動(dòng)，否則重新初始化；在正確配置MPU-6050并檢查無(wú)誤后，開(kāi)始讀取MPU-6050的數(shù)據(jù)；

讀取MPU-6050的數(shù)據(jù)采用輪詢的辦法，每100ms讀取一次加速度計(jì)和角速度計(jì)的數(shù)據(jù)，其中MPU-6050測(cè)量得到加速度數(shù)據(jù)和角速度數(shù)據(jù)各占用了六個(gè)連續(xù)的寄存器，溫度傳感器的測(cè)量值占用了兩個(gè)寄存器；

通過(guò)無(wú)線串口將下位機(jī)數(shù)據(jù)發(fā)送至上位機(jī)是通過(guò)制定相關(guān)的通信協(xié)議實(shí)現(xiàn)的；

包括加速度的傳輸：第一個(gè)字節(jié)為0x55表示數(shù)據(jù)包的開(kāi)頭；第二個(gè)字節(jié)為0x51，表示這個(gè)數(shù)據(jù)包中存放的是加速度數(shù)據(jù)；第三個(gè)字節(jié)為AxL，表示加速度計(jì)X軸數(shù)據(jù)的低8位；第四個(gè)字節(jié)為AxH，表示速度計(jì)X軸數(shù)據(jù)的高8位；第五個(gè)字節(jié)為AyL，表示加速度計(jì)Y軸數(shù)據(jù)的低8位；第六個(gè)字節(jié)為AyH，表示加速度計(jì)Y軸數(shù)據(jù)的高8位；第七個(gè)字節(jié)為AzL，表示加速度計(jì)Z軸數(shù)據(jù)的低8位；第八個(gè)字節(jié)為AzH，表示加速度計(jì)Z軸數(shù)據(jù)的高8位；第九位為T(mén)L，為溫度信息的低8位；第十位為T(mén)H，為溫度信息的高8位；第十一位為，校驗(yàn)位，其值為前面所有字節(jié)之和對(duì)0x100的余數(shù)；角速度的傳輸：第一個(gè)字節(jié)為0x55表示數(shù)據(jù)包的開(kāi)頭；第二個(gè)字節(jié)為0x52，表示這個(gè)數(shù)據(jù)包中存放的是角速度數(shù)據(jù)；第三個(gè)字節(jié)為WxL，表示角速度計(jì)X軸數(shù)據(jù)的低8位；第四個(gè)字節(jié)為WxH，表示速度計(jì)X軸數(shù)據(jù)的高8位；第五個(gè)字節(jié)為WyL，表示角速度計(jì)Y軸數(shù)據(jù)的低8位；第六個(gè)字節(jié)為WyH，表示角速度計(jì)Y軸數(shù)據(jù)的高8位；第七個(gè)字節(jié)為WzL，表示角速度計(jì)Z軸數(shù)據(jù)的低8位；第八個(gè)字節(jié)為WzH，表示角速度計(jì)Z軸數(shù)據(jù)的高8位；第九位為T(mén)L，為溫度信息的低8位；第十位為T(mén)H，為溫度信息的高8位；第十一位為，校驗(yàn)位，其值為前面所有字節(jié)之和對(duì)0x100的余數(shù)；

(3)設(shè)計(jì)系統(tǒng)的上位機(jī)模塊：接收下位機(jī)發(fā)送來(lái)的信息、對(duì)接收到的信息進(jìn)行姿態(tài)解算、顯示曲線、3D繪圖；

串口通信模塊借助于Qt5的SerialPort庫(kù)來(lái)完成，負(fù)責(zé)與下位機(jī)通信，通過(guò)監(jiān)聽(tīng)串口發(fā)送來(lái)的數(shù)據(jù)，按照既定通信協(xié)議對(duì)數(shù)據(jù)進(jìn)行解析，將數(shù)據(jù)發(fā)送給姿態(tài)解算模塊進(jìn)行姿態(tài)解算；串口模塊由兩部分組成：第一部分SerialDialog類(lèi)，負(fù)責(zé)UI中對(duì)串口的設(shè)置和人機(jī)交互，界面如圖6所示；第二部分是SerialThread類(lèi)，負(fù)責(zé)在另外一個(gè)線程中實(shí)時(shí)監(jiān)聽(tīng)串口，并根據(jù)通信協(xié)議對(duì)其進(jìn)行解析；串口數(shù)據(jù)解析采用的方法是建立一個(gè)類(lèi)似數(shù)據(jù)結(jié)構(gòu)中的隊(duì)列的緩沖區(qū)，每次接收到數(shù)據(jù)就將其添加到緩沖區(qū)的尾部，之后從緩沖區(qū)的頭部開(kāi)始尋找通信協(xié)議的開(kāi)頭，找到開(kāi)頭之后，判斷從此處到緩沖區(qū)的結(jié)尾的長(zhǎng)度是否大于等于通信協(xié)議的長(zhǎng)度，如果滿足這個(gè)條件，就從緩沖區(qū)中從協(xié)議開(kāi)頭開(kāi)始，取出通信協(xié)議規(guī)定長(zhǎng)度的字節(jié)數(shù)組，并把緩沖區(qū)開(kāi)頭到取出數(shù)組結(jié)尾位置的數(shù)據(jù)刪除，之后重復(fù)這個(gè)動(dòng)作，直到緩沖區(qū)中沒(méi)有通信協(xié)義開(kāi)頭，或其后的長(zhǎng)度小于協(xié)議的長(zhǎng)度，跳出，等待下一次接收到數(shù)據(jù)；

姿態(tài)解算模塊是上位機(jī)程序中的靈魂，將從串口模塊獲得的數(shù)據(jù)進(jìn)行姿態(tài)解算，并繼續(xù)將數(shù)據(jù)傳遞給曲線顯示模塊和3D顯示模塊；姿態(tài)解算是根據(jù)三軸加速度、三軸角速度到四元數(shù)的理論計(jì)算完成的，用四元數(shù)表示姿態(tài)矩陣：

求解姿態(tài)即求解姿態(tài)矩陣，等價(jià)為四元數(shù)的更新，而四元數(shù)的更新可通過(guò)三軸角速度實(shí)現(xiàn)，具體步驟為：

(3.1)獲取三軸角速度，并用gx、gy、gz分別代表角速度的三軸，采用積分的方法獲得姿態(tài)，根據(jù)此前的四元數(shù)對(duì)重力方向進(jìn)行估計(jì)；

(3.2)選用陀螺儀獲取加速度計(jì)的值，并用ax、ay、az分別代表三軸加速度。在進(jìn)行姿態(tài)解算之前，對(duì)其進(jìn)行規(guī)范化；

(3.3)利用互補(bǔ)濾波算法融合角速度和加速度值并求取誤差，將此誤差經(jīng)過(guò)比例和積分環(huán)節(jié)之后補(bǔ)償?shù)浇撬俣鹊臏y(cè)量值當(dāng)中；

(3.4)一階龍格庫(kù)塔法更新四元數(shù)；

上位機(jī)曲線顯示模塊使用的是Qt的第三方開(kāi)源庫(kù)QCustomPlot，曲線函數(shù)的初始化和數(shù)據(jù)更新函數(shù)位于主界面的類(lèi)MainWindow中；系統(tǒng)的上位機(jī)程序主界面中顯示三軸加速度曲線、三軸角速度曲線、姿態(tài)角曲線和四元數(shù)曲線四個(gè)曲線，曲線的繪制是與主界面UI的更新一同進(jìn)行的；

3D顯示模塊負(fù)責(zé)3D模塊的載入，骨骼變換的計(jì)算及應(yīng)用，模型顯示的更新；其中模型載入使用的是Assimp開(kāi)源庫(kù)導(dǎo)入3D模型的頂點(diǎn)、貼圖和骨骼信息，使用OpenGL對(duì)導(dǎo)入的信息進(jìn)行渲染；本系統(tǒng)在建模時(shí)使用Blender建模軟件構(gòu)建了33塊骨骼，并將其變化信息存儲(chǔ)在數(shù)據(jù)結(jié)構(gòu)樹(shù)中；3D模型更新的方法為：將3D模型的第一個(gè)頂點(diǎn)都綁定在了不同的骨骼之上，不同骨骼對(duì)同一個(gè)頂點(diǎn)的影響程度使用權(quán)重表示，而當(dāng)相應(yīng)骨骼進(jìn)行變換時(shí)，受其影響的頂點(diǎn)也會(huì)根據(jù)權(quán)重做出相應(yīng)的響應(yīng)。通過(guò)控制骨骼的運(yùn)動(dòng)，來(lái)實(shí)現(xiàn)使模型運(yùn)動(dòng)的目的。

本發(fā)明的有益效果在于：

(1)采用醫(yī)生腳部的姿態(tài)變化控制內(nèi)窺鏡的方位變化充分考慮到了手術(shù)時(shí)醫(yī)生一手控制手術(shù)刀一手控制內(nèi)窺鏡對(duì)手術(shù)造成的影響，最大程度提高了手術(shù)的成功率，減輕了醫(yī)生的手術(shù)負(fù)擔(dān)。

(2)考慮直接通過(guò)腳部動(dòng)作控制內(nèi)窺鏡會(huì)增加醫(yī)生的不便，對(duì)手術(shù)造成一定的影響，設(shè)計(jì)上位機(jī)3D模型直接復(fù)現(xiàn)腳部動(dòng)作和曲線變化反映姿態(tài)變化，進(jìn)而控制內(nèi)窺鏡，進(jìn)一步降低了手術(shù)的復(fù)雜度，提高了成功率。

附圖說(shuō)明

圖1系統(tǒng)整體結(jié)構(gòu)框圖；

圖2系統(tǒng)下位機(jī)硬件結(jié)構(gòu)設(shè)計(jì)圖；

圖3系統(tǒng)下位機(jī)硬件實(shí)物圖；

圖4系統(tǒng)下位機(jī)軟件流程圖；

圖5系統(tǒng)上位機(jī)總體結(jié)構(gòu)；

圖6串口相關(guān)窗口；

圖7串口執(zhí)行流程；

圖8曲線顯示界面；

圖9 3D顯示界面；

圖10骨骼樹(shù)。

具體實(shí)施方式

下面結(jié)合附圖舉例對(duì)本發(fā)明做更詳細(xì)地描述：

本方法實(shí)現(xiàn)了人體姿態(tài)控制醫(yī)療機(jī)械進(jìn)行手術(shù)，解決了在需要用到內(nèi)窺鏡的外科手術(shù)中醫(yī)生需一手控制手術(shù)刀，一手控制內(nèi)窺鏡的問(wèn)題。這樣不僅可以減輕醫(yī)生的手術(shù)負(fù)擔(dān)，同時(shí)也提高了手術(shù)的成功率。

本發(fā)明的設(shè)計(jì)是通過(guò)以下方案實(shí)現(xiàn)的：

(1)設(shè)計(jì)系統(tǒng)的整體結(jié)構(gòu)。

(2)設(shè)計(jì)系統(tǒng)的下位機(jī)模塊。對(duì)姿態(tài)傳感器、主控芯片、通信器件進(jìn)行選型，并分別進(jìn)行下位機(jī)硬件設(shè)計(jì)和下位機(jī)軟件設(shè)計(jì)。

(3)設(shè)計(jì)系統(tǒng)的上位機(jī)模塊，需要進(jìn)行的工作有：接收下位機(jī)發(fā)送來(lái)的信息、對(duì)接收到的信息進(jìn)行姿態(tài)解算、顯示曲線、3D繪圖。

采用醫(yī)生腳部的姿態(tài)變化控制內(nèi)窺鏡的方位變化充分考慮到了手術(shù)時(shí)醫(yī)生一手控制手術(shù)刀一手控制內(nèi)窺鏡對(duì)手術(shù)造成的影響，最大程度提高了手術(shù)的成功率，減輕了醫(yī)生的手術(shù)負(fù)擔(dān)。

考慮直接通過(guò)腳部動(dòng)作控制內(nèi)窺鏡會(huì)增加醫(yī)生的不便，對(duì)手術(shù)造成一定的影響，設(shè)計(jì)上位機(jī)3D模型直接復(fù)現(xiàn)腳部動(dòng)作和曲線變化反映姿態(tài)變化，進(jìn)而控制內(nèi)窺鏡，進(jìn)一步降低了手術(shù)的復(fù)雜度，提高了成功率。

面向醫(yī)療機(jī)器人的肢體語(yǔ)言檢測(cè)與跟蹤系統(tǒng)設(shè)計(jì)，主要包括兩大模塊：下位機(jī)模塊和上位機(jī)模塊。其特點(diǎn)是：針對(duì)需要內(nèi)窺鏡的外科手術(shù)，采用醫(yī)生腳部的姿態(tài)變化代替手動(dòng)操作控制內(nèi)窺鏡的方位；為使系統(tǒng)不影響整個(gè)手術(shù)過(guò)程，腳部姿態(tài)的變化采用上位機(jī)3D模型和曲線變化顯示。

本發(fā)明提出的一種面向醫(yī)療機(jī)器人的肢體語(yǔ)言檢測(cè)與跟蹤系統(tǒng)具體包括以下幾個(gè)步驟。

步驟一：設(shè)計(jì)系統(tǒng)的整體結(jié)構(gòu)。

系統(tǒng)的整體結(jié)構(gòu)框圖如圖1所示。整體系統(tǒng)從功能上分為兩個(gè)大的功能模塊組成，分別是下位機(jī)模塊和上位機(jī)模塊。

下位機(jī)由主控芯片、姿態(tài)傳感器和通信元件組成。主要的工作方式是在上電以后，主控芯片對(duì)各個(gè)部件進(jìn)行初始化，之后以固定的頻率從姿態(tài)傳感器中讀取數(shù)據(jù)，在經(jīng)過(guò)簡(jiǎn)單的平滑之后，通過(guò)通信模塊，以既定的通信協(xié)議，向上位機(jī)進(jìn)行發(fā)送。

上位機(jī)從功能上分為通信模塊，姿態(tài)解算模塊，曲線顯示模塊，3D模型創(chuàng)建模塊，骨骼創(chuàng)建模塊，骨骼變換模塊，3D顯示及更新模塊等組成。主要的工作方式是：?jiǎn)?dòng)時(shí)使用3D模型創(chuàng)建模塊對(duì)3D模型的頂點(diǎn)、貼圖、內(nèi)骨骼擬合等信息進(jìn)行初始化，導(dǎo)入要用的渲染器，并創(chuàng)建好繪圖的環(huán)境；之后通過(guò)通信模塊與下位機(jī)進(jìn)行通信，每當(dāng)接收到下位機(jī)傳來(lái)的姿態(tài)信息之后，將其通過(guò)既定接口送給姿態(tài)解算模塊；姿態(tài)解算模塊通過(guò)相關(guān)原理，將下位機(jī)傳來(lái)的原始數(shù)據(jù)解算成所需的姿態(tài)信息，并進(jìn)一步將數(shù)據(jù)傳給曲線顯示模塊和骨骼變換模塊；曲線顯示模塊將數(shù)據(jù)用實(shí)時(shí)數(shù)據(jù)的形式顯示出來(lái)；骨骼變換模塊根據(jù)姿態(tài)信息，深度優(yōu)先遍歷骨骼樹(shù)，對(duì)骨骼所要做的姿態(tài)變換進(jìn)行計(jì)算，之后將骨骼位姿信息傳給3D更新模塊；3D更新模塊根據(jù)收到的骨骼位姿信息，最終將結(jié)果以3D模型的形式顯示在電腦的屏幕上。

步驟二：設(shè)計(jì)系統(tǒng)的下位機(jī)模塊。對(duì)姿態(tài)傳感器、主控芯片、通信器件進(jìn)行選型，并分別進(jìn)行下位機(jī)硬件設(shè)計(jì)和下位機(jī)軟件設(shè)計(jì)。

考慮到實(shí)驗(yàn)需求和經(jīng)濟(jì)問(wèn)題，姿態(tài)傳感器采用低功耗的六軸跟蹤傳感器MPU-6050，主控芯片采用功能豐富且結(jié)構(gòu)簡(jiǎn)單的STM32F103C8T6單片機(jī)，通信模塊使用無(wú)線串口進(jìn)行通信。

系統(tǒng)的下位機(jī)硬件結(jié)構(gòu)設(shè)計(jì)圖如圖2所示，硬件實(shí)物圖如圖3所示。其中MPU-6050固定在醫(yī)生的腳上，采集姿態(tài)信息，單片機(jī)STM32F103C8T6負(fù)責(zé)MPU-6050的初始化工作，并從MPU-6050中讀取數(shù)據(jù)，在簡(jiǎn)單處理后，通過(guò)無(wú)線串口發(fā)送給上位機(jī)。系統(tǒng)分別采用兩種不同的供電方式應(yīng)用于開(kāi)發(fā)過(guò)程中的調(diào)試和實(shí)際應(yīng)用。在實(shí)際開(kāi)發(fā)過(guò)程中由于系統(tǒng)硬件結(jié)構(gòu)簡(jiǎn)單、消耗功率小以及電流基本沒(méi)有太大波動(dòng)，簡(jiǎn)單使用STM32F103C8T6的SWD接口下載器的VCC和GND接口即可完成供電，MPU-6050和無(wú)線串口連接在STM32F103C8T6上，不需要額外增加供電模塊。這樣的連接方式，簡(jiǎn)單方便，工作量基本為零。但在實(shí)際應(yīng)用中，考慮到下位機(jī)要固定在醫(yī)生的腳上，隨醫(yī)生的腳一同運(yùn)動(dòng)，不能使用有線的供電方式，因此引入TPS76833供電。

系統(tǒng)下位機(jī)的軟件設(shè)計(jì)流程圖如圖4所示，主要包括STM32F103C8T6與MPU-6050的初始化，MPU-6050的數(shù)據(jù)的讀取，通過(guò)無(wú)線串口將數(shù)據(jù)發(fā)送至上位機(jī)等步驟。

STM32F103C8T6的初始化主要完成四個(gè)步驟：

(1)初始化系統(tǒng)的SysTick定時(shí)器，使之啟動(dòng)之后將每1m秒觸發(fā)一次中斷。

(2)串口的初始化：?jiǎn)?dòng)GIPOA和USART1(即GPIOA9和GPIOA10復(fù)用為串口功能時(shí)對(duì)應(yīng)的串口)的設(shè)備時(shí)鐘，將GPIOA9和GPIOA10設(shè)為復(fù)用推挽輸出模式、浮空輸入模式；配置USART1將其波特率設(shè)為9600，數(shù)據(jù)位為8位，1位停止位，無(wú)奇偶校驗(yàn)，無(wú)硬件控制，使用數(shù)據(jù)接收和數(shù)據(jù)發(fā)送。

(3)將C標(biāo)準(zhǔn)庫(kù)中的printf()的輸出重定向到USART1。

(4)I2C通信的初始化：?jiǎn)?dòng)GPIOB和I2C1的時(shí)鐘，GPIOB6和GPIOB7設(shè)為復(fù)用漏輸出模式I2C1的工作模式設(shè)為I2C，低電平數(shù)據(jù)變化時(shí)SCL線的Duty為0.33，開(kāi)啟消息應(yīng)答，7位地址，設(shè)置自身地址為0xB。

MPU-6050的初始化主要是對(duì)內(nèi)部的寄存器進(jìn)行設(shè)置，其中寄存器PWR_MGMT_1設(shè)置為0x00，寄存器SAMPLE RATE DIVIDER設(shè)為0x07，寄存器CONFIG為0x06，寄存器ACCELEROMETER CONFIGURATION設(shè)為0x01，寄存器GYROSCOPE CONFIGURATION設(shè)為0x18。接著讀取MPU-6050的WHO_AM_I寄存器，如果其值為0x68則正常啟動(dòng)，否則說(shuō)明出現(xiàn)了問(wèn)題，需重新初始化。在正確配置MPU-6050并檢查無(wú)誤后，即可開(kāi)始讀取MPU-6050的數(shù)據(jù)。

讀取MPU-6050的數(shù)據(jù)可以采用中斷法或是輪詢法，為實(shí)現(xiàn)方便，系統(tǒng)采用的是輪詢的辦法，每100ms讀取一次加速度計(jì)和角速度計(jì)的數(shù)據(jù)，其中MPU-6050測(cè)量得到加速度數(shù)據(jù)和角速度數(shù)據(jù)各占用了六個(gè)連續(xù)的寄存器，溫度傳感器的測(cè)量值占用了兩個(gè)寄存器。

通過(guò)無(wú)線串口將下位機(jī)數(shù)據(jù)發(fā)送至上位機(jī)是通過(guò)制定相關(guān)的通信協(xié)議實(shí)現(xiàn)的，具體描述為：(1)加速度的傳輸：第一個(gè)字節(jié)為0x55表示數(shù)據(jù)包的開(kāi)頭；第二個(gè)字節(jié)為0x51，表示這個(gè)數(shù)據(jù)包中存放的是加速度數(shù)據(jù)；第三個(gè)字節(jié)為AxL，表示加速度計(jì)X軸數(shù)據(jù)的低8位；第四個(gè)字節(jié)為AxH，表示速度計(jì)X軸數(shù)據(jù)的高8位；第五個(gè)字節(jié)為AyL，表示加速度計(jì)Y軸數(shù)據(jù)的低8位；第六個(gè)字節(jié)為AyH，表示加速度計(jì)Y軸數(shù)據(jù)的高8位；第七個(gè)字節(jié)為AzL，表示加速度計(jì)Z軸數(shù)據(jù)的低8位；第八個(gè)字節(jié)為AzH，表示加速度計(jì)Z軸數(shù)據(jù)的高8位；第九位為T(mén)L，為溫度信息的低8位；第十位為T(mén)H，為溫度信息的高8位；第十一位為，校驗(yàn)位，其值為前面所有字節(jié)之和對(duì)0x100的余數(shù)。(2)角速度的傳輸：第一個(gè)字節(jié)為0x55表示數(shù)據(jù)包的開(kāi)頭；第二個(gè)字節(jié)為0x52，表示這個(gè)數(shù)據(jù)包中存放的是角速度數(shù)據(jù)；第三個(gè)字節(jié)為WxL，表示角速度計(jì)X軸數(shù)據(jù)的低8位；第四個(gè)字節(jié)為WxH，表示速度計(jì)X軸數(shù)據(jù)的高8位；第五個(gè)字節(jié)為WyL，表示角速度計(jì)Y軸數(shù)據(jù)的低8位；第六個(gè)字節(jié)為WyH，表示角速度計(jì)Y軸數(shù)據(jù)的高8位；第七個(gè)字節(jié)為WzL，表示角速度計(jì)Z軸數(shù)據(jù)的低8位；第八個(gè)字節(jié)為WzH，表示角速度計(jì)Z軸數(shù)據(jù)的高8位；第九位為T(mén)L，為溫度信息的低8位；第十位為T(mén)H，為溫度信息的高8位；第十一位為，校驗(yàn)位，其值為前面所有字節(jié)之和對(duì)0x100的余數(shù)。

步驟三：設(shè)計(jì)系統(tǒng)的上位機(jī)模塊，需要進(jìn)行的工作有：接收下位機(jī)發(fā)送來(lái)的信息、對(duì)接收到的信息進(jìn)行姿態(tài)解算、顯示曲線、3D繪圖。

上位機(jī)系統(tǒng)在整體系統(tǒng)的實(shí)現(xiàn)中占有至關(guān)重要的地位，本質(zhì)上說(shuō)，整個(gè)下位機(jī)系統(tǒng)相對(duì)于上位機(jī)而言，只是充當(dāng)了一個(gè)傳感器的角色。準(zhǔn)確的對(duì)腳部姿態(tài)進(jìn)行檢測(cè)與跟蹤主要是通過(guò)上位機(jī)的姿態(tài)解算模塊、曲線繪制和3D模型實(shí)現(xiàn)的。因此，上位機(jī)的設(shè)計(jì)需具有模塊耦合度低、性能好、方便易用的特點(diǎn)。

系統(tǒng)的上位機(jī)系統(tǒng)主要使用C++語(yǔ)言完成，使用Qt作為UI解決方案，OpenGL作為3D繪圖支持，為方便編程，還使用了C++的STL標(biāo)準(zhǔn)庫(kù)，以及QCustomPlot和Assimp等開(kāi)源軟件。為提高程序的運(yùn)行效率，上位機(jī)多處使用了多線程技術(shù)。

上位機(jī)設(shè)計(jì)的總體結(jié)構(gòu)如圖5所示。

圖中串口通信模塊主要借助于Qt5的SerialPort庫(kù)來(lái)完成，負(fù)責(zé)與下位機(jī)通信，通過(guò)監(jiān)聽(tīng)串口發(fā)送來(lái)的數(shù)據(jù)，按照既定通信協(xié)議對(duì)數(shù)據(jù)進(jìn)行解析，將數(shù)據(jù)發(fā)送給姿態(tài)解算模塊進(jìn)行姿態(tài)解算。根據(jù)需求，串口模塊由兩部分組成：第一部分SerialDialog類(lèi)，負(fù)責(zé)UI中對(duì)串口的設(shè)置和人機(jī)交互，界面如圖6所示；第二部分是SerialThread類(lèi)，主要負(fù)責(zé)在另外一個(gè)線程中實(shí)時(shí)監(jiān)聽(tīng)串口，并根據(jù)通信協(xié)議對(duì)其進(jìn)行解析。串口數(shù)據(jù)解析采用的方法是建立一個(gè)類(lèi)似數(shù)據(jù)結(jié)構(gòu)中的隊(duì)列的緩沖區(qū)，每次接收到數(shù)據(jù)就將其添加到緩沖區(qū)的尾部，之后從緩沖區(qū)的頭部開(kāi)始尋找通信協(xié)議的開(kāi)頭，找到開(kāi)頭之后，判斷從此處到緩沖區(qū)的結(jié)尾的長(zhǎng)度是否大于等于通信協(xié)議的長(zhǎng)度，如果滿足這個(gè)條件，就從緩沖區(qū)中從協(xié)議開(kāi)頭開(kāi)始，取出通信協(xié)議規(guī)定長(zhǎng)度的字節(jié)數(shù)組，并把緩沖區(qū)開(kāi)頭到取出數(shù)組結(jié)尾位置的數(shù)據(jù)刪除，之后重復(fù)這個(gè)動(dòng)作，直到緩沖區(qū)中沒(méi)有通信協(xié)義開(kāi)頭，或其后的長(zhǎng)度小于協(xié)議的長(zhǎng)度，跳出，等待下一次接收到數(shù)據(jù)。整體的串口流程如圖7所示。

姿態(tài)解算模塊是上位機(jī)程序中的靈魂，將從串口模塊獲得的數(shù)據(jù)進(jìn)行姿態(tài)解算，并繼續(xù)將數(shù)據(jù)傳遞給曲線顯示模塊和3D顯示模塊。姿態(tài)解算是根據(jù)三軸加速度、三軸角速度到四元數(shù)的理論計(jì)算完成的，用四元數(shù)表示姿態(tài)矩陣如下所示：

求解姿態(tài)即求解姿態(tài)矩陣，又可以等價(jià)為四元數(shù)的更新，而四元數(shù)的更新可通過(guò)三軸角速度實(shí)現(xiàn)，具體步驟為：

(1)獲取三軸角速度，并用gx、gy、gz分別代表角速度的三軸，采用積分的方法獲得姿態(tài)，根據(jù)此前的四元數(shù)對(duì)重力方向進(jìn)行估計(jì)；

(2)選用陀螺儀獲取加速度計(jì)的值，并用ax、ay、az分別代表三軸加速度。在進(jìn)行姿態(tài)解算之前，對(duì)其進(jìn)行規(guī)范化；

(3)利用互補(bǔ)濾波算法融合角速度和加速度值并求取誤差，將此誤差經(jīng)過(guò)比例和積分環(huán)節(jié)之后補(bǔ)償?shù)浇撬俣鹊臏y(cè)量值當(dāng)中；

(4)一階龍格庫(kù)塔法更新四元數(shù)。方程如下：

其中，

上位機(jī)曲線顯示模塊使用的是Qt的第三方開(kāi)源庫(kù)QCustomPlot，曲線函數(shù)的初始化和數(shù)據(jù)更新函數(shù)位于主界面的類(lèi)MainWindow中。如圖8所示，系統(tǒng)的上位機(jī)程序主界面中將會(huì)顯示三軸加速度曲線、三軸角速度曲線、姿態(tài)角曲線和四元數(shù)曲線四個(gè)曲線，曲線的繪制是與主界面UI的更新一同進(jìn)行的，因此并不需要另加線程。

3D顯示模塊上位機(jī)系統(tǒng)中最為復(fù)雜的一部分，主要負(fù)責(zé)3D模塊的載入，骨骼變換的計(jì)算及應(yīng)用，模型顯示的更新。其中模型載入使用的是Assimp開(kāi)源庫(kù)導(dǎo)入3D模型的頂點(diǎn)、貼圖和骨骼信息，使用OpenGL對(duì)導(dǎo)入的信息進(jìn)行渲染，得到3D模型如圖9。本系統(tǒng)在建模時(shí)使用Blender建模軟件構(gòu)建了33塊骨骼，并將其變化信息存儲(chǔ)在數(shù)據(jù)結(jié)構(gòu)樹(shù)中，如圖10所示。3D模型更新的主要方法為：將3D模型的第一個(gè)頂點(diǎn)都綁定在了不同的骨骼之上，不同骨骼對(duì)同一個(gè)頂點(diǎn)的影響程度使用權(quán)重表示，而當(dāng)相應(yīng)骨骼進(jìn)行變換時(shí)，受其影響的頂點(diǎn)也會(huì)根據(jù)權(quán)重做出相應(yīng)的響應(yīng)。這樣，就可以通過(guò)控制骨骼的運(yùn)動(dòng)，來(lái)實(shí)現(xiàn)使模型運(yùn)動(dòng)的目的。本發(fā)明通過(guò)設(shè)計(jì)下位機(jī)完成了醫(yī)生腳部姿態(tài)信息的采集，通過(guò)設(shè)計(jì)上位機(jī)實(shí)現(xiàn)了腳部姿態(tài)信息的準(zhǔn)確檢測(cè)與跟蹤，并用3D模型實(shí)時(shí)復(fù)現(xiàn)醫(yī)生腳部動(dòng)作，為后續(xù)機(jī)械精確控制內(nèi)窺鏡做準(zhǔn)備，也為生物醫(yī)學(xué)的研究開(kāi)啟一個(gè)新篇章。