工業機器人操作空間路徑復合限制求解方法
【技術領域】
[0001] 本發明涉及工業機器人技術領域，特別涉及一種工業機器人操作空間路徑復合限 制求解方法。
【背景技術】
[0002] 工業機器人末端執行器的軌跡規劃在工業機器人控制系統中占重要地位。軌跡規 劃生成伺服驅動器的指令值，從而控制機器人各軸電機產生相應的運動，各軸的運動組成 末端執行器的操作空間的運動。若使得末端執行器產生用戶期望的運動，如跟蹤用戶指定 的曲線，軌跡規劃就必須滿足伺服驅動器和電機的輸入要求，主要體現為:軌跡規劃的輸出 不能使得各軸電機超速;不能使得各軸電機超出容許的最大力矩;不能使得各軸電機超過 伺服驅動器容許的加速度和加加速度。同時生成的操作空間運動也不能超過用戶指定的最 大速度和最大加速度。因此，機器人的軌跡規劃，作為規劃問題而言，其約束條件是復合的。
[0003] 現有的軌跡規劃方法通常將操作空間的運動映射到機器人的關節空間，映射方法 通常是先采樣然后在關節空間應用樣條曲線進行插補，用這樣的方法將電機和伺服驅動器 的約束條件直接體現在關節空間，在根據這些條件進行規劃。
[0004] 然而，現有的這種方法有如下缺點:將操作空間路徑在關節空間用樣條曲線表示， 不可避免會與原來的路徑有所差別，特別是當采樣點較大時，差別尤為明顯。并且，這種方 法不能體現出操作空間的約束條件。
[0005] 專利CN 104742127A公開了一種機器人的控制方法及機器人，有效控制了機械臂 末端的運行軌跡，提高了機械臂末端的控制精度。但是，該技術方案仍是基于關節空間進行 路徑規劃，采用插補的形式約束機械臂末端的運行軌跡，這種方式不能體現出操作空間的 約束條件。
【發明內容】
[0006] 本發明的目的旨在至少解決所述技術缺陷之一。
[0007]為此，本發明的目的在于提出一種工業機器人操作空間路徑復合限制求解方法， 能夠避免樣條曲線插補造成的路徑跟蹤誤差，有效地求解關節空間和操作空間的多種限 制條件，將這些限制條件統一地用變換為路徑參數s的限制，即將復合限制統一的表示出 來。
[0008] 為了實現上述目的，本發明一方面的實施例提供一種工業機器人操作空間路徑復 合限制求解方法，包括如下步驟：
[0009] 步驟S1，設置工業機器人的操作空間路徑，所述操作空間路徑包括:平動部分路徑 /H.0和旋轉部分路徑q(s);
[0010] 步驟S2,根據平動部分路徑h.v)和旋轉部分路徑q(s)，計算所述工業機器人關節 空間的各軸的速度限制條件和加速度限制條件，根據所述各軸的速度限制條件和加速度限 制條件計算參數s的二階導數的限制條件；
[0011]步驟S3,根據所述工業機器人的剛體動力學模型和旋轉部分路徑q(s)，計算所述 工業機器人的各軸力矩限制條件，并根據所述力矩限制條件計算參數S的一階導數的限制 條件；
[0012]步驟S4,根據所述工業機器人的操作空間的六維速度旋量計算操作工件的速度限 制條件和加速度限制條件，根據所述速度限制條件和加速度限制條件計算對參數S的限制 條件；
[0013] 步驟S5,根據步驟S2、S3和S4中計算得到對參數S、參數S的一階導數、參數S的二階 導數的限制條件，將對所述工業機器人的復合限制條件統一制對路徑參數s的限制條件的 計算上。
[0014] 進一步，在所述步驟S1中，
[0015]所述平動部分路徑為三維歐式空間的一條曲線，用參數化幾何曲線的形式來 表示如下：
[0017] 所述旋轉部分路徑q(s)采用參數化曲線的形式，以四元數表示工業機器人的末端 執行器的姿態
[0018] q(s)=slerp(qo，qi，s)，
[0019] 其中，s為路徑參數，g平動部分的起點坐標，瓦為平動部分的終點坐標，qo為旋轉 部分的起點坐標，qi為旋轉部分的終點坐標。
[0020] 進一步，在所述步驟S2中，
[0021 ]設置所述工業機器人的關節空間和操作空間速度有雅克比矩陣映射關系如下：
[0022] y = Jq
[0023] 其中，v為操作空間的六維速度旋量，可以表示為:V = Vd ? sd，sd表示參數s的一階 導數，
[0024] 計算得到各軸的速度限制可表示為： ? ittin ^ max
[0025] < a, (s)scl < ,k = \ ,2...dof
[0026] 設置關節空間和操作空間加速度的映射關系：《=^+^，$ =
[0027] 其中，笛卡爾加速度：a = + L、《/_\Sdd表示路徑參數s的二階導數，計算關 節空間各軸的加速度限制為： 丨_ ?? ??_
[0028] < < ,k = \,2...aof
[0029]當機器人沿某路徑運動時，運動狀態為（s，sd)，此時機器人可用的沿路徑的加速 度sdd，需要滿足下述不等式，求解該不等式，得到對參數S的二階導數的限制條件。 .'min .....max ? - ?? -
[0030] -p, (s)sct < a, (s)sdd < -fi,(s)sct H k H k '
[0031] 其中，/：?々(S)=J 丨⑴ 士⑴-^/ VJCOJ '(.v)
[0032] 進一步，在所述步驟S3中，
[0033] 設置所述工業機器人的剛體動力學模型為：M(y)t/_fC(y,(/) + (，'((；/) = r，
[0034] 將參數s帶入上式，可改寫為參數s的形式，
[0035] (M(s)a(s))sdd+(M(s)(s)+C(s))sd2+G(s) = x
[0036]由于力矩約束條件為 ，則 a(s)sdd+b(S)Sd2 + c(SH〇;
[0037]上述不等式在（sd2，sdd)平面構成了一個可行域，通過一般的線性規劃方法求得 各軸力矩限制對參數s的一階導數的約束條件。
[0038] 進一步，在所述步驟S4中，
[0039] 設置操作空間的六維速度旋量為:v = Vd ? sd，每個速度分量的限制可以表示為v <Vmax，計算上述不等式得到操作空間速度分量的約束條件對參數s的一階導數的限制條 件；
[0040] 計算平動速度大小的約束對參數s的限制條件為：|Vdp| ? scK |v|P max，旋轉速度 大小的約束對參數S的限制條件為：| Vdr I ? SCK I V I r max，求解得到操作空間速度約束對路 徑參數S的限制條件；
[0041 ]設置操作空間的六維加速度旋量的參數化形式如下：a = & . W ,每個加 速度分量的約束條件可以表示為c^amax，求解該不等式以得到操作空間每個方向的加速度 約束對參數s的限制條件。
[0042] 根據本發明實施例的工業機器人操作空間路徑復合限制求解方法，結合關節空間 和操作空間約束條件對路徑參數限制，將機器人軌跡規劃所涉及的各種約束條件統一地體 現到操作空間的路徑上，作為沿路徑速度規劃的輸入，能夠避免樣條曲線插補造成的路徑 跟蹤誤差，有效地求解關節空間和操作空間的多種限制條件，將這些限制條件統一地用變 換為路徑參數s的限制，即將復合限制統一的表示出來，從而為精確的軌跡跟蹤等應用提供 了先決條件。
[0043] 本發明附加的方面和優點將在下面的描述中部分給出，部分將從下面的描述中變 得明顯，或通過本發明的實踐了解到。
【附圖說明】
[0044] 本發明的上述和/或附加的方面和優點從結合下面附圖對實施例的描述中將變得 明顯和容易理解，其中：
[0045] 圖1為根據本發明實施例的工業機器人操作空間路徑復合限制求解方法的流程 圖；
[0046] 圖2為根據本發明實施例的工業機器人操作空間路徑復合限制求解方法的示意 圖；
[0047] 圖3為根據本發明實施例的生成的操作空間的一條圓弧軌跡的對于參數s的一階 導數的復合限制的示意圖；
[0048] 圖4為根據本發明實施例的操作空間的路徑示意圖。
【具體實施方式】
[0049] 下面詳細描述本發明的實施例，實施例的示例在附圖中示出，其中自始至終相同 或類似的標號表示相同或類似的元件或具有相同或類似功能的元件。下面通過參考附圖描 述的實施例是示例性的，旨在用于解釋本發明，而不能理解為對本發明的限制。
[0050] 本發明提出一種工業機器人操作空間路徑復合限制求解方法，該方法使用統一參 數對操作空間路徑進行參數化，并將機器人復合限制條件統一到此參數上。
[0051] 如圖1所示，本發明實施例的工業機器人操作空間路徑復合限制求解方法，包括如 下步驟：
[0052] 步驟S1，設置工業機器人的操作空間路徑，操作空間路徑包括:平動部分路徑》(.〇 和旋轉部分路徑q(s)。
[0053] 具體地，機器人的末端執行器可視為剛體，操作空間路徑可視為剛體的運動路徑。 其中，操作空間路徑包括:平動部分路徑/心)和旋轉部分路徑q(s)，兩部分和在一起組成SE ⑶。
[0054] 在本步驟中，平動部分路徑A.s")為三維歐式空間的一條曲線，用參數化幾何曲線 的形式來表示。操作空間直線路徑的平動部分可以表示為，起點坐標加上沿起點到終點的 方向增量的形式，如下所示：
[0056] 其中，s為從0到1的參數，當s從0變到1時直線從起點變到終點。
[0057] 旋轉部分路徑q(s)采用參數化曲線的形式，以四元數表示工業機器人的末端執行 器的姿態
[0058] q(s) = s 1 erp(qo, qi, s),
[0059] 其中，i平動部分的起點坐標，$為平動部分的終點坐標，qo為旋轉部分的起點坐 標，qi為旋轉部分的終點坐標，s為路徑參數，從0到1的參數，slerp為標準的四元數線性插 補函數，當s從0變到1時末端執行器的姿態從初始姿態變化到終點姿態。所以，可以將工業 機器人操作空間路徑表示為統一的參數化的形式P(s)。這樣，工業機器人的多種限制條件 就可以轉化為對s的約束條件。
[0060] 步驟S2,根據平動部分路徑/;(.v)和旋轉部分路徑q(s)，計算工業機器人關節空間 的各軸的速度限制條件和加速度限制條件，根據各軸的速度限制條件和加速度限制條件計 算參數s的二階導數的限制條件。（算法1)
[0061] 具體地，參考圖2,設置工業機器人的關節空間和操作空間速度有雅克比矩陣映射 關系如下：
[0062] - Jq
[0063] 其中，v為操作空間的六維速度旋量，可以表示為:V = Vd ? sd，即速度方向（包括平 動和旋轉)乘以速度大小s d表示參數s的一階導數，
[0064] § - {J ) vt/ = a(.v) sit
[0065] 計算得到各軸的速度限制可表示為： min .max ? ?
[0066] < ak{s)sd < ，kd...(Jof '^'k h ，.
[0067] 得到2Xdof(自由度)個不等式，通過解這些方程就可以得到sd的取值范圍。
[0068] 設置關節空間和操作空間加速度的映射關系：J +J c/，$二^ i(a _
[0069] 其中，笛卡爾加速度：《 = v，(, _ W + sdd表示路徑參數s的二階導數，即切向 加速度(切向方向乘sdd，sdd表示路徑參數s的二階導數)加上法向加速度(速度方向的導數 乘速度平方）。帶入得到，
[0070] .§ = J .(、\! L + v" _、'd1 - J
[0071] 其中，a(s) =J，J =,/(.、'),?/，
[00