Page 46 - 《软件学报》2025年第8期
P. 46
张善强 等: 单球驱动平衡机器人运动学和动力学形式化验证 3469
[30] v st = v sb + p st ×R sb ω bt ; 公式 (5) 的结果为公式 (6) 的进一步简化计
v bt = 0, 因为 v st = v sb +R sb v bt + p st ×R sb ω bt , 故有
[30]
算提供了便利. ②到③中 v sb 的化简和①类似, 再结合 ω st = ω sb +R sb ω bt 得到③; 后续的化简过程相对简单, 这里不
再详细说明.
公式 (6) 的计算涉及等式代换、向量叉乘和向量减法等性质, 对上式进行形式化描述如定理 3 所示.
定理 3. 末端速度.
!Rsb Rbt Rst Rsb' Rbt' psb pbt gsb gbt gst wsb vsb wbt vbt wst vst t.
[A1] : (!t.rotation_matrix(Rsb t))/\(!t.rotation_matrix(Rbt t))/\
[A2] : (Rsb has_v1matrix_derivative(Rsb t))(at t)/\(Rbt has_v1matrix_derivative(Rbt t))(at t)/\
′
′
[A3] : (psb has_vector_derivative(psb t))(at t)/\
′
[A4] : Rst = (\t.(Rsb t)∗∗(Rbt t))/\gst = (\t.gsb(t)∗∗gbt(t))/\
[A5] : pst = (\t.psb t +Rsb t ∗∗l%%Rbt t ∗∗basis 3)/\pbt = (\t.l%%(Rbt t)∗∗(basis 3))/\
[A6] : gsb = (\t.homo_trans(psb t)(Rsb t))/\
gbt = (\t.homo_trans(pbt t)(Rbt t))/\
gst = (\t.homo_trans(pst t)(Rst t))/\
[A7] : vsb = homo_matrix_snd(spatial_velocity gsb UNIV t)/\
wsb = matrix3_vee(homo_matrix_fst(spatial_velocity gsb UNIV t))/\
vbt = homo_matrix_snd(spatial_velocity gbt UNIV t)/\
wbt = matrix3_vee(homo_matrix_fst(spatial_velocity gbt UNIV t))/\
vst = homo_matrix_snd(spatial_velocity gst UNIV t)/\
wst = matrix3_vee(homo_matrix_fst(spatial_velocity gst UNIV t))/\
[G1] : ==> vector_derivative pst (at t) =
vector_derivative psb (at t)+l%wst cross (Rst t ∗∗(basis 3)),
其中, homo_matrix_ f st 表示取齐次矩阵的第 1 个元素, 得到对应的旋转矩阵; homo_matrix_snd 表示取齐次矩阵的
第 2 个元素, 得到对应的位置向量; vector_derivative 表示向量函数的导函数.
条件 A1 是 R sb 和 R bt 是旋转矩阵的形式化表示; 条件 A2 是旋转矩阵 R sb 的导函数为 ˙ R sb 和旋转矩阵 R bt 的
˙ R bt 的形式化表示; 条件 ˙ p sb 的形式化表示; 条件 R st = R sb R bt 和
导函数为 A3 是位置向量 p sb 的导函数为 A4 是
[ ]
g st = g sb g bt 的形式化表示; 条件 A5 是 p st = p sb +R sb lR bt e 3 和 p bt = lR bt e 3 的形式化表示; 条件 A6 是 g sb = R sb p sb 、
0 1
[ ] [ ]
R bt p bt R st p st
g bt = 和 g st = 的形式化表示; 条件 A7 表示坐标系 B 相对于惯性系 S、坐标系 T 相对于坐
0 1 0 1
标系 B 以及坐标系 T 相对于惯性系 S 的线速度和角速度; 目标 G1 是 ˙ p st = ˙p sb +lω st ×R st e 3 的形式化表示.
根据上述数学推导过程, 可以得到其形式化的推导. 定理 3 的证明首先建立末端速度这一子目标 ( ˙ p st =
[ ]
˙ R st R −1 −1
st
−1 st − ˙ R st R p st + ˙p st ˙ R st = ˆω st R st 这一等式执行
v st +ω st × p st ), 之后通过空间速度的定义 ˆ V st = ˙g st g st = 结合
0 0
SIMP_TAC 策略应用叉乘和点乘的运算性质证明这一子目标. 接下来, 使用伴随变换结合坐标系 T 相对于坐标系
B 的速度和坐标系 B 相对于惯性系 S 的速度等于坐标系 T 相对于惯性系 S 的线速度和角速度 ( V st = V sb + Ad g sb V bt =
v st v sb +R sb v bt + p sb ×R sb ω bt
˙ p st = v st +ω st × p st 这一子目标的验
= ) 这一性质, 对 v st 和 ω st 进行变量代换, 之后类比
ω st ω sb +R sb ω bt
证对 v sb 进行变量代换, 应用定理 2, 对当前等式进行化简, 最后使用 REWRITE_TAC 这一策略重写加法分配律、
向量加法结合律和负矩阵和向量乘法等定理完成该定理的证明.
通过验证机器人的位姿, 能够确定其在空间中的位置和姿态. 进一步地, 通过分析从位姿得出的速度信息, 不
仅能够深化对机器人当前运动状态的理解和分析, 还能确保动能计算的正确性, 为之后的动力学分析与验证打下
