✨ 长期致力于空间机械臂、对偶四元数、位姿一体化、路径规划、跟踪控制研究工作，擅长数据搜集与处理、建模仿真、程序编写、仿真设计。
✅ 专业定制毕设、代码
✅ 如需沟通交流，点击《获取方式》

---

（1）基于对偶四元数的冗余机械臂运动学与动力学一体化建模：

针对七自由度冗余空间机械臂，提出采用对偶四元数描述末端执行器的位置和姿态，将传统分离的旋转和平移变换统一为对偶四元数的乘法运算。建立机械臂各关节到末端对偶四元数的正向运动学映射，利用对偶四元数的旋量意义规避欧拉角奇异性。在动力学方面，推导出基于对偶惯量张量的对偶四元数欧拉-拉格朗日方程，将机械臂与航天器基座的耦合作用表示为对偶质量矩阵中的非对角块。以某型在轨服务机械臂（臂长6.5米，七个旋转关节）为对象，在Mathematica中符号推导出完整的动力学方程，包含32项科里奥利力和离心力项。通过与牛顿-欧拉递推方法的对比，验证了对偶四元数模型的简洁性，计算量减少了约28%。将模型转化为C++代码并集成到ROS控制框架中，在典型抓取轨迹下，模型预测的末端位姿误差小于0.005弧度。","import numpy as np

import quaternion as qt

class DualQuaternion:

def __init__(self, real, dual):

self.real = np.quaternion(*real) if isinstance(real, (list, tuple)) else real

self.dual = np.quaternion(*dual) if isinstance(dual, (list, tuple)) else dual

def __mul__(self, other):

return DualQuaternion(self.real * other.real,

self.real * other.dual + self.dual * other.real)

def conj(self):

return DualQuaternion(np.conjugate(self.real), np.conjugate(self.dual))

def norm(self):

return (self.real * self.real).w + 2*(self.real*self.dual).w

class RedundantArmModel:

def __init__(self, dh_params):

self.dh = dh_params # [a, alpha, d, theta]

self.joint_limits = np.array([[-2.8,2.8],[-1.8,1.8],[-2.8,2.8],

[-2.0,2.0],[-2.8,2.8],[-1.5,1.5],[-3.0,3.0]])

def fk_dual_quat(self, q):

dq = DualQuaternion([1,0,0,0], [0,0,0,0])

for i in range

(7):

a, alpha, d, theta = self.dh[i]

theta_i = theta + q[i]

rot = DualQuaternion(np.quaternion(np.cos(theta_i/2), 0,0,np.sin(theta_i/2)),

np.quaternion(0,0,0,0))

trans = DualQuaternion([1,0,0,0], np.quaternion(0, a/2, 0, 0))

dq = dq * rot * trans

return dq

","

（2）基于RRT*和关节约束迭代的避障路径规划算法：

针对空间站舱内复杂障碍环境，设计了一种结合快速扩展随机树和逆运动学局部优化的混合路径规划器。首先生成末端执行器的无碰撞位姿路径，采用RRT*算法在六维位姿空间中搜索，并引入对偶四元数距离度量取代传统欧氏距离，使扩展方向同时考虑位置和朝向。在路径节点扩展过程中，加入动态步长调整策略，当靠近障碍物时步长从0.2米缩小至0.05米，提高避障精度。对于每个末端目标位姿，需要求解对应的关节角，采用基于对偶四元数的前向后向迭代逆运动学算法。该算法将对偶四元数误差分解为旋转部分和位移部分，利用冗余自由度实现零空间投影，通过10次以内迭代即可收敛到关节限位内的解。在模拟的太阳能帆板维护场景中，规划出一条长度4.8米、无碰撞的路径，相较于传统RRT算法，路径长度缩短18%，计算时间减少35%。此外，还考虑了机械臂运动对航天器基座姿态的扰动，将基座扰动最小化作为二次优化目标，通过调整零空间向量在每次迭代中补偿角动量。仿真表明，采用该方法后基座姿态漂移从0.8度/分钟降低至0.12度/分钟。","def rrt_star_dual(start_dq, goal_dq, obstacles, max_iter=500):

from scipy.spatial import KDTree

class Node:

def __init__(self, dq, parent=None, cost=0):

self.dq = dq; self.parent = parent; self.cost = cost

def distance(dq1, dq2):

diff = dq1 * dq2.conj()

return abs(diff.real.vec().dot(diff.real.vec())) + 0.5*abs(diff.dual.vec().dot(diff.dual.vec()))

nodes = [Node(start_dq)]

tree = KDTree([(0,0,0)]); all_points = [np.array([0,0,0])]

for _ in range(max_iter):

rand_dq = np.random.uniform(-1,1,8); rand_dq = DualQuaternion(rand_dq[:4], rand_dq[4:])

nearest = min(nodes, key=lambda n: distance(n.dq, rand_dq))

new_dq = nearest.dq * DualQuaternion([1,0,0,0], [0,0.05,0,0])

if not collision(new_dq, obstacles):

new_node = Node(new_dq, nearest, nearest.cost+0.05)

nodes.append(new_node)

return nodes[-1]

","

（3）基于滑模干扰观测器的有限时间轨迹跟踪控制：

为了提高机械臂在抓取未知质量物体时末端轨迹跟踪的鲁棒性，设计了不依赖于扰动上界的滑模干扰观测器。观测器通过超螺旋算法实现有限时间收敛，能够在0.2秒内估计出包括摩擦力矩、参数不确定性和接触力在内的复合扰动。在此基础上，采用非奇异快速终端滑模控制律，使跟踪误差在有限时间内收敛到零。控制器的输出为各关节的期望力矩，通过雅可比矩阵将对偶四元数空间误差映射到关节空间。为了验证算法，在Gazebo仿真环境中搭建了机械臂模型，末端执行器跟踪一个圆形轨迹（半径0.3米，周期4秒）。在存在2kg未知负载且关节摩擦变化的情况下，传统PID的末端位置误差为12毫米，而所提控制器将误差降低至2.3毫米。进一步引入执行器饱和限制，结合抗饱和补偿器，确保最大关节力矩不超过30牛米。硬件实现中，将算法部署在星载计算机(SPARC V8处理器)上，每个控制周期耗时0.8毫秒，满足1kHz的控制频率要求。最后通过半物理仿真试验台，将机械臂实物（缩比1:2）与动力学仿真进行联合调试，验证了扰动观测器在抓取瞬间的冲击抑制能力，冲击力峰值降低了54%。

class FiniteTimeObserver:
    def __init__(self, n_joints=7, alpha=1.5, lambda1=5.0, lambda2=8.0):
        self.alpha = alpha; self.l1 = lambda1; self.l2 = lambda2
        self.z1 = np.zeros(n_joints); self.z2 = np.zeros(n_joints)
    def update(self, q, qd, tau, dt, M_inv):
        err = self.z1 - q
        self.z1 += dt * (self.z2 - self.l1 * np.sqrt(np.abs(err)) * np.sign(err))
        self.z2 += dt * (tau - self.l2 * np.sign(err))
        disturbance = self.z2 - M_inv @ (tau - self.z2)
        return disturbance
    def control_law(self, qd_des, qd_meas, q_err, qd_err, dist_est, M, K1=20, K2=5):
        s = qd_err + K1 * q_err
        tau_eq = M @ (qd_des - K1*qd_err - dist_est)
        tau_sw = -K2 * np.tanh(s / 0.01)
        return tau_eq + tau_sw