使用JSXGraph进行平面二连杆机构动力学仿真

对于自由运动的机器人来说，其控制器设计可以按是否考虑机器人动力学特性分为两类。一类是完全不考虑动力学特性，只是按照机器人实际轨迹与期望轨迹间的偏差进行负反馈控制。这类方法通常被称为“运动控制（Kinematic Contorl）”，其中的控制器常采用PD或PID控制。运动控制的主要优点是控制规律简单，易于实现。但对于控制高速高精度机器人来说，这类方法有两个明显的缺点：一是难于保证受控机器人具有良好的动态和静态品质；二是需要较大的控制能量。另一类控制器设计方法通常被称为“动态控制（Dynamic Control）”。这类方法是根据机器人动力学模型的性质设计出更精细的非线性控制律，又常被称为“以模型为基础的控制（Model-based Control）”。用动态控制方法设计的控制器可使被控机器人具有良好的动态和静态品质，克服了运动控制方法的缺点。然而由于各种动态控制方案需要实时进行某些机器人动力学计算，而机器人又是一个复杂的多变量强耦合的非线性系统，这就需要较大的在线计算量，给实时控制带来困难。随着计算机运算能力的大大提高，人们越来越重视发展各种以完整的机器人动力学模型为基础的动态控制方法，以充分发挥出机器人的潜力。

下面以平面二连杆机构为例，进行动力学仿真。其中 $\theta_2$ 是连杆2相对于连杆1的夹角， $l_{c1}$ 和 $l_{c2}$ 分别为两个连杆质心到其关节的距离， $I_1$ 和 $I_2$ 为两个连杆相对于其质心的转动惯量。

在纯位置控制下施加在机械臂末端的外力并不会影响末端的运动，因为这种情况可以认为机械臂是完全刚性的。而如果要实现主动柔顺控制，即使机械臂表现出一定的柔性就需要考虑其与环境之间的相互作用。阻抗控制是一种建立在力与速度或位置之间的动态关系上的控制方法。通过建立机器人运动和外部作用力之间的动态关系，对机器人运动进行调节，从而实现机器人与环境的动态调整。机械臂与环境交互产生的力矩可写为： $\tau_{ext}=J^T[M(\ddot{x_d}-\ddot{x})+D(\dot{x_d}-\dot{x})+K(x_d-x)]+G(q)$

上面等式中， $J$ 为机械臂的雅可比矩阵，用于将关节空间速度映射到操作空间： $v=J\dot{\theta}$ ，雅可比矩阵的转置也可将操作空间中的力映射到关节空间中： $\tau=J^TF$ 。 $x_d$ 和 $x$ 分别代表目标位置和实际位置， $\dot{x_d}$ 和 $\dot{x}$ 分别代表目标速度和实际速度。矩阵 $K$ 和 $D$ 代表与环境交互的刚度和阻尼。机械臂末端的加速度 $\ddot{x}$ 一般难以测量，如果直接对速度进行微分又会产生大量噪声。通常对于协作型机械臂，自身重量设计的较轻，因此可以忽略其惯性，即令 $M=0$ 。 $G(q)$ 代表重力补偿项。控制框图如下：

仿真过程中需要预先计算的符号表达式可通过Mathematica计算：

Calculate symbolic expression

er1 = {Cos[th1], Sin[th1], 0}; (*连杆1方向向量*)
er2 = {Cos[th1 + th2], Sin[th1 + th2], 0};(*连杆2方向向量*)
r01 = l1 er1; 
r12 = l2 er2;
r02 = r01 + r12; (*连杆末端位置坐标*)
J = D[r02, {{th1, th2}}]; (*雅可比矩阵*)
z = {x, y, 0};   (*期望位置*)
v = {vx, vy, 0}; (*期望速度*)
Ta = J\[Transpose].(Kp (z - r02) + Kd (v - J.{thdot1, thdot2}) );(* Ta//FullSimplify *)

值得注意的是，惯性项被忽略时的阻抗控制控制规律与PD控制的形式非常相似，但机器人阻抗控制和PD控制是两种不同的控制策略。关键区别在于：力与位置的关系。阻抗控制的核心是通过调节位置误差来间接控制接触力，其本质是力与位置之间的动态关系，而PD控制仅关注位置和速度误差，不涉及力与位置的动态关系。其它区别如下：

① 控制目标和应用场景

阻抗控制：旨在调节机器人与环境之间的动态交互，使机器人表现出特定的阻抗特性（惯性、阻尼和刚度），适用于需要柔顺操作的场景，如装配、打磨等。
PD控制：通过比例和微分反馈来跟踪期望轨迹或位置，目标是减少误差，适用于无交互的精确轨迹跟踪，如定点控制、轨迹跟踪。

② 控制结构

阻抗控制：基于力与位置的关系，通常包含力传感器反馈，动态调整阻抗参数。
PD控制：基于位置和速度误差，结构简单，不涉及力反馈。

③ 参数的物理意义
阻抗控制： $M$ 、 $D$ 、 $K$ 具有明确的物理意义，分别代表期望的惯性、阻尼和刚度特性，用于描述机器人与环境之间的动态交互行为。
PD控制：比例和微分增益是纯粹的数学参数，用于调节系统的响应速度和稳定性。

使用数值积分来对动力学系统进行模拟

正动力学算法主要用于机器人模拟。若要模拟受控机器人在时间区间 $[t_0,t_1]$ 中的运动，可将此区间分为若干以 $\Delta t$ 为间隔的小区间，从 $t=t_0$ 开始，用正动力学算法由已知的 $x(t)$ ， $v(t)$ 和 $\tau(t)$ 计算出 $a(t)$ 后，对加速度进行数值积分获得速度和位置， $\begin{cases}v(t+\Delta t)=v(t)+a(t) \cdot \Delta t \\x(t+\Delta t)=x(t)+ v(t+\Delta t) \cdot \Delta t\end{cases}$ 上面的数值积分使用半隐式欧拉法（Semi-implicit Euler method）也称为改进的欧拉法，它结合了显式和隐式欧拉法的特点，能够在保证一定精度的同时提高数值解的稳定性，尤其适用于那些涉及快速变化但不是极端刚性的系统。半隐式欧拉法广泛应用于计算机图形学、游戏开发以及需要实时模拟物理系统的领域。半隐式欧拉法的核心思想是在更新过程中分别对待不同类型的变量。对于一个具有位置 $x$ 和速度 $v$ 的系统，在时间步长 $\Delta t$ 下，半隐式欧拉法的更新步骤为：

1. 更新速度：使用显式方法，基于当前时刻的位置和速度计算下一时刻的速度。

2. 更新位置：使用隐式方法，基于更新后的速度来计算下一时刻的位置。

这里的关键点是使用新计算的速度 $v(t+\Delta t)$ 来更新位置，而不是旧的速度 $v(t)$ 。尽管在稳定性和准确性方面优于显式欧拉法，但对于高度非线性或复杂的动力学系统，可能仍需采用更高级的数值积分技术以获得更高的精度。例如，上面对于位置的积分可以采用如下公式： $x(t+\Delta t)=x(t)+v(t)\Delta t+\frac{1}{2}a(t)(\Delta t)^2$ $Δ t$

JSXGraph.js测试

 <!DOCTYPE html>
<title>JSXGraph.js测试</title>
<head>
    <link href="https://cdnjs.cloudflare.com/ajax/libs/jsxgraph/1.2.3/jsxgraph.css" rel="stylesheet">
    <script src="https://cdnjs.cloudflare.com/ajax/libs/jsxgraph/1.2.3/jsxgraphcore.js"></script>
</head>

<body>
    <h2>Linkage mechanism simulation</h2>
    <form>
        Select a mode:<br>
        <input type="radio" name="mode" id="PD" onchange="PD_mode()">PD control<br>
        <input type="radio" name="mode" id="kinematics" onchange="kinematics_mode()" checked="true">Kinematics mode<br>
    </form>


    <div id="box" class="jxgbox" style="width:500px; height:500px;"></div> 

    <script type="text/javascript">
        var board = JXG.JSXGraph.initBoard('box', {boundingbox: [-3, 3, 3, -3], axis:true, keepAspectRatio:true,
             showCopyright:true, showNavigation:false, showScreenshot:true, showClearTraces:true});
            
        // Create point elements
        var p1 = board.create('point',[0,0], {name:'A',size:8, fixed:true, face:'^', highlight:false});
        var p2 = board.create('point',[0,1], {name:'B',size:4, fixed:false, trace:false, highlight:false});
        var p3 = board.create('point',[0,2], {name:'E',size:0.1, fixed:false, trace:false, highlight:false});
        var target = board.create('point',[function(){return p3.X();}, function(){return p3.Y();}],
                             {name:'',size:8, face:'cross', highlightStrokeColor:'yellow'}); 

        // Create circle elements
        board.create('circle', [p1, 2], {fillColor: 'green', fillOpacity:0.1, strokeWidth:0, highlight:false});

        // Create segment elements
        var l1 = board.create('segment', [p1, p2, 1], {strokeWidth:6, highlight:false});
        var l2 = board.create('segment', [p2, p3, 1], {strokeWidth:6, highlight:false});
        
        // Restrict the target point to limited area by limited segment length
        board.create('segment', [p1, target, function(){ return Math.min(2, target.Dist(p1));}],{strokeWidth:0, highlight:false});
        
        // Create slider elements
        var s1 = board.create('slider', [[1,2.5],[2.5,2.5],[0, 50, 300]], {name:'Kp', snapWidth:5});
        var s2 = board.create('slider', [[1,2],[2.5,2],[0, 5, 30]], {name:'Kd', snapWidth:1});

        var checkbox = board.create('checkbox', [0.8, -2.8, 'Show trajectory'],{fontSize:14});
        //JXG.addEvent(obj, type, fn, owner)  Adds an event listener to a DOM element.
        JXG.addEvent(checkbox.rendNodeCheckbox, 'change',
            function() {
                if (this.Value()) 
                    p3.setAttribute({trace:true});
                else 
                    p3.setAttribute({trace:false});
            }, checkbox);
        
        // 关节扭矩全局变量
        var T1 = 0.0; 
        var T2 = 0.0; 

        // Create text element
        var info = board.create('text',[-2.5,-2,
                function(){return 'T1:'+T1.toFixed(2) + '<br>'
                + 'T2:'+T2.toFixed(2);}],
                {fontSize:15});


        // 连杆长度, 连杆质量都设定为1，连杆质心到其关节的距离为1/2
        // ...
        // 重力加速度
        const g = 9.8;    // m/s^2
        // 时间步长
        const dt = 0.01;  // s

        // 存储定时器 ID 的变量
        var simulationInterval;

        function PD_mode()
        {
            console.log('Change to PD control mode');
            clearInterval(simulationInterval);
            //Converts a calculated element into a free element
            target.free(); 

            // 计算初始关节角度
            let th1 = Math.atan2(p2.Y() - p1.Y(), p2.X() - p1.X());
            let relativeAngle = Math.atan2(p3.Y() - p2.Y(), p3.X() - p2.X());
            let th2 = relativeAngle - th1;
            // 初始角速度
            let thdot1 = 0;
            let thdot2 = 0;
            
            // 开启一个定时器来进行迭代更新
            simulationInterval = setInterval(() => {
                // 目标点的位置和速度
                let x = target.X();
                let y = target.Y();
                // const vx = 0;
                // const vy = 0;

                // 获取 PD 控制器的参数
                let Kp = s1.Value();
                let Kd = s2.Value();

                // 计算关节力矩
                let Ta = [-2*Math.cos(th2/2)*(Kd*(2*thdot1+thdot2)*Math.cos(th2/2)-Kp*y*Math.cos(th1+th2/2)+Kp*x*Math.sin(th1+th2/2)),
                             -Kd*(thdot1+thdot2)-Kd*thdot1*Math.cos(th2)+Kp*(y*Math.cos(th1+th2)+Math.sin(th2)-x*Math.sin(th1+th2))];
       
                // 计算重力补偿项
                let Ta_g = [0.5*g*(3*Math.cos(th1)+Math.cos(th1+th2)), 0.5*g*Math.cos(th1+th2)];

                // 添加重力补偿项
                T1 = Ta[0] + Ta_g[0];
                T2 = Ta[1] + Ta_g[1];

                // 根据牛顿-欧拉方程计算角加速度
                let H11 = Math.cos(th2) + 37.0 / 24.0;
                let H22 = 13.0 / 48.0;
                let H12 = Math.cos(th2) / 2.0 + 13.0 / 48.0;
                let h = Math.sin(th2) / 2.0;
                let G1 =  0.5 * g * (3*Math.cos(th1) + Math.cos(th1+th2));
                let G2 = 0.5 * g * Math.cos(th1+th2);
                // 求解关于角加速度的联立方程组
                let ddtheta1 = (G2*H12-G1*H22+H22*T1-H12*T2+h*H12*thdot1*thdot1+2*h*H22*thdot1*thdot2+h*H22*thdot2*thdot2)/(H11*H22-H12*H12);
                let ddtheta2 = (G2*H11-G1*H12+H12*T1-H11*T2+h*H11*thdot1*thdot1+2*h*H12*thdot1*thdot2+h*H12*thdot2*thdot2)/(H12*H12-H11*H22);

                // 数值积分：更新角速度
                thdot1 += ddtheta1 * dt;
                thdot2 += ddtheta2 * dt;

                // 数值积分：更新角度
                th1 += thdot1 * dt;
                th2 += thdot2 * dt;
                // th1 += thdot1 * dt + 0.5 * ddtheta1 * dt * dt;
                // th2 += thdot2 * dt + 0.5 * ddtheta2 * dt * dt;

                // 更新点 p2 和 p3 的位置
                p2.setPosition(JXG.COORDS_BY_USER, [Math.cos(th1), Math.sin(th1)]);
                p3.setPosition(JXG.COORDS_BY_USER, [p2.X() + Math.cos(th1 + th2), p2.Y() + Math.sin(th1 + th2)]);

                // 更新图形显示
                board.update();

            }, dt * 1000);
        };

        
        function kinematics_mode()
        {
            console.log('Change to kinematics mode');
            clearInterval(simulationInterval);
            p3.free();
            target.addConstraint([function(){return p3.X();}, function(){return p3.Y();}]); 
            board.update() 
        };

    </script>
</body>
</html>