CN111977006B

CN111977006B - Initialization method and device for joint angle and aircraft

Info

Publication number: CN111977006B
Application number: CN202010801302.4A
Authority: CN
Inventors: 汪康利
Original assignee: Shenzhen Autel Intelligent Aviation Technology Co Ltd
Current assignee: Shenzhen Autel Intelligent Aviation Technology Co Ltd
Priority date: 2020-08-11
Filing date: 2020-08-11
Publication date: 2024-06-14
Anticipated expiration: 2040-08-11
Also published as: CN111977006A

Abstract

The invention relates to the technical field of aircrafts, and discloses a method and a device for initializing a joint angle and an aircraft, wherein the method comprises the following steps: acquiring a first joint angle corresponding to the first motor power-on moment, a second electrical angle corresponding to the second motor power-on moment and a third electrical angle corresponding to the third motor power-on moment, further determining a joint angle combination, determining a posture quaternion corresponding to the joint angle combination and a posture quaternion of the base end, and determining a posture quaternion of the tool end according to the posture quaternion of the base end and the posture quaternion of the joint angle combination; and according to the gesture quaternion of the tool end, combining the readings of the triaxial accelerometer of the tool end to determine the initialized joint angle combination of the triaxial motor. By carrying out self-checking on the first motor and determining the initialized joint angle combination of the triaxial motor, the self-checking efficiency of the triaxial holder after being electrified can be improved.

Description

Initialization method and device for joint angle and aircraft

Technical Field

The present invention relates to the technical field of aircrafts, and in particular, to a method and an apparatus for initializing a joint angle, and an aircraft.

Background

Aircraft, such as Unmanned aircraft (un-managed AERIAL VEHICLE, UAV), also known as Unmanned aircraft, have been increasingly used for their advantages of small size, light weight, flexibility, rapid response, unmanned, low operational requirements, etc. The unmanned aerial vehicle is generally provided with a tripod head, for example, a triaxial tripod head, and the triaxial tripod head comprises a triaxial motor, and a linear hall sensor is generally arranged on the triaxial motor and used for detecting a magnetic field of a motor rotor so as to calculate an electrical angle of the motor, thereby determining a joint angle and further realizing control of the motor.

At present, in order to acquire the joint angle of the motor, the motor can be subjected to self-checking after power-on, so that the normal work of the cradle head is realized, but the mode needs to be subjected to self-checking by the triaxial motor after power-on, so that the working efficiency is influenced.

Disclosure of Invention

The embodiment of the invention provides a joint angle initializing method and device and an aircraft, solves the problem of low efficiency caused by self-checking of three-axis motors of the existing aircraft, and improves the self-checking efficiency of a three-axis cradle head after power-on.

In order to solve the technical problems, the embodiment of the invention provides the following technical scheme:

In a first aspect, an embodiment of the present invention provides a method for initializing a joint angle, which is applied to an aircraft, where the aircraft is provided with a three-axis cradle head, the three-axis cradle head includes three-axis motors, which are a first motor, a second motor, and a third motor, the second motor corresponds to a second pole pair number, the third motor corresponds to a third pole pair number, the three-axis cradle head further includes a base end and a tool end, and the tool end is provided with a three-axis accelerometer, the method includes:

acquiring a first joint angle corresponding to the first motor power-on time, a second electrical angle corresponding to the second motor power-on time and a third electrical angle corresponding to a third motor power-on time;

Traversing the second joint angle and the third joint angle of the second motor and the third motor according to the second electrical angle and the second pole logarithm of the second motor and the third electrical angle and the third pole logarithm of the third motor, and determining joint angle combinations of the first motor, the second motor and the third motor;

Determining a gesture quaternion corresponding to the joint angle combination and a gesture quaternion of the base end, and determining the gesture quaternion of the tool end according to the gesture quaternion of the base end and the gesture quaternion of the joint angle combination;

and according to the gesture quaternion of the tool end, combining the readings of the triaxial accelerometer of the tool end to determine the initialized joint angle combination of the triaxial motor.

In some embodiments, the base end is provided with a tri-axial accelerometer, and the determining the attitude quaternion of the base end includes:

acquiring readings of a triaxial accelerometer at the base end, fixing a yaw angle of the base end, and calculating a pitch angle and a roll angle of the base end according to the readings of the triaxial accelerometer at the base end;

And determining the attitude quaternion of the base end according to the yaw angle, the pitch angle and the roll angle of the base end.

In some embodiments, after the determining the joint angle combination of the first motor, the second motor, and the third motor, the method further comprises: screening joint angle combinations of the first motor, the second motor and the third motor, determining the screened joint angle combinations, and determining posture quaternions corresponding to the joint angle combinations, wherein the method comprises the following steps:

And determining the gesture quaternion corresponding to each screened joint angle combination.

In some embodiments, the obtaining the first joint angle corresponding to the first motor at the power-on time includes:

And determining a first joint angle corresponding to the first motor in a self-checking limit mode.

In some embodiments, the method further comprises:

calculating an accelerometer module value of the base end and an accelerometer module value of the tool end;

and determining whether the cradle head is in a static state according to the accelerometer module value of the base end and the accelerometer module value of the tool end.

In some embodiments, the determining the gesture quaternion of the tool end according to the gesture quaternion of the base end and the gesture quaternion of the joint angle combination includes:

And carrying out quaternion multiplication on the attitude quaternion of the base end and the attitude quaternion of the joint angle combination, and determining the attitude quaternion of the tool end.

In some embodiments, the determining the initialized joint angle combination of the tri-axis motor according to the posture quaternion of the tool end and in combination with the readings of the tri-axis accelerometer of the tool end comprises:

determining a rotation matrix according to the gesture quaternion of the tool end;

Generating a gravity vector according to the readings of the triaxial accelerometer at the tool end;

and determining an initialization joint angle combination of the triaxial motor according to the rotation matrix and the gravity vector.

In some embodiments, the determining an initialized joint angle combination for the three-axis motor from the rotation matrix and the gravity vector comprises:

acquiring a column vector of the rotation matrix;

Performing dot multiplication operation on the column vector of the rotation matrix and the gravity vector to generate a plurality of dot multiplication results;

and taking the first joint angle, the second joint angle and the third joint angle corresponding to the maximum value of the point multiplication result as an initialization joint angle combination of the triaxial motor.

In a second aspect, an embodiment of the present invention provides an initialization apparatus for a joint angle, which is applied to an aircraft, where the aircraft is provided with a triaxial holder, the triaxial holder includes a triaxial motor, which is a first motor, a second motor, and a third motor, the second motor corresponds to a second pole pair number, the third motor corresponds to a third pole pair number, the triaxial holder further includes a base end and a tool end, and the tool end is provided with a triaxial accelerometer, and the apparatus includes:

the acquisition unit is used for acquiring a first joint angle corresponding to the first motor power-on time, a second electrical angle corresponding to the second motor power-on time and a third electrical angle corresponding to the third motor power-on time;

The joint angle combination unit is used for traversing the second joint angles and the third joint angles of the second motor and the third motor according to the second electrical angle and the second pole pair number of the second motor and the third electrical angle and the third pole pair number of the third motor, and determining joint angle combinations of the first motor, the second motor and the third motor;

the gesture quaternion unit is used for determining gesture quaternions corresponding to the joint angle combination and the gesture quaternion of the base end, and determining the gesture quaternion of the tool end according to the gesture quaternion of the base end and the gesture quaternion of the joint angle combination;

And the initialization joint angle combination unit is used for determining the initialization joint angle combination of the triaxial motor according to the posture quaternion of the tool end and the reading of the triaxial accelerometer of the tool end.

In some embodiments, the gesture quaternion unit is specifically configured to:

In some embodiments, the apparatus further comprises:

And the screening unit is used for screening joint angle combinations of the first motor, the second motor and the third motor, determining the screened joint angle combinations and determining gesture quaternions corresponding to the joint angle combinations.

In some embodiments, the acquiring unit is specifically configured to:

In some embodiments, the apparatus further comprises:

The static state unit is used for calculating the accelerometer module value of the base end and the accelerometer module value of the tool end;

In some embodiments, the gesture quaternion unit is specifically configured to:

In some embodiments, the initialization joint angle combining unit is specifically configured to:

acquiring a column vector of the rotation matrix;

In a third aspect, an embodiment of the present invention provides an aircraft, comprising:

A body;

The arm is connected with the machine body;

the power device is arranged on the machine body and/or the horn and is used for providing flying power for the aircraft;

The three-axis tripod head is arranged on the machine body and comprises a three-axis motor, namely a first motor, a second motor and a third motor, and further comprises a base end and a tool end;

the flight controller is arranged on the machine body;

wherein the flight controller comprises:

At least one processor; and

A memory communicatively coupled to the at least one processor; wherein,

The memory stores instructions executable by the at least one processor to enable the at least one processor to perform the method of initializing the joint angle as described above.

In a fourth aspect, embodiments of the present invention also provide a non-transitory computer-readable storage medium storing computer-executable instructions for enabling an aircraft to perform a method of initializing a joint angle as described above.

The invention provides an initialization method of a joint angle, which is applied to an aircraft, wherein the aircraft is provided with a triaxial holder, the triaxial holder comprises a triaxial motor, the triaxial motor is respectively a first motor, a second motor and a third motor, the second motor corresponds to a second pole pair number, the third motor corresponds to a third pole pair number, the triaxial holder further comprises a base end and a tool end, and the tool end is provided with a triaxial accelerometer, and the method comprises the following steps: acquiring a first joint angle corresponding to the first motor power-on time, a second electrical angle corresponding to the second motor power-on time and a third electrical angle corresponding to a third motor power-on time; traversing the second joint angle and the third joint angle of the second motor and the third motor according to the second electrical angle and the second pole logarithm of the second motor and the third electrical angle and the third pole logarithm of the third motor, and determining joint angle combinations of the first motor, the second motor and the third motor; determining a gesture quaternion corresponding to the joint angle combination and a gesture quaternion of the base end, and determining the gesture quaternion of the tool end according to the gesture quaternion of the base end and the gesture quaternion of the joint angle combination; and according to the gesture quaternion of the tool end, combining the readings of the triaxial accelerometer of the tool end to determine the initialized joint angle combination of the triaxial motor. By carrying out self-checking on the first motor and determining the initialized joint angle combination of the triaxial motor, the self-checking efficiency of the triaxial holder after being electrified can be improved.

Drawings

One or more embodiments are illustrated by way of example and not limitation in the figures of the accompanying drawings, in which like references indicate similar elements, and in which the figures of the drawings are not to be taken in a limiting sense, unless otherwise indicated.

FIG. 1 is a specific block diagram of an aircraft provided by an embodiment of the present invention;

fig. 2 is a schematic diagram of a triaxial holder according to an embodiment of the present invention;

fig. 3 is a flow chart of an initialization method of a joint angle according to an embodiment of the present invention;

fig. 4 is a detailed flowchart of step S30 in fig. 3;

Fig. 5 is a detailed flowchart of step S40 in fig. 3;

fig. 6 is a detailed flowchart of step S43 in fig. 5;

fig. 7 is a schematic structural diagram of an initializing device for joint angles according to an embodiment of the present invention;

FIG. 8 is a schematic diagram of a hardware architecture of an aircraft according to an embodiment of the present invention;

FIG. 9 is a connection block diagram of an aircraft provided by an embodiment of the invention;

Fig. 10 is a schematic diagram of the powertrain of fig. 9.

Detailed Description

For the purpose of making the objects, technical solutions and advantages of the embodiments of the present invention more apparent, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention, and it is apparent that the described embodiments are some embodiments of the present invention, but not all embodiments of the present invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.

In addition, the technical features of the embodiments of the present invention described below may be combined with each other as long as they do not collide with each other.

The method for initializing the joint angle provided by the embodiment of the invention can be applied to various movable objects driven by a motor or a motor, including but not limited to aircrafts, robots and the like. Wherein the aircraft may include unmanned aircraft (unmanned AERIAL VEHICLE, UAV), unmanned spacecraft, and the like.

The method for initializing the joint angle is applied to a flight controller of an aircraft.

Referring to fig. 1, fig. 1 is a specific structural diagram of an aircraft according to an embodiment of the present invention;

as shown in fig. 1, the aircraft 10 includes: the device comprises a body 11, a horn 12 connected with the body 11, a power device 13 arranged on the horn 12, a cradle head 14 connected to the bottom of the body 11, a camera 15 arranged on the cradle head 14 and a flight controller (not shown) arranged in the body 11.

Wherein the flight controller is connected to a power unit 13, the power unit 13 being mounted on the fuselage 11 for providing flight power to the aircraft 10. Specifically, the flight controller is configured to execute the above-mentioned method for initializing the joint angle to generate a control command, and send the control command to the electric modulator of the power device 13, where the electric modulator controls the driving motor of the power device 13 through the control command. Or the flight controller is used to perform an initialization method of the joint angle in order to generate a control command and control the drive motor of the power plant 13 by means of the control command.

The body 11 includes: the device comprises a center shell and one or more arms connected with the center shell, wherein the one or more arms radially extend out of the center shell. The connection of the horn and the center housing may be an integral connection or a fixed connection. The power device is arranged on the horn.

The flight controller is used for executing the initialization method of the joint angle to determine the joint angle of the triaxial motor, conveniently generating a control command and sending the control command to the electric regulator of the power device so that the electric regulator controls the driving motor of the power device through the control command. The controller is a device with certain logic processing capability, such as a control chip, a singlechip, a micro control unit (Microcontroller Unit, MCU) and the like.

The power unit 13 includes: electrically adjusting, driving a motor and a propeller. The electrical regulator is located within a cavity formed by the horn or center housing. The electric regulator is respectively connected with the controller and the driving motor. Specifically, the electric regulator is electrically connected with the driving motor and is used for controlling the driving motor. The driving motor is arranged on the horn, and the rotating shaft of the driving motor is connected with the propeller. The propeller generates a force to move the aircraft 10, for example, a lift force or a thrust force to move the aircraft 10, under the drive of the drive motor.

The completion of each prescribed speed, motion (or attitude) of the aircraft 10 is accomplished by electrically controlling the drive motors. An electric governor, which is known as an electronic governor, adjusts the rotational speed of the drive motor of the aircraft 10 in response to a control signal. The controller is an execution main body for executing the initialization method of the joint angle, and generates a control instruction to control the driving motor. The principle of electrically controlling the driving motor is approximately as follows: the drive motor is an open loop control element that converts an electrical pulse signal into an angular displacement or a linear displacement. In the case of non-overload, the rotation speed and stop position of the driving motor are only dependent on the frequency and pulse number of the pulse signal, and are not affected by load change, when the driver receives a pulse signal, it drives the driving motor of the power device to rotate a fixed angle in a set direction, and the rotation of the driving motor is operated at a fixed angle. Therefore, the electric adjustment can control the angular displacement by controlling the number of pulses, thereby achieving the purpose of accurate positioning; meanwhile, the speed and the acceleration of the rotation of the driving motor can be controlled by controlling the pulse frequency, so that the aim of speed regulation is fulfilled.

The main functions of the present aircraft 10 are aerial photography, real-time image transmission, high-risk area detection and the like. In order to realize functions such as aerial photography, real-time image transmission, high-risk area detection and the like, an image pickup assembly is connected to the aircraft 10. Specifically, the aircraft 10 and the camera assembly are coupled by a coupling structure, such as a vibration-damping ball or the like. The camera assembly is used for acquiring a shooting picture in the process of aerial shooting of the aircraft 10.

Specifically, the subassembly of making a video recording includes: cradle head and shooting device. The cradle head is connected to the aircraft 10. Wherein, take the device and carry on the cloud platform, take the device can be image acquisition device for gather the image, this take the device including but not limited to: cameras, video cameras, scanners, camera phones, etc. The cradle head is used for carrying the photographing device to fix the photographing device or randomly adjust the posture of the photographing device (for example, change the height, the inclination angle and/or the direction of the photographing device) and stably maintain the photographing device at the set posture. For example, when the aircraft 10 performs aerial photography, the cradle head is mainly used for stably keeping the photographing device in a set posture, preventing the photographing device from shaking a photographing picture, and ensuring the stability of the photographing picture.

The cradle head 14 is connected with a flight controller to realize data interaction between the cradle head 14 and the flight controller. For example, the flight controller sends a yaw command to the head 14, the head 14 obtains and executes the speed and direction commands of yaw, and data information generated after executing the yaw command is sent to the flight controller so that the flight controller detects the current yaw condition.

The cloud platform includes: cradle head motor and cradle head base. Wherein, the tripod head motor is installed in the tripod head base. The flight controller can also control the pan-tilt motor through the electric tuning of the power device 13, and specifically, the flight controller is connected with the electric tuning, the electric tuning is electrically connected with the pan-tilt motor, the flight controller generates a pan-tilt motor control instruction, and the electric tuning controls the pan-tilt motor through the pan-tilt motor control instruction.

The cradle head base is connected with the body of the aircraft and used for fixedly mounting the camera shooting assembly on the body of the aircraft.

The cradle head motor is respectively connected with the cradle head base and the shooting device. The cradle head can be a multi-axis cradle head and is adaptive to the multi-axis cradle head, namely, each axis is provided with a plurality of cradle head motors. The cradle head motor can drive the shooting device to rotate on one hand, so that the adjustment of the horizontal rotation and pitching angle of the shooting rotating shaft is met, and the motor is automatically rotated by manually remotely controlling the cradle head motor to rotate or utilizing a program, so that the effect of omnibearing scanning monitoring is achieved; on the other hand, in the process of aerial photography of the aircraft, the disturbance suffered by the shooting device is counteracted in real time through the rotation of the cradle head motor, so that the shaking of the shooting device is prevented, and the stability of a shot picture is ensured.

In the invention, the cradle head is a three-axis cradle head, the cradle head motor is a three-axis motor, and the three-axis motor respectively comprises a first motor, a second motor and a third motor.

Specifically, the triaxial holder can be a stability augmentation device and a support device for mounting and fixing a load such as a camera or a sensor. In the embodiment of the invention, the three-axis cradle head comprises a yaw axis arm, a roll axis arm and a pitch axis arm, each axis arm corresponds to a motor, and is respectively a yaw axis motor (a first motor) for controlling the rotation of the yaw axis arm, a roll axis motor (a second motor) for controlling the rotation of the roll axis arm and a pitch axis motor (a third motor) for controlling the rotation of the pitch axis arm, and the yaw axis motor, the roll axis motor and the pitch axis motor correspondingly control the rotation of the yaw axis arm, the roll axis arm and the pitch axis arm, so that the three-axis cradle head is controlled.

The three-axis cradle head comprises a tool end, wherein the tool end can be a shooting device, the shooting device is carried on the cradle head, an inertial measurement unit (Inertial measurement unit, IMU) is arranged on the shooting device, and the inertial measurement unit is a device for measuring three-axis attitude angles (or angular rates) and acceleration of an object. In general, a three-axis gyroscope and three-direction accelerometers are installed in one IMU, that is, angular velocity and acceleration of an object in a three-dimensional space are measured through the three-axis gyroscope and the three-axis accelerometer, and the gesture of the object is calculated according to the angular velocity and the acceleration. To improve reliability, more sensors may also be provided for each shaft. Generally the IMU is to be mounted on the center of gravity of the aircraft.

In the working process of the triaxial tripod head, the gyroscope signals at the tail end are required to be converted into the joint angular velocities of triaxial through the Jacobian inverse matrix, and the calculation of the Jacobian inverse matrix requires the use of triaxial joint angles.

At present, most of small-sized holders are provided with linear Hall sensors on a three-axis brushless motor for detecting the magnetic field of a motor rotor, and further calculating the electrical angle of the motor, so that the motor is controlled.

The electrical angle and the joint angle of the motor are not in one-to-one correspondence and are related to the pole pair number of the motor. For an N-pair motor, the bread contains N electrical angular cycles in one joint angular cycle. Therefore, in order to obtain the joint angle of the motor, the motor can be self-checked after power-on, the motor is enabled to touch the mechanical limit of each shaft, then the joint angle of the motor is calculated through the electrical angle by taking the mechanical limit as a reference point, and therefore the normal work of the cradle head is realized. The method needs to perform self-checking on the three-axis motor after power-on, and is low in efficiency and poor in body feeling brought to a user.

Based on the above problems, the embodiment of the invention provides a method and a device for initializing a joint angle and an aircraft, so as to improve the self-checking efficiency of a triaxial holder after power-up.

Embodiments of the present invention will be further described below with reference to the accompanying drawings.

Example 1

Referring to fig. 2, fig. 2 is a schematic diagram of a triaxial holder according to an embodiment of the present invention;

As shown in fig. 2, the three-axis tripod head comprises three-axis motors, namely a first motor M ₁, a second motor M ₂ and a third motor M ₃, wherein the first motor is used for controlling the rotation of a yaw axis arm of the three-axis tripod head, the second motor is used for controlling the rotation of a roll axis arm of the three-axis tripod head, the third motor is used for controlling the rotation of a pitch axis arm of the three-axis tripod head, so as to further realize the control of the posture of the three-axis tripod head, the first motor M ₁ corresponds to a base end, the second motor M ₂ corresponds to a tool end, the third motor M ₃ corresponds to a tail end, i.e. the first motor is located at the position where the base end is located, the second motor is located at the position where the tool end is located, the third motor is located at the position where the end is the end of the tool end, i.e. the tool end, wherein each motor corresponds to a pole pair number, assuming that the first pole pair number corresponding to the first motor M ₁ is N ₁, the second pole pair corresponding to the second motor M ₂ is N ₂, the third pole pair number corresponding to the third motor M ₃ is N ₃, wherein, the electrical angle of the first motor is θ _e1 at the power-on time, the electrical angle of the second motor is θ _e2, the electrical angle of the third motor is θ _e3, the first joint angle corresponding to the first motor is θ _j1, the second joint angle corresponding to the second motor is θ _j2, and the third joint angle corresponding to the third motor is θ _j3, wherein, the corresponding relation between the joint angle and the electrical angle is the following formula (1):

wherein the function of the wrap (·) function is to limit the joint angle between [ -pi, pi ].

Since the axial direction of the first motor M ₁ is parallel to the Z-axis direction of the base end, when the base end is horizontal, the rotation of the first motor M ₁ does not affect the value of gravity acting on the triaxial accelerometer of the tool end, so the joint angles of the second motor M ₂ and the third motor M ₃ can be estimated by the base end accelerometer and the tool end accelerometer.

Specifically, referring to fig. 3 again, fig. 3 is a flow chart of an initialization method of a joint angle according to an embodiment of the present invention;

The joint angle refers to a joint angle of a motor, and the initialization method of the joint angle is applied to the aircraft as described above, the aircraft is provided with a three-axis cradle head, the three-axis cradle head comprises three-axis motors, namely, a first motor M ₁, a second motor M ₂ and a third motor M ₃, the three-axis motors respectively correspond to one pole pair number, namely, the first motor M ₁ corresponds to a first pole pair number N ₁, the second motor M ₂ corresponds to a second pole pair number N ₂, the third motor M ₃ corresponds to a third pole pair number N ₃, the three-axis cradle head further comprises a base end and a tool end, and the base end and the tool end are both provided with three-axis accelerometers, as shown in fig. 3, the initialization method of the joint angle comprises:

Step S10, a first joint angle corresponding to the first motor power-on time, a second electrical angle corresponding to the second motor power-on time and a third electrical angle corresponding to the third motor power-on time are obtained;

specifically, at the time of power-on, a first joint angle corresponding to the first motor is obtained, specifically, the obtaining of the first joint angle corresponding to the first motor includes:

And determining a first joint angle corresponding to the first motor in a self-checking limit mode. Specifically, the first motor is controlled to touch the mechanical limit, so that the first joint angle theta _j1 corresponding to the first motor is determined, and after the first joint angle corresponding to the first motor is determined, the second joint angle of the second motor and the third joint angle of the third motor can be calculated through the base end accelerometer and the tool end accelerometer.

Specifically, since the linear hall sensor is mounted on the triaxial motor on the cradle head and is used for detecting the magnetic field of the motor rotor, a first electrical angle corresponding to the first motor, a second electrical angle corresponding to the second motor and a third electrical angle corresponding to the third motor can be calculated, and then the first electrical angle corresponding to the first motor, the second electrical angle corresponding to the second motor and the third electrical angle corresponding to the third motor are obtained.

In an embodiment of the present invention, the method further includes:

Specifically, when the accelerometer module at the base end and the accelerometer module at the tool end are simultaneously at a gravity acceleration, the cradle head is determined to be in a static state, and it can be understood that when the accelerometer module at the base end and the accelerometer module at the tool end are simultaneously near the gravity acceleration, the cradle head is also considered to be in a static state, specifically, an error value is preset, and when the error between the accelerometer module at the base end and the gravity acceleration is smaller than the error value, and/or the error between the accelerometer module at the tool end and the gravity acceleration is smaller than the error value, the cradle head is determined to be in a static state.

Step S20: traversing the second joint angle and the third joint angle of the second motor and the third motor according to the second electrical angle and the second pole logarithm of the second motor and the third electrical angle and the third pole logarithm of the third motor, and determining joint angle combinations of the first motor, the second motor and the third motor;

Specifically, each motor corresponds to a pole pair number, the first motor corresponds to a first pole pair number, the second motor corresponds to a second pole pair number, and the third motor corresponds to a third pole pair number, and since the first joint angle of the first motor has been determined by a self-checking limit mode, and the pole pair number of the second motor is N ₂ and the pole pair number of the third motor is N ₃, the joint angles of the first motor, the second motor and the third motor have N ₂*N₃ combinations, so as to determine the joint angle combinations of the first motor, the second motor and the third motor.

It will be appreciated that in the N ₂*N₃ combinations, there are some combinations of joint angles that are not satisfactory, and that screening is required to exclude, so that after determining the combinations of joint angles of the first motor, the second motor, and the third motor, the method further includes: screening joint angle combinations of the first motor, the second motor and the third motor, determining the joint angle combination after screening, specifically, determining the joint angle combination which does not meet the mechanical limit of the cradle head, and excluding the joint angle which does not meet the mechanical limit of the cradle head, for example: and the joint angles which do not accord with the mechanical limit of the cradle head in the second joint angle and the third joint angle are determined, so that the joint angle combination after screening is determined.

Step S30: determining a gesture quaternion corresponding to the joint angle combination and a gesture quaternion of the base end, and determining the gesture quaternion of the tool end according to the gesture quaternion of the base end and the gesture quaternion of the joint angle combination;

Specifically, the determining the gesture quaternion corresponding to the joint angle combination includes:

Determining a posture quaternion corresponding to each joint angle combination, specifically, for a tripod head with three orthogonal axes, determining a quaternion based on a base coordinate system according to the rotation sequence of M1-M2-M3, namely the rotation sequence of a base end-a tool end-a tail end, wherein the quaternion based on the base coordinate system is expressed as the following formula (2):

Wherein, θ _j1 is a first joint angle corresponding to the first motor, θ _j2 is a second joint angle corresponding to the second motor, and θ _j3 is a third joint angle corresponding to the third motor.

Specifically, referring to fig. 4 again, fig. 4 is a detailed flowchart of step S30 in fig. 3;

as shown in fig. 4, the step S30: determining the attitude quaternion of the base end comprises the following steps:

Step S31: acquiring readings of a triaxial accelerometer at the base end, fixing a yaw angle of the base end, and calculating a pitch angle and a roll angle of the base end according to the readings of the triaxial accelerometer at the base end;

Specifically, by acquiring the readings of the triaxial accelerometer at the base end and fixing the yaw angle at the base end, that is, making the yaw angle at the base end be 0, the pitch angle and the roll angle at the base end are calculated according to the readings of the triaxial accelerometer at the base end, and the specific calculation mode is the prior art and is not repeated here. And the yaw angle of the base end is fixed, and the pitch angle and the roll angle of the base end are calculated through the readings of the triaxial accelerometer of the base end, so that a group of Euler angles are determined, and the attitude quaternion of the base end is determined according to the determined group of Euler angles.

Step S32: and determining the attitude quaternion of the base end according to the yaw angle, the pitch angle and the roll angle of the base end.

Specifically, assuming that the pitch angle in the euler angles is θ _p, the roll angle is θ _r, the yaw angle is θ _y, and the rotation angle sequence thereof is "ZXY", the calculation method for determining the attitude quaternion of the base end according to the euler angles is as follows:

Wherein, θ _p is a pitch angle, θ _r is a roll angle, θ _y is a yaw angle, θ _j1 is a first joint angle corresponding to a first motor, θ _j2 is a second joint angle corresponding to a second motor, and θ _j3 is a third joint angle corresponding to a third motor.

It may be appreciated that, when the joint angle combinations of the first motor, the second motor and the third motor are screened, the screened joint angle combinations are determined, and the determining the posture quaternion corresponding to the joint angle combinations includes:

Specifically, the determining the attitude quaternion of the tool end according to the attitude quaternion of the base end and the attitude quaternion of the joint angle combination includes:

Specifically, assuming that the posture quaternion of the base end is p= [ P ₀ p₁ p₂ p₃ ], the posture quaternion of the joint angle combination is q= [ Q ₀ q₁ q₂ q₃ ], and the posture quaternion of the tool end is R, the posture quaternion of the tool end is a result obtained by multiplying the posture quaternion of the base end and the quaternion of the posture quaternion of the joint angle combination, namelyThe quaternion multiplication is as follows (4):

Namely, the calculated attitude quaternion r= [ R ₀ r₁ r₂ r₃ ] of the tool end.

Step S40: and according to the gesture quaternion of the tool end, combining the readings of the triaxial accelerometer of the tool end to determine the initialized joint angle combination of the triaxial motor.

Specifically, referring to fig. 5 again, fig. 5 is a detailed flowchart of step S40 in fig. 3;

As shown in fig. 5, the determining the initialized joint angle combination of the tri-axial motor according to the gesture quaternion of the tool end and in combination with the readings of the tri-axial accelerometer of the tool end includes:

Step S41: determining a rotation matrix according to the gesture quaternion of the tool end;

Specifically, assuming that the posture quaternion of the tool end is q= [ Q ₀ q₁ q₂ q₃ ], the calculation mode of the corresponding rotation matrix M is the following formula (5):

namely, the rotation matrix M is:

Step S42: generating a gravity vector according to the readings of the triaxial accelerometer at the tool end;

Specifically, a reading of the triaxial accelerometer of the tool end is obtained, and the reading of the triaxial accelerometer of the tool end is taken as a gravity vector [ a _x2 a_y2 a_z2 ].

Step S43: and determining an initialization joint angle combination of the triaxial motor according to the rotation matrix and the gravity vector.

Referring to fig. 6 again, fig. 6 is a detailed flowchart of step S43 in fig. 5;

as shown in fig. 6, the determining the initialized joint angle combination of the triaxial motor according to the rotation matrix and the gravity vector includes:

step S431: acquiring a column vector of the rotation matrix;

Specifically, a column vector of the last column of the rotation matrix, that is, the last column element of the rotation matrix, is obtained.

Step S432: performing dot multiplication operation on the column vector of the rotation matrix and the gravity vector to generate a plurality of dot multiplication results;

Specifically, the last column element [ a _x1 a_y1 a_z1 ] of the rotation matrix is obtained, the gravity vector [ a _x2a_y2 a_z2 ] and the last column element [ a _x1 a_y1 a_z1 ] of the rotation matrix are subjected to vector dot-multiplication operation, a dot-multiplication result is obtained, and the dot-multiplication result is stored.

It will be appreciated that there may be a plurality of readings of the tri-axial accelerometer and therefore a plurality of point products may occur and therefore a corresponding joint angle combination needs to be determined from the plurality of point products as an initialisation joint angle combination.

Step S433: and taking the first joint angle, the second joint angle and the third joint angle corresponding to the maximum value of the point multiplication result as an initialization joint angle combination of the triaxial motor.

Specifically, from all the calculated point multiplication results, determining the maximum value of the point multiplication results, wherein the second joint angle θ _j2 and the third joint angle θ _j3 corresponding to the maximum value of the point multiplication results are real joint angles of the second motor and the third motor, and further determining the first initialization joint angle corresponding to the first motor, the second initialization joint angle corresponding to the second motor and the third initialization joint angle corresponding to the third motor of the three-axis motor because the first joint angle θ _j1 corresponding to the first motor is already determined, so that the joint angle initialization of the three-axis motor is completed.

In an embodiment of the present invention, by providing an initialization method for a joint angle, the initialization method is applied to an aircraft, the aircraft is provided with a three-axis cradle head, the three-axis cradle head includes three-axis motors, which are a first motor, a second motor, and a third motor, the second motor corresponds to a second pole pair number, the third motor corresponds to a third pole pair number, the three-axis cradle head further includes a base end and a tool end, and the tool end is provided with a three-axis accelerometer, the method includes: acquiring a first joint angle corresponding to the first motor power-on time, a second electrical angle corresponding to the second motor power-on time and a third electrical angle corresponding to a third motor power-on time; traversing the second joint angle and the third joint angle of the second motor and the third motor according to the second electrical angle and the second pole logarithm of the second motor and the third electrical angle and the third pole logarithm of the third motor, and determining joint angle combinations of the first motor, the second motor and the third motor; determining a gesture quaternion corresponding to the joint angle combination and a gesture quaternion of the base end, and determining the gesture quaternion of the tool end according to the gesture quaternion of the base end and the gesture quaternion of the joint angle combination; and according to the gesture quaternion of the tool end, combining the readings of the triaxial accelerometer of the tool end to determine the initialized joint angle combination of the triaxial motor. By carrying out self-checking on the first motor and determining the initialized joint angle combination of the triaxial motor, the embodiment of the invention can improve the self-checking efficiency of the triaxial holder after being electrified.

Example two

Referring to fig. 7, fig. 7 is a schematic diagram of an initialization apparatus for joint angles according to an embodiment of the invention;

wherein the initialization device 70 of the joint angle is applied to an aircraft, the aircraft is provided with a triaxial holder, the triaxial holder comprises a triaxial motor, which is respectively a first motor, a second motor and a third motor, the second motor corresponds to a second pole pair number, the third motor corresponds to a third pole pair number, the triaxial holder also comprises a base end and a tool end, the tool end is provided with a triaxial accelerometer,

The three-axis tripod head comprises three-axis motors, namely a first motor M ₁, a second motor M ₂ and a third motor M ₃, wherein the first motor is used for controlling the rotation of a yaw axis arm of the three-axis tripod head, the second motor is used for controlling the rotation of a roll axis arm of the three-axis tripod head, the third motor is used for controlling the rotation of a pitch axis arm of the three-axis tripod head so as to further realize the control of the posture of the three-axis tripod head, the first motor M ₁ corresponds to a base end, the second motor M ₂ corresponds to a tool end, the third motor M ₃ corresponds to a tail end, wherein each motor corresponds to a pole pair number, the first pole pair number corresponding to the first motor M ₁ is assumed to be N ₁, the second pole pair number corresponding to the second motor M ₂ is assumed to be N ₂, the third pole pair number corresponding to the third motor M ₃ is assumed to be N ₃, wherein the electrical angle of the first motor at the time of power-up is θ _e1, the electrical angle of the second motor is θ _e2, the electrical angle of the third motor is θ _e3, the first joint angle corresponding to the first motor is θ _j1, the second joint angle corresponding to the second motor is theta _j2, the third joint angle corresponding to the third motor is theta _j3, and the corresponding relation between the joint angle and the electrical angle is the following formula (1):

As shown in fig. 7, the apparatus includes:

An obtaining unit 71, configured to obtain a first joint angle corresponding to the first motor power-on time, a second electrical angle corresponding to the second motor power-on time, and a third electrical angle corresponding to a third motor power-on time;

In an embodiment of the present invention, the method further includes:

A joint angle combination unit 72 configured to determine a joint angle combination of the first motor, the second motor, and the third motor by traversing the second joint angle and the third joint angle of the second motor and the third motor according to the second electrical angle, the second pole pair number, and the third electrical angle, the third pole pair number of the third motor;

A posture quaternion unit 73, configured to determine a posture quaternion corresponding to the joint angle combination and a posture quaternion of the base end, and determine a posture quaternion of the tool end according to the posture quaternion of the base end and the posture quaternion of the joint angle combination;

An initialized joint angle combination unit 74, configured to determine an initialized joint angle combination of the three-axis motor according to the posture quaternion of the tool end and in combination with a reading of the three-axis accelerometer of the tool end.

In the embodiment of the present invention, the gesture quaternion unit is specifically configured to:

In an embodiment of the present invention, the apparatus further includes:

In an embodiment of the present invention, the obtaining unit is specifically configured to:

In an embodiment of the present invention, the apparatus further includes:

namely, the rotation matrix M is:

In an embodiment of the present invention, the initialization joint angle combining unit is specifically configured to:

acquiring a column vector of the rotation matrix;

It should be noted that, the device can execute the method provided by the embodiment of the application, and has the corresponding functional modules and beneficial effects of executing the method. Technical details which are not described in detail in the device embodiments may be found in the methods provided by the embodiments of the present application.

In an embodiment of the present invention, by providing an initializing device for a joint angle, the initializing device is applied to an aircraft, the aircraft is provided with a triaxial tripod head, the triaxial tripod head includes a triaxial motor, which is a first motor, a second motor and a third motor, respectively, the second motor corresponds to a second pole pair number, the third motor corresponds to a third pole pair number, the triaxial tripod head further includes a base end and a tool end, and the tool end is provided with a triaxial accelerometer, the device includes: the acquisition unit is used for acquiring a first joint angle corresponding to the first motor power-on time, a second electrical angle corresponding to the second motor power-on time and a third electrical angle corresponding to the third motor power-on time; the joint angle combination unit is used for traversing the second joint angles and the third joint angles of the second motor and the third motor according to the second electrical angle and the second pole pair number of the second motor and the third electrical angle and the third pole pair number of the third motor, and determining joint angle combinations of the first motor, the second motor and the third motor; the gesture quaternion unit is used for determining gesture quaternions corresponding to the joint angle combination and the gesture quaternion of the base end, and determining the gesture quaternion of the tool end according to the gesture quaternion of the base end and the gesture quaternion of the joint angle combination; and the initialization joint angle combination unit is used for determining the initialization joint angle combination of the triaxial motor according to the posture quaternion of the tool end and the reading of the triaxial accelerometer of the tool end. By carrying out self-checking on the first motor and determining the initialized joint angle combination of the triaxial motor, the embodiment of the invention can improve the self-checking efficiency of the triaxial holder after being electrified.

Referring to fig. 8, fig. 8 is a schematic diagram of a hardware structure of an aircraft according to an embodiment of the invention. The aircraft may be an unmanned aircraft (unmanned AERIAL VEHICLE, UAV), an unmanned airship, or other electronic device.

As shown in fig. 8, the aircraft 800 includes one or more processors 801 and memory 802. In fig. 8, a processor 801 is taken as an example.

The processor 801 and the memory 802 may be connected by a bus or otherwise, for example in fig. 8.

The memory 802 is used as a non-volatile computer readable storage medium, and may be used to store a non-volatile software program, a non-volatile computer executable program, and modules, such as units (e.g., the respective modules or units described in fig. 7) corresponding to a method for initializing a joint angle in an embodiment of the present invention. The processor 801 executes various functional applications and data processing of the method of initializing the joint angle, that is, the functions of the respective modules and units of the method of initializing the joint angle of the above-described method embodiment and the above-described apparatus embodiment, by running the nonvolatile software programs, instructions, and modules stored in the memory 802.

The memory 802 may include high-speed random access memory, and may also include non-volatile memory, such as at least one magnetic disk storage device, flash memory device, or other non-volatile solid-state storage device. In some embodiments, memory 802 may optionally include memory located remotely from processor 801, which may be connected to processor 801 via a network. Examples of such networks include, but are not limited to, the internet, intranets, local area networks, mobile communication networks, and combinations thereof.

The modules are stored in the memory 802, which when executed by the one or more processors 801, perform the method of initializing joint angles in any of the method embodiments described above, for example, performing the steps shown in fig. 3-6 described above; the functions of the various modules or units described in fig. 7 may also be implemented.

Referring to fig. 9 and 10, the aircraft 800 further includes a power system 803, the power system 803 is configured to provide flight power to the aircraft, and the power system 803 is coupled to the processor 801. The power system 803 includes: the electric power meter 8032 is electrically connected with the driving motor 8031 and is used for controlling the driving motor 8031. Specifically, the electric power adjuster 8032 executes the above-mentioned method for initializing the joint angle based on the processor 801, so as to generate a control command conveniently, and controls the driving motor 8031 through the control command.

The aircraft 800 may execute the method for initializing the joint angle according to the first embodiment of the present invention, and has the corresponding functional modules and beneficial effects of the execution method. For technical details not described in detail in the aircraft embodiments, reference may be made to the method for initializing the joint angle provided in the first embodiment of the present invention.

Embodiments of the present invention provide a computer program product comprising a computer program stored on a non-transitory computer readable storage medium, the computer program comprising program instructions which, when executed by a computer, cause the computer to perform a method of initializing joint angles as described above. For example, the above-described method steps S10 to S40 in fig. 3 are performed.

Embodiments of the present invention also provide a non-volatile computer storage medium storing computer-executable instructions that are executed by one or more processors, such as the one processor 801 in fig. 8, to cause the one or more processors to perform the method of initializing the joint angle in any of the method embodiments described above, such as performing the steps shown in fig. 3-6 described above; the functions of the individual modules or units shown in fig. 7 may also be implemented.

The above-described embodiments of the apparatus or device are merely illustrative, in which the unit modules illustrated as separate components may or may not be physically separate, and the components shown as unit modules may or may not be physical units, may be located in one place, or may be distributed over multiple network module units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

From the above description of embodiments, it will be apparent to those skilled in the art that the embodiments may be implemented by means of software plus a general purpose hardware platform, or may be implemented by hardware. Based on such understanding, the foregoing technical solution may be embodied essentially or in a part contributing to the related art in the form of a software product, which may be stored in a computer readable storage medium, such as ROM/RAM, a magnetic disk, an optical disk, etc., including several instructions for up to a computer device (which may be a personal computer, a server, or a network device, etc.) to execute the method described in the respective embodiments or some parts of the embodiments.

Finally, it should be noted that: the above embodiments are only for illustrating the technical solution of the present application, and are not limiting; the technical features of the above embodiments or in the different embodiments may also be combined within the idea of the application, the steps may be implemented in any order, and there are many other variations of the different aspects of the application as described above, which are not provided in detail for the sake of brevity; although the application has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit of the application.

Claims

1. The utility model provides an initialization method of joint angle, is applied to the aircraft, the aircraft is provided with triaxial cloud platform, its characterized in that, triaxial cloud platform includes triaxial motor, is first motor, second motor and third motor respectively, the second motor corresponds the second pole logarithm, the third motor corresponds the third pole logarithm, triaxial cloud platform still includes base end and instrument end, instrument end with the base end all is provided with triaxial accelerometer, the method includes:

According to the attitude quaternion of the tool end, combining the readings of a triaxial accelerometer of the tool end to determine an initialized joint angle combination of the triaxial motor;

The obtaining the first joint angle corresponding to the first motor power-on time, the second electrical angle corresponding to the second motor power-on time and the third electrical angle corresponding to the third motor power-on time includes:

Determining a first joint angle corresponding to the first motor in a self-checking limit mode, and calculating a second joint angle of the second motor and a third joint angle of the third motor through the triaxial accelerometer at the base end and the triaxial accelerometer at the tool end after determining the first joint angle corresponding to the first motor;

And calculating a first electrical angle corresponding to the first motor, a second electrical angle corresponding to the second motor and a third electrical angle corresponding to the third motor by detecting the magnetic field of the motor rotor, and further obtaining the first electrical angle corresponding to the first motor, the second electrical angle corresponding to the second motor and the third electrical angle corresponding to the third motor.

2. The method of claim 1, wherein the determining the attitude quaternion of the base end comprises:

3. The method of claim 1, wherein after the determining the joint angle combination of the first motor, the second motor, and the third motor, the method further comprises: screening joint angle combinations of the first motor, the second motor and the third motor, determining the screened joint angle combinations, and determining posture quaternions corresponding to the joint angle combinations, wherein the method comprises the following steps:

4. The method according to claim 1, wherein the method further comprises:

5. The method of claim 1, wherein the determining the pose quaternion for the tool end from the combined pose quaternion for the base end and the joint angle comprises:

6. The method of claim 1, wherein the determining the initialized joint angle combination of the tri-axis motor based on the pose quaternion of the tool end in combination with readings of the tri-axis accelerometer of the tool end comprises:

7. The method of claim 6, wherein determining an initialized joint angle combination for the three-axis motor based on the rotation matrix and the gravity vector comprises:

acquiring a column vector of the rotation matrix;

8. An initialisation device for joint angles implementing the method of any of claims 1-7, applied to an aircraft provided with a three-axis cradle head, characterized in that the three-axis cradle head comprises three-axis motors, a first motor, a second motor and a third motor, respectively, the second motor corresponding to a second pole pair number and the third motor corresponding to a third pole pair number, the three-axis cradle head further comprising a base end and a tool end, the tool end being provided with a three-axis accelerometer, the device comprising:

9. An aircraft, comprising:

A body;

The arm is connected with the machine body;

the power device is arranged on the machine body and/or the arm and is used for providing flying power for the aircraft;

the flight controller is arranged on the machine body;

wherein the flight controller comprises:

At least one processor; and

A memory communicatively coupled to the at least one processor; wherein,

The memory stores instructions executable by the at least one processor to enable the at least one processor to perform the method of initializing the joint angle of any one of claims 1-7.