Page 48 - 《软件学报》2025年第8期

张善强 等: 单球驱动平衡机器人运动学和动力学形式化验证 3471

其中, I b 和 I t 分别表示球轮和机身的转动惯量.
拉格朗日方程如公式 (8) 所示. L 是拉格朗日量, 它是广义坐标, 广义速度和时间的函数, q 是广义坐标, 是时
s
间 t 的函数, ˙ q 是广义速度, 是系统的自由度, Q 是对应的广义力.

( )
d ∂L ∂L
− = Q i (i = 1,2,..., s) (8)
dt ∂˙q i ∂q i
将拉格朗日量代入拉格朗日方程得到单球驱动平衡机器人 XOZ 面对应的拉格朗日动力学方程, 在计算拉格
朗日方程中的导数和重新排列项之后, 运动方程如公式 (9) 所示:

 
0
 
 
M(q)¨q+C(q, ˙q)˙q+g(q) =   (9)
 
τ
[ ] [ ]
α+γ +βcos(x) α+βcos(x) −(1/2)βsin(x)˙x 0
惯性矩阵 M(q) = , 科氏力与离心力矩阵 C(q, ˙q) = , 重力
α+βcos(x) α −βsin(x)˙x 0
[ ]
−µsin(x) [10]
2
2
矩阵 g(q) = , τ 表示广义力. 其中 α = I b +(m b +m t )r , β = m t rl, γ = I t +m t l , µ = m t gl . 对公式 (9) 进行
0
形式化描述如定理 4 所示.
定理 4. XOZ 面的拉格朗日动力学方程.
!q t0 t1 Mt It Ib l r a b c x x' x'' theta theta' theta'' q q' q'' t.
[A1] : drop t0 < drop t1/\interval[t0,t1] = s/\t IN s/\
∧
[A2] : {u : real (2,1) finite_sum|?y.y IN mspace(fspace s)/\u IN IMAGE y s} = s0/\
{u : real (2,2) finite_sum|?y.y IN mspace(fspace s)/\u IN IMAGE y s} = s1/\
∧
∧
[A3] : q = (\t.vector[drop(x t);drop(theta t)] : real 2)/\
q = (\t.vector[drop(x t);drop(theta t)] : real 2)/\
′
′
∧
′
q = (\t.vector[drop(x t);drop(theta t)] : real 2)/\
∧
′′
′′
′′
[A4] : (!t.t IN s ==> (x has_vector_derivative x t)(at t within s))/\
′
(!t.t IN s ==> (theta has_vector_derivative theta t)(at t within s))/\
′
(!t.t IN s ==> (x has_vector_derivative x t)(at t within s))/\
′
′′
′′
(!t.t IN s ==> (theta has_vector_derivative theta t)(at t within s))/\
′
[A5] : ke = (\u : real (2,2) finite_sum.lift(&1/&2∗
∧
(It + Mt ∗l∗l+ Mt ∗r ∗l∗cos(fstcart u$1)+ Ib+(Mb+ Mt)∗r ∗r)∗sndcart u$1∗
sndcart u$1+(Ib+(Mb+ Mt)∗r ∗r + Mt ∗r ∗l∗cos(fstcart u$1))∗sndcart u$1∗
sndcart u$2+&1/&2∗(Ib+(Mb+ Mt)∗r ∗r)∗sndcart u$2∗sndcart u$2))/\
ue = (\u : real 2.lift(Mt ∗g∗l∗cos(u$1)))/\
∧
[A6] : alpha = Ib+(Mb+ Mt)∗r ∗r/\beta = Mt ∗r ∗l/\
beta = Mt ∗r ∗l/\
gamma = It + Mt ∗l∗l/\mu = Mt ∗g∗r/\
M = vector[vector[alpha+gamma+beta∗cos(drop(x t));alpha+beta∗cos(drop(x t))] : real 2;
∧
vector[alpha+beta∗cos(drop(x t));alpha] : real 2] : real 2 2/\
∧ ∧
∧
C = vector[vector[−−(&1/&2)∗beta∗sin(drop(x t))∗drop(x t);&0] : real 2;
∧
′
vector[−−beta∗sin(drop(x t))∗drop(x t);&0] : real 2] : real 2 2/\
∧ ∧
∧
′
G = vector[(−−mu)∗sin(drop(x t));&0] : real 2/\
∧
[A7] : f = (\t : real 1.(vector[lift(&0);lift(tau)] : real 1 2))/\
∧
∧ ∧
rr = (\u : real (2,1)finite_sum.(vector[lift(fstcart u$1);lift(fstcart u$2)] : real 1 2))/\
∧ ∧
∧
[A8] : lagrange_equations_autonomous s s0 s1 ke ue q f rr t
′
[G1] : ==> M ∗∗(q t)+C ∗∗(q t)+G = vector[&0;tau]
′′