(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211200214.4 (22)申请日 2022.09.29 (71)申请人 上海航天控制技 术研究所 地址 201109 上海市闵行区中春路15 55号 (72)发明人 叶立军　朱文山　尹海宁　何毅杰　 张灿恒　李芳华　范蕾懿　 (74)专利代理 机构 上海元好知识产权代理有限 公司 31323 专利代理师 徐雯琼　张双红 (51)Int.Cl. G06F 17/16(2006.01) G06F 17/18(2006.01) B25J 9/16(2006.01) (54)发明名称 一种采用蒙特卡洛方法确定机械臂关节行 程的方法 (57)摘要 本发明涉及一种采用蒙特卡洛方法确定机 械臂关节行程的方法， 包含以下步骤： 步骤1、 确 定机械臂的位姿约束， 位姿约束包含机械臂末端 位置的活动范围和指向范围； 步骤2、 根据包含机 械臂各相邻关节旋转轴之间夹角的机械臂构型 参数， 构建机械臂逆运动方程； 步骤3、 包含不同 关节角信息的机械臂某个状态作为一个蒙特卡 洛样本点， 根据机械臂的位姿约束以及逆运动方 程， 计算出不同蒙特卡洛样本点中各关节角信息 集合， 该关节角信息集合包含了机械臂中不同关 节的行程包络。 本发明可在机械臂构型已知的情 况下， 以某个指定的概率确定组成机械臂关节单 元行程包络， 以指导机械臂关节形成包络的精细 化设计。 权利要求书3页 说明书9页 附图1页 CN 115470452 A 2022.12.13 CN 115470452 A 1.一种采用蒙特卡洛方法确定 机械臂关节行程的方法， 其特 征在于， 包 含以下步骤： 步骤1、 确定机械臂的位姿约束， 位姿约束包含机械臂末端位置的活动范围和指向范 围； 步骤2、 根据包含机械臂各相邻关节旋转轴之间夹角的机械臂构型参数， 构建机械臂逆 运动方程； 步骤3、 包含不同关节角信息的机械臂的某个状态作为一个蒙特卡洛样本点， 根据机械 臂的位姿约束以及逆运动方程， 计算出不同蒙特卡洛样本点中各关节角 信息集合， 该关节 角信息集 合包含了机械臂中不同关节的行程包络 。 2.如权利要求1所述的确定机械臂关节行程的方法， 其特征在于， 所述步骤1中根据机 械臂末端位置的活动范围和指向范围进行概 率建模。 3.如权利要求2所述的确定 机械臂关节行程的方法， 其特 征在于， 所述 概率建模包含： 机械臂位姿极限状态由卫星滚动姿态以及安装在机 械臂末端的推力器共同确定； 其中， 标称状态下卫星质心到机械臂末端 的距离L0， 卫星偏航范围包括： 卫星X轴旋转 姿态范围为( ‑phi_1， phi_1)， 卫星Z轴旋转姿态范围为( ‑psi_1， psi_1)； 机械臂末端推力器 向±X轴偏斜角度范围为( ‑psi_2， psi_2)； 推力器向 ±Z轴偏斜角度范围为( ‑phi_2， phi_ 2)； 机械末端绕卫星X轴偏离的极限角度为phi ＝phi_1+phi_2； 机械臂末端绕卫星Z轴偏离的极限角度为psi ＝psi_1+psi_2； 机械臂末端位置椭圆包络X 方向半长轴为Apx＝R+L0 ×sin(phi)； 机械臂末端位置椭圆包络Z方向半长轴为Apz＝R+L0 ×sin(psi)； 机械臂末端指向椭圆X 方向半长轴的角度为Av x＝phi+alpha； 机械臂末端位置椭圆Z方向半长轴的角度为Avz＝psi+alpha； 对于第n个样本点： 机械臂末端位置表达式为： X(n)＝Apx ×rand(‑1,1)×cos(rand( ‑1,1)×2×π ) Z(n)＝Apz ×rand(‑1,1)×sin(rand(‑1,1)×2×π ) 机械臂指向表达式为： Vx(n)＝Av x×rand(‑1,1)×cos(rand( ‑1,1)×2×π ) Vz(n)＝Avz ×rand(‑1,1)×sin(rand(‑1,1)×2×π ) rand(‑1,1)表示产生范围从 ‑1到1的均匀分布随机数。 4.如权利要求3所述的确定机械臂关节行程的方法， 其特征在于， 所述概率建模还包 含： 对于机械臂末端离卫星质心的距离的约束， 增加约束为： Y(n)＝ran d(y0,y1)， 其中： y0 为机械臂末端离卫星质心的最近距离； y1为机 械臂末端离卫星质心的最远距离 。 5.如权利要求3所述的确定机械臂关节行程的方法， 其特征在于， 所述概率建模还包 含： 对于机械臂末端推力器沿轴向的约束， 增加约束为： Vy(n)＝ran d(vy0,vy1)， 其中： vy0 为机械臂坐标系y轴转角最小值； vy1为机 械臂坐标系y轴转角最大值。 6.如权利要求1所述的确定机械臂关节行程的方法， 其特征在于， 机械臂关节数为6个， 机械臂第i‑1个关节坐标系到第i个关节坐标系位姿转移 矩阵为：权　利　要　求　书 1/3 页 2 CN 115470452 A 2其中， θi为机械臂第i个关节角， 表征了第i个关节到第i+1个关节的信息， 其取值范围是 机械臂第i个关节行程； αi‑1为机械臂第i ‑1个关节指向到第i个关节指向的转角， 表征了第 i‑1个关节到第i个 关节的信息， 为机械臂构型参数； xi,yi,zi为第i个关节坐标原点在第i ‑1 个关节坐标系的位置 矢量； 特别的， 卫星质心本体坐标系到第1个关节坐标系位姿转移 矩阵为： 其中， C0b为卫星质心本体坐标系到基座坐标系的姿态转移矩阵； xb,yb,zb为基座位置矢 量在卫星质心本体坐标系的位置 矢量， 基座设置在机 械臂与卫星的连接处； 对于6关节机械臂而言， 卫星质心本体坐标系到机械臂末端坐标系的位姿转换矩阵bT6 为：bT6＝bT11T22T33T44T55T6；bT6即表征了 机械臂位姿正 运动； 将bT6定义为： 对于第n个样本， 机 械臂末端位姿状态方程满足： X(n)＝T14； Y(n)＝T24； Z(n)＝T34； Vx(n)＝T12； Vy(n)＝T22； Vz(n)＝T32。 7.如权利要求6所述的确定机械臂关节行程的方法， 其特征在于， 针对每个蒙特卡洛样 本点， 采用牛顿梯度法计算出所有关节角， 每个关节关于样本点的集合形成的包络即为该 关节角的行程包络； 对于第n个样本， 机 械臂末端位姿状态方程形式上为： fx( θ1, θ2, θ3, θ4, θ5, θ6)＝0 fy( θ1, θ2, θ3, θ4, θ5, θ6)＝0 fz( θ1, θ2, θ3, θ4, θ5, θ6)＝0 fvx( θ1, θ2, θ3, θ4, θ5, θ6)＝0权　利　要　求　书 2/3 页 3 CN 115470452 A 3