一種移動機器人軌跡修正控制算法制造技術

本發明專利技術公開了一種移動機器人軌跡修正控制算法，包括以下步驟：利用設于機器人上的IMU模塊計算機器人的姿態角；將姿態角的變化轉換為運動速度矢量；計算出合速度；計算出合速度作用下當前及剩余全部目標點修正位置；輸出已修正的運動軌跡。本發明專利技術中所述的一種移動機器人軌跡修正控制算法，通過使用彈簧及慣性裝置模塊感知機器人上身的姿態變化，達到對外力大小和方向的判斷，從而在主動運動基礎上，根據人在機器人運動過程中施加的力實時調整運動軌跡。

A trajectory correction control algorithm for mobile robot

The invention discloses a modified control algorithm for mobile robot trajectory, which comprises the following steps: Calculation of the robot by the IMU module is arranged on the robot's attitude angle; will change the attitude angle into the velocity vectors; calculate velocity; calculate the velocity under the action when before and all the remaining target position trajectory correction; the output has been modified. Modified control algorithm of mobile robot trajectory described in the invention, by using a spring posture change and inertial device module sensing robot body, the magnitude and direction of force judgment, resulting in active movement on the basis of real-time adjustment according to the trajectory of people applied in robot motion force.

【技術實現步驟摘要】
一種移動機器人軌跡修正控制算法
本專利技術屬于機器人人機協作控制領域，尤其是涉及一種移動機器人軌跡修正控制算法。
技術介紹
自傳統工業機器人誕生以來，機器人技術在短短的60年內便取得了顯著的進步。如今，機器人已經不再受限于工業應用，它的研究重點和發展方向更趨向于日常生活的應用，如餐飲、迎賓、娛樂、陪伴等。同時，機器人技術的發展也對人機協作的主動性要求也越來越高，也就是說，機器人不僅能夠根據人的施力情況而被動運動，而且能根據預先學習的知識與人協調作業，即主動協調控制。當前市場上絕大多數的移動機器人，在運動過程中并不能感受外力作用，也不能在主動運動過程中向人力做出妥協，這在很大程度上增加了機器人的危險性，使機器人無法與人安全共處。而且當前現有的人機協作機器人控制系統中，大多采用多維力傳感器來感受外力的大小和方向，以此作為控制算法的輸入。盡管這種控制系統對外力的感知簡便、準確，但是價格十分昂貴。
技術實現思路
有鑒于此，本專利技術旨在提出一種移動機器人軌跡修正控制算法，該方法在主動運動的基礎上，根據外力大小和方向修正運動軌跡。為達到上述目的，本專利技術的技術方案是這樣實現的：一種移動機器人軌跡修正控制算法，包括以下步驟：(101)利用設于機器人上的IMU模塊計算機器人的姿態角；(102)將姿態角的變化轉換為運動速度矢量；(103)計算出合速度；(104)計算出合速度作用下當前及剩余全部目標點修正位置；(105)輸出已修正的運動軌跡。進一步的，所述步驟101中，姿態角的計算采用卡爾曼濾波融合算法，(201)系統觀測方程：其中，α為機器人姿態角，β為系統穩態誤差，T為采樣周期，uk-1為k-1時刻陀螺儀角度測量量；(202)系統狀態方程：X(k)＝AX(k-1)+Buk-1其中，X(k)為k時刻預測的結果，X(k-1)為k-1時刻的最優結果；(203)由上一次的誤差協方差P(k-1)和過程噪聲預測新的誤差P(k)：P(k)＝AP(k-1)AT+Q其中，Q是系統過程的協方差矩陣，且Q_acce為加速度計的協方差，Q_gyro為陀螺儀測量協方差；(204)計算卡爾曼增益：Kg(k)＝P(k)HT(HP(k)HT+R)-1其中，H為系統觀測矩陣；(205)進行校正更新，得到狀態K的最優估算值X(k)：X(k)＝X(k-1)+Kg(k)(V(k)-HX(K-1))(206)為下一步估計k+1狀態最優值迭代進行更新操作，并更新k狀態協方差P(k)值：P(k)＝(I-Kg(k)H)P(k-1)其中，I為單位矩陣；按照如上步驟，計算完成時間更新和測量更新后，將上一次的后驗估計作為下一次的先驗估計，往復運算直至結果最優。進一步的，所述步驟101中，所述機器人的上身和下身通過裝有彈簧的圓盤連接，所述IMU模塊設于機器人腰部的圓盤上，IMU模塊包括3軸陀螺儀及3軸加速度計。進一步的，所述步驟102中，通過姿態角計算出人的作用力，作用力通過正比變換得到人為的運動速度矢量，人作用力大小與姿態角的關系為：Fhuman＝Fmax(1-e-bθ),|θ|≤θmax其中，θ為機器人上身受外力作用下的姿態角(橫滾角/俯仰角/扭轉角)，θmax為彈簧系統允許的最大姿態角度的大小。進一步的，所述步驟103中，合速度計算公式為：其中，為機器人期望速度矢量，為人為速度矢量，k1是對的影響權值，且0＜k1＜1。相對于現有技術，本專利技術所述的一種移動機器人軌跡修正控制算法具有以下優勢：借助安裝在機器人腰部彈簧裝置和慣性模塊來感知機器人上身姿態變化，從而在主動運動的基礎上，根據人在機器人運動過程中施加的力實時調整運動軌跡，實現人與機器人之間的協調運動，具有更高的主動性和智能性；采用卡爾曼濾波算法將加速度計與陀螺儀數據進行融合，提高測量精度。附圖說明圖1為本專利技術實施例所述的控制流程示意圖。具體實施方式下面將參考附圖并結合實施例來詳細說明本專利技術。如圖1所示，一種移動機器人軌跡修正控制算法，包括以下步驟：(101)利用設于機器人上的IMU模塊計算機器人的姿態角；(102)將姿態角的變化轉換為運動速度矢量；(103)計算出合速度；(104)計算出合速度作用下當前及剩余全部目標點修正位置；(105)輸出已修正的運動軌跡。本專利技術的具體實施例中，機器人的上身和下身通過裝有彈簧的圓盤連接，將IMU水平放置在腰部的圓盤上。當人對機器人上身產生推拉或扭轉的作用力時，IMU的實時輸出值就會發生變化，經Kalman濾波后就會反應到橫滾角、俯仰角和扭轉角的變化上，由此來判斷人對機器人作用力的大小和方向。機器人則在主動運動的同時，根據人力的大小判斷人的意圖，向人力做出妥協，避免人受到傷害。具體步驟如下：1、姿態角的計算采用卡爾曼(Kalman)濾波融合算法。IMU模塊包含1個3軸陀螺儀和1個3軸加速度計。加速度計瞬時讀數誤差較大，而陀螺儀數據通過積分得到的姿態角隨時間的增長累積誤差逐漸增大。因此在進行姿態判斷前，需要使用Kalman濾波算法將加速度計和陀螺儀數據進行融合，提高測量精度。(201)系統觀測方程：其中，α為機器人姿態角，β為系統穩態誤差，T為采樣周期，uk-1為k-1時刻陀螺儀角度測量量；(202)系統狀態方程：X(k)＝AX(k-1)+Buk-1(2)其中，X(k)為k時刻預測的結果，X(k-1)為k-1時刻的最優結果；(203)由上一次的誤差協方差P(k-1)和過程噪聲預測新的誤差P(k)：P(k)＝AP(k-1)AT+Q(3)其中，Q是系統過程的協方差矩陣，且Q_acce為加速度計的協方差，Q_gyro為陀螺儀測量協方差；(204)計算卡爾曼增益：Kg(k)＝P(k)HT(HP(k)HT+R)-1(4)其中，H為系統觀測矩陣；(205)進行校正更新，得到狀態K的最優估算值X(k)：X(k)＝X(k-1)+Kg(k)(V(k)-HX(K-1))(5)(206)為下一步估計k+1狀態最優值迭代進行更新操作，并更新k狀態協方差P(k)值：P(k)＝(I-Kg(k)H)P(k-1)(6)其中，I為單位矩陣；按照如上步驟，計算完成時間更新和測量更新后，將上一次的后驗估計作為下一次的先驗估計，往復運算直至結果最優。2、軌跡修正控制過程主動型移動機器人的運動狀態由兩部分合成，第一部分是機器人根據預先學習的知識得到的目標坐標和期望速度矢量，另一部分是人對機器人的實時作用力產生的人為速度矢量。假設預先規劃的機器人期望速度矢量為人對機器人的作用力所對應的人為速度矢量為則機器人運動合速度并且：其中k1是對的影響權值，且0＜k1＜1。假定在某一段軌跡上，機器人原始的運動規律可表示為：其中，fdi(x)、gdi(x)為經三次樣條差值得到的連續軌跡函數，為機器人在期望軌跡上的期望速度。通過姿態角可以計算出人的作用力。但由于外力過大時會造成彈簧的剛性形變，機器人結構會被損壞，同時，由于為避免機器人運動過程中的劇烈晃動，選擇的彈簧剛度較大，一般的人力無法使彈簧發生十分大的形變，所以作用力必須小于一個最大值，姿態角也相應的收斂。在數學函數表達式中，指數函數可以很好的表達這一特性，因此，人作用力大小與姿態角的關系可以用指數函數表示，如下式所示：Fhuman＝Fmax(1-e-bθ),|θ|≤θmax(9)其本文檔來自技高網...

【技術保護點】
一種移動機器人軌跡修正控制算法，其特征在于：包括以下步驟：(101)利用設于機器人上的IMU模塊計算機器人的姿態角；(102)將姿態角的變化轉換為運動速度矢量；(103)計算出合速度；(104)計算出合速度作用下當前及剩余全部目標點修正位置；(105)輸出已修正的運動軌跡。

【技術特征摘要】
1.一種移動機器人軌跡修正控制算法，其特征在于：包括以下步驟：(101)利用設于機器人上的IMU模塊計算機器人的姿態角；(102)將姿態角的變化轉換為運動速度矢量；(103)計算出合速度；(104)計算出合速度作用下當前及剩余全部目標點修正位置；(105)輸出已修正的運動軌跡。2.根據權利要求1所述的一種移動機器人軌跡修正控制算法，其特征在于：所述步驟101中，姿態角的計算采用卡爾曼濾波融合算法，(201)系統觀測方程：其中，α為機器人姿態角，β為系統穩態誤差，T為采樣周期，uk-1為k-1時刻陀螺儀角度測量量；(202)系統狀態方程：X(k)＝AX(k-1)+Buk-1其中，X(k)為k時刻預測的結果，X(k-1)為k-1時刻的最優結果；(203)由上一次的誤差協方差P(k-1)和過程噪聲預測新的誤差P(k)：P(k)＝AP(k-1)AT+Q其中，Q是系統過程的協方差矩陣，且Q_acce為加速度計的協方差，Q_gyro為陀螺儀測量協方差；(204)計算卡爾曼增益：Kg(k)＝P(k)HT(HP(k)HT+R)-1其中，H為系統觀測矩陣；(205)進行校正更新，得到狀態K的最優估算值X(k)：X(k)＝X(k-1)+Kg(k)(V(...

【專利技術屬性】
技術研發人員：劉召，宋立濱，于濤，陳懇，劉莉，陳洪安，
申請(專利權)人：清研華宇智能機器人天津有限責任公司，
類型：發明
國別省市：天津,12